Tutorial/docs/FleetCrabWalkWorkContext.md

自动蟹行（FleetCrabWalk）工作上下文

本文档汇总 当前仓库状态、外部依赖、运行/日志路径、代码地图与待解决问题，便于后续继续调试「车队联动-自动蟹行」。

最后更新：2026-07-01

---

1. 功能现状

阶段	状态	说明
动作能启动、能下发运动	✅ 已解决	方案 1（预热）修复了启动期 (0,0,0) 快照导致 Track() 立即结束（iter=0）的问题
路径跟踪质量	🔧 已改，待实测	2026-07-01 改为自动字段链路；MovementTest 中 FleetCrabAngleDeg=-x 表示车身保持当前角度，以 x 度夹角追踪路径
与手动蟹行对照	✅ 已验证	手动模式（FleetRemote mode==1）仍保留；自动蟹行不再复用脚本手动链路

触发方式：主车 Clumsy → MovementTest 面板 → 「车队联动-自动蟹行」（FleetCrabWalkTest）。

前提：

• 主车 MultiVehicleMasterEndpoint="/"
• 主车有 Detour 定位（反推车队中心起点）
• 双车 Medulla + Clumsy + Detour 均已启动，编队成员数 = MultiVehicleFleetNum

---

2. 当前已知问题（待排查）

2.1 向 Y+ 方向漂移、偏离路径

现象：车队整体沿世界坐标 Y 正方向持续偏移，横向误差越来越大，未收敛到 LineTrack。

可能相关机制（按优先级，供下一轮对照日志）：

1. 控制器读到的「当前位姿」与真实 SLAM 中心不同步

   • AbstractGeometricController.Track() 在 MultiVehicleSync=true 时通过 MultiVehicleGetFleetPos() 读 PilotDefinition.GetFleetCenterSnapshot()。
   • 已处理：TickMultiVehicle 每拍开头的 PublishFleetCenter(0,0,0) 已移除，避免动作/控制线程并发读到假中心。

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。
   • 各车 SLAM 偏差经 PosBias* 叠加到 SendMotion，可能与几何控制器横向纠偏形成耦合振荡。

4. 理想车队中心前馈

   • 当前动作会发布 MultiVehicleAutoIdealX/Y/Th。
   • MultiVehicleAutoUseIdealCenter=true 时，从车使用该理想中心做 layout 前馈；关闭后只用当前广播中心和补偿项。

5. 路径/起点几何

   • 起点：TryGetFleetCenterFromSlam()；路径：LineTrack(x0,y0 → dst)，phi = theta + CrabAngleDeg。
   • 若 theta 与运行时 CenterTh 不一致，或 layout 反推中心与控制器使用的快照中心有系统偏差，会表现为沿某一轴漂移。

建议下一轮日志对照：

• 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/旧方向修正/旧命令 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
• 主/从 MultiVehicleDbg 中 BASE vx 在正负之间跳，同时 fTh/rTh 在 +90°/-90° 附近翻转；这是同一横移矢量被错误地按 ±90° 边界转换成两种等价表示，底盘执行层会看到接近 180° 的转向跳变。
• POS 补偿在该批日志中为关闭/零补偿（corr:false、POS comp 0），Detect 补偿有小幅值但不是主因。
• 因此本轮判定为 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°，纠偏分量被吞掉；这比继续调横向纠偏增益更像 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 补偿 与主车控制器不同相位
5. 预热结束后 PrimeMasterAutoFromSlam 不再调用（正常）；若 WARMUP 期间日志显示 cnt 反复变化，说明编队 TTL/register 不稳定

建议对照实验：

• 手动 FleetRemote 蟹行（同速度、同角度）是否也抖
• 临时 MultiVehicleSyncUseDetour=false 复测
• 看 FleetCrabDbg 的 auto(vx,fTh,rTh) 与 MultiVehicleDbg 的 BASE/SEND 是否周期跳变

---

3. Tutorial 仓库（本仓库）

路径：D:\MDCS\Source\Tutorial 分支：master（截至文档编写时，自动蟹行相关改动尚未单独 commit，均为工作区修改）

3.1 已修改文件（git status）

路径	作用
MultiWheel/MultiWheelC/MovementTests.cs	FleetCrabWalk / FleetCrabWalkTest；预热 WARMUP；诊断 FleetCrabDbg
MultiWheel/MultiWheelC/PilotDefinition.cs	TryGetFleetCenterFromSlam、PrimeMasterAutoFromSlam；联动循环；fleet 快照
MultiWheel/MultiWheelC/PilotConfig.cs	FleetCrab* 配置字段
MultiWheel/MultiWheelC/ChassisController.cs	MultiVehicleSendMotion / MultiVehicleGetFleetPos；SENDMOTION 诊断
MultiWheel/MultiWheelC/VehicleSyncModels.cs	同步模型（联动机制相关）
MultiWheel/MultiWheelM/MotorRoutine.cs	Medulla 侧电机例程
deploy/clumsy_agv1/clumsy.json	主车 Clumsy 配置模板
deploy/clumsy_agv2/clumsy.json	从车 Clumsy 配置模板
docs/MultiVehicleConfig.md	§6 自动蟹行参数说明
docs/MultiVehicleAutoSyncReview.md	自动联动机制问题清单
docs/RunAndDeploy.md	运行部署说明

3.2 相关文档（本仓库）

文档	内容
RunAndDeploy.md	编译、双车启动、端口/tag 对照
MultiVehicleConfig.md	全部联动参数；§6 自动蟹行
MultiVehicleSync.md	联动算法背景
MultiVehicleAutoSyncReview.md	自动联动已知缺陷（A–H）
BugFixes.md	历史修复清单

---

4. 外部仓库 / 依赖（非 Tutorial git 管理）

Tutorial 插件通过 D:\MDCS\Release\ 引用预编译二进制；改 MDCSToolbox 源码后须先编译再编 Tutorial。

组件	源码 / 产物路径	说明
MDCSToolBox	源码：D:\MDCS\Source\Products\mdcstoolbox\	几何控制器、BasicGo、LineTrack
	编译：dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release	PostBuild → D:\MDCS\Release\MDCSToolBox.dll
	蟹行相关改动：Clumsy/MotionControllers/MultiWheelGeometricController.cs	CrabHoldHeading / CrabTargetHeading；CrabDbg
		Clumsy/MotionControllers/AbstractGeometricController.cs
	参考：Clumsy/AgvInterfaces/BasicInterface.cs	BasicGo + AddTrack 模式
Clumsy	D:\MDCS\Release\Clumsy\ClumsyLite.exe	运行时宿主
Medulla	D:\MDCS\Release\Medulla\	车体插件宿主
CommonUsage	D:\MDCS\Release\CommonUsage.dll	CommonMath、坐标变换
FundamentalLib	D:\MDCS\Release\deps\RefFundamentalLib.dll	DLog 落盘
Simple（可选调度）	D:\MDCS\Source\Core\Simple\	MultiWheelS → SimpleComposer.exe
Detour / Playground	通常随仿真环境部署	非 Tutorial 子目录；见 §5 运行目录

4.1 编译顺序（改动了 MDCSToolBox 时）

# 1. 工具箱
dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release

# 2. Tutorial 插件（MultiWheelC PostBuild 会把 Release 下 DLL 复制到 build/Clumsy*）
cd D:\MDCS\Source\Tutorial
dotnet build MultiWheel\MultiWheelC\MultiWheelC.csproj
dotnet build MultiWheel\MultiWheelM\MultiWheelM.csproj

仅改 Tutorial 侧 C# 时，只需第二步。

---

5. 测试执行：程序与工作目录

build/ 为 gitignore 运行目录（首次编译后生成）。下列路径均相对于 D:\MDCS\Source\Tutorial\。

5.1 双车仿真典型启动顺序

1. Playground（仿真场景，含 agv_multi_1 / agv_multi_2）
2. Detour ×2（工作目录一般在 Clumsy build 树下）
3. Medulla ×2
4. ClumsyLite ×2

角色	工作目录	主程序	关键配置
AGV1 主车	build\Medulla\	Medulla 控制台	startup.iocmd：SetShareObjectTag Multi1，CarNum 1
AGV1 Clumsy	build\Clumsy\	ClumsyLite.exe	deploy\clumsy_agv1\ 模板；port 8008
AGV1 Detour	build\Clumsy\（或同树 Detour\）	DetourLite	HTTP 4321，tag Multi1
AGV2 从车	build\Medulla_AGV2\	Medulla 控制台	tag Multi2，CarNum 2
AGV2 Clumsy	build\Clumsy_AGV2\	ClumsyLite.exe	port 8009，master 127.0.0.1:8008
AGV2 Detour	build\Clumsy_AGV2\Detour_AGV2\ 等	DetourLite	HTTP 4421，tag Multi2

快捷脚本（在已配置好的 build 目录内）：

• deploy\start_clumsy_agv1.bat → 复制配置后启动 ClumsyLite.exe（主车）
• deploy\start_clumsy_agv2.bat → 从车
• 一键 7 进程（若环境已装）：DetourLite\bin\Debug\net8.0\start_all_sim.bat（路径见 RunAndDeploy.md）

编译产物落点：

项目	输出
MultiWheelC.csproj	build\Clumsy\MultiWheelC.dll + PostBuild 同步到 build\Clumsy_AGV2\
MultiWheelM.csproj	build\Medulla\plugins\MultiWheelM.dll（AGV2 Medulla 需另行复制或 PostBuild）

5.2 触发自动蟹行测试

1. 按上表启动双车栈
2. 主车 Medulla 开启「车队联动」（手动联调时常按 F；纯自动蟹行 MovementTest 依赖 MultiVehicleAutoEnabled，动作内会自行置位 + 预热）
3. 主车 build\Clumsy\ 的 Clumsy UI → MovementTest → 车队联动-自动蟹行

参数来源：clumsy.json → msConf → PilotConfig（未写入 json 的字段用代码默认值）。

---

6. DLog 日志目录与 Topic

6.1 落盘根目录

DLog 由 Clumsy 进程工作目录下的 dlog\ 管理（FundamentalLib）。双车仿真时：

进程	日志根目录
主车 Clumsy	D:\MDCS\Source\Tutorial\build\Medulla\dlog\
从车 Clumsy	D:\MDCS\Source\Tutorial\build\Medulla_AGV2\dlog\

说明：用户实测路径为上述两处；topic 名对应子文件夹/文件。若 Clumsy 工作目录 strictly 为 build\Clumsy*，也可能在 build\Clumsy\dlog\ —— 以实际进程 cwd 下是否生成 dlog 为准。

目录结构（概念上）：dlog\<TopicName>\ 下按 topic 滚动；同一 topic 的 DLog.Log(msg, topic) 归并到同一目录。

6.2 自动蟹行相关 Topic

Topic	来源	内容
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 错误
MotionControl	控制器框架	通用运动控制（若启用）

6.3 建议抓取顺序（排查漂移/抖动）

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 超时

---

7. 代码地图（数据流）

MovementTest「车队联动-自动蟹行」
  FleetCrabWalk.Get()
    TryGetFleetCenterFromSlam()          → 路径起点 (x0,y0,θ)
    phi = theta + FleetCrabAngleDeg
    BodyToPathAngleDeg = FleetCrabAngleDeg
    targetBodyTh = phi - BodyToPathAngleDeg = theta
    MultiVehicleScriptEnabled = false
    MultiVehicleAutoEnabled = true       → 进入 TickMultiVehicle 自动分支
    WARMUP                               → 等编队成员就位
    loop:
      TryGetFleetCenterFromSlam()        → 当前车队中心
      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)
      auto branch
      MultiVehicleAuto* → speed + frontTh/rearTh + ideal center
      notify → 从车 SendMotion + POS/Detect 补偿

对照 baseline：PilotDefinition.cs 手动分支 fleetMode == 1（FleetRemote 蟹行）仍直接合成 frontTh/rearTh；自动蟹行当前不走该分支。

---

8. 配置参数速查

8.1 自动蟹行专用（PilotConfig / msConf）

字段	默认	作用
FleetCrabAngleDeg	45	路径方向相对启动时车队朝向的夹角 (deg)。MovementTest 同时把车身-路径夹角设为该值；若输入“路径与小车夹角 x 度”，应填 -x 以保持当前车身角度
FleetCrabLengthMm	2000	路径长度 (mm)
FleetCrabSpeed	0.2	巡航速度 (m/s)，接近终点时由自动蟹行专用减速参数下调
FleetCrabAccel	0.2	速度命令加速度限制 (m/s^2)，限制 MultiVehicleAutoVx 每拍变化量；<=0 表示不限制
FleetCrabSlowDistance	2000	末端开始减速距离 (mm)
FleetCrabFinishDistance	20	完成距离 (mm)，剩余距离低于该值时结束动作
FleetCrabFinishSpeed	0.02	末端最低速度 (m/s)
FleetCrabSlowingPow	0.8	末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速
FleetCrabGcpThetaThreshold	95	自动蟹行输出 frontTh/rearTh 的绝对值上限，应给实际舵角限位与 AngleLimitMarginDeg 留余量

已删除旧字段：FleetCrabCorrectionGain、FleetCrabCorrectionAngleDeg、FleetCrabCommandAccel。旧 clumsy.json 若残留这些 key，会被配置反序列化忽略；新的自动链路使用 FleetCrabAccel 控制 MultiVehicleAutoVx 速度命令斜率。

动作行为：当前不再改 MultiVehicleAutoUseIdealCenter，结束/急停会清零 MultiVehicleAuto*，并保持 MultiVehicleScriptEnabled=false。

8.2 影响跟踪/手感的全局项（节选）

字段	deploy 主车当前值	备注
MultiVehicleSyncUseDetour	true	逐车 SLAM POS 补偿；怀疑与漂移/抖动相关
MultiVehicleUseDetect	false（默认）	true 时互识别安全门
TestCarSyncDistance	2400	与 Playground 双车间距一致
MultiVehicleSyncInterval	50	联动周期 ms
DriveTaskInterval	50	clumsy.json 顶层
BiasFac / BiasThreshold	继承 MultiWheelPilotConfig	横向偏差 lateral → 前后 GCP 同向修正
DthLinearFac / DthLinearThreshold	继承 MultiWheelPilotConfig	车身目标朝向偏差 headingErr → 前后 GCP 反向修正
FleetCrabSlowDistance / FleetCrabSlowingPow / FleetCrabFinishDistance / FleetCrabFinishSpeed	PilotConfig	自动蟹行专用终点减速和结束判定
MultiVehicleAutoUseIdealCenter	true（默认）	使用自动蟹行发布的 ideal center 给从车做前馈
MultiVehicleAutoRequireFleetCenter	true（默认）	自动模式无有效车队中心时整队停车
MultiVehicleAutoCmdTimeoutMs	0（auto）	自动命令新鲜度超时，避免控制器停发后沿末速度滑行

详见 MultiVehicleConfig.md §2–§6。

---

9. 已实现的关键修复（便于回溯）

问题	处理
自动蟹行完全不动 (iter=0)	方案1：PrimeMasterAutoFromSlam + WARMUP 后再 Track()
蟹行要求朝向不变但有纠偏	CrabHoldHeading + CrabTargetHeading；保留 dTh
多车 firstTurn 破坏队形	MultiVehicleSync 时跳过 firstTurnN（TODO 整队预旋转）
gcp 被 45° 上限截断	动作侧 GcpThetaThreshold=95
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. 复测自动蟹行：重点看 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。

---

11. 快速命令备忘

# 编译
dotnet build D:\MDCS\Source\Products\mdcstoolbox\MDCSToolBox.csproj -c Release
dotnet build D:\MDCS\Source\Tutorial\MultiWheel\MultiWheelC\MultiWheelC.csproj

# 查看工作区状态
cd D:\MDCS\Source\Tutorial
git status --short

# 查看最新 FleetCrab 日志（主车，PowerShell）
Get-ChildItem D:\MDCS\Source\Tutorial\build\Medulla\dlog\FleetCrabDbg -ErrorAction SilentlyContinue |
  Sort-Object LastWriteTime -Descending | Select-Object -First 3
Get-ChildItem D:\MDCS\Source\Tutorial\build\Medulla\dlog\CrabDbg -ErrorAction SilentlyContinue |
  Sort-Object LastWriteTime -Descending | Select-Object -First 3