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

.gitattributes

@@ -0,0 +1,2 @@
+*.cs text eol=crlf
+.gitattributes text eol=lf

MultiWheel/MultiWheelC/AGV.cs

@@ -1,6 +1,11 @@
 using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Sensors;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
+using MDCSToolBox.Clumsy.Calibration;
 using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Tracks;
 using MDCSToolBox.Commons.Controllers;
@@ -9,6 +14,7 @@ using System;
 using System.Collections.Generic;
 using System.Net.Http;
 using System.Numerics;
+using System.Security.Cryptography;
 using System.Threading;
 using System.Threading.Tasks;
 using static ClumsyCore.DTools.Painter;
@@ -63,6 +69,25 @@ namespace MultiWheelC
                 PilotDefinition.Self.IOObstacleArea = area;
             }
         }
+        public void RotateToTarget(float target)
+        {
+            //if (!needrotate) return;
+            var dl = new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = target,
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get());
+            dl.Wait();
+        }
 
         //参数1：tireNum 需要钻过的轮胎对数量
         //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
@@ -132,7 +157,7 @@ namespace MultiWheelC
                     0)
                 },
             };
-            DLog.Log($"检测器数量为{detectors.Count}", "TireFollowing");
+            DLog.Log($"钻胎为{tireNum}", "TireFollowing");
             var following = new TireFollowing()
             {
                 GetController = () => new ChassisController().Get(),
@@ -141,7 +166,7 @@ namespace MultiWheelC
                 detectors = detectors,
                 SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
                 MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
-                TireNum = detectors.Count,
+                TireNum = tireNum,
                 CarDirection = frontLidarDetect ? 0f : 180f,
                 WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
             };
@@ -151,7 +176,7 @@ namespace MultiWheelC
         }
 
         //离车一定是后雷达识别一个轮胎
-        public void LeaveCar(int srcId, int dstId)
+        public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
         {
             while (!TryLock(dstId))
             {
@@ -159,7 +184,8 @@ namespace MultiWheelC
             }
             DLog.Log($"锁点{dstId}完成", "TireFollowing");
             DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
-
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
             var following = new TireFollowing()
             {
                 GetController = () => new ChassisController().Get(),
@@ -172,7 +198,7 @@ namespace MultiWheelC
                         DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
                         StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
                         StartGuessingY = 0,
-                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
                         FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
                         PathTransformation = new Tuple<float, float, float>(
                         PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
@@ -185,13 +211,91 @@ namespace MultiWheelC
                 },
                 CarDirection = 180f,
                 SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                MaxSpeed = 0.25f,
+                EnableHandover = true,
+                HandoverDistance = 200f,
+                HandoverSpeed = 0.3f,
                 WalkBlindTh = 0,
                 TireNum = 1
             };
-            var _dt = new DriveTask(following.Get());
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    CarDirectionBias = 180f,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            var _dt = new DriveTask(LeaveThenFollow());
             _dt.Wait();
-            DLog.Log("钻车动作结束", "TireFollowing");
+            DLog.Log("离车动作1结束", "TireFollowing");
+        }
+
+        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
+        {
+            while (!TryLock(dstId))
+            {
+                Thread.Sleep(50);
+            }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    LeaveSrcFunction = Leave,
+                    SrcId = srcId,
+                    EnableHandover = true,
+                    HandoverDistance = 200, 
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
+                DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
+                while (!TryLock(426))
+                {
+                    Thread.Sleep(20);
+                }
+                Leave(dstId);
+                DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
+                foreach (var running in new DstTracker()
+                {
+                    Src = new Vector2(srcX, srcY),
+                    Dst = new Vector2(dstX, dstY),
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            var _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+            DLog.Log("离车动作2结束", "TireFollowing");
         }
 
         //驱动器上使能
@@ -210,20 +314,187 @@ namespace MultiWheelC
             DLog.Log("驱动器下使能完成", "TireFollowing");
         }
 
-        public void LineTracking(int srcId, int dstId, float LineDistance)
+        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
+        public void ClamptoTarget(bool close)
         {
-            while (!TryLock(dstId))
+            if (PilotDefinition.Self.GhostMode)
             {
-                Thread.Sleep(50);
+                Thread.Sleep(2000);
+                Console.WriteLine("夹抱完成");
+                return;
             }
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
+            new DriveTask(new ClampToTarget()
-            new DriveTask(new LineTracking()
             {
-                Target = LineDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
-                LeaveSrcFunction = Leave,
+                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
-                SrcId = srcId,
             }.Get()).Wait();
         }
+        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
+        public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed)
+        {
+            var dx = dstX - srcX;
+            var dy = dstY - srcY;
+            var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
+
+            if (pathLength <= 1f)
+            {
+                DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
+                return;
+            }
+
+            var self = PilotDefinition.Self;
+            if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
+                !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+            {
+                DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
+                Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
+                return;
+            }
+
+            var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
+            var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
+            var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
+            var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
+            DLog.Log(
+                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
+                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
+                $"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
+                "FleetCrabDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
+            }
+
+            var action = new MultiWheelC.FleetCrabWalk
+            {
+                CrabAngleDeg = crabAngle,
+                BodyToPathAngleDeg = bodyToPathAngle,
+                CrabLengthMm = pathLength,
+                CrabSpeed = speed,
+                FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
+                FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
+                FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
+                FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
+                FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
+                FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
+                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
+                }
+            }
+        }
+
+        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed, params float[] trackTypeInfo)
+        {
+            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
+            {
+                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            var trackType = (int)trackTypeInfo[0];
+            if (trackType != 2)
+            {
+                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
+                return;
+            }
+
+            var controlPointNum = (int)trackTypeInfo[1];
+            var expectedLength = 2 + controlPointNum * 2;
+            if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
+            {
+                DLog.Log(
+                    $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
+                    $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            BezierTrack track;
+            try
+            {
+                track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
+            }
+            catch (Exception ex)
+            {
+                DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
+                return;
+            }
+
+            if (track == null)
+            {
+                DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
+                return;
+            }
+
+            track.Speed = speed;
+            track.CarDirectionBias = 0f;
+
+            DLog.Log(
+                $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
+                $"carDirectionBias=0.0)",
+                "FleetCurveDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
+            }
+
+            var action = new MultiWheelC.FleetCurveWalk
+            {
+                Track = track,
+                CurveSpeed = speed,
+                CarDirectionBias = 0f,
+                SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+                FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+                FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+                SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+                StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
+                }
+            }
+        }
         
         public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
         {
@@ -257,7 +528,7 @@ namespace MultiWheelC
                     if (setLocationRes != null && setLocationRes.l_step == 2) break;
                 }
             });
-
         }
+        public float baseSpeed = 0;
     }
 }

MultiWheel/MultiWheelC/FleetCrabWalk.cs

@@ -0,0 +1,526 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using CommonUsage.Chassis;
+using CommonUsage.Mathematics;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC;
+
+// ===== 车队联动-自动蟹行动作 =====
+// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
+//
+// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
+// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
+// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
+// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
+//
+// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
+public class FleetCrabWalk : MovementDefinition
+{
+    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
+    public float CrabAngleDeg = 45f;
+
+    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
+    public float BodyToPathAngleDeg = 45f;
+
+    /// <summary>路径长度(mm)。</summary>
+    public float CrabLengthMm = 2000f;
+
+    /// <summary>行驶速度(m/s)。</summary>
+    public float CrabSpeed = 0.2f;
+
+    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabAccel = 0.2f;
+
+    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
+    public float FleetCrabStartAccel = 0.01f;
+
+    /// <summary>末端开始减速距离(mm)。</summary>
+    public float FleetCrabSlowDistance = 2000f;
+
+    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
+    public float FleetCrabFinishDistance = 20f;
+
+    /// <summary>末端最低速度(m/s)。</summary>
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    /// <summary>末端减速曲线指数。</summary>
+    public float FleetCrabSlowingPow = 0.8f;
+
+    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
+    public float GcpThetaThreshold = 95f;
+
+    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 Clamp(float value, float min, float max)
+    {
+        if (value < min) return min;
+        if (value > max) return max;
+        return value;
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    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;
+    }
+
+    private static float AverageAngle(float frontTh, float rearTh)
+    {
+        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
+        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
+    }
+
+    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
+        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
+    {
+        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
+        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
+        driveSpeed = speed;
+        frontTh = rawFrontTh;
+        rearTh = rawRearTh;
+        reverseEquivalent = false;
+
+        if (rawBaseTh > limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+        else if (rawBaseTh < -limit)
+        {
+            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
+            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
+            driveSpeed = -driveSpeed;
+            reverseEquivalent = true;
+        }
+
+        frontTh = ClampAbs(frontTh, limit);
+        rearTh = ClampAbs(rearTh, limit);
+    }
+
+    private static float ProbeSpeed(float speed)
+    {
+        return Math.Abs(speed) > 1e-4f ? speed : 1f;
+    }
+
+    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
+        out float yawSign)
+    {
+        yawSign = 0f;
+        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
+            return false;
+
+        var radius = Math.Max(1f, Math.Abs(controlRadius));
+        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
+            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
+            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
+        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
+        if (!intersect)
+            return false;
+
+        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
+        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
+        var tangentSign = center.Y > 1f ? 1f : -1f;
+        var speedSign = driveSpeed >= 0f ? 1f : -1f;
+        yawSign = speedSign * tangentSign;
+        return true;
+    }
+
+    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
+    {
+        const float probeDth = 1f;
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
+            ? yawSign
+            : 1f;
+    }
+
+    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
+        Vector2 pathLeft)
+    {
+        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
+        var rad = motionTh / 180f * Math.PI;
+        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
+        if (driveSpeed < 0f)
+            dir = -dir;
+        return Vector2.Dot(dir, pathLeft);
+    }
+
+    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral, float biasProbe)
+    {
+        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
+            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
+        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
+        return -Math.Sign(lateral) * lateralVelocity;
+    }
+
+    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
+        float lateral)
+    {
+        if (Math.Abs(lateral) <= 1e-3f)
+            return 1f;
+
+        const float probeBias = 1f;
+        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
+        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
+        return positiveScore >= negativeScore ? 1f : -1f;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
+            yield break;
+        }
+        _stopping = false;
+
+        DLog.Log(
+            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
+            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
+            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
+            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
+            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
+            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac: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 (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
+            yield break;
+        }
+        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
+
+        var pathStart = new Vector2(x0, y0);
+        var pathLengthMm = CrabLengthMm;
+        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+        var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+        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}) pathMode=relative " +
+            $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
+            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
+            $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
+            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
+            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
+            "FleetCrabDbg");
+
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
+        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
+            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
+            out _, out _);
+        var warmStart = DateTime.Now;
+        var warmSeqBaseline = self.BeginFleetMotionWarmup();
+
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
+        self.MultiVehicleAutoIdealX = pathStart.X;
+        self.MultiVehicleAutoIdealY = pathStart.Y;
+        self.MultiVehicleAutoIdealTh = targetBodyTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+        self.PrimeMasterAutoFromSlam();
+        DLog.Log(
+            $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
+            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
+            "FleetCrabDbg");
+
+        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
+        var warmIter = 0;
+        var warmReady = false;
+        var warmDetail = "";
+        while (!_stopping && DateTime.Now < warmEnd)
+        {
+            warmIter++;
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            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} " +
+                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
+                    $"detail={warmDetail}",
+                    "FleetCrabDbg");
+            if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                    conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+            {
+                warmReady = true;
+                DLog.Log(
+                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
+                    "FleetCrabDbg");
+                break;
+            }
+            yield return true;
+        }
+        if (!warmReady)
+        {
+            DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
+                "FleetCrabDbg");
+            Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
+            Cleanup();
+            yield break;
+        }
+
+        Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
+
+        if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
+        {
+            x0 = warmX;
+            y0 = warmY;
+            theta = warmTh;
+            pathStart = new Vector2(x0, y0);
+            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
+            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
+            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
+            phiRad = phi / 180.0 * Math.PI;
+            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
+            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
+            self.MultiVehicleAutoIdealX = pathStart.X;
+            self.MultiVehicleAutoIdealY = pathStart.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            DLog.Log(
+                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
+                "FleetCrabDbg");
+        }
+
+        var iter = 0;
+        var lastLog = DateTime.MinValue;
+        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(FleetCrabFinishSpeed));
+        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
+        var accel = Math.Abs(FleetCrabAccel);
+        var startAccel = Math.Abs(FleetCrabStartAccel);
+        var cmdSpeed = 0f;
+        var lastTick = DateTime.Now;
+        var speedRampStart = DateTime.Now;
+        var stopReason = "done";
+
+        while (!_stopping)
+        {
+            iter++;
+
+            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
+            {
+                stopReason = "fleet center invalid";
+                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
+                break;
+            }
+            var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
+            var along = Vector2.Dot(delta, pathDir);
+            var lateral = Vector2.Dot(delta, pathLeft);
+            var remain = pathLengthMm - along;
+            if (remain <= finishDistance)
+                break;
+
+            var targetSpeed = baseSpeed;
+            var slowRatio = 1f;
+            if (remain < slowDistance)
+            {
+                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
+                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
+            }
+            var now = DateTime.Now;
+            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
+            lastTick = now;
+            var rampElapsed = (now - speedRampStart).TotalSeconds;
+            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
+            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
+            cmdSpeed = speed;
+
+            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
+            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
+            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
+            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
+            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
+                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
+            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
+            var rawBiasItem = rawBiasMagnitude * biasSign;
+            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
+            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
+            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
+            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
+            var rawFrontTh = baseCrabTh + biasItem + dthItem;
+            var rawRearTh = baseCrabTh + biasItem - dthItem;
+            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
+                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
+            holdFrontTh = frontTh;
+            holdRearTh = rearTh;
+            var idealAlong = Clamp(along, 0f, pathLengthMm);
+            var ideal = pathStart + pathDir * idealAlong;
+
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
+            self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = driveSpeed;
+            self.MultiVehicleAutoFrontTh = frontTh;
+            self.MultiVehicleAutoRearTh = rearTh;
+            self.MultiVehicleAutoIdealX = ideal.X;
+            self.MultiVehicleAutoIdealY = ideal.Y;
+            self.MultiVehicleAutoIdealTh = targetBodyTh;
+            self.MultiVehicleAutoHasIdeal = true;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+
+            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
+            {
+                lastLog = DateTime.Now;
+                var snap = self.GetFleetCenterSnapshot();
+                int fleetCnt;
+                lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
+                DLog.Log(
+                    $"ITER#{iter} centerSrc={centerSource} 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} " +
+                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed: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");
+                DLog.Log(
+                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
+                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
+                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
+                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
+                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
+                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
+                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
+                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
+                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
+                    "FleetCrabHeadingDbg");
+            }
+            yield return true;
+        }
+
+        if (_stopping)
+            stopReason = "stop";
+
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = holdFrontTh;
+        self.MultiVehicleAutoRearTh = holdRearTh;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+        DLog.Log(
+            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
+            "FleetCrabDbg");
+        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+        while (!_stopping && DateTime.Now < settleEnd)
+        {
+            self.MultiVehicleScriptEnabled = false;
+            self.MultiVehicleScriptMode = 0;
+            self.MultiVehicleAutoEnabled = true;
+            self.MultiVehicleAutoVx = 0;
+            self.MultiVehicleAutoFrontTh = holdFrontTh;
+            self.MultiVehicleAutoRearTh = holdRearTh;
+            self.MultiVehicleAutoCmdTime = DateTime.Now;
+            yield return true;
+        }
+
+        Cleanup();
+        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
+        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
+    }
+}

MultiWheel/MultiWheelC/FleetCurveWalk.cs

@@ -0,0 +1,411 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+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;
+
+public class FleetCurveWalk : MovementDefinition
+{
+    public BezierTrack Track;
+    public List<Vector2> ControlPoints = new();
+    public float CurveSpeed = 0.2f;
+    public float CarDirectionBias = 0f;
+    public int BezierResolution = 100;
+    public float SlowDistance = 2000f;
+    public float FinishDistance = 20f;
+    public float FinishSpeed = 0.02f;
+    public float SlowingPow = 0.8f;
+    public float GcpThetaThreshold = 95f;
+    public float StartSyncTimeoutSec = 8f;
+
+    private bool _stopping;
+    private MultiWheelGeometricController _controller;
+    private MultiWheelChassis _chassis;
+    private bool _savedControlPoints;
+    private float _savedControlRadius;
+    private Vector2 _savedGcp0;
+    private Vector2 _savedGcp1;
+
+    public void Stop()
+    {
+        _stopping = true;
+        if (_controller != null)
+            _controller.BreakAndHold = true;
+        Cleanup();
+    }
+
+    public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
+    {
+        points = new List<Vector2>();
+        error = "";
+        if (string.IsNullOrWhiteSpace(text))
+        {
+            error = "empty control point list";
+            return false;
+        }
+
+        var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
+        for (var i = 0; i < segments.Length; i++)
+        {
+            var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
+            if (pair.Length != 2)
+            {
+                error = $"invalid point #{i + 1}: {segments[i]}";
+                return false;
+            }
+
+            if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
+            {
+                error = $"invalid number in point #{i + 1}: {segments[i]}";
+                return false;
+            }
+            points.Add(new Vector2(x, y));
+        }
+
+        if (points.Count < 3)
+        {
+            error = "Bezier curve requires at least 3 control points";
+            return false;
+        }
+
+        return true;
+    }
+
+    public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
+    {
+        var source = relativePoints ?? new List<Vector2>();
+        var normalized = new List<Vector2>();
+        if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
+            normalized.Add(Vector2.Zero);
+        for (var i = 0; i < source.Count; i++)
+            normalized.Add(source[i]);
+        if (normalized.Count < 2)
+            normalized.Add(new Vector2(1000f, 0f));
+        if (normalized.Count < 3)
+            normalized.Add(new Vector2(2000f, 0f));
+
+        var result = new List<Vector2>();
+        for (var i = 0; i < normalized.Count; i++)
+            result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
+        return result;
+    }
+
+    public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
+        params float[] controlPointCoords)
+    {
+        var src = new Vector2(srcX, srcY);
+        var dst = new Vector2(dstX, dstY);
+        var result = new List<Vector2>();
+        if (controlPointCoords == null || controlPointCoords.Length == 0)
+        {
+            result.Add(src);
+            result.Add((src + dst) / 2f);
+            result.Add(dst);
+            return result;
+        }
+
+        if (controlPointCoords.Length % 2 != 0)
+            throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
+
+        var supplied = new List<Vector2>();
+        for (var i = 0; i < controlPointCoords.Length; i += 2)
+            supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
+
+        if (supplied.Count >= 3 &&
+            Vector2.Distance(supplied[0], src) <= 10f &&
+            Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
+            return supplied;
+
+        result.Add(src);
+        for (var i = 0; i < supplied.Count; i++)
+            result.Add(supplied[i]);
+        result.Add(dst);
+        if (result.Count < 3)
+            result.Insert(1, (src + dst) / 2f);
+        return result;
+    }
+
+    private static bool TryParseFloat(string text, out float value)
+    {
+        return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
+               float.TryParse(text, out value);
+    }
+
+    private static float ClampAbs(float value, float limit)
+    {
+        var absLimit = Math.Abs(limit);
+        if (absLimit <= 0) return value;
+        if (value > absLimit) return absLimit;
+        if (value < -absLimit) return -absLimit;
+        return value;
+    }
+
+    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
+        out float centerTh, out string source)
+    {
+        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
+        {
+            source = "fleet";
+            return true;
+        }
+
+        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
+        {
+            source = "slam";
+            return true;
+        }
+
+        source = "none";
+        return false;
+    }
+
+    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;
+        RestoreControlPointRadius();
+    }
+
+    private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
+    {
+        if (!_savedControlPoints)
+        {
+            _chassis = chassis;
+            _savedControlRadius = chassis.ControlPointRadius;
+            var gcps = chassis.GetGeometricControlPoints();
+            if (gcps.Count >= 2)
+            {
+                _savedGcp0 = gcps[0].Position;
+                _savedGcp1 = gcps[1].Position;
+            }
+            _savedControlPoints = true;
+        }
+
+        chassis.ControlPointRadius = radius;
+        var points = chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = new Vector2(radius, 0);
+            points[1].Position = new Vector2(-radius, 0);
+        }
+    }
+
+    private void RestoreControlPointRadius()
+    {
+        if (!_savedControlPoints || _chassis == null)
+            return;
+
+        _chassis.ControlPointRadius = _savedControlRadius;
+        var points = _chassis.GetGeometricControlPoints();
+        if (points.Count >= 2)
+        {
+            points[0].Position = _savedGcp0;
+            points[1].Position = _savedGcp1;
+        }
+        _savedControlPoints = false;
+    }
+
+    private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
+        float frontTh, float rearTh)
+    {
+        self.MultiVehicleScriptEnabled = false;
+        self.MultiVehicleScriptMode = 0;
+        self.MultiVehicleScriptVx = 0;
+        self.MultiVehicleScriptVy = 0;
+        self.MultiVehicleScriptVth = 0;
+        self.MultiVehicleAutoEnabled = true;
+        self.MultiVehicleAutoVx = 0;
+        self.MultiVehicleAutoFrontTh = frontTh;
+        self.MultiVehicleAutoRearTh = rearTh;
+        self.MultiVehicleAutoIdealX = idealPos.X;
+        self.MultiVehicleAutoIdealY = idealPos.Y;
+        self.MultiVehicleAutoIdealTh = idealTh;
+        self.MultiVehicleAutoHasIdeal = true;
+        self.MultiVehicleAutoCmdTime = DateTime.Now;
+    }
+
+    public override IEnumerable<bool> Get()
+    {
+        var self = PilotDefinition.Self;
+        var conf = PilotDefinition.Conf;
+        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
+        _stopping = false;
+
+        if (chassis == null)
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
+            yield break;
+        }
+
+        if (conf.MultiVehicleMasterEndpoint != "/")
+        {
+            DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
+                "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
+            yield break;
+        }
+
+        if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
+        {
+            DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
+            yield break;
+        }
+
+        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
+        {
+            DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
+            yield break;
+        }
+
+        var baseSpeed = Math.Abs(CurveSpeed);
+        if (baseSpeed <= 1e-4f)
+        {
+            DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
+            yield break;
+        }
+
+        var resolution = Math.Max(2, BezierResolution);
+        var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
+        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
+        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
+
+        ApplyFleetControlPointRadius(chassis, controlRadius);
+
+        try
+        {
+            var track = Track;
+            var trackSource = "external";
+            if (track == null)
+            {
+                var points = new List<Vector2>(ControlPoints);
+                track = new BezierTrack(points, resolution);
+                trackSource = "controlPoints";
+            }
+            track.CarDirectionBias = CarDirectionBias;
+            track.Speed = baseSpeed;
+
+            var center = new Vector2(x0, y0);
+            var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
+            var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
+            var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
+            var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
+
+            DLog.Log(
+                $"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
+                $"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
+                $"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
+                $"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
+                $"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
+                "FleetCurveDbg");
+
+            var warmStart = DateTime.Now;
+            var warmSeqBaseline = self.BeginFleetMotionWarmup();
+            WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+            self.PrimeMasterAutoFromSlam();
+
+            var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
+            var warmIter = 0;
+            var warmReady = false;
+            var warmDetail = "";
+            while (!_stopping && DateTime.Now < warmEnd)
+            {
+                warmIter++;
+                WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
+                self.PrimeMasterAutoFromSlam();
+
+                if (warmIter % 5 == 0)
+                {
+                    var snap = self.GetFleetCenterSnapshot();
+                    int cnt;
+                    lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
+                    DLog.Log(
+                        $"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
+                        $"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
+                        "FleetCurveDbg");
+                }
+
+                if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
+                        conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
+                {
+                    warmReady = true;
+                    DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
+                    break;
+                }
+                yield return true;
+            }
+
+            if (!warmReady)
+            {
+                DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
+                Cleanup();
+                yield break;
+            }
+
+            _controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
+            _controller.MultiVehicleSync = true;
+            _controller.BaseSpeed = baseSpeed;
+            _controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
+            _controller.FinishDistance = Math.Max(0f, FinishDistance);
+            _controller.FinishSpeed = speedFinish;
+            _controller.SlowingPow = Math.Max(0.01f, SlowingPow);
+            _controller.GcpThetaThreshold = gcpLimit;
+            _controller.AddTrack(track, "FleetCurve");
+
+            Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
+
+            foreach (var running in _controller.Track())
+            {
+                if (_stopping)
+                    break;
+                if (!running)
+                    break;
+                yield return true;
+            }
+
+            if (!_stopping)
+            {
+                self.MultiVehicleAutoVx = 0;
+                self.MultiVehicleAutoCmdTime = DateTime.Now;
+                var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
+                while (!_stopping && DateTime.Now < settleEnd)
+                {
+                    self.MultiVehicleAutoEnabled = true;
+                    self.MultiVehicleAutoVx = 0;
+                    self.MultiVehicleAutoCmdTime = DateTime.Now;
+                    yield return true;
+                }
+            }
+
+            DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
+        }
+        finally
+        {
+            Cleanup();
+            _controller = null;
+        }
+    }
+}

MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs

@@ -210,6 +210,52 @@ namespace MultiWheelC
         private DriveTask _dt;
     }
 
+    [MovementTest(name = "抱夹关闭")]
+    public class ClampTest1 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
+                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "抱夹打开")]
+    public class ClampTest2 : MovementTest
+    {
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            PilotDefinition.Self.SpeedLeftArm = 0;
+            PilotDefinition.Self.SpeedRightArm = 0;
+        }
+
+        public override void Test()
+        {
+            _dt = new DriveTask(new ClampToTarget()
+            {
+                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
+                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
     [MovementTest(name = "测试前进基于轮里程")]
     public class LineTrackingTest : MovementTest
     {
@@ -246,6 +292,206 @@ namespace MultiWheelC
         private DriveTask _dt;
     }
 
+    [MovementTest(name = "测试终点跟踪动作-前进")]
+    public class DstTrackerForward : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试终点跟踪动作-后退")]
+    public class DstTrackerhoutui : MovementTest
+    {
+        public bool UseInteractivePick = true;
+        public float srcX;
+        public float srcY;
+        public float dstX;
+        public float dstY;
+        public float carDirectionBias = 180f;
+        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var p1 = UI.GetPoint("point1");
+            var p2 = UI.GetPoint("point2");
+
+            _painter.Clear();
+            _dt = new DriveTask(new DstTracker()
+            {
+                Src = p1,
+                Dst = p2,
+                CarDirectionBias = carDirectionBias,
+            }.Get());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先直行再终点跟踪")]
+    public class LineTrackThenDstTrackerTest : MovementTest
+    {
+        public float carDirectionBias = 0f;
+        private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+            IEnumerable<bool> TrackThenFollow()
+            {
+                foreach (var running in new LineTracking()
+                {
+                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                    EnableHandover = true,
+                    HandoverDistance = 200f,
+                    HandoverSpeed = 0.3f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = carDirectionBias,
+                    InitialSendSpeed = 0.3f
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+                yield return false;
+            }
+            _dt = new DriveTask(TrackThenFollow());
+            _dt.Wait();
+        }
+        private DriveTask _dt;
+    }
+
+    [MovementTest(name = "测试先离车再终点跟踪")]
+    public class LeaveCarThenDstTrackerTest : MovementTest
+    {
+        private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
+
+        public override void TestStop()
+        {
+            _dt?.Stop();
+            _painter?.Clear();
+        }
+
+        public override void Test()
+        {
+            var src = UI.GetPoint("请在上位机选择离车后起点(src)");
+            var dst = UI.GetPoint("请在上位机选择终点(dst)");
+
+            _painter.Clear();
+            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
+            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
+            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
+            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
+            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
+
+            IEnumerable<bool> LeaveThenFollow()
+            {
+                var following = new TireFollowing()
+                {
+                    GetController = () => new ChassisController().Get(),
+                    GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
+                    GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
+                    detectors = new List<TireFollowing.DetectorDefinition>()
+                    {
+                        new TireFollowing.DetectorDefinition()
+                        {
+                            DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
+                            StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
+                            StartGuessingY = 0,
+                            SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
+                            FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
+                            PathTransformation = new Tuple<float, float, float>(
+                                PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
+                                PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
+                                0),
+                        },
+                    },
+                    CarDirection = 180f,
+                    SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
+                    MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                    WalkBlindTh = 0,
+                    TireNum = 1
+                };
+
+                foreach (var running in following.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                foreach (var running in new DstTracker()
+                {
+                    Src = src,
+                    Dst = dst,
+                    CarDirectionBias = 180f,
+                }.Get())
+                {
+                    if (!running) break;
+                    yield return true;
+                }
+
+                yield return false;
+            }
+
+            _dt = new DriveTask(LeaveThenFollow());
+            _dt.Wait();
+        }
+
+        private DriveTask _dt;
+    }
+
+
     [MovementTest(name = "驱动器下使能测试")]
     public class DriverDisableTest : MovementTest
     {
@@ -274,6 +520,35 @@ namespace MultiWheelC
         }
     }
 
+    [MovementTest(name = "底盘旋转测试")]
+    public class RotateToAngleTest : MovementTest
+    {
+        public override void TestStop()
+        {
+            throw new NotImplementedException();
+        }
+
+        public override void Test()
+        {
+            var target = UI.GetInput("输入旋转角度：");
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            new DriveTask(new MultiWheelRotateInPlace()
+            {
+                AngleTarget = float.Parse(target),
+                PidparamsRead = () => new PIDParams()
+                {
+                    Kp = PilotDefinition.Conf.TireFollowingThkp,
+                    Ki = PilotDefinition.Conf.TireFollowingThki,
+                    Kd = PilotDefinition.Conf.TireFollowingThkd,
+                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
+                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
+                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
+                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
+                }
+            }.Get()).Wait();
+        }
+    }
+
     public class utils
     {
         public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)

MultiWheel/MultiWheelC/MovementTests.TwoLegDetect.cs

@@ -86,12 +86,25 @@ public class TwoLegDetect : MovementTest
         };
 #pragma warning restore CS0612, CS0618
 
-        return detector.DetectWithGuess(
+        var result = detector.DetectWithGuess(
             lidarName,
             new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
             guessCoordinateSystem: CoordinateSystem.Car2D,
             outCoordinateSystem: CoordinateSystem.Car2D,
             filters);
+        return ApplyOutputBias(result);
+    }
+
+    private static LineSegment ApplyOutputBias(LineSegment result)
+    {
+        if (result == null) return null;
+
+        var conf = PilotDefinition.Conf;
+        if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
+            return result;
+
+        var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
+        return new LineSegment(result.Src + bias, result.Dst + bias);
     }
 
     /// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>

MultiWheel/MultiWheelC/MovementTests.cs

@@ -7,7 +7,6 @@ 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;
@@ -206,13 +205,17 @@ public class FleetRotateInPlace : MovementDefinition
         var start = DateTime.Now;
         var lastTime = start;
         var lastLog = DateTime.MinValue;
+        var lastCenterLog = DateTime.MinValue;
         var cmdMag = 0f;      // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
+        var centerTracking = false;
+        float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
+        float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 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} " +
+            $"REQUEST 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");
@@ -224,6 +227,58 @@ public class FleetRotateInPlace : MovementDefinition
         self.MultiVehicleScriptMode = 2;
         self.MultiVehicleScriptVth = 0;
         self.MultiVehicleScriptEnabled = true;
+        self.MultiVehicleRotateWheelsReady = false;
+        self.MultiVehicleRotateFleetReady = false;
+
+        DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
+        while (!self.MultiVehicleRotateFleetReady)
+        {
+            self.MultiVehicleScriptVx = 0;
+            self.MultiVehicleScriptVy = 0;
+            self.MultiVehicleScriptMode = 2;
+            self.MultiVehicleScriptVth = 0;
+            self.MultiVehicleScriptEnabled = true;
+            Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
+            yield return true;
+        }
+
+        float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
+        if (hasPos)
+        {
+            var startPos = DetourInterface.getCartLocation();
+            centerStartCarX = (float)startPos.x;
+            centerStartCarY = (float)startPos.y;
+            centerStartCarTh = (float)startPos.th;
+            prevTh = centerStartCarTh;
+            startTh = prevTh;
+            if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
+                    out centerStartX, out centerStartY, out centerStartTh))
+            {
+                centerLastX = centerStartX;
+                centerLastY = centerStartY;
+                centerLastTh = centerStartTh;
+                centerMaxDrift = 0;
+                centerTracking = true;
+            }
+        }
+        accumulated = 0f;
+        start = DateTime.Now;
+        lastTime = start;
+        lastLog = DateTime.MinValue;
+        lastCenterLog = DateTime.MinValue;
+        cmdMag = 0f;
+        DLog.Log(
+            $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
+            $"fleetAligned={self.MultiVehicleRotateFleetReady}",
+            "FleetRotateDbg");
+        if (centerTracking)
+        {
+            DLog.Log(
+                $"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                $"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
+                $"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
+                "FleetRotateCenterDbg");
+        }
 
         var stopReason = "stop()";
         while (true)
@@ -237,7 +292,8 @@ public class FleetRotateInPlace : MovementDefinition
             float curTh = 0f, remaining = 0f, actualRate = 0f;
             if (hasPos)
             {
-                curTh = (float)DetourInterface.getCartLocation().th;
+                var carPos = DetourInterface.getCartLocation();
+                curTh = (float)carPos.th;
                 var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
                 accumulated += step;
                 actualRate = dt > 1e-3 ? step / dt : 0f;            // 实际角速率(deg/s)，用于对比指令
@@ -251,6 +307,28 @@ public class FleetRotateInPlace : MovementDefinition
                 desiredMag = remaining < slowDeg
                     ? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
                     : maxOmega;
+
+                if (centerTracking &&
+                    self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
+                        out centerLastX, out centerLastY, out centerLastTh))
+                {
+                    var centerDx = centerLastX - centerStartX;
+                    var centerDy = centerLastY - centerStartY;
+                    var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+                    centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
+                    var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+                    if ((now - lastCenterLog).TotalMilliseconds >= 250)
+                    {
+                        lastCenterLog = now;
+                        DLog.Log(
+                            $"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+                            $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                            $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+                            $"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
+                            $"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
+                            "FleetRotateCenterDbg");
+                    }
+                }
             }
             else
             {
@@ -300,6 +378,19 @@ public class FleetRotateInPlace : MovementDefinition
             $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
             $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
             "FleetRotateDbg");
+        if (centerTracking)
+        {
+            var centerDx = centerLastX - centerStartX;
+            var centerDy = centerLastY - centerStartY;
+            var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
+            var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
+            DLog.Log(
+                $"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
+                $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
+                $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
+                $"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
+                "FleetRotateCenterDbg");
+        }
         Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
     }
 }
@@ -334,233 +425,57 @@ public class FleetRotateInPlaceTest : MovementTest
     }
 }
 
-// ===== 车队联动-自动蟹行动作 =====
+[MovementTest(name = "车队联动-曲线行走")]
-// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用
+public class FleetCurveWalkTest : MovementTest
-// 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>
+    private FleetCurveWalk _proc;
-    public float CrabAngleDeg = 45f;
+    private DriveTask _task;
 
-    /// <summary>路径长度(mm)。</summary>
+    public override void Test()
-    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;
+        if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
-        self.MultiVehicleScriptVy = 0;
+            !self.TryGetFleetCenterFromSlam(out x, out y, out th))
-        self.MultiVehicleScriptVth = 0;
+        {
-        self.MultiVehicleScriptMode = 0;
+            DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
-        self.MultiVehicleScriptEnabled = false;
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
-        self.MultiVehicleAutoVx = 0;
+            return;
-        self.MultiVehicleAutoFrontTh = 0;
+        }
-        self.MultiVehicleAutoRearTh = 0;
+
-        self.MultiVehicleAutoHasIdeal = false;
+        var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
-        self.MultiVehicleAutoEnabled = false;
+        var controlPoints = new List<Vector2>();
+        for (var i = 0; i < pointCount; i++)
+            controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
+        var fleetCenter = new Vector2(x, y);
+        if (Vector2.Distance(fleetCenter, controlPoints[0]) >
+            Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
+            controlPoints.Reverse();
+        var track = new BezierTrack(controlPoints)
+        {
+            Speed = PilotDefinition.Conf.FleetCurveSpeed,
+            CarDirectionBias = 0f
+        };
+
+        _proc = new FleetCurveWalk
+        {
+            Track = track,
+            CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
+            CarDirectionBias = 0f,
+            SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+            FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+            FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+            SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+            StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+        };
+        _task = new DriveTask(_proc.Get());
+        _task.Wait();
     }
 
-    public void Stop()
+    public override void TestStop()
     {
-        _stopping = true;
+        _proc?.Stop();
-        Cleanup();
+        _task?.Stop();
-    }
-
-    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");
     }
 }
 
@@ -575,12 +490,16 @@ public class FleetCrabWalkTest : MovementTest
         _proc = new FleetCrabWalk
         {
             CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
+            BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
             CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
             CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
-            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+            FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
-            CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain,
+            FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
-            CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg,
+            FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
-            CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel
+            FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
+            FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
+            FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
+            GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
         };
         _task = new DriveTask(_proc.Get());
         _task.Wait();

MultiWheel/MultiWheelC/Movements.cs

@@ -25,6 +25,7 @@ using System.Numerics;
 using System.Reflection;
 using System.Text;
 using System.Threading;
+using static ClumsyCore.DTools.Painter;
 
 namespace MultiWheelC
 {
@@ -73,6 +74,50 @@ 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 ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
+        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
+        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+        private PIDController leftpid, rightpid;
+
+        public override IEnumerable<bool> Get()
+        {
+            leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd,
+                    ClampMaxI, ClampDeadZone, MaxClampSpeed)
+                { SpeedAccPerSec = ClampSpeedAcc };
+
+            rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd,
+                    ClampMaxI, ClampDeadZone, MaxClampSpeed)
+                { SpeedAccPerSec = ClampSpeedAcc };
+
+            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 (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;
@@ -123,6 +168,41 @@ namespace MultiWheelC
         }
     }
 
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+
+        public float InitialSendSpeed = 0;
+
+        public override IEnumerable<bool> Get()
+        {
+            Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+            Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+
+           if (InitialSendSpeed != 0)
+            {
+                tracker.SkipInitialRotate = true;
+                tracker.InitialSendSpeed = InitialSendSpeed;
+            }
+
+            var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
+            tracker.AddTrack(linePath);
+            var task = new DriveTask(tracker.Track());
+            task.Wait();
+
+            // 到点后兜底停车
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            chassis.SendXYThSpeed(0f, 0f, 0f);
+            yield return false;
+        }
+    }
+
     //直线行走基于轮里程
     public class LineTracking : MovementDefinition
     {
@@ -136,25 +216,42 @@ namespace MultiWheelC
         public int DstId = -1;
         public Action<int> LeaveSrcFunction = null;
         private PIDController pid;
+        // 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 80f; // mm
+        public float HandoverSpeed = 0.15f;  // m/s
 
         public override IEnumerable<bool> Get()
         {
-            pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0,
+
-                    DeadZone, MaxSpeed)
+            pid = new PIDController(() =>
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
+                Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
             { SpeedAccPerSec = MaxSpeed / 2f };
+
             var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            //chassis.SetOriginBias(0, 0, 0);
+            DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
             while (true)
             {
+                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
+                var remain = Target - current;
+                if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
+                {
+                    var handoverSign = Math.Sign(remain);
+                    if (handoverSign == 0) handoverSign = 1;
+                    var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
+                    Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
+                    chassis.SendXYThSpeed(handoverSpeed, 0, 0);
+                    // 保留一拍接力速度，让后续 DstTracker 无缝接管
+                    yield return true;
+                    break;
+                }
                 var speed = pid.GetResponse(Target);
                 Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
-                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
+                
                 chassis.SendXYThSpeed(speed, 0, 0);
-                //if (Math.Abs(current - Target) < pid.DeadZone)
+
-                //{
-                //    chassis.SendXYThSpeed(0f, 0f, 0f);
-                //    Console.WriteLine($"调整退出:当前({current:f2}) ,目标:({Target:f2})");
-                //    break;
-                //}
                 if (pid.IsArrived()) break;
                 yield return true;
             }
@@ -169,26 +266,60 @@ namespace MultiWheelC
 
     public class DriverAble : MovementDefinition
     {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
         public override IEnumerable<bool> Get()
         {
             Console.WriteLine("驱动器上使能");
             PilotDefinition.Self.ResetFromC = true;
-            Thread.Sleep(200);
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
             PilotDefinition.Self.ResetFromC = false;
-            Console.WriteLine("驱动器上使能完成");
+            if (success)
+                Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
             yield return false;
         }
     }
 
     public class DriverDisable : MovementDefinition
     {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
         public override IEnumerable<bool> Get()
         {
             Console.WriteLine("驱动器下使能");
             PilotDefinition.Self.DisableFromC = true;
-            Thread.Sleep(200);
+
+            var start = DateTime.Now;
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var success = !PilotDefinition.Self.WheelAbleState;
+            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+            {
+                Thread.Sleep(pollMs);
+                success = !PilotDefinition.Self.WheelAbleState;
+                if (!success) yield return true;
+            }
+
             PilotDefinition.Self.DisableFromC = false;
-            Console.WriteLine("驱动器下使能完成");
+            if (success)
+                Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
+            else
+                Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
             yield return false;
         }
     }

MultiWheel/MultiWheelC/PilotConfig.cs

@@ -6,14 +6,14 @@ namespace MultiWheelC;
 
 public class PilotConfig : MultiWheelPilotConfig
 {
-    // ===== 多车联动（已从 MDCSToolBox 内联回 Tutorial）=====
+    [FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
-    [FieldMember(desc = "[sync] 转向角加速度(deg/s^2)")] public float SyncThAccPerSec = 30f;
+    [FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
-    [FieldMember(desc = "[sync] 编队车间距(mm)")] public float TestCarSyncDistance = 2400f;
+    [FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
-    [FieldMember(desc = "[sync] 编队布局偏角(deg)")] public float TestCarSyncTh = 0f;
+    // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
-    // 手动遥控 Vx 已是 m/s、Vth 已是转向角(deg)，此处系数保持 1（直通），不要再次缩放。
+    [FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
-    [FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f;
+    [FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
-    [FieldMember(desc = "[sync] 手动Vy系数(蟹行横向)")] public float ManualCarSyncVyFac = 1f;
+    [FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
-    [FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
+    [FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
     [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
     [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
     // 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
@@ -22,12 +22,16 @@ public class PilotConfig : MultiWheelPilotConfig
     // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
     // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
     [FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
+    // 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
+    [FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
 
     [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
     [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
     [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
     [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
 
+    [FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
+
     [JsonProperty("MultiVehicleMasterIp")]
     private string LegacyMasterIpSetter
     {
@@ -41,9 +45,6 @@ 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;
@@ -82,10 +83,9 @@ public class PilotConfig : MultiWheelPilotConfig
     // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
     // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
     [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
-    // 可选硬安全网：每轮纠偏速度幅值 ≤ 该比例×本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
+    // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
-    // 默认 <0 关闭——纠偏随转速缩放(代码 #1)已让"纠偏:切向"比例全程恒定，匀速段不应再被削弱。
+    // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
-    // 仅在极端启动偏差导致匀速段仍乱打方向时，可设为 ~1.0(偏角≤45°) 兜底。
+    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
-    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限(默认-1关闭)")] public float MultiVehicleRotateCompTangentFrac = -1f;
 
     [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
     [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
@@ -93,6 +93,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "Playground WebAPI 基地址")]
     public string PlaygroundWebApiUrl = "http://localhost:18090";
 
+    [FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
+    public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
+
     [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
     public string PlaygroundRobotName = "agv_multi_1";
 
@@ -117,6 +120,9 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
     public float InPlaceRotateWheelAlignDeg = 2f;
 
+    [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
+    public float InPlaceRotateActiveWheelAlignDeg = 10f;
+
     // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
     // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
     [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
@@ -146,29 +152,71 @@ public class PilotConfig : MultiWheelPilotConfig
     public bool FleetRotateUseDetourHeading = true;
 
     // ===== 车队联动-自动蟹行（FleetCrabWalk）=====
-    // 以当前车队中心为起点，构造与车队朝向夹角 FleetCrabAngleDeg、长度 FleetCrabLengthMm 的直线路径，
+    // 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
-    // 复用脚本手动等价输入(mode=1)斜向平移；动作侧只把横向误差转换成小幅、带斜率限制的蟹行方向修正。
+    // 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
-    [FieldMember(desc = "车队蟹行：与车队朝向夹角(deg，逆时针为正)")]
+    [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
     public float FleetCrabAngleDeg = 45f;
 
+    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
+    public float FleetCrabBodyWorldHeadingDeg = 0f;
+
     [FieldMember(desc = "车队蟹行：路径长度(mm)")]
     public float FleetCrabLengthMm = 2000f;
 
     [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
     public float FleetCrabSpeed = 0.2f;
 
-    // 兼容旧版几何控制器实现；当前自动蟹行走脚本手动等价输入，不再直接使用该上限。
+    [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
-    [FieldMember(desc = "车队蟹行：旧几何控制器gcp角度上限(deg)")]
+    public float FleetCrabAccel = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
+    public float FleetCrabStartAccel = 0.01f;
+
+    [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;
 
-    [FieldMember(desc = "车队蟹行：横向误差纠偏增益")]
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
-    public float FleetCrabCorrectionGain = 1f;
+    public float FleetCrabDthLinearFac = 1f;
 
-    [FieldMember(desc = "车队蟹行：自动纠偏最大改向角(deg)")]
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
-    public float FleetCrabCorrectionAngleDeg = 8f;
+    public float FleetCrabDthLinearThreshold = 10f;
 
-    [FieldMember(desc = "车队蟹行：脚本速度命令斜率(m/s^2)")]
+    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
-    public float FleetCrabCommandAccel = 0.4f;
+    public float FleetCrabStartSyncTimeoutSec = 8f;
+
+    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
+    public float FleetCrabStartWheelAlignDeg = 2f;
+
+    // ===== Fleet linked Bezier curve walk =====
+    [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
+    public int FleetCurveTestControlPointCount = 4;
+
+    [FieldMember(desc = "FleetCurve speed(m/s)")]
+    public float FleetCurveSpeed = 0.2f;
+
+    [FieldMember(desc = "FleetCurve slow distance(mm)")]
+    public float FleetCurveSlowDistance = 2000f;
+
+    [FieldMember(desc = "FleetCurve finish distance(mm)")]
+    public float FleetCurveFinishDistance = 20f;
+
+    [FieldMember(desc = "FleetCurve finish speed(m/s)")]
+    public float FleetCurveFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "FleetCurve slowing curve exponent")]
+    public float FleetCurveSlowingPow = 0.8f;
 
     // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
@@ -204,6 +252,12 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "2腿检测：中心X偏移(mm)")]
     public float TwoLegCenterChangeX = 0f;
 
+    [FieldMember(desc = "2腿检测：输出X补偿(mm)")]
+    public float TwoLegOutputBiasX = 0f;
+
+    [FieldMember(desc = "2腿检测：输出Y补偿(mm)")]
+    public float TwoLegOutputBiasY = 0f;
+
     [FieldMember(desc = "2腿检测：ROI滤波框长(mm)")]
     public float TwoLegFilterLength = 1800f;
 
@@ -230,6 +284,15 @@ 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;
@@ -257,5 +320,15 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
     [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
 
+    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
+    [FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
     #endregion
+
+    [FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
+    [FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
+    [FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
+    [FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
+    [FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
+    [FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
 }

MultiWheel/MultiWheelC/TireFollowing.cs

@@ -50,6 +50,11 @@ namespace MultiWheelC
         /// </summary>
         public float MaxSpeed = 0.3f;
 
+        // 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
+        public bool EnableHandover = false;
+        public float HandoverDistance = 200f; // mm
+        public float HandoverSpeed = 0.2f;    // m/s
+
         /// <summary>
         /// 钻轮胎数量
         /// </summary>
@@ -200,7 +205,12 @@ namespace MultiWheelC
             controller.FinishDistance = float.MinValue;
             controller.FirstThAccuracy = 999;
             _dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
-
+            void HardStop()
+            {
+                _dt?.Stop();
+                ((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
+                DLog.Log($"Hard Stop!", "TireFollowing");
+            }
             float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
             bool WalkBlindStage1 = false, WalkBlindStage2 = false;
             var angle2target = -1f;
@@ -280,9 +290,16 @@ namespace MultiWheelC
                     //第二次盲走时或只钻一个轮胎时
                     if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
                     {
+                        //controller.FinishDistance = 10f;
                         controller.SlowDistance = SlowDistance;
                         controller.SlowingPow = 0.7f;
                     }
+                    if (EnableHandover)
+                    {
+                        controller.SlowDistance = float.MinValue;
+                        controller.FinishSpeed = 0.2f;
+                        controller.FinishDistance = 50;
+                    }
                     (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
                     var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
                     var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
@@ -318,16 +335,19 @@ namespace MultiWheelC
                             _remainAngleList.Clear();
                             _remainDistanceList.Clear();
                             detectorIndex++;
-                            if (detectors.Count == 1 || TireNum == 1)
+                            if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
                             {
-                                _dt.Stop();
+                                HardStop();
                                 yield return false;
                             }
                         }
                         else if (WalkBlindStage2)
                         {
                             DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
-                            _dt.Stop();
+                            if (!EnableHandover)
+                            {
+                                HardStop();
+                            }
                             yield return false;
                         }
                     }
@@ -397,6 +417,14 @@ namespace MultiWheelC
                     while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
                     rd = _remainDistanceList.Average();
                     _painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
+                    if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
+                    {
+                        if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                        {
+                            detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                        }
+                    }
 
                     if (detectorIndex < detectors.Count - 1)
                     {
@@ -404,11 +432,11 @@ namespace MultiWheelC
                         if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
                         {
                             WalkBlindStage1 = true;
-                            if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
+                            //if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
-                            {
+                            //{
-                                detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
+                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
-                                DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
+                            //    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
-                            }
+                            //}
                             WalkBlindCarPathDstX = trackDst.X;
                             WalkBlindCarPathDstY = trackDst.Y;
                             WalkBlindCarPathDstTh = angle2target + WalkBlindTh;

MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs

@@ -0,0 +1,277 @@
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Text;
+
+namespace MultiWheelC;
+
+internal static class VehicleSyncBinaryCodec
+{
+    private const byte Version = 2;
+    private const byte RegisterType = 1;
+    private const byte NotificationType = 2;
+    private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
+
+    public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
+    {
+        using var stream = new MemoryStream();
+        using var writer = new BinaryWriter(stream, Encoding.UTF8);
+        WriteHeader(writer, RegisterType);
+        writer.Write(carNum);
+        WriteInfo(writer, info);
+        writer.Flush();
+        return stream.ToArray();
+    }
+
+    public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
+    {
+        using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
+        using var reader = new BinaryReader(stream, Encoding.UTF8);
+        var version = ReadHeader(reader, RegisterType);
+        var carNum = reader.ReadInt32();
+        var info = ReadInfo(reader, version);
+        EnsureFullyRead(stream);
+        return (carNum, info);
+    }
+
+    public static byte[] EncodeNotification(VehicleSyncNotification notification)
+    {
+        using var stream = new MemoryStream();
+        using var writer = new BinaryWriter(stream, Encoding.UTF8);
+        WriteHeader(writer, NotificationType);
+
+        writer.Write(notification.Seq);
+        writer.Write(BuildNotificationFlags(notification));
+        writer.Write(notification.Mode);
+        writer.Write(notification.FleetStopSourceCar);
+        writer.Write(notification.CenterX);
+        writer.Write(notification.CenterY);
+        writer.Write(notification.CenterTh);
+        writer.Write(notification.FleetVx);
+        writer.Write(notification.FleetFrontTh);
+        writer.Write(notification.FleetRearTh);
+        writer.Write(notification.FleetOmega);
+        writer.Write(notification.RequestedFleetOmega);
+        writer.Write(notification.SyncTh);
+        writer.Write(notification.SyncDistance);
+        writer.Write(notification.DeltaDetectCenter);
+        writer.Write(notification.RotateActiveOmega);
+        writer.Write(notification.RotateCompXyFac);
+        writer.Write(notification.RotateCompXyIFac);
+        writer.Write(notification.RotateCompXyMax);
+        writer.Write(notification.RotateCompThFac);
+        writer.Write(notification.RotateCompThIFac);
+        writer.Write(notification.RotateCompThMax);
+        writer.Write(notification.RotateCompTangentFrac);
+        writer.Write(notification.RotateStartWheelAlignDeg);
+        writer.Write(notification.RotateActiveWheelAlignDeg);
+        writer.Write(notification.IdealX);
+        writer.Write(notification.IdealY);
+        writer.Write(notification.IdealTh);
+        WriteString(writer, notification.FleetStopReason);
+
+        var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
+        if (fleet.Count > ushort.MaxValue)
+            throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
+        writer.Write((ushort)fleet.Count);
+        foreach (var kv in fleet)
+        {
+            writer.Write(kv.Key);
+            WriteInfo(writer, kv.Value);
+        }
+
+        writer.Flush();
+        return stream.ToArray();
+    }
+
+    public static VehicleSyncNotification DecodeNotification(byte[] payload)
+    {
+        using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
+        using var reader = new BinaryReader(stream, Encoding.UTF8);
+        var version = ReadHeader(reader, NotificationType);
+
+        var notification = new VehicleSyncNotification
+        {
+            Seq = reader.ReadInt64()
+        };
+
+        ApplyNotificationFlags(notification, reader.ReadUInt16());
+        notification.Mode = reader.ReadInt32();
+        notification.FleetStopSourceCar = reader.ReadInt32();
+        notification.CenterX = reader.ReadSingle();
+        notification.CenterY = reader.ReadSingle();
+        notification.CenterTh = reader.ReadSingle();
+        notification.FleetVx = reader.ReadSingle();
+        notification.FleetFrontTh = reader.ReadSingle();
+        notification.FleetRearTh = reader.ReadSingle();
+        notification.FleetOmega = reader.ReadSingle();
+        notification.RequestedFleetOmega = reader.ReadSingle();
+        notification.SyncTh = reader.ReadSingle();
+        notification.SyncDistance = reader.ReadSingle();
+        notification.DeltaDetectCenter = reader.ReadSingle();
+        notification.RotateActiveOmega = reader.ReadSingle();
+        notification.RotateCompXyFac = reader.ReadSingle();
+        notification.RotateCompXyIFac = reader.ReadSingle();
+        notification.RotateCompXyMax = reader.ReadSingle();
+        notification.RotateCompThFac = reader.ReadSingle();
+        notification.RotateCompThIFac = reader.ReadSingle();
+        notification.RotateCompThMax = reader.ReadSingle();
+        notification.RotateCompTangentFrac = reader.ReadSingle();
+        notification.RotateStartWheelAlignDeg = reader.ReadSingle();
+        notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
+        notification.IdealX = reader.ReadSingle();
+        notification.IdealY = reader.ReadSingle();
+        notification.IdealTh = reader.ReadSingle();
+        notification.FleetStopReason = ReadString(reader);
+
+        var fleetCount = reader.ReadUInt16();
+        notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
+        for (var i = 0; i < fleetCount; ++i)
+        {
+            var carNum = reader.ReadInt32();
+            notification.Fleet[carNum] = ReadInfo(reader, version);
+        }
+
+        EnsureFullyRead(stream);
+        return notification;
+    }
+
+    private static void WriteHeader(BinaryWriter writer, byte type)
+    {
+        writer.Write(Magic);
+        writer.Write(Version);
+        writer.Write(type);
+        writer.Write((ushort)0);
+    }
+
+    private static byte ReadHeader(BinaryReader reader, byte expectedType)
+    {
+        for (var i = 0; i < Magic.Length; ++i)
+        {
+            if (reader.ReadByte() != Magic[i])
+                throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
+        }
+
+        var version = reader.ReadByte();
+        if (version < 1 || version > Version)
+            throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
+
+        var type = reader.ReadByte();
+        if (type != expectedType)
+            throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
+
+        var reserved = reader.ReadUInt16();
+        if (reserved != 0)
+            throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
+
+        return version;
+    }
+
+    private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
+    {
+        writer.Write(BuildInfoFlags(info));
+        WriteString(writer, info.Ip);
+        writer.Write(info.Port);
+        writer.Write(info.X);
+        writer.Write(info.Y);
+        writer.Write(info.Th);
+        writer.Write(info.LayoutX);
+        writer.Write(info.LayoutY);
+        writer.Write(info.LayoutTh);
+        WriteString(writer, info.MotionInfeasibleReason);
+        WriteString(writer, info.RotateWheelAlignDetail);
+        writer.Write(info.AppliedNotificationSeq);
+    }
+
+    private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
+    {
+        var info = new VehicleSyncInfo();
+        ApplyInfoFlags(info, reader.ReadUInt16());
+        info.Ip = ReadString(reader);
+        info.Port = reader.ReadInt32();
+        info.X = reader.ReadSingle();
+        info.Y = reader.ReadSingle();
+        info.Th = reader.ReadSingle();
+        info.LayoutX = reader.ReadSingle();
+        info.LayoutY = reader.ReadSingle();
+        info.LayoutTh = reader.ReadSingle();
+        info.MotionInfeasibleReason = ReadString(reader);
+        info.RotateWheelAlignDetail = ReadString(reader);
+        info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
+        return info;
+    }
+
+    private static ushort BuildInfoFlags(VehicleSyncInfo info)
+    {
+        ushort flags = 0;
+        if (info.Master) flags |= 1 << 0;
+        if (info.PosAvailable) flags |= 1 << 1;
+        if (info.Aligned) flags |= 1 << 2;
+        if (info.DetectOk) flags |= 1 << 3;
+        if (info.MotionFeasible) flags |= 1 << 4;
+        if (info.RotateWheelsAligned) flags |= 1 << 5;
+        return flags;
+    }
+
+    private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
+    {
+        info.Master = (flags & (1 << 0)) != 0;
+        info.PosAvailable = (flags & (1 << 1)) != 0;
+        info.Aligned = (flags & (1 << 2)) != 0;
+        info.DetectOk = (flags & (1 << 3)) != 0;
+        info.MotionFeasible = (flags & (1 << 4)) != 0;
+        info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
+    }
+
+    private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
+    {
+        ushort flags = 0;
+        if (notification.PosAvailable) flags |= 1 << 0;
+        if (notification.Aligned) flags |= 1 << 1;
+        if (notification.FleetMotionReleased) flags |= 1 << 2;
+        if (notification.FleetStopActive) flags |= 1 << 3;
+        if (notification.AutoEnabled) flags |= 1 << 4;
+        if (notification.ManualEnabled) flags |= 1 << 5;
+        if (notification.HasIdeal) flags |= 1 << 6;
+        if (notification.RotateParamsValid) flags |= 1 << 7;
+        if (notification.UseDetourCorrection) flags |= 1 << 8;
+        return flags;
+    }
+
+    private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
+    {
+        notification.PosAvailable = (flags & (1 << 0)) != 0;
+        notification.Aligned = (flags & (1 << 1)) != 0;
+        notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
+        notification.FleetStopActive = (flags & (1 << 3)) != 0;
+        notification.AutoEnabled = (flags & (1 << 4)) != 0;
+        notification.ManualEnabled = (flags & (1 << 5)) != 0;
+        notification.HasIdeal = (flags & (1 << 6)) != 0;
+        notification.RotateParamsValid = (flags & (1 << 7)) != 0;
+        notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
+    }
+
+    private static void WriteString(BinaryWriter writer, string value)
+    {
+        var bytes = Encoding.UTF8.GetBytes(value ?? "");
+        if (bytes.Length > ushort.MaxValue)
+            throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
+        writer.Write((ushort)bytes.Length);
+        writer.Write(bytes);
+    }
+
+    private static string ReadString(BinaryReader reader)
+    {
+        var length = reader.ReadUInt16();
+        var bytes = reader.ReadBytes(length);
+        if (bytes.Length != length)
+            throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
+        return Encoding.UTF8.GetString(bytes);
+    }
+
+    private static void EnsureFullyRead(MemoryStream stream)
+    {
+        if (stream.Position != stream.Length)
+            throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
+    }
+}

MultiWheel/MultiWheelC/VehicleSyncModels.cs

@@ -19,6 +19,11 @@ public class VehicleSyncInfo
     [JsonProperty("Aligned")] public bool Aligned { get; set; }
     // 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
     [JsonProperty("DetectOk")] public bool DetectOk { get; set; }
+    [JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
+    [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
+    [JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
+    [JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
+    [JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
 }
 
 public class VehicleSyncNotification
@@ -36,11 +41,29 @@ public class VehicleSyncNotification
     [JsonProperty("Mode")] public int Mode { get; set; }
     // 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
     [JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
+    [JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
+    [JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
+    [JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
+    [JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
+    [JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
     [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
     [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
+    [JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
     [JsonProperty("SyncTh")] public float SyncTh { get; set; }
     [JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
     [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
+    // 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
+    [JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
+    [JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
+    [JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
+    [JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
+    [JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
+    [JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
+    [JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
+    [JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
+    [JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
+    [JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
+    [JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
     // F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
     [JsonProperty("Seq")] public long Seq { get; set; }
     // D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。

MultiWheel/MultiWheelM/CartDefinition.cs

@@ -89,22 +89,25 @@ public partial class CartDefinition : CartActivator.CartDefinition
     [AsLowerIO(desc = "（手动）多车联动模式")]
     public int MultiVehicleManualMode;
 
-    [AsLowerIO(desc = "多车联动：遥控器Vx")]
+    [AsLowerIO(desc = "多车联动：遥控器Vx比例")]
     public float MultiVehicleManualVx;
 
-    [AsLowerIO(desc = "多车联动：遥控器Vy")]
+    [AsLowerIO(desc = "多车联动：遥控器Vy比例")]
     public float MultiVehicleManualVy;
 
-    [AsLowerIO(desc = "多车联动：遥控器Vth")]
+    [AsLowerIO(desc = "多车联动：遥控器Vth比例")]
     public float MultiVehicleManualVth;
 
-    [AsInitParam(desc = "手动最大速度(m/s)")]
+    [AsLowerIO(desc = "多车联动：遥控暂停")]
+    public bool MultiVehicleHold;
+
+    [AsInitParam(desc = "普通手动最大速度(m/s)，车队联动不使用")]
     public float MaxManualSpeed = 0.3f;
 
-    [AsInitParam(desc = "手动最大自旋角速度(deg/s)")]
+    [AsInitParam(desc = "普通手动最大自旋角速度(deg/s)，车队联动不使用")]
     public float MaxManualAngularSpeed = 45f;
 
-    [AsInitParam(desc = "手动最大转向角度(deg)")]
+    [AsInitParam(desc = "普通手动最大转向角度(deg)，车队联动不使用")]
     public float MaxManualTheta = 45f;
 
     [AsInitParam(desc = "遥控转向输入幂数")]

MultiWheel/MultiWheelM/FleetRemote.cs

@@ -26,6 +26,7 @@ public partial class CartDefinition
         MultiVehicleManualVx = 0;
         MultiVehicleManualVy = 0;
         MultiVehicleManualVth = 0;
+        MultiVehicleHold = false;
     }
 
     [IOObjectUtility]
@@ -43,7 +44,6 @@ public partial class CartDefinition
         var crabOn = false;   // 蟹行（四轮同向平移）
         var rotateOn = false; // 原地旋转（绕车队中心）
         // 速度比例 0~1，作用于摇杆输出的线速度/横移/角速度。
-        var speedRatio = 1f;
         UseGesture? manip = null;
 
         // 0=常规(前进+转向) 1=蟹行 2=原地旋转。crab 优先于 rotate（同时打开时蟹行生效）。
@@ -87,7 +87,6 @@ public partial class CartDefinition
                     return;
                 }
 
-                var ratio = Math.Clamp(speedRatio, 0, 1);
                 var px = Math.Clamp(pos.X, -1, 1);
                 var py = Math.Clamp(pos.Y, -1, 1);
                 var mode = CurrentMode();
@@ -95,31 +94,27 @@ public partial class CartDefinition
 
                 if (mode == 2)
                 {
-                    // 原地旋转：pos.X(左右) → 角速度(deg/s)，绕车队中心。
                     MultiVehicleManualVx = 0;
                     MultiVehicleManualVy = 0;
-                    MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+                    MultiVehicleManualVth = px;
                 }
                 else if (mode == 1)
                 {
-                    // 蟹行：pos.Y → 前后向线速度，pos.X → 横向线速度（m/s）。
+                    MultiVehicleManualVx = py;
-                    MultiVehicleManualVx = py * MaxManualSpeed * ratio;
+                    MultiVehicleManualVy = px;
-                    MultiVehicleManualVy = px * MaxManualSpeed * ratio;
                     MultiVehicleManualVth = 0;
                 }
                 else
                 {
-                    // 常规：pos.Y → 线速度(m/s)，pos.X → 转向角(deg)。
+                    MultiVehicleManualVx = py;
-                    MultiVehicleManualVx = py * MaxManualSpeed * ratio;
                     MultiVehicleManualVy = 0;
-                    MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
+                    MultiVehicleManualVth = px;
                 }
-
                 if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
                 {
                     _fleetDiagLastStick = DateTime.Now;
-                    FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
+                    FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} raw " +
-                              $"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
+                              $"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.000} en={MultiVehicleManualEnabled}");
                 }
             }
         });
@@ -169,15 +164,6 @@ public partial class CartDefinition
             }
         });
 
-        manip.AddWidget(new UseGesture.ThrottleWidget
-        {
-            name = "fleet_speed_ratio",
-            text = "速度比例",
-            position = "50%+10px, 64%+10px",
-            size = "37.5%-10px, 10%-10px",
-            bounceBack = false,
-            OnValue = (val, _) => speedRatio = Math.Clamp(val, 0, 1)
-        });
 
         manip.AddWidget(new UseGesture.ButtonWidget
         {
@@ -218,8 +204,7 @@ public partial class CartDefinition
             pb.SeparatorText("状态");
             var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "蟹行" : "常规";
             pb.Label($"车队联动: {MultiVehicleManualEnabled}  模式: {modeName}");
-            pb.Label($"速度比例: {speedRatio:0.00}");
+            pb.Label($"VxRatio={MultiVehicleManualVx:0.000} VyRatio={MultiVehicleManualVy:0.000}, VthRatio={MultiVehicleManualVth:0.000}");
-            pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
             pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
             pb.Label("请勿同时打开「手动控制」面板");
             pb.Panel.Repaint();

MultiWheel/MultiWheelS/MultiVehicleCar.cs

@@ -1,22 +0,0 @@
-using System.Threading.Tasks;
-using SimpleComposer.RCS;
-using SimpleCore.PropType;
-
-namespace MultiWheelS
-{
-    [CarType(Name = "多车联动AGV")]
-    public class MultiVehicleCar : GhostCar
-    {
-        public bool MultiVehicleSync = true;
-
-        public static new async Task<MultiVehicleCar> Create()
-        {
-            return new MultiVehicleCar
-            {
-                lstatus = "连接中",
-                address = "127.0.0.1",
-                name = "联动AGV"
-            };
-        }
-    }
-}

MultiWheel/MultiWheelS/MultiWheelS.csproj

@@ -1,62 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
-  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
-  <PropertyGroup>
-    <LangVersion>latest</LangVersion>
-  </PropertyGroup>
-  <PropertyGroup>
-    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
-    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
-    <ProjectGuid>{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}</ProjectGuid>
-    <OutputType>Library</OutputType>
-    <RootNamespace>MultiWheelS</RootNamespace>
-    <AssemblyName>MultiWheelS</AssemblyName>
-    <TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
-    <FileAlignment>512</FileAlignment>
-    <Deterministic>true</Deterministic>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
-    <DebugSymbols>true</DebugSymbols>
-    <DebugType>full</DebugType>
-    <Optimize>false</Optimize>
-    <OutputPath>bin\Debug\</OutputPath>
-    <DefineConstants>DEBUG;TRACE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-  </PropertyGroup>
-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
-    <DebugType>pdbonly</DebugType>
-    <Optimize>true</Optimize>
-    <OutputPath>bin\Release\</OutputPath>
-    <DefineConstants>TRACE</DefineConstants>
-    <ErrorReport>prompt</ErrorReport>
-    <WarningLevel>4</WarningLevel>
-  </PropertyGroup>
-  <ItemGroup>
-    <Reference Include="LessokajiWeaverUtilities">
-      <HintPath>D:\MDCS\Release\deps\LessokajiWeaverUtilities.dll</HintPath>
-    </Reference>
-    <Reference Include="RefSimpleCore">
-      <HintPath>D:\MDCS\Dependencies\Simple\RefSimpleCore.dll</HintPath>
-    </Reference>
-    <Reference Include="SimpleComposer">
-      <HintPath>D:\MDCS\Executables\Simple\SimpleComposer.exe</HintPath>
-    </Reference>
-    <Reference Include="System" />
-    <Reference Include="System.Core" />
-  </ItemGroup>
-  <ItemGroup>
-    <Compile Include="MultiVehicleCar.cs" />
-  </ItemGroup>
-  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
-  <PropertyGroup>
-    <PostBuildEvent>if not exist "$(SolutionDir)build\Simple\plugins" mkdir "$(SolutionDir)build\Simple\plugins"
-if not exist "$(SolutionDir)build\Simple" mkdir "$(SolutionDir)build\Simple"
-xcopy "$(TargetDir)$(TargetFileName)" "$(SolutionDir)build\Simple\plugins" /y
-xcopy "$(TargetDir)$(TargetName).pdb" "$(SolutionDir)build\Simple\plugins" /y
-copy /Y "D:\MDCS\Executables\Simple\SimpleComposer.exe" "$(SolutionDir)build\Simple\"
-copy /Y "D:\MDCS\Release\MDCSToolBox.dll" "$(SolutionDir)build\Simple\"
-copy /Y "D:\MDCS\Release\CommonUsage.dll" "$(SolutionDir)build\Simple\"
-copy /Y "D:\MDCS\Dependencies\Simple\RefSimpleCore.dll" "$(SolutionDir)build\Simple\"</PostBuildEvent>
-  </PropertyGroup>
-</Project>

Tutorial.sln

@@ -1,4 +1,3 @@
-
 Microsoft Visual Studio Solution File, Format Version 12.00
 # Visual Studio Version 17
 VisualStudioVersion = 17.5.33424.131
@@ -15,8 +14,6 @@ Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelM", "MultiWheel\M
 EndProject
 Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MultiWheelC", "MultiWheel\MultiWheelC\MultiWheelC.csproj", "{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}"
 EndProject
-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MultiWheelS", "MultiWheel\MultiWheelS\MultiWheelS.csproj", "{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}"
-EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -39,10 +36,6 @@ Global
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0}.Release|Any CPU.Build.0 = Release|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Debug|Any CPU.Build.0 = Debug|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Release|Any CPU.ActiveCfg = Release|Any CPU
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE
@@ -52,7 +45,6 @@ Global
 		{A1B2C3D4-E5F6-7890-ABCD-EF1234567890} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E1}
 		{B7B53B6B-98B5-4B6F-9B09-598F7B8166C9} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
 		{C8C64C7C-A9C6-5C7F-0C1A-609F8C9277D0} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
-		{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3} = {A1A1A1A1-B2B2-C3C3-D4D4-E5E5E5E5E5E2}
 	EndGlobalSection
 	GlobalSection(ExtensibilityGlobals) = postSolution
 		SolutionGuid = {EF6B92D5-5691-42DE-8B38-C17AE7DD7418}

deploy/chassis.json

@@ -1,15 +0,0 @@
-{
-  "WheelConfig": {
-    "LeftFront": { "Position": { "X": 525.0, "Y": 200.0 } },
-    "LeftRear": { "Position": { "X": -525.0, "Y": 200.0 } },
-    "RightFront": { "Position": { "X": 525.0, "Y": -200.0 } },
-    "RightRear": { "Position": { "X": -525.0, "Y": -200.0 } }
-  },
-  "MaxSpeed": 0.3,
-  "AccPerSecond": 0.3,
-  "DeAccPerSecond": 0.5,
-  "MinTurnSpeedFac": 0.25,
-  "ControlPointRadius": 500.0,
-  "GcpThetaPerSecond": 10.0,
-  "MinimumTurningAngleForAckermann": 60.0
-}

deploy/clumsy_agv1/clumsy.json

@@ -1,60 +0,0 @@
-{
-  "basicSpeed": 0.2,
-  "DriveTaskInterval": 50,
-  "script": "MultiWheelC.dll",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4321,
-  "msConf": {
-    "MultiVehicleMasterEndpoint": "/",
-    "MultiVehicleFleetNum": 2,
-    "MultiVehicleSyncInterval": 50,
-    "TestCarSyncDistance": 2400,
-    "TestCarSyncTh": 0,
-    "ManualCarSyncVxFac": 1.0,
-    "ManualCarSyncVthFac": 1.0,
-    "SyncThAccPerSec": 30,
-    "MultiVehicleSyncUseDetour": true,
-    "SimpleIp": "127.0.0.1",
-    "CarNum": 1
-  },
-  "layout": {
-    "chassis": {
-      "width": 1000,
-      "length": 1550,
-      "contour": [
-        750, -500, -750, -500, -750, -300, -800, -280, -800, -220,
-        -750, -200, -750, 200, -800, 220, -800, 280, -750, 300,
-        -750, 500, 750, 500
-      ]
-    },
-    "components": [
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "front_lidar_1",
-          "x": 700, "y": 0, "yaw": 0, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_left_lidar_1",
-          "x": -700, "y": 450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_right_lidar_1",
-          "x": -700, "y": -450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      }
-    ]
-  }
-}

deploy/clumsy_agv1/clumsyconsole.json

@@ -1,11 +0,0 @@
-{
-  "port": 8008,
-  "allowMultiple": true,
-  "soTag": "Multi1",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4321,
-  "HideConsoleOnStart": false,
-  "FollowCarOnStart": true,
-  "ShowRobot3dModel": true,
-  "ShowRobotArrow": true
-}

deploy/clumsy_agv2/clumsy.json

@@ -1,60 +0,0 @@
-{
-  "basicSpeed": 0.2,
-  "DriveTaskInterval": 50,
-  "script": "MultiWheelC.dll",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4421,
-  "msConf": {
-    "MultiVehicleMasterEndpoint": "127.0.0.1:8008",
-    "MultiVehicleFleetNum": 2,
-    "MultiVehicleSyncInterval": 50,
-    "TestCarSyncDistance": 2400,
-    "TestCarSyncTh": 0,
-    "ManualCarSyncVxFac": 1.0,
-    "ManualCarSyncVthFac": 1.0,
-    "SyncThAccPerSec": 30,
-    "MultiVehicleSyncUseDetour": true,
-    "SimpleIp": "127.0.0.1",
-    "CarNum": 2
-  },
-  "layout": {
-    "chassis": {
-      "width": 1000,
-      "length": 1550,
-      "contour": [
-        750, -500, -750, -500, -750, -300, -800, -280, -800, -220,
-        -750, -200, -750, 200, -800, 220, -800, 280, -750, 300,
-        -750, 500, 750, 500
-      ]
-    },
-    "components": [
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "front_lidar_2",
-          "x": 700, "y": 0, "yaw": 0, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_left_lidar_2",
-          "x": -700, "y": 450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      },
-      {
-        "type": "lidar2d",
-        "options": {
-          "name": "rear_right_lidar_2",
-          "x": -700, "y": -450, "yaw": 180, "z": 0, "pitch": 0, "roll": 0,
-          "isCircle": false, "ignoreDist": 100, "maxDist": 200000,
-          "filterChassis": true
-        }
-      }
-    ]
-  }
-}

deploy/clumsy_agv2/clumsyconsole.json

@@ -1,11 +0,0 @@
-{
-  "port": 8009,
-  "allowMultiple": true,
-  "soTag": "Multi2",
-  "detourHost": "127.0.0.1",
-  "detourPort": 4421,
-  "HideConsoleOnStart": false,
-  "FollowCarOnStart": true,
-  "ShowRobot3dModel": true,
-  "ShowRobotArrow": true
-}

deploy/clumsy_diff/clumsy.json

@@ -1,17 +0,0 @@
-{
-  "basicSpeed": 0.2,
-  "DriveTaskInterval": 50,
-  "script": "DiffWheelC.dll",
-  "msConf": {
-    "ChassisWidth": 600,
-    "SimpleIp": "127.0.0.1"
-  },
-  "layout": {
-    "chassis": {
-      "width": 600,
-      "length": 900,
-      "contour": [-450, 300, 450, 300, 450, -300, -450, -300]
-    },
-    "components": []
-  }
-}

deploy/start_clumsy_agv1.bat

@@ -1,9 +0,0 @@
-@echo off
-setlocal
-cd /d "%~dp0"
-if not exist chassis.json copy /Y ..\..\deploy\chassis.json chassis.json >nul
-if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv1\clumsy.json clumsy.json >nul
-if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv1\clumsyconsole.json clumsyconsole.json >nul
-echo [Clumsy AGV1] Master, tag Multi1. Start Medulla build\Medulla first.
-ClumsyLite.exe
-pause

deploy/start_clumsy_agv2.bat

@@ -1,9 +0,0 @@
-@echo off
-setlocal
-cd /d "%~dp0"
-if not exist chassis.json copy /Y ..\..\deploy\chassis.json chassis.json >nul
-if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv2\clumsy.json clumsy.json >nul
-if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv2\clumsyconsole.json clumsyconsole.json >nul
-echo [Clumsy AGV2] Slave, tag Multi2. Start Medulla build\Medulla_AGV2 first.
-ClumsyLite.exe
-pause

docs/FleetCrabWalkWorkContext.md

@@ -2,7 +2,7 @@
 
 > 本文档汇总 **当前仓库状态、外部依赖、运行/日志路径、代码地图与待解决问题**，便于后续继续调试「车队联动-自动蟹行」。
 >
-> 最后更新：2026-06-29
+> 最后更新：2026-07-01
 
 ---
 
@@ -11,8 +11,8 @@
 | 阶段 | 状态 | 说明 |
 |------|------|------|
 | 动作能启动、能下发运动 | ✅ 已解决 | 方案 1（预热）修复了启动期 `(0,0,0)` 快照导致 `Track()` 立即结束（`iter=0`）的问题 |
-| 路径跟踪质量 | 🔧 已改，待实测 | 2026-06-29 继续处理：自动蟹行改为复用手动蟹行同款 `mode=1` 下发链路，只叠加小幅平滑横向纠偏 |
+| 路径跟踪质量 | 🔧 已改，待实测 | 2026-07-01 改为自动字段链路；MovementTest 中 `FleetCrabAngleDeg=-x` 表示车身保持当前角度，以 x 度夹角追踪路径 |
-| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）丝滑；因此自动抖动主要来自纠偏链路而非底盘执行能力 |
+| 与手动蟹行对照 | ✅ 已验证 | 手动模式（FleetRemote `mode==1`）仍保留；自动蟹行不再复用脚本手动链路 |
 
 **触发方式**：主车 Clumsy → MovementTest 面板 → **「车队联动-自动蟹行」**（`FleetCrabWalkTest`）。
 
@@ -36,37 +36,39 @@
    - 已处理：`TickMultiVehicle` 每拍开头的 `PublishFleetCenter(0,0,0)` 已移除，避免动作/控制线程并发读到假中心。
 
 2. **横向纠偏 `bias` 项在蟹行模式下的参考系**
-   - `MultiWheelGeometricController.PerformGoing` 中 `bias = -bias` 后按 Stanley 形式修正 gcp（`BiasFac` / `BiasThreshold`）。  
+   - 当前实现不改 MDCSToolbox，只参考几何控制器思路在 `MultiWheelC` 内计算。
-   - 蟹行时 `thDiff` 来自路径切线（≈夹角），`dTh` 参考固定 `CrabTargetHeading`；若 `bias` 符号或 fleet 中心更新滞后，会持续向一侧推。
+   - `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向修正。
-   - 已绕开：`FleetCrabWalk` 当前不再用几何控制器直接下发 gcp；改为 Detour 计算 `along/lateral/remain`，再写脚本 `MultiVehicleScriptVx/Vy`。
+   - 输出直接写 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/Y/Th`，由 `TickMultiVehicle` 自动分支统一下发。
+   - MovementTest 会令 `BodyToPathAngleDeg = FleetCrabAngleDeg`，因此 `targetBodyTh = pathTh - BodyToPathAngleDeg = 启动时车队朝向`。
 
 3. **`MultiVehicleSyncUseDetour=true` 时的 POS 补偿与控制器抢方向盘**
    - 当前 `deploy/clumsy_agv1/clumsy.json` 中 `MultiVehicleSyncUseDetour: true`。
    - 各车 SLAM 偏差经 `PosBias*` 叠加到 `SendMotion`，可能与几何控制器横向纠偏形成耦合振荡。
 
-4. **动作期间关闭了 `MultiVehicleAutoUseIdealCenter`**  
+4. **理想车队中心前馈**
-   - 有意为之（避免 ideal 中心回灌快照、抹平真实 bias）。副作用是仅依赖「快照中心 + bias 闭环」，对快照质量更敏感。
+   - 当前动作会发布 `MultiVehicleAutoIdealX/Y/Th`。
+   - `MultiVehicleAutoUseIdealCenter=true` 时，从车使用该理想中心做 layout 前馈；关闭后只用当前广播中心和补偿项。
 
 5. **路径/起点几何**
    - 起点：`TryGetFleetCenterFromSlam()`；路径：`LineTrack(x0,y0 → dst)`，`phi = theta + CrabAngleDeg`。
    - 若 `theta` 与运行时 `CenterTh` 不一致，或 layout 反推中心与控制器使用的快照中心有系统偏差，会表现为沿某一轴漂移。
 
 **建议下一轮日志对照**：
-- `FleetCrabDbg`：`lateral` 是否收敛、`corr/localAngle/cmd` 是否平滑、有无到达纠偏上限  
+- `FleetCrabDbg`：`lateral` 是否收敛、`headingErr` 是否收敛、`bias/dth` 是否到达阈值、`auto(vx,fTh,rTh)` 是否稳定
-- `MultiVehicleDbg`：`frontTh/rearTh/speed` 是否接近手动蟹行、POS/Detect 补偿是否在持续驱动，`CRAB in/raw/limit/rev` 是否显示 `raw=-95°` 这类角度未被反向等价转换  
+- `MultiVehicleDbg`：自动分支是否为 `auto:true/manual:false/script:false`，`BASE/SEND` 是否接近 `FleetCrabDbg` 输出，POS/Detect 补偿是否持续驱动
-- 如需回退旧几何控制器路线，再看 `CrabDbg` 的 `bias/biasItem/gcp/fleetPos`
+- 重点看 `FleetCrabGcpThetaThreshold`、`BiasThreshold`、`DthLinearThreshold` 三个限幅是否过早截断纠偏
 
-**2026-06-29 DLog 结论（自动蟹行仍抖动）**：
+**2026-06-29 DLog 结论（旧脚本链路下自动蟹行仍抖动）**：
-- `FleetCrabDbg` 中 `along/lateral/remain/corr/localAngle/cmd` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
+- 旧 `FleetCrabDbg` 中 `along/lateral/remain/旧方向修正/旧命令` 基本平滑，横向误差多在几十 mm 内，未见路径控制器发散。
 - 主/从 `MultiVehicleDbg` 中 `BASE vx` 在正负之间跳，同时 `fTh/rTh` 在 `+90°/-90°` 附近翻转；这是同一横移矢量被错误地按 ±90° 边界转换成两种等价表示，底盘执行层会看到接近 180° 的转向跳变。
 - POS 补偿在该批日志中为关闭/零补偿（`corr:false`、`POS comp 0`），Detect 补偿有小幅值但不是主因。
-- 因此本轮判定为 **mode=1 蟹行矢量合成把 ±90° 误当舵角边界**，不是优先调 `FleetCrabCorrectionGain`。Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 默认约为 `-120/+120`，自动蟹行应允许 `-95°` 直接下发。
+- 因此本轮判定为 **mode=1 蟹行矢量合成把 ±90° 误当舵角边界**，不是优先调横向纠偏增益。Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 默认约为 `-120/+120`，自动蟹行应允许 `-95°` 直接下发。
 
 **2026-06-29 DLog 结论（±120 修复后仍 Y+ 漂移）**：
 - Clumsy 侧 `MultiVehicleDbg` 已显示 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，`BASE vx` 不再正负翻转，说明上层 `mode=1` 表达已连续，剧烈抖动问题已消失。
 - 但 `FleetCrabDbg` 中 `lateral` 仍从 `0` 单调增长到约 `+171mm`，`corr` 到达 `-8°` 上限后无法拉回；主/从 `DETECT dy` 也增长到百毫米量级，`DETECT comp y` 达到 `20mm/s` 上限。
 - 进一步检查 Playground 发现：`D:\MDCS\Source\Core\Medulla\Playground\default_scene.json` 与运行目录 `bin\Debug\net8.0\default_scene.json` 中两台 `multi-steering` 仍为 `"maxSteeringAngle": 90`，而 `ActuatorModels.cs` 会把模块舵角 clamp 到 `[-MaxSteeringAngleRad,+MaxSteeringAngleRad]`。
-- 这意味着 Clumsy 发出的 `-98°` 路径纠偏，在 Playground 实际执行时会被夹回 `-90°`，纠偏分量被吞掉；这比继续调 `FleetCrabCorrectionGain` 更像 Y+ 漂移的直接原因。
+- 这意味着 Clumsy 发出的 `-98°` 路径纠偏，在 Playground 实际执行时会被夹回 `-90°`，纠偏分量被吞掉；这比继续调横向纠偏增益更像 Y+ 漂移的直接原因。
 - 已把 Playground 源码场景和运行目录场景改为 `maxSteeringAngle: 120`，并在仿真器中加入 `multi-steering clamp` 节流日志；复测前必须重启 Playground 使场景重载。若复测时仍出现该日志，说明还有其他配置或场景副本在限制舵角。
 
 ### 2.2 两车抖动、不丝滑
@@ -82,7 +84,7 @@
 **建议对照实验**：
 - 手动 FleetRemote 蟹行（同速度、同角度）是否也抖
 - 临时 `MultiVehicleSyncUseDetour=false` 复测
-- 看 `FleetCrabDbg` 的 `corr/localAngle/cmd` 与 `MultiVehicleDbg` 的 `frontTh/rearTh` 是否周期跳变
+- 看 `FleetCrabDbg` 的 `auto(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `BASE/SEND` 是否周期跳变
 
 ---
 
@@ -214,8 +216,8 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
 
 | Topic | 来源 | 内容 |
 |-------|------|------|
-| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/corr/localAngle/cmd` |
+| **`FleetCrabDbg`** | `MovementTests.cs` | `ENTER/CENTER/START/WARMUP/ITER/DONE`，含 `along/lateral/remain/headingErr/baseTh/bias/dth/auto/ideal` |
-| **`CrabDbg`** | `MultiWheelGeometricController.cs` | 旧几何控制器路线诊断；当前脚本蟹行实现不再依赖 |
+| **`CrabDbg`** | `MultiWheelGeometricController.cs` | MDCSToolbox 几何控制器诊断；当前自动蟹行只参考其思路，不修改也不依赖该源码 |
 | **`MultiVehicleDbg`** | `PilotDefinition.cs` | 联动循环：速度、舵角、补偿、ready 状态 |
 | **`FleetDiagClumsy`** | `PilotDefinition.cs` | 精简 fleet 诊断（带 `car{N}` 前缀） |
 | **`MultiVehicle`** | `PilotDefinition.cs` | 初始化、心跳、HTTP 错误 |
@@ -223,9 +225,9 @@ DLog 由 **Clumsy 进程工作目录**下的 `dlog\` 管理（FundamentalLib）
 
 ### 6.3 建议抓取顺序（排查漂移/抖动）
 
-1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/corr/localAngle/cmd`  
+1. 主车 `FleetCrabDbg`：`WARMUP done` → `ITER#` 中 `lateral/remain/headingErr/bias/dth/auto(vx,fTh,rTh)`
-2. 主车 + 从车 `MultiVehicleDbg`：`frontTh/rearTh`、`PosBias*`、是否 `ready=false`  
+2. 主车 + 从车 `MultiVehicleDbg`：`auto:true/manual:false`、`BASE/SEND`、`PosBias*`、是否 `ready=false`
-3. 若回退旧几何控制器路线，再看主车 `CrabDbg`：`bias` 是否单调增大；`fleetPos` 是否偶发 `(0,0,0)`  
+3. 若怀疑 MDCSToolbox 自动路径，再看主车 `CrabDbg`；当前 `FleetCrabWalk` 不直接调用该控制器
 4. 从车 `FleetDiagClumsy`：是否频繁掉线 / register 超时
 
 ---
@@ -237,23 +239,26 @@ MovementTest「车队联动-自动蟹行」
   FleetCrabWalk.Get()
     TryGetFleetCenterFromSlam()          → 路径起点 (x0,y0,θ)
     phi = theta + FleetCrabAngleDeg
-    MultiVehicleScriptEnabled = true
+    BodyToPathAngleDeg = FleetCrabAngleDeg
-    MultiVehicleScriptMode = 1           → 复用 FleetRemote 手动蟹行下发链路
+    targetBodyTh = phi - BodyToPathAngleDeg = theta
+    MultiVehicleScriptEnabled = false
+    MultiVehicleAutoEnabled = true       → 进入 TickMultiVehicle 自动分支
     WARMUP                               → 等编队成员就位
     loop:
       TryGetFleetCenterFromSlam()        → 当前车队中心
-      along/lateral/remain               → 沿线进度、横向偏差、剩余距离
+      along/lateral/remain/headingErr    → 沿线进度、横向偏差、剩余距离、车身目标朝向偏差
-      corr = clamp(Stanley(lateral), ±FleetCrabCorrectionAngleDeg)
+      bias = clamp(Stanley(lateral), ±BiasThreshold)
-      localAngle = (phi + corr) - currentTheta
+      dth = clamp(DthLinearFac * (targetBodyTh-currentTheta), ±DthLinearThreshold)
-      Vx/Vy slew limit                   → FleetCrabCommandAccel 平滑
+      frontTh/rearTh = clamp(phi-currentTheta + bias ± dth, ±FleetCrabGcpThetaThreshold)
-      MultiVehicleScriptVx/Vy = cmd
+      ideal = pathStart + pathDir * clamp(along, 0, FleetCrabLengthMm)
+      MultiVehicleAutoVx/FrontTh/RearTh/Ideal* = cmd
     PilotDefinition.TickMultiVehicle (50ms)
-      manual/script mode==1
+      auto branch
-      Vx/Vy → speed + frontTh==rearTh
+      MultiVehicleAuto* → speed + frontTh/rearTh + ideal center
       notify → 从车 SendMotion + POS/Detect 补偿
 ```
 
-**对照 baseline**：`PilotDefinition.cs` 手动分支 `fleetMode == 1`（FleetRemote 蟹行）直接合成 `frontTh/rearTh`，不经几何控制器 `bias` 闭环。
+**对照 baseline**：`PilotDefinition.cs` 手动分支 `fleetMode == 1`（FleetRemote 蟹行）仍直接合成 `frontTh/rearTh`；自动蟹行当前不走该分支。
 
 ---
 
@@ -263,15 +268,19 @@ MovementTest「车队联动-自动蟹行」
 
 | 字段 | 默认 | 作用 |
 |------|------|------|
-| `FleetCrabAngleDeg` | 45 | 路径与车队朝向夹角 (deg) |
+| `FleetCrabAngleDeg` | 45 | 路径方向相对启动时车队朝向的夹角 (deg)。MovementTest 同时把车身-路径夹角设为该值；若输入“路径与小车夹角 x 度”，应填 `-x` 以保持当前车身角度 |
 | `FleetCrabLengthMm` | 2000 | 路径长度 (mm) |
-| `FleetCrabSpeed` | 0.2 | 速度 (m/s) |
+| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由自动蟹行专用减速参数下调 |
-| `FleetCrabGcpThetaThreshold` | 95 | 兼容旧几何控制器实现；当前脚本蟹行不直接使用 |
+| `FleetCrabAccel` | 0.2 | 速度命令加速度限制 (m/s^2)，限制 `MultiVehicleAutoVx` 每拍变化量；`<=0` 表示不限制 |
-| `FleetCrabCorrectionGain` | 1.0 | 横向误差纠偏增益 |
+| `FleetCrabSlowDistance` | 2000 | 末端开始减速距离 (mm) |
-| `FleetCrabCorrectionAngleDeg` | 8 | 自动纠偏最大改向角，越小越接近手动蟹行 |
+| `FleetCrabFinishDistance` | 20 | 完成距离 (mm)，剩余距离低于该值时结束动作 |
-| `FleetCrabCommandAccel` | 0.4 | 脚本 `Vx/Vy` 命令斜率限制(m/s²) |
+| `FleetCrabFinishSpeed` | 0.02 | 末端最低速度 (m/s) |
+| `FleetCrabSlowingPow` | 0.8 | 末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速 |
+| `FleetCrabGcpThetaThreshold` | 95 | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限，应给实际舵角限位与 `AngleLimitMarginDeg` 留余量 |
 
-动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleScript*` 和 `MultiVehicleAuto*`。
+已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略；新的自动链路使用 `FleetCrabAccel` 控制 `MultiVehicleAutoVx` 速度命令斜率。
+
+动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleAuto*`，并保持 `MultiVehicleScriptEnabled=false`。
 
 ### 8.2 影响跟踪/手感的全局项（节选）
 
@@ -282,7 +291,12 @@ MovementTest「车队联动-自动蟹行」
 | `TestCarSyncDistance` | 2400 | 与 Playground 双车间距一致 |
 | `MultiVehicleSyncInterval` | 50 | 联动周期 ms |
 | `DriveTaskInterval` | 50 | `clumsy.json` 顶层 |
-| `BiasFac` / `DthLinearFac` 等 | 继承 `MultiWheelPilotConfig` | 几何控制器 PID 形态参数 |
+| `BiasFac` / `BiasThreshold` | 继承 `MultiWheelPilotConfig` | 横向偏差 `lateral` → 前后 GCP 同向修正 |
+| `DthLinearFac` / `DthLinearThreshold` | 继承 `MultiWheelPilotConfig` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正 |
+| `FleetCrabSlowDistance` / `FleetCrabSlowingPow` / `FleetCrabFinishDistance` / `FleetCrabFinishSpeed` | `PilotConfig` | 自动蟹行专用终点减速和结束判定 |
+| `MultiVehicleAutoUseIdealCenter` | true（默认） | 使用自动蟹行发布的 ideal center 给从车做前馈 |
+| `MultiVehicleAutoRequireFleetCenter` | true（默认） | 自动模式无有效车队中心时整队停车 |
+| `MultiVehicleAutoCmdTimeoutMs` | 0（auto） | 自动命令新鲜度超时，避免控制器停发后沿末速度滑行 |
 
 详见 [MultiVehicleConfig.md](./MultiVehicleConfig.md) §2–§6。
 
@@ -296,20 +310,20 @@ MovementTest「车队联动-自动蟹行」
 | 蟹行要求朝向不变但有纠偏 | `CrabHoldHeading` + `CrabTargetHeading`；保留 `dTh` |
 | 多车 firstTurn 破坏队形 | `MultiVehicleSync` 时跳过 `firstTurnN`（TODO 整队预旋转） |
 | gcp 被 45° 上限截断 | 动作侧 `GcpThetaThreshold=95` |
-| ideal 中心抹平横向误差 | 动作期间关 `MultiVehicleAutoUseIdealCenter` |
+| ideal 中心抹平横向误差 | 已改为显式发布 `MultiVehicleAutoIdealX/Y/Th`，由 `MultiVehicleAutoUseIdealCenter` 控制是否前馈 |
 | Tick 中间窗口发布 `(0,0,0)` 假中心 | 已移除 tick 开头 `PublishFleetCenter(0,0,0)` |
-| 自动蟹行纠偏导致抖动 | 已改为脚本手动蟹行链路 + 小幅平滑横向纠偏 |
+| 自动蟹行纠偏导致抖动 | 已改为自动字段链路，按 `lateral/headingErr/remain` 计算 `MultiVehicleAuto*` |
 | 接近纯横移时速度符号/舵角表示翻转 | `fleetMode==1` 改为按 `MultiVehicleCrabSteerLimitDeg`（默认 120°）归一化；`-95°` 直接下发，超过上限才做速度取反的等价转换，并在 `MultiVehicleDbg` 输出 `CRAB in/raw/limit/rev` |
 
 ---
 
 ## 10. 后续工作建议（优先级）
 
-1. **复测 -90° 自动蟹行**：重点看 `MultiVehicleDbg` 的 `CRAB raw=-9x`、`limit=120.0`、`rev:false`，以及 `BASE vx/fTh/rTh` 是否不再正负翻转。  
+1. **复测自动蟹行**：重点看 `FleetCrabDbg` 的 `auto=(vx,fTh,rTh)` 与 `MultiVehicleDbg` 的 `auto:true/manual:false` 是否一致。
-2. **A/B：`MultiVehicleCrabSteerLimitDeg`** 默认 120，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；若实际轮角限制不同，先同步该值。  
+2. **A/B：`FleetCrabGcpThetaThreshold`** 默认 95，应与实车舵角限制和 `AngleLimitMarginDeg` 匹配；若输出很快被限幅，先核对该值。
-3. **A/B：`FleetCrabCorrectionAngleDeg`** 先试 4、8、12：4 最接近手动，12 收敛更快；当前不再因跨 ±90° 直接翻面。  
+3. **A/B：`BiasFac/BiasThreshold`** 若 `lateral` 单向增长，先看 `bias` 是否到上限；需要更强横向纠偏时调这组参数。
-4. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分脚本纠偏 vs POS 补偿贡献。  
+4. **A/B：`DthLinearFac/DthLinearThreshold`** 若车队朝向偏差收敛慢或前后 GCP 差动过大，调这组参数。
-5. **路径误差**：若仍持续 Y+ 漂移，看 `lateral` 是否持续单向增长；若增长但 `corr` 已到上限，增大 `FleetCrabCorrectionAngleDeg` 或 `FleetCrabCorrectionGain`。  
+5. **A/B：`MultiVehicleSyncUseDetour=false`** 若仍抖，跑同一条蟹行，区分自动路径纠偏 vs POS 补偿贡献。
 6. **notify 平滑**（中长期）：见 `MultiVehicleAutoSyncReview.md` §F。
 
 ---

docs/MultiVehicleAutoSyncReview.md

@@ -127,7 +127,7 @@ lock (MultiVehicleFleet)
 编队间距配置变更时，实际控制点半径仍固定 510mm，补偿/转向几何与 `TestCarSyncDistance` 不一致，调参困难。
 
 **建议修复**  
-统一使用 `syncDistance / 2f` 或配置项 `MultiVehicleControlRadius`，删除 magic number 510。
+统一使用 `syncDistance / 2f`，删除 magic number 510。
 
 ---
 
@@ -203,7 +203,7 @@ lock (MultiVehicleFleet)
 - **B**：`MultiVehicleSendMotion` 回调写入 `MultiVehicleAutoCmdTime`；主车自动分支按 `MultiVehicleAutoCmdTimeoutMs`(0=auto) 判定命令新鲜度，超时清零速度/idealPos 并关闭 `AutoEnabled`，避免末速度滑行。
 - **C**：新增本地 `_multiVehicleFleetSeen` 存活时刻表，register/notify 收到即刷新；主车 Tick `PruneStaleFleetMembers()` 按 `MultiVehicleMemberTtlMs`(0=auto) 剔除掉线成员，`fleetReady`(数量==总数) 因此蕴含全员新鲜。
 - **D**：回调不再丢弃 `idealPos/idealAngle`，写入 `MultiVehicleAutoIdeal*` 并经 notify(`HasIdeal/IdealX/Y/Th`) 广播；自动模式下以理想车队中心作为各车 layout 前馈目标（`MultiVehicleAutoUseIdealCenter`，默认开）。
-- **E**：新增 `MultiVehicleControlRadius`(0=syncDistance/2)，`ControlPointRadius` 与 `SendMotion(localControlRadius)` 统一取该值，删除硬编码 510。
+- **E**：`ControlPointRadius` 与 `SendMotion(localControlRadius)` 统一取 `syncDistance / 2f`，删除硬编码 510。
 - **F**：notify 改为 POST + JSON body（取代 GET query 串）；新增单调递增 `Seq`，从车丢弃乱序旧包（含主车重启回退识别）。
 - **G**：新增 `FleetCenterSnapshot` 不可变快照 + `volatile` 引用，`PublishFleetCenter` 整体赋值，控制器线程 `GetFleetCenterSnapshot()` 只读完整快照，消除 torn read。
 - **H**：自动模式新增 `MultiVehicleAutoRequireFleetCenter`(默认开) 门控——无有效车队中心（定位丢失）时强制停车，补上纯 SLAM 模式安全网；手动模式不受限。

docs/MultiVehicleConfig.md

@@ -94,17 +94,18 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 
 ## 4. 手动遥控（Medulla 车队遥控）
 
-遥控在 Medulla 侧产生指令（`MultiVehicleManual*`，`[AsLowerIO]` 上报 Clumsy）。摇杆输出已是物理单位，Clumsy 端系数应保持 **1（直通）**。
+遥控在 Medulla 侧产生指令（`MultiVehicleManual*`，`[AsLowerIO]` 上报 Clumsy）。车队联动遥控只上报归一化摇杆比例 `[-1,1]`；实际速度/舵角/角速度统一在 Clumsy 的 `FleetManual*` 参数中换算，避免 Medulla 和 Clumsy 两层缩放叠加。
 
 | 端 | 字段 | 含义 | 推荐值 |
 |----|------|------|--------|
-| Medulla `[AsInitParam]` | `MaxManualSpeed` | 车队手动**最大线速度(m/s)** | `0.3` |
+| Medulla `[AsLowerIO]` | `MultiVehicleManualVx/Vy/Vth` | 车队遥控摇杆比例（仅 `[-1,1]`，不带物理单位） | 摇杆值 |
-| Medulla `[AsInitParam]` | `MaxManualAngularSpeed` | 车队手动**最大转向角(deg)** | `45` |
+| Clumsy `clumsy.json` | `FleetManualMaxSpeed` | 满杆线速度(m/s) | `0.3` |
-| Clumsy `clumsy.json` | `ManualCarSyncVxFac` | 手动 Vx 系数（**保持 1，勿再缩放**） | `1.0` |
+| Clumsy `clumsy.json` | `FleetManualMaxSteerAngleDeg` | 常规模式满杆转向舵角(deg) | `45` |
-| Clumsy `clumsy.json` | `ManualCarSyncVthFac` | 手动 Vth 系数（**保持 1**） | `1.0` |
+| Clumsy `clumsy.json` | `FleetManualMaxCrabAngleDeg` | 蟹行模式满杆蟹行舵角(deg) | `60` |
+| Clumsy `clumsy.json` | `FleetManualMaxRotateOmegaDegPerSec` | 原地旋转模式满杆角速度(deg/s) | `45` |
 | Clumsy `clumsy.json` | `SyncThAccPerSec` | 转向角爬升速率(deg/s) | `30` |
 
-> 历史坑：`ManualCarSyncVxFac=0.2 × MaxManualSpeed=0.3 → 0.06 m/s`，肉眼几乎不动；`VthFac=5 → 225°` 超舵轮范围。已统一为系数=1。
+> 历史坑：旧链路会出现 `ManualCarSyncVxFac × MaxManualSpeed` 叠乘，导致满杆只有 `0.06 m/s` 这类异常低速；现在车队联动不再使用 Medulla 的 `MaxManualSpeed/MaxManualAngularSpeed`，这些字段只属于普通手动遥控。
 
 操作：在 Medulla 打开车体工具 **「FleetRemote / 车队联动遥控」**（workspace 摇杆，松手自动回零），开「车队联动」开关后拖摇杆即可。主车摇杆驱动全队；从车由主车广播自动跟随，**无需**各自开开关。**不要**同时打开普通「手动控制」面板（会抢占优先级）。
 
@@ -144,28 +145,49 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 
 ## 6. 自动蟹行动作（FleetCrabWalk / MovementTest「车队联动-自动蟹行」）
 
-在 Clumsy 侧 MovementTest 面板触发，以**当前车队中心**为起点，构造一条与车队朝向夹角 `FleetCrabAngleDeg`、长度 `FleetCrabLengthMm` 的**直线路径**，执行侧复用 FleetRemote 已验证丝滑的脚本手动等价输入（`MultiVehicleScriptEnabled + mode=1`）让整队**斜向平移（蟹行）**：
+在 Clumsy 侧 MovementTest 面板触发，以**当前车队中心**为起点，构造一条与启动时车队朝向夹角 `FleetCrabAngleDeg`、长度 `FleetCrabLengthMm` 的**直线路径**。MovementTest 会保持启动时车身朝向追踪路径；因此如果“路径相对小车”的夹角为 `x` 度（路径在车体右侧为正），应配置 `FleetCrabAngleDeg = -x`。当前实现不再复用脚本手动链路，而是参考几何控制器思路，在 `MultiWheelC` 内计算并写入 `MultiVehicleAuto...` 字段：
 
-- 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral` 和剩余距离 `remain`。
+- 动作每拍读取主车 Detour 反推车队中心，计算直线进度 `along`、横向偏差 `lateral`、剩余距离 `remain` 和车身目标朝向偏差 `headingErr`。
-- 横向偏差只转成一个**小幅、带斜率限制的蟹行方向修正**，再写入 `MultiVehicleScriptVx/Vy`；`TickMultiVehicle` 仍按手动蟹行逻辑合成 `frontTh==rearTh` 并广播从车。
+- `lateral` 通过 `BiasFac/BiasThreshold` 转为前后 GCP 同向舵角修正；`headingErr` 通过 `DthLinearFac/DthLinearThreshold` 转为前后 GCP 反向舵角修正。
-- 这样保留手动蟹行的平滑执行链路，同时让自动动作具备温和的路径纠偏；避免旧几何控制器 `bias/dTh` 直接叠到 gcp 时出现舵角阶跃。
+- 动作直接输出 `MultiVehicleAutoVx/FrontTh/RearTh/IdealX/IdealY/IdealTh`，由 `TickMultiVehicle` 的自动分支统一广播、下发 `SendMotion`，并继续受识别丢失、成员超时、舵角余量不足等整队缓停联锁保护。
-- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点）。
+- **前提**：在**主车**（`MultiVehicleMasterEndpoint="/"`）上运行，且主车有 Detour 定位（用于反推车队中心起点与运行中闭环）。
 
 | 字段（`clumsy.json` → `msConf`） | 含义 | 默认值 |
 |------|------|--------|
-| `FleetCrabAngleDeg` | 蟹行路径**与当前车队朝向的夹角**(deg，逆时针为正)。决定斜行方向：0=正前方，90=正左方平移，-90=正右方。稳态下即各舵轮的蟹行角 | `45` |
+| `FleetCrabAngleDeg` | 蟹行路径方向相对**启动时车队朝向**的夹角(deg，逆时针为正)。MovementTest 同时把车身-路径夹角设为该值，因此 `FleetCrabAngleDeg=-x` 会让车身保持启动朝向，并以 `x` 度夹角追踪路径 | `45` |
 | `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` |
-| `FleetCrabSpeed` | 蟹行**行驶速度**(m/s) | `0.2` |
+| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被自动蟹行专用减速参数下调 | `0.2` |
-| `FleetCrabGcpThetaThreshold` | 兼容旧几何控制器实现的 gcp 舵角上限；当前脚本手动等价实现不直接使用 | `95` |
+| `FleetCrabAccel` | 蟹行速度命令加速度限制(m/s^2)，限制 `MultiVehicleAutoVx` 每拍变化量；`<=0` 表示不限制 | `0.2` |
-| `FleetCrabCorrectionGain` | 横向误差纠偏增益。增大后收敛更快，但更容易出现方向摆动 | `1` |
+| `FleetCrabSlowDistance` | 蟹行末端开始减速距离(mm) | `2000` |
-| `FleetCrabCorrectionAngleDeg` | 自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大纠偏越强 | `8` |
+| `FleetCrabFinishDistance` | 蟹行完成距离(mm)，剩余距离低于该值时结束动作 | `20` |
-| `FleetCrabCommandAccel` | `Vx/Vy` 命令斜率限制(m/s²)，抑制纠偏方向突变 | `0.4` |
+| `FleetCrabFinishSpeed` | 蟹行末端最低速度(m/s) | `0.02` |
-| `MultiVehicleCrabSteerLimitDeg` | mode=1 蟹行舵角上限，应与 Medulla 侧 `WheelAngleLowerLimit/UpperLimit` 匹配；`-95°` 在默认 120° 内会直接下发 | `120` |
+| `FleetCrabSlowingPow` | 蟹行末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速 | `0.8` |
+| `FleetCrabGcpThetaThreshold` | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限(deg)。应小于实际舵角可行范围，并给 `AngleLimitMarginDeg` 留余量 | `95` |
+
+自动蟹行还会使用下列通用控制参数：
+
+| 字段 | 影响 | 默认来源 |
+|------|------|----------|
+| `BiasFac` / `BiasThreshold` | 横向偏差 `lateral` → 前后 GCP 同向修正。增大后收敛更快，但过大可能摆动 | `MultiWheelPilotConfig` |
+| `DthLinearFac` / `DthLinearThreshold` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正，用于保持车身与路径夹角 | `MultiWheelPilotConfig` |
+| `MultiVehicleAutoUseIdealCenter` | 是否把自动蟹行计算出的 `IdealX/Y/Th` 广播给从车做前馈 | `true` |
+| `MultiVehicleAutoRequireFleetCenter` | 自动模式是否要求有效车队中心；定位/车队中心失效时整队停车 | `true` |
+| `MultiVehicleAutoCmdTimeoutMs` | 自动命令新鲜度超时，0 表示按联动周期自动计算 | `0` |
+| `MultiVehicleUseDetect` / `MultiVehicleDetectBias*` | 互识别纠正与安全门；检测丢失时整队停车 | 见 §5 |
+| `MultiVehicleSyncUseDetour` / `MultiVehiclePosBias*` | 车队内姿态纠正（POS 补偿）；不影响整车队中心计算 | 见 §5 |
+
+已删除的旧自动蟹行参数：
+
+| 已删除字段 | 原用途 | 当前替代 |
+|------------|--------|----------|
+| `FleetCrabCorrectionGain` | 旧脚本链路的横向误差纠偏增益 | `BiasFac` |
+| `FleetCrabCorrectionAngleDeg` | 旧脚本链路的最大改向角 | `BiasThreshold` / `FleetCrabGcpThetaThreshold` |
+| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 新自动链路使用 `FleetCrabAccel` 限制 `MultiVehicleAutoVx` |
 
 **行为要点 / 注意**
 
-- 当前实现不走 `MultiVehicleAuto*`/`MultiVehicleSendMotion`，也不再临时改 `MultiVehicleAutoUseIdealCenter`；结束/急停会清零脚本字段。
+- 当前实现走 `MultiVehicleAuto*` 自动字段链路，不再开启 `MultiVehicleScriptEnabled`，也不受 `MultiVehicleCrabSteerLimitDeg` 影响（该字段只影响手动 `mode=1` 蟹行）。
-- `FleetCrabDbg` 会记录 `along/lateral/remain/corr/localAngle/cmd(Vx,Vy)`；`MultiVehicleDbg` 可继续对照最终 `frontTh≈rearTh`、是否有 POS/Detect 补偿，以及 `CRAB in/raw/limit/rev` 是否在舵角上限内保持连续表达。
+- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/targetV/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。
 - `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。
 - Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。
 
@@ -191,8 +213,10 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 "TwoLegGuessX": -1600,
 "MultiVehicleFleetNum": 2,
 "MultiVehicleUseDetect": true,
-"ManualCarSyncVxFac": 1.0,
+"FleetManualMaxSpeed": 0.3,
-"ManualCarSyncVthFac": 1.0,
+"FleetManualMaxSteerAngleDeg": 45,
+"FleetManualMaxCrabAngleDeg": 60,
+"FleetManualMaxRotateOmegaDegPerSec": 45,
 "SyncThAccPerSec": 30,
 "MultiVehicleMasterEndpoint": "/",            // 从车: "127.0.0.1:8008"
 "PlaygroundRobotName": "agv_multi_1",          // 从车: "agv_multi_2"
@@ -202,11 +226,12 @@ TwoLegGuessX            = -(TestCarSyncDistance - DeltaDetectCenter)
 "FleetCrabAngleDeg": 45,
 "FleetCrabLengthMm": 2000,
 "FleetCrabSpeed": 0.2,
-"FleetCrabGcpThetaThreshold": 95,
+"FleetCrabAccel": 0.2,
-"FleetCrabCorrectionGain": 1.0,
+"FleetCrabSlowDistance": 2000,
-"FleetCrabCorrectionAngleDeg": 8,
+"FleetCrabFinishDistance": 20,
-"FleetCrabCommandAccel": 0.4,
+"FleetCrabFinishSpeed": 0.02,
-"MultiVehicleCrabSteerLimitDeg": 120
+"FleetCrabSlowingPow": 0.8,
+"FleetCrabGcpThetaThreshold": 95
 ```
 
 ```text

docs/停车机器人产品标准化规划书_V1.0.md

@@ -0,0 +1,332 @@
+# 停车机器人产品标准化规划书
+
+## 文档信息
+
+| 项目 | 内容 |
+|------|------|
+| 产品名称 | P2800 停车机器人 |
+| 文档版本 | V1.0 |
+| 编写日期 | 2026-07-14 |
+| 文档类型 | 产品规划书 |
+
+---
+
+# 一、项目目标
+
+## 1.1 总体目标
+
+将停车机器人建设成为具备标准化交付能力的产品，实现车辆自主识别、自主搬运及双车协同控制，逐步形成可复制、可推广、可持续迭代的产品体系。
+
+## 1.2 阶段目标
+
+### **短期目标（7.15~9.15）**
+
+- 保证固定场景下稳定完成车辆搬运演示
+- 提升现有算法稳定性
+- 补齐路径规划能力
+- 完善双车联动基础功能
+
+### **中期目标（9.15~11.30）**
+
+- 引入3D相机
+- 提升复杂场景适应能力
+- 建立稳定性测试体系
+
+### **长期目标（11.30~12.31）**
+
+- 车辆自主识别、自主搬运及双车稳定协同控制
+
+---
+
+# 二、系统现状
+
+目前停车机器人整体流程如下：
+
+```text
+轮胎识别
+    │
+    ▼
+轮胎定位
+    │
+    ▼
+钻车控制
+    │
+    ▼
+双车协同搬运
+```
+
+| 模块 | 当前状态 | 存在问题 |
+|------|----------|----------|
+| 雷达轮胎识别 | 已完成 | 识别误差最大±20mm，稳定性不足 |
+| 路径规划 | 未完成 | 当前仅目标跟踪，无规划能力 |
+| 钻车控制 | 已完成 | 对车辆停放姿态适应能力不足 |
+| 双车联动 | 已完成部分功能 | 横移、原地旋转能力缺失 |
+
+---
+
+# 三、产品演进规划
+
+## 第一阶段：展会保障（7.15~9.15）
+
+### 3.1 雷达轮胎识别优化
+
+保持当前3D雷达方案，不进行硬件更换。
+
+优化方向：
+
+- 提升识别稳定性
+
+目标：
+
+- 连续识别稳定
+
+---
+
+### 3.2 路径规划建设
+
+新增停车机器人路径规划模块。
+
+整体流程：
+
+```text
+轮胎识别
+    ↓
+目标位姿生成
+    ↓
+路径规划
+    ↓
+轨迹跟踪
+    ↓
+钻车控制
+```
+
+建设内容：
+
+- 固定场景路径规划
+- 钻车轨迹生成
+- 轨迹跟踪控制
+
+预计开发周期：约1个月。
+
+---
+
+### 3.3 双车联动完善
+
+完成手动模式：
+
+- 自由运动
+- 横移
+- 原地旋转
+- 任意角度斜行
+
+优化自动斜行稳定性。
+
+阶段交付目标：
+
+完成上海展会1：1场景下的稳定搬运。
+
+---
+
+## 第二阶段：产品完善（9月~年底）
+
+### 轮胎识别升级
+
+技术路线：
+
+```text
+3D雷达
+    ↓
+3D相机（规则识别）
+    ↓
+数据采集
+    ↓
+模型训练
+    ↓
+AI轮胎识别
+```
+
+说明：
+
+优先验证3D相机点云质量；若点云质量满足要求，可先替代雷达方案，再逐步推进深度识别算法。
+
+### 路径规划升级
+
+完善：
+
+- 自动规划
+- 轨迹优化
+- 自动纠偏
+- 障碍物绕行（预研）
+
+### 双车协同升级
+
+实现自动模式：
+
+- 自动横移
+- 自动原地旋转
+- 自动姿态调整
+- 双车同步控制
+
+---
+
+## 第三阶段：产品智能化（长期）
+
+建设统一算法平台。
+
+包括：
+
+- 3D相机AI识别
+- 模型持续优化
+- 多车型适配
+
+形成持续迭代能力。
+
+---
+
+# 四、稳定性建设
+
+稳定性测试贯穿整个研发周期。
+
+## 感知稳定性
+
+验证：
+
+- 不同车型
+- 不同轮胎尺寸
+- 杂物遮挡
+- 点云噪声
+- 光照变化（相机阶段）
+
+统计：
+
+- 识别成功率
+- 定位误差
+- 重复性
+
+## 钻车稳定性
+
+验证：
+
+- 左右偏移
+- 前后偏移
+- 初始角度偏差
+- 不同停车姿态
+
+验证规划算法鲁棒性。
+
+## 双车协同稳定性
+
+验证：
+
+- 横向偏差
+- 纵向偏差
+- 姿态误差
+
+验证自动纠偏能力和稳定双车联动能力。
+
+## 长时间运行测试
+
+开展：
+
+- 连续搬运测试
+- 连续运行测试
+- 异常恢复测试
+
+确保满足工程交付要求。
+
+---
+
+# 五、阶段里程碑
+
+| 时间节点 | 目标 |
+|-----------|------|
+| 9月 | 完成上海展会1：1场景下的稳定演示 |
+| 年底 | 完成路径规划、双车联动及稳定性建设 |
+| 长期 | 完成3D相机替代、AI识别及产品智能化 |
+
+
+
+
+
+然后你再帮我写一个停车机器人目标达成指标的一个MD文档，里面需要包含达成了短/中/长期目标的一些能力：
+
+**短期目标需要达到：**
+
+车端能力：
+
+1.实现钻车/出车过程中的路径规划能力；
+
+2.实现在AGV相对车体有略微偏移(角度≤3°、横向偏移≤50mm)的情况下能够成功规划路径并保证钻入过程无碰撞；
+
+3.实现钻入过程平滑无卡顿；
+
+4.实现在2D雷达/3D雷达/3D相机(非Learning)识别的前提下：识别—规划—控制钻车到位停止精度要达到±15mm/1°以内，重复性测试须达到至少N次；
+
+5.实现钻车过程从开始识别至AGV到位，钻两对轮需满足30s内完成，钻一对轮需满足18s内完成；
+
+6.实现双车联动(带载/空载)手动遥控器控制模式下(0.1m/s~0.3m/s)的自由运动、任意角度斜行、横移以及原地旋转；
+
+7.实现双车联动(带载/空载)自动控制模式下(0.1m/s~0.3m/s)的任意角度稳定斜行；
+
+8.实现双车联动(带载/空载)自动控制模式下斜行到位精度达到±10mm/1°以内；
+
+9.实现双车联动(带载/空载)手/自动控制模式下在两车之间有略微偏移(角度≤2°，横向偏移≤30mm)的情况下依旧能够稳定完成移动操作；
+
+10.稳定完成上海展会同场景下的循环搬运任务，保证任务成功率为100%；
+
+注意：双车联动功能无论是基于网络信号较好的Simple服务器转发或双车直连通信模块，都需要实现前面提到的能力。
+
+调度能力：
+
+1.实现在收到搬车任务后，在没有开启车端避障的前提下，两台AGV的运动过程不发生碰撞；
+
+2.实现在收到搬车任务后，两台AGV的运动需要相对同步，不允许出现锁点问题导致两台AGV距离过远；
+
+3.实现在收到搬车任务后，两台AGV能够分别从车头和车尾同时钻入；
+
+
+
+**中期目标需达到：**
+
+在短期目标能力的基础上额外实现：
+
+车端能力：
+
+1.实现钻车过程从开始识别至AGV到位，钻两对轮需满足20s内完成，钻一对轮需满足12s内完成；
+
+2.实现在AGV相对车体有较大偏移(角度≤6°、横向偏移≤100mm)的情况下能够成功规划并保证钻入过程无碰撞；
+
+3.实现在3D相机(Learning)识别的前提下：识别—规划—控制钻车到位停止精度要达到±5mm/0.5°以内，重复性测试须达到至少N次；
+
+4.实现从车体侧面的钻车能力；
+
+5.实现双车联动(带载/空载)自动控制模式下(0.3m/s~1.0m/s)的自由运动、任意角度斜行、横移以及原地旋转；
+
+6.实现双车联动(带载/空载)自动控制模式下双车通讯、定位异常及其他异常时两车同时立即停车的安全机制；
+
+7.实现双车联动(带载/空载)手/自动控制模式下在两车之间有略微偏移(角度≤5°，横向偏移≤50mm)的情况下依旧能够稳定完成移动操作；
+
+调度能力：
+
+1.实现在收到搬车任务后，若场景内有超过两台以上的AGV，根据AGV状态自动选择最合适执行当前搬运任务的两台AGV；
+
+1.实现在收到搬车任务后，根据两台AGV的状态自动判断二者钻入顺序，同时保证第一台AGV以车头传感器识别的形式钻入，第二台AGV以车尾传感器识别的形式钻入；
+
+
+
+**长期目标须达到：**
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

docs/停车机器人产品能力达成指标_V1.0.md

@@ -0,0 +1,127 @@
+# 停车机器人产品能力达成指标（V1.0）
+
+> 本文档定义停车机器人产品在短期、中期及长期三个阶段应达到的核心能力指标，作为产品研发、测试验收及版本演进依据。
+
+---
+
+# 一、短期目标（展会交付版本）
+
+## 1. 车端能力
+
+### 1.1 钻车能力
+
+- 基于2D雷达/3D雷达/3D相机（非Learning）完成轮胎稳定识别；
+- 建立钻车、出车完整路径规划能力；
+- 完成"识别→目标生成→路径规划→轨迹跟踪→停车"闭环；
+- 重复定位测试≥100次，满足到位精度≤±15 mm，姿态误差≤±1°，成功率≥99%；
+- 实现从开始识别至AGV到位，钻两对轮需满足30s内完成，钻一对轮需满足18s内完成；
+- 当AGV相对车辆存在角度≤3°、横向偏移≤50 mm时，可自动规划无碰撞轨迹；
+- 支持轨迹平滑、速度连续，无明显急停、倒车抖动及振荡；
+- 钻车过程中绝不允许发生机械干涉；
+- 稳定完成上海展会同场景下的循环搬运任务，保证任务成功率为100%；
+
+### 1.2 双车联动能力
+
+支持带载/空载：
+
+- 手动(0.1m/s~0.3m/s)：自由运动、横移、任意角度斜行、原地旋转；
+- 自动(0.1m/s~0.3m/s)：稳定任意角度斜行；
+- 自动斜行停止精度≤±10 mm / ±1°；
+- 两车存在角度≤2°、横向误差≤30 mm安装误差时仍可稳定协同；
+- 支持Simple服务器转发及点对点通信两种模式；
+
+### 1.3 工程能力
+
+- 全过程日志记录；
+- 异常时能够人工接管；
+- 故障定位能力。
+
+---
+
+## 2. 调度能力
+
+- 在没有开启车端避障的前提下，车体间运动不干涉；
+- 车体间的运动需要相对同步，不允许出现锁点问题导致的两台AGV距离过远；
+- 两台AGV能够分别从车头和车尾同时钻入；
+
+---
+
+# 二、中期目标（产品化版本）
+
+在短期目标基础上新增：
+
+## 1. 车端能力
+
+### 1.1 钻车能力
+
+- 引入3D相机Learning算法；
+- 到位精度≤±5 mm、±0.5°；
+- 支持不同车型自动识别；
+- 支持车辆侧向钻车。
+
+- 偏移≤6°、≤100 mm条件下完成自主规划；
+- 支持复杂停车姿态自动修正。
+- 实现从开始识别至AGV到位，钻两对轮需满足20s内完成，钻一对轮需满足12s内完成；
+
+### 1.2 双车联动能力
+
+- 自动模式支持自由运动、横移、斜行、原地旋转（0.3~1.0 m/s）；
+- 通信、定位异常或其他异常时同步急停；
+- 两车≤5°、≤50 mm误差下稳定协同。
+
+## 2. 调度能力
+
+- 多AGV自动选车；
+- 自动确定钻车顺序；
+- 多任务调度；
+- AGV健康状态参与调度决策。
+
+---
+
+# 三、长期目标（标准化产品）
+
+## 1. 智能车端
+
+- AI轮胎识别持续学习；
+- 自动识别车型、轮胎规格、车辆姿态；
+- 多传感器融合感知（雷达+3D相机）。
+
+- 任意方向自主钻车；
+- 动态环境自主避障；
+
+- 双车高速协同（≥1.0 m/s）；
+- 全自动搬运无需人工干预；
+- 故障降级、自恢复、重新编队。
+
+## 2. 智能调度
+
+- 停车场级多机器人调度；
+- 全局交通管理；
+- 自动充电、自动换班；
+- 云端监控与远程运维；
+- 跨停车场统一调度。
+
+## 3. 产品平台能力
+
+- 算法平台化；
+- 数据闭环（采集→标注→训练→部署）；
+- 自动化回归测试；
+- 多车型快速适配。
+
+---
+
+# 四、产品成熟度目标
+
+| 阶段 | 产品定位 | 核心目标 |
+|------|----------|----------|
+| 短期 | Demo交付 | 展会稳定演示，形成基础闭环 |
+| 中期 | 工程产品 | 满足工程交付及批量部署 |
+| 长期 | 标准产品 | 智能化、多车型、多机器人停车搬运平台 |
+
+
+
+# 五、说明
+
+本文档为停车机器人能力建设阶段的目标达成指标说明，用于明确阶段性能力建设方向与评估基准，并非最终产品定版标准。
+
+文档中所列精度、耗时、偏移容差、重复性次数等定量指标，以及部分定性能力描述，将依据实际研发验证、现场测试数据、产品定位变更及项目交付要求进行动态调整。如指标发生变更，以后续正式发布的修订版本或相关专项技术方案为准。