diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs
index b8b96fc..f09ed91 100644
--- a/MultiWheel/MultiWheelC/MovementTests.cs
+++ b/MultiWheel/MultiWheelC/MovementTests.cs
@@ -364,39 +364,32 @@ public class FleetRotateInPlaceTest : MovementTest
}
// ===== 车队联动-自动蟹行动作 =====
-// 以当前车队中心为起点,构造与车队朝向夹角 x、长度 y 的直线路径;执行侧复用
-// TickMultiVehicle 的脚本手动等价输入(mode=1),也就是 FleetRemote 手动蟹行同一条下发链路。
+// 以当前车队中心为起点,构造指定方向和长度的直线路径;
+// 执行侧直接写 MultiVehicleAuto...,由 TickMultiVehicle 自动分支统一下发。
//
-// 手动蟹行已验证丝滑,自动动作只额外做两件事:
-// 1) 读取主车 Detour 反推车队中心,计算沿直线的进度和横向偏差;
-// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy,避免几何控制器 bias/dTh 阶跃造成抖动。
-//
-// 与 FleetRemote 手动蟹行(mode==1)对照:TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
+// 控制思路参考 MDCSToolbox 几何控制器,但实现收在 MultiWheelC 内:
+// 1) 读取主车 Detour 反推车队中心,计算沿直线的进度、横向偏差和车身目标朝向偏差;
+// 2) 根据横向偏差给前后 GCP 同向修正,根据车身目标朝向偏差给前后 GCP 反向修正;
+// 3) 根据终点距离减速,并发布 ideal fleet center 给从车做前馈。
//
// 前提:在主车(MultiVehicleMasterEndpoint=="/")运行,且主车有 Detour 定位。
public class FleetCrabWalk : MovementDefinition
{
- /// 与当前车队朝向的夹角(deg,逆时针为正)。
+ /// 路径方向相对启动时车队朝向的夹角(deg,逆时针为正)。
public float CrabAngleDeg = 45f;
+ /// 路径方向相对车身目标朝向的夹角(deg,逆时针为正)。MovementTest 会设为 CrabAngleDeg,以保持启动时车身朝向。
+ public float BodyToPathAngleDeg = 45f;
+
/// 路径长度(mm)。
public float CrabLengthMm = 2000f;
/// 行驶速度(m/s)。
public float CrabSpeed = 0.2f;
- /// 兼容旧配置;当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。
+ /// 前后 GCP 舵角修正上限(deg)。
public float GcpThetaThreshold = 95f;
- /// 横向误差转向增益,沿用 Stanley 形式:atan(gain * lateral / speed)。
- public float CorrectionGain = 1f;
-
- /// 自动纠偏最大改向角(deg)。越小越接近手动蟹行,越大收敛越快。
- public float CorrectionAngleThreshold = 8f;
-
- /// 脚本 Vx/Vy 命令斜率限制(m/s^2),避免纠偏量变化造成舵角阶跃。
- public float CommandAccel = 0.4f;
-
private bool _stopping;
private void Cleanup()
@@ -420,11 +413,20 @@ public class FleetCrabWalk : MovementDefinition
Cleanup();
}
- private static float Slew(float current, float target, float maxStep)
+ private static float Clamp(float value, float min, float max)
{
- var diff = target - current;
- if (Math.Abs(diff) <= maxStep) return target;
- return current + Math.Sign(diff) * maxStep;
+ if (value < min) return min;
+ if (value > max) return max;
+ return value;
+ }
+
+ private static float ClampAbs(float value, float limit)
+ {
+ var absLimit = Math.Abs(limit);
+ if (absLimit <= 0) return value;
+ if (value > absLimit) return absLimit;
+ if (value < -absLimit) return -absLimit;
+ return value;
}
public override IEnumerable Get()
@@ -437,7 +439,8 @@ public class FleetCrabWalk : MovementDefinition
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
- $"manualLike=true corrGain={CorrectionGain:0.00} corrMax={CorrectionAngleThreshold:0.0} accel={CommandAccel:0.00}",
+ $"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+ $"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
"FleetCrabDbg");
if (conf.MultiVehicleMasterEndpoint != "/")
@@ -458,25 +461,34 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+ var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
var phiRad = phi / 180.0 * Math.PI;
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
DLog.Log(
- $"START center=({x0:0},{y0:0},{theta:0.0}) crabAngle={CrabAngleDeg:0.0} phi={phi:0.0} " +
+ $"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+ $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
$"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}",
"FleetCrabDbg");
- // 手动蟹行已验证丝滑:这里复用 TickMultiVehicle 的脚本手动等价输入(mode=1),
- // 只在本动作内根据车队中心相对直线的横向误差缓慢调整 Vx/Vy 方向。
- self.MultiVehicleScriptEnabled = true;
- self.MultiVehicleScriptMode = 1;
+ self.MultiVehicleScriptEnabled = false;
+ self.MultiVehicleScriptMode = 0;
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoFrontTh = 0;
+ self.MultiVehicleAutoRearTh = 0;
+ self.MultiVehicleAutoIdealX = x0;
+ self.MultiVehicleAutoIdealY = y0;
+ self.MultiVehicleAutoIdealTh = targetBodyTh;
+ self.MultiVehicleAutoHasIdeal = true;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
self.PrimeMasterAutoFromSlam();
- DLog.Log("WARMUP 已启用脚本蟹行(mode=1),等待编队成员就位…", "FleetCrabDbg");
+ DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");
var warmEnd = DateTime.Now.AddSeconds(2.0);
var warmIter = 0;
@@ -484,8 +496,17 @@ public class FleetCrabWalk : MovementDefinition
while (!_stopping && DateTime.Now < warmEnd)
{
warmIter++;
- self.MultiVehicleScriptEnabled = true;
- self.MultiVehicleScriptMode = 1;
+ self.MultiVehicleScriptEnabled = false;
+ self.MultiVehicleScriptMode = 0;
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoFrontTh = 0;
+ self.MultiVehicleAutoRearTh = 0;
+ self.MultiVehicleAutoIdealX = x0;
+ self.MultiVehicleAutoIdealY = y0;
+ self.MultiVehicleAutoIdealTh = targetBodyTh;
+ self.MultiVehicleAutoHasIdeal = true;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
self.PrimeMasterAutoFromSlam();
var snap = self.GetFleetCenterSnapshot();
int cnt;
@@ -493,7 +514,7 @@ public class FleetCrabWalk : MovementDefinition
if (warmIter % 5 == 0)
DLog.Log(
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
- $"scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
+ $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg");
if (cnt >= conf.MultiVehicleFleetNum)
{
@@ -506,27 +527,30 @@ public class FleetCrabWalk : MovementDefinition
yield return true;
}
if (!warmReady)
- DLog.Log("WARMUP 超时:编队仍未就位,继续进入脚本蟹行(若不动请查看 FleetDiagClumsy ready/cnt)",
+ DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
"FleetCrabDbg");
- Hedingben.ToastText($"车队蟹行 夹角{CrabAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
+ Hedingben.ToastText($"车队蟹行 路径{CrabAngleDeg:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
var iter = 0;
- var cmdVx = 0f;
- var cmdVy = 0f;
- var lastTime = DateTime.Now;
var lastLog = DateTime.MinValue;
var finishDistance = Math.Max(20f, conf.FinishDistance);
+ var slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance);
+ var baseSpeed = Math.Abs(CrabSpeed);
+ var finishSpeed = Math.Min(baseSpeed, Math.Abs(conf.FinishSpeed));
+ var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
var stopReason = "done";
while (!_stopping)
{
iter++;
- var now = DateTime.Now;
- var dt = (float)Math.Min(0.2, Math.Max(0.001, (now - lastTime).TotalSeconds));
- lastTime = now;
- self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth);
+ if (!self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth))
+ {
+ stopReason = "fleet center invalid";
+ DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
+ break;
+ }
var delta = new Vector2(cx - x0, cy - y0);
var along = Vector2.Dot(delta, pathDir);
var lateral = Vector2.Dot(delta, pathLeft);
@@ -534,29 +558,37 @@ public class FleetCrabWalk : MovementDefinition
if (remain <= finishDistance)
break;
- var speed = Math.Abs(CrabSpeed);
- if (remain < conf.SlowDistance && conf.SlowDistance > 1)
+ var speed = baseSpeed;
+ if (remain < slowDistance)
{
- var ratio = (float)Math.Pow(Math.Max(0, remain) / conf.SlowDistance, conf.SlowingPow);
- speed = ratio * (speed - Math.Abs(conf.FinishSpeed)) + Math.Abs(conf.FinishSpeed);
+ var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
+ speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
}
- var correction = (float)(-Math.Atan(CorrectionGain * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
- correction = Math.Sign(correction) * Math.Min(Math.Abs(correction), Math.Abs(CorrectionAngleThreshold));
- var desiredWorldAngle = CommonMath.RoundTh(phi + correction);
- var localAngle = (float)CommonMath.ThDiff(desiredWorldAngle, cth);
- var localRad = localAngle / 180.0 * Math.PI;
- var targetVx = speed * (float)Math.Cos(localRad);
- var targetVy = speed * (float)Math.Sin(localRad);
- var maxStep = Math.Max(0.05f, CommandAccel) * dt;
- cmdVx = Slew(cmdVx, targetVx, maxStep);
- cmdVy = Slew(cmdVy, targetVy, maxStep);
+ var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
+ var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
+ var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
+ var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+ biasItem = ClampAbs(biasItem, conf.BiasThreshold);
+ var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
+ var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
+ var idealAlong = Clamp(along, 0f, CrabLengthMm);
+ var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
- self.MultiVehicleScriptEnabled = true;
- self.MultiVehicleScriptMode = 1;
- self.MultiVehicleScriptVx = cmdVx;
- self.MultiVehicleScriptVy = cmdVy;
+ self.MultiVehicleScriptEnabled = false;
+ self.MultiVehicleScriptMode = 0;
+ self.MultiVehicleScriptVx = 0;
+ self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = speed;
+ self.MultiVehicleAutoFrontTh = frontTh;
+ self.MultiVehicleAutoRearTh = rearTh;
+ self.MultiVehicleAutoIdealX = ideal.X;
+ self.MultiVehicleAutoIdealY = ideal.Y;
+ self.MultiVehicleAutoIdealTh = targetBodyTh;
+ self.MultiVehicleAutoHasIdeal = true;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
{
@@ -566,8 +598,11 @@ public class FleetCrabWalk : MovementDefinition
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
DLog.Log(
$"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
- $"along={along:0} lateral={lateral:0} remain={remain:0} corr={correction:0.0} " +
- $"localAngle={localAngle:0.0} cmd=({cmdVx:0.000},{cmdVy:0.000}) cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
+ $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
+ $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
+ $"auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
+ $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
+ $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg");
}
yield return true;
@@ -576,14 +611,20 @@ public class FleetCrabWalk : MovementDefinition
if (_stopping)
stopReason = "stop";
- self.MultiVehicleScriptVx = 0;
- self.MultiVehicleScriptVy = 0;
- self.MultiVehicleScriptVth = 0;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoFrontTh = 0;
+ self.MultiVehicleAutoRearTh = 0;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
while (!_stopping && DateTime.Now < settleEnd)
{
- self.MultiVehicleScriptEnabled = true;
- self.MultiVehicleScriptMode = 1;
+ self.MultiVehicleScriptEnabled = false;
+ self.MultiVehicleScriptMode = 0;
+ self.MultiVehicleAutoEnabled = true;
+ self.MultiVehicleAutoVx = 0;
+ self.MultiVehicleAutoFrontTh = 0;
+ self.MultiVehicleAutoRearTh = 0;
+ self.MultiVehicleAutoCmdTime = DateTime.Now;
yield return true;
}
@@ -604,12 +645,10 @@ public class FleetCrabWalkTest : MovementTest
_proc = new FleetCrabWalk
{
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
+ BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
- GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
- CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain,
- CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg,
- CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel
+ GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
};
_task = new DriveTask(_proc.Get());
_task.Wait();
diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs
index 1a41616..912e607 100644
--- a/MultiWheel/MultiWheelC/PilotConfig.cs
+++ b/MultiWheel/MultiWheelC/PilotConfig.cs
@@ -23,6 +23,8 @@ public class PilotConfig : MultiWheelPilotConfig
// 注意:无论该开关如何,自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位;
// 主车自动模式必调用 getCartLocation(),若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度(安全停车)。
[FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
+ // 手动外部遥控联动默认只走 2 腿检测/几何同步,避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
+ [FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
[FieldMember(desc = "多车联动:总车数")] public int MultiVehicleFleetNum = 2;
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
@@ -96,6 +98,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "Playground WebAPI 基地址")]
public string PlaygroundWebApiUrl = "http://localhost:18090";
+ [FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
+ public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
+
[FieldMember(desc = "Playground 小车名称(场景 robots[].name)")]
public string PlaygroundRobotName = "agv_multi_1";
@@ -149,9 +154,9 @@ public class PilotConfig : MultiWheelPilotConfig
public bool FleetRotateUseDetourHeading = true;
// ===== 车队联动-自动蟹行(FleetCrabWalk)=====
- // 以当前车队中心为起点,构造与车队朝向夹角 FleetCrabAngleDeg、长度 FleetCrabLengthMm 的直线路径,
- // 复用脚本手动等价输入(mode=1)斜向平移;动作侧只把横向误差转换成小幅、带斜率限制的蟹行方向修正。
- [FieldMember(desc = "车队蟹行:与车队朝向夹角(deg,逆时针为正)")]
+ // 以当前车队中心为起点,构造一条直线路径;MovementTest 中车身保持启动朝向追踪该路径。
+ // 动作侧参考几何控制器的路径跟踪思路,直接写入 MultiVehicleAuto... 字段,不再复用脚本手动链路。
+ [FieldMember(desc = "车队蟹行:路径方向相对启动时车队朝向夹角(deg,逆时针为正;路径在车右侧x度时填-x)")]
public float FleetCrabAngleDeg = 45f;
[FieldMember(desc = "车队蟹行:路径长度(mm)")]
@@ -160,19 +165,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "车队蟹行:行驶速度(m/s)")]
public float FleetCrabSpeed = 0.2f;
- // 兼容旧版几何控制器实现;当前自动蟹行走脚本手动等价输入,不再直接使用该上限。
- [FieldMember(desc = "车队蟹行:旧几何控制器gcp角度上限(deg)")]
+ [FieldMember(desc = "车队蟹行:GCP舵角修正上限(deg)")]
public float FleetCrabGcpThetaThreshold = 95f;
- [FieldMember(desc = "车队蟹行:横向误差纠偏增益")]
- public float FleetCrabCorrectionGain = 1f;
-
- [FieldMember(desc = "车队蟹行:自动纠偏最大改向角(deg)")]
- public float FleetCrabCorrectionAngleDeg = 8f;
-
- [FieldMember(desc = "车队蟹行:脚本速度命令斜率(m/s^2)")]
- public float FleetCrabCommandAccel = 0.4f;
-
// ===== 2腿检测(单线雷达识别两腿托盘 / 轮胎)=====
[FieldMember(desc = "2腿检测:雷达名(逗号分隔可多个)")]
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs
index 5fc8996..4128399 100644
--- a/MultiWheel/MultiWheelC/PilotDefinition.cs
+++ b/MultiWheel/MultiWheelC/PilotDefinition.cs
@@ -176,6 +176,7 @@ public class PilotDefinition : MultiWheelPilotDefinition();
+ for (var i = 0; i < wheels.Count; i++)
+ {
+ var wheel = wheels[i];
+ if (wheel is DiffSteerWheel diff)
+ {
+ parts.Add(
+ $"w{i}:tgt={diff.GetSendAngle():0.0} read={diff.ReadAngle():0.0} " +
+ $"L={diff.GetLeftSendSpeed():0.000} R={diff.GetRightSendSpeed():0.000}");
+ }
+ else
+ {
+ parts.Add(
+ $"w{i}:tgt={wheel.GetSendAngle():0.0} read={wheel.ReadAngle():0.0} " +
+ $"v={wheel.GetSendSpeed():0.000}");
+ }
+ }
+
+ DLog.Log(
+ $"car{CarNum} phase={phase} omega={omega:0.000} " + string.Join(" | ", parts),
+ "MultiVehicleRotateWheelOutput");
+ }
+
private void SetMultiVehicleMotionFeasible(bool feasible, string reason = "")
{
_multiVehicleMotionFeasible = feasible;
@@ -598,7 +631,8 @@ public class PilotDefinition : MultiWheelPilotDefinition
private void LogRotatePoseSample()
{
+ if (!Conf.MultiVehicleRotatePoseWebApiDiagEnabled) return;
+
var now = DateTime.Now;
if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return;
_rotPoseLastLog = now;
diff --git a/docs/FleetCrabWalkWorkContext.md b/docs/FleetCrabWalkWorkContext.md
index 7bd7bc1..0970702 100644
--- a/docs/FleetCrabWalkWorkContext.md
+++ b/docs/FleetCrabWalkWorkContext.md
@@ -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。
---
diff --git a/docs/MultiVehicleConfig.md b/docs/MultiVehicleConfig.md
index 4d76524..75c8f0e 100644
--- a/docs/MultiVehicleConfig.md
+++ b/docs/MultiVehicleConfig.md
@@ -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