First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update fleet crab walk control

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-01 20:34:22 +08:00
parent 1c46d85587
commit 63ef3c8bb8
5 changed files with 298 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/FleetCrabWalkWorkContext.md
+72 -63
View File
@@ -2,7 +2,7 @@
> 本文档汇总 **当前仓库状态、外部依赖、运行/日志路径、代码地图与待解决问题**，便于后续继续调试「车队联动-自动蟹行」。
>
> 最后更新：2026-06-29
> 最后更新：2026-07-01
---
@@ -11,8 +11,8 @@
| 阶段 | 状态 | 说明 |
|------|------|------|
| 动作能启动、能下发运动 | ✅ 已解决 | 方案 1（预热）修复了启动期 `(0,0,0)` 快照导致 `Track()` 立即结束（`iter=0`）的问题 |
| 路径跟踪质量 | 🔧 已改，待实测 | 2026-06-29 继续处理：自动蟹行改为复用手动蟹行同款 `mode=1` 下发链路，只叠加小幅平滑横向纠偏 |
| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）丝滑；因此自动抖动主要来自纠偏链路而非底盘执行能力 |
| 路径跟踪质量 | 🔧 已改，待实测 | 2026-07-01 改为自动字段链路；MovementTest 中 `FleetCrabAngleDeg=-x` 表示车身保持当前角度，以 x 度夹角追踪路径 |
| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）仍保留；自动蟹行不再复用脚本手动链路 |
**触发方式**：主车 Clumsy → MovementTest 面板 → **「车队联动-自动蟹行」**（`FleetCrabWalkTest`）。
@@ -31,64 +31,66 @@
**可能相关机制**（按优先级，供下一轮对照日志）：
1. **控制器读到的「当前位姿」与真实 SLAM 中心不同步**
- `AbstractGeometricController.Track()` 在 `MultiVehicleSync=true` 时通过 `MultiVehicleGetFleetPos()` 读 `PilotDefinition.GetFleetCenterSnapshot()`。
1. **控制器读到的「当前位姿」与真实 SLAM 中心不同步**
- `AbstractGeometricController.Track()` 在 `MultiVehicleSync=true` 时通过 `MultiVehicleGetFleetPos()` 读 `PilotDefinition.GetFleetCenterSnapshot()`。
- 已处理：`TickMultiVehicle` 每拍开头的 `PublishFleetCenter(0,0,0)` 已移除，避免动作/控制线程并发读到假中心。
2. **横向纠偏 `bias` 项在蟹行模式下的参考系**
- `MultiWheelGeometricController.PerformGoing` 中 `bias = -bias` 后按 Stanley 形式修正 gcp（`BiasFac` / `BiasThreshold`）。
- 蟹行时 `thDiff` 来自路径切线（≈夹角），`dTh` 参考固定 `CrabTargetHeading`；若 `bias` 符号或 fleet 中心更新滞后，会持续向一侧推。
- 已绕开：`FleetCrabWalk` 当前不再用几何控制器直接下发 gcp；改为 Detour 计算 `along/lateral/remain`，再写脚本 `MultiVehicleScriptVx/Vy`。
2. **横向纠偏 `bias` 项在蟹行模式下的参考系**
- 当前实现不改 MDCSToolbox，只参考几何控制器思路在 `MultiWheelC` 内计算。
- `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向修正。
- 输出直接写 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/Y/Th`，由 `TickMultiVehicle` 自动分支统一下发。
- MovementTest 会令 `BodyToPathAngleDeg = FleetCrabAngleDeg`，因此 `targetBodyTh = pathTh - BodyToPathAngleDeg = 启动时车队朝向`。
3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘**
- 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。
3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘**
- 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。
- 各车 SLAM 偏差经 `PosBias*` 叠加到 `SendMotion`，可能与几何控制器横向纠偏形成耦合振荡。
4. **动作期间关闭了 `MultiVehicleAutoUseIdealCenter`**
- 有意为之（避免 ideal 中心回灌快照、抹平真实 bias）。副作用是仅依赖「快照中心 + bias 闭环」，对快照质量更敏感。
4. **理想车队中心前馈**
- 当前动作会发布 `MultiVehicleAutoIdealX/Y/Th`。
- `MultiVehicleAutoUseIdealCenter=true` 时，从车使用该理想中心做 layout 前馈；关闭后只用当前广播中心和补偿项。
5. **路径/起点几何**
- 起点：`TryGetFleetCenterFromSlam()`；路径：`LineTrack(x0,y0 → dst)`，`phi = theta + CrabAngleDeg`。
5. **路径/起点几何**
- 起点：`TryGetFleetCenterFromSlam()`；路径：`LineTrack(x0,y0 → dst)`，`phi = theta + CrabAngleDeg`。
- 若 `theta` 与运行时 `CenterTh` 不一致，或 layout 反推中心与控制器使用的快照中心有系统偏差，会表现为沿某一轴漂移。
**建议下一轮日志对照**：
- `FleetCrabDbg`：`lateral` 是否收敛、`corr/localAngle/cmd` 是否平滑、有无到达纠偏上限
- `MultiVehicleDbg`：`frontTh/rearTh/speed` 是否接近手动蟹行、POS/Detect 补偿是否在持续驱动，`CRAB in/raw/limit/rev` 是否显示 `raw=-95°` 这类角度未被反向等价转换
- 如需回退旧几何控制器路线，再看 `CrabDbg` 的 `bias/biasItem/gcp/fleetPos`
- `FleetCrabDbg`：`lateral` 是否收敛、`headingErr` 是否收敛、`bias/dth` 是否到达阈值、`auto(vx,fTh,rTh)` 是否稳定
- `MultiVehicleDbg`：自动分支是否为 `auto:true/manual:false/script:false`，`BASE/SEND` 是否接近 `FleetCrabDbg` 输出，POS/Detect 补偿是否持续驱动
- 重点看 `FleetCrabGcpThetaThreshold`、`BiasThreshold`、`DthLinearThreshold` 三个限幅是否过早截断纠偏
**2026-06-29 DLog 结论（自动蟹行仍抖动）**：
- `FleetCrabDbg` 中 `along/lateral/remain/corr/localAngle/cmd` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
**2026-06-29 DLog 结论（旧脚本链路下自动蟹行仍抖动）**：
- 旧 `FleetCrabDbg` 中 `along/lateral/remain/旧方向修正/旧命令` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
- 主/从 `MultiVehicleDbg` 中 `BASE vx` 在正负之间跳，同时 `fTh/rTh` 在 `+90°/-90°` 附近翻转；这是同一横移矢量被错误地按 ±90° 边界转换成两种等价表示，底盘执行层会看到接近 180° 的转向跳变。
- POS 补偿在该批日志中为关闭/零补偿（`corr:false`、`POS comp 0`），Detect 补偿有小幅值但不是主因。
- 因此本轮判定为 **mode=1 蟹行矢量合成把 ±90° 误当舵角边界**，不是优先调 `FleetCrabCorrectionGain`。Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 默认约为 `-120/+120`，自动蟹行应允许 `-95°` 直接下发。
- 因此本轮判定为 **mode=1 蟹行矢量合成把 ±90° 误当舵角边界**，不是优先调横向纠偏增益。Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 默认约为 `-120/+120`，自动蟹行应允许 `-95°` 直接下发。
**2026-06-29 DLog 结论（±120 修复后仍 Y+ 漂移）**：
- Clumsy 侧 `MultiVehicleDbg` 已显示 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，`BASE vx` 不再正负翻转，说明上层 `mode=1` 表达已连续，剧烈抖动问题已消失。
- 但 `FleetCrabDbg` 中 `lateral` 仍从 `0` 单调增长到约 `+171mm`，`corr` 到达 `-8°` 上限后无法拉回；主/从 `DETECT dy` 也增长到百毫米量级，`DETECT comp y` 达到 `20mm/s` 上限。
- 进一步检查 Playground 发现：`D:\MDCS\Source\Core\Medulla\Playground\default_scene.json` 与运行目录 `bin\Debug\net8.0\default_scene.json` 中两台 `multi-steering` 仍为 `"maxSteeringAngle": 90`，而 `ActuatorModels.cs` 会把模块舵角 clamp 到 `[-MaxSteeringAngleRad,+MaxSteeringAngleRad]`。
- 这意味着 Clumsy 发出的 `-98°` 路径纠偏，在 Playground 实际执行时会被夹回 `-90°`，纠偏分量被吞掉；这比继续调 `FleetCrabCorrectionGain` 更像 Y+ 漂移的直接原因。
- 这意味着 Clumsy 发出的 `-98°` 路径纠偏，在 Playground 实际执行时会被夹回 `-90°`，纠偏分量被吞掉；这比继续调横向纠偏增益更像 Y+ 漂移的直接原因。
- 已把 Playground 源码场景和运行目录场景改为 `maxSteeringAngle: 120`，并在仿真器中加入 `multi-steering clamp` 节流日志；复测前必须重启 Playground 使场景重载。若复测时仍出现该日志，说明还有其他配置或场景副本在限制舵角。
### 2.2 两车抖动、不丝滑
**可能原因**：
1. 上节 **快照 `(0,0,0)` 窗口** + 50ms 联动周期 + 50ms `DriveTaskInterval` beat frequency
2. **notify 经 GET fire-and-forget**（`MultiVehicleAutoSyncReview.md` §F），从车命令阶跃
3. **`dTh` 差动 + `bias` 限幅** 在阈值边界来回切换（`DthLinearThreshold` / `BiasThreshold`）
4. **`MultiVehicleSyncUseDetour` POS 补偿** 与主车控制器不同相位
1. 上节 **快照 `(0,0,0)` 窗口** + 50ms 联动周期 + 50ms `DriveTaskInterval` beat frequency
2. **notify 经 GET fire-and-forget**（`MultiVehicleAutoSyncReview.md` §F），从车命令阶跃
3. **`dTh` 差动 + `bias` 限幅** 在阈值边界来回切换（`DthLinearThreshold` / `BiasThreshold`）
4. **`MultiVehicleSyncUseDetour` POS 补偿** 与主车控制器不同相位
5. 预热结束后 **`PrimeMasterAutoFromSlam` 不再调用**（正常）；若 `WARMUP` 期间日志显示 `cnt` 反复变化，说明编队 TTL/register 不稳定
**建议对照实验**：
- 手动 FleetRemote 蟹行（同速度、同角度）是否也抖
- 临时 `MultiVehicleSyncUseDetour=false` 复测
- 看 `FleetCrabDbg` 的 `corr/localAngle/cmd` 与 `MultiVehicleDbg` 的 `frontTh/rearTh` 是否周期跳变
- 手动 FleetRemote 蟹行（同速度、同角度）是否也抖
- 临时 `MultiVehicleSyncUseDetour=false` 复测
- 看 `FleetCrabDbg` 的 `auto(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `BASE/SEND` 是否周期跳变
---
## 3. Tutorial 仓库（本仓库）
**路径**：`D:\MDCS\Source\Tutorial`
**路径**：`D:\MDCS\Source\Tutorial`
**分支**：`master`（截至文档编写时，自动蟹行相关改动**尚未单独 commit**，均为工作区修改）
### 3.1 已修改文件（git status）
@@ -187,8 +189,8 @@ dotnet build MultiWheel\MultiWheelM\MultiWheelM.csproj
### 5.2 触发自动蟹行测试
1. 按上表启动双车栈
2. 主车 Medulla 开启「车队联动」（手动联调时常按 **F**；纯自动蟹行 MovementTest 依赖 `MultiVehicleAutoEnabled`，动作内会自行置位 + 预热）
1. 按上表启动双车栈
2. 主车 Medulla 开启「车队联动」（手动联调时常按 **F**；纯自动蟹行 MovementTest 依赖 `MultiVehicleAutoEnabled`，动作内会自行置位 + 预热）
3. 主车 `build\Clumsy\` 的 Clumsy UI → MovementTest → **车队联动-自动蟹行**
参数来源：`clumsy.json` → `msConf` → `PilotConfig`（未写入 json 的字段用代码默认值）。
@@ -214,8 +216,8 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
| Topic | 来源 | 内容 |
|-------|------|------|
| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/corr/localAngle/cmd` |
| **`CrabDbg`** | `MultiWheelGeometricController.cs` | 旧几何控制器路线诊断；当前脚本蟹行实现不再依赖 |
| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/headingErr/baseTh/bias/dth/auto/ideal` |
| **`CrabDbg`** | `MultiWheelGeometricController.cs` | MDCSToolbox 几何控制器诊断；当前自动蟹行只参考其思路，不修改也不依赖该源码 |
| **`MultiVehicleDbg`** | `PilotDefinition.cs` | 联动循环：速度、舵角、补偿、ready 状态 |
| **`FleetDiagClumsy`** | `PilotDefinition.cs` | 精简 fleet 诊断（带 `car{N}` 前缀） |
| **`MultiVehicle`** | `PilotDefinition.cs` | 初始化、心跳、HTTP 错误 |
@@ -223,10 +225,10 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
### 6.3 建议抓取顺序（排查漂移/抖动）
1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/corr/localAngle/cmd`
2. 主车 + 从车 `MultiVehicleDbg`：`frontTh/rearTh`、`PosBias*`、是否 `ready=false`
3. 若回退旧几何控制器路线，再看主车 `CrabDbg`：`bias` 是否单调增大；`fleetPos` 是否偶发 `(0,0,0)`
4. 从车 `FleetDiagClumsy`：是否频繁掉线 / register 超时
1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/headingErr/bias/dth/auto(vx,fTh,rTh)`
2. 主车 + 从车 `MultiVehicleDbg`：`auto:true/manual:false`、`BASE/SEND`、`PosBias*`、是否 `ready=false`
3. 若怀疑 MDCSToolbox 自动路径，再看主车 `CrabDbg`；当前 `FleetCrabWalk` 不直接调用该控制器
4. 从车 `FleetDiagClumsy`：是否频繁掉线 / register 超时
---
@@ -237,23 +239,26 @@ MovementTest「车队联动-自动蟹行」
FleetCrabWalk.Get()
TryGetFleetCenterFromSlam() → 路径起点 (x0,y0,θ)
phi = theta + FleetCrabAngleDeg
MultiVehicleScriptEnabled = true
MultiVehicleScriptMode = 1 → 复用 FleetRemote 手动蟹行下发链路
BodyToPathAngleDeg = FleetCrabAngleDeg
targetBodyTh = phi - BodyToPathAngleDeg = theta
MultiVehicleScriptEnabled = false
MultiVehicleAutoEnabled = true → 进入 TickMultiVehicle 自动分支
WARMUP → 等编队成员就位
loop:
TryGetFleetCenterFromSlam() → 当前车队中心
along/lateral/remain → 沿线进度、横向偏差、剩余距离
corr = clamp(Stanley(lateral), ±FleetCrabCorrectionAngleDeg)
localAngle = (phi + corr) - currentTheta
Vx/Vy slew limit → FleetCrabCommandAccel 平滑
MultiVehicleScriptVx/Vy = cmd
along/lateral/remain/headingErr → 沿线进度、横向偏差、剩余距离、车身目标朝向偏差
bias = clamp(Stanley(lateral), ±BiasThreshold)
dth = clamp(DthLinearFac * (targetBodyTh-currentTheta), ±DthLinearThreshold)
frontTh/rearTh = clamp(phi-currentTheta + bias ± dth, ±FleetCrabGcpThetaThreshold)
ideal = pathStart + pathDir * clamp(along, 0, FleetCrabLengthMm)
MultiVehicleAutoVx/FrontTh/RearTh/Ideal* = cmd
PilotDefinition.TickMultiVehicle (50ms)
manual/script mode==1
Vx/Vy → speed + frontTh==rearTh
auto branch
MultiVehicleAuto* → speed + frontTh/rearTh + ideal center
notify → 从车 SendMotion + POS/Detect 补偿
```
**对照 baseline**：`PilotDefinition.cs` 手动分支 `fleetMode == 1`（FleetRemote 蟹行）直接合成 `frontTh/rearTh`，不经几何控制器 `bias` 闭环。
**对照 baseline**：`PilotDefinition.cs` 手动分支 `fleetMode == 1`（FleetRemote 蟹行）仍直接合成 `frontTh/rearTh`；自动蟹行当前不走该分支。
---
@@ -263,15 +268,14 @@ MovementTest「车队联动-自动蟹行」
| 字段 | 默认 | 作用 |
|------|------|------|
| `FleetCrabAngleDeg` | 45 | 路径与车队朝向夹角 (deg) |
| `FleetCrabAngleDeg` | 45 | 路径方向相对启动时车队朝向的夹角 (deg)。MovementTest 同时把车身-路径夹角设为该值；若输入“路径与小车夹角 x 度”，应填 `-x` 以保持当前车身角度 |
| `FleetCrabLengthMm` | 2000 | 路径长度 (mm) |
| `FleetCrabSpeed` | 0.2 | 速度 (m/s) |
| `FleetCrabGcpThetaThreshold` | 95 | 兼容旧几何控制器实现；当前脚本蟹行不直接使用 |
| `FleetCrabCorrectionGain` | 1.0 | 横向误差纠偏增益 |
| `FleetCrabCorrectionAngleDeg` | 8 | 自动纠偏最大改向角，越小越接近手动蟹行 |
| `FleetCrabCommandAccel` | 0.4 | 脚本 `Vx/Vy` 命令斜率限制(m/s²) |
| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由通用减速参数下调 |
| `FleetCrabGcpThetaThreshold` | 95 | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限，应给实际舵角限位与 `AngleLimitMarginDeg` 留余量 |
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleScript*` 和 `MultiVehicleAuto*`。
已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略，不能再作为有效调参项。
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleAuto*`，并保持 `MultiVehicleScriptEnabled=false`。
### 8.2 影响跟踪/手感的全局项（节选）
@@ -282,7 +286,12 @@ MovementTest「车队联动-自动蟹行」
| `TestCarSyncDistance` | 2400 | 与 Playground 双车间距一致 |
| `MultiVehicleSyncInterval` | 50 | 联动周期 ms |
| `DriveTaskInterval` | 50 | `clumsy.json` 顶层 |
| `BiasFac` / `DthLinearFac` 等 | 继承 `MultiWheelPilotConfig` | 几何控制器 PID 形态参数 |
| `BiasFac` / `BiasThreshold` | 继承 `MultiWheelPilotConfig` | 横向偏差 `lateral` → 前后 GCP 同向修正 |
| `DthLinearFac` / `DthLinearThreshold` | 继承 `MultiWheelPilotConfig` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正 |
| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 继承 `BasicPilotConfig` | 自动蟹行终点减速和结束判定 |
| `MultiVehicleAutoUseIdealCenter` | true（默认） | 使用自动蟹行发布的 ideal center 给从车做前馈 |
| `MultiVehicleAutoRequireFleetCenter` | true（默认） | 自动模式无有效车队中心时整队停车 |
| `MultiVehicleAutoCmdTimeoutMs` | 0（auto） | 自动命令新鲜度超时，避免控制器停发后沿末速度滑行 |
详见 [MultiVehicleConfig.md](./MultiVehicleConfig.md) §2–§6。
@@ -296,20 +305,20 @@ MovementTest「车队联动-自动蟹行」
| 蟹行要求朝向不变但有纠偏 | `CrabHoldHeading` + `CrabTargetHeading`；保留 `dTh` |
| 多车 firstTurn 破坏队形 | `MultiVehicleSync` 时跳过 `firstTurnN`（TODO 整队预旋转） |
| gcp 被 45° 上限截断 | 动作侧 `GcpThetaThreshold=95` |
| ideal 中心抹平横向误差 | 动作期间关 `MultiVehicleAutoUseIdealCenter` |
| ideal 中心抹平横向误差 | 已改为显式发布 `MultiVehicleAutoIdealX/Y/Th`，由 `MultiVehicleAutoUseIdealCenter` 控制是否前馈 |
| Tick 中间窗口发布 `(0,0,0)` 假中心 | 已移除 tick 开头 `PublishFleetCenter(0,0,0)` |
| 自动蟹行纠偏导致抖动 | 已改为脚本手动蟹行链路 + 小幅平滑横向纠偏 |
| 自动蟹行纠偏导致抖动 | 已改为自动字段链路，按 `lateral/headingErr/remain` 计算 `MultiVehicleAuto*` |
| 接近纯横移时速度符号/舵角表示翻转 | `fleetMode==1` 改为按 `MultiVehicleCrabSteerLimitDeg`（默认 120°）归一化；`-95°` 直接下发，超过上限才做速度取反的等价转换，并在 `MultiVehicleDbg` 输出 `CRAB in/raw/limit/rev` |
---
## 10. 后续工作建议（优先级）
1. **复测 -90° 自动蟹行**：重点看 `MultiVehicleDbg` 的 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，以及 `BASE vx/fTh/rTh` 是否不再正负翻转。
2. **A/B：`MultiVehicleCrabSteerLimitDeg`** 默认 120，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；若实际轮角限制不同，先同步该值。
3. **A/B：`FleetCrabCorrectionAngleDeg`** 先试 4、8、12：4 最接近手动，12 收敛更快；当前不再因跨 ±90° 直接翻面。
4. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分脚本纠偏 vs POS 补偿贡献。
5. **路径误差**：若仍持续 Y+ 漂移，看 `lateral` 是否持续单向增长；若增长但 `corr` 已到上限，增大 `FleetCrabCorrectionAngleDeg` 或 `FleetCrabCorrectionGain`。
1. **复测自动蟹行**：重点看 `FleetCrabDbg` 的 `auto=(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `auto:true/manual:false` 是否一致。
2. **A/B：`FleetCrabGcpThetaThreshold`** 默认 95，应与实车舵角限制和 `AngleLimitMarginDeg` 匹配；若输出很快被限幅，先核对该值。
3. **A/B：`BiasFac/BiasThreshold`** 若 `lateral` 单向增长，先看 `bias` 是否到上限；需要更强横向纠偏时调这组参数。
4. **A/B：`DthLinearFac/DthLinearThreshold`** 若车队朝向偏差收敛慢或前后 GCP 差动过大，调这组参数。
5. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分自动路径纠偏 vs POS 补偿贡献。
6. **notify 平滑**（中长期）：见 `MultiVehicleAutoSyncReview.md` §F。
---
docs/MultiVehicleConfig.md
+32 -19
View File
@@ -144,28 +144,45 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
## 6. 自动蟹行动作（FleetCrabWalk / MovementTest「车队联动-自动蟹行」）
在 Clumsy 侧 MovementTest 面板触发，以**当前车队中心**为起点，构造一条与车队朝向夹角 `FleetCrabAngleDeg`、长度 `FleetCrabLengthMm` 的**直线路径**，执行侧复用 FleetRemote 已验证丝滑的脚本手动等价输入（`MultiVehicleScriptEnabled + mode=1`）让整队**斜向平移（蟹行）**：
在 Clumsy 侧 MovementTest 面板触发，以**当前车队中心**为起点，构造一条与启动时车队朝向夹角 `FleetCrabAngleDeg`、长度 `FleetCrabLengthMm` 的**直线路径**。MovementTest 会保持启动时车身朝向追踪路径；因此如果“路径相对小车”的夹角为 `x` 度（路径在车体右侧为正），应配置 `FleetCrabAngleDeg = -x`。当前实现不再复用脚本手动链路，而是参考几何控制器思路，在 `MultiWheelC` 内计算并写入 `MultiVehicleAuto...` 字段：
- 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral` 和剩余距离 `remain`。
- 横向偏差只转成一个**小幅、带斜率限制的蟹行方向修正**，再写入 `MultiVehicleScriptVx/Vy`；`TickMultiVehicle` 仍按手动蟹行逻辑合成 `frontTh==rearTh` 并广播从车。
- 这样保留手动蟹行的平滑执行链路，同时让自动动作具备温和的路径纠偏；避免旧几何控制器 `bias/dTh` 直接叠到 gcp 时出现舵角阶跃。
- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点）。
- 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral`、剩余距离 `remain` 和车身目标朝向偏差 `headingErr`。
- `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向舵角修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向舵角修正。
- 动作直接输出 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/IdealY/IdealTh`，由 `TickMultiVehicle` 的自动分支统一广播、下发 `SendMotion`，并继续受识别丢失、成员超时、舵角余量不足等整队缓停联锁保护。
- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点与运行中闭环）。
| 字段（`clumsy.json` → `msConf`） | 含义 | 默认值 |
|------|------|--------|
| `FleetCrabAngleDeg` | 蟹行路径**与当前车队朝向的夹角**(deg，逆时针为正)。决定斜行方向：0=正前方，90=正左方平移，-90=正右方。稳态下即各舵轮的蟹行角 | `45` |
| `FleetCrabAngleDeg` | 蟹行路径方向相对**启动时车队朝向**的夹角(deg，逆时针为正)。MovementTest 同时把车身-路径夹角设为该值，因此 `FleetCrabAngleDeg=-x` 会让车身保持启动朝向，并以 `x` 度夹角追踪路径 | `45` |
| `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` |
| `FleetCrabSpeed` | 蟹行**行驶速度**(m/s) | `0.2` |
| `FleetCrabGcpThetaThreshold` | 兼容旧几何控制器实现的 gcp 舵角上限；当前脚本手动等价实现不直接使用 | `95` |
| `FleetCrabCorrectionGain` | 横向误差纠偏增益。增大后收敛更快，但更容易出现方向摆动 | `1` |
| `FleetCrabCorrectionAngleDeg` | 自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大纠偏越强 | `8` |
| `FleetCrabCommandAccel` | `Vx/Vy` 命令斜率限制(m/s²)，抑制纠偏方向突变 | `0.4` |
| `MultiVehicleCrabSteerLimitDeg` | mode=1 蟹行舵角上限，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；`-95°` 在默认 120° 内会直接下发 | `120` |
| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被 `SlowDistance/SlowingPow/FinishSpeed` 降速 | `0.2` |
| `FleetCrabGcpThetaThreshold` | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限(deg)。应小于实际舵角可行范围，并给 `AngleLimitMarginDeg` 留余量 | `95` |
自动蟹行还会使用下列通用控制参数：
| 字段 | 影响 | 默认来源 |
|------|------|----------|
| `BiasFac` / `BiasThreshold` | 横向偏差 `lateral` → 前后 GCP 同向修正。增大后收敛更快，但过大可能摆动 | `MultiWheelPilotConfig` |
| `DthLinearFac` / `DthLinearThreshold` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正，用于保持车身与路径夹角 | `MultiWheelPilotConfig` |
| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 终点减速和结束判定 | `BasicPilotConfig` |
| `MultiVehicleAutoUseIdealCenter` | 是否把自动蟹行计算出的 `IdealX/Y/Th` 广播给从车做前馈 | `true` |
| `MultiVehicleAutoRequireFleetCenter` | 自动模式是否要求有效车队中心；定位/车队中心失效时整队停车 | `true` |
| `MultiVehicleAutoCmdTimeoutMs` | 自动命令新鲜度超时，0 表示按联动周期自动计算 | `0` |
| `MultiVehicleUseDetect` / `MultiVehicleDetectBias*` | 互识别纠正与安全门；检测丢失时整队停车 | 见 §5 |
| `MultiVehicleSyncUseDetour` / `MultiVehiclePosBias*` | 车队内姿态纠正（POS 补偿）；不影响整车队中心计算 | 见 §5 |
已删除的旧自动蟹行参数：
| 已删除字段 | 原用途 | 当前替代 |
|------------|--------|----------|
| `FleetCrabCorrectionGain` | 旧脚本链路的横向误差纠偏增益 | `BiasFac` |
| `FleetCrabCorrectionAngleDeg` | 旧脚本链路的最大改向角 | `BiasThreshold` / `FleetCrabGcpThetaThreshold` |
| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 由自动联动周期、底盘速度斜坡和终点减速共同约束 |
**行为要点 / 注意**
- 当前实现不走 `MultiVehicleAuto*`/`MultiVehicleSendMotion`，也不再临时改 `MultiVehicleAutoUseIdealCenter`；结束/急停会清零脚本字段。
- `FleetCrabDbg` 会记录 `along/lateral/remain/corr/localAngle/cmd(Vx,Vy)`；`MultiVehicleDbg` 可继续对照最终 `frontTh≈rearTh`、是否有 POS/Detect 补偿，以及 `CRAB in/raw/limit/rev` 是否在舵角上限内保持连续表达。
- 当前实现走 `MultiVehicleAuto*` 自动字段链路，不再开启 `MultiVehicleScriptEnabled`，也不受 `MultiVehicleCrabSteerLimitDeg` 影响（该字段只影响手动 `mode=1` 蟹行）。
- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。
- `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。
- Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。
@@ -202,11 +219,7 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
"FleetCrabAngleDeg": 45,
"FleetCrabLengthMm": 2000,
"FleetCrabSpeed": 0.2,
"FleetCrabGcpThetaThreshold": 95,
"FleetCrabCorrectionGain": 1.0,
"FleetCrabCorrectionAngleDeg": 8,
"FleetCrabCommandAccel": 0.4,
"MultiVehicleCrabSteerLimitDeg": 120
"FleetCrabGcpThetaThreshold": 95
```
```text