Merge branch 'master' of http://git.fairylandtech.com/ruifeng.zhou/Tutorial

MultiWheel/MultiWheelC/AGV.cs

@@ -5,8 +5,6 @@ using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Tracks;
 using MDCSToolBox.Commons.Controllers;
 using Newtonsoft.Json;
-using OpenCvSharp.Dnn;
-using OpenCvSharp.XFeatures2D;
 using System;
 using System.Collections.Generic;
 using System.Net.Http;
@@ -14,7 +12,6 @@ using System.Numerics;
 using System.Threading;
 using System.Threading.Tasks;
 using static ClumsyCore.DTools.Painter;
-using static OpenCvSharp.ConnectedComponents;
 
 namespace MultiWheelC
 {
@@ -213,22 +210,6 @@ namespace MultiWheelC
             DLog.Log("驱动器下使能完成", "TireFollowing");
         }
 
-        //夹抱 close为true时夹抱，否则为还原
-        public void ClamptoTarget(bool close)
-        {
-            if (PilotDefinition.Self.GhostMode)
-            {
-                Thread.Sleep(2000);
-                Console.WriteLine("夹抱完成");
-                return;
-            }
-            new DriveTask(new ClampToTarget()
-            {
-                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
-                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
-            }.Get()).Wait();
-        }
-
         public void LineTracking(int srcId, int dstId, float LineDistance)
         {
             while (!TryLock(dstId))

MultiWheel/MultiWheelC/ChassisController.cs

@@ -3,6 +3,7 @@ using System.Numerics;
 using ClumsyCore;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
+using FundamentalLib;
 using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
@@ -13,6 +14,8 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
 {
     public float BaseSpeed = Configuration.conf.basicSpeed;
 
+    private DateTime _sendMotionDbgLast = DateTime.MinValue;
+
     public override MultiWheelGeometricController Get()
     {
         return new MultiWheelGeometricController
@@ -45,25 +48,52 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
 
             MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
             {
-                PilotDefinition.Self.MultiVehicleAutoEnabled = true;
-                lock (PilotDefinition.Self.MultiVehicleFleet)
+                var self = PilotDefinition.Self;
+                self.MultiVehicleAutoEnabled = true;
+                // A: 用固定锁对象（不再锁会被替换的字段引用）。
+                int fleetCnt;
+                lock (self.FleetLock)
+                    fleetCnt = self.MultiVehicleFleet.Count;
+
+                // 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
+                if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
                 {
-                    if (PilotDefinition.Self.MultiVehicleFleet.Count != PilotDefinition.Conf.MultiVehicleFleetNum)
-                        return;
+                    _sendMotionDbgLast = DateTime.Now;
+                    DLog.Log(
+                        $"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
+                        $"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
+                        $"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
+                        $"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
+                        "FleetCrabDbg");
                 }
 
-                PilotDefinition.Self.MultiVehicleAutoVx = speed;
-                PilotDefinition.Self.MultiVehicleAutoFrontTh = frontTh;
-                PilotDefinition.Self.MultiVehicleAutoRearTh = rearTh;
+                if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
+                    return;
+
+                self.MultiVehicleAutoVx = speed;
+                self.MultiVehicleAutoFrontTh = frontTh;
+                self.MultiVehicleAutoRearTh = rearTh;
+                // D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
+                self.MultiVehicleAutoIdealX = idealPos.X;
+                self.MultiVehicleAutoIdealY = idealPos.Y;
+                self.MultiVehicleAutoIdealTh = idealAngle;
+                self.MultiVehicleAutoHasIdeal = true;
+                // B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
             },
 
-            MultiVehicleGetFleetPos = () => new Location
+            // G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
+            MultiVehicleGetFleetPos = () =>
             {
-                x = PilotDefinition.Self.CenterX,
-                y = PilotDefinition.Self.CenterY,
-                th = PilotDefinition.Self.CenterTh,
-                l_step = 1,
-                tick = DateTime.Now.Ticks
+                var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
+                return new Location
+                {
+                    x = snap.X,
+                    y = snap.Y,
+                    th = snap.Th,
+                    l_step = 1,
+                    tick = DateTime.Now.Ticks
+                };
             }
         };
     }

MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

@@ -11,7 +11,6 @@ using FundamentalLib;
 using MDCSToolBox.Clumsy.Calibration;
 using MDCSToolBox.Clumsy.Tracks;
 using MDCSToolBox.Commons.Controllers;
-using OpenCvSharp.Dnn;
 using System;
 using System.Collections.Generic;
 using System.Drawing;
@@ -211,42 +210,6 @@ namespace MultiWheelC
         private DriveTask _dt;
     }
 
-    [MovementTest(name = "抱夹关闭")]
-    public class ClampTest1 : MovementTest
-    {
-        public override void TestStop()
-        {
-            throw new NotImplementedException();
-        }
-
-        public override void Test()
-        {
-            new DriveTask(new ClampToTarget()
-            {
-                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
-                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
-            }.Get()).Wait();
-        }
-    }
-
-    [MovementTest(name = "抱夹打开")]
-    public class ClampTest2 : MovementTest
-    {
-        public override void TestStop()
-        {
-            throw new NotImplementedException();
-        }
-
-        public override void Test()
-        {
-            new DriveTask(new ClampToTarget()
-            {
-                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
-                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
-            }.Get()).Wait();
-        }
-    }
-    
     [MovementTest(name = "测试前进基于轮里程")]
     public class LineTrackingTest : MovementTest
     {

MultiWheel/MultiWheelC/MovementTests.cs

@@ -7,8 +7,10 @@ using ClumsyCore.Pilot;
 using FundamentalLib;
 using CommonUsage.Chassis;
 using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
 
 namespace MultiWheelC;
 
@@ -124,6 +126,473 @@ public class MultiRotateMovementTest : MovementTest
     }
 }
 
+// ===== 车队联动-原地旋转动作 =====
+// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
+// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
+// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
+// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
+// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
+//
+// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
+// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
+//          无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
+public class FleetRotateInPlace : MovementDefinition
+{
+    /// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
+    public float Omega = 15f;
+
+    /// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
+    public float TargetDeltaDeg = 90f;
+
+    /// <summary>到位角度精度(deg)。</summary>
+    public float ArriveDeg = 1.5f;
+
+    /// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
+    public float SlowDeg = 25f;
+
+    /// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
+    public float MinOmega = 3f;
+
+    /// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，&lt;=0 关闭缓启动（阶跃起步）。</summary>
+    public float AccelDegPerSec2 = 20f;
+
+    /// <summary>
+    /// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
+    /// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
+    /// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
+    /// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
+    /// </summary>
+    public bool UseDetourHeading = true;
+
+    // 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
+
+    /// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
+    public float SettleSec = 0.5f;
+
+    private void ClearScript()
+    {
+        var self = PilotDefinition.Self;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptEnabled = false;
+    }
+
+    public void Stop() => ClearScript();
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
+            yield break;
+        }
+
+        var dir = Math.Sign(TargetDeltaDeg);
+        if (dir == 0) dir = 1;
+        var maxOmega = Math.Abs(Omega);
+        var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
+        var slowDeg = Math.Max(1e-3f, SlowDeg);                // 减速区宽度
+        var accel = AccelDegPerSec2;                           // 缓启动角加速度，仅作用于起步，<=0 关闭
+        var targetMag = Math.Abs(TargetDeltaDeg);
+
+        var hasPos = UseDetourHeading;
+        var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
+        var startTh = prevTh;
+        var accumulated = 0f; // 累计带符号转角(deg)
+        var start = DateTime.Now;
+        var lastTime = start;
+        var lastLog = DateTime.MinValue;
+        var cmdMag = 0f;      // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
+        // 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
+        var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
+        if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
+
+        DLog.Log(
+            $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
+            $"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
+            $"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
+            "FleetRotateDbg");
+
+        // 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
+        // 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptMode = 2;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptEnabled = true;
+
+        var stopReason = "stop()";
+        while (true)
+        {
+            var now = DateTime.Now;
+            var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
+            lastTime = now;
+            var elapsed = (now - start).TotalSeconds;
+
+            float desiredMag;
+            float curTh = 0f, remaining = 0f, actualRate = 0f;
+            if (hasPos)
+            {
+                curTh = (float)DetourInterface.getCartLocation().th;
+                var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
+                accumulated += step;
+                actualRate = dt > 1e-3 ? step / dt : 0f;            // 实际角速率(deg/s)，用于对比指令
+                prevTh = curTh;
+
+                remaining = targetMag - Math.Abs(accumulated);
+                if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
+
+                // 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
+                // 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
+                desiredMag = remaining < slowDeg
+                    ? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
+                    : maxOmega;
+            }
+            else
+            {
+                // 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
+                desiredMag = maxOmega;
+                if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
+            }
+
+            // 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
+            // “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
+            if (accel > 1e-3 && desiredMag > cmdMag)
+                cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
+            else
+                cmdMag = desiredMag;
+
+            self.MultiVehicleScriptVth = dir * cmdMag;
+
+            // 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
+            if ((now - lastLog).TotalMilliseconds >= 150)
+            {
+                lastLog = now;
+                DLog.Log(
+                    hasPos
+                        ? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
+                          $"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
+                        : $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
+                    "FleetRotateDbg");
+            }
+
+            Hedingben.ToastText(
+                hasPos
+                    ? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
+                    : $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
+                "FleetRotate");
+
+            yield return true;
+        }
+
+        // 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
+        self.MultiVehicleScriptVth = 0;
+        var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
+        while (DateTime.Now < settleEnd)
+            yield return true;
+
+        ClearScript();
+        DLog.Log(
+            $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
+            $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
+            "FleetRotateDbg");
+        Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
+    }
+}
+
+[MovementTest(name = "车队联动-原地旋转")]
+public class FleetRotateInPlaceTest : MovementTest
+{
+    private FleetRotateInPlace _proc;
+    private DriveTask _task;
+
+    public override void Test()
+    {
+        _proc = new FleetRotateInPlace
+        {
+            Omega = PilotDefinition.Conf.FleetRotateOmega,
+            TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
+            ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
+            SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
+            MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
+            AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
+            SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
+            UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
+        };
+        _task = new DriveTask(_proc.Get());
+        _task.Wait();
+    }
+
+    public override void TestStop()
+    {
+        _proc?.Stop();
+        _task?.Stop();
+    }
+}
+
+// ===== 车队联动-自动蟹行动作 =====
+// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用
+// TickMultiVehicle 的脚本手动等价输入(mode=1)，也就是 FleetRemote 手动蟹行同一条下发链路。
+//
+// 手动蟹行已验证丝滑，自动动作只额外做两件事：
+// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度和横向偏差；
+// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy，避免几何控制器 bias/dTh 阶跃造成抖动。
+//
+// 与 FleetRemote 手动蟹行(mode==1)对照：TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
+//
+// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
+public class FleetCrabWalk : MovementDefinition
+{
+    /// <summary>与当前车队朝向的夹角(deg，逆时针为正)。</summary>
+    public float CrabAngleDeg = 45f;
+
+    /// <summary>路径长度(mm)。</summary>
+    public float CrabLengthMm = 2000f;
+
+    /// <summary>行驶速度(m/s)。</summary>
+    public float CrabSpeed = 0.2f;
+
+    /// <summary>兼容旧配置；当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。</summary>
+    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 void Cleanup()
+    {
+        var self = PilotDefinition.Self;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = 0;
+        self.MultiVehicleAutoRearTh = 0;
+        self.MultiVehicleAutoHasIdeal = false;
+        self.MultiVehicleAutoEnabled = false;
+    }
+
+    public void Stop()
+    {
+        _stopping = true;
+        Cleanup();
+    }
+
+    private static float Slew(float current, float target, float maxStep)
+    {
+        var diff = target - current;
+        if (Math.Abs(diff) <= maxStep) return target;
+        return current + Math.Sign(diff) * maxStep;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        _stopping = false;
+
+        DLog.Log(
+            $"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}",
+            "FleetCrabDbg");
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
+            yield break;
+        }
+
+        // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
+        DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
+        if (!self.TryGetFleetCenterFromSlam(out var x0, out var y0, out var theta))
+        {
+            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
+            yield break;
+        }
+        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+        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} " +
+            $"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.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.PrimeMasterAutoFromSlam();
+        DLog.Log("WARMUP 已启用脚本蟹行(mode=1)，等待编队成员就位…", "FleetCrabDbg");
+
+        var warmEnd = DateTime.Now.AddSeconds(2.0);
+        var warmIter = 0;
+        var warmReady = false;
+        while (!_stopping && DateTime.Now < warmEnd)
+        {
+            warmIter++;
+            self.MultiVehicleScriptEnabled = true;
+            self.MultiVehicleScriptMode = 1;
+            self.PrimeMasterAutoFromSlam();
+            var snap = self.GetFleetCenterSnapshot();
+            int cnt;
+            lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+            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}",
+                    "FleetCrabDbg");
+            if (cnt >= conf.MultiVehicleFleetNum)
+            {
+                warmReady = true;
+                DLog.Log(
+                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt}",
+                    "FleetCrabDbg");
+                break;
+            }
+            yield return true;
+        }
+        if (!warmReady)
+            DLog.Log("WARMUP 超时：编队仍未就位，继续进入脚本蟹行（若不动请查看 FleetDiagClumsy ready/cnt）",
+                "FleetCrabDbg");
+
+        Hedingben.ToastText($"车队蟹行 夹角{CrabAngleDeg: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 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);
+            var delta = new Vector2(cx - x0, cy - y0);
+            var along = Vector2.Dot(delta, pathDir);
+            var lateral = Vector2.Dot(delta, pathLeft);
+            var remain = CrabLengthMm - along;
+            if (remain <= finishDistance)
+                break;
+
+            var speed = Math.Abs(CrabSpeed);
+            if (remain < conf.SlowDistance && conf.SlowDistance > 1)
+            {
+                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 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);
+
+            self.MultiVehicleScriptEnabled = true;
+            self.MultiVehicleScriptMode = 1;
+            self.MultiVehicleScriptVx = cmdVx;
+            self.MultiVehicleScriptVy = cmdVy;
+            self.MultiVehicleScriptVth = 0;
+
+            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
+            {
+                lastLog = DateTime.Now;
+                var snap = self.GetFleetCenterSnapshot();
+                int fleetCnt;
+                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}",
+                    "FleetCrabDbg");
+            }
+            yield return true;
+        }
+
+        if (_stopping)
+            stopReason = "stop";
+
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+        while (!_stopping && DateTime.Now < settleEnd)
+        {
+            self.MultiVehicleScriptEnabled = true;
+            self.MultiVehicleScriptMode = 1;
+            yield return true;
+        }
+
+        Cleanup();
+        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
+        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
+    }
+}
+
+[MovementTest(name = "车队联动-自动蟹行")]
+public class FleetCrabWalkTest : MovementTest
+{
+    private FleetCrabWalk _proc;
+    private DriveTask _task;
+
+    public override void Test()
+    {
+        _proc = new FleetCrabWalk
+        {
+            CrabAngleDeg = 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
+        };
+        _task = new DriveTask(_proc.Get());
+        _task.Wait();
+    }
+
+    public override void TestStop()
+    {
+        _proc?.Stop();
+        _task?.Stop();
+    }
+}
+
 // ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
 // 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
 

MultiWheel/MultiWheelC/Movements.cs

@@ -16,7 +16,6 @@ using MDCSToolBox.Clumsy.Tracks;
 using MDCSToolBox.Commons;
 using MDCSToolBox.Commons.Controllers;
 using Newtonsoft.Json;
-using OpenCvSharp.Dnn;
 using System;
 using System.Collections.Generic;
 using System.Drawing;
@@ -74,55 +73,6 @@ namespace MultiWheelC
     }
 
 
-    public class ClampToTarget : MovementDefinition
-    {
-        public float LeftClampTarget;
-        public float RightClampTarget;
-
-        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
-        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
-        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
-        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
-        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
-        private PIDController leftpid, rightpid;
-
-        public override IEnumerable<bool> Get()
-        {
-            leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd, 0,
-                    ClampDeadZone, MaxClampSpeed)
-            { SpeedAccPerSec = MaxClampSpeed / 2f };
-
-            rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd, 0,
-                    ClampDeadZone, MaxClampSpeed)
-            { SpeedAccPerSec = MaxClampSpeed / 2f };
-
-            while (true)
-            {
-                var leftspeed = leftpid.GetResponse(LeftClampTarget);
-                var rightspeed = rightpid.GetResponse(RightClampTarget);
-                Console.WriteLine($"left arm speed:{leftspeed}  right arm speed:{rightspeed}");
-                PilotDefinition.Self.SpeedLeftArm = leftspeed;
-                PilotDefinition.Self.SpeedRightArm = rightspeed;
-                if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
-                if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
-                //if(Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos) > PilotDefinition.Conf.ClampOutOfSyncThr)
-                //{
-                //    PilotDefinition.Self.ClampOutOfSync = true;
-                //    Console.WriteLine($"左夹臂位置{PilotDefinition.Self.LeftArmActualPos} 右夹臂位置{PilotDefinition.Self.RightArmActualPos}");
-                //    Console.WriteLine($"两个夹臂的位置差了{Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos)}");
-                //    break;
-                //}
-
-                if (leftpid.IsArrived() && rightpid.IsArrived()) break;
-                yield return true;
-            }
-
-            PilotDefinition.Self.SpeedLeftArm = 0;
-            PilotDefinition.Self.SpeedRightArm = 0;
-            Console.WriteLine($"left clamp to target:{LeftClampTarget}  right clamp to target:{RightClampTarget}");
-        }
-    }
-
     public class Sleep : MovementDefinition
     {
         public float Second = 2;

MultiWheel/MultiWheelC/MultiWheelC.csproj

@@ -10,8 +10,6 @@
 
   <ItemGroup>
     <PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
-    <PackageReference Include="OpenCvSharp4" Version="4.10.0.20240615" />
-    <PackageReference Include="OpenCvSharp4.Extensions" Version="4.10.0.20240615" />
     <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
   </ItemGroup>
 

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -14,8 +14,14 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f;
     [FieldMember(desc = "[sync] 手动Vy系数(蟹行横向)")] public float ManualCarSyncVyFac = 1f;
     [FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
+    [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
     [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
-    [FieldMember(desc = "[sync] 是否启用Detour定位")] public bool PosAvailable = true;
+    // 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
+    // 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。
+    // 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。
+    // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
+    // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
+    [FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
 
     [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
     [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
@@ -34,6 +40,21 @@ public class PilotConfig : MultiWheelPilotConfig
     }
 
     [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
+
+    // E: 编队控制点半径(mm)。0 表示自动取 syncDistance/2（与 SetOriginBias 几何一致），>0 时按本值固定。
+    //    取代历史硬编码 510，避免改间距后控制点半径不跟随导致转向/补偿几何错位。
+    [FieldMember(desc = "多车联动：控制点半径(mm，0=syncDistance/2)")] public float MultiVehicleControlRadius = 0f;
+    // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
+    //    即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
+    [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
+    // C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。
+    //    0 表示自动取 max(500, interval*6)。
+    [FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
+    // D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标，
+    //    弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。
+    [FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
+    // H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。
+    [FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
     [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
     [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
     [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
@@ -61,6 +82,10 @@ public class PilotConfig : MultiWheelPilotConfig
     // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
     // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
     [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
+    // 可选硬安全网：每轮纠偏速度幅值 ≤ 该比例×本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
+    // 默认 <0 关闭——纠偏随转速缩放(代码 #1)已让"纠偏:切向"比例全程恒定，匀速段不应再被削弱。
+    // 仅在极端启动偏差导致匀速段仍乱打方向时，可设为 ~1.0(偏角≤45°) 兜底。
+    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限(默认-1关闭)")] public float MultiVehicleRotateCompTangentFrac = -1f;
 
     [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
     [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
@@ -92,6 +117,59 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
     public float InPlaceRotateWheelAlignDeg = 2f;
 
+    // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
+    // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
+    [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
+    public float FleetRotateOmega = 15f;
+
+    [FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
+    public float FleetRotateTargetDeltaDeg = 90f;
+
+    [FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
+    public float FleetRotateArriveDeg = 1.5f;
+
+    [FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
+    public float FleetRotateSlowDeg = 25f;
+
+    [FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
+    public float FleetRotateMinOmega = 3f;
+
+    [FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
+    public float FleetRotateAccel = 20f;
+
+    [FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
+    public float FleetRotateSettleSec = 0.5f;
+
+    // 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
+    // false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
+    [FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
+    public bool FleetRotateUseDetourHeading = true;
+
+    // ===== 车队联动-自动蟹行（FleetCrabWalk）=====
+    // 以当前车队中心为起点，构造与车队朝向夹角 FleetCrabAngleDeg、长度 FleetCrabLengthMm 的直线路径，
+    // 复用脚本手动等价输入(mode=1)斜向平移；动作侧只把横向误差转换成小幅、带斜率限制的蟹行方向修正。
+    [FieldMember(desc = "车队蟹行：与车队朝向夹角(deg，逆时针为正)")]
+    public float FleetCrabAngleDeg = 45f;
+
+    [FieldMember(desc = "车队蟹行：路径长度(mm)")]
+    public float FleetCrabLengthMm = 2000f;
+
+    [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
+    public float FleetCrabSpeed = 0.2f;
+
+    // 兼容旧版几何控制器实现；当前自动蟹行走脚本手动等价输入，不再直接使用该上限。
+    [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";
@@ -152,15 +230,6 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
     [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
 
-    [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
-    [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
-    [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
-    [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
-    [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
-    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
-    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
-    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
-
     [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
     [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
     [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;

MultiWheel/MultiWheelC/PilotDefinition.cs

@@ -41,6 +41,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     [AsLowerIO(desc = "多车联动：遥控器Vy")] public float MultiVehicleManualVy;
     [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth;
 
+    // ===== Clumsy 侧脚本/动作驱动的手动等价输入（不走 Medulla IO，不会被 IO 同步覆盖）=====
+    // 手动 IO 字段是 [AsLowerIO]（Medulla→Clumsy，Medulla 每周期回写），Clumsy 侧 MovementTest 写它们会被覆盖。
+    // 因此提供这组内部字段，让 Clumsy 侧动作（如 FleetRotateInPlace）能像 FleetRemote 一样驱动车队联动：
+    // ScriptEnabled=使能；Mode 0=常规 1=蟹行 2=原地旋转；Vx/Vy/Vth 语义与手动遥控完全一致（m/s、m/s、deg/s）。
+    [FieldMember(desc = "多车联动：脚本驱动使能")] public bool MultiVehicleScriptEnabled;
+    [FieldMember(desc = "多车联动：脚本驱动模式")] public int MultiVehicleScriptMode;
+    [FieldMember(desc = "多车联动：脚本驱动Vx")] public float MultiVehicleScriptVx;
+    [FieldMember(desc = "多车联动：脚本驱动Vy")] public float MultiVehicleScriptVy;
+    [FieldMember(desc = "多车联动：脚本驱动Vth")] public float MultiVehicleScriptVth;
+
     [AsUpperIO(desc = "多车联动：自动驾驶", timeOutReset = true)] public bool MultiVehicleAutoEnabled;
     [FieldMember(desc = "多车联动：自动驾驶Vx")] public float MultiVehicleAutoVx;
     [FieldMember(desc = "多车联动：自动驾驶FrontTh")] public float MultiVehicleAutoFrontTh;
@@ -49,24 +59,47 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     public Dictionary<int, VehicleSyncInfo> MultiVehicleFleet = new();
     public VehicleSyncNotification MultiVehicleNotification;
 
+    // A: 互斥锁固定为独立 readonly 对象，绝不随 MultiVehicleFleet 字段被整体替换而失效。
+    //    所有对 MultiVehicleFleet / _multiVehicleFleetSeen 的读写都必须 lock(FleetLock)。
+    public readonly object FleetLock = new();
+
+    // C: 各 fleet 成员最近一次被 register/notify 刷新的本地时刻（用本地时钟，避免远端时钟偏差）。
+    //    主车 Tick 据此剔除掉线成员；编队就绪要求所有成员新鲜。
+    private readonly Dictionary<int, DateTime> _multiVehicleFleetSeen = new();
+
     [FieldMember(desc = "多车联动：车队姿态x")] public float CenterX;
     [FieldMember(desc = "多车联动：车队姿态y")] public float CenterY;
     [FieldMember(desc = "多车联动：车队姿态th")] public float CenterTh;
 
+    // G: 车队中心原子快照。三个 float 无法整体原子写，改为整体新建对象后引用赋值（引用赋值原子），
+    //    路径控制器线程只读最近一次完整快照，避免读到 x 新 / y,th 旧的撕裂组合。
+    public sealed class FleetCenterSnapshot
+    {
+        public float X, Y, Th;
+        public long Tick;
+    }
+    private volatile FleetCenterSnapshot _fleetCenter = new();
+    public FleetCenterSnapshot GetFleetCenterSnapshot() => _fleetCenter;
+    private void PublishFleetCenter(float x, float y, float th)
+    {
+        CenterX = x;
+        CenterY = y;
+        CenterTh = th;
+        _fleetCenter = new FleetCenterSnapshot { X = x, Y = y, Th = th, Tick = DateTime.Now.Ticks };
+    }
+
+    // B: 路径控制器最近一次写入自动速度命令的时刻；超时即视为失效（路径结束/早退/卡顿），清零下发。
+    public DateTime MultiVehicleAutoCmdTime = DateTime.MinValue;
+    // D: 路径控制器透传的理想车队中心位姿（世界系），由 idealPos/idealAngle 写入。
+    public bool MultiVehicleAutoHasIdeal;
+    public float MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh;
+
+    // F: notify 序列号（主车单调递增）与从车已应用的最大序列号（丢弃乱序旧包）。
+    private long _multiVehicleNotifySeq;
+    private long _multiVehicleAppliedSeq = -1;
+
     [AsLowerIO(desc = "车号")] public int CarNum = 1;
 
-    #region 夹臂变量
-    [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
-
-    [AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
-
-    [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
-
-    [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
-
-    [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
-    #endregion
-
     [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
     [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
     [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
@@ -76,11 +109,6 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
     [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
 
-    [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
-    [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
-    [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
-    [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
-
     [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
     [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
 
@@ -97,6 +125,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
     // 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
     private float _rotIntegX, _rotIntegY, _rotIntegTh;
+    private float _rotOmegaPeak; // 本次旋转过程中观测到的指令角速度峰值(deg/s)，用于纠偏随转速缩放
     private DateTime _rotPiLastTime = DateTime.MinValue;
 
     // 原地旋转“两车实际 sim 位姿”诊断采样状态（仅主车，通过 Playground WebAPI 读取）。
@@ -211,8 +240,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 var infoJson = Uri.UnescapeDataString(query.Info ?? "");
                 var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(infoJson)
                            ?? throw new Exception("VehicleSyncInfo is null");
-                lock (MultiVehicleFleet)
+                lock (FleetLock)
+                {
                     MultiVehicleFleet[query.CarNum] = info;
+                    _multiVehicleFleetSeen[query.CarNum] = DateTime.Now; // C: 刷新存活时刻
+                }
                 return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
             }
             catch (Exception e)
@@ -222,16 +254,30 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             }
         });
 
-        PicoHttpServer.AddGetHandler("/multi-vehicle-notify", new { Notification = "" }, query =>
+        // F: notify 改用 POST + JSON body（取代 GET query 串），避免整队 Fleet 字典撑爆 URL 长度上限；
+        //    用 Seq 丢弃乱序到达的旧包，避免从车短暂套用过期指令。
+        PicoHttpServer.AddPostTextHandler("/multi-vehicle-notify", body =>
         {
             try
             {
-                var notifyJson = Uri.UnescapeDataString(query.Notification ?? "");
-                var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(notifyJson)
+                var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(body ?? "")
                                    ?? throw new Exception("notification is null");
+                // 乱序丢弃：仅应用序列号大于已应用值的包。Seq==0 视为旧版无序列号始终接受；
+                // 若 Seq 明显回退(差值>100)，判定为主车重启的新会话，重新接受并对齐序列号。
+                if (notification.Seq != 0 && notification.Seq <= _multiVehicleAppliedSeq &&
+                    notification.Seq > _multiVehicleAppliedSeq - 100)
+                    return JsonConvert.SerializeObject(new { code = 200, message = "stale" });
+                _multiVehicleAppliedSeq = notification.Seq;
+
                 MultiVehicleAligned = notification.Aligned;
-                lock (MultiVehicleFleet)
+                lock (FleetLock)
+                {
                     MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
+                    // C: notify 内含整队成员，逐一刷新其本地存活时刻。
+                    var nowSeen = DateTime.Now;
+                    foreach (var key in MultiVehicleFleet.Keys)
+                        _multiVehicleFleetSeen[key] = nowSeen;
+                }
                 lock (_multiVehicleNotificationLock)
                 {
                     MultiVehicleNotification = notification;
@@ -298,7 +344,14 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     {
         var isMaster = IsMultiVehicleMaster();
         var autoEnabled = false;
-        var manualEnabled = MultiVehicleManualEnabled;
+        // 手动等价输入 = Medulla 遥控(IO) 或 Clumsy 脚本/动作驱动（二者择一，脚本优先）。
+        var scriptOn = MultiVehicleScriptEnabled;
+        var manualEnabled = MultiVehicleManualEnabled || scriptOn;
+        // 主车实际生效的手动模式与速度：脚本使能时取脚本字段，否则取 Medulla 手动 IO 字段。
+        var manualMode = scriptOn ? MultiVehicleScriptMode : MultiVehicleManualMode;
+        var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
+        var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
+        var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
 
         if (isMaster)
             autoEnabled = MultiVehicleAutoEnabled;
@@ -324,24 +377,30 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         {
             _mvDiskLastLog = DateTime.Now;
             int fleetCnt;
-            lock (MultiVehicleFleet) fleetCnt = MultiVehicleFleet.Count;
+            lock (FleetLock) fleetCnt = MultiVehicleFleet.Count;
             var notifFresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
                              Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
             FleetDiag(
                 $"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
                 $"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
+                $"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
                 $"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
                 $"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
-                $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} pos={Conf.PosAvailable}");
+                $"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} useDetour={Conf.MultiVehicleSyncUseDetour}");
         }
 
         if (!manualEnabled && !autoEnabled)
         {
             UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
             sendMotionPainter.Clear();
-            lock (MultiVehicleFleet)
+            lock (FleetLock)
+            {
                 MultiVehicleFleet.Clear();
+                _multiVehicleFleetSeen.Clear();
+            }
             MultiVehicleAutoEnabled = false;
+            MultiVehicleAutoHasIdeal = false;
+            _multiVehicleAppliedSeq = -1;
             _multiVehicleAccumulateTh = 0f;
             _multiVehicleLastThTime = DateTime.Now;
 
@@ -351,41 +410,53 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             return;
         }
 
-        if (isMaster && autoEnabled && !MultiVehicleManualEnabled && !FleetHasPosAvailable())
+        // 自动模式整队姿态依赖 Detour 全局定位（与 MultiVehicleSyncUseDetour 无关）：要求编队至少一台车有定位。
+        if (isMaster && autoEnabled && !manualEnabled && !FleetHasPosAvailable())
         {
             MultiVehicleAutoEnabled = false;
             Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
             return;
         }
 
-        var posAvailable = Conf.PosAvailable;
+        // 两类用途解耦（关键语义）：
+        //  - useDetourCorrection (= MultiVehicleSyncUseDetour)：是否用 SLAM 位姿做"车队内姿态纠正"(POS 补偿)。
+        //    false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
+        //  - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
+        //    SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
+        //    纠偏开启时本车也读。读取若因无有效定位阻塞，则联动线程随之阻塞、不下发速度（安全停车）。
+        var autoMode = autoEnabled && !manualEnabled;
+        var useDetourCorrection = Conf.MultiVehicleSyncUseDetour;
+        var slamRead = useDetourCorrection || (isMaster && autoMode);
         float selfX = 0, selfY = 0, selfTh = 0;
-        if (posAvailable)
+        if (slamRead)
         {
             var carPos = DetourInterface.getCartLocation();
             selfX = (float)carPos.x;
             selfY = (float)carPos.y;
             selfTh = (float)carPos.th;
         }
+        // fleetPosValid：整队全局姿态是否已知。主车=自身读到 SLAM；从车=主车广播标志（下方覆盖）。
+        var fleetPosValid = slamRead;
 
         var syncTh = Conf.TestCarSyncTh;
         var syncDistance = Conf.TestCarSyncDistance;
         var deltaDetectCenter = Conf.DeltaDetectCenter;
         float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
         var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转
-
-        CenterX = CenterY = CenterTh = 0;
+        float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
+        float crabSteerLimit = 0;
+        var crabReverseEquivalent = false;
 
         if (isMaster)
         {
-            if (MultiVehicleManualEnabled)
+            if (manualEnabled)
             {
                 syncTh = 0;
-                fleetMode = MultiVehicleManualMode;
+                fleetMode = manualMode;
                 if (fleetMode == 2)
                 {
                     // 原地旋转：摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心。
-                    fleetOmega = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
+                    fleetOmega = manualVth * Conf.ManualCarSyncVthFac;
                     fleetVx = 0;
                     fleetFrontTh = 0;
                     fleetRearTh = 0;
@@ -395,13 +466,19 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 else if (fleetMode == 1)
                 {
                     // 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
-                    // 舵轮角限制在 ±90°，超出则取反向并令速度取负，避免出现 180° 这类不可达转角。
-                    var vx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
-                    var vy = MultiVehicleManualVy * Conf.ManualCarSyncVyFac;
+                    // 舵轮物理/仿真限制约为 ±120°，不要把 ±90° 当边界；否则纯横移附近会在
+                    // -90° 与 +90°/反向速度两种等价表示之间跳变。只有超过蟹行舵角上限时才取等价反向。
+                    var vx = manualVx * Conf.ManualCarSyncVxFac;
+                    var vy = manualVy * Conf.ManualCarSyncVyFac;
                     var speed = (float)Math.Sqrt(vx * vx + vy * vy);
                     var crabAngle = (float)(Math.Atan2(vy, vx) * 180.0 / Math.PI);
-                    if (crabAngle > 90f) { crabAngle -= 180f; speed = -speed; }
-                    else if (crabAngle < -90f) { crabAngle += 180f; speed = -speed; }
+                    crabInputVx = vx;
+                    crabInputVy = vy;
+                    crabRawAngle = crabAngle;
+                    crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
+
+                    if (crabAngle > crabSteerLimit) { crabAngle -= 180f; speed = -speed; crabReverseEquivalent = true; }
+                    else if (crabAngle < -crabSteerLimit) { crabAngle += 180f; speed = -speed; crabReverseEquivalent = true; }
                     fleetVx = speed;
                     fleetFrontTh = crabAngle;
                     fleetRearTh = crabAngle;
@@ -410,8 +487,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 }
                 else
                 {
-                    fleetVx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
-                    var targetTh = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
+                    fleetVx = manualVx * Conf.ManualCarSyncVxFac;
+                    var targetTh = manualVth * Conf.ManualCarSyncVthFac;
                     var now = DateTime.Now;
                     var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
                     _multiVehicleLastThTime = now;
@@ -424,9 +501,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             }
             else
             {
-                fleetVx = MultiVehicleAutoVx;
-                fleetFrontTh = MultiVehicleAutoFrontTh;
-                fleetRearTh = MultiVehicleAutoRearTh;
+                // B: 自动模式速度命令新鲜度门控。路径结束/回调早退/卡顿后，控制器不再刷新 AutoCmdTime，
+                //    超时即视为失效：清零下发速度与 idealPos，关闭 AutoEnabled，避免车队按末速度滑行。
+                var autoTimeoutMs = Conf.MultiVehicleAutoCmdTimeoutMs > 0
+                    ? Conf.MultiVehicleAutoCmdTimeoutMs
+                    : Math.Max(200, Conf.MultiVehicleSyncInterval * 4);
+                var autoFresh = (DateTime.Now - MultiVehicleAutoCmdTime).TotalMilliseconds < autoTimeoutMs;
+                if (autoFresh)
+                {
+                    fleetVx = MultiVehicleAutoVx;
+                    fleetFrontTh = MultiVehicleAutoFrontTh;
+                    fleetRearTh = MultiVehicleAutoRearTh;
+                }
+                else
+                {
+                    fleetVx = fleetFrontTh = fleetRearTh = 0;
+                    MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0;
+                    MultiVehicleAutoHasIdeal = false;
+                    MultiVehicleAutoEnabled = false;
+                    Hedingben.ToastText("自动速度命令超时，已停车（路径结束/控制器停发）", "MultiVehicle-auto-timeout");
+                }
             }
         }
         else if (MultiVehicleNotification != null)
@@ -438,32 +532,42 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             syncTh = notification.SyncTh;
             syncDistance = notification.SyncDistance;
             deltaDetectCenter = notification.DeltaDetectCenter;
-            CenterX = notification.CenterX;
-            CenterY = notification.CenterY;
-            CenterTh = notification.CenterTh;
-            posAvailable = notification.PosAvailable;
+            PublishFleetCenter(notification.CenterX, notification.CenterY, notification.CenterTh);
+            // 从车整队姿态来自主车广播：以广播标志作为 fleetPosValid（自身 slamRead 仅决定是否做本车纠偏）。
+            fleetPosValid = notification.PosAvailable;
             MultiVehicleAutoEnabled = notification.AutoEnabled;
             fleetVx = notification.FleetVx;
             fleetFrontTh = notification.FleetFrontTh;
             fleetRearTh = notification.FleetRearTh;
             fleetMode = notification.Mode;
             fleetOmega = notification.FleetOmega;
+            // D: 从车采用主车广播的理想车队中心（弧线时由 idealPos/idealAngle 而来）做前馈目标。
+            MultiVehicleAutoHasIdeal = notification.HasIdeal;
+            MultiVehicleAutoIdealX = notification.IdealX;
+            MultiVehicleAutoIdealY = notification.IdealY;
+            MultiVehicleAutoIdealTh = notification.IdealTh;
         }
 
         var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
 
         if (isMaster)
         {
-            lock (MultiVehicleFleet)
-                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
+            lock (FleetLock)
+            {
+                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
                     layoutX, layoutY, layoutTh, true);
+                _multiVehicleFleetSeen[CarNum] = DateTime.Now; // 主车自身恒新鲜
+                PruneStaleFleetMembers();                      // C: 剔除掉线从车
+            }
 
             if (TryInferFleetCenter(out var cx, out var cy, out var cth))
-            {
-                CenterX = cx;
-                CenterY = cy;
-                CenterTh = cth;
-            }
+                PublishFleetCenter(cx, cy, cth);
+
+            // D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
+            //    使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
+            if (!manualEnabled && MultiVehicleAutoEnabled &&
+                Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal)
+                PublishFleetCenter(MultiVehicleAutoIdealX, MultiVehicleAutoIdealY, MultiVehicleAutoIdealTh);
         }
 
         var selfAligned = !Conf.MultiVehicleUseDetect;
@@ -488,8 +592,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             else selfAligned = false;
         }
 
-        // 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（两台车都需有定位）
-        if (float.IsNaN(currentSpacing) && posAvailable)
+        // 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（需本车已读到全局定位）
+        if (float.IsNaN(currentSpacing) && slamRead)
             currentSpacing = TryGetSlamSpacing(selfX, selfY);
 
         if (float.IsNaN(currentSpacing))
@@ -499,7 +603,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 $"间距 当前:{currentSpacing:F0}mm 目标:{syncDistance:F0}mm 差:{currentSpacing - syncDistance:F0}mm",
                 $"MultiVehicle{CarNum}-spacing");
 
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
         {
             MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum &&
                                   MultiVehicleFleet.Values.All(v => v.Aligned);
@@ -509,9 +613,24 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         // ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
         var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
         bool othersDetectOk;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
             othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
         var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);
+
+        // H: 自动模式（非手动）必须有有效车队中心——主车由 SLAM 反推、从车依赖主车广播 fleetPosValid。
+        //    自动模式整队姿态始终依赖 Detour（与 MultiVehicleSyncUseDetour 无关）：定位全程丢失时强制停车。
+        //    （用 Detour 时若定位丢失，getCartLocation() 已先行阻塞，此处再兜底要求有效车队中心。）
+        //    手动模式不受限（允许仅靠互识别/遥控行驶）。
+        if (canMove && autoMode && Conf.MultiVehicleAutoRequireFleetCenter)
+        {
+            var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _));
+            if (!fleetCenterValid)
+            {
+                canMove = false;
+                Hedingben.ToastText("自动模式无有效车队中心（定位丢失），已停车", $"MultiVehicle{CarNum}-autostop");
+            }
+        }
+
         if (!canMove)
         {
             fleetVx = 0;
@@ -531,7 +650,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
         var fleetReady = false;
         var fleetCount = 0;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
         {
             fleetCount = MultiVehicleFleet.Count;
             fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
@@ -545,7 +664,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         if (fleetReady)
         {
             chassis.SetOriginBias(layoutX, layoutY, layoutTh);
-            chassis.ControlPointRadius = syncDistance / 2f;
+            // E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
+            var controlRadius = Conf.MultiVehicleControlRadius > 0
+                ? Conf.MultiVehicleControlRadius
+                : syncDistance / 2f;
+            chassis.ControlPointRadius = controlRadius;
+            // #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
+            chassis.RotateCompTangentFrac = Conf.MultiVehicleRotateCompTangentFrac;
 
             if (canMove && Conf.MultiVehicleUseDetect)
             {
@@ -561,7 +686,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 thDetectCompensate = ClampBias(thDetectCompensate, Conf.MultiVehicleDetectBiasThThreshold);
             }
 
-            if (canMove && posAvailable && TryInferFleetCenter(out _, out _, out _))
+            // 车队内姿态纠正(POS 补偿)：仅当 useDetourCorrection 开启且本车读到全局定位时施加。
+            // 这是"定位参与车队内姿态纠正"的唯一开关点——关闭它不影响上面的整队姿态计算/安全门。
+            if (canMove && useDetourCorrection && slamRead && TryInferFleetCenter(out _, out _, out _))
             {
                 var supposedPos = LessMath.Transform2D(
                     Tuple.Create(CenterX, CenterY, CenterTh),
@@ -600,24 +727,41 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 var rotating = Math.Abs(fleetOmega) > Conf.MultiVehicleRotateActiveOmega;
                 if (canMove && rotating)
                 {
+                    // #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。
+                    var allowInteg = chassis.LastRotateAligned;
                     var errX = detectDx + posBiasX;    // mm，车体系：本车纵向(前+)应移动量
                     var errY = detectDy + posBiasY;    // mm，车体系：本车横向(左+)应移动量
                     var errTh = detectDth + posBiasTh; // deg，本车应转角
                     rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy,
                         Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
-                        Conf.MultiVehicleRotateCompXyMax, dt);
+                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
                     rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy,
                         Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac,
-                        Conf.MultiVehicleRotateCompXyMax, dt);
+                        Conf.MultiVehicleRotateCompXyMax, dt, allowInteg);
                     rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh,
                         Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac,
-                        Conf.MultiVehicleRotateCompThMax, dt);
+                        Conf.MultiVehicleRotateCompThMax, dt, allowInteg);
+
+                    // #1 纠偏随旋转指令缩放：comp ×= |fleetOmega| / 本次峰值。
+                    // 加速+匀速段峰值≈当前 → 系数≈1（全力纠偏，不削弱）；减速段当前<峰值 → 系数随转速同步下降。
+                    // 关键：旋转切向也∝转速，故"纠偏:切向"比例全程恒定=匀速段比例（远<1），既杜绝末段轮子乱打方向，
+                    // 又不像按切向钳位那样在匀速段就削弱纠偏。
+                    var absOmega = Math.Abs(fleetOmega);
+                    _rotOmegaPeak = Math.Max(_rotOmegaPeak, absOmega);
+                    if (_rotOmegaPeak > 1e-3f)
+                    {
+                        var compScale = Math.Min(1f, absOmega / _rotOmegaPeak);
+                        rotCompVx *= compScale;
+                        rotCompVy *= compScale;
+                        rotCompOmega *= compScale;
+                    }
                 }
                 else
                 {
-                    // 检测丢失或未指令旋转：清零补偿并复位积分/计时，停止时不再有残留驱动。
+                    // 检测丢失或未指令旋转：清零补偿并复位积分/计时/峰值，停止时不再有残留驱动。
                     _rotIntegX = _rotIntegY = _rotIntegTh = 0;
                     _rotPiLastTime = DateTime.MinValue;
+                    _rotOmegaPeak = 0;
                 }
 
                 _mvRotCompVx = rotCompVx;
@@ -642,7 +786,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                 _rotPoseEpisode = false;
                 _rotPosePrevTime = DateTime.MinValue;
                 // 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
-                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
+                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
                     localCompensateX: xDetectCompensate + xPosCompensate,
                     localCompensateY: yDetectCompensate + yPosCompensate,
                     localCompensateTh: thDetectCompensate + thPosCompensate);
@@ -660,11 +804,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
             var cx = xDetectCompensate + xPosCompensate;
             var cy = yDetectCompensate + yPosCompensate;
             var cth = thDetectCompensate + thPosCompensate;
+            var crabDbg = isMaster && manualEnabled && fleetMode == 1
+                ? $"| CRAB in({crabInputVx:F3},{crabInputVy:F3}) raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
+                : "";
 
             var dbg =
-                $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} pos:{posAvailable} " +
+                $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} slam:{slamRead} corr:{useDetourCorrection} fleetPos:{fleetPosValid} " +
                 $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " +
                 $"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
+                crabDbg +
                 $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
                 $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} " +
                 $"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
@@ -690,20 +838,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
         if (isMaster)
         {
-            lock (MultiVehicleFleet)
+            lock (FleetLock)
             {
-                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
+                MultiVehicleFleet[CarNum] = BuildSelfInfo(true, slamRead, selfX, selfY, selfTh,
                     layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
             }
 
             if (fleetReady)
             {
                 VehicleSyncNotification notification;
-                lock (MultiVehicleFleet)
+                lock (FleetLock)
                 {
                     notification = new VehicleSyncNotification
                     {
-                        PosAvailable = posAvailable,
+                        // 广播"整队姿态有效"：主车读到全局定位即为真，从车据此放行自动模式安全门。
+                        PosAvailable = slamRead,
                         CenterX = CenterX,
                         CenterY = CenterY,
                         CenterTh = CenterTh,
@@ -715,10 +864,19 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                         Mode = fleetMode,
                         FleetOmega = fleetOmega,
                         AutoEnabled = MultiVehicleAutoEnabled,
-                        ManualEnabled = MultiVehicleManualEnabled,
+                        // 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
+                        ManualEnabled = manualEnabled,
                         SyncTh = syncTh,
                         SyncDistance = syncDistance,
-                        DeltaDetectCenter = deltaDetectCenter
+                        DeltaDetectCenter = deltaDetectCenter,
+                        // F: 单调递增序列号（从 1 起），从车据此丢弃乱序旧包。
+                        Seq = ++_multiVehicleNotifySeq,
+                        // D: 透传理想车队中心（仅自动模式且控制器给出时有效）。
+                        HasIdeal = !manualEnabled && MultiVehicleAutoEnabled &&
+                                   Conf.MultiVehicleAutoUseIdealCenter && MultiVehicleAutoHasIdeal,
+                        IdealX = MultiVehicleAutoIdealX,
+                        IdealY = MultiVehicleAutoIdealY,
+                        IdealTh = MultiVehicleAutoIdealTh
                     };
                 }
 
@@ -734,16 +892,40 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         }
         else
         {
-            FireAndForgetRegister(hc, BuildSelfInfo(false, posAvailable, selfX, selfY, selfTh,
+            FireAndForgetRegister(hc, BuildSelfInfo(false, slamRead, selfX, selfY, selfTh,
                 layoutX, layoutY, layoutTh, selfAligned, ownDetectOk));
         }
     }
 
     private bool IsMultiVehicleMaster() => Conf.MultiVehicleMasterEndpoint == "/";
 
+    /// <summary>
+    /// C: 剔除超过 TTL 未刷新（register/notify）的 fleet 成员。调用方须已持有 FleetLock。
+    /// 从车崩溃/断网后其条目过期被删除，使 fleetReady(数量==总数) 同时蕴含"全部成员在线且新鲜"，
+    /// 主车不再基于过期 layout/DetectOk 继续 SendMotion+notify。
+    /// </summary>
+    private void PruneStaleFleetMembers()
+    {
+        var ttlMs = Conf.MultiVehicleMemberTtlMs > 0
+            ? Conf.MultiVehicleMemberTtlMs
+            : Math.Max(500, Conf.MultiVehicleSyncInterval * 6);
+        var now = DateTime.Now;
+        var stale = MultiVehicleFleet.Keys
+            .Where(k => k != CarNum &&
+                        (!_multiVehicleFleetSeen.TryGetValue(k, out var seen) ||
+                         (now - seen).TotalMilliseconds > ttlMs))
+            .ToList();
+        foreach (var k in stale)
+        {
+            MultiVehicleFleet.Remove(k);
+            _multiVehicleFleetSeen.Remove(k);
+            Hedingben.ToastText($"剔除掉线成员 car{k}（{ttlMs:F0}ms 未刷新）", $"MultiVehicle{CarNum}-prune");
+        }
+    }
+
     private bool FleetHasPosAvailable()
     {
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
             return MultiVehicleFleet.Values.Any(v => v.PosAvailable);
     }
 
@@ -751,7 +933,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     private float TryGetSlamSpacing(float selfX, float selfY)
     {
         List<VehicleSyncInfo> others;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
             others = MultiVehicleFleet
                 .Where(kv => kv.Key != CarNum && kv.Value.PosAvailable)
                 .Select(kv => kv.Value).ToList();
@@ -764,7 +946,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     {
         centerX = centerY = centerTh = 0;
         List<VehicleSyncInfo> positioned;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
             positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList();
 
         if (positioned.Count == 0) return false;
@@ -791,6 +973,56 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         return ((float)center.Item1, (float)center.Item2, (float)center.Item3);
     }
 
+    /// <summary>
+    /// 由本车（通常为主车）当前 Detour SLAM 位姿反推车队中心位姿（世界系）。
+    /// 复用 GetLayoutPose + InferFleetCenterFromCar 的同款 SE(2) 反推（center = carWorld ∘ layout⁻¹），
+    /// 保证与自动模式车队中心计算一致。供动作（如 FleetCrabWalk）取路径起点用；无有效定位时返回 false。
+    /// </summary>
+    public bool TryGetFleetCenterFromSlam(out float centerX, out float centerY, out float centerTh)
+    {
+        centerX = centerY = centerTh = 0;
+        var carPos = DetourInterface.getCartLocation();
+        var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
+        var self = new VehicleSyncInfo
+        {
+            X = (float)carPos.x,
+            Y = (float)carPos.y,
+            Th = (float)carPos.th,
+            LayoutX = layoutX,
+            LayoutY = layoutY,
+            LayoutTh = layoutTh
+        };
+        var (cx, cy, cth) = InferFleetCenterFromCar(self);
+        centerX = cx;
+        centerY = cy;
+        centerTh = cth;
+        return true;
+    }
+
+    /// <summary>
+    /// 预热：用主车 SLAM 位姿，立即(1) 对外发布有效"车队中心快照"，(2) 把主车自身以 posAvailable=true
+    /// 写入 fleet 表。供 FleetCrabWalk 等动作在启动控制器 Track() 前调用，解决两个启动期问题：
+    ///  - 控制器首帧通过 MultiVehicleGetFleetPos 读到 (0,0,0) → 误判已到终点、立即结束；
+    ///  - 自动联动循环的安全门 FleetHasPosAvailable（PilotDefinition.cs ~402）在冷启动(无车上报定位)时
+    ///    会把 AutoEnabled 关掉、循环无法发布真实中心。播种主车 posAvailable 条目即可越过该门，使循环正常接管。
+    /// 需在主车上调用；getCartLocation() 无定位时会阻塞（与自动模式一致）。
+    /// </summary>
+    public bool PrimeMasterAutoFromSlam()
+    {
+        var carPos = DetourInterface.getCartLocation();
+        var (lx, ly, lth) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
+        var info = BuildSelfInfo(true, true, (float)carPos.x, (float)carPos.y, (float)carPos.th,
+            lx, ly, lth, false);
+        var (cx, cy, cth) = InferFleetCenterFromCar(info);
+        PublishFleetCenter(cx, cy, cth);
+        lock (FleetLock)
+        {
+            MultiVehicleFleet[CarNum] = info;
+            _multiVehicleFleetSeen[CarNum] = DateTime.Now;
+        }
+        return true;
+    }
+
     private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
     {
         // 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
@@ -829,7 +1061,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         // 这样手动模式无 SLAM 定位也能正确显示编队相对关系；避免之前用车队系 layout 坐标叠加本车位姿造成的整体平移。
         var fleetPainter = UI.GetPainter("MultiVehicleFleet-vis", false);
         Dictionary<int, VehicleSyncInfo> fleet;
-        lock (MultiVehicleFleet)
+        lock (FleetLock)
             fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet);
 
         var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh);
@@ -874,10 +1106,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
 
     private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
     {
+        // F: POST + JSON body，payload 不再受 URL 长度限制；fire-and-forget 但记录失败。
         var notifyJson = JsonConvert.SerializeObject(notification);
-        var url = $"http://{ip}:{port}/multi-vehicle-notify?Notification={Uri.EscapeDataString(notifyJson)}";
-        _ = hc.GetStringAsync(url).ContinueWith(t =>
+        var url = $"http://{ip}:{port}/multi-vehicle-notify";
+        var content = new StringContent(notifyJson, System.Text.Encoding.UTF8, "application/json");
+        _ = hc.PostAsync(url, content).ContinueWith(t =>
         {
+            content.Dispose();
             if (t.IsFaulted)
                 DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
         }, TaskScheduler.Default);
@@ -904,18 +1139,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     /// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。
     /// </summary>
     private static float RotatePiTerm(float err, ref float integ, float deadband,
-        float pFac, float iFac, float max, float dt)
+        float pFac, float iFac, float max, float dt, bool allowIntegrate = true)
     {
         // 死区内：不再累加误差，仅输出已积累的积分项(维持对恒定扰动的稳态补偿)。
         if (Math.Abs(err) < deadband)
             return ClampBias(integ * iFac, max);
 
-        // 条件积分(抗 windup)：仅当总输出未在同向饱和时才累加误差。
-        // 起步阶段大误差会让 P 项接近/超过 max，此时继续积分会顶满积分器，
-        // 误差反向后需很久才能泄放，造成纠偏"过冲再回拉"。此处饱和即停积分。
+        // 条件积分(抗 windup)：仅当 (a) 允许积分(舵轮已对齐、车在真正转动) 且
+        // (b) 总输出未在同向饱和 时才累加误差。
+        // allowIntegrate=false：上一拍舵轮未对齐(gate=0、车没动)，此时积分误差是纯 windup，
+        // 会让纠偏越积越大、舵轮更对不齐 → 原地卡死；故冻结积分(保留已有值，仅输出 P+已积分)。
+        // 同向饱和停积分：起步大误差让 P 顶满 max 时继续积分会顶满积分器，误差反向后泄放慢、造成过冲回拉。
         var unclamped = err * pFac + integ * iFac;
         var saturatedSameSign = Math.Abs(unclamped) >= max && Math.Sign(unclamped) == Math.Sign(err);
-        if (!saturatedSameSign)
+        if (allowIntegrate && !saturatedSameSign)
             integ += err * dt;
 
         var iTerm = integ * iFac;

MultiWheel/MultiWheelC/VehicleSyncModels.cs

@@ -41,4 +41,12 @@ public class VehicleSyncNotification
     [JsonProperty("SyncTh")] public float SyncTh { get; set; }
     [JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
     [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
+    // F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
+    [JsonProperty("Seq")] public long Seq { get; set; }
+    // D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
+    // HasIdeal=true 时各从车按各自 layout 推算 per-car 目标位姿做前馈+补偿，弧线路径不再只靠事后纠偏。
+    [JsonProperty("HasIdeal")] public bool HasIdeal { get; set; }
+    [JsonProperty("IdealX")] public float IdealX { get; set; }
+    [JsonProperty("IdealY")] public float IdealY { get; set; }
+    [JsonProperty("IdealTh")] public float IdealTh { get; set; }
 }

MultiWheel/MultiWheelM/MotorRoutine.cs

@@ -6,13 +6,6 @@ public class MotorRoutine : LadderLogic<CartDefinition>
 {
     public override void Operation(int iteration)
     {
-        cart.ActualSpeedLeftFront = cart.SpeedLeftFront;
-        cart.ActualSpeedLeftRear = cart.SpeedLeftRear;
-        cart.ActualSpeedRightFront = cart.SpeedRightFront;
-        cart.ActualSpeedRightRear = cart.SpeedRightRear;
-        cart.ActualThLeftFront = cart.ThLeftFront;
-        cart.ActualThLeftRear = cart.ThLeftRear;
-        cart.ActualThRightFront = cart.ThRightFront;
-        cart.ActualThRightRear = cart.ThRightRear;
+        
     }
 }