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
MultiWheel/MultiWheelC/MovementTests.cs
+109 -70
View File
@@ -364,39 +364,32 @@ public class FleetRotateInPlaceTest : MovementTest
} }
// ===== 车队联动-自动蟹行动作 ===== // ===== 车队联动-自动蟹行动作 =====
// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用 // 以当前车队中心为起点，构造指定方向和长度的直线路径；
// TickMultiVehicle 的脚本手动等价输入(mode=1)，也就是 FleetRemote 手动蟹行同一条下发链路。 // 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
// //
// 手动蟹行已验证丝滑，自动动作只额外做两件事： // 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度和横向偏差； // 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy，避免几何控制器 bias/dTh 阶跃造成抖动。 // 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
// // 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
// 与 FleetRemote 手动蟹行(mode==1)对照：TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
// //
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。 // 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
public class FleetCrabWalk : MovementDefinition public class FleetCrabWalk : MovementDefinition
{ {
/// <summary>与当前车队朝向的夹角(deg，逆时针为正)。</summary> /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
public float CrabAngleDeg = 45f; public float CrabAngleDeg = 45f;
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
public float BodyToPathAngleDeg = 45f;
/// <summary>路径长度(mm)。</summary> /// <summary>路径长度(mm)。</summary>
public float CrabLengthMm = 2000f; public float CrabLengthMm = 2000f;
/// <summary>行驶速度(m/s)。</summary> /// <summary>行驶速度(m/s)。</summary>
public float CrabSpeed = 0.2f; public float CrabSpeed = 0.2f;
/// <summary>兼容旧配置；当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。</summary> /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
public float GcpThetaThreshold = 95f; public float GcpThetaThreshold = 95f;
/// <summary>横向误差转向增益，沿用 Stanley 形式：atan(gain * lateral / speed)。</summary>
public float CorrectionGain = 1f;
/// <summary>自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大收敛越快。</summary>
public float CorrectionAngleThreshold = 8f;
/// <summary>脚本 Vx/Vy 命令斜率限制(m/s^2)，避免纠偏量变化造成舵角阶跃。</summary>
public float CommandAccel = 0.4f;
private bool _stopping; private bool _stopping;
private void Cleanup() private void Cleanup()
@@ -420,11 +413,20 @@ public class FleetCrabWalk : MovementDefinition
Cleanup(); 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 (value < min) return min;
if (Math.Abs(diff) <= maxStep) return target; if (value > max) return max;
return current + Math.Sign(diff) * maxStep; 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<bool> Get() public override IEnumerable<bool> Get()
@@ -437,7 +439,8 @@ public class FleetCrabWalk : MovementDefinition
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " + $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " + $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " + $"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"); "FleetCrabDbg");
if (conf.MultiVehicleMasterEndpoint != "/") if (conf.MultiVehicleMasterEndpoint != "/")
@@ -458,25 +461,34 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg"); DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
var phi = CommonMath.RoundTh(theta + CrabAngleDeg); 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 dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
var phiRad = phi / 180.0 * Math.PI; var phiRad = phi / 180.0 * Math.PI;
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad)); var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
var pathLeft = new Vector2(-pathDir.Y, pathDir.X); var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
DLog.Log( 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}", $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}",
"FleetCrabDbg"); "FleetCrabDbg");
// 手动蟹行已验证丝滑：这里复用 TickMultiVehicle 的脚本手动等价输入(mode=1)， self.MultiVehicleScriptEnabled = false;
// 只在本动作内根据车队中心相对直线的横向误差缓慢调整 Vx/Vy 方向。 self.MultiVehicleScriptMode = 0;
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptVx = 0; self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0; self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 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(); 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 warmEnd = DateTime.Now.AddSeconds(2.0);
var warmIter = 0; var warmIter = 0;
@@ -484,8 +496,17 @@ public class FleetCrabWalk : MovementDefinition
while (!_stopping && DateTime.Now < warmEnd) while (!_stopping && DateTime.Now < warmEnd)
{ {
warmIter++; warmIter++;
self.MultiVehicleScriptEnabled = true; self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 1; 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(); self.PrimeMasterAutoFromSlam();
var snap = self.GetFleetCenterSnapshot(); var snap = self.GetFleetCenterSnapshot();
int cnt; int cnt;
@@ -493,7 +514,7 @@ public class FleetCrabWalk : MovementDefinition
if (warmIter % 5 == 0) if (warmIter % 5 == 0)
DLog.Log( DLog.Log(
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " + $"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"); "FleetCrabDbg");
if (cnt >= conf.MultiVehicleFleetNum) if (cnt >= conf.MultiVehicleFleetNum)
{ {
@@ -506,27 +527,30 @@ public class FleetCrabWalk : MovementDefinition
yield return true; yield return true;
} }
if (!warmReady) 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"); "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 iter = 0;
var cmdVx = 0f;
var cmdVy = 0f;
var lastTime = DateTime.Now;
var lastLog = DateTime.MinValue; var lastLog = DateTime.MinValue;
var finishDistance = Math.Max(20f, conf.FinishDistance); 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"; var stopReason = "done";
while (!_stopping) while (!_stopping)
{ {
iter++; 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 delta = new Vector2(cx - x0, cy - y0);
var along = Vector2.Dot(delta, pathDir); var along = Vector2.Dot(delta, pathDir);
var lateral = Vector2.Dot(delta, pathLeft); var lateral = Vector2.Dot(delta, pathLeft);
@@ -534,29 +558,37 @@ public class FleetCrabWalk : MovementDefinition
if (remain <= finishDistance) if (remain <= finishDistance)
break; break;
var speed = Math.Abs(CrabSpeed); var speed = baseSpeed;
if (remain < conf.SlowDistance && conf.SlowDistance > 1) if (remain < slowDistance)
{ {
var ratio = (float)Math.Pow(Math.Max(0, remain) / conf.SlowDistance, conf.SlowingPow); var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
speed = ratio * (speed - Math.Abs(conf.FinishSpeed)) + Math.Abs(conf.FinishSpeed); speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
} }
var correction = (float)(-Math.Atan(CorrectionGain * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0); var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
correction = Math.Sign(correction) * Math.Min(Math.Abs(correction), Math.Abs(CorrectionAngleThreshold)); var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
var desiredWorldAngle = CommonMath.RoundTh(phi + correction); var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
var localAngle = (float)CommonMath.ThDiff(desiredWorldAngle, cth); var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
var localRad = localAngle / 180.0 * Math.PI; biasItem = ClampAbs(biasItem, conf.BiasThreshold);
var targetVx = speed * (float)Math.Cos(localRad); var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
var targetVy = speed * (float)Math.Sin(localRad); var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
var maxStep = Math.Max(0.05f, CommandAccel) * dt; var idealAlong = Clamp(along, 0f, CrabLengthMm);
cmdVx = Slew(cmdVx, targetVx, maxStep); var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
cmdVy = Slew(cmdVy, targetVy, maxStep);
self.MultiVehicleScriptEnabled = true; self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 1; self.MultiVehicleScriptMode = 0;
self.MultiVehicleScriptVx = cmdVx; self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = cmdVy; self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 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) if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
{ {
@@ -566,8 +598,11 @@ public class FleetCrabWalk : MovementDefinition
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count; lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
DLog.Log( DLog.Log(
$"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " + $"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} " + $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
$"localAngle={localAngle:0.0} cmd=({cmdVx:0.000},{cmdVy:0.000}) cnt={fleetCnt}/{conf.MultiVehicleFleetNum}", $"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"); "FleetCrabDbg");
} }
yield return true; yield return true;
@@ -576,14 +611,20 @@ public class FleetCrabWalk : MovementDefinition
if (_stopping) if (_stopping)
stopReason = "stop"; stopReason = "stop";
self.MultiVehicleScriptVx = 0; self.MultiVehicleAutoVx = 0;
self.MultiVehicleScriptVy = 0; self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleScriptVth = 0; self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoCmdTime = DateTime.Now;
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3)); var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
while (!_stopping && DateTime.Now < settleEnd) while (!_stopping && DateTime.Now < settleEnd)
{ {
self.MultiVehicleScriptEnabled = true; self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 1; self.MultiVehicleScriptMode = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoCmdTime = DateTime.Now;
yield return true; yield return true;
} }
@@ -604,12 +645,10 @@ public class FleetCrabWalkTest : MovementTest
_proc = new FleetCrabWalk _proc = new FleetCrabWalk
{ {
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg, CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm, CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed, CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold, GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain,
CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg,
CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel
}; };
_task = new DriveTask(_proc.Get()); _task = new DriveTask(_proc.Get());
_task.Wait(); _task.Wait();
MultiWheel/MultiWheelC/PilotConfig.cs
+9 -14
View File
@@ -23,6 +23,8 @@ public class PilotConfig : MultiWheelPilotConfig
// 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位； // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
// 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。 // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
[FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false; [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 = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50; [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
@@ -96,6 +98,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "Playground WebAPI 基地址")] [FieldMember(desc = "Playground WebAPI 基地址")]
public string PlaygroundWebApiUrl = "http://localhost:18090"; public string PlaygroundWebApiUrl = "http://localhost:18090";
[FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
[FieldMember(desc = "Playground 小车名称（场景 robots[].name）")] [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
public string PlaygroundRobotName = "agv_multi_1"; public string PlaygroundRobotName = "agv_multi_1";
@@ -149,9 +154,9 @@ public class PilotConfig : MultiWheelPilotConfig
public bool FleetRotateUseDetourHeading = true; public bool FleetRotateUseDetourHeading = true;
// ===== 车队联动-自动蟹行（FleetCrabWalk）===== // ===== 车队联动-自动蟹行（FleetCrabWalk）=====
// 以当前车队中心为起点，构造与车队朝向夹角 FleetCrabAngleDeg、长度 FleetCrabLengthMm 的直线路径， // 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
// 复用脚本手动等价输入(mode=1)斜向平移；动作侧只把横向误差转换成小幅、带斜率限制的蟹行方向修正。 // 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
[FieldMember(desc = "车队蟹行：与车队朝向夹角(deg，逆时针为正)")] [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
public float FleetCrabAngleDeg = 45f; public float FleetCrabAngleDeg = 45f;
[FieldMember(desc = "车队蟹行：路径长度(mm)")] [FieldMember(desc = "车队蟹行：路径长度(mm)")]
@@ -160,19 +165,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")] [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
public float FleetCrabSpeed = 0.2f; public float FleetCrabSpeed = 0.2f;
// 兼容旧版几何控制器实现；当前自动蟹行走脚本手动等价输入，不再直接使用该上限。 [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
[FieldMember(desc = "车队蟹行：旧几何控制器gcp角度上限(deg)")]
public float FleetCrabGcpThetaThreshold = 95f; 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腿检测（单线雷达识别两腿托盘 / 轮胎）===== // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")] [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1"; public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
MultiWheel/MultiWheelC/PilotDefinition.cs
+73 -20
View File
@@ -176,6 +176,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
private DateTime _mvRemoteInputLastLog = DateTime.MinValue; private DateTime _mvRemoteInputLastLog = DateTime.MinValue;
private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue; private DateTime _mvRemoteDecisionLastLog = DateTime.MinValue;
private DateTime _mvNotifyApplyLastLog = DateTime.MinValue; private DateTime _mvNotifyApplyLastLog = DateTime.MinValue;
private DateTime _mvRotateWheelOutputLastLog = DateTime.MinValue;
private void LogMultiVehicleRemoteInput(bool isMaster, bool scriptOn, bool manualEnabled, bool autoEnabled, private void LogMultiVehicleRemoteInput(bool isMaster, bool scriptOn, bool manualEnabled, bool autoEnabled,
int manualMode, float manualVx, float manualVy, float manualVth) int manualMode, float manualVx, float manualVy, float manualVth)
@@ -207,6 +208,38 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
DLog.Log($"car{CarNum} {msg}", "MultiVehicleRemoteDbg"); DLog.Log($"car{CarNum} {msg}", "MultiVehicleRemoteDbg");
} }
private void LogRotateWheelOutputs(MultiWheelChassis chassis, string phase, float omega, bool force = false)
{
var now = DateTime.Now;
if (!force && (now - _mvRotateWheelOutputLastLog).TotalMilliseconds < 300) return;
_mvRotateWheelOutputLastLog = now;
#pragma warning disable CS0612, CS0618
var wheels = chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618
var parts = new List<string>();
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 = "") private void SetMultiVehicleMotionFeasible(bool feasible, string reason = "")
{ {
_multiVehicleMotionFeasible = feasible; _multiVehicleMotionFeasible = feasible;
@@ -598,7 +631,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
$"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " + $"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " + $"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " + $"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
$"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} useDetour={Conf.MultiVehicleSyncUseDetour}"); $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} " +
$"useDetour={Conf.MultiVehicleSyncUseDetour} manualDetour={Conf.MultiVehicleManualUseDetourCorrection}");
} }
if (!manualEnabled && !autoEnabled) if (!manualEnabled && !autoEnabled)
@@ -650,9 +684,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
// false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。 // false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
// - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、 // - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
// SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关； // SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
// 纠偏开启时本车也读。读取若因无有效定位阻塞，则联动线程随之阻塞、不下发速度（安全停车）。 // 手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；确需手动 POS 纠偏时再开
// MultiVehicleManualUseDetourCorrection。
var autoMode = autoEnabled && !manualEnabled; var autoMode = autoEnabled && !manualEnabled;
var useDetourCorrection = Conf.MultiVehicleSyncUseDetour; var manualDetourCorrection = manualEnabled && Conf.MultiVehicleManualUseDetourCorrection;
var useDetourCorrection = Conf.MultiVehicleSyncUseDetour && (!manualEnabled || manualDetourCorrection);
var slamRead = useDetourCorrection || (isMaster && autoMode); var slamRead = useDetourCorrection || (isMaster && autoMode);
float selfX = 0, selfY = 0, selfTh = 0; float selfX = 0, selfY = 0, selfTh = 0;
if (slamRead) if (slamRead)
@@ -689,8 +725,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
if (fleetMode == 2) if (fleetMode == 2)
{ {
// 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。 // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
var rotateInput = scriptOn ? manualVth : ClampFloat(manualVth, -1f, 1f); fleetOmega = manualVth;
fleetOmega = scriptOn ? rotateInput : rotateInput * Conf.ManualCarSyncVthFac;
fleetVx = 0; fleetVx = 0;
fleetFrontTh = 0; fleetFrontTh = 0;
fleetRearTh = 0; fleetRearTh = 0;
@@ -701,7 +736,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
{ {
// 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。 // 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。
var speed = manualVx * Conf.ManualCarSyncVxFac; var speed = manualVx * Conf.ManualCarSyncVxFac;
var crabRatio = Math.Max(-60f, Math.Min(60f, manualVy)); var crabRatio = Math.Max(-90f, Math.Min(90f, manualVy));
var crabAngle = crabRatio * Conf.ManualCarSyncVyFac; var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
crabInputVx = speed; crabInputVx = speed;
crabInputVy = crabRatio; crabInputVy = crabRatio;
@@ -1125,27 +1160,41 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
rotCompVx, rotCompVy, rotCompOmega); rotCompVx, rotCompVy, rotCompOmega);
} }
else if (rotating) else if (rotating)
{
// Preparation calls SendRotateMotion with zero delta; after release it would starve the speed ramp.
if (!MultiVehicleRotateWheelsReady)
{ {
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
rotCompVx, rotCompVy, rotCompOmega, rampStop: false); rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
if (motionOk && MultiVehicleRotateWheelsReady) rotateHoldForAlignment = true;
fleetOmega = 0;
chassis.RampStop();
if (!motionOk || !MultiVehicleRotateWheelsReady)
{
LogMultiVehicleRemoteDecision(
$"ROTATE_HOLD_LOCAL_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
}
else
{
LogMultiVehicleRemoteDecision(
$"ROTATE_HOLD_LOCAL_READY master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true);
}
}
else
{ {
motionOk = chassis.SendRotateMotion(fleetOmega, motionOk = chassis.SendRotateMotion(fleetOmega,
localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateX: rotCompVx, localCompensateY: rotCompVy,
localCompensateTh: rotCompOmega); localCompensateTh: rotCompOmega);
MultiVehicleRotateWheelsReady = motionOk && var activeAligned = TryCheckRotateWheelAlignment(chassis,
TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg, out _multiVehicleRotateAlignDetail);
Conf.InPlaceRotateWheelAlignDeg, if (!motionOk)
out _multiVehicleRotateAlignDetail); MultiVehicleRotateWheelsReady = false;
} if (!activeAligned)
else
{
rotateHoldForAlignment = true;
fleetOmega = 0;
chassis.RampStop();
LogMultiVehicleRemoteDecision( LogMultiVehicleRemoteDecision(
$"ROTATE_HOLD_LOCAL_ALIGN master={isMaster} manual={manualEnabled} auto={autoEnabled} " + $"ROTATE_ACTIVE_ALIGN_WAIT master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}", true); $"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}");
} }
} }
else else
@@ -1165,6 +1214,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
canMove = false; canMove = false;
LogMultiVehicleStop(fleetStopReason, true); LogMultiVehicleStop(fleetStopReason, true);
} }
LogRotateWheelOutputs(chassis, rotateHoldForAlignment ? "hold-align" : (rotating ? "rotate" : "idle"),
fleetOmega);
LogMultiVehicleRemoteDecision( LogMultiVehicleRemoteDecision(
$"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " + $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
$"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " + $"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " +
@@ -1173,7 +1224,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
$"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}"); $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
// 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。 // 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
if (isMaster) if (isMaster && Conf.MultiVehicleRotatePoseWebApiDiagEnabled)
LogRotatePoseSample(); LogRotatePoseSample();
} }
else else
@@ -1653,6 +1704,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
/// </summary> /// </summary>
private void LogRotatePoseSample() private void LogRotatePoseSample()
{ {
if (!Conf.MultiVehicleRotatePoseWebApiDiagEnabled) return;
var now = DateTime.Now; var now = DateTime.Now;
if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return; if ((now - _rotPoseLastLog).TotalMilliseconds < 200) return;
_rotPoseLastLog = now; _rotPoseLastLog = now;
docs/FleetCrabWalkWorkContext.md
+55 -46
View File
@@ -2,7 +2,7 @@
> 本文档汇总 **当前仓库状态、外部依赖、运行/日志路径、代码地图与待解决问题**，便于后续继续调试「车队联动-自动蟹行」。 > 本文档汇总 **当前仓库状态、外部依赖、运行/日志路径、代码地图与待解决问题**，便于后续继续调试「车队联动-自动蟹行」。
> >
> 最后更新：2026-06-29 > 最后更新：2026-07-01
--- ---
@@ -11,8 +11,8 @@
| 阶段 | 状态 | 说明 | | 阶段 | 状态 | 说明 |
|------|------|------| |------|------|------|
| 动作能启动、能下发运动 | ✅ 已解决 | 方案 1（预热）修复了启动期 `(0,0,0)` 快照导致 `Track()` 立即结束（`iter=0`）的问题 | | 动作能启动、能下发运动 | ✅ 已解决 | 方案 1（预热）修复了启动期 `(0,0,0)` 快照导致 `Track()` 立即结束（`iter=0`）的问题 |
| 路径跟踪质量 | 🔧 已改，待实测 | 2026-06-29 继续处理：自动蟹行改为复用手动蟹行同款 `mode=1` 下发链路，只叠加小幅平滑横向纠偏 | | 路径跟踪质量 | 🔧 已改，待实测 | 2026-07-01 改为自动字段链路；MovementTest 中 `FleetCrabAngleDeg=-x` 表示车身保持当前角度，以 x 度夹角追踪路径 |
| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）丝滑；因此自动抖动主要来自纠偏链路而非底盘执行能力 | | 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）仍保留；自动蟹行不再复用脚本手动链路 |
**触发方式**：主车 Clumsy → MovementTest 面板 → **「车队联动-自动蟹行」**（`FleetCrabWalkTest`）。 **触发方式**：主车 Clumsy → MovementTest 面板 → **「车队联动-自动蟹行」**（`FleetCrabWalkTest`）。
@@ -36,37 +36,39 @@
- 已处理：`TickMultiVehicle` 每拍开头的 `PublishFleetCenter(0,0,0)` 已移除，避免动作/控制线程并发读到假中心。 - 已处理：`TickMultiVehicle` 每拍开头的 `PublishFleetCenter(0,0,0)` 已移除，避免动作/控制线程并发读到假中心。
2. **横向纠偏 `bias` 项在蟹行模式下的参考系** 2. **横向纠偏 `bias` 项在蟹行模式下的参考系**
- `MultiWheelGeometricController.PerformGoing` 中 `bias = -bias` 后按 Stanley 形式修正 gcp（`BiasFac` / `BiasThreshold`）。 - 当前实现不改 MDCSToolbox，只参考几何控制器思路在 `MultiWheelC` 内计算。
- 蟹行时 `thDiff` 来自路径切线（≈夹角），`dTh` 参考固定 `CrabTargetHeading`；若 `bias` 符号或 fleet 中心更新滞后，会持续向一侧推。 - `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向修正。
- 已绕开：`FleetCrabWalk` 当前不再用几何控制器直接下发 gcp；改为 Detour 计算 `along/lateral/remain`，再写脚本 `MultiVehicleScriptVx/Vy`。 - 输出直接写 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/Y/Th`，由 `TickMultiVehicle` 自动分支统一下发。
- MovementTest 会令 `BodyToPathAngleDeg = FleetCrabAngleDeg`，因此 `targetBodyTh = pathTh - BodyToPathAngleDeg = 启动时车队朝向`。
3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘** 3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘**
- 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。 - 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。
- 各车 SLAM 偏差经 `PosBias*` 叠加到 `SendMotion`，可能与几何控制器横向纠偏形成耦合振荡。 - 各车 SLAM 偏差经 `PosBias*` 叠加到 `SendMotion`，可能与几何控制器横向纠偏形成耦合振荡。
4. **动作期间关闭了 `MultiVehicleAutoUseIdealCenter`** 4. **理想车队中心前馈**
- 有意为之（避免 ideal 中心回灌快照、抹平真实 bias）。副作用是仅依赖「快照中心 + bias 闭环」，对快照质量更敏感。 - 当前动作会发布 `MultiVehicleAutoIdealX/Y/Th`。
- `MultiVehicleAutoUseIdealCenter=true` 时，从车使用该理想中心做 layout 前馈；关闭后只用当前广播中心和补偿项。
5. **路径/起点几何** 5. **路径/起点几何**
- 起点：`TryGetFleetCenterFromSlam()`；路径：`LineTrack(x0,y0 → dst)`，`phi = theta + CrabAngleDeg`。 - 起点：`TryGetFleetCenterFromSlam()`；路径：`LineTrack(x0,y0 → dst)`，`phi = theta + CrabAngleDeg`。
- 若 `theta` 与运行时 `CenterTh` 不一致，或 layout 反推中心与控制器使用的快照中心有系统偏差，会表现为沿某一轴漂移。 - 若 `theta` 与运行时 `CenterTh` 不一致，或 layout 反推中心与控制器使用的快照中心有系统偏差，会表现为沿某一轴漂移。
**建议下一轮日志对照**： **建议下一轮日志对照**：
- `FleetCrabDbg`：`lateral` 是否收敛、`corr/localAngle/cmd` 是否平滑、有无到达纠偏上限 - `FleetCrabDbg`：`lateral` 是否收敛、`headingErr` 是否收敛、`bias/dth` 是否到达阈值、`auto(vx,fTh,rTh)` 是否稳定
- `MultiVehicleDbg`：`frontTh/rearTh/speed` 是否接近手动蟹行、POS/Detect 补偿是否在持续驱动，`CRAB in/raw/limit/rev` 是否显示 `raw=-95°` 这类角度未被反向等价转换 - `MultiVehicleDbg`：自动分支是否为 `auto:true/manual:false/script:false`，`BASE/SEND` 是否接近 `FleetCrabDbg` 输出，POS/Detect 补偿是否持续驱动
- 如需回退旧几何控制器路线，再看 `CrabDbg` 的 `bias/biasItem/gcp/fleetPos` - 重点看 `FleetCrabGcpThetaThreshold`、`BiasThreshold`、`DthLinearThreshold` 三个限幅是否过早截断纠偏
**2026-06-29 DLog 结论（自动蟹行仍抖动）**： **2026-06-29 DLog 结论（旧脚本链路下自动蟹行仍抖动）**：
- `FleetCrabDbg` 中 `along/lateral/remain/corr/localAngle/cmd` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。 - 旧 `FleetCrabDbg` 中 `along/lateral/remain/旧方向修正/旧命令` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
- 主/从 `MultiVehicleDbg` 中 `BASE vx` 在正负之间跳，同时 `fTh/rTh` 在 `+90°/-90°` 附近翻转；这是同一横移矢量被错误地按 ±90° 边界转换成两种等价表示，底盘执行层会看到接近 180° 的转向跳变。 - 主/从 `MultiVehicleDbg` 中 `BASE vx` 在正负之间跳，同时 `fTh/rTh` 在 `+90°/-90°` 附近翻转；这是同一横移矢量被错误地按 ±90° 边界转换成两种等价表示，底盘执行层会看到接近 180° 的转向跳变。
- POS 补偿在该批日志中为关闭/零补偿（`corr:false`、`POS comp 0`），Detect 补偿有小幅值但不是主因。 - 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+ 漂移）**： **2026-06-29 DLog 结论（±120 修复后仍 Y+ 漂移）**：
- Clumsy 侧 `MultiVehicleDbg` 已显示 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，`BASE vx` 不再正负翻转，说明上层 `mode=1` 表达已连续，剧烈抖动问题已消失。 - 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` 上限。 - 但 `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]`。 - 进一步检查 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 使场景重载。若复测时仍出现该日志，说明还有其他配置或场景副本在限制舵角。 - 已把 Playground 源码场景和运行目录场景改为 `maxSteeringAngle: 120`，并在仿真器中加入 `multi-steering clamp` 节流日志；复测前必须重启 Playground 使场景重载。若复测时仍出现该日志，说明还有其他配置或场景副本在限制舵角。
### 2.2 两车抖动、不丝滑 ### 2.2 两车抖动、不丝滑
@@ -82,7 +84,7 @@
**建议对照实验**： **建议对照实验**：
- 手动 FleetRemote 蟹行（同速度、同角度）是否也抖 - 手动 FleetRemote 蟹行（同速度、同角度）是否也抖
- 临时 `MultiVehicleSyncUseDetour=false` 复测 - 临时 `MultiVehicleSyncUseDetour=false` 复测
- 看 `FleetCrabDbg` 的 `corr/localAngle/cmd` 与 `MultiVehicleDbg` 的 `frontTh/rearTh` 是否周期跳变 - 看 `FleetCrabDbg` 的 `auto(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `BASE/SEND` 是否周期跳变
--- ---
@@ -214,8 +216,8 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
| Topic | 来源 | 内容 | | Topic | 来源 | 内容 |
|-------|------|------| |-------|------|------|
| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/corr/localAngle/cmd` | | **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/headingErr/baseTh/bias/dth/auto/ideal` |
| **`CrabDbg`** | `MultiWheelGeometricController.cs` | 旧几何控制器路线诊断；当前脚本蟹行实现不再依赖 | | **`CrabDbg`** | `MultiWheelGeometricController.cs` | MDCSToolbox 几何控制器诊断；当前自动蟹行只参考其思路，不修改也不依赖该源码 |
| **`MultiVehicleDbg`** | `PilotDefinition.cs` | 联动循环：速度、舵角、补偿、ready 状态 | | **`MultiVehicleDbg`** | `PilotDefinition.cs` | 联动循环：速度、舵角、补偿、ready 状态 |
| **`FleetDiagClumsy`** | `PilotDefinition.cs` | 精简 fleet 诊断（带 `car{N}` 前缀） | | **`FleetDiagClumsy`** | `PilotDefinition.cs` | 精简 fleet 诊断（带 `car{N}` 前缀） |
| **`MultiVehicle`** | `PilotDefinition.cs` | 初始化、心跳、HTTP 错误 | | **`MultiVehicle`** | `PilotDefinition.cs` | 初始化、心跳、HTTP 错误 |
@@ -223,9 +225,9 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
### 6.3 建议抓取顺序（排查漂移/抖动） ### 6.3 建议抓取顺序（排查漂移/抖动）
1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/corr/localAngle/cmd` 1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/headingErr/bias/dth/auto(vx,fTh,rTh)`
2. 主车 + 从车 `MultiVehicleDbg`：`frontTh/rearTh`、`PosBias*`、是否 `ready=false` 2. 主车 + 从车 `MultiVehicleDbg`：`auto:true/manual:false`、`BASE/SEND`、`PosBias*`、是否 `ready=false`
3. 若回退旧几何控制器路线，再看主车 `CrabDbg`：`bias` 是否单调增大；`fleetPos` 是否偶发 `(0,0,0)` 3. 若怀疑 MDCSToolbox 自动路径，再看主车 `CrabDbg`；当前 `FleetCrabWalk` 不直接调用该控制器
4. 从车 `FleetDiagClumsy`：是否频繁掉线 / register 超时 4. 从车 `FleetDiagClumsy`：是否频繁掉线 / register 超时
--- ---
@@ -237,23 +239,26 @@ MovementTest「车队联动-自动蟹行」
FleetCrabWalk.Get() FleetCrabWalk.Get()
TryGetFleetCenterFromSlam() → 路径起点 (x0,y0,θ) TryGetFleetCenterFromSlam() → 路径起点 (x0,y0,θ)
phi = theta + FleetCrabAngleDeg phi = theta + FleetCrabAngleDeg
MultiVehicleScriptEnabled = true BodyToPathAngleDeg = FleetCrabAngleDeg
MultiVehicleScriptMode = 1 → 复用 FleetRemote 手动蟹行下发链路 targetBodyTh = phi - BodyToPathAngleDeg = theta
MultiVehicleScriptEnabled = false
MultiVehicleAutoEnabled = true → 进入 TickMultiVehicle 自动分支
WARMUP → 等编队成员就位 WARMUP → 等编队成员就位
loop: loop:
TryGetFleetCenterFromSlam() → 当前车队中心 TryGetFleetCenterFromSlam() → 当前车队中心
along/lateral/remain → 沿线进度、横向偏差、剩余距离 along/lateral/remain/headingErr → 沿线进度、横向偏差、剩余距离、车身目标朝向偏差
corr = clamp(Stanley(lateral), ±FleetCrabCorrectionAngleDeg) bias = clamp(Stanley(lateral), ±BiasThreshold)
localAngle = (phi + corr) - currentTheta dth = clamp(DthLinearFac * (targetBodyTh-currentTheta), ±DthLinearThreshold)
Vx/Vy slew limit → FleetCrabCommandAccel 平滑 frontTh/rearTh = clamp(phi-currentTheta + bias ± dth, ±FleetCrabGcpThetaThreshold)
MultiVehicleScriptVx/Vy = cmd ideal = pathStart + pathDir * clamp(along, 0, FleetCrabLengthMm)
MultiVehicleAutoVx/FrontTh/RearTh/Ideal* = cmd
PilotDefinition.TickMultiVehicle (50ms) PilotDefinition.TickMultiVehicle (50ms)
manual/script mode==1 auto branch
Vx/Vy → speed + frontTh==rearTh MultiVehicleAuto* → speed + frontTh/rearTh + ideal center
notify → 从车 SendMotion + POS/Detect 补偿 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) | | `FleetCrabLengthMm` | 2000 | 路径长度 (mm) |
| `FleetCrabSpeed` | 0.2 | 速度 (m/s) | | `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由通用减速参数下调 |
| `FleetCrabGcpThetaThreshold` | 95 | 兼容旧几何控制器实现；当前脚本蟹行不直接使用 | | `FleetCrabGcpThetaThreshold` | 95 | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限，应给实际舵角限位与 `AngleLimitMarginDeg` 留余量 |
| `FleetCrabCorrectionGain` | 1.0 | 横向误差纠偏增益 |
| `FleetCrabCorrectionAngleDeg` | 8 | 自动纠偏最大改向角，越小越接近手动蟹行 |
| `FleetCrabCommandAccel` | 0.4 | 脚本 `Vx/Vy` 命令斜率限制(m/s²) |
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleScript*` 和 `MultiVehicleAuto*`。 已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略，不能再作为有效调参项。
动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleAuto*`，并保持 `MultiVehicleScriptEnabled=false`。
### 8.2 影响跟踪/手感的全局项（节选） ### 8.2 影响跟踪/手感的全局项（节选）
@@ -282,7 +286,12 @@ MovementTest「车队联动-自动蟹行」
| `TestCarSyncDistance` | 2400 | 与 Playground 双车间距一致 | | `TestCarSyncDistance` | 2400 | 与 Playground 双车间距一致 |
| `MultiVehicleSyncInterval` | 50 | 联动周期 ms | | `MultiVehicleSyncInterval` | 50 | 联动周期 ms |
| `DriveTaskInterval` | 50 | `clumsy.json` 顶层 | | `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。 详见 [MultiVehicleConfig.md](./MultiVehicleConfig.md) §2–§6。
@@ -296,20 +305,20 @@ MovementTest「车队联动-自动蟹行」
| 蟹行要求朝向不变但有纠偏 | `CrabHoldHeading` + `CrabTargetHeading`；保留 `dTh` | | 蟹行要求朝向不变但有纠偏 | `CrabHoldHeading` + `CrabTargetHeading`；保留 `dTh` |
| 多车 firstTurn 破坏队形 | `MultiVehicleSync` 时跳过 `firstTurnN`（TODO 整队预旋转） | | 多车 firstTurn 破坏队形 | `MultiVehicleSync` 时跳过 `firstTurnN`（TODO 整队预旋转） |
| gcp 被 45° 上限截断 | 动作侧 `GcpThetaThreshold=95` | | gcp 被 45° 上限截断 | 动作侧 `GcpThetaThreshold=95` |
| ideal 中心抹平横向误差 | 动作期间关 `MultiVehicleAutoUseIdealCenter` | | ideal 中心抹平横向误差 | 已改为显式发布 `MultiVehicleAutoIdealX/Y/Th`，由 `MultiVehicleAutoUseIdealCenter` 控制是否前馈 |
| Tick 中间窗口发布 `(0,0,0)` 假中心 | 已移除 tick 开头 `PublishFleetCenter(0,0,0)` | | Tick 中间窗口发布 `(0,0,0)` 假中心 | 已移除 tick 开头 `PublishFleetCenter(0,0,0)` |
| 自动蟹行纠偏导致抖动 | 已改为脚本手动蟹行链路 + 小幅平滑横向纠偏 | | 自动蟹行纠偏导致抖动 | 已改为自动字段链路，按 `lateral/headingErr/remain` 计算 `MultiVehicleAuto*` |
| 接近纯横移时速度符号/舵角表示翻转 | `fleetMode==1` 改为按 `MultiVehicleCrabSteerLimitDeg`（默认 120°）归一化；`-95°` 直接下发，超过上限才做速度取反的等价转换，并在 `MultiVehicleDbg` 输出 `CRAB in/raw/limit/rev` | | 接近纯横移时速度符号/舵角表示翻转 | `fleetMode==1` 改为按 `MultiVehicleCrabSteerLimitDeg`（默认 120°）归一化；`-95°` 直接下发，超过上限才做速度取反的等价转换，并在 `MultiVehicleDbg` 输出 `CRAB in/raw/limit/rev` |
--- ---
## 10. 后续工作建议（优先级） ## 10. 后续工作建议（优先级）
1. **复测 -90° 自动蟹行**：重点看 `MultiVehicleDbg` 的 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，以及 `BASE vx/fTh/rTh` 是否不再正负翻转。 1. **复测自动蟹行**：重点看 `FleetCrabDbg` 的 `auto=(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `auto:true/manual:false` 是否一致。
2. **A/B：`MultiVehicleCrabSteerLimitDeg`** 默认 120，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；若实际轮角限制不同，先同步该值。 2. **A/B：`FleetCrabGcpThetaThreshold`** 默认 95，应与实车舵角限制和 `AngleLimitMarginDeg` 匹配；若输出很快被限幅，先核对该值。
3. **A/B：`FleetCrabCorrectionAngleDeg`** 先试 4、8、12：4 最接近手动，12 收敛更快；当前不再因跨 ±90° 直接翻面。 3. **A/B：`BiasFac/BiasThreshold`** 若 `lateral` 单向增长，先看 `bias` 是否到上限；需要更强横向纠偏时调这组参数。
4. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分脚本纠偏 vs POS 补偿贡献。 4. **A/B：`DthLinearFac/DthLinearThreshold`** 若车队朝向偏差收敛慢或前后 GCP 差动过大，调这组参数。
5. **路径误差**：若仍持续 Y+ 漂移，看 `lateral` 是否持续单向增长；若增长但 `corr` 已到上限，增大 `FleetCrabCorrectionAngleDeg` 或 `FleetCrabCorrectionGain`。 5. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分自动路径纠偏 vs POS 补偿贡献。
6. **notify 平滑**（中长期）：见 `MultiVehicleAutoSyncReview.md` §F。 6. **notify 平滑**（中长期）：见 `MultiVehicleAutoSyncReview.md` §F。
--- ---
docs/MultiVehicleConfig.md
+32 -19
View File
@@ -144,28 +144,45 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
## 6. 自动蟹行动作（FleetCrabWalk / MovementTest「车队联动-自动蟹行」） ## 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`。 - 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral`、剩余距离 `remain` 和车身目标朝向偏差 `headingErr`。
- 横向偏差只转成一个**小幅、带斜率限制的蟹行方向修正**，再写入 `MultiVehicleScriptVx/Vy`；`TickMultiVehicle` 仍按手动蟹行逻辑合成 `frontTh==rearTh` 并广播从车。 - `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向舵角修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向舵角修正。
- 这样保留手动蟹行的平滑执行链路，同时让自动动作具备温和的路径纠偏；避免旧几何控制器 `bias/dTh` 直接叠到 gcp 时出现舵角阶跃。 - 动作直接输出 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/IdealY/IdealTh`，由 `TickMultiVehicle` 的自动分支统一广播、下发 `SendMotion`，并继续受识别丢失、成员超时、舵角余量不足等整队缓停联锁保护。
- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点）。 - **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点与运行中闭环）。
| 字段（`clumsy.json` → `msConf`） | 含义 | 默认值 | | 字段（`clumsy.json` → `msConf`） | 含义 | 默认值 |
|------|------|--------| |------|------|--------|
| `FleetCrabAngleDeg` | 蟹行路径**与当前车队朝向的夹角**(deg，逆时针为正)。决定斜行方向：0=正前方，90=正左方平移，-90=正右方。稳态下即各舵轮的蟹行角 | `45` | | `FleetCrabAngleDeg` | 蟹行路径方向相对**启动时车队朝向**的夹角(deg，逆时针为正)。MovementTest 同时把车身-路径夹角设为该值，因此 `FleetCrabAngleDeg=-x` 会让车身保持启动朝向，并以 `x` 度夹角追踪路径 | `45` |
| `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` | | `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` |
| `FleetCrabSpeed` | 蟹行**行驶速度**(m/s) | `0.2` | | `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被 `SlowDistance/SlowingPow/FinishSpeed` 降速 | `0.2` |
| `FleetCrabGcpThetaThreshold` | 兼容旧几何控制器实现的 gcp 舵角上限；当前脚本手动等价实现不直接使用 | `95` | | `FleetCrabGcpThetaThreshold` | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限(deg)。应小于实际舵角可行范围，并给 `AngleLimitMarginDeg` 留余量 | `95` |
| `FleetCrabCorrectionGain` | 横向误差纠偏增益。增大后收敛更快，但更容易出现方向摆动 | `1` |
| `FleetCrabCorrectionAngleDeg` | 自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大纠偏越强 | `8` | 自动蟹行还会使用下列通用控制参数：
| `FleetCrabCommandAccel` | `Vx/Vy` 命令斜率限制(m/s²)，抑制纠偏方向突变 | `0.4` |
| `MultiVehicleCrabSteerLimitDeg` | mode=1 蟹行舵角上限，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；`-95°` 在默认 120° 内会直接下发 | `120` | | 字段 | 影响 | 默认来源 |
|------|------|----------|
| `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`；结束/急停会清零脚本字段。 - 当前实现走 `MultiVehicleAuto*` 自动字段链路，不再开启 `MultiVehicleScriptEnabled`，也不受 `MultiVehicleCrabSteerLimitDeg` 影响（该字段只影响手动 `mode=1` 蟹行）。
- `FleetCrabDbg` 会记录 `along/lateral/remain/corr/localAngle/cmd(Vx,Vy)`；`MultiVehicleDbg` 可继续对照最终 `frontTh≈rearTh`、是否有 POS/Detect 补偿，以及 `CRAB in/raw/limit/rev` 是否在舵角上限内保持连续表达。 - `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。
- `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。 - `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。
- Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。 - Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。
@@ -202,11 +219,7 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter)
"FleetCrabAngleDeg": 45, "FleetCrabAngleDeg": 45,
"FleetCrabLengthMm": 2000, "FleetCrabLengthMm": 2000,
"FleetCrabSpeed": 0.2, "FleetCrabSpeed": 0.2,
"FleetCrabGcpThetaThreshold": 95, "FleetCrabGcpThetaThreshold": 95
"FleetCrabCorrectionGain": 1.0,
"FleetCrabCorrectionAngleDeg": 8,
"FleetCrabCommandAccel": 0.4,
"MultiVehicleCrabSteerLimitDeg": 120
``` ```
```text ```text