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 ```