diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs
index f09ed91..90e3ed4 100644
--- a/MultiWheel/MultiWheelC/MovementTests.cs
+++ b/MultiWheel/MultiWheelC/MovementTests.cs
@@ -387,6 +387,21 @@ public class FleetCrabWalk : MovementDefinition
/// 行驶速度(m/s)。
public float CrabSpeed = 0.2f;
+ /// 速度命令加速度限制(m/s^2),小于等于 0 表示不限制。
+ public float FleetCrabAccel = 0.2f;
+
+ /// 末端开始减速距离(mm)。
+ public float FleetCrabSlowDistance = 2000f;
+
+ /// 完成距离(mm),低于该剩余距离结束动作。
+ public float FleetCrabFinishDistance = 20f;
+
+ /// 末端最低速度(m/s)。
+ public float FleetCrabFinishSpeed = 0.02f;
+
+ /// 末端减速曲线指数。
+ public float FleetCrabSlowingPow = 0.8f;
+
/// 前后 GCP 舵角修正上限(deg)。
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 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 slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance);
+ var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
+ 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);
- speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
+ var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
+ 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());
diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs
index 912e607..d44e3d5 100644
--- a/MultiWheel/MultiWheelC/PilotConfig.cs
+++ b/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;
diff --git a/docs/FleetCrabWalkWorkContext.md b/docs/FleetCrabWalkWorkContext.md
index 0970702..57ef759 100644
--- a/docs/FleetCrabWalkWorkContext.md
+++ b/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) | 自动命令新鲜度超时,避免控制器停发后沿末速度滑行 |
diff --git a/docs/MultiVehicleConfig.md b/docs/MultiVehicleConfig.md
index 75c8f0e..25403c2 100644
--- a/docs/MultiVehicleConfig.md
+++ b/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
```