Add dedicated fleet crab control parameters

MultiWheel/MultiWheelC/MovementTests.cs

@@ -387,6 +387,21 @@ public class FleetCrabWalk : MovementDefinition
     /// <summary>行驶速度(m/s)。</summary>
     public float CrabSpeed = 0.2f;
 
+    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabAccel = 0.2f;
+
+    /// <summary>末端开始减速距离(mm)。</summary>
+    public float FleetCrabSlowDistance = 2000f;
+
+    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
+    public float FleetCrabFinishDistance = 20f;
+
+    /// <summary>末端最低速度(m/s)。</summary>
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    /// <summary>末端减速曲线指数。</summary>
+    public float FleetCrabSlowingPow = 0.8f;
+
     /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
     public float GcpThetaThreshold = 95f;
 
@@ -429,6 +444,14 @@ public class FleetCrabWalk : MovementDefinition
         return value;
     }
 
+    private static float Slew(float current, float target, float maxDelta)
+    {
+        if (maxDelta <= 0) return target;
+        if (target > current + maxDelta) return current + maxDelta;
+        if (target < current - maxDelta) return current - maxDelta;
+        return target;
+    }
+
     public override IEnumerable<bool> Get()
     {
         var self = PilotDefinition.Self;
@@ -470,7 +493,9 @@ public class FleetCrabWalk : MovementDefinition
         DLog.Log(
             $"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} accel={FleetCrabAccel:0.000} " +
+            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
+            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
             "FleetCrabDbg");
 
         self.MultiVehicleScriptEnabled = false;
@@ -534,10 +559,14 @@ public class FleetCrabWalk : MovementDefinition
 
         var iter = 0;
         var lastLog = DateTime.MinValue;
-        var finishDistance = Math.Max(20f, conf.FinishDistance);
+        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
-        var slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance);
+        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
         var baseSpeed = Math.Abs(CrabSpeed);
-        var finishSpeed = Math.Min(baseSpeed, Math.Abs(conf.FinishSpeed));
+        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
+        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
+        var accel = Math.Abs(FleetCrabAccel);
+        var cmdSpeed = 0f;
+        var lastTick = DateTime.Now;
         var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
         var stopReason = "done";
 
@@ -558,12 +587,17 @@ public class FleetCrabWalk : MovementDefinition
             if (remain <= finishDistance)
                 break;
 
-            var speed = baseSpeed;
+            var targetSpeed = baseSpeed;
             if (remain < slowDistance)
             {
-                var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
+                var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
-                speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
+                targetSpeed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
             }
+            var now = DateTime.Now;
+            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
+            lastTick = now;
+            var speed = accel > 0 ? Slew(cmdSpeed, targetSpeed, accel * dt) : targetSpeed;
+            cmdSpeed = speed;
 
             var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
             var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
@@ -600,7 +634,7 @@ public class FleetCrabWalk : MovementDefinition
                     $"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} 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}) " +
+                    $"targetV={targetSpeed:0.000} 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");
@@ -648,6 +682,11 @@ public class FleetCrabWalkTest : MovementTest
             BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
             CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
             CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
+            FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+            FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
+            FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
+            FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
+            FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
             GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
         };
         _task = new DriveTask(_proc.Get());

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -165,6 +165,21 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
     public float FleetCrabSpeed = 0.2f;
 
+    [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
+    public float FleetCrabAccel = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
+    public float FleetCrabSlowDistance = 2000f;
+
+    [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
+    public float FleetCrabFinishDistance = 20f;
+
+    [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "车队蟹行：末端减速曲线指数")]
+    public float FleetCrabSlowingPow = 0.8f;
+
     [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
     public float FleetCrabGcpThetaThreshold = 95f;
 

docs/FleetCrabWalkWorkContext.md

@@ -270,10 +270,15 @@ MovementTest「车队联动-自动蟹行」
 |------|------|------|
 | `FleetCrabAngleDeg` | 45 | 路径方向相对启动时车队朝向的夹角 (deg)。MovementTest 同时把车身-路径夹角设为该值；若输入“路径与小车夹角 x 度”，应填 `-x` 以保持当前车身角度 |
 | `FleetCrabLengthMm` | 2000 | 路径长度 (mm) |
-| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由通用减速参数下调 |
+| `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，会被配置反序列化忽略，不能再作为有效调参项。
+已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略；新的自动链路使用 `FleetCrabAccel` 控制 `MultiVehicleAutoVx` 速度命令斜率。
 
 动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleAuto*`，并保持 `MultiVehicleScriptEnabled=false`。
 
@@ -288,7 +293,7 @@ MovementTest「车队联动-自动蟹行」
 | `DriveTaskInterval` | 50 | `clumsy.json` 顶层 |
 | `BiasFac` / `BiasThreshold` | 继承 `MultiWheelPilotConfig` | 横向偏差 `lateral` → 前后 GCP 同向修正 |
 | `DthLinearFac` / `DthLinearThreshold` | 继承 `MultiWheelPilotConfig` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正 |
-| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 继承 `BasicPilotConfig` | 自动蟹行终点减速和结束判定 |
+| `FleetCrabSlowDistance` / `FleetCrabSlowingPow` / `FleetCrabFinishDistance` / `FleetCrabFinishSpeed` | `PilotConfig` | 自动蟹行专用终点减速和结束判定 |
 | `MultiVehicleAutoUseIdealCenter` | true（默认） | 使用自动蟹行发布的 ideal center 给从车做前馈 |
 | `MultiVehicleAutoRequireFleetCenter` | true（默认） | 自动模式无有效车队中心时整队停车 |
 | `MultiVehicleAutoCmdTimeoutMs` | 0（auto） | 自动命令新鲜度超时，避免控制器停发后沿末速度滑行 |

docs/MultiVehicleConfig.md

@@ -155,7 +155,12 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 |------|------|--------|
 | `FleetCrabAngleDeg` | 蟹行路径方向相对**启动时车队朝向**的夹角(deg，逆时针为正)。MovementTest 同时把车身-路径夹角设为该值，因此 `FleetCrabAngleDeg=-x` 会让车身保持启动朝向，并以 `x` 度夹角追踪路径 | `45` |
 | `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` |
-| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被 `SlowDistance/SlowingPow/FinishSpeed` 降速 | `0.2` |
+| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被自动蟹行专用减速参数下调 | `0.2` |
+| `FleetCrabAccel` | 蟹行速度命令加速度限制(m/s^2)，限制 `MultiVehicleAutoVx` 每拍变化量；`<=0` 表示不限制 | `0.2` |
+| `FleetCrabSlowDistance` | 蟹行末端开始减速距离(mm) | `2000` |
+| `FleetCrabFinishDistance` | 蟹行完成距离(mm)，剩余距离低于该值时结束动作 | `20` |
+| `FleetCrabFinishSpeed` | 蟹行末端最低速度(m/s) | `0.02` |
+| `FleetCrabSlowingPow` | 蟹行末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速 | `0.8` |
 | `FleetCrabGcpThetaThreshold` | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限(deg)。应小于实际舵角可行范围，并给 `AngleLimitMarginDeg` 留余量 | `95` |
 
 自动蟹行还会使用下列通用控制参数：
@@ -164,7 +169,6 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 |------|------|----------|
 | `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` |
@@ -177,12 +181,12 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 |------------|--------|----------|
 | `FleetCrabCorrectionGain` | 旧脚本链路的横向误差纠偏增益 | `BiasFac` |
 | `FleetCrabCorrectionAngleDeg` | 旧脚本链路的最大改向角 | `BiasThreshold` / `FleetCrabGcpThetaThreshold` |
-| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 由自动联动周期、底盘速度斜坡和终点减速共同约束 |
+| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 新自动链路使用 `FleetCrabAccel` 限制 `MultiVehicleAutoVx` |
 
 **行为要点 / 注意**
 
 - 当前实现走 `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 状态。
+- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/targetV/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。
 - `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。
 - Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。
 
@@ -219,6 +223,11 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 "FleetCrabAngleDeg": 45,
 "FleetCrabLengthMm": 2000,
 "FleetCrabSpeed": 0.2,
+"FleetCrabAccel": 0.2,
+"FleetCrabSlowDistance": 2000,
+"FleetCrabFinishDistance": 20,
+"FleetCrabFinishSpeed": 0.02,
+"FleetCrabSlowingPow": 0.8,
 "FleetCrabGcpThetaThreshold": 95
 ```