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

MultiWheel/MultiWheelC/AGV.cs

@@ -69,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:后雷达识别
@@ -192,7 +211,7 @@ namespace MultiWheelC
                 },
                 CarDirection = 180f,
                 SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
+                MaxSpeed = 0.25f,
                 EnableHandover = true,
                 HandoverDistance = 200f,
                 HandoverSpeed = 0.3f,
@@ -246,6 +265,9 @@ namespace MultiWheelC
                     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;
@@ -253,6 +275,12 @@ namespace MultiWheelC
                 }
                 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),
@@ -373,6 +401,101 @@ namespace MultiWheelC
             }
         }
 
+        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)
         {
             DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");

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

@@ -520,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.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;
@@ -426,518 +425,57 @@ public class FleetRotateInPlaceTest : MovementTest
     }
 }
 
-// ===== 车队联动-自动蟹行动作 =====
+[MovementTest(name = "车队联动-曲线行走")]
-// 以当前车队中心为起点，构造指定方向和长度的直线路径；
+public class FleetCurveWalkTest : MovementTest
-// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
-//
-// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
-// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
-// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
-// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
-//
-// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
-public class FleetCrabWalk : MovementDefinition
 {
-    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
+    private FleetCurveWalk _proc;
-    public float CrabAngleDeg = 45f;
+    private DriveTask _task;
 
-    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
+    public override void Test()
-    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;
+        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;
-        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);
+            DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
-            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
+            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
-            driveSpeed = -driveSpeed;
+            return;
-            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);
+        var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
-        rearTh = ClampAbs(rearTh, limit);
+        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();
     }
 
-    private static float ProbeSpeed(float speed)
+    public override void TestStop()
     {
-        return Math.Abs(speed) > 1e-4f ? speed : 1f;
+        _proc?.Stop();
-    }
+        _task?.Stop();
-
-    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.MultiVehicleControlRadius > 0
-            ? conf.MultiVehicleControlRadius
-            : 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/PilotConfig.cs

@@ -45,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;
@@ -202,6 +199,25 @@ public class PilotConfig : MultiWheelPilotConfig
     [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腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
@@ -305,6 +321,14 @@ public class PilotConfig : MultiWheelPilotConfig
     [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/PilotDefinition.cs

@@ -1123,10 +1123,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
         if (fleetReady && !fleetStopActive)
         {
             chassis.SetOriginBias(layoutX, layoutY, layoutTh);
-            // E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
+            // E: 统一控制点半径，取 syncDistance/2，与编队几何一致。
-            var controlRadius = Conf.MultiVehicleControlRadius > 0
+            var controlRadius = syncDistance / 2f;
-                ? Conf.MultiVehicleControlRadius
-                : syncDistance / 2f;
             chassis.ControlPointRadius = controlRadius;
             // #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
             chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac;

MultiWheel/MultiWheelC/TireFollowing.cs

@@ -297,7 +297,8 @@ namespace MultiWheelC
                     if (EnableHandover)
                     {
                         controller.SlowDistance = float.MinValue;
-                        controller.FinishDistance = 20;
+                        controller.FinishSpeed = 0.2f;
+                        controller.FinishDistance = 50;
                     }
                     (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
                     var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
@@ -416,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)
                     {
@@ -423,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;

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/停车机器人产品标准化规划书_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交付 | 展会稳定演示，形成基础闭环 |
+| 中期 | 工程产品 | 满足工程交付及批量部署 |
+| 长期 | 标准产品 | 智能化、多车型、多机器人停车搬运平台 |
+
+
+
+# 五、说明
+
+本文档为停车机器人能力建设阶段的目标达成指标说明，用于明确阶段性能力建设方向与评估基准，并非最终产品定版标准。
+
+文档中所列精度、耗时、偏移容差、重复性次数等定量指标，以及部分定性能力描述，将依据实际研发验证、现场测试数据、产品定位变更及项目交付要求进行动态调整。如指标发生变更，以后续正式发布的修订版本或相关专项技术方案为准。