拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Control/Abstractions/ILateralController.cs

@@ -0,0 +1 @@
+// 所有横向控制器的统一接口

MultiWheelC/Control/Abstractions/ILongitudinalController.cs

@@ -0,0 +1 @@
+// 统一纵向控制接口

MultiWheelC/Control/Abstractions/LateralControlCommand.cs

@@ -0,0 +1 @@
+// 横向控制器的输出

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -0,0 +1 @@
+// 保存一次控制周期需要的完整输入

MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs

@@ -0,0 +1 @@
+// 车体中心命令曲率转换成前后GCP方向

MultiWheelC/Control/Allocation/GcpMotionCommand.cs

@@ -0,0 +1,9 @@
+// 表示发送给底盘前的中间命令
+// public readonly struct GcpMotionCommand
+// {
+//     public double SpeedMetersPerSecond { get; }
+
+//     public double FrontAngleRadians { get; }
+
+//     public double RearAngleRadians { get; }
+// }

MultiWheelC/Control/Execution/GcpCommandExecutor.cs

@@ -0,0 +1 @@
+// 负责把纯数学命令转换成现有底盘调用

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -0,0 +1 @@
+// 负责组织一个控制周期

MultiWheelC/Experiments/ClampTests.cs

@@ -0,0 +1,90 @@
+
+
+using System;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC
+{
+    public abstract class ClampMovementTestBase : MovementTest
+    {
+        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
+
+        private DriveTask _task;
+        protected abstract bool Close { get; }
+
+        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
+        public override void Test()
+        {
+            var leftTarget = Close
+                ? PilotDefinition.Self.LeftArmUpperPos
+                : PilotDefinition.Self.LeftArmLowerPos;
+            var rightTarget = Close
+                ? PilotDefinition.Self.RightArmUpperPos
+                : PilotDefinition.Self.RightArmLowerPos;
+
+            if (float.IsNaN(leftTarget) ||
+                float.IsInfinity(leftTarget) ||
+                float.IsNaN(rightTarget) ||
+                float.IsInfinity(rightTarget))
+            {
+                Console.WriteLine(
+                    "夹臂目标位置无效，取消夹臂运动测试。");
+                return;
+            }
+
+            // 防止重复点击时上一项夹臂任务仍在运行。
+            TestStop();
+            Console.WriteLine(
+                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
+                $"左目标={leftTarget}，右目标={rightTarget}");
+
+            var task = new DriveTask(
+                new ClampToTarget
+                {
+                    LeftClampTarget = leftTarget,
+                    RightClampTarget = rightTarget,
+                    TimeoutSeconds = TimeoutSeconds
+                }.Get());
+            _task = task;
+
+            try
+            {
+                task.Wait();
+            }
+            finally
+            {
+                PilotDefinition.Self.SpeedLeftArm = 0f;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+                if (ReferenceEquals(_task, task))
+                    _task = null;
+            }
+        }
+
+        // 停止夹臂任务并立即清零左右夹臂下发速度。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+            PilotDefinition.Self.SpeedLeftArm = 0f;
+            PilotDefinition.Self.SpeedRightArm = 0f;
+        }
+    }
+
+    [MovementTest(name = "夹臂关闭测试")]
+    public sealed class TestClampCloseMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => false;
+    }
+
+    [MovementTest(name = "夹臂启动测试")]
+    public sealed class TestClampOpenMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => true;
+    }
+}

MultiWheelC/Experiments/PathTrackingTests.cs

@@ -1,108 +1,20 @@
-using ClumsyCore;
+
-using ClumsyCore.DTools;
+
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using MDCSToolBox.Commons.Controllers;
-using MDCSToolBox.Clumsy.Tracks;
-using MyParking.Shared;
 using System;
 using System.Collections.Generic;
 using System.Numerics;
 using System.Threading;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Clumsy.Tracks;
+using MyParking.Shared;
 
 namespace MultiWheelC
 {
-    internal static class MovementTestPreparation
-    {
-        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
-        public static bool AlignWheelsForward(
-            ref DriveTask activeTask)
-        {
-            var preparation = new PrepareWheelsForward();
-            var task = new DriveTask(preparation.Get());
-            activeTask = task;
-
-            try
-            {
-                task.Wait();
-                return preparation.Completed;
-            }
-            catch (Exception ex)
-            {
-                Console.WriteLine(
-                    $"测试前舵轮回正失败：{ex.Message}");
-                return false;
-            }
-            finally
-            {
-                task.Stop();
-                if (ReferenceEquals(activeTask, task))
-                    activeTask = null;
-            }
-        }
-
-        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
-        public static bool AreWheelsForward(
-            float toleranceDegrees = 2f)
-        {
-            var chassis =
-                PilotDefinition.Chassis as MultiWheelChassis;
-            if (chassis == null)
-            {
-                Console.WriteLine(
-                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
-                return false;
-            }
-
-            try
-            {
-                var adapter = new MultiWheelChassisAdapter(
-                    chassis,
-                    PilotDefinition.Self.CarNum);
-                var toleranceRadians =
-                    AngleMath.DegreesToRadians(toleranceDegrees);
-
-                if (adapter.AreParallelWheelsAligned(
-                        0.0,
-                        toleranceRadians))
-                {
-                    return true;
-                }
-
-                Console.WriteLine(
-                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
-                return false;
-            }
-            catch (Exception ex)
-            {
-                Console.WriteLine(
-                    $"检查舵轮方向失败：{ex.Message}");
-                return false;
-            }
-        }
-    }
-
-    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
-    public class AlignWheelsForwardTest : MovementTest
-    {
-        private DriveTask _task;
-
-        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
-        public override void Test()
-        {
-            MovementTestPreparation.AlignWheelsForward(
-                ref _task);
-        }
-
-        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _task = null;
-        }
-    }
-
     [MovementTest(name = "SendMotion：连续前进4m")]
     public class TestForward4m : MovementTest
     {
@@ -179,148 +91,6 @@ namespace MultiWheelC
         }
     }
 
-    public abstract class InPlaceRotateTestBase : MovementTest
-    {
-        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
-        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
-        public int TrialNumber = 1; // 重复实验编号。
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-        private readonly string _trajectoryName;
-
-        protected InPlaceRotateTestBase(
-            float relativeAngleDegrees,
-            string trajectoryName)
-        {
-            RelativeAngleDegrees =
-                relativeAngleDegrees;
-            _trajectoryName =
-                trajectoryName;
-        }
-
-        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
-        public override void Test()
-        {
-            if (float.IsNaN(RelativeAngleDegrees) ||
-                float.IsInfinity(RelativeAngleDegrees) ||
-                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
-                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
-                MaxAngularSpeedDegreesPerSecond <= 0f)
-            {
-                Console.WriteLine("原地旋转测试参数无效。");
-                return;
-            }
-
-            var location = DetourInterface.getCartLocation();
-            if (double.IsNaN(location.x) ||
-                double.IsInfinity(location.x) ||
-                double.IsNaN(location.y) ||
-                double.IsInfinity(location.y) ||
-                double.IsNaN(location.th) ||
-                double.IsInfinity(location.th))
-            {
-                Console.WriteLine(
-                    "Detour当前位姿无效，取消原地旋转测试。");
-                return;
-            }
-
-            var rotationCenter =
-                new Vector2((float)location.x, (float)location.y);
-            var targetWorldAngle =
-                (float)AngleMath.NormalizeDegrees(
-                    location.th + RelativeAngleDegrees);
-
-            _recorder = new TrackingExperimentRecorder(
-                controllerName: "InPlaceRotatePID",
-                trajectoryName: _trajectoryName,
-                trialNumber: TrialNumber,
-                referenceStart: rotationCenter,
-                referenceEnd: rotationCenter,
-                referenceSpeed: 0f,
-                referenceAngularSpeed:
-                    (float)AngleMath.DegreesToRadians(
-                        MaxAngularSpeedDegreesPerSecond));
-            _recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(
-                    new MultiWheelRotateInPlace
-                    {
-                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
-                        AngleTarget = targetWorldAngle,
-                        PidparamsRead = () => new PIDParams
-                        {
-                            Kp =
-                                PilotDefinition.Conf.InPlaceRotateKp,
-                            Ki =
-                                PilotDefinition.Conf.InPlaceRotateKi,
-                            Kd =
-                                PilotDefinition.Conf.InPlaceRotateKd,
-                            DeadZone =
-                                PilotDefinition.Conf
-                                    .InPlaceRotateArriveDeg,
-                            SpeedAccPerSec =
-                                PilotDefinition.Conf.InPlaceRotateAcc,
-                            OutputUpperThreshold =
-                                MaxAngularSpeedDegreesPerSecond,
-                            MaxI =
-                                PilotDefinition.Conf.InPlaceRotateMaxI
-                        },
-                        CommandAngularSpeedObserver =
-                            commandAngularSpeed =>
-                                _recorder?.UpdateCommand(
-                                    0f,
-                                    (float)AngleMath.DegreesToRadians(
-                                        commandAngularSpeed))
-                    }.Get());
-
-                _task.Wait();
-
-                // 保留少量停止后的样本，用于观察角速度是否回到零。
-                Thread.Sleep(300);
-            }
-            finally
-            {
-                _task?.Stop();
-                _recorder?.UpdateCommand(0f, 0f);
-                _recorder?.StopAndSave();
-                _task = null;
-                _recorder = null;
-            }
-        }
-
-        // 停止原地旋转并保存当前已经采集的实验数据。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateCommand(0f, 0f);
-            _recorder?.StopAndSave();
-        }
-
-    }
-
-    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
-    public sealed class TestRotate90 :
-        InPlaceRotateTestBase
-    {
-        public TestRotate90()
-            : base(90f, "Rotate90")
-        {
-        }
-    }
-
-    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
-    public sealed class TestRotate180 :
-        InPlaceRotateTestBase
-    {
-        public TestRotate180()
-            : base(180f, "Rotate180")
-        {
-        }
-    }
-
     [MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
     public class TestArcMovement : MovementTest
     {
@@ -449,7 +219,6 @@ namespace MultiWheelC
         }
     }
 
-    #region 蟹行运动测试
     [MovementTest(name = "SendMotion：蟹行直线4m")]
     public class TestCrabForward4m : MovementTest
     {
@@ -863,87 +632,4 @@ namespace MultiWheelC
                 origin.Y + localX * sin + localY * cos);
         }
     }
-
-    #endregion
-    
-    #region 夹臂运动测试
-    public abstract class ClampMovementTestBase : MovementTest
-    {
-        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
-
-        private DriveTask _task;
-        protected abstract bool Close { get; }
-
-        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
-        public override void Test()
-        {
-            var leftTarget = Close
-                ? PilotDefinition.Self.LeftArmUpperPos
-                : PilotDefinition.Self.LeftArmLowerPos;
-            var rightTarget = Close
-                ? PilotDefinition.Self.RightArmUpperPos
-                : PilotDefinition.Self.RightArmLowerPos;
-
-            if (float.IsNaN(leftTarget) ||
-                float.IsInfinity(leftTarget) ||
-                float.IsNaN(rightTarget) ||
-                float.IsInfinity(rightTarget))
-            {
-                Console.WriteLine(
-                    "夹臂目标位置无效，取消夹臂运动测试。");
-                return;
-            }
-
-            // 防止重复点击时上一项夹臂任务仍在运行。
-            TestStop();
-            Console.WriteLine(
-                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
-                $"左目标={leftTarget}，右目标={rightTarget}");
-
-            var task = new DriveTask(
-                new ClampToTarget
-                {
-                    LeftClampTarget = leftTarget,
-                    RightClampTarget = rightTarget,
-                    TimeoutSeconds = TimeoutSeconds
-                }.Get());
-            _task = task;
-
-            try
-            {
-                task.Wait();
-            }
-            finally
-            {
-                PilotDefinition.Self.SpeedLeftArm = 0f;
-                PilotDefinition.Self.SpeedRightArm = 0f;
-                if (ReferenceEquals(_task, task))
-                    _task = null;
-            }
-        }
-
-        // 停止夹臂任务并立即清零左右夹臂下发速度。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _task = null;
-            PilotDefinition.Self.SpeedLeftArm = 0f;
-            PilotDefinition.Self.SpeedRightArm = 0f;
-        }
-    }
-
-    [MovementTest(name = "夹臂关闭测试")]
-    public sealed class TestClampCloseMovement
-        : ClampMovementTestBase
-    {
-        protected override bool Close => false;
-    }
-
-    [MovementTest(name = "夹臂启动测试")]
-    public sealed class TestClampOpenMovement
-        : ClampMovementTestBase
-    {
-        protected override bool Close => true;
-    }
-    #endregion
 }

MultiWheelC/Experiments/RotationTests.cs

@@ -0,0 +1,158 @@
+
+
+using System;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MDCSToolBox.Commons.Controllers;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    public abstract class InPlaceRotateTestBase : MovementTest
+    {
+        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
+        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
+        public int TrialNumber = 1; // 重复实验编号。
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        private readonly string _trajectoryName;
+
+        protected InPlaceRotateTestBase(
+            float relativeAngleDegrees,
+            string trajectoryName)
+        {
+            RelativeAngleDegrees =
+                relativeAngleDegrees;
+            _trajectoryName =
+                trajectoryName;
+        }
+
+        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
+        public override void Test()
+        {
+            if (float.IsNaN(RelativeAngleDegrees) ||
+                float.IsInfinity(RelativeAngleDegrees) ||
+                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
+                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
+                MaxAngularSpeedDegreesPerSecond <= 0f)
+            {
+                Console.WriteLine("原地旋转测试参数无效。");
+                return;
+            }
+
+            var location = DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消原地旋转测试。");
+                return;
+            }
+
+            var rotationCenter =
+                new Vector2((float)location.x, (float)location.y);
+            var targetWorldAngle =
+                (float)AngleMath.NormalizeDegrees(
+                    location.th + RelativeAngleDegrees);
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "InPlaceRotatePID",
+                trajectoryName: _trajectoryName,
+                trialNumber: TrialNumber,
+                referenceStart: rotationCenter,
+                referenceEnd: rotationCenter,
+                referenceSpeed: 0f,
+                referenceAngularSpeed:
+                    (float)AngleMath.DegreesToRadians(
+                        MaxAngularSpeedDegreesPerSecond));
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(
+                    new MultiWheelRotateInPlace
+                    {
+                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
+                        AngleTarget = targetWorldAngle,
+                        PidparamsRead = () => new PIDParams
+                        {
+                            Kp =
+                                PilotDefinition.Conf.InPlaceRotateKp,
+                            Ki =
+                                PilotDefinition.Conf.InPlaceRotateKi,
+                            Kd =
+                                PilotDefinition.Conf.InPlaceRotateKd,
+                            DeadZone =
+                                PilotDefinition.Conf
+                                    .InPlaceRotateArriveDeg,
+                            SpeedAccPerSec =
+                                PilotDefinition.Conf.InPlaceRotateAcc,
+                            OutputUpperThreshold =
+                                MaxAngularSpeedDegreesPerSecond,
+                            MaxI =
+                                PilotDefinition.Conf.InPlaceRotateMaxI
+                        },
+                        CommandAngularSpeedObserver =
+                            commandAngularSpeed =>
+                                _recorder?.UpdateCommand(
+                                    0f,
+                                    (float)AngleMath.DegreesToRadians(
+                                        commandAngularSpeed))
+                    }.Get());
+
+                _task.Wait();
+
+                // 保留少量停止后的样本，用于观察角速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        // 停止原地旋转并保存当前已经采集的实验数据。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+
+    }
+
+    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
+    public sealed class TestRotate90 :
+        InPlaceRotateTestBase
+    {
+        public TestRotate90()
+            : base(90f, "Rotate90")
+        {
+        }
+    }
+
+    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
+    public sealed class TestRotate180 :
+        InPlaceRotateTestBase
+    {
+        public TestRotate180()
+            : base(180f, "Rotate180")
+        {
+        }
+    }
+}

MultiWheelC/Experiments/WheelPreparationTests.cs

@@ -0,0 +1,103 @@
+
+
+using System;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    internal static class MovementTestPreparation
+    {
+        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
+        public static bool AlignWheelsForward(
+            ref DriveTask activeTask)
+        {
+            var preparation = new PrepareWheelsForward();
+            var task = new DriveTask(preparation.Get());
+            activeTask = task;
+
+            try
+            {
+                task.Wait();
+                return preparation.Completed;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"测试前舵轮回正失败：{ex.Message}");
+                return false;
+            }
+            finally
+            {
+                task.Stop();
+                if (ReferenceEquals(activeTask, task))
+                    activeTask = null;
+            }
+        }
+
+        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
+        public static bool AreWheelsForward(
+            float toleranceDegrees = 2f)
+        {
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
+                return false;
+            }
+
+            try
+            {
+                var adapter = new MultiWheelChassisAdapter(
+                    chassis,
+                    PilotDefinition.Self.CarNum);
+                var toleranceRadians =
+                    AngleMath.DegreesToRadians(toleranceDegrees);
+
+                if (adapter.AreParallelWheelsAligned(
+                        0.0,
+                        toleranceRadians))
+                {
+                    return true;
+                }
+
+                Console.WriteLine(
+                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
+                return false;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"检查舵轮方向失败：{ex.Message}");
+                return false;
+            }
+        }
+    }
+
+    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
+    public class AlignWheelsForwardTest : MovementTest
+    {
+        private DriveTask _task;
+
+        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
+        public override void Test()
+        {
+            MovementTestPreparation.AlignWheelsForward(
+                ref _task);
+        }
+
+        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+        }
+    }
+}

MultiWheelC/Movements.cs

@@ -1,629 +0,0 @@
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Tracks;
-using MDCSToolBox.Commons.Controllers;
-using System;
-using System.Collections.Generic;
-using System.Drawing;
-using System.Numerics;
-using System.Threading;
-using FundamentalLib;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
-    public class PrepareWheelsForward : MovementDefinition
-    {
-        public float ToleranceDegrees = 2f;
-        public float StableSeconds = 0.3f;
-        public float TimeoutSeconds = 10f;
-        public bool Completed { get; private set; }
-
-        public override IEnumerable<bool> Get()
-        {
-            var chassis =
-                PilotDefinition.Chassis as MultiWheelChassis;
-            if (chassis == null)
-            {
-                throw new InvalidOperationException(
-                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
-            }
-
-            var adapter = new MultiWheelChassisAdapter(
-                chassis,
-                PilotDefinition.Self.CarNum);
-            adapter.ResetToBodyFrame();
-            var toleranceRadians =
-                AngleMath.DegreesToRadians(ToleranceDegrees);
-            var startTime = DateTime.UtcNow;
-            DateTime? alignedSince = null;
-
-            Completed = false;
-            if (!adapter.PrepareParallelDirection(0.0))
-            {
-                throw new InvalidOperationException(
-                    "无法将所有舵轮下发到车体前向0°。");
-            }
-
-            try
-            {
-                while (true)
-                {
-                    var aligned =
-                        adapter.AreParallelWheelsAligned(
-                            0.0,
-                            toleranceRadians);
-
-                    if (aligned)
-                    {
-                        if (!alignedSince.HasValue)
-                            alignedSince = DateTime.UtcNow;
-
-                        if ((DateTime.UtcNow -
-                             alignedSince.Value).TotalSeconds >=
-                            StableSeconds)
-                        {
-                            Completed = true;
-                            yield break;
-                        }
-                    }
-                    else
-                    {
-                        alignedSince = null;
-                    }
-
-                    if (TimeoutSeconds > 0f &&
-                        (DateTime.UtcNow - startTime).TotalSeconds >
-                        TimeoutSeconds)
-                    {
-                        throw new TimeoutException(
-                            $"舵轮回正超过{TimeoutSeconds:F1}s，" +
-                            "测试已经取消。");
-                    }
-
-                    yield return true;
-                }
-            }
-            finally
-            {
-                // 只清零驱动速度，保留已经下发的0°舵角。
-                adapter.StopImmediately();
-            }
-        }
-    }
-
-    #region 功能项
-    public class Sleep : MovementDefinition
-    {
-        public float Second = 2f;
-
-        public override IEnumerable<bool> Get()
-        {
-            if (Second <= 0)
-            {
-                yield return false;
-                yield break;
-            }
-
-            var endTime = DateTime.UtcNow.AddSeconds(Second);
-            while (DateTime.UtcNow < endTime)
-            {
-                Thread.Sleep(50);
-                yield return true;
-            }
-
-            yield return false;
-        }
-    }
-    public class DriverAble : MovementDefinition
-    {
-        public int WaitTimeoutMs = 2000;
-        public int PollIntervalMs = 50;
-
-        // C层单车硬件：请求全部驱动轮复位并恢复使能。
-        public override IEnumerable<bool> Get()
-        {
-            PilotDefinition.Self.ResetFromC = true;
-
-            try
-            {
-                var start = DateTime.Now;
-                var timeoutMs = Math.Max(0, WaitTimeoutMs);
-                var pollMs = Math.Max(1, PollIntervalMs);
-
-                // 至少保留一个调度周期，确保M层能收到复位请求。
-                yield return true;
-
-                while (!PilotDefinition.Self.WheelAbleState &&
-                        (DateTime.Now - start).TotalMilliseconds < timeoutMs)
-                {
-                    Thread.Sleep(pollMs);
-                    yield return true;
-                }
-            }
-            finally
-            {
-                PilotDefinition.Self.ResetFromC = false;
-            }
-        }
-    }
-    public class DriverDisable : MovementDefinition
-    {
-        public int WaitTimeoutMs = 3000;
-        public int PollIntervalMs = 20;
-
-        // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
-        public override IEnumerable<bool> Get()
-        {
-            var timeoutMs = Math.Max(0, WaitTimeoutMs);
-            var pollMs = Math.Max(1, PollIntervalMs);
-            var startTime = DateTime.UtcNow;
-            var success = false;
-
-            PilotDefinition.Self.DisableFromC = true;
-
-            try
-            {
-                // 至少保持一个C层调度周期，确保M层能收到下使能请求。
-                yield return true;
-
-                success = !PilotDefinition.Self.WheelAbleState;
-
-                while (!success &&
-                        (DateTime.UtcNow - startTime).TotalMilliseconds <
-                            timeoutMs)
-                {
-                    Thread.Sleep(pollMs);
-
-                    success =
-                        !PilotDefinition.Self.WheelAbleState;
-
-                    if (!success)
-                    {
-                        yield return true;
-                    }
-                }
-            }
-            finally
-            {
-                // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
-                PilotDefinition.Self.DisableFromC = false;
-            }
-            if (success)
-            {
-                Console.WriteLine(
-                    $"驱动器下使能完成，" +
-                    $"WheelAbleState=" +
-                    $"{PilotDefinition.Self.WheelAbleState}");
-            }
-            else
-            {
-                Console.WriteLine(
-                    $"驱动器下使能超时，" +
-                    $"WheelAbleState=" +
-                    $"{PilotDefinition.Self.WheelAbleState}，" +
-                    $"等待{timeoutMs}ms");
-            }
-            yield return false;
-        }
-    }
-    #endregion
-
-    #region 直线运动
-    //在世界坐标系下，从路径起点追踪到终点并停车
-    public class DstTracker : MovementDefinition
-    {
-        public Vector2 Src;
-        public Vector2 Dst;
-        // 本次轨迹的巡航速度上限，单位m/s。
-        public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
-        public float CarDirectionBias = 0f;
-        public Painter Painter = UI.GetPainter("DstTracker");
-        public override IEnumerable<bool> Get()
-        {
-            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-            DriveTask task = null;
-            try
-            {
-                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
-                {
-                    BaseSpeed = MaxSpeed
-                }.Get();
-                // 要求路径末端速度下降到零。
-                tracker.FinishSpeed = 0f;
-                var linePath = new LineTrack(Src, Dst)
-                {
-                    CarDirectionBias = CarDirectionBias,
-                    Speed = MaxSpeed
-                };
-                tracker.AddTrack(linePath);
-                task = new DriveTask(tracker.Track());
-                task.Wait();
-                yield return false;
-            }
-            finally
-            {
-                task?.Stop();
-                chassis.PredefinedDriveStop();
-            }
-        }
-
-    }
-    //直线行走基于轮里程
-    // C层单车底盘：按照车轮里程行驶指定的相对距离。
-    public class LineTracking : MovementDefinition
-    {
-        // 相对动作启动位置的行驶距离，单位mm。
-        // 正数表示前进，负数表示后退。
-        public float TargetDistance;
-        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
-        public float Kp = PilotDefinition.Conf.LineTrackKp;
-        public float Ki = PilotDefinition.Conf.LineTrackKi;
-        public float Kd = PilotDefinition.Conf.LineTrackKd;
-        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
-        public int SrcId = -1;
-        public int DstId = -1;
-        public Action<int> LeaveSrcFunction;
-        // 接近目标后是否保留速度，交给下一个动作接管。
-        public bool EnableHandover;
-        // 进入动作衔接的剩余距离，单位mm。
-        public float HandoverDistance = 80f;
-        // HandoverSpeed小于0时，使用MaxSpeed的此比例。
-        public float HandoverSpeedRatio = 0.5f;
-        // 大于等于0时，直接作为衔接速度，单位m/s。
-        public float HandoverSpeed = -1f;
-        public float MinHandoverSpeed = 0.05f;
-        private PIDController _pid;
-        // 读取当前单车直线行驶里程，单位mm。
-        private static float ReadPosition()
-        {
-            return
-                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
-        }
-
-        // 根据动作启动位置和目标距离执行直线里程闭环。
-        public override IEnumerable<bool> Get()
-        {
-            if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(TargetDistance),
-                    "目标行驶距离必须是有限值。");
-            }
-
-            if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(MaxSpeed),
-                    "最大速度必须是大于零的有限值。");
-            }
-            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-            // 每次启动动作时重新读取起始编码器位置。
-            var startPosition = ReadPosition();
-            // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
-            var targetPosition = startPosition + TargetDistance;
-            _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
-            {
-                SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
-            };
-            var handoverRequested = false;
-            var keepHandoverSpeed = false;
-            DLog.Log(
-                $"直线里程动作：" +
-                $"起点={startPosition:F1}mm，" +
-                $"距离={TargetDistance:F1}mm，" +
-                $"目标={targetPosition:F1}mm",
-                "straight_line");
-            try
-            {
-                while (true)
-                {
-                    var currentPosition = ReadPosition();
-                    var remainingDistance = targetPosition - currentPosition;
-                    // 接近目标后，保留一定速度交给后续动作。
-                    if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
-                    {
-                        var direction = Math.Sign(remainingDistance);
-                        if (direction == 0)
-                        {
-                            direction = Math.Sign(TargetDistance);
-                        }
-                        var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
-                        var maximumSpeed = Math.Abs(MaxSpeed);
-                        var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
-                        var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
-                        var handoverSpeed = limitedSpeed * direction;
-                        chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
-                        handoverRequested = true;
-                        // 保持一个调度周期，让速度命令实际生效。
-                        yield return true;
-                        break;
-                    }
-                    var speed = _pid.GetResponse(targetPosition);
-                    chassis.SendXYThSpeed(speed, 0f, 0f);
-                    if (_pid.IsArrived())
-                    {
-                        break;
-                    }
-                    yield return true;
-                }
-                if (SrcId != -1 &&
-                    LeaveSrcFunction != null)
-                {
-                    LeaveSrcFunction(SrcId);
-                    DLog.Log($"释放放车点{SrcId}", "straight_line");
-                }
-                // 只有正常完成动作衔接时才允许保留非零速度。
-                keepHandoverSpeed = handoverRequested;
-            }
-            finally
-            {
-                // 普通完成、人工停止或异常退出时都必须停车。
-                if (!keepHandoverSpeed)
-                {
-                    chassis.SendXYThSpeed(0f, 0f, 0f);
-                }
-            }
-            yield return false;
-        }
-    }
-    //直线行走基于detour
-    public class LineTracking_based_detour : MovementDefinition
-    {
-        public float LineDistance = 1000f;
-        public int SrcId = -1;
-        public int DstId = -1;
-        public Action<int> LeaveSrcFunction = null;
-        public Painter painter = UI.GetPainter("Line", false);
-        // C层单车轨迹：执行早期版本的两点直线跟踪动作。
-        public override IEnumerable<bool> Get()
-        {
-            var curpose = DetourInterface.getCartLocation();
-            Console.WriteLine($"curpose.th:{curpose.th}");
-            var src = new Vector2((float)curpose.x, (float)curpose.y);
-            var headingRadians =
-                AngleMath.DegreesToRadians(curpose.th);
-            var dst = new Vector2(
-                (float)(curpose.x +
-                    LineDistance * Math.Cos(headingRadians)),
-                (float)(curpose.y +
-                    LineDistance * Math.Sin(headingRadians)));
-            // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
-            //                       (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
-            Console.WriteLine($"src:{src.X} {src.Y}");
-            Console.WriteLine($"dst:{dst.X} {dst.Y}");
-            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
-
-            var tracker = new ChassisController().Get();
-            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
-            tracker.AddTrack(linePath);
-            var _dt = new DriveTask(tracker.Track());
-            _dt.Wait();
-            if (SrcId != -1 && LeaveSrcFunction != null)
-            {
-                LeaveSrcFunction(SrcId);
-                DLog.Log($"释放放车点{SrcId}", "straight_line");
-            }
-            yield return false;
-        }
-    }
-    #endregion
-
-    #region 旋转运动
-    public class MultiWheelRotateInPlace : MovementDefinition
-    {
-        /// <summary>
-        /// 旋转目标角度
-        /// </summary>
-        public float AngleTarget;
-
-        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
-
-        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-
-        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
-
-        public PIDController thPid;
-
-        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
-        public Action<float> CommandAngularSpeedObserver;
-
-        // 自转前舵轮实际角度允许误差，单位deg。
-        public float WheelAlignmentToleranceDegrees = 2f;
-
-        // 自转舵轮连续保持到位的时间，单位s。
-        public float WheelAlignmentStableSeconds = 0.3f;
-
-        // 自转舵轮准备超时时间，单位s。
-        public float WheelAlignmentTimeoutSeconds = 10f;
-
-        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
-        public override IEnumerable<bool> Get()
-        {
-            if (Chassis == null)
-                throw new InvalidOperationException(
-                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");
-
-            var adapter = new MultiWheelChassisAdapter(
-                Chassis,
-                PilotDefinition.Self.CarNum);
-            adapter.ResetToBodyFrame();
-
-            try
-            {
-                var alignmentStarted = DateTime.Now;
-                DateTime? alignedSince = null;
-                while (true)
-                {
-                    if (!adapter.PrepareSpin())
-                        throw new InvalidOperationException(
-                            "无法生成原地自转舵轮目标：" +
-                            adapter.LastFailureReason);
-
-                    if (adapter.AreSpinWheelsAligned)
-                    {
-                        if (alignedSince == null)
-                            alignedSince = DateTime.Now;
-
-                        if ((DateTime.Now - alignedSince.Value)
-                                .TotalSeconds >=
-                            WheelAlignmentStableSeconds)
-                            break;
-                    }
-                    else
-                    {
-                        alignedSince = null;
-                    }
-
-                    if ((DateTime.Now - alignmentStarted)
-                            .TotalSeconds >
-                        WheelAlignmentTimeoutSeconds)
-                        throw new TimeoutException(
-                            "原地自转舵轮在限定时间内未稳定到位。");
-
-                    yield return true;
-                }
-
-                var targetAngle =
-                    (float)AngleMath.NormalizeDegrees(AngleTarget);
-                var p = PidparamsRead();
-                thPid = new PIDController(ThetaReader, p.Kp);
-                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
-                    p.OutputUpperThreshold, p.SpeedAccPerSec);
-                var lastCommandTime = DateTime.Now;
-
-                while (true)
-                {
-                    var s = thPid.GetResponse(targetAngle, true);
-                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
-                    CommandAngularSpeedObserver?.Invoke(s);
-                    var now = DateTime.Now;
-                    var interval = now - lastCommandTime;
-                    lastCommandTime = now;
-
-                    // PID输出s为deg/s，Shared命令统一使用rad/s。
-                    // adapter.Send最终调用普通安全版SendXYThSpeed。
-                    var omegaRadiansPerSecond =
-                        (float)AngleMath.DegreesToRadians(s);
-                    if (!adapter.Send(
-                            new ChassisCommand(
-                                PilotDefinition.Self.CarNum,
-                                new Twist2D(
-                                    0.0,
-                                    0.0,
-                                    omegaRadiansPerSecond)),
-                            interval))
-                    {
-                        throw new InvalidOperationException(
-                            "安全XYTh原地旋转底盘解算失败：" +
-                            adapter.LastFailureReason);
-                    }
-                    if (thPid.IsArrived()) break;
-                    yield return true;
-                }
-
-                Console.WriteLine($"final rotate to {targetAngle}");
-            }
-            finally
-            {
-                CommandAngularSpeedObserver?.Invoke(0f);
-                adapter.StopImmediately();
-            }
-        }
-    }
-    #endregion
-
-    #region 夹臂运动
-    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;
-        public float TimeoutSeconds = 30f;
-        private PIDController leftpid, rightpid;
-
-        // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
-        public override IEnumerable<bool> Get()
-        {
-            try
-            {
-                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
-                };
-
-                var startTime = DateTime.UtcNow;
-                while (true)
-                {
-                    if (TimeoutSeconds > 0f &&
-                        (DateTime.UtcNow - startTime).TotalSeconds >
-                        TimeoutSeconds)
-                    {
-                        Console.WriteLine(
-                            $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
-                            "停止左右夹臂。");
-                        yield break;
-                    }
-
-                    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;
-
-                    var leftArrived = leftpid.IsArrived();
-                    var rightArrived = rightpid.IsArrived();
-                    if (leftArrived)
-                        PilotDefinition.Self.SpeedLeftArm = 0f;
-                    if (rightArrived)
-                        PilotDefinition.Self.SpeedRightArm = 0f;
-
-                    if (leftArrived && rightArrived)
-                        break;
-
-                    yield return true;
-                }
-
-                Console.WriteLine(
-                    $"left clamp to target:{LeftClampTarget}  " +
-                    $"right clamp to target:{RightClampTarget}");
-            }
-            finally
-            {
-                PilotDefinition.Self.SpeedLeftArm = 0f;
-                PilotDefinition.Self.SpeedRightArm = 0f;
-            }
-        }
-    }
-    #endregion
-}

MultiWheelC/Movements/ClampToTarget.cs

@@ -0,0 +1,91 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Commons.Controllers;
+
+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;
+        public float TimeoutSeconds = 30f;
+        private PIDController leftpid, rightpid;
+
+        // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
+        public override IEnumerable<bool> Get()
+        {
+            try
+            {
+                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
+                };
+
+                var startTime = DateTime.UtcNow;
+                while (true)
+                {
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        Console.WriteLine(
+                            $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
+                            "停止左右夹臂。");
+                        yield break;
+                    }
+
+                    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;
+
+                    var leftArrived = leftpid.IsArrived();
+                    var rightArrived = rightpid.IsArrived();
+                    if (leftArrived)
+                        PilotDefinition.Self.SpeedLeftArm = 0f;
+                    if (rightArrived)
+                        PilotDefinition.Self.SpeedRightArm = 0f;
+
+                    if (leftArrived && rightArrived)
+                        break;
+
+                    yield return true;
+                }
+
+                Console.WriteLine(
+                    $"left clamp to target:{LeftClampTarget}  " +
+                    $"right clamp to target:{RightClampTarget}");
+            }
+            finally
+            {
+                PilotDefinition.Self.SpeedLeftArm = 0f;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+            }
+        }
+    }
+}

MultiWheelC/Movements/CrabMotionFrameTracker.cs

@@ -794,8 +794,8 @@ namespace MultiWheelC
                  Math.PI / 2.0 ||
                  !IsFinite(
                      MaximumVirtualSteeringRadians))
-                 throw new ArgumentOutOfRangeException(
+                throw new ArgumentOutOfRangeException(
-                     "蟹行轨迹测试参数无效。");
+                    "蟹行轨迹测试参数无效。");
         }
 
         private static double Limit(

MultiWheelC/Movements/DriverMovements.cs

@@ -0,0 +1,123 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using ClumsyCore.Pilot;
+
+namespace MultiWheelC
+{
+    public class Sleep : MovementDefinition
+    {
+        public float Second = 2f;
+
+        public override IEnumerable<bool> Get()
+        {
+            if (Second <= 0)
+            {
+                yield return false;
+                yield break;
+            }
+
+            var endTime = DateTime.UtcNow.AddSeconds(Second);
+            while (DateTime.UtcNow < endTime)
+            {
+                Thread.Sleep(50);
+                yield return true;
+            }
+
+            yield return false;
+        }
+    }
+
+    public class DriverAble : MovementDefinition
+    {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
+        // C层单车硬件：请求全部驱动轮复位并恢复使能。
+        public override IEnumerable<bool> Get()
+        {
+            PilotDefinition.Self.ResetFromC = true;
+
+            try
+            {
+                var start = DateTime.Now;
+                var timeoutMs = Math.Max(0, WaitTimeoutMs);
+                var pollMs = Math.Max(1, PollIntervalMs);
+
+                // 至少保留一个调度周期，确保M层能收到复位请求。
+                yield return true;
+
+                while (!PilotDefinition.Self.WheelAbleState &&
+                        (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+                    yield return true;
+                }
+            }
+            finally
+            {
+                PilotDefinition.Self.ResetFromC = false;
+            }
+        }
+    }
+    public class DriverDisable : MovementDefinition
+    {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
+        // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
+        public override IEnumerable<bool> Get()
+        {
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var startTime = DateTime.UtcNow;
+            var success = false;
+
+            PilotDefinition.Self.DisableFromC = true;
+
+            try
+            {
+                // 至少保持一个C层调度周期，确保M层能收到下使能请求。
+                yield return true;
+
+                success = !PilotDefinition.Self.WheelAbleState;
+
+                while (!success &&
+                        (DateTime.UtcNow - startTime).TotalMilliseconds <
+                            timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+
+                    success =
+                        !PilotDefinition.Self.WheelAbleState;
+
+                    if (!success)
+                    {
+                        yield return true;
+                    }
+                }
+            }
+            finally
+            {
+                // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
+                PilotDefinition.Self.DisableFromC = false;
+            }
+            if (success)
+            {
+                Console.WriteLine(
+                    $"驱动器下使能完成，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}");
+            }
+            else
+            {
+                Console.WriteLine(
+                    $"驱动器下使能超时，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}，" +
+                    $"等待{timeoutMs}ms");
+            }
+            yield return false;
+        }
+    }
+}

MultiWheelC/Movements/DstTracker.cs

@@ -0,0 +1,55 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MDCSToolBox.Clumsy.Tracks;
+
+namespace MultiWheelC
+{
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        // 本次轨迹的巡航速度上限，单位m/s。
+        public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+        public override IEnumerable<bool> Get()
+        {
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            DriveTask task = null;
+            try
+            {
+                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
+                {
+                    BaseSpeed = MaxSpeed
+                }.Get();
+                // 要求路径末端速度下降到零。
+                tracker.FinishSpeed = 0f;
+                var linePath = new LineTrack(Src, Dst)
+                {
+                    CarDirectionBias = CarDirectionBias,
+                    Speed = MaxSpeed
+                };
+                tracker.AddTrack(linePath);
+                task = new DriveTask(tracker.Track());
+                task.Wait();
+                yield return false;
+            }
+            finally
+            {
+                task?.Stop();
+                chassis.PredefinedDriveStop();
+            }
+        }
+
+    }
+}

MultiWheelC/Movements/LineTracking.cs

@@ -0,0 +1,178 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Numerics;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    // C层单车底盘：按照车轮里程行驶指定的相对距离。
+    public class LineTracking : MovementDefinition
+    {
+        // 相对动作启动位置的行驶距离，单位mm。
+        // 正数表示前进，负数表示后退。
+        public float TargetDistance;
+        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
+        public float Kp = PilotDefinition.Conf.LineTrackKp;
+        public float Ki = PilotDefinition.Conf.LineTrackKi;
+        public float Kd = PilotDefinition.Conf.LineTrackKd;
+        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction;
+        // 接近目标后是否保留速度，交给下一个动作接管。
+        public bool EnableHandover;
+        // 进入动作衔接的剩余距离，单位mm。
+        public float HandoverDistance = 80f;
+        // HandoverSpeed小于0时，使用MaxSpeed的此比例。
+        public float HandoverSpeedRatio = 0.5f;
+        // 大于等于0时，直接作为衔接速度，单位m/s。
+        public float HandoverSpeed = -1f;
+        public float MinHandoverSpeed = 0.05f;
+        private PIDController _pid;
+        // 读取当前单车直线行驶里程，单位mm。
+        private static float ReadPosition()
+        {
+            return
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
+        }
+
+        // 根据动作启动位置和目标距离执行直线里程闭环。
+        public override IEnumerable<bool> Get()
+        {
+            if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(TargetDistance),
+                    "目标行驶距离必须是有限值。");
+            }
+
+            if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(MaxSpeed),
+                    "最大速度必须是大于零的有限值。");
+            }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            // 每次启动动作时重新读取起始编码器位置。
+            var startPosition = ReadPosition();
+            // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
+            var targetPosition = startPosition + TargetDistance;
+            _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
+            {
+                SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
+            };
+            var handoverRequested = false;
+            var keepHandoverSpeed = false;
+            DLog.Log(
+                $"直线里程动作：" +
+                $"起点={startPosition:F1}mm，" +
+                $"距离={TargetDistance:F1}mm，" +
+                $"目标={targetPosition:F1}mm",
+                "straight_line");
+            try
+            {
+                while (true)
+                {
+                    var currentPosition = ReadPosition();
+                    var remainingDistance = targetPosition - currentPosition;
+                    // 接近目标后，保留一定速度交给后续动作。
+                    if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
+                    {
+                        var direction = Math.Sign(remainingDistance);
+                        if (direction == 0)
+                        {
+                            direction = Math.Sign(TargetDistance);
+                        }
+                        var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
+                        var maximumSpeed = Math.Abs(MaxSpeed);
+                        var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
+                        var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
+                        var handoverSpeed = limitedSpeed * direction;
+                        chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
+                        handoverRequested = true;
+                        // 保持一个调度周期，让速度命令实际生效。
+                        yield return true;
+                        break;
+                    }
+                    var speed = _pid.GetResponse(targetPosition);
+                    chassis.SendXYThSpeed(speed, 0f, 0f);
+                    if (_pid.IsArrived())
+                    {
+                        break;
+                    }
+                    yield return true;
+                }
+                if (SrcId != -1 &&
+                    LeaveSrcFunction != null)
+                {
+                    LeaveSrcFunction(SrcId);
+                    DLog.Log($"释放放车点{SrcId}", "straight_line");
+                }
+                // 只有正常完成动作衔接时才允许保留非零速度。
+                keepHandoverSpeed = handoverRequested;
+            }
+            finally
+            {
+                // 普通完成、人工停止或异常退出时都必须停车。
+                if (!keepHandoverSpeed)
+                {
+                    chassis.SendXYThSpeed(0f, 0f, 0f);
+                }
+            }
+            yield return false;
+        }
+    }
+
+
+
+
+    //直线行走基于detour
+    public class LineTracking_based_detour : MovementDefinition
+    {
+        public float LineDistance = 1000f;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction = null;
+        public Painter painter = UI.GetPainter("Line", false);
+        // C层单车轨迹：执行早期版本的两点直线跟踪动作。
+        public override IEnumerable<bool> Get()
+        {
+            var curpose = DetourInterface.getCartLocation();
+            Console.WriteLine($"curpose.th:{curpose.th}");
+            var src = new Vector2((float)curpose.x, (float)curpose.y);
+            var headingRadians =
+                AngleMath.DegreesToRadians(curpose.th);
+            var dst = new Vector2(
+                (float)(curpose.x +
+                    LineDistance * Math.Cos(headingRadians)),
+                (float)(curpose.y +
+                    LineDistance * Math.Sin(headingRadians)));
+            // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
+            //                       (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
+            Console.WriteLine($"src:{src.X} {src.Y}");
+            Console.WriteLine($"dst:{dst.X} {dst.Y}");
+            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
+            tracker.AddTrack(linePath);
+            var _dt = new DriveTask(tracker.Track());
+            _dt.Wait();
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "straight_line");
+            }
+            yield return false;
+        }
+    }
+}

MultiWheelC/Movements/PrepareWheelsForward.cs

@@ -0,0 +1,92 @@
+
+
+
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
+    public class PrepareWheelsForward : MovementDefinition
+    {
+        public float ToleranceDegrees = 2f;
+        public float StableSeconds = 0.3f;
+        public float TimeoutSeconds = 10f;
+        public bool Completed { get; private set; }
+
+        public override IEnumerable<bool> Get()
+        {
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+            adapter.ResetToBodyFrame();
+            var toleranceRadians =
+                AngleMath.DegreesToRadians(ToleranceDegrees);
+            var startTime = DateTime.UtcNow;
+            DateTime? alignedSince = null;
+
+            Completed = false;
+            if (!adapter.PrepareParallelDirection(0.0))
+            {
+                throw new InvalidOperationException(
+                    "无法将所有舵轮下发到车体前向0°。");
+            }
+
+            try
+            {
+                while (true)
+                {
+                    var aligned =
+                        adapter.AreParallelWheelsAligned(
+                            0.0,
+                            toleranceRadians);
+
+                    if (aligned)
+                    {
+                        if (!alignedSince.HasValue)
+                            alignedSince = DateTime.UtcNow;
+
+                        if ((DateTime.UtcNow -
+                                alignedSince.Value).TotalSeconds >=
+                            StableSeconds)
+                        {
+                            Completed = true;
+                            yield break;
+                        }
+                    }
+                    else
+                    {
+                        alignedSince = null;
+                    }
+
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"舵轮回正超过{TimeoutSeconds:F1}s，" +
+                            "测试已经取消。");
+                    }
+
+                    yield return true;
+                }
+            }
+            finally
+            {
+                // 只清零驱动速度，保留已经下发的0°舵角。
+                adapter.StopImmediately();
+            }
+        }
+    }
+}

MultiWheelC/Movements/RotateInPlace.cs

@@ -0,0 +1,134 @@
+
+
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MDCSToolBox.Commons.Controllers;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    public class MultiWheelRotateInPlace : MovementDefinition
+    {
+        /// <summary>
+        /// 旋转目标角度
+        /// </summary>
+        public float AngleTarget;
+
+        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+
+        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+
+        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
+
+        public PIDController thPid;
+
+        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
+        public Action<float> CommandAngularSpeedObserver;
+
+        // 自转前舵轮实际角度允许误差，单位deg。
+        public float WheelAlignmentToleranceDegrees = 2f;
+
+        // 自转舵轮连续保持到位的时间，单位s。
+        public float WheelAlignmentStableSeconds = 0.3f;
+
+        // 自转舵轮准备超时时间，单位s。
+        public float WheelAlignmentTimeoutSeconds = 10f;
+
+        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
+        public override IEnumerable<bool> Get()
+        {
+            if (Chassis == null)
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");
+
+            var adapter = new MultiWheelChassisAdapter(
+                Chassis,
+                PilotDefinition.Self.CarNum);
+            adapter.ResetToBodyFrame();
+
+            try
+            {
+                var alignmentStarted = DateTime.Now;
+                DateTime? alignedSince = null;
+                while (true)
+                {
+                    if (!adapter.PrepareSpin())
+                        throw new InvalidOperationException(
+                            "无法生成原地自转舵轮目标：" +
+                            adapter.LastFailureReason);
+
+                    if (adapter.AreSpinWheelsAligned)
+                    {
+                        if (alignedSince == null)
+                            alignedSince = DateTime.Now;
+
+                        if ((DateTime.Now - alignedSince.Value)
+                                .TotalSeconds >=
+                            WheelAlignmentStableSeconds)
+                            break;
+                    }
+                    else
+                    {
+                        alignedSince = null;
+                    }
+
+                    if ((DateTime.Now - alignmentStarted)
+                            .TotalSeconds >
+                        WheelAlignmentTimeoutSeconds)
+                        throw new TimeoutException(
+                            "原地自转舵轮在限定时间内未稳定到位。");
+
+                    yield return true;
+                }
+
+                var targetAngle =
+                    (float)AngleMath.NormalizeDegrees(AngleTarget);
+                var p = PidparamsRead();
+                thPid = new PIDController(ThetaReader, p.Kp);
+                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
+                    p.OutputUpperThreshold, p.SpeedAccPerSec);
+                var lastCommandTime = DateTime.Now;
+
+                while (true)
+                {
+                    var s = thPid.GetResponse(targetAngle, true);
+                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                    CommandAngularSpeedObserver?.Invoke(s);
+                    var now = DateTime.Now;
+                    var interval = now - lastCommandTime;
+                    lastCommandTime = now;
+
+                    // PID输出s为deg/s，Shared命令统一使用rad/s。
+                    // adapter.Send最终调用普通安全版SendXYThSpeed。
+                    var omegaRadiansPerSecond =
+                        (float)AngleMath.DegreesToRadians(s);
+                    if (!adapter.Send(
+                            new ChassisCommand(
+                                PilotDefinition.Self.CarNum,
+                                new Twist2D(
+                                    0.0,
+                                    0.0,
+                                    omegaRadiansPerSecond)),
+                            interval))
+                    {
+                        throw new InvalidOperationException(
+                            "安全XYTh原地旋转底盘解算失败：" +
+                            adapter.LastFailureReason);
+                    }
+                    if (thPid.IsArrived()) break;
+                    yield return true;
+                }
+
+                Console.WriteLine($"final rotate to {targetAngle}");
+            }
+            finally
+            {
+                CommandAngularSpeedObserver?.Invoke(0f);
+                adapter.StopImmediately();
+            }
+        }
+    }
+}

MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs

@@ -0,0 +1,509 @@
+using System;
+using System.Diagnostics;
+using ClumsyCore.Interfaces;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。
+    /// </summary>
+    public sealed class DetourVehicleStateProvider
+        : IVehicleStateProvider
+    {
+        public const double DefaultMaximumLinearSpeedMetersPerSecond =
+            1.20;
+        public const double DefaultMaximumAngularSpeedRadiansPerSecond =
+            Math.PI / 4.0;
+        public const double DefaultPositionJumpMarginMeters =
+            0.03;
+        public const double DefaultHeadingJumpMarginRadians =
+            5.0 * Math.PI / 180.0;
+        public const double DefaultVelocityPositionResidualMeters =
+            0.04;
+        public const double DefaultVelocityHeadingResidualRadians =
+            5.0 * Math.PI / 180.0;
+        public const double DefaultStationaryConfirmationSeconds =
+            0.35;
+
+        private const double MillimetersPerMeter = 1000.0;
+        private const double PositionEqualityToleranceMeters = 1e-9;
+        private const double HeadingEqualityToleranceRadians = 1e-8;
+
+        private readonly object _syncRoot = new object();
+        private readonly Stopwatch _clock = Stopwatch.StartNew();
+        private readonly VelocityEstimator2D _velocityEstimator;
+        private readonly double _maximumLinearSpeedMetersPerSecond;
+        private readonly double _maximumAngularSpeedRadiansPerSecond;
+        private readonly double _positionJumpMarginMeters;
+        private readonly double _headingJumpMarginRadians;
+        private readonly double _velocityPositionResidualMeters;
+        private readonly double _velocityHeadingResidualRadians;
+        private readonly double _stationaryConfirmationSeconds;
+
+        private bool _hasAcceptedPose;
+        private Pose2D _acceptedPoseInWorld;
+        private double _acceptedTimestampSeconds;
+        private VehicleState _latestState;
+        private bool _stationaryHoldActive;
+
+        /// <summary>
+        /// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。
+        /// </summary>
+        public DetourVehicleStateProvider()
+            : this(
+                new VelocityEstimator2D(),
+                DefaultMaximumLinearSpeedMetersPerSecond,
+                DefaultMaximumAngularSpeedRadiansPerSecond,
+                DefaultPositionJumpMarginMeters,
+                DefaultHeadingJumpMarginRadians,
+                DefaultVelocityPositionResidualMeters,
+                DefaultVelocityHeadingResidualRadians,
+                DefaultStationaryConfirmationSeconds)
+        {
+        }
+
+        /// <summary>
+        /// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。
+        /// </summary>
+        public DetourVehicleStateProvider(
+            VelocityEstimator2D velocityEstimator,
+            double maximumLinearSpeedMetersPerSecond,
+            double maximumAngularSpeedRadiansPerSecond,
+            double positionJumpMarginMeters,
+            double headingJumpMarginRadians,
+            double velocityPositionResidualMeters,
+            double velocityHeadingResidualRadians,
+            double stationaryConfirmationSeconds)
+        {
+            _velocityEstimator = velocityEstimator ??
+                throw new ArgumentNullException(
+                    nameof(velocityEstimator));
+
+            EnsureFinitePositive(
+                maximumLinearSpeedMetersPerSecond,
+                nameof(maximumLinearSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                maximumAngularSpeedRadiansPerSecond,
+                nameof(maximumAngularSpeedRadiansPerSecond));
+            EnsureFiniteNonNegative(
+                positionJumpMarginMeters,
+                nameof(positionJumpMarginMeters));
+            EnsureFiniteNonNegative(
+                headingJumpMarginRadians,
+                nameof(headingJumpMarginRadians));
+            EnsureFinitePositive(
+                velocityPositionResidualMeters,
+                nameof(velocityPositionResidualMeters));
+            EnsureFinitePositive(
+                velocityHeadingResidualRadians,
+                nameof(velocityHeadingResidualRadians));
+            EnsureFinitePositive(
+                stationaryConfirmationSeconds,
+                nameof(stationaryConfirmationSeconds));
+
+            _maximumLinearSpeedMetersPerSecond =
+                maximumLinearSpeedMetersPerSecond;
+            _maximumAngularSpeedRadiansPerSecond =
+                maximumAngularSpeedRadiansPerSecond;
+            _positionJumpMarginMeters =
+                positionJumpMarginMeters;
+            _headingJumpMarginRadians =
+                headingJumpMarginRadians;
+            _velocityPositionResidualMeters =
+                velocityPositionResidualMeters;
+            _velocityHeadingResidualRadians =
+                velocityHeadingResidualRadians;
+            _stationaryConfirmationSeconds =
+                stationaryConfirmationSeconds;
+        }
+
+        /// <summary>
+        /// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。
+        /// </summary>
+        public string LastFailureReason { get; private set; } = "";
+
+        /// <summary>
+        /// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。
+        /// </summary>
+        public bool TryGetState(out VehicleState state)
+        {
+            lock (_syncRoot)
+            {
+                try
+                {
+                    var poseInWorld =
+                        ReadDetourPoseInWorld();
+                    var timestampSeconds =
+                        _clock.Elapsed.TotalSeconds;
+
+                    if (!_hasAcceptedPose)
+                    {
+                        state = AcceptPoseAfterReset(
+                            poseInWorld,
+                            timestampSeconds);
+                        LastFailureReason = "";
+                        return true;
+                    }
+
+                    if (ArePosesEquivalent(
+                            poseInWorld,
+                            _acceptedPoseInWorld))
+                    {
+                        state = HandleRepeatedPose(
+                            timestampSeconds);
+                        LastFailureReason = "";
+                        return true;
+                    }
+
+                    // 静止保持后出现新定位时重新建立差分基准，
+                    // 避免用很长的静止时间稀释第一次运动速度。
+                    if (_stationaryHoldActive)
+                    {
+                        state = AcceptPoseAfterReset(
+                            poseInWorld,
+                            timestampSeconds);
+                        LastFailureReason = "";
+                        return true;
+                    }
+
+                    var elapsedSeconds =
+                        timestampSeconds -
+                        _acceptedTimestampSeconds;
+
+                    if (!IsMotionPlausible(
+                            _acceptedPoseInWorld,
+                            poseInWorld,
+                            elapsedSeconds))
+                    {
+                        // 单帧异常不进入差分器，也不中断调用方；下一次
+                        // 正常观测仍相对最近有效位姿和真实时间差计算。
+                        state = _latestState;
+                        LastFailureReason =
+                            "Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。";
+                        return true;
+                    }
+
+                    if (IsVelocityInnovationAbnormal(
+                            poseInWorld,
+                            elapsedSeconds))
+                    {
+                        state = AcceptPoseAfterVelocityRebase(
+                            poseInWorld,
+                            timestampSeconds);
+                        LastFailureReason =
+                            "Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
+                        return true;
+                    }
+
+                    state = AcceptContinuousPose(
+                        poseInWorld,
+                        timestampSeconds);
+                    LastFailureReason = "";
+                    return true;
+                }
+                catch (Exception exception)
+                {
+                    state = default;
+                    LastFailureReason =
+                        "Detour车辆状态读取失败：" +
+                        exception.Message;
+                    return false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// 清除Detour位姿历史和速度估计状态。
+        /// </summary>
+        public void Reset()
+        {
+            lock (_syncRoot)
+            {
+                _velocityEstimator.Reset();
+                _hasAcceptedPose = false;
+                _acceptedPoseInWorld = Pose2D.Identity;
+                _acceptedTimestampSeconds = 0.0;
+                _latestState = default;
+                _stationaryHoldActive = false;
+                LastFailureReason = "";
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。
+        /// </summary>
+        private static Pose2D ReadDetourPoseInWorld()
+        {
+            var location =
+                DetourInterface.getCartLocation();
+
+            EnsureFinite(location.x, "DetourX");
+            EnsureFinite(location.y, "DetourY");
+            EnsureFinite(location.th, "DetourTheta");
+
+            return new Pose2D(
+                location.x / MillimetersPerMeter,
+                location.y / MillimetersPerMeter,
+                AngleMath.NormalizeRadians(
+                    AngleMath.DegreesToRadians(
+                        location.th)));
+        }
+
+        /// <summary>
+        /// 接受连续有效定位并更新速度估计和差分基准。
+        /// </summary>
+        private VehicleState AcceptContinuousPose(
+            Pose2D poseInWorld,
+            double timestampSeconds)
+        {
+            _latestState =
+                _velocityEstimator.Update(
+                    poseInWorld,
+                    timestampSeconds);
+            _acceptedPoseInWorld = poseInWorld;
+            _acceptedTimestampSeconds =
+                timestampSeconds;
+            _stationaryHoldActive = false;
+            return _latestState;
+        }
+
+        /// <summary>
+        /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
+        /// </summary>
+        private VehicleState AcceptPoseAfterVelocityRebase(
+            Pose2D poseInWorld,
+            double timestampSeconds)
+        {
+            _latestState =
+                _velocityEstimator
+                    .RebasePreservingVelocity(
+                        poseInWorld,
+                        timestampSeconds);
+            _acceptedPoseInWorld = poseInWorld;
+            _acceptedTimestampSeconds =
+                timestampSeconds;
+            _stationaryHoldActive = false;
+            return _latestState;
+        }
+
+        /// <summary>
+        /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
+        /// </summary>
+        private VehicleState AcceptPoseAfterReset(
+            Pose2D poseInWorld,
+            double timestampSeconds)
+        {
+            _latestState =
+                _velocityEstimator.Reset(
+                    poseInWorld,
+                    timestampSeconds);
+            _acceptedPoseInWorld = poseInWorld;
+            _acceptedTimestampSeconds =
+                timestampSeconds;
+            _hasAcceptedPose = true;
+            _stationaryHoldActive = false;
+            return _latestState;
+        }
+
+        /// <summary>
+        /// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。
+        /// </summary>
+        private VehicleState HandleRepeatedPose(
+            double timestampSeconds)
+        {
+            var unchangedSeconds =
+                timestampSeconds -
+                _acceptedTimestampSeconds;
+
+            if (!_stationaryHoldActive &&
+                unchangedSeconds >=
+                _stationaryConfirmationSeconds)
+            {
+                _latestState =
+                    new VehicleState(
+                        timestampSeconds,
+                        _acceptedPoseInWorld,
+                        Twist2D.Zero,
+                        true);
+                _stationaryHoldActive = true;
+            }
+
+            return _latestState;
+        }
+
+        /// <summary>
+        /// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。
+        /// </summary>
+        private bool IsMotionPlausible(
+            Pose2D startPoseInWorld,
+            Pose2D endPoseInWorld,
+            double deltaTimeSeconds)
+        {
+            if (!IsFinite(deltaTimeSeconds) ||
+                deltaTimeSeconds <= 0.0)
+            {
+                return false;
+            }
+
+            var deltaX =
+                endPoseInWorld.XMeters -
+                startPoseInWorld.XMeters;
+            var deltaY =
+                endPoseInWorld.YMeters -
+                startPoseInWorld.YMeters;
+            var displacementMeters =
+                Math.Sqrt(
+                    deltaX * deltaX +
+                    deltaY * deltaY);
+            var headingChangeRadians =
+                Math.Abs(
+                    AngleMath.ShortestDifferenceRadians(
+                        endPoseInWorld.YawRadians,
+                        startPoseInWorld.YawRadians));
+
+            var maximumDisplacementMeters =
+                _maximumLinearSpeedMetersPerSecond *
+                deltaTimeSeconds +
+                _positionJumpMarginMeters;
+            var maximumHeadingChangeRadians =
+                _maximumAngularSpeedRadiansPerSecond *
+                deltaTimeSeconds +
+                _headingJumpMarginRadians;
+
+            return displacementMeters <=
+                       maximumDisplacementMeters &&
+                   headingChangeRadians <=
+                       maximumHeadingChangeRadians;
+        }
+
+        /// <summary>
+        /// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。
+        /// </summary>
+        private bool IsVelocityInnovationAbnormal(
+            Pose2D poseInWorld,
+            double deltaTimeSeconds)
+        {
+            if (!_latestState.HasValidVelocityEstimate)
+            {
+                return false;
+            }
+
+            var predictedX =
+                _acceptedPoseInWorld.XMeters +
+                _latestState.TwistInWorld
+                    .VxMetersPerSecond *
+                deltaTimeSeconds;
+            var predictedY =
+                _acceptedPoseInWorld.YMeters +
+                _latestState.TwistInWorld
+                    .VyMetersPerSecond *
+                deltaTimeSeconds;
+            var predictedYaw =
+                AngleMath.NormalizeRadians(
+                    _acceptedPoseInWorld.YawRadians +
+                    _latestState.TwistInWorld
+                        .OmegaRadiansPerSecond *
+                    deltaTimeSeconds);
+
+            var positionResidualX =
+                poseInWorld.XMeters - predictedX;
+            var positionResidualY =
+                poseInWorld.YMeters - predictedY;
+            var positionResidualMeters =
+                Math.Sqrt(
+                    positionResidualX * positionResidualX +
+                    positionResidualY * positionResidualY);
+            var headingResidualRadians =
+                Math.Abs(
+                    AngleMath.ShortestDifferenceRadians(
+                        poseInWorld.YawRadians,
+                        predictedYaw));
+
+            return positionResidualMeters >
+                       _velocityPositionResidualMeters ||
+                   headingResidualRadians >
+                       _velocityHeadingResidualRadians;
+        }
+
+        /// <summary>
+        /// 判断两次读取是否为Detour保持输出的同一数值帧。
+        /// </summary>
+        private static bool ArePosesEquivalent(
+            Pose2D firstPose,
+            Pose2D secondPose)
+        {
+            return Math.Abs(
+                       firstPose.XMeters -
+                       secondPose.XMeters) <=
+                   PositionEqualityToleranceMeters &&
+                   Math.Abs(
+                       firstPose.YMeters -
+                       secondPose.YMeters) <=
+                   PositionEqualityToleranceMeters &&
+                   Math.Abs(
+                       AngleMath.ShortestDifferenceRadians(
+                           firstPose.YawRadians,
+                           secondPose.YawRadians)) <=
+                   HeadingEqualityToleranceRadians;
+        }
+
+        /// <summary>
+        /// 检查数值是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "状态源参数必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "状态源参数必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (!IsFinite(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "状态源参数和Detour位姿必须是有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 判断数值是否可用于状态估计。
+        /// </summary>
+        private static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+    }
+}

MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs

@@ -0,0 +1,142 @@
+using System;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。
+    /// </summary>
+    public sealed class FirstOrderLowPassFilter
+    {
+        private readonly double _timeConstantSeconds;
+        private bool _isInitialized;
+        private double _value;
+
+        /// <summary>
+        /// 创建使用指定时间常数的一阶低通滤波器。
+        /// </summary>
+        public FirstOrderLowPassFilter(
+            double timeConstantSeconds)
+        {
+            EnsureFinitePositive(
+                timeConstantSeconds,
+                nameof(timeConstantSeconds));
+
+            _timeConstantSeconds =
+                timeConstantSeconds;
+        }
+
+        /// <summary>
+        /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
+        /// </summary>
+        public double TimeConstantSeconds =>
+            _timeConstantSeconds;
+
+        /// <summary>
+        /// 获取滤波器是否已经接收过有效初值。
+        /// </summary>
+        public bool IsInitialized =>
+            _isInitialized;
+
+        /// <summary>
+        /// 获取当前滤波输出；尚未初始化时读取会抛出异常。
+        /// </summary>
+        public double Value
+        {
+            get
+            {
+                if (!_isInitialized)
+                {
+                    throw new InvalidOperationException(
+                        "一阶低通滤波器尚未初始化。");
+                }
+
+                return _value;
+            }
+        }
+
+        /// <summary>
+        /// 使用当前输入和真实采样间隔更新滤波结果。
+        /// </summary>
+        public double Update(
+            double input,
+            double deltaTimeSeconds)
+        {
+            EnsureFinite(
+                input,
+                nameof(input));
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (!_isInitialized)
+            {
+                _value = input;
+                _isInitialized = true;
+                return _value;
+            }
+
+            var alpha =
+                deltaTimeSeconds /
+                (_timeConstantSeconds +
+                 deltaTimeSeconds);
+
+            _value += alpha * (input - _value);
+            return _value;
+        }
+
+        /// <summary>
+        /// 清除历史输出，使下一次有效输入直接成为新的初值。
+        /// </summary>
+        public void Reset()
+        {
+            _value = 0.0;
+            _isInitialized = false;
+        }
+
+        /// <summary>
+        /// 将滤波器立即重置到指定的有限初值。
+        /// </summary>
+        public void Reset(double initialValue)
+        {
+            EnsureFinite(
+                initialValue,
+                nameof(initialValue));
+
+            _value = initialValue;
+            _isInitialized = true;
+        }
+
+        /// <summary>
+        /// 检查数值是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "滤波时间常数和采样间隔必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "滤波输入必须是有限值。");
+            }
+        }
+    }
+}

MultiWheelC/StateEstimation/IVehicleStateProvider.cs

@@ -0,0 +1,14 @@
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 为轨迹控制器提供与具体定位来源无关的统一车辆状态读取接口。
+    /// </summary>
+    public interface IVehicleStateProvider
+    {
+        /// <summary>
+        /// 尝试读取当前有效车辆状态；定位不可用或过期时返回false。
+        /// </summary>
+        bool TryGetState(out VehicleState state);
+    }
+}
+

MultiWheelC/StateEstimation/VehicleState.cs

@@ -0,0 +1,138 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
+    /// </summary>
+    public readonly struct VehicleState
+    {
+        /// <summary>
+        /// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
+        /// </summary>
+        public VehicleState(
+            double sampleTimestampSeconds,
+            Pose2D poseInWorld,
+            Twist2D twistInWorld,
+            bool hasValidVelocityEstimate)
+        {
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteTwist(
+                twistInWorld,
+                nameof(twistInWorld));
+
+            SampleTimestampSeconds =
+                sampleTimestampSeconds;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+
+            // 第一帧或定位重置后的速度不可用于闭环控制，
+            // 此时显式置零，避免调用方误用残留速度。
+            TwistInWorld = hasValidVelocityEstimate
+                ? twistInWorld
+                : Twist2D.Zero;
+
+            var worldPoseInBody =
+                FrameTransform2D.Inverse(
+                    PoseInWorld);
+            TwistInBody =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    worldPoseInBody,
+                    TwistInWorld);
+        }
+
+        /// <summary>
+        /// 获取状态源单调时钟中的采样时刻，单位为s。
+        /// </summary>
+        public double SampleTimestampSeconds { get; }
+
+        /// <summary>
+        /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
+        /// </summary>
+        public Pose2D PoseInWorld { get; }
+
+        /// <summary>
+        /// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
+        /// </summary>
+        public Twist2D TwistInWorld { get; }
+
+        /// <summary>
+        /// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
+        /// </summary>
+        public Twist2D TwistInBody { get; }
+
+        /// <summary>
+        /// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
+        /// </summary>
+        public bool HasValidVelocityEstimate { get; }
+
+        /// <summary>
+        /// 检查位姿是否由有限数值组成。
+        /// </summary>
+        private static void EnsureFinitePose(
+            Pose2D pose,
+            string parameterName)
+        {
+            if (!IsFinite(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "车辆位姿必须由有限数值组成。");
+            }
+        }
+
+        /// <summary>
+        /// 检查速度是否由有限数值组成。
+        /// </summary>
+        private static void EnsureFiniteTwist(
+            Twist2D twist,
+            string parameterName)
+        {
+            if (!IsFinite(twist.VxMetersPerSecond) ||
+                !IsFinite(twist.VyMetersPerSecond) ||
+                !IsFinite(twist.OmegaRadiansPerSecond))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "车辆速度必须由有限数值组成。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (!IsFinite(value) || value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "采样时刻必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 判断数值是否可用于车辆状态计算。
+        /// </summary>
+        private static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+    }
+}

MultiWheelC/StateEstimation/VelocityEstimator2D.cs

@@ -0,0 +1,275 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。
+    /// </summary>
+    public sealed class VelocityEstimator2D
+    {
+        public const double DefaultLinearFilterTimeConstantSeconds =
+            0.15;
+        public const double DefaultAngularFilterTimeConstantSeconds =
+            0.20;
+
+        private readonly FirstOrderLowPassFilter
+            _worldVelocityXFilter;
+        private readonly FirstOrderLowPassFilter
+            _worldVelocityYFilter;
+        private readonly FirstOrderLowPassFilter
+            _angularVelocityFilter;
+
+        private bool _hasPreviousSample;
+        private Pose2D _previousPoseInWorld;
+        private double _previousTimestampSeconds;
+
+        /// <summary>
+        /// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。
+        /// </summary>
+        public VelocityEstimator2D()
+            : this(
+                DefaultLinearFilterTimeConstantSeconds,
+                DefaultAngularFilterTimeConstantSeconds)
+        {
+        }
+
+        /// <summary>
+        /// 创建使用指定线速度和角速度滤波时间常数的估计器。
+        /// </summary>
+        public VelocityEstimator2D(
+            double linearFilterTimeConstantSeconds,
+            double angularFilterTimeConstantSeconds)
+        {
+            _worldVelocityXFilter =
+                new FirstOrderLowPassFilter(
+                    linearFilterTimeConstantSeconds);
+            _worldVelocityYFilter =
+                new FirstOrderLowPassFilter(
+                    linearFilterTimeConstantSeconds);
+            _angularVelocityFilter =
+                new FirstOrderLowPassFilter(
+                    angularFilterTimeConstantSeconds);
+        }
+
+        /// <summary>
+        /// 获取是否已经保存了可用于下一次差分的位姿基准。
+        /// </summary>
+        public bool HasPreviousSample =>
+            _hasPreviousSample;
+
+        /// <summary>
+        /// 使用一个新的有效定位样本更新并返回车辆状态。
+        /// </summary>
+        public VehicleState Update(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            var normalizedPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+
+            if (!_hasPreviousSample)
+            {
+                return Reset(
+                    normalizedPoseInWorld,
+                    sampleTimestampSeconds);
+            }
+
+            var deltaTimeSeconds =
+                sampleTimestampSeconds -
+                _previousTimestampSeconds;
+
+            if (deltaTimeSeconds <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(sampleTimestampSeconds),
+                    "新定位样本的单调时间戳必须严格大于上一帧。");
+            }
+
+            var rawVelocityXInWorld =
+                (normalizedPoseInWorld.XMeters -
+                 _previousPoseInWorld.XMeters) /
+                deltaTimeSeconds;
+            var rawVelocityYInWorld =
+                (normalizedPoseInWorld.YMeters -
+                 _previousPoseInWorld.YMeters) /
+                deltaTimeSeconds;
+            var rawAngularVelocity =
+                AngleMath.ShortestDifferenceRadians(
+                    normalizedPoseInWorld.YawRadians,
+                    _previousPoseInWorld.YawRadians) /
+                deltaTimeSeconds;
+
+            var filteredTwistInWorld =
+                new Twist2D(
+                    _worldVelocityXFilter.Update(
+                        rawVelocityXInWorld,
+                        deltaTimeSeconds),
+                    _worldVelocityYFilter.Update(
+                        rawVelocityYInWorld,
+                        deltaTimeSeconds),
+                    _angularVelocityFilter.Update(
+                        rawAngularVelocity,
+                        deltaTimeSeconds));
+
+            _previousPoseInWorld =
+                normalizedPoseInWorld;
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                normalizedPoseInWorld,
+                filteredTwistInWorld,
+                true);
+        }
+
+        /// <summary>
+        /// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
+        /// </summary>
+        public VehicleState RebasePreservingVelocity(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            var normalizedPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+
+            _previousPoseInWorld =
+                normalizedPoseInWorld;
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+            _hasPreviousSample = true;
+
+            var hasValidVelocityEstimate =
+                _worldVelocityXFilter.IsInitialized &&
+                _worldVelocityYFilter.IsInitialized &&
+                _angularVelocityFilter.IsInitialized;
+
+            var retainedTwistInWorld =
+                hasValidVelocityEstimate
+                    ? new Twist2D(
+                        _worldVelocityXFilter.Value,
+                        _worldVelocityYFilter.Value,
+                        _angularVelocityFilter.Value)
+                    : Twist2D.Zero;
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                normalizedPoseInWorld,
+                retainedTwistInWorld,
+                hasValidVelocityEstimate);
+        }
+
+        /// <summary>
+        /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
+        /// </summary>
+        public VehicleState Reset(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            _previousPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+            _hasPreviousSample = true;
+
+            _worldVelocityXFilter.Reset();
+            _worldVelocityYFilter.Reset();
+            _angularVelocityFilter.Reset();
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                _previousPoseInWorld,
+                Twist2D.Zero,
+                false);
+        }
+
+        /// <summary>
+        /// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。
+        /// </summary>
+        public void Reset()
+        {
+            _hasPreviousSample = false;
+            _previousPoseInWorld = Pose2D.Identity;
+            _previousTimestampSeconds = 0.0;
+
+            _worldVelocityXFilter.Reset();
+            _worldVelocityYFilter.Reset();
+            _angularVelocityFilter.Reset();
+        }
+
+        /// <summary>
+        /// 检查位姿是否由有限数值组成。
+        /// </summary>
+        private static void EnsureFinitePose(
+            Pose2D pose,
+            string parameterName)
+        {
+            if (!IsFinite(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "速度估计使用的车辆位姿必须由有限数值组成。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (!IsFinite(value) || value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "速度估计使用的采样时刻必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 判断数值是否可用于速度估计。
+        /// </summary>
+        private static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+    }
+}

MultiWheelC/Trajectory/LegacyTrackAdapter.cs

@@ -0,0 +1 @@
+// 兼容现有 MDCS 的 AbstractTrack

MultiWheelC/Trajectory/Trajectory2D.cs

@@ -0,0 +1,172 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Linq;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 保存一条经过基本合法性检查的只读二维参考轨迹。
+    /// </summary>
+    public sealed class Trajectory2D
+    {
+        private const double StartArcLengthToleranceMeters = 1e-9;
+        private const double MinimumSegmentLengthMeters = 1e-6;
+
+        private readonly TrajectoryPoint[] _points;
+        private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
+
+        /// <summary>
+        /// 复制并验证按累计弧长升序排列的参考轨迹点。
+        /// </summary>
+        public Trajectory2D(
+            IEnumerable<TrajectoryPoint> points)
+        {
+            if (points == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(points));
+            }
+
+            _points = points.ToArray();
+
+            if (_points.Length < 2)
+            {
+                throw new ArgumentException(
+                    "二维轨迹至少需要两个轨迹点。",
+                    nameof(points));
+            }
+
+            if (Math.Abs(_points[0].ArcLengthMeters) >
+                StartArcLengthToleranceMeters)
+            {
+                throw new ArgumentException(
+                    "二维轨迹起点的累计弧长必须为0m。",
+                    nameof(points));
+            }
+
+            for (var index = 1;
+                 index < _points.Length;
+                 index++)
+            {
+                ValidateSegment(
+                    _points[index - 1],
+                    _points[index],
+                    index,
+                    nameof(points));
+            }
+
+            _readOnlyPoints =
+                Array.AsReadOnly(_points);
+        }
+
+        /// <summary>
+        /// 获取轨迹点数量。
+        /// </summary>
+        public int Count => _points.Length;
+
+        /// <summary>
+        /// 获取指定索引处的轨迹点。
+        /// </summary>
+        public TrajectoryPoint this[int index] =>
+            _points[index];
+
+        /// <summary>
+        /// 获取不可修改的有序轨迹点集合。
+        /// </summary>
+        public IReadOnlyList<TrajectoryPoint> Points =>
+            _readOnlyPoints;
+
+        /// <summary>
+        /// 获取轨迹起点。
+        /// </summary>
+        public TrajectoryPoint StartPoint =>
+            _points[0];
+
+        /// <summary>
+        /// 获取轨迹终点。
+        /// </summary>
+        public TrajectoryPoint EndPoint =>
+            _points[_points.Length - 1];
+
+        /// <summary>
+        /// 获取轨迹总弧长，单位为m。
+        /// </summary>
+        public double TotalLengthMeters =>
+            EndPoint.ArcLengthMeters;
+
+        /// <summary>
+        /// 根据当前累计弧长计算到轨迹终点的剩余距离。
+        /// </summary>
+        public double GetRemainingDistanceMeters(
+            double arcLengthMeters)
+        {
+            EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+                return TotalLengthMeters;
+
+            if (arcLengthMeters >= TotalLengthMeters)
+                return 0.0;
+
+            return TotalLengthMeters - arcLengthMeters;
+        }
+
+        /// <summary>
+        /// 检查相邻轨迹点是否构成有效的非零长度有序线段。
+        /// </summary>
+        private static void ValidateSegment(
+            TrajectoryPoint previous,
+            TrajectoryPoint current,
+            int currentIndex,
+            string parameterName)
+        {
+            if (current.ArcLengthMeters <=
+                previous.ArcLengthMeters)
+            {
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
+                    parameterName);
+            }
+
+            var deltaX =
+                current.PoseInWorld.XMeters -
+                previous.PoseInWorld.XMeters;
+            var deltaY =
+                current.PoseInWorld.YMeters -
+                previous.PoseInWorld.YMeters;
+            var segmentLengthSquared =
+                deltaX * deltaX +
+                deltaY * deltaY;
+            var minimumLengthSquared =
+                MinimumSegmentLengthMeters *
+                MinimumSegmentLengthMeters;
+
+            if (segmentLengthSquared <
+                minimumLengthSquared)
+            {
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
+                    parameterName);
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹弧长必须是有限值。");
+            }
+        }
+    }
+}

MultiWheelC/Trajectory/TrajectoryPoint.cs

@@ -0,0 +1,102 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
+    /// </summary>
+    public readonly struct TrajectoryPoint
+    {
+        /// <summary>
+        /// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
+        /// </summary>
+        public TrajectoryPoint(
+            double arcLengthMeters,
+            Pose2D poseInWorld,
+            double curvaturePerMeter,
+            double referenceSpeedMetersPerSecond)
+        {
+            EnsureFiniteNonNegative(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+            EnsureFinite(
+                poseInWorld.XMeters,
+                nameof(poseInWorld));
+            EnsureFinite(
+                poseInWorld.YMeters,
+                nameof(poseInWorld));
+            EnsureFinite(
+                poseInWorld.YawRadians,
+                nameof(poseInWorld));
+            EnsureFinite(
+                curvaturePerMeter,
+                nameof(curvaturePerMeter));
+            EnsureFinite(
+                referenceSpeedMetersPerSecond,
+                nameof(referenceSpeedMetersPerSecond));
+            ArcLengthMeters = arcLengthMeters;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+            CurvaturePerMeter = curvaturePerMeter;
+            ReferenceSpeedMetersPerSecond =
+                referenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 获取从轨迹起点累计到当前点的弧长，单位为m。
+        /// </summary>
+        public double ArcLengthMeters { get; }
+
+        /// <summary>
+        /// 获取车体中心参考坐标系在世界坐标系中的位姿。
+        /// </summary>
+        public Pose2D PoseInWorld { get; }
+
+        /// <summary>
+        /// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
+        /// </summary>
+        public double CurvaturePerMeter { get; }
+
+        /// <summary>
+        /// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
+        /// </summary>
+        public double ReferenceSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹点参数必须是有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹累计弧长不能为负数。");
+            }
+        }
+    }
+}

MultiWheelC/Trajectory/TrajectoryProjection.cs

@@ -0,0 +1,123 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 保存车体中心投影到二维参考轨迹后得到的只读结果。
+    /// </summary>
+    public readonly struct TrajectoryProjection
+    {
+        /// <summary>
+        /// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
+        /// </summary>
+        public TrajectoryProjection(
+            int segmentStartIndex,
+            TrajectoryPoint referencePoint,
+            double lateralErrorMeters,
+            double headingErrorRadians,
+            double distanceToTrajectoryMeters,
+            double remainingDistanceMeters)
+        {
+            if (segmentStartIndex < 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(segmentStartIndex),
+                    "投影线段起点索引不能为负数。");
+            }
+
+            EnsureFinite(
+                lateralErrorMeters,
+                nameof(lateralErrorMeters));
+            EnsureFinite(
+                headingErrorRadians,
+                nameof(headingErrorRadians));
+            EnsureFiniteNonNegative(
+                distanceToTrajectoryMeters,
+                nameof(distanceToTrajectoryMeters));
+            EnsureFiniteNonNegative(
+                remainingDistanceMeters,
+                nameof(remainingDistanceMeters));
+
+            SegmentStartIndex = segmentStartIndex;
+            ReferencePoint = referencePoint;
+            LateralErrorMeters = lateralErrorMeters;
+            HeadingErrorRadians =
+                AngleMath.NormalizeRadians(
+                    headingErrorRadians);
+            DistanceToTrajectoryMeters =
+                distanceToTrajectoryMeters;
+            RemainingDistanceMeters =
+                remainingDistanceMeters;
+        }
+
+        /// <summary>
+        /// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
+        /// </summary>
+        public int SegmentStartIndex { get; }
+
+        /// <summary>
+        /// 获取投影位置插值得到的车体中心参考轨迹点。
+        /// </summary>
+        public TrajectoryPoint ReferencePoint { get; }
+
+        /// <summary>
+        /// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
+        /// </summary>
+        public double LateralErrorMeters { get; }
+
+        /// <summary>
+        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
+        /// </summary>
+        public double HeadingErrorRadians { get; }
+
+        /// <summary>
+        /// 获取车体中心到投影点的欧氏距离，单位为m。
+        /// </summary>
+        public double DistanceToTrajectoryMeters { get; }
+
+        /// <summary>
+        /// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
+        /// </summary>
+        public double RemainingDistanceMeters { get; }
+
+        /// <summary>
+        /// 获取投影位置从轨迹起点累计的弧长，单位为m。
+        /// </summary>
+        public double ArcLengthMeters =>
+            ReferencePoint.ArcLengthMeters;
+
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹投影参数必须是有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹投影距离不能为负数。");
+            }
+        }
+    }
+}

MultiWheelC/Trajectory/TrajectoryProjector.cs

@@ -0,0 +1,219 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
+    /// </summary>
+    public static class TrajectoryProjector
+    {
+        /// <summary>
+        /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
+        /// </summary>
+        public static TrajectoryProjection Project(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld)
+        {
+            if (trajectory == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(trajectory));
+            }
+
+            EnsureFinitePose(
+                vehiclePoseInWorld,
+                nameof(vehiclePoseInWorld));
+
+            var bestSegmentStartIndex = 0;
+            var bestInterpolationRatio = 0.0;
+            var bestProjectedX = 0.0;
+            var bestProjectedY = 0.0;
+            var bestDistanceSquared =
+                double.PositiveInfinity;
+
+            for (var segmentStartIndex = 0;
+                 segmentStartIndex < trajectory.Count - 1;
+                 segmentStartIndex++)
+            {
+                var segmentStart =
+                    trajectory[segmentStartIndex];
+                var segmentEnd =
+                    trajectory[segmentStartIndex + 1];
+
+                var segmentX =
+                    segmentEnd.PoseInWorld.XMeters -
+                    segmentStart.PoseInWorld.XMeters;
+                var segmentY =
+                    segmentEnd.PoseInWorld.YMeters -
+                    segmentStart.PoseInWorld.YMeters;
+                var segmentLengthSquared =
+                    segmentX * segmentX +
+                    segmentY * segmentY;
+
+                var vehicleFromSegmentStartX =
+                    vehiclePoseInWorld.XMeters -
+                    segmentStart.PoseInWorld.XMeters;
+                var vehicleFromSegmentStartY =
+                    vehiclePoseInWorld.YMeters -
+                    segmentStart.PoseInWorld.YMeters;
+
+                var interpolationRatio =
+                    InterpolationMath.Clamp01(
+                        (vehicleFromSegmentStartX * segmentX +
+                         vehicleFromSegmentStartY * segmentY) /
+                        segmentLengthSquared);
+
+                var projectedX =
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.XMeters,
+                        segmentEnd.PoseInWorld.XMeters,
+                        interpolationRatio);
+                var projectedY =
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.YMeters,
+                        segmentEnd.PoseInWorld.YMeters,
+                        interpolationRatio);
+
+                var projectionErrorX =
+                    projectedX -
+                    vehiclePoseInWorld.XMeters;
+                var projectionErrorY =
+                    projectedY -
+                    vehiclePoseInWorld.YMeters;
+                var distanceSquared =
+                    projectionErrorX * projectionErrorX +
+                    projectionErrorY * projectionErrorY;
+
+                if (distanceSquared >= bestDistanceSquared)
+                {
+                    continue;
+                }
+
+                bestSegmentStartIndex =
+                    segmentStartIndex;
+                bestInterpolationRatio =
+                    interpolationRatio;
+                bestProjectedX = projectedX;
+                bestProjectedY = projectedY;
+                bestDistanceSquared = distanceSquared;
+            }
+
+            return BuildProjection(
+                trajectory,
+                vehiclePoseInWorld,
+                bestSegmentStartIndex,
+                bestInterpolationRatio,
+                bestProjectedX,
+                bestProjectedY,
+                bestDistanceSquared);
+        }
+
+        /// <summary>
+        /// 根据最近线段和插值比例生成控制器使用的完整投影结果。
+        /// </summary>
+        private static TrajectoryProjection BuildProjection(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld,
+            int segmentStartIndex,
+            double interpolationRatio,
+            double projectedX,
+            double projectedY,
+            double distanceSquared)
+        {
+            var segmentStart =
+                trajectory[segmentStartIndex];
+            var segmentEnd =
+                trajectory[segmentStartIndex + 1];
+
+            var referenceYawRadians =
+                AngleMath.LerpRadians(
+                    segmentStart.PoseInWorld.YawRadians,
+                    segmentEnd.PoseInWorld.YawRadians,
+                    interpolationRatio);
+            var referenceArcLengthMeters =
+                InterpolationMath.Lerp(
+                    segmentStart.ArcLengthMeters,
+                    segmentEnd.ArcLengthMeters,
+                    interpolationRatio);
+            var referenceCurvaturePerMeter =
+                InterpolationMath.Lerp(
+                    segmentStart.CurvaturePerMeter,
+                    segmentEnd.CurvaturePerMeter,
+                    interpolationRatio);
+            var referenceSpeedMetersPerSecond =
+                InterpolationMath.Lerp(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    interpolationRatio);
+
+            var referencePoint =
+                new TrajectoryPoint(
+                    referenceArcLengthMeters,
+                    new Pose2D(
+                        projectedX,
+                        projectedY,
+                        referenceYawRadians),
+                    referenceCurvaturePerMeter,
+                    referenceSpeedMetersPerSecond);
+
+            var segmentX =
+                segmentEnd.PoseInWorld.XMeters -
+                segmentStart.PoseInWorld.XMeters;
+            var segmentY =
+                segmentEnd.PoseInWorld.YMeters -
+                segmentStart.PoseInWorld.YMeters;
+            var segmentLength =
+                Math.Sqrt(
+                    segmentX * segmentX +
+                    segmentY * segmentY);
+
+            // 以轨迹线段的前进方向判断左右：
+            // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
+            var vehicleToProjectionX =
+                projectedX -
+                vehiclePoseInWorld.XMeters;
+            var vehicleToProjectionY =
+                projectedY -
+                vehiclePoseInWorld.YMeters;
+            var lateralErrorMeters =
+                (segmentX * vehicleToProjectionY -
+                 segmentY * vehicleToProjectionX) /
+                segmentLength;
+
+            var headingErrorRadians =
+                AngleMath.ShortestDifferenceRadians(
+                    referenceYawRadians,
+                    vehiclePoseInWorld.YawRadians);
+
+            return new TrajectoryProjection(
+                segmentStartIndex,
+                referencePoint,
+                lateralErrorMeters,
+                headingErrorRadians,
+                Math.Sqrt(distanceSquared),
+                trajectory.GetRemainingDistanceMeters(
+                    referenceArcLengthMeters));
+        }
+
+        /// <summary>
+        /// 检查用于投影的实际车体中心位姿是否包含有限数值。
+        /// </summary>
+        private static void EnsureFinitePose(
+            Pose2D pose,
+            string parameterName)
+        {
+            if (double.IsNaN(pose.XMeters) ||
+                double.IsInfinity(pose.XMeters) ||
+                double.IsNaN(pose.YMeters) ||
+                double.IsInfinity(pose.YMeters) ||
+                double.IsNaN(pose.YawRadians) ||
+                double.IsInfinity(pose.YawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "用于轨迹投影的车体位姿必须是有限值。");
+            }
+        }
+    }
+}

Shared/Mathematics/AngleMath.cs

@@ -81,6 +81,29 @@ namespace MyParking.Shared
             return NormalizeDegrees(targetDegrees - currentDegrees);
         }
 
+        /// <summary>
+        /// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。
+        /// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio，
+        /// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。
+        /// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。
+        /// </summary>
+        public static double LerpRadians(
+            double startRadians,
+            double endRadians,
+            double ratio)
+        {
+            EnsureFinite(startRadians, nameof(startRadians));
+            EnsureFinite(endRadians, nameof(endRadians));
+            EnsureFinite(ratio, nameof(ratio));
+
+            var shortestDifference = ShortestDifferenceRadians(
+                endRadians,
+                startRadians);
+
+            return NormalizeRadians(
+                startRadians + ratio * shortestDifference);
+        }
+
         /// <summary>
         /// 将角度从度转换为弧度，不进行归一化。
         /// </summary>

Shared/Mathematics/InterpolationMath.cs

@@ -0,0 +1,61 @@
+
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供与具体业务和坐标系无关的基础插值功能。
+    /// </summary>
+    public static class InterpolationMath
+    {
+        /// <summary>
+        /// 将插值比例限制到[0, 1]闭区间。
+        /// </summary>
+        public static double Clamp01(double value)
+        {
+            EnsureFinite(value, nameof(value));
+
+            if (value <= 0.0)
+            {
+                return 0.0;
+            }
+
+            if (value >= 1.0)
+            {
+                return 1.0;
+            }
+
+            return value;
+        }
+
+        /// <summary>
+        /// 对两个标量执行线性插值。
+        /// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio，
+        /// 因此也支持区间外的线性外插。
+        /// </summary>
+        public static double Lerp(
+            double start,
+            double end,
+            double ratio)
+        {
+            EnsureFinite(start, nameof(start));
+            EnsureFinite(end, nameof(end));
+            EnsureFinite(ratio, nameof(ratio));
+
+            return start + ratio * (end - start);
+        }
+
+        /// <summary>
+        /// 验证输入是可用于插值计算的有限数值。
+        /// </summary>
+        private static void EnsureFinite(double value, string parameterName)
+        {
+            if (double.IsNaN(value) || double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "插值参数必须是有限数值。");
+            }
+        }
+    }
+}

data_process/轨迹测试处理/compare_state_estimation.py

@@ -0,0 +1,760 @@
+"""对比原始Detour差分与C#在线车辆状态估计结果。"""
+
+from __future__ import annotations
+
+import argparse
+from collections import Counter
+from dataclasses import dataclass
+from pathlib import Path
+
+import matplotlib
+
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+
+REQUIRED_COLUMNS = {
+    "ElapsedSeconds",
+    "DetourX",
+    "DetourY",
+    "DetourTheta",
+}
+
+
+def wrap_radians(angle: float | np.ndarray) -> float | np.ndarray:
+    """将弧度归一化到[-π, π)区间。"""
+    return (angle + np.pi) % (2.0 * np.pi) - np.pi
+
+
+def angle_difference(target: float, current: float) -> float:
+    """计算从当前角到目标角的最短有符号弧度差。"""
+    return float(wrap_radians(target - current))
+
+
+@dataclass(frozen=True)
+class Pose:
+    """保存世界坐标系中的二维位姿，单位为m和rad。"""
+
+    x: float
+    y: float
+    yaw: float
+
+
+@dataclass(frozen=True)
+class State:
+    """保存脚本复现得到的世界位姿和世界速度。"""
+
+    timestamp: float
+    pose: Pose
+    vx: float
+    vy: float
+    omega: float
+    velocity_valid: bool
+
+
+class LowPassFilter:
+    """复现FirstOrderLowPassFilter的一阶低通计算。"""
+
+    def __init__(self, time_constant_seconds: float) -> None:
+        if not np.isfinite(time_constant_seconds) or time_constant_seconds <= 0:
+            raise ValueError("滤波时间常数必须是正有限值。")
+        self.time_constant_seconds = float(time_constant_seconds)
+        self.initialized = False
+        self.value = 0.0
+
+    def update(self, value: float, delta_time_seconds: float) -> float:
+        """按照真实采样间隔更新滤波输出。"""
+        if not self.initialized:
+            self.value = float(value)
+            self.initialized = True
+            return self.value
+        alpha = delta_time_seconds / (
+            self.time_constant_seconds + delta_time_seconds
+        )
+        self.value += alpha * (float(value) - self.value)
+        return self.value
+
+    def reset(self) -> None:
+        """清除滤波历史。"""
+        self.initialized = False
+        self.value = 0.0
+
+
+class VelocityEstimator:
+    """复现VelocityEstimator2D的世界速度差分与低通处理。"""
+
+    def __init__(self, linear_tau: float, angular_tau: float) -> None:
+        self.vx_filter = LowPassFilter(linear_tau)
+        self.vy_filter = LowPassFilter(linear_tau)
+        self.omega_filter = LowPassFilter(angular_tau)
+        self.previous_pose: Pose | None = None
+        self.previous_timestamp = 0.0
+
+    def reset(self, pose: Pose | None = None, timestamp: float = 0.0) -> State | None:
+        """清除历史，并可使用当前位姿建立新的零速差分基准。"""
+        self.vx_filter.reset()
+        self.vy_filter.reset()
+        self.omega_filter.reset()
+        self.previous_pose = pose
+        self.previous_timestamp = float(timestamp)
+        if pose is None:
+            return None
+        return State(timestamp, pose, 0.0, 0.0, 0.0, False)
+
+    def update(self, pose: Pose, timestamp: float) -> State:
+        """使用一个新的有效位姿更新速度估计。"""
+        if self.previous_pose is None:
+            state = self.reset(pose, timestamp)
+            assert state is not None
+            return state
+        delta_time = timestamp - self.previous_timestamp
+        if delta_time <= 0.0:
+            raise ValueError("新样本时间戳必须严格递增。")
+        raw_vx = (pose.x - self.previous_pose.x) / delta_time
+        raw_vy = (pose.y - self.previous_pose.y) / delta_time
+        raw_omega = angle_difference(
+            pose.yaw,
+            self.previous_pose.yaw,
+        ) / delta_time
+        state = State(
+            timestamp,
+            pose,
+            self.vx_filter.update(raw_vx, delta_time),
+            self.vy_filter.update(raw_vy, delta_time),
+            self.omega_filter.update(raw_omega, delta_time),
+            True,
+        )
+        self.previous_pose = pose
+        self.previous_timestamp = timestamp
+        return state
+
+    def rebase_preserving_velocity(
+        self,
+        pose: Pose,
+        timestamp: float,
+    ) -> State:
+        """更新差分基准但保留三个低通滤波器的当前输出。"""
+        self.previous_pose = pose
+        self.previous_timestamp = timestamp
+        velocity_valid = (
+            self.vx_filter.initialized
+            and self.vy_filter.initialized
+            and self.omega_filter.initialized
+        )
+        return State(
+            timestamp,
+            pose,
+            self.vx_filter.value if velocity_valid else 0.0,
+            self.vy_filter.value if velocity_valid else 0.0,
+            self.omega_filter.value if velocity_valid else 0.0,
+            velocity_valid,
+        )
+
+
+class DetourProviderSimulator:
+    """按当前简化版DetourVehicleStateProvider处理离线CSV样本。"""
+
+    def __init__(
+        self,
+        linear_tau: float = 0.15,
+        angular_tau: float = 0.20,
+        maximum_linear_speed: float = 1.20,
+        maximum_angular_speed: float = np.pi / 4.0,
+        position_jump_margin: float = 0.03,
+        heading_jump_margin: float = np.deg2rad(5.0),
+        stationary_seconds: float = 0.35,
+    ) -> None:
+        self.estimator = VelocityEstimator(linear_tau, angular_tau)
+        self.maximum_linear_speed = maximum_linear_speed
+        self.maximum_angular_speed = maximum_angular_speed
+        self.position_jump_margin = position_jump_margin
+        self.heading_jump_margin = heading_jump_margin
+        self.stationary_seconds = stationary_seconds
+
+        self.accepted_pose: Pose | None = None
+        self.accepted_timestamp = 0.0
+        self.latest_state: State | None = None
+        self.stationary_hold = False
+
+    @staticmethod
+    def poses_equal(first: Pose, second: Pose) -> bool:
+        """判断两次读取是否为Detour保持输出的同一数值帧。"""
+        return (
+            abs(first.x - second.x) <= 1e-9
+            and abs(first.y - second.y) <= 1e-9
+            and abs(angle_difference(first.yaw, second.yaw)) <= 1e-8
+        )
+
+    def motion_plausible(self, start: Pose, end: Pose, delta_time: float) -> bool:
+        """按照车辆绝对运动能力判断两帧是否连续。"""
+        if not np.isfinite(delta_time) or delta_time <= 0.0:
+            return False
+        displacement = np.hypot(end.x - start.x, end.y - start.y)
+        heading_change = abs(angle_difference(end.yaw, start.yaw))
+        return (
+            displacement
+            <= self.maximum_linear_speed * delta_time
+            + self.position_jump_margin
+            and heading_change
+            <= self.maximum_angular_speed * delta_time
+            + self.heading_jump_margin
+        )
+
+    def accept_after_reset(self, pose: Pose, timestamp: float) -> State:
+        """接受首帧或确认后的重定位并清除速度历史。"""
+        state = self.estimator.reset(pose, timestamp)
+        assert state is not None
+        self.latest_state = state
+        self.accepted_pose = pose
+        self.accepted_timestamp = timestamp
+        self.stationary_hold = False
+        return state
+
+    def accept_continuous(self, pose: Pose, timestamp: float) -> State:
+        """接受连续正常位姿并更新速度估计。"""
+        state = self.estimator.update(pose, timestamp)
+        self.latest_state = state
+        self.accepted_pose = pose
+        self.accepted_timestamp = timestamp
+        self.stationary_hold = False
+        return state
+
+    def handle_repeated(self, timestamp: float) -> tuple[State, str]:
+        """保留重复帧，并在长期不变后将估计速度归零。"""
+        assert self.accepted_pose is not None
+        assert self.latest_state is not None
+        unchanged = timestamp - self.accepted_timestamp
+        if not self.stationary_hold and unchanged >= self.stationary_seconds:
+            self.latest_state = State(
+                timestamp,
+                self.accepted_pose,
+                0.0,
+                0.0,
+                0.0,
+                True,
+            )
+            self.stationary_hold = True
+            return self.latest_state, "stationary_zero"
+        return self.latest_state, "duplicate"
+
+    def process(
+        self,
+        pose: Pose,
+        timestamp: float,
+        velocity_innovation_abnormal: bool = False,
+    ) -> tuple[State | None, str]:
+        """处理一帧CSV中的Detour读取结果。"""
+        if self.accepted_pose is None:
+            return self.accept_after_reset(pose, timestamp), "initialized"
+        if self.poses_equal(pose, self.accepted_pose):
+            return self.handle_repeated(timestamp)
+        if self.stationary_hold:
+            return self.accept_after_reset(pose, timestamp), "restart_after_stationary"
+
+        elapsed = timestamp - self.accepted_timestamp
+        if not self.motion_plausible(self.accepted_pose, pose, elapsed):
+            assert self.latest_state is not None
+            return self.latest_state, "physical_anomaly"
+        if velocity_innovation_abnormal:
+            self.latest_state = self.estimator.rebase_preserving_velocity(
+                pose,
+                timestamp,
+            )
+            self.accepted_pose = pose
+            self.accepted_timestamp = timestamp
+            self.stationary_hold = False
+            return self.latest_state, "velocity_rebase"
+        return self.accept_continuous(pose, timestamp), "accepted"
+
+
+def segmented_unwrap_degrees(values_radians: np.ndarray) -> np.ndarray:
+    """分别展开由NaN分隔的有效航向区间。"""
+    result = np.full(values_radians.shape, np.nan, dtype=float)
+    finite = np.isfinite(values_radians)
+    indices = np.flatnonzero(finite)
+    if not indices.size:
+        return result
+    starts = np.r_[0, np.flatnonzero(np.diff(indices) > 1) + 1]
+    ends = np.r_[starts[1:], indices.size]
+    for start, end in zip(starts, ends):
+        segment_indices = indices[start:end]
+        result[segment_indices] = np.rad2deg(
+            np.unwrap(values_radians[segment_indices])
+        )
+    return result
+
+
+def calculate_naive_derivatives(
+    time: np.ndarray,
+    x: np.ndarray,
+    y: np.ndarray,
+    yaw: np.ndarray,
+) -> tuple[np.ndarray, np.ndarray]:
+    """直接逐记录帧差分，保留重复帧造成的零值和更新尖峰。"""
+    speed = np.full(time.shape, np.nan, dtype=float)
+    omega = np.full(time.shape, np.nan, dtype=float)
+    delta_time = np.diff(time)
+    valid = np.isfinite(delta_time) & (delta_time > 0.0)
+    delta_x = np.diff(x)
+    delta_y = np.diff(y)
+    delta_yaw = wrap_radians(np.diff(yaw))
+    speed_values = np.full(delta_time.shape, np.nan, dtype=float)
+    omega_values = np.full(delta_time.shape, np.nan, dtype=float)
+    speed_values[valid] = (
+        np.hypot(delta_x[valid], delta_y[valid])
+        / delta_time[valid]
+    )
+    omega_values[valid] = delta_yaw[valid] / delta_time[valid]
+    speed[1:] = speed_values
+    omega[1:] = omega_values
+    return speed, omega
+
+
+def configure_matplotlib() -> None:
+    """配置常见中文字体和负号显示。"""
+    plt.rcParams["font.sans-serif"] = [
+        "Microsoft YaHei",
+        "SimHei",
+        "Arial Unicode MS",
+        "DejaVu Sans",
+    ]
+    plt.rcParams["axes.unicode_minus"] = False
+
+
+def load_csv(csv_path: Path) -> pd.DataFrame:
+    """读取并校验状态估计对比所需的CSV字段。"""
+    frame = pd.read_csv(csv_path)
+    missing = REQUIRED_COLUMNS.difference(frame.columns)
+    if missing:
+        raise ValueError(
+            f"{csv_path.name}缺少字段：{', '.join(sorted(missing))}"
+        )
+    for column in REQUIRED_COLUMNS:
+        frame[column] = pd.to_numeric(frame[column], errors="coerce")
+    frame = (
+        frame.dropna(subset=list(REQUIRED_COLUMNS))
+        .sort_values("ElapsedSeconds")
+        .drop_duplicates("ElapsedSeconds", keep="last")
+        .reset_index(drop=True)
+    )
+    if len(frame) < 3:
+        raise ValueError(f"{csv_path.name}有效数据不足3行。")
+    frame["ElapsedSeconds"] -= frame["ElapsedSeconds"].iloc[0]
+    return frame
+
+
+def detect_visual_anomalies(
+    time: np.ndarray,
+    x: np.ndarray,
+    y: np.ndarray,
+    yaw: np.ndarray,
+    linear_tau: float,
+    angular_tau: float,
+    position_residual_meters: float,
+    heading_residual_radians: float,
+    stationary_seconds: float,
+) -> np.ndarray:
+    """用恒速预测残差标注可疑跳变，不修改任何状态估计数据。"""
+    anomalies = np.zeros(time.shape, dtype=bool)
+    estimator = VelocityEstimator(linear_tau, angular_tau)
+    previous_pose: Pose | None = None
+    previous_update_time = 0.0
+    latest_state: State | None = None
+    stationary = False
+
+    for index, timestamp in enumerate(time):
+        pose = Pose(
+            float(x[index]),
+            float(y[index]),
+            float(wrap_radians(yaw[index])),
+        )
+
+        if previous_pose is None:
+            latest_state = estimator.reset(pose, float(timestamp))
+            previous_pose = pose
+            previous_update_time = float(timestamp)
+            continue
+
+        if DetourProviderSimulator.poses_equal(pose, previous_pose):
+            if (
+                not stationary
+                and timestamp - previous_update_time >= stationary_seconds
+            ):
+                stationary = True
+            continue
+
+        # 静止后的第一个新定位只重新建立差分基准，避免把起步误标为跳变。
+        if stationary:
+            latest_state = estimator.reset(pose, float(timestamp))
+            previous_pose = pose
+            previous_update_time = float(timestamp)
+            stationary = False
+            continue
+
+        delta_time = float(timestamp) - previous_update_time
+        if (
+            latest_state is not None
+            and latest_state.velocity_valid
+            and delta_time > 0.0
+        ):
+            predicted_x = previous_pose.x + latest_state.vx * delta_time
+            predicted_y = previous_pose.y + latest_state.vy * delta_time
+            predicted_yaw = float(
+                wrap_radians(
+                    previous_pose.yaw + latest_state.omega * delta_time
+                )
+            )
+            position_residual = np.hypot(
+                pose.x - predicted_x,
+                pose.y - predicted_y,
+            )
+            heading_residual = abs(
+                angle_difference(pose.yaw, predicted_yaw)
+            )
+
+            if (
+                position_residual > position_residual_meters
+                or heading_residual > heading_residual_radians
+            ):
+                anomalies[index] = True
+                # 标注后从当前观测重新开始，避免一个跳变引发连续误标。
+                latest_state = estimator.rebase_preserving_velocity(
+                    pose,
+                    float(timestamp),
+                )
+                previous_pose = pose
+                previous_update_time = float(timestamp)
+                continue
+
+        latest_state = estimator.update(pose, float(timestamp))
+        previous_pose = pose
+        previous_update_time = float(timestamp)
+
+    return anomalies
+
+
+def simulate(
+    frame: pd.DataFrame,
+    args: argparse.Namespace,
+) -> tuple[pd.DataFrame, Counter]:
+    """使用当前C#参数处理整份Detour记录。"""
+    time = frame["ElapsedSeconds"].to_numpy(float)
+    raw_x = frame["DetourX"].to_numpy(float) / 1000.0
+    raw_y = frame["DetourY"].to_numpy(float) / 1000.0
+    raw_yaw = np.deg2rad(frame["DetourTheta"].to_numpy(float))
+    raw_speed, raw_omega = calculate_naive_derivatives(
+        time,
+        raw_x,
+        raw_y,
+        raw_yaw,
+    )
+    visual_anomalies = detect_visual_anomalies(
+        time,
+        raw_x,
+        raw_y,
+        raw_yaw,
+        args.linear_tau,
+        args.angular_tau,
+        args.annotation_position_residual_mm / 1000.0,
+        np.deg2rad(args.annotation_heading_residual_deg),
+        args.stationary_seconds,
+    )
+
+    simulator = DetourProviderSimulator(
+        linear_tau=args.linear_tau,
+        angular_tau=args.angular_tau,
+        maximum_linear_speed=args.maximum_linear_speed,
+        maximum_angular_speed=np.deg2rad(args.maximum_angular_speed_deg),
+        position_jump_margin=args.position_jump_margin_mm / 1000.0,
+        heading_jump_margin=np.deg2rad(args.heading_jump_margin_deg),
+        stationary_seconds=args.stationary_seconds,
+    )
+
+    processed_x = np.full(time.shape, np.nan)
+    processed_y = np.full(time.shape, np.nan)
+    processed_yaw = np.full(time.shape, np.nan)
+    processed_speed = np.full(time.shape, np.nan)
+    processed_omega = np.full(time.shape, np.nan)
+    events: list[str] = []
+
+    for index, timestamp in enumerate(time):
+        pose = Pose(
+            raw_x[index],
+            raw_y[index],
+            float(wrap_radians(raw_yaw[index])),
+        )
+        state, event = simulator.process(
+            pose,
+            float(timestamp),
+            bool(visual_anomalies[index]),
+        )
+        events.append(event)
+        if state is None:
+            continue
+        processed_x[index] = state.pose.x
+        processed_y[index] = state.pose.y
+        processed_yaw[index] = state.pose.yaw
+        if state.velocity_valid:
+            processed_speed[index] = np.hypot(state.vx, state.vy)
+            processed_omega[index] = state.omega
+
+    result = pd.DataFrame(
+        {
+            "TimeSeconds": time,
+            "RawX": raw_x,
+            "RawY": raw_y,
+            "RawYawRadians": raw_yaw,
+            "RawSpeed": raw_speed,
+            "RawOmegaRadiansPerSecond": raw_omega,
+            "ProcessedX": processed_x,
+            "ProcessedY": processed_y,
+            "ProcessedYawRadians": processed_yaw,
+            "ProcessedSpeed": processed_speed,
+            "ProcessedOmegaRadiansPerSecond": processed_omega,
+            "VisualAnomaly": visual_anomalies,
+            "Event": events,
+        }
+    )
+    counts = Counter(events)
+    counts["visual_anomaly"] = int(visual_anomalies.sum())
+    return result, counts
+
+
+def plot_comparison(
+    csv_path: Path,
+    frame: pd.DataFrame,
+    result: pd.DataFrame,
+    event_counts: Counter,
+    output_directory: str | None,
+    show: bool,
+) -> Path:
+    """生成位置、航向、线速度和角速度处理前后对比图。"""
+    time = result["TimeSeconds"].to_numpy(float)
+    anomalous = result["VisualAnomaly"].to_numpy(bool)
+    raw_yaw_degrees = segmented_unwrap_degrees(
+        result["RawYawRadians"].to_numpy(float)
+    )
+    processed_yaw_degrees = segmented_unwrap_degrees(
+        result["ProcessedYawRadians"].to_numpy(float)
+    )
+
+    fig, axes = plt.subplots(
+        5,
+        1,
+        figsize=(13.0, 16.0),
+        sharex=True,
+    )
+
+    series = [
+        ("RawX", "ProcessedX", "世界坐标X / m"),
+        ("RawY", "ProcessedY", "世界坐标Y / m"),
+    ]
+    for axis, (raw_name, processed_name, ylabel) in zip(axes[:2], series):
+        axis.plot(time, result[raw_name], color="0.65", linewidth=1.0, label="原始Detour")
+        axis.plot(time, result[processed_name], color="tab:blue", linewidth=1.5, label="在线处理后")
+        axis.scatter(
+            time[anomalous],
+            result.loc[anomalous, raw_name],
+            color="tab:red",
+            marker="x",
+            s=26,
+            label="异常位置",
+            zorder=3,
+        )
+        axis.set_ylabel(ylabel)
+        axis.grid(True, alpha=0.3)
+        axis.legend(loc="best")
+
+    axes[2].plot(time, raw_yaw_degrees, color="0.65", linewidth=1.0, label="原始Detour")
+    axes[2].plot(time, processed_yaw_degrees, color="tab:blue", linewidth=1.5, label="在线处理后")
+    axes[2].scatter(
+        time[anomalous],
+        raw_yaw_degrees[anomalous],
+        color="tab:red",
+        marker="x",
+        s=26,
+        label="异常位置",
+        zorder=3,
+    )
+    axes[2].set_ylabel("展开航向角 / deg")
+    axes[2].grid(True, alpha=0.3)
+    axes[2].legend(loc="best")
+
+    axes[3].plot(time, result["RawSpeed"], color="0.65", linewidth=1.0, label="逐记录帧直接差分")
+    axes[3].plot(time, result["ProcessedSpeed"], color="tab:green", linewidth=1.5, label="去重、跳变保护和低通后")
+    if "CommandSpeed" in frame.columns:
+        command_speed = pd.to_numeric(
+            frame["CommandSpeed"], errors="coerce"
+        ).to_numpy(float)
+        axes[3].plot(time, command_speed, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令线速度")
+    axes[3].set_ylabel("合线速度 / (m/s)")
+    axes[3].grid(True, alpha=0.3)
+    axes[3].legend(loc="best")
+
+    axes[4].plot(
+        time,
+        np.rad2deg(result["RawOmegaRadiansPerSecond"]),
+        color="0.65",
+        linewidth=1.0,
+        label="逐记录帧最短角差",
+    )
+    axes[4].plot(
+        time,
+        np.rad2deg(result["ProcessedOmegaRadiansPerSecond"]),
+        color="tab:purple",
+        linewidth=1.5,
+        label="去重、跳变保护和低通后",
+    )
+    if "CommandAngularSpeedRadPerSecond" in frame.columns:
+        command_omega = np.rad2deg(
+            pd.to_numeric(
+                frame["CommandAngularSpeedRadPerSecond"],
+                errors="coerce",
+            ).to_numpy(float)
+        )
+        axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
+    elif "CommandAngularSpeed" in frame.columns:
+        # 旧版CSV只有CommandAngularSpeed列，该列历史单位是deg/s；
+        # 新版CSV另增RadPerSecond列，不能把旧列再次按rad/s换算。
+        command_omega = pd.to_numeric(
+            frame["CommandAngularSpeed"],
+            errors="coerce",
+        ).to_numpy(float)
+        axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
+    axes[4].set_ylabel("角速度 / (deg/s)")
+    axes[4].set_xlabel("时间 / s")
+    axes[4].grid(True, alpha=0.3)
+    axes[4].legend(loc="best")
+
+    controller = (
+        str(frame["ControllerName"].iloc[0])
+        if "ControllerName" in frame.columns
+        else "UnknownController"
+    )
+    trajectory = (
+        str(frame["TrajectoryName"].iloc[0])
+        if "TrajectoryName" in frame.columns
+        else csv_path.stem
+    )
+    anomaly_count = int(anomalous.sum())
+    fig.suptitle(
+        "Detour状态估计处理前后对比\n"
+        f"{controller} - {trajectory}，"
+        f"标注异常位置{anomaly_count}帧",
+        fontsize=14,
+    )
+    fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.965))
+
+    if output_directory:
+        destination_directory = Path(output_directory)
+    else:
+        destination_directory = csv_path.parent / "state_estimation_plots"
+    destination_directory.mkdir(parents=True, exist_ok=True)
+    destination = destination_directory / (
+        csv_path.stem + "_state_estimation_comparison.png"
+    )
+    fig.savefig(destination, dpi=220, bbox_inches="tight")
+    if show:
+        plt.show()
+    plt.close(fig)
+    return destination
+
+
+def discover_csv_files(arguments: list[str]) -> list[Path]:
+    """解析文件或目录；目录会被递归展开为全部轨迹CSV。"""
+    input_paths = (
+        [Path(argument).resolve() for argument in arguments]
+        if arguments
+        else [Path(__file__).resolve().parent]
+    )
+    csv_files: set[Path] = set()
+
+    for input_path in input_paths:
+        if input_path.is_file():
+            if input_path.suffix.lower() == ".csv":
+                csv_files.add(input_path)
+            continue
+
+        if input_path.is_dir():
+            csv_files.update(
+                path.resolve()
+                for path in input_path.rglob("*.csv")
+                if not any(
+                    part.startswith("state_estimation_plots")
+                    for part in path.parts
+                )
+            )
+            continue
+
+        raise FileNotFoundError(
+            f"输入文件或目录不存在：{input_path}"
+        )
+
+    return sorted(csv_files)
+
+
+def main() -> None:
+    """解析命令行并批量生成Detour状态估计对比图。"""
+    configure_matplotlib()
+    parser = argparse.ArgumentParser(
+        description="比较原始Detour与当前C#在线状态估计算法。"
+    )
+    parser.add_argument(
+        "files",
+        nargs="*",
+        help="一个或多个轨迹实验CSV或包含CSV的目录",
+    )
+    parser.add_argument("--output-dir")
+    parser.add_argument("--show", action="store_true")
+    parser.add_argument("--linear-tau", type=float, default=0.15)
+    parser.add_argument("--angular-tau", type=float, default=0.20)
+    parser.add_argument("--maximum-linear-speed", type=float, default=1.20)
+    parser.add_argument("--maximum-angular-speed-deg", type=float, default=45.0)
+    parser.add_argument("--position-jump-margin-mm", type=float, default=30.0)
+    parser.add_argument("--heading-jump-margin-deg", type=float, default=5.0)
+    parser.add_argument(
+        "--annotation-position-residual-mm",
+        type=float,
+        default=40.0,
+        help="只用于图中红色异常位置标注的预测位置残差阈值",
+    )
+    parser.add_argument(
+        "--annotation-heading-residual-deg",
+        type=float,
+        default=5.0,
+        help="只用于图中红色异常位置标注的预测航向残差阈值",
+    )
+    parser.add_argument("--stationary-seconds", type=float, default=0.35)
+    args = parser.parse_args()
+
+    processed_count = 0
+    for csv_path in discover_csv_files(args.files):
+        try:
+            frame = load_csv(csv_path)
+            result, counts = simulate(frame, args)
+            destination = plot_comparison(
+                csv_path,
+                frame,
+                result,
+                counts,
+                args.output_dir,
+                args.show,
+            )
+            print(
+                f"{csv_path.name}: "
+                f"重复帧={counts['duplicate']}，"
+                f"异常位置={counts['visual_anomaly']}"
+            )
+            print(f"已生成：{destination}")
+            processed_count += 1
+        except Exception as exception:
+            print(f"跳过{csv_path.name}：{exception}")
+
+    if processed_count == 0:
+        raise SystemExit("没有找到包含有效Detour字段的轨迹实验CSV。")
+
+
+if __name__ == "__main__":
+    main()

docs/detour数据处理.txt

@@ -0,0 +1,365 @@
+需要处理，而且对横向误差、航向误差和速度估计都会有明显影响。检测本身不难，困难的是区分：
+
+```text
+单帧错误定位
+持续性的定位重定位/地图修正
+车辆真实的快速运动
+```
+
+对你这种低速停车机器人，可以先采用一套偏安全、容易验证的处理。
+
+## 定位跳变会造成什么影响
+
+假设Detour在50ms内突然跳了10cm：
+
+\[
+v=\frac{0.1}{0.05}=2m/s
+\]
+
+实际车辆可能只有 `0.3m/s`，但差分速度会产生 `2m/s` 的尖峰。
+
+对控制器还有三个直接影响：
+
+- 横向位置跳10cm，横向误差可能瞬间变化10cm。
+- 航向跳5°，航向误差会瞬间变化5°。
+- 全局轨迹投影可能跳到另一条临近或相交的轨迹线段。
+
+Stanley在低速时尤其敏感：
+
+\[
+\delta =
+e_\theta+
+\arctan\left(\frac{k e_y}{v+\varepsilon}\right)
+\]
+
+低速时分母较小，横向误差突然变大，会产生很大的转向指令。因此不能完全不处理。
+
+## 不建议直接低通掉定位跳变
+
+不要简单地对Detour位置做强低通：
+
+```text
+错误位置跳变
+→ 低通缓慢跟过去
+```
+
+这样虽然曲线看起来平滑，但控制器会在一段时间内使用滞后、虚构的位置，可能更加危险。
+
+更好的做法是：
+
+```text
+检测跳变
+→ 暂时不把该帧用于速度差分和控制
+→ 观察后续定位
+→ 判断是单帧异常还是持续重定位
+```
+
+## 第一层：运动学合理性检查
+
+将当前Detour位姿和上一次接受的位姿比较。
+
+位置变化：
+
+\[
+\Delta p=\sqrt{\Delta x^2+\Delta y^2}
+\]
+
+航向变化：
+
+\[
+\Delta\theta=
+|\operatorname{ShortestAngleDifference}|
+\]
+
+允许的最大变化量可以按照车辆物理能力计算：
+
+```csharp
+var maximumAllowedDistance =
+    maximumLinearSpeedMetersPerSecond *
+    deltaTimeSeconds +
+    positionJumpMarginMeters;
+
+var maximumAllowedHeadingChange =
+    maximumAngularSpeedRadiansPerSecond *
+    deltaTimeSeconds +
+    headingJumpMarginRadians;
+```
+
+然后判断：
+
+```csharp
+var positionJump =
+    displacementMeters >
+    maximumAllowedDistance;
+
+var headingJump =
+    headingChangeRadians >
+    maximumAllowedHeadingChange;
+```
+
+你当前小车最高约 `1.2m/s`，假设Detour更新周期为50ms：
+
+```text
+物理最大位移约为：
+1.2 × 0.05 = 0.06m
+```
+
+初期可以额外留出约 `0.03～0.05m` 的定位余量。不过这些阈值最终应根据实际Detour数据确定，不建议永久写死。
+
+## 第二层：不要立即接受异常帧
+
+检测到一个异常帧时，不要立刻改变车辆状态：
+
+```text
+上一正常位置 A
+异常位置 B
+下一帧又回到 A 附近
+```
+
+这种情况说明B很可能是单帧异常，应直接丢弃。
+
+如果后续连续若干帧都稳定在B附近：
+
+```text
+A → B → B附近 → B附近
+```
+
+这更可能是Detour发生了持续性的重定位。
+
+可以使用：
+
+```text
+连续2～3个新定位帧相互一致
+```
+
+作为重新接受定位的条件。
+
+## 第三层：重定位后必须重置速度估计
+
+如果确认新的定位是持续有效的，不能用：
+
+```text
+新位置B - 旧位置A
+```
+
+计算速度，因为A到B是定位修正，不是车辆真实运动。
+
+正确处理是：
+
+```csharp
+_velocityEstimator.Reset(
+    newPose,
+    currentTimestamp);
+```
+
+这会：
+
+- 将新位姿作为新的差分起点。
+- 清除之前的速度历史。
+- 重置三个低通滤波器。
+- 将速度暂时标记为无效或零。
+- 等下一次正常Detour更新后重新开始估计。
+
+## 第四层：控制器应该如何响应
+
+对于实车轨迹跟踪，建议状态分为：
+
+```text
+Valid        正常定位，可以控制
+Suspected    检测到疑似跳变
+Reacquiring  正在确认新的定位
+Stale        定位长时间没有更新
+```
+
+你的第一版不一定需要单独增加复杂枚举，但控制行为至少应该满足：
+
+```text
+正常：
+继续轨迹跟踪
+
+疑似单帧跳变：
+不使用异常帧更新状态
+短时间保持上一状态
+
+连续异常或定位超时：
+停车，不继续使用旧状态运动
+
+确认重定位：
+接受新位姿
+重置速度估计
+重新执行轨迹投影
+确认稳定后恢复控制
+```
+
+停车机器人速度低、场地有限，定位连续异常时停车比盲目继续跟踪更合适。
+
+## 还需要限制轨迹投影进度
+
+即使Detour跳变检测做了，轨迹投影也最好增加进度保护。
+
+当前投影器在整条轨迹上找最近点，如果轨迹自交，车辆可能从：
+
+```text
+当前第20段
+```
+
+突然投影到：
+
+```text
+第80段
+```
+
+后续可以增加一个 `TrajectoryProgressTracker`：
+
+```text
+正常时只在上次线段索引附近搜索
+例如 [previousIndex - 5, previousIndex + 20]
+
+定位重定位后：
+重新执行一次全局搜索
+```
+
+同时限制单周期允许的弧长进度变化：
+
+\[
+|\Delta s|
+\leq
+v_{\max}\Delta t+\text{margin}
+\]
+
+这样可以避免定位轻微波动导致轨迹进度突然跳段。
+
+## 在五个文件中的分工
+
+```text
+DetourVehicleStateProvider
+├─ 检查数值有效性
+├─ 检查定位是否过期
+├─ 检测重复帧
+├─ 检测位置/航向跳变
+├─ 暂存疑似新定位
+└─ 确认重定位后触发Reset
+
+VelocityEstimator2D
+├─ 正常位姿差分
+├─ 真实时间差计算
+├─ 速度滤波
+└─ 提供Reset()
+
+FirstOrderLowPassFilter
+└─ 提供Update()和Reset()
+
+VehicleState
+├─ 保存位姿和速度
+└─ 表示速度估计是否有效
+
+IVehicleStateProvider
+└─ 向控制器返回有效状态或读取失败
+```
+
+因此定位跳变的主要逻辑放在 `DetourVehicleStateProvider`，而不是低通滤波器。
+
+第一版建议先实现：
+
+1. 非有限值检查。
+2. 重复帧识别。
+3. 基于最大速度和最大角速度的跳变检测。
+4. 单帧异常拒绝。
+5. 连续2～3帧确认重定位。
+6. 重定位后重置速度估计。
+7. 长时间定位无效时通知控制器停车。
+
+这套方案足以保护当前Stanley和后续纵向控制，不需要现在就修改SLAM源码。
+
+
+已经给 [DetourVehicleStateProvider.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:233) 增加了基于上一有效速度的预测残差检测。
+
+现在采用两级判断：
+
+```text
+第一级：绝对物理运动边界
+第二级：上一状态恒速预测残差
+```
+
+预测公式为：
+
+\[
+x_{pred}=x_{last}+v_x\Delta t
+\]
+
+\[
+y_{pred}=y_{last}+v_y\Delta t
+\]
+
+\[
+\theta_{pred}=
+\theta_{last}+\omega\Delta t
+\]
+
+实测残差为：
+
+\[
+e_p=
+\sqrt{
+(x_{measured}-x_{pred})^2+
+(y_{measured}-y_{pred})^2
+}
+\]
+
+以及：
+
+\[
+e_\theta=
+\operatorname{ShortestDifference}
+(\theta_{measured},\theta_{pred})
+\]
+
+对应实现在：
+
+- [IsPredictionResidualPlausible()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:536)
+- [PredictPoseInWorld()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:573)
+
+默认阈值为：
+
+```csharp
+位置预测残差：0.04m
+航向预测残差：5°
+```
+
+没有采用最初考虑的30mm，是因为用现有数据离线统计后发现：
+
+| 位置残差阈值 | 触发次数 | 占有效更新比例 |
+|---:|---:|---:|
+| 30mm | 55次 | 1.24% |
+| 35mm | 28次 | 0.63% |
+| 40mm | 13次 | 0.29% |
+| 45mm | 7次 | 0.16% |
+| 50mm | 4次 | 0.09% |
+
+30mm容易把正常Detour波动也判成异常；40mm仍能识别现有数据中约49mm和66mm的明显跳变，同时误触发风险更低。
+
+当前判定逻辑：
+
+```csharp
+if (!isWithinPhysicalBoundary ||
+    !isWithinPredictionResidual)
+{
+    // 进入疑似重定位确认状态。
+}
+```
+
+另外同步修正了跳变恢复逻辑：
+
+- 发生预测残差跳变时，保存当时的预测位姿。
+- 后续定位必须回到预测位姿附近，才能认为是单帧异常后的正常恢复。
+- 不再使用较宽松的绝对物理范围立即放行。
+- 持续远端定位仍需满足3次观测和0.25秒，才按重定位接管。
+- 接管后重置速度估计，避免把坐标修正计算成车辆速度。
+
+第一帧或重定位后的速度还没有建立时，不启用预测残差检测，只使用物理极限检查，避免没有速度基准时误判。
+
+完整构建结果：
+
+- CommonUsage：0警告、0错误
+- MedullaAdapter：0警告、0错误
+- MultiWheelC：0警告、0错误

docs/记录.txt

@@ -1,23 +1,28 @@
-还可以把底盘的失败原因暴露出来：
-/// <summary>
-/// 获取最近一次底盘运动分解失败原因。
-/// </summary>
-public string LastFailureReason =>
-    _chassis.LastMotionDecomposeFailureReason;
-这样调用方可以打印：
-if (!adapter.Send(command))
-{
-    Console.WriteLine(
-        $"底盘命令执行失败：{adapter.LastFailureReason}");
-}
-
-
-需要注意，Detour 差分速度会有噪声，建议在 Python 中：
-按固定频率重新采样。
-对位置做轻微滤波或使用 Savitzky–Golay 求导。
-再计算速度，避免直接逐点差分产生尖峰。
-Stanley 和 LQR 必须使用相同的滤波和采样参数。
-
-
 编译命令：
 powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
+
+
+
+1. TrajectoryPoint.cs       已完成
+2. Trajectory2D.cs          下一步
+3. TrajectoryProjection.cs  定义一次投影结果
+4. TrajectoryProjector.cs   实现连续线段投影
+5. TrajectoryBuilder.cs     原始离散点转标准轨迹
+
+1. VehicleState.cs
+        ↓
+2. FirstOrderLowPassFilter.cs
+        ↓
+3. VelocityEstimator2D.cs
+        ↓
+4. IVehicleStateProvider.cs
+        ↓
+5. DetourVehicleStateProvider.cs
+
+
+
+private const double LinearVelocityFilterTimeConstantSeconds =
+    0.10;
+
+private const double AngularVelocityFilterTimeConstantSeconds =
+    0.10;