feat: integrate trajectory tracking controller runtime

ClumsyPilot/ClumsyPilot.csproj

@@ -23,6 +23,9 @@
     <Compile Remove="ParkrobTrajplanner\Trajplanner_output\**\*.cs" />
     <Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
              Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
+    <!-- 嵌套验证项目的旧输出不能参与主插件的程序集解析。 -->
+    <None Remove="**\bin\**\*" />
+    <None Remove="**\obj\**\*" />
   </ItemGroup>
 
   <ItemGroup>

ClumsyPilot/Control/Abstractions/ILateralController.cs

@@ -0,0 +1,19 @@
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 定义Stanley、LQR和MPC等车体中心横向控制器的统一接口。
+    /// </summary>
+    public interface ILateralController
+    {
+        /// <summary>
+        /// 根据本周期车辆状态和轨迹误差计算车体中心目标曲率。
+        /// </summary>
+        LateralControlCommand Compute(
+            PathTrackingContext context);
+
+        /// <summary>
+        /// 清除控制器跨周期状态，以便开始新轨迹或异常恢复后重新运行。
+        /// </summary>
+        void Reset();
+    }
+}

ClumsyPilot/Control/Abstractions/ILongitudinalController.cs

@@ -0,0 +1,19 @@
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 定义根据参考速度和实际纵向速度生成底盘命令速度的统一接口。
+    /// </summary>
+    public interface ILongitudinalController
+    {
+        /// <summary>
+        /// 根据本周期速度目标、速度反馈和时间间隔计算有符号底盘命令速度。
+        /// </summary>
+        double ComputeSpeedMetersPerSecond(
+            PathTrackingContext context);
+
+        /// <summary>
+        /// 清除积分、历史误差和其他跨周期状态，以便安全开始新的控制过程。
+        /// </summary>
+        void Reset();
+    }
+}

ClumsyPilot/Control/Abstractions/LateralControlCommand.cs

@@ -0,0 +1,76 @@
+using System;
+
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
+    /// </summary>
+    public readonly struct LateralControlCommand
+    {
+        /// <summary>
+        /// 创建前、后GCP目标转角命令。
+        /// </summary>
+        public LateralControlCommand(
+            double frontGcpAngleRadians,
+            double rearGcpAngleRadians)
+        {
+            EnsureFinite(
+                frontGcpAngleRadians,
+                nameof(frontGcpAngleRadians));
+            EnsureFinite(
+                rearGcpAngleRadians,
+                nameof(rearGcpAngleRadians));
+
+            FrontGcpAngleRadians =
+                frontGcpAngleRadians;
+            RearGcpAngleRadians =
+                rearGcpAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取前GCP目标转角，单位为rad，逆时针为正。
+        /// </summary>
+        public double FrontGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 获取后GCP目标转角，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 获取前后GCP的共同转角分量，主要用于横向平移修正。
+        /// </summary>
+        public double CommonAngleRadians =>
+            (FrontGcpAngleRadians +
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
+        /// </summary>
+        public double DifferentialAngleRadians =>
+            (FrontGcpAngleRadians -
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 创建前后GCP均保持车头方向的直线命令。
+        /// </summary>
+        public static LateralControlCommand Straight =>
+            new LateralControlCommand(0.0, 0.0);
+
+        /// <summary>
+        /// 检查GCP目标转角是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP目标转角必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Abstractions/PathTrackingContext.cs

@@ -0,0 +1,126 @@
+using System;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。
+    /// </summary>
+    public readonly struct PathTrackingContext
+    {
+        /// <summary>
+        /// 创建横向和纵向控制器共享的只读控制输入快照。
+        /// </summary>
+        public PathTrackingContext(
+            VehicleState vehicleState,
+            TrajectoryProjection projection,
+            double referenceSpeedMetersPerSecond,
+            double deltaTimeSeconds)
+        {
+            EnsureFinite(
+                referenceSpeedMetersPerSecond,
+                nameof(referenceSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            VehicleState = vehicleState;
+            Projection = projection;
+            ReferenceSpeedMetersPerSecond =
+                referenceSpeedMetersPerSecond;
+            DeltaTimeSeconds = deltaTimeSeconds;
+        }
+
+        /// <summary>
+        /// 获取本周期经过校验的实际车辆位姿和速度状态。
+        /// </summary>
+        public VehicleState VehicleState { get; }
+
+        /// <summary>
+        /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
+        /// </summary>
+        public TrajectoryProjection Projection { get; }
+
+        /// <summary>
+        /// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。
+        /// </summary>
+        public double DeltaTimeSeconds { get; }
+
+        /// <summary>
+        /// 获取轨迹投影点要求的有符号参考速度，单位为m/s。
+        /// </summary>
+        public double ReferenceSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
+        /// </summary>
+        public double ActualLongitudinalSpeedMetersPerSecond =>
+            VehicleState.TwistInBody
+                .VxMetersPerSecond;
+
+        /// <summary>
+        /// 获取轨迹投影点的参考曲率，单位为1/m，左转为正。
+        /// </summary>
+        public double ReferenceCurvaturePerMeter =>
+            Projection.ReferencePoint
+                .CurvaturePerMeter;
+
+        /// <summary>
+        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+        /// </summary>
+        public double LateralErrorMeters =>
+            Projection.LateralErrorMeters;
+
+        /// <summary>
+        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
+        /// </summary>
+        public double HeadingErrorRadians =>
+            Projection.HeadingErrorRadians;
+
+        /// <summary>
+        /// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。
+        /// </summary>
+        public double RemainingDistanceMeters =>
+            Projection.RemainingDistanceMeters;
+
+        /// <summary>
+        /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
+        /// </summary>
+        public bool HasValidVelocityEstimate =>
+            VehicleState.HasValidVelocityEstimate;
+
+        /// <summary>
+        /// 检查控制周期是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                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,
+                    "轨迹跟踪参考速度必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Allocation/GcpCommandAllocator.cs

@@ -0,0 +1,104 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
+    /// </summary>
+    public sealed class GcpCommandAllocator
+    {
+        /// <summary>
+        /// 创建使用指定前后GCP最大转角的命令分配器。
+        /// </summary>
+        public GcpCommandAllocator(double maximumGcpAngleRadians)
+        {
+            EnsureFinitePositive(
+                maximumGcpAngleRadians,
+                nameof(maximumGcpAngleRadians));
+
+            if (maximumGcpAngleRadians >= Math.PI / 2.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(maximumGcpAngleRadians),
+                    "最大GCP转角必须小于π/2。");
+            }
+
+            MaximumGcpAngleRadians = maximumGcpAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取前后GCP允许的最大转角绝对值，单位为rad。
+        /// </summary>
+        public double MaximumGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 将纵向速度和前后GCP转角组合为底盘运动命令。
+        /// </summary>
+        public GcpMotionCommand Allocate(
+            double speedMetersPerSecond,
+            LateralControlCommand lateralCommand)
+        {
+            EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+
+            var frontAngleRadians = ClampSymmetric(
+                lateralCommand.FrontGcpAngleRadians,
+                MaximumGcpAngleRadians);
+            var rearAngleRadians = ClampSymmetric(
+                lateralCommand.RearGcpAngleRadians,
+                MaximumGcpAngleRadians);
+
+            return new GcpMotionCommand(
+                speedMetersPerSecond,
+                frontAngleRadians,
+                rearAngleRadians);
+        }
+
+        /// <summary>
+        /// 将数值按正负对称方式限制在指定绝对值内。
+        /// </summary>
+        private static double ClampSymmetric(
+            double value,
+            double maximumAbsoluteValue)
+        {
+            return Math.Max(
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, value));
+        }
+
+        /// <summary>
+        /// 检查参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP分配参数必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查参数或命令是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP分配参数和命令必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Allocation/GcpMotionCommand.cs

@@ -0,0 +1,67 @@
+using System;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 表示发送给旧版多舵轮四轮解算前的有符号速度和前后GCP角度命令。
+    /// </summary>
+    public readonly struct GcpMotionCommand
+    {
+        /// <summary>
+        /// 创建统一使用m/s和rad的前后几何控制点运动命令。
+        /// </summary>
+        public GcpMotionCommand(
+            double speedMetersPerSecond,
+            double frontAngleRadians,
+            double rearAngleRadians)
+        {
+            EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+            EnsureFinite(
+                frontAngleRadians,
+                nameof(frontAngleRadians));
+            EnsureFinite(
+                rearAngleRadians,
+                nameof(rearAngleRadians));
+
+            SpeedMetersPerSecond =
+                speedMetersPerSecond;
+            FrontAngleRadians =
+                frontAngleRadians;
+            RearAngleRadians =
+                rearAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取准备交给底盘的有符号纵向速度，单位为m/s，正值表示前进。
+        /// </summary>
+        public double SpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取前几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double FrontAngleRadians { get; }
+
+        /// <summary>
+        /// 获取后几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearAngleRadians { get; }
+
+        /// <summary>
+        /// 检查底盘中间命令是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP运动命令必须由有限值组成。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Common/PidController.cs

@@ -0,0 +1,307 @@
+using System;
+
+namespace MultiWheelC.Control.Common
+{
+    /// <summary>
+    /// 使用真实控制周期计算带积分限幅、输出限幅和抗饱和的通用有状态PID输出。
+    /// </summary>
+    public sealed class PidController
+    {
+        private double _integralState;
+        private double _previousError;
+        private double _previousMeasurement;
+        private bool _hasPreviousSample;
+
+        /// <summary>
+        /// 创建具有指定增益、积分输出限制和微分形式的PID控制器。
+        /// </summary>
+        public PidController(
+            double proportionalGain,
+            double integralGainPerSecond,
+            double derivativeGainSeconds,
+            double maximumIntegralOutput,
+            bool derivativeOnMeasurement = true)
+        {
+            EnsureFiniteNonNegative(
+                proportionalGain,
+                nameof(proportionalGain));
+            EnsureFiniteNonNegative(
+                integralGainPerSecond,
+                nameof(integralGainPerSecond));
+            EnsureFiniteNonNegative(
+                derivativeGainSeconds,
+                nameof(derivativeGainSeconds));
+            EnsureFiniteNonNegative(
+                maximumIntegralOutput,
+                nameof(maximumIntegralOutput));
+
+            ProportionalGain = proportionalGain;
+            IntegralGainPerSecond = integralGainPerSecond;
+            DerivativeGainSeconds = derivativeGainSeconds;
+            MaximumIntegralOutput = maximumIntegralOutput;
+            DerivativeOnMeasurement = derivativeOnMeasurement;
+        }
+
+        /// <summary>
+        /// 获取比例增益。
+        /// </summary>
+        public double ProportionalGain { get; }
+
+        /// <summary>
+        /// 获取积分增益，单位为1/s。
+        /// </summary>
+        public double IntegralGainPerSecond { get; }
+
+        /// <summary>
+        /// 获取微分增益，单位为s。
+        /// </summary>
+        public double DerivativeGainSeconds { get; }
+
+        /// <summary>
+        /// 获取积分项允许产生的最大输出绝对值。
+        /// </summary>
+        public double MaximumIntegralOutput { get; }
+
+        /// <summary>
+        /// 获取微分项是否作用于测量值，以避免设定值变化产生微分冲击。
+        /// </summary>
+        public bool DerivativeOnMeasurement { get; }
+
+        /// <summary>
+        /// 获取最近一次设定值减测量值的误差。
+        /// </summary>
+        public double LastError { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次比例项输出。
+        /// </summary>
+        public double LastProportionalOutput { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次积分项输出。
+        /// </summary>
+        public double LastIntegralOutput { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次微分项输出。
+        /// </summary>
+        public double LastDerivativeOutput { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次经过输出范围限制后的PID输出。
+        /// </summary>
+        public double LastOutput { get; private set; }
+
+        /// <summary>
+        /// 根据设定值、测量值、真实时间间隔和本周期输出范围更新PID。
+        /// </summary>
+        public double Update(
+            double setPoint,
+            double measurement,
+            double deltaTimeSeconds,
+            double minimumOutput,
+            double maximumOutput)
+        {
+            EnsureFinite(setPoint, nameof(setPoint));
+            EnsureFinite(measurement, nameof(measurement));
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+            EnsureFinite(minimumOutput, nameof(minimumOutput));
+            EnsureFinite(maximumOutput, nameof(maximumOutput));
+
+            if (minimumOutput > maximumOutput)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(minimumOutput),
+                    "PID最小输出不能大于最大输出。");
+            }
+
+            var error = setPoint - measurement;
+            var proportionalOutput =
+                ProportionalGain * error;
+            var derivativeOutput = CalculateDerivativeOutput(
+                error,
+                measurement,
+                deltaTimeSeconds);
+
+            var candidateIntegralState =
+                _integralState +
+                error * deltaTimeSeconds;
+            var integralOutput = CalculateIntegralOutput(
+                candidateIntegralState);
+
+            // 同步截断积分状态本身，避免积分输出虽已限幅、内部状态仍继续增长。
+            candidateIntegralState =
+                IntegralGainPerSecond > 0.0 &&
+                MaximumIntegralOutput > 0.0
+                    ? integralOutput /
+                      IntegralGainPerSecond
+                    : 0.0;
+
+            var unlimitedOutput =
+                proportionalOutput +
+                integralOutput +
+                derivativeOutput;
+            var output = Clamp(
+                unlimitedOutput,
+                minimumOutput,
+                maximumOutput);
+
+            // 根据实际允许输出反算积分项，避免执行器饱和期间继续积累误差。
+            if (IntegralGainPerSecond > 0.0 &&
+                output != unlimitedOutput)
+            {
+                integralOutput = Clamp(
+                    output -
+                    proportionalOutput -
+                    derivativeOutput,
+                    -MaximumIntegralOutput,
+                    MaximumIntegralOutput);
+                candidateIntegralState =
+                    integralOutput /
+                    IntegralGainPerSecond;
+            }
+
+            _integralState =
+                IntegralGainPerSecond > 0.0 &&
+                MaximumIntegralOutput > 0.0
+                    ? candidateIntegralState
+                    : 0.0;
+            _previousError = error;
+            _previousMeasurement = measurement;
+            _hasPreviousSample = true;
+
+            LastError = error;
+            LastProportionalOutput = proportionalOutput;
+            LastIntegralOutput = integralOutput;
+            LastDerivativeOutput = derivativeOutput;
+            LastOutput = output;
+
+            return output;
+        }
+
+        /// <summary>
+        /// 清除积分、历史采样和最近一次PID诊断输出。
+        /// </summary>
+        public void Reset()
+        {
+            _integralState = 0.0;
+            _previousError = 0.0;
+            _previousMeasurement = 0.0;
+            _hasPreviousSample = false;
+            LastError = 0.0;
+            LastProportionalOutput = 0.0;
+            LastIntegralOutput = 0.0;
+            LastDerivativeOutput = 0.0;
+            LastOutput = 0.0;
+        }
+
+        /// <summary>
+        /// 使用测量值微分或误差微分计算本周期微分项输出。
+        /// </summary>
+        private double CalculateDerivativeOutput(
+            double error,
+            double measurement,
+            double deltaTimeSeconds)
+        {
+            if (!_hasPreviousSample ||
+                DerivativeGainSeconds <= 0.0)
+            {
+                return 0.0;
+            }
+
+            if (DerivativeOnMeasurement)
+            {
+                return -DerivativeGainSeconds *
+                    (measurement - _previousMeasurement) /
+                    deltaTimeSeconds;
+            }
+
+            return DerivativeGainSeconds *
+                (error - _previousError) /
+                deltaTimeSeconds;
+        }
+
+        /// <summary>
+        /// 根据积分状态计算经过绝对值限制的积分项输出。
+        /// </summary>
+        private double CalculateIntegralOutput(
+            double integralState)
+        {
+            if (IntegralGainPerSecond <= 0.0 ||
+                MaximumIntegralOutput <= 0.0)
+            {
+                return 0.0;
+            }
+
+            return Clamp(
+                IntegralGainPerSecond * integralState,
+                -MaximumIntegralOutput,
+                MaximumIntegralOutput);
+        }
+
+        /// <summary>
+        /// 将数值限制在指定闭区间内。
+        /// </summary>
+        private static double Clamp(
+            double value,
+            double minimum,
+            double maximum)
+        {
+            return Math.Max(
+                minimum,
+                Math.Min(maximum, value));
+        }
+
+        /// <summary>
+        /// 检查参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "PID时间间隔必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查参数是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "PID增益和积分输出限幅必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查参数是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "PID参数和输入必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Execution/GcpCommandExecutor.cs

@@ -0,0 +1,177 @@
+using System;
+using MultiWheelC.Control.Allocation;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Execution
+{
+    /// <summary>
+    /// 将SI单位的GCP运动命令安全转换为现有多舵轮底盘调用。
+    /// </summary>
+    public sealed class GcpCommandExecutor
+    {
+        private const double StopSpeedDeadbandMetersPerSecond =
+            1e-6;
+
+        private readonly MultiWheelChassisAdapter _chassisAdapter;
+        private double _lastFrontAngleRadians;
+        private double _lastRearAngleRadians;
+
+        /// <summary>
+        /// 创建绑定指定单车底盘适配器的GCP命令执行器。
+        /// </summary>
+        public GcpCommandExecutor(
+            MultiWheelChassisAdapter chassisAdapter,
+            double maximumGcpAngleRateRadiansPerSecond =
+                10.0 * Math.PI / 180.0)
+        {
+            _chassisAdapter = chassisAdapter ??
+                throw new ArgumentNullException(
+                    nameof(chassisAdapter));
+            EnsureFinitePositive(
+                maximumGcpAngleRateRadiansPerSecond,
+                nameof(maximumGcpAngleRateRadiansPerSecond));
+
+            MaximumGcpAngleRateRadiansPerSecond =
+                maximumGcpAngleRateRadiansPerSecond;
+        }
+
+        /// <summary>
+        /// 获取执行器绑定的车辆编号。
+        /// </summary>
+        public int VehicleId =>
+            _chassisAdapter.VehicleId;
+
+        /// <summary>
+        /// 获取前后GCP目标角度允许的最大变化率，单位为rad/s。
+        /// </summary>
+        public double MaximumGcpAngleRateRadiansPerSecond { get; }
+
+        /// <summary>
+        /// 获取最近一次控制器请求的未限速GCP命令。
+        /// </summary>
+        public GcpMotionCommand? LastRequestedCommand { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次经过GCP角速度限制后实际发送给底盘的命令。
+        /// </summary>
+        public GcpMotionCommand? LastSentCommand { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次旧版底盘运动分解失败原因。
+        /// </summary>
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 使用真实控制周期执行一条GCP命令，并在分解失败时保持停车。
+        /// </summary>
+        public bool Execute(
+            GcpMotionCommand command,
+            double deltaTimeSeconds)
+        {
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+            LastRequestedCommand = command;
+
+            if (Math.Abs(command.SpeedMetersPerSecond) <=
+                StopSpeedDeadbandMetersPerSecond)
+            {
+                Stop();
+                LastSentCommand = new GcpMotionCommand(
+                    0.0,
+                    _lastFrontAngleRadians,
+                    _lastRearAngleRadians);
+                return true;
+            }
+
+            var maximumAngleChangeRadians =
+                MaximumGcpAngleRateRadiansPerSecond *
+                deltaTimeSeconds;
+            _lastFrontAngleRadians = MoveTowards(
+                _lastFrontAngleRadians,
+                command.FrontAngleRadians,
+                maximumAngleChangeRadians);
+            _lastRearAngleRadians = MoveTowards(
+                _lastRearAngleRadians,
+                command.RearAngleRadians,
+                maximumAngleChangeRadians);
+
+            var limitedCommand = new GcpMotionCommand(
+                command.SpeedMetersPerSecond,
+                _lastFrontAngleRadians,
+                _lastRearAngleRadians);
+            LastSentCommand = limitedCommand;
+
+            var success = _chassisAdapter.SendGcpMotion(
+                limitedCommand.SpeedMetersPerSecond,
+                limitedCommand.FrontAngleRadians,
+                limitedCommand.RearAngleRadians,
+                TimeSpan.FromSeconds(deltaTimeSeconds));
+
+            LastFailureReason = success
+                ? string.Empty
+                : BuildFailureReason();
+
+            return success;
+        }
+
+        /// <summary>
+        /// 立即清零底盘驱动速度并清除执行器失败状态。
+        /// </summary>
+        public void Stop()
+        {
+            _chassisAdapter.StopImmediately();
+            LastFailureReason = string.Empty;
+        }
+
+        /// <summary>
+        /// 以不超过指定单周期变化量的速度使当前值接近目标值。
+        /// </summary>
+        private static double MoveTowards(
+            double current,
+            double target,
+            double maximumChange)
+        {
+            var difference = target - current;
+
+            if (Math.Abs(difference) <= maximumChange)
+            {
+                return target;
+            }
+
+            return current +
+                   Math.Sign(difference) *
+                   maximumChange;
+        }
+
+        /// <summary>
+        /// 将底盘返回的空失败原因替换为可诊断的默认说明。
+        /// </summary>
+        private string BuildFailureReason()
+        {
+            return string.IsNullOrWhiteSpace(
+                    _chassisAdapter.LastFailureReason)
+                ? "旧版SendMotion未能完成GCP运动分解。"
+                : _chassisAdapter.LastFailureReason;
+        }
+
+        /// <summary>
+        /// 检查控制周期是否为正有限值且能够转换为TimeSpan。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0 ||
+                value > TimeSpan.MaxValue.TotalSeconds)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP命令控制周期必须是TimeSpan可表示的正有限秒数。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Execution/ParkingGeometricController.cs

@@ -0,0 +1,551 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Execution
+{
+    /// <summary>
+    /// 表示新版停车机器人单周期轨迹控制的执行结果。
+    /// </summary>
+    public enum ParkingControlCycleResult
+    {
+        Inactive = 0,
+        CommandSent = 1,
+        Completed = 2,
+        StateUnavailable = 3,
+        Faulted = 4
+    }
+
+    /// <summary>
+    /// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
+    /// </summary>
+    public sealed class ParkingGeometricController
+    {
+        private const double ZeroReferenceSpeedToleranceMetersPerSecond =
+            1e-6;
+        private const double StartupRegionMeters = 0.02;
+        private const double StartupPreviewDistanceMeters = 0.05;
+        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
+
+        private readonly IVehicleStateProvider _stateProvider;
+        private readonly ILateralController _lateralController;
+        private readonly ILongitudinalController _longitudinalController;
+        private readonly GcpCommandAllocator _gcpAllocator;
+        private readonly GcpCommandExecutor _commandExecutor;
+
+        private Trajectory2D _trajectory;
+
+        /// <summary>
+        /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
+        /// </summary>
+        public ParkingGeometricController(
+            IVehicleStateProvider stateProvider,
+            ILateralController lateralController,
+            ILongitudinalController longitudinalController,
+            GcpCommandAllocator gcpAllocator,
+            GcpCommandExecutor commandExecutor,
+            double finishDistanceMeters = 0.04,
+            double finishSpeedMetersPerSecond = 0.02,
+            double finishHeadingToleranceRadians =
+                3.0 * Math.PI / 180.0,
+            double maximumDistanceToTrajectoryMeters = 0.30)
+        {
+            _stateProvider = stateProvider ??
+                throw new ArgumentNullException(
+                    nameof(stateProvider));
+            _lateralController = lateralController ??
+                throw new ArgumentNullException(
+                    nameof(lateralController));
+            _longitudinalController = longitudinalController ??
+                throw new ArgumentNullException(
+                    nameof(longitudinalController));
+            _gcpAllocator = gcpAllocator ??
+                throw new ArgumentNullException(
+                    nameof(gcpAllocator));
+            _commandExecutor = commandExecutor ??
+                throw new ArgumentNullException(
+                    nameof(commandExecutor));
+
+            EnsureFinitePositive(
+                finishDistanceMeters,
+                nameof(finishDistanceMeters));
+            EnsureFiniteNonNegative(
+                finishSpeedMetersPerSecond,
+                nameof(finishSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                finishHeadingToleranceRadians,
+                nameof(finishHeadingToleranceRadians));
+            EnsureFinitePositive(
+                maximumDistanceToTrajectoryMeters,
+                nameof(maximumDistanceToTrajectoryMeters));
+
+            FinishDistanceMeters = finishDistanceMeters;
+            FinishSpeedMetersPerSecond =
+                finishSpeedMetersPerSecond;
+            FinishHeadingToleranceRadians =
+                finishHeadingToleranceRadians;
+            MaximumDistanceToTrajectoryMeters =
+                maximumDistanceToTrajectoryMeters;
+        }
+
+        /// <summary>
+        /// 获取终点位置和剩余弧长允许的误差，单位为m。
+        /// </summary>
+        public double FinishDistanceMeters { get; }
+
+        /// <summary>
+        /// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
+        /// </summary>
+        public double FinishSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。
+        /// </summary>
+        public double FinishHeadingToleranceRadians { get; }
+
+        /// <summary>
+        /// 获取允许车辆偏离参考轨迹的最大距离，单位为m。
+        /// </summary>
+        public double MaximumDistanceToTrajectoryMeters { get; }
+
+        /// <summary>
+        /// 获取控制器当前是否持有并正在执行一条轨迹。
+        /// </summary>
+        public bool IsActive { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次轨迹是否已经满足终点完成条件。
+        /// </summary>
+        public bool IsCompleted { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次控制失败原因，正常时为空字符串。
+        /// </summary>
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 获取最近一次控制异常，正常时为空。
+        /// </summary>
+        public Exception LastException { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次有效车辆状态。
+        /// </summary>
+        public VehicleState? LastVehicleState { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次车体中心到参考轨迹的投影结果。
+        /// </summary>
+        public TrajectoryProjection? LastProjection { get; private set; }
+
+        /// <summary>
+        /// 获取最近一次发送或准备发送的GCP运动命令。
+        /// </summary>
+        public GcpMotionCommand? LastCommand { get; private set; }
+
+        /// <summary>
+        /// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
+        /// </summary>
+        public double? LastReferenceSpeedMetersPerSecond { get; private set; }
+
+        /// <summary>
+        /// 停止当前底盘并从起点开始执行指定二维轨迹。
+        /// </summary>
+        public void Start(Trajectory2D trajectory)
+        {
+            if (trajectory == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(trajectory));
+            }
+
+            StopAndResetControllers();
+            _trajectory = trajectory;
+            IsActive = true;
+            IsCompleted = false;
+            ClearDiagnostics();
+        }
+
+        /// <summary>
+        /// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。
+        /// </summary>
+        public ParkingControlCycleResult ExecuteCycle(
+            double deltaTimeSeconds)
+        {
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            if (!IsActive || _trajectory == null)
+            {
+                return ParkingControlCycleResult.Inactive;
+            }
+
+            try
+            {
+                if (!_stateProvider.TryGetState(
+                        out var vehicleState))
+                {
+                    StopForUnavailableState();
+                    return ParkingControlCycleResult
+                        .StateUnavailable;
+                }
+
+                LastVehicleState = vehicleState;
+
+                var projection = TrajectoryProjector.Project(
+                    _trajectory,
+                    vehicleState.PoseInWorld);
+                LastProjection = projection;
+
+                if (projection.DistanceToTrajectoryMeters >
+                    MaximumDistanceToTrajectoryMeters)
+                {
+                    return EnterFault(
+                        "车辆距离参考轨迹" +
+                        $"{projection.DistanceToTrajectoryMeters:F3}m，" +
+                        "超过允许值" +
+                        $"{MaximumDistanceToTrajectoryMeters:F3}m。");
+                }
+
+                if (HasReachedEnd(
+                        vehicleState,
+                        projection))
+                {
+                    CompleteTrajectory();
+                    return ParkingControlCycleResult.Completed;
+                }
+
+                if (HasStoppedAtUnsatisfiedTerminal(
+                        vehicleState,
+                        projection,
+                        out var terminalFailureReason))
+                {
+                    return EnterFault(
+                        terminalFailureReason);
+                }
+
+                var referenceSpeedMetersPerSecond =
+                    ResolveReferenceSpeedForControl(
+                        projection);
+                LastReferenceSpeedMetersPerSecond =
+                    referenceSpeedMetersPerSecond;
+                var context = new PathTrackingContext(
+                    vehicleState,
+                    projection,
+                    referenceSpeedMetersPerSecond,
+                    deltaTimeSeconds);
+                var lateralCommand =
+                    _lateralController.Compute(context);
+                var commandSpeedMetersPerSecond =
+                    _longitudinalController
+                        .ComputeSpeedMetersPerSecond(context);
+                var gcpCommand = _gcpAllocator.Allocate(
+                    commandSpeedMetersPerSecond,
+                    lateralCommand);
+
+                if (!_commandExecutor.Execute(
+                        gcpCommand,
+                        deltaTimeSeconds))
+                {
+                    return EnterFault(
+                        string.IsNullOrWhiteSpace(
+                            _commandExecutor.LastFailureReason)
+                            ? "GCP底盘命令执行失败。"
+                            : _commandExecutor.LastFailureReason);
+                }
+
+                LastCommand =
+                    _commandExecutor.LastSentCommand;
+
+                LastFailureReason = string.Empty;
+                LastException = null;
+                return ParkingControlCycleResult.CommandSent;
+            }
+            catch (Exception exception)
+            {
+                return EnterFault(
+                    "停车机器人轨迹控制周期异常：" +
+                    exception.Message,
+                    exception);
+            }
+        }
+
+        /// <summary>
+        /// 主动取消当前轨迹、立即停车并清除全部控制器状态。
+        /// </summary>
+        public void Cancel()
+        {
+            StopAndResetControllers();
+            _trajectory = null;
+            IsActive = false;
+            IsCompleted = false;
+            ClearDiagnostics();
+        }
+
+        /// <summary>
+        /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
+        /// </summary>
+        private double ResolveReferenceSpeedForControl(
+            TrajectoryProjection projection)
+        {
+            var currentReferenceSpeed =
+                projection.ReferencePoint
+                    .ReferenceSpeedMetersPerSecond;
+
+            var requiresStartupRelease =
+                projection.ArcLengthMeters <=
+                    StartupRegionMeters &&
+                projection.RemainingDistanceMeters >
+                    FinishDistanceMeters &&
+                Math.Abs(currentReferenceSpeed) <=
+                    ZeroReferenceSpeedToleranceMetersPerSecond;
+
+            if (!requiresStartupRelease)
+            {
+                return currentReferenceSpeed;
+            }
+
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                StartupPreviewDistanceMeters);
+            var previewReferenceSpeed =
+                _trajectory
+                    .SampleAtArcLength(
+                        previewArcLengthMeters)
+                    .ReferenceSpeedMetersPerSecond;
+
+            if (Math.Abs(previewReferenceSpeed) <=
+                ZeroReferenceSpeedToleranceMetersPerSecond)
+            {
+                return 0.0;
+            }
+
+            return Math.Sign(previewReferenceSpeed) *
+                   Math.Min(
+                       Math.Abs(previewReferenceSpeed),
+                       MaximumStartupSpeedMetersPerSecond);
+        }
+
+        /// <summary>
+        /// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
+        /// </summary>
+        private bool HasReachedEnd(
+            VehicleState vehicleState,
+            TrajectoryProjection projection)
+        {
+            if (!vehicleState.HasValidVelocityEstimate)
+            {
+                return false;
+            }
+
+            return projection.RemainingDistanceMeters <=
+                       FinishDistanceMeters &&
+                   CalculateDistanceToEndMeters(
+                       vehicleState) <=
+                       FinishDistanceMeters &&
+                   CalculateHeadingErrorToEndRadians(
+                       vehicleState) <=
+                       FinishHeadingToleranceRadians &&
+                   CalculateActualLinearSpeedMetersPerSecond(
+                       vehicleState) <=
+                       FinishSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 检查车辆是否已在终点零速参考处停稳但最终位置或航向仍不合格。
+        /// </summary>
+        private bool HasStoppedAtUnsatisfiedTerminal(
+            VehicleState vehicleState,
+            TrajectoryProjection projection,
+            out string failureReason)
+        {
+            failureReason = string.Empty;
+
+            var isTerminalZeroSpeedReference =
+                projection.RemainingDistanceMeters <=
+                    FinishDistanceMeters &&
+                Math.Abs(
+                    projection.ReferencePoint
+                        .ReferenceSpeedMetersPerSecond) <=
+                    ZeroReferenceSpeedToleranceMetersPerSecond;
+
+            if (!isTerminalZeroSpeedReference ||
+                !vehicleState.HasValidVelocityEstimate ||
+                CalculateActualLinearSpeedMetersPerSecond(
+                    vehicleState) >
+                FinishSpeedMetersPerSecond)
+            {
+                return false;
+            }
+
+            var positionErrorMeters =
+                CalculateDistanceToEndMeters(
+                    vehicleState);
+            var headingErrorRadians =
+                CalculateHeadingErrorToEndRadians(
+                    vehicleState);
+
+            failureReason =
+                "车辆已在终点零速参考处停稳，但终点精度不满足要求：" +
+                $"位置误差={positionErrorMeters:F3}m，" +
+                "航向误差=" +
+                $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
+            return true;
+        }
+
+        /// <summary>
+        /// 计算实际车体中心到轨迹终点的欧氏距离，单位为m。
+        /// </summary>
+        private double CalculateDistanceToEndMeters(
+            VehicleState vehicleState)
+        {
+            var endPoint = _trajectory.EndPoint.PoseInWorld;
+            var deltaX =
+                vehicleState.PoseInWorld.XMeters -
+                endPoint.XMeters;
+            var deltaY =
+                vehicleState.PoseInWorld.YMeters -
+                endPoint.YMeters;
+
+            return Math.Sqrt(
+                deltaX * deltaX +
+                deltaY * deltaY);
+        }
+
+        /// <summary>
+        /// 计算实际车体航向到轨迹终点航向的最短角度误差绝对值，单位为rad。
+        /// </summary>
+        private double CalculateHeadingErrorToEndRadians(
+            VehicleState vehicleState)
+        {
+            return Math.Abs(
+                AngleMath.ShortestDifferenceRadians(
+                    _trajectory.EndPoint
+                        .PoseInWorld.YawRadians,
+                    vehicleState
+                        .PoseInWorld.YawRadians));
+        }
+
+        /// <summary>
+        /// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
+        /// </summary>
+        private static double CalculateActualLinearSpeedMetersPerSecond(
+            VehicleState vehicleState)
+        {
+            return Math.Sqrt(
+                vehicleState.TwistInBody.VxMetersPerSecond *
+                vehicleState.TwistInBody.VxMetersPerSecond +
+                vehicleState.TwistInBody.VyMetersPerSecond *
+                vehicleState.TwistInBody.VyMetersPerSecond);
+        }
+
+        /// <summary>
+        /// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。
+        /// </summary>
+        private void StopForUnavailableState()
+        {
+            _commandExecutor.Stop();
+            _lateralController.Reset();
+            _longitudinalController.Reset();
+            LastCommand = null;
+            LastFailureReason =
+                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
+            LastException = null;
+        }
+
+        /// <summary>
+        /// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。
+        /// </summary>
+        private void CompleteTrajectory()
+        {
+            StopAndResetControllers();
+            IsActive = false;
+            IsCompleted = true;
+            LastCommand = new GcpMotionCommand(
+                0.0,
+                0.0,
+                0.0);
+            LastFailureReason = string.Empty;
+            LastException = null;
+        }
+
+        /// <summary>
+        /// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。
+        /// </summary>
+        private ParkingControlCycleResult EnterFault(
+            string reason,
+            Exception exception = null)
+        {
+            StopAndResetControllers();
+            IsActive = false;
+            IsCompleted = false;
+            LastCommand = null;
+            LastFailureReason = reason;
+            LastException = exception;
+            return ParkingControlCycleResult.Faulted;
+        }
+
+        /// <summary>
+        /// 立即停止底盘并清除横向和纵向控制器的跨周期状态。
+        /// </summary>
+        private void StopAndResetControllers()
+        {
+            _commandExecutor.Stop();
+            _lateralController.Reset();
+            _longitudinalController.Reset();
+        }
+
+        /// <summary>
+        /// 清除上一条轨迹留下的状态、命令和故障诊断信息。
+        /// </summary>
+        private void ClearDiagnostics()
+        {
+            LastVehicleState = null;
+            LastProjection = null;
+            LastCommand = null;
+            LastReferenceSpeedMetersPerSecond = null;
+            LastFailureReason = string.Empty;
+            LastException = null;
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹控制器距离和周期参数必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹控制器速度参数必须是非负有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Lateral/StanleyLateralController.cs

@@ -0,0 +1,250 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+
+namespace MultiWheelC.Control.Lateral
+{
+    /// <summary>
+    /// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。
+    /// </summary>
+    public sealed class StanleyLateralController : ILateralController
+    {
+        /// <summary>
+        /// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。
+        /// </summary>
+        public StanleyLateralController(
+            double controlPointRadiusMeters,
+            double crossTrackGainPerSecond,
+            double headingErrorGain,
+            double minimumSpeedMetersPerSecond,
+            bool useActualSpeedForGain = true,
+            double maximumCrossTrackCorrectionRadians =
+                10.0 * Math.PI / 180.0,
+            double maximumHeadingCorrectionRadians =
+                10.0 * Math.PI / 180.0)
+        {
+            EnsureFinitePositive(
+                controlPointRadiusMeters,
+                nameof(controlPointRadiusMeters));
+            EnsureFiniteNonNegative(
+                crossTrackGainPerSecond,
+                nameof(crossTrackGainPerSecond));
+            EnsureFiniteNonNegative(
+                headingErrorGain,
+                nameof(headingErrorGain));
+            EnsureFinitePositive(
+                minimumSpeedMetersPerSecond,
+                nameof(minimumSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                maximumCrossTrackCorrectionRadians,
+                nameof(maximumCrossTrackCorrectionRadians));
+            EnsureFinitePositive(
+                maximumHeadingCorrectionRadians,
+                nameof(maximumHeadingCorrectionRadians));
+
+            ControlPointRadiusMeters = controlPointRadiusMeters;
+            CrossTrackGainPerSecond = crossTrackGainPerSecond;
+            HeadingErrorGain = headingErrorGain;
+            MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
+            UseActualSpeedForGain = useActualSpeedForGain;
+            MaximumCrossTrackCorrectionRadians =
+                maximumCrossTrackCorrectionRadians;
+            MaximumHeadingCorrectionRadians =
+                maximumHeadingCorrectionRadians;
+        }
+
+        /// <summary>
+        /// 获取车体中心到前、后GCP的距离，单位为m。
+        /// </summary>
+        public double ControlPointRadiusMeters { get; }
+
+        /// <summary>
+        /// 获取横向误差增益，单位为1/s。
+        /// </summary>
+        public double CrossTrackGainPerSecond { get; }
+
+        /// <summary>
+        /// 获取航向误差的无量纲增益。
+        /// </summary>
+        public double HeadingErrorGain { get; }
+
+        /// <summary>
+        /// 获取Stanley分母使用的最小速度绝对值，单位为m/s。
+        /// </summary>
+        public double MinimumSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
+        /// </summary>
+        public bool UseActualSpeedForGain { get; }
+
+        /// <summary>
+        /// 获取横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians { get; }
+
+        /// <summary>
+        /// 获取航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians { get; }
+
+        /// <summary>
+        /// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。
+        /// </summary>
+        public LateralControlCommand Compute(
+            PathTrackingContext context)
+        {
+            var speedForGain = SelectSpeedForGain(context);
+            var speedMagnitude = Math.Max(
+                Math.Abs(speedForGain),
+                MinimumSpeedMetersPerSecond);
+            var travelDirection = SelectTravelDirection(context);
+
+            // 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
+            var feedforwardAngleRadians = Math.Atan(
+                context.ReferenceCurvaturePerMeter *
+                ControlPointRadiusMeters);
+
+            // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
+            var crossTrackCorrectionRadians =
+                ClampSymmetric(
+                    Math.Atan(
+                        CrossTrackGainPerSecond *
+                        context.LateralErrorMeters /
+                        speedMagnitude),
+                    MaximumCrossTrackCorrectionRadians);
+
+            // 航向误差生成前后反向的差动转角，只负责调整车身朝向。
+            var headingCorrectionRadians =
+                ClampSymmetric(
+                    HeadingErrorGain *
+                    context.HeadingErrorRadians,
+                    MaximumHeadingCorrectionRadians);
+
+            var commonAngleRadians =
+                travelDirection *
+                crossTrackCorrectionRadians;
+            var differentialAngleRadians =
+                feedforwardAngleRadians +
+                travelDirection *
+                headingCorrectionRadians;
+
+            return new LateralControlCommand(
+                commonAngleRadians +
+                differentialAngleRadians,
+                commonAngleRadians -
+                differentialAngleRadians);
+        }
+
+        /// <summary>
+        /// 清除横向控制器状态；当前Stanley实现没有跨周期状态。
+        /// </summary>
+        public void Reset()
+        {
+        }
+
+        /// <summary>
+        /// 选择Stanley横向误差项使用的实际速度或参考速度。
+        /// </summary>
+        private double SelectSpeedForGain(
+            PathTrackingContext context)
+        {
+            if (UseActualSpeedForGain &&
+                context.HasValidVelocityEstimate)
+            {
+                return context
+                    .ActualLongitudinalSpeedMetersPerSecond;
+            }
+
+            return context.ReferenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 根据有符号参考速度确定前进或倒车时的反馈修正方向。
+        /// </summary>
+        private static double SelectTravelDirection(
+            PathTrackingContext context)
+        {
+            const double directionDeadbandMetersPerSecond = 1e-6;
+
+            if (Math.Abs(context.ReferenceSpeedMetersPerSecond) >
+                directionDeadbandMetersPerSecond)
+            {
+                return Math.Sign(
+                    context.ReferenceSpeedMetersPerSecond);
+            }
+
+            if (context.HasValidVelocityEstimate &&
+                Math.Abs(
+                    context.ActualLongitudinalSpeedMetersPerSecond) >
+                directionDeadbandMetersPerSecond)
+            {
+                return Math.Sign(
+                    context.ActualLongitudinalSpeedMetersPerSecond);
+            }
+
+            return 1.0;
+        }
+
+        /// <summary>
+        /// 将数值按正负对称方式限制在指定绝对值内。
+        /// </summary>
+        private static double ClampSymmetric(
+            double value,
+            double maximumAbsoluteValue)
+        {
+            return Math.Max(
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, value));
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制器的几何尺寸、速度和角度限制必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制增益是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制增益必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制参数必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Control/Longitudinal/PidLongitudinalController.cs

@@ -0,0 +1,224 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Common;
+
+namespace MultiWheelC.Control.Longitudinal
+{
+    /// <summary>
+    /// 将轨迹参考速度前馈与通用PID速度反馈组合为有符号底盘命令速度。
+    /// </summary>
+    public sealed class PidLongitudinalController
+        : ILongitudinalController
+    {
+        private const double ReferenceStopDeadbandMetersPerSecond =
+            1e-6;
+
+        private readonly PidController _feedbackPid;
+
+        /// <summary>
+        /// 创建具有积分抗饱和和命令速度限幅的纵向速度外环。
+        /// </summary>
+        public PidLongitudinalController(
+            double proportionalGain,
+            double integralGainPerSecond,
+            double derivativeGainSeconds,
+            double maximumIntegralCorrectionMetersPerSecond,
+            double maximumCommandSpeedMetersPerSecond,
+            double speedErrorDeadbandMetersPerSecond = 0.025)
+        {
+            EnsureFinitePositive(
+                maximumCommandSpeedMetersPerSecond,
+                nameof(maximumCommandSpeedMetersPerSecond));
+            EnsureFiniteNonNegative(
+                speedErrorDeadbandMetersPerSecond,
+                nameof(speedErrorDeadbandMetersPerSecond));
+
+            _feedbackPid = new PidController(
+                proportionalGain,
+                integralGainPerSecond,
+                derivativeGainSeconds,
+                maximumIntegralCorrectionMetersPerSecond,
+                derivativeOnMeasurement: true);
+            MaximumCommandSpeedMetersPerSecond =
+                maximumCommandSpeedMetersPerSecond;
+            SpeedErrorDeadbandMetersPerSecond =
+                speedErrorDeadbandMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 获取负责计算速度误差修正量的通用PID控制器。
+        /// </summary>
+        public PidController FeedbackPid => _feedbackPid;
+
+        /// <summary>
+        /// 获取底盘命令速度的最大绝对值，单位为m/s。
+        /// </summary>
+        public double MaximumCommandSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double SpeedErrorDeadbandMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
+        /// </summary>
+        public double LastSpeedErrorMetersPerSecond =>
+            _feedbackPid.LastError;
+
+        /// <summary>
+        /// 获取最近一次比例项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastProportionalCorrectionMetersPerSecond =>
+            _feedbackPid.LastProportionalOutput;
+
+        /// <summary>
+        /// 获取最近一次积分项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastIntegralCorrectionMetersPerSecond =>
+            _feedbackPid.LastIntegralOutput;
+
+        /// <summary>
+        /// 获取最近一次微分项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastDerivativeCorrectionMetersPerSecond =>
+            _feedbackPid.LastDerivativeOutput;
+
+        /// <summary>
+        /// 根据轨迹参考速度和Detour实际纵向速度计算底盘命令速度。
+        /// </summary>
+        public double ComputeSpeedMetersPerSecond(
+            PathTrackingContext context)
+        {
+            var referenceSpeedMetersPerSecond =
+                context.ReferenceSpeedMetersPerSecond;
+
+            // 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
+            if (Math.Abs(referenceSpeedMetersPerSecond) <=
+                ReferenceStopDeadbandMetersPerSecond)
+            {
+                Reset();
+                return 0.0;
+            }
+
+            // 定位速度尚不可用时只透传参考速度，不使用无效反馈更新PID状态。
+            if (!context.HasValidVelocityEstimate)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    referenceSpeedMetersPerSecond);
+            }
+
+            var speedErrorMetersPerSecond =
+                referenceSpeedMetersPerSecond -
+                context.ActualLongitudinalSpeedMetersPerSecond;
+
+            // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
+            // 同时清除PID历史，避免噪声持续积累后产生突发修正。
+            if (Math.Abs(speedErrorMetersPerSecond) <=
+                SpeedErrorDeadbandMetersPerSecond)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    referenceSpeedMetersPerSecond);
+            }
+
+            GetCorrectionOutputRange(
+                referenceSpeedMetersPerSecond,
+                out var minimumCorrectionMetersPerSecond,
+                out var maximumCorrectionMetersPerSecond);
+
+            var correctionMetersPerSecond =
+                _feedbackPid.Update(
+                    referenceSpeedMetersPerSecond,
+                    context
+                        .ActualLongitudinalSpeedMetersPerSecond,
+                    context.DeltaTimeSeconds,
+                    minimumCorrectionMetersPerSecond,
+                    maximumCorrectionMetersPerSecond);
+
+            return referenceSpeedMetersPerSecond +
+                   correctionMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 清除纵向速度外环的积分、历史测量值和诊断输出。
+        /// </summary>
+        public void Reset()
+        {
+            _feedbackPid.Reset();
+        }
+
+        /// <summary>
+        /// 根据参考行驶方向计算PID修正量允许使用的动态输出范围。
+        /// </summary>
+        private void GetCorrectionOutputRange(
+            double referenceSpeedMetersPerSecond,
+            out double minimumCorrectionMetersPerSecond,
+            out double maximumCorrectionMetersPerSecond)
+        {
+            if (referenceSpeedMetersPerSecond > 0.0)
+            {
+                minimumCorrectionMetersPerSecond =
+                    -referenceSpeedMetersPerSecond;
+                maximumCorrectionMetersPerSecond =
+                    MaximumCommandSpeedMetersPerSecond -
+                    referenceSpeedMetersPerSecond;
+                return;
+            }
+
+            minimumCorrectionMetersPerSecond =
+                -MaximumCommandSpeedMetersPerSecond -
+                referenceSpeedMetersPerSecond;
+            maximumCorrectionMetersPerSecond =
+                -referenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 在没有有效速度反馈时限制参考速度的绝对值。
+        /// </summary>
+        private double LimitReferenceSpeed(
+            double referenceSpeedMetersPerSecond)
+        {
+            return Math.Max(
+                -MaximumCommandSpeedMetersPerSecond,
+                Math.Min(
+                    MaximumCommandSpeedMetersPerSecond,
+                    referenceSpeedMetersPerSecond));
+        }
+
+        /// <summary>
+        /// 检查最大命令速度是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器最大命令速度必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查速度误差死区是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器速度误差死区必须是非负有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Movements/TrajectoryTrackingMovement.cs

@@ -0,0 +1,306 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.Control.Lateral;
+using MultiWheelC.Control.Longitudinal;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 使用新版横纵向控制器持续跟踪一条世界坐标系二维轨迹。
+    /// </summary>
+    public sealed class TrajectoryTrackingMovement
+        : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
+        /// </summary>
+        public Trajectory2D Trajectory;
+
+        /// <summary>
+        /// 获取或设置本次动作使用的车辆状态源；为空时自动创建Detour状态源。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
+        /// </summary>
+        public Action<ParkingGeometricController> CycleObserver;
+
+        /// <summary>
+        /// Stanley横向误差增益，单位为1/s。
+        /// </summary>
+        public double StanleyCrossTrackGainPerSecond = 0.4;
+
+        /// <summary>
+        /// Stanley航向误差增益。
+        /// </summary>
+        public double StanleyHeadingErrorGain = 1.0;
+
+        /// <summary>
+        /// Stanley低速分母保护速度，单位为m/s。
+        /// </summary>
+        public double StanleyMinimumSpeedMetersPerSecond = 0.15;
+
+        /// <summary>
+        /// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
+        /// </summary>
+        public bool StanleyUsesActualSpeed = true;
+
+        /// <summary>
+        /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// 纵向速度外环比例增益。
+        /// </summary>
+        public double LongitudinalKp = 0.5;
+
+        /// <summary>
+        /// 纵向速度外环积分增益，单位为1/s。
+        /// </summary>
+        public double LongitudinalKiPerSecond;
+
+        /// <summary>
+        /// 纵向速度外环微分增益，单位为s。
+        /// </summary>
+        public double LongitudinalKdSeconds;
+
+        /// <summary>
+        /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
+        /// </summary>
+        public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
+
+        /// <summary>
+        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
+            0.025;
+
+        /// <summary>
+        /// 底盘纵向命令速度绝对值上限，单位为m/s。
+        /// </summary>
+        public double MaximumCommandSpeedMetersPerSecond = 0.50;
+
+        /// <summary>
+        /// 前后GCP允许的最大转角绝对值，单位为rad。
+        /// </summary>
+        public double MaximumGcpAngleRadians =
+            AngleMath.DegreesToRadians(45.0);
+
+        /// <summary>
+        /// 前后GCP目标转角最大变化率，单位为rad/s。
+        /// </summary>
+        public double MaximumGcpAngleRateRadiansPerSecond =
+            AngleMath.DegreesToRadians(15.0);
+
+        /// <summary>
+        /// 终点位置和剩余弧长的完成容差，单位为m。
+        /// </summary>
+        public double FinishDistanceMeters = 0.03;
+
+        /// <summary>
+        /// 终点停稳判定允许的实际线速度，单位为m/s。
+        /// </summary>
+        public double FinishSpeedMetersPerSecond = 0.02;
+
+        /// <summary>
+        /// 终点航向完成容差，单位为rad。
+        /// </summary>
+        public double FinishHeadingToleranceRadians =
+            AngleMath.DegreesToRadians(3.0);
+
+        /// <summary>
+        /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
+        /// </summary>
+        public double MaximumDistanceToTrajectoryMeters = 0.30;
+
+        /// <summary>
+        /// 单次轨迹动作允许的最长执行时间，单位为s。
+        /// </summary>
+        public double ExecutionTimeoutSeconds = 120.0;
+
+        /// <summary>
+        /// 获取本次动作创建的控制器，尚未开始时为空。
+        /// </summary>
+        public ParkingGeometricController Controller { get; private set; }
+
+        /// <summary>
+        /// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            ValidateParameters();
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+
+            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
+            adapter.ResetToBodyFrame();
+
+            var stateProvider =
+                StateProvider ??
+                new DetourVehicleStateProvider();
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius / 1000.0;
+
+            var lateralController =
+                new StanleyLateralController(
+                    controlPointRadiusMeters,
+                    StanleyCrossTrackGainPerSecond,
+                    StanleyHeadingErrorGain,
+                    StanleyMinimumSpeedMetersPerSecond,
+                    StanleyUsesActualSpeed,
+                    MaximumCrossTrackCorrectionRadians,
+                    MaximumHeadingCorrectionRadians);
+            var longitudinalController =
+                new PidLongitudinalController(
+                    LongitudinalKp,
+                    LongitudinalKiPerSecond,
+                    LongitudinalKdSeconds,
+                    MaximumIntegralCorrectionMetersPerSecond,
+                    MaximumCommandSpeedMetersPerSecond,
+                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
+            var gcpAllocator =
+                new GcpCommandAllocator(
+                    MaximumGcpAngleRadians);
+            var commandExecutor =
+                new GcpCommandExecutor(
+                    adapter,
+                    MaximumGcpAngleRateRadiansPerSecond);
+
+            Controller = new ParkingGeometricController(
+                stateProvider,
+                lateralController,
+                longitudinalController,
+                gcpAllocator,
+                commandExecutor,
+                FinishDistanceMeters,
+                FinishSpeedMetersPerSecond,
+                FinishHeadingToleranceRadians,
+                MaximumDistanceToTrajectoryMeters);
+
+            var clock = Stopwatch.StartNew();
+            var previousCycleSeconds =
+                clock.Elapsed.TotalSeconds;
+            Controller.Start(Trajectory);
+
+            try
+            {
+                while (true)
+                {
+                    if (clock.Elapsed.TotalSeconds >
+                        ExecutionTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。");
+                    }
+
+                    var currentCycleSeconds =
+                        clock.Elapsed.TotalSeconds;
+                    var deltaTimeSeconds =
+                        currentCycleSeconds -
+                        previousCycleSeconds;
+                    previousCycleSeconds =
+                        currentCycleSeconds;
+
+                    // 极短首周期不参与PID和GCP角速度限制，等待调度器进入下一周期。
+                    if (deltaTimeSeconds <= 1e-6)
+                    {
+                        yield return true;
+                        continue;
+                    }
+
+                    var result =
+                        Controller.ExecuteCycle(
+                            deltaTimeSeconds);
+
+                    if (Controller.LastVehicleState.HasValue)
+                    {
+                        CycleObserver?.Invoke(Controller);
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Completed)
+                    {
+                        break;
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Faulted)
+                    {
+                        throw new InvalidOperationException(
+                            string.IsNullOrWhiteSpace(
+                                Controller.LastFailureReason)
+                                ? "新版轨迹跟踪控制器发生未知故障。"
+                                : Controller.LastFailureReason,
+                            Controller.LastException);
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Inactive)
+                    {
+                        throw new InvalidOperationException(
+                            "新版轨迹跟踪控制器在轨迹完成前意外停止活动。");
+                    }
+
+                    // CommandSent和短暂StateUnavailable均继续下一控制周期；
+                    // 后者已经由控制器主动停车，等待Detour恢复。
+                    yield return true;
+                }
+            }
+            finally
+            {
+                Controller.Cancel();
+            }
+
+            yield return false;
+        }
+
+        /// <summary>
+        /// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
+        /// </summary>
+        private void ValidateParameters()
+        {
+            if (Trajectory == null)
+            {
+                throw new InvalidOperationException(
+                    "新版轨迹跟踪动作没有设置Trajectory。");
+            }
+
+            if (double.IsNaN(ExecutionTimeoutSeconds) ||
+                double.IsInfinity(ExecutionTimeoutSeconds) ||
+                ExecutionTimeoutSeconds <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(ExecutionTimeoutSeconds),
+                    "轨迹跟踪超时时间必须是正有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -0,0 +1,576 @@
+// 将统一命令转换为原 Chassis API 调用
+using System;
+using CommonUsage.Chassis;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
+    /// 车体坐标系固定为X向前、Y向左、逆时针为正。
+    /// </summary>
+    public sealed class MultiWheelChassisAdapter
+    {
+        #region 辅助内容
+        private const double RadiansToDegrees = 180.0 / Math.PI;
+        private const float BiasTolerance = 0.001f;
+        private readonly MultiWheelChassis _chassis;
+        /// <summary>
+        /// 当前适配器对应的车辆编号。
+        /// </summary>
+        public int VehicleId { get; }
+
+        /// <summary>
+        /// Maximum distance from the body origin to a wheel center, in metres.
+        /// </summary>
+        public double MaximumWheelRadiusMeters { get; }
+
+        /// <summary>
+        /// Maximum longitudinal wheel offset from the body origin, in metres.
+        /// For a symmetric four-wheel-steering chassis this is half the wheelbase.
+        /// </summary>
+        public double HalfWheelBaseMeters { get; }
+
+        /// <summary>
+        /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
+        /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
+        /// </summary>
+        public double HalfTrackWidthMeters { get; }
+
+        /// <summary>
+        /// Width of the steering-alignment speed gate, in degrees.
+        /// </summary>
+        public double SteeringAlignmentSigmaDegrees
+        {
+            get => _chassis.SteeringAlignmentSigmaDegrees;
+            set
+            {
+                if (double.IsNaN(value) ||
+                    double.IsInfinity(value) ||
+                    value <= 0.0 ||
+                    value > float.MaxValue)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(value),
+                        "Steering alignment sigma must be a positive finite value.");
+                }
+
+                _chassis.SteeringAlignmentSigmaDegrees =
+                    (float)value;
+            }
+        }
+
+        /// <summary>
+        /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
+        /// </summary>
+        private void EnsureBodyFrameIsActive()
+        {
+            EnsureMotionFrameIsActive(0.0);
+        }
+
+        /// <summary>
+        /// 检查旧底盘当前是否处于指定的运动坐标系。
+        /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
+        /// </summary>
+        private void EnsureMotionFrameIsActive(
+            double motionDirectionRadians)
+        {
+            ValidateFinite(
+                motionDirectionRadians,
+                nameof(motionDirectionRadians));
+
+            var expectedBiasDegrees =
+                (float)(
+                    -FrameTransform2D.NormalizeAngle(
+                        motionDirectionRadians) *
+                    RadiansToDegrees);
+            var bias = _chassis.GetOriginBias();
+            var angleErrorDegrees =
+                NormalizeDegrees(
+                    bias.Z - expectedBiasDegrees);
+
+            if (Math.Abs(bias.X) <= BiasTolerance &&
+                Math.Abs(bias.Y) <= BiasTolerance &&
+                Math.Abs(angleErrorDegrees) <=
+                BiasTolerance)
+            {
+                return;
+            }
+
+            throw new InvalidOperationException(
+                "MultiWheelChassis当前运动坐标系与命令不一致。" +
+                $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" +
+                $"期望Th={expectedBiasDegrees}°。");
+        }
+
+        /// <summary>
+        /// 将角度归一化到[-180°,180°]附近。
+        /// </summary>
+        private static float NormalizeDegrees(float degrees)
+        {
+            return (float)(
+                degrees -
+                Math.Round(degrees / 360.0) * 360.0);
+        }
+        /// <summary>
+        /// 检查底盘命令是否包含无效数值。
+        /// </summary>
+        private static void ValidateTwist(Twist2D twist)
+        {
+            ValidateFinite(
+                twist.VxMetersPerSecond,
+                nameof(twist.VxMetersPerSecond));
+
+            ValidateFinite(
+                twist.VyMetersPerSecond,
+                nameof(twist.VyMetersPerSecond));
+
+            ValidateFinite(
+                twist.OmegaRadiansPerSecond,
+                nameof(twist.OmegaRadiansPerSecond));
+        }
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void ValidateFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "底盘速度命令不能是NaN或无穷大。");
+            }
+
+            if (value > float.MaxValue ||
+                value < -float.MaxValue)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "底盘速度命令超过float可表示范围。");
+            }
+        }
+
+        /// <summary>
+        /// 获取最近一次底盘运动分解失败原因。
+        /// </summary>
+        public string LastFailureReason =>
+            _chassis.LastMotionDecomposeFailureReason;
+
+        #endregion
+
+        /// <summary>
+        /// 将旧底盘的原点偏置恢复为真实单车车体坐标系。
+        /// </summary>
+        public void ResetToBodyFrame()
+        {
+            ActivateMotionFrame(0.0);
+        }
+
+        /// <summary>
+        /// 激活指定运动方向对应的SendMotion坐标系。
+        /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
+        /// </summary>
+        public void ActivateMotionFrame(
+            double motionDirectionRadians)
+        {
+            ValidateFinite(
+                motionDirectionRadians,
+                nameof(motionDirectionRadians));
+
+            var biasDegrees =
+                (float)(
+                    -FrameTransform2D.NormalizeAngle(
+                        motionDirectionRadians) *
+                    RadiansToDegrees);
+            var currentBias =
+                _chassis.GetOriginBias();
+
+            if (Math.Abs(currentBias.X) <=
+                    BiasTolerance &&
+                Math.Abs(currentBias.Y) <=
+                    BiasTolerance &&
+                Math.Abs(
+                    NormalizeDegrees(
+                        currentBias.Z -
+                        biasDegrees)) <=
+                    BiasTolerance)
+            {
+                return;
+            }
+
+            _chassis.SetOriginBias(
+                x: 0.0f,
+                y: 0.0f,
+                th: biasDegrees);
+        }
+        public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
+        {
+            _chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "车辆编号必须大于零。");
+            }
+            VehicleId = vehicleId;
+#pragma warning disable CS0612, CS0618
+            var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+            if (wheels.Count == 0)
+            {
+                throw new InvalidOperationException(
+                    "MultiWheelChassis尚未完成舵轮初始化，" +
+                    "不能创建底盘适配器。");
+            }
+            // 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
+            // 保证SendXYThSpeed直接使用真实车体坐标系。
+            var maximumWheelRadiusMillimeters = 0.0;
+            var maximumLongitudinalOffsetMillimeters = 0.0;
+            var maximumLateralOffsetMillimeters = 0.0;
+            foreach (var wheel in wheels)
+            {
+                maximumWheelRadiusMillimeters = Math.Max(
+                    maximumWheelRadiusMillimeters,
+                    wheel.PhysicalPosition.Length());
+
+                maximumLongitudinalOffsetMillimeters = Math.Max(
+                    maximumLongitudinalOffsetMillimeters,
+                    Math.Abs(wheel.PhysicalPosition.X));
+
+                maximumLateralOffsetMillimeters = Math.Max(
+                    maximumLateralOffsetMillimeters,
+                    Math.Abs(wheel.PhysicalPosition.Y));
+            }
+
+            MaximumWheelRadiusMeters =
+                maximumWheelRadiusMillimeters / 1000.0;
+            HalfWheelBaseMeters =
+                maximumLongitudinalOffsetMillimeters / 1000.0;
+            HalfTrackWidthMeters =
+                maximumLateralOffsetMillimeters / 1000.0;
+
+            if (MaximumWheelRadiusMeters <= 0.0 ||
+                HalfWheelBaseMeters <= 0.0 ||
+                HalfTrackWidthMeters <= 0.0)
+            {
+                throw new InvalidOperationException(
+                    "Wheel positions cannot produce valid chassis dimensions.");
+            }
+
+            // 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
+            _chassis.PreferMinimumSteeringTravel = true;
+        }
+
+        /// <summary>
+        /// 将车体坐标系速度命令发送给多舵轮底盘。
+        /// </summary>
+        public bool Send(ChassisCommand command, TimeSpan? interval = null)
+        {
+            if (command.VehicleId != VehicleId)
+            {
+                throw new InvalidOperationException(
+                    $"命令车辆编号{command.VehicleId}与适配器车辆编号" +
+                    $"{VehicleId}不一致。");
+            }
+            ValidateTwist(command.BodyTwist);
+            // 防止其他旧逻辑再次调用DirectionAngle或
+            // SetOriginBias改变底盘坐标语义。
+            EnsureBodyFrameIsActive();
+            var vxMetersPerSecond =
+                            (float)command.BodyTwist.VxMetersPerSecond;
+            var vyMetersPerSecond =
+                (float)command.BodyTwist.VyMetersPerSecond;
+            var omegaDegreesPerSecond =
+                (float)(
+                    command.BodyTwist.OmegaRadiansPerSecond *
+                    RadiansToDegrees);
+            var success = _chassis.SendXYThSpeed(
+                                vxMetersPerSecond,
+                                vyMetersPerSecond,
+                                omegaDegreesPerSecond,
+                                interval,
+                                enableDifferentialSteerFeedforward: true);
+            if (!success)
+            {
+                // 防止分解失败后继续执行上一条运动命令。
+                _chassis.PredefinedDriveStop();
+            }
+            return success;
+        }
+
+
+        /// <summary>
+        /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
+        /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
+        /// </summary>
+        public bool SendVirtualAckermannMotion(
+            double motionDirectionRadians,
+            double speedMetersPerSecond,
+            double steeringRadians,
+            TimeSpan? interval = null)
+        {
+            ValidateFinite(
+                motionDirectionRadians,
+                nameof(motionDirectionRadians));
+            ValidateFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+            ValidateFinite(
+                steeringRadians,
+                nameof(steeringRadians));
+            EnsureMotionFrameIsActive(
+                motionDirectionRadians);
+
+            if (Math.Abs(steeringRadians) >=
+                Math.PI / 2.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(steeringRadians),
+                    "虚拟阿克曼转向角必须位于正负90度以内。");
+            }
+
+            var steeringDegrees =
+                (float)(
+                    steeringRadians *
+                    RadiansToDegrees);
+            var success =
+                _chassis.SendMotion(
+                    (float)speedMetersPerSecond,
+                    steeringDegrees,
+                    -steeringDegrees,
+                    interval);
+
+            if (!success)
+            {
+                _chassis.PredefinedDriveStop();
+            }
+
+            return success;
+        }
+
+        /// <summary>
+        /// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
+        /// </summary>
+        public bool SendGcpMotion(
+            double speedMetersPerSecond,
+            double frontAngleRadians,
+            double rearAngleRadians,
+            TimeSpan? interval = null)
+        {
+            ValidateFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+            ValidateFinite(
+                frontAngleRadians,
+                nameof(frontAngleRadians));
+            ValidateFinite(
+                rearAngleRadians,
+                nameof(rearAngleRadians));
+            EnsureBodyFrameIsActive();
+
+            if (Math.Abs(frontAngleRadians) >=
+                    Math.PI / 2.0 ||
+                Math.Abs(rearAngleRadians) >=
+                    Math.PI / 2.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(frontAngleRadians),
+                    "前后GCP角度必须位于正负90度以内，避免四轮几何解算出现奇异值。");
+            }
+
+            var success = _chassis.SendMotion(
+                (float)speedMetersPerSecond,
+                (float)(frontAngleRadians *
+                        RadiansToDegrees),
+                (float)(rearAngleRadians *
+                        RadiansToDegrees),
+                interval);
+
+            if (!success)
+            {
+                // 分解失败后立即清除上一条驱动速度，避免车辆继续执行陈旧命令。
+                _chassis.PredefinedDriveStop();
+            }
+
+            return success;
+        }
+
+        /// <summary>
+        /// 立即将所有驱动轮速度下发为零。
+        /// </summary>
+        public void StopImmediately()
+        {
+            _chassis.PredefinedDriveStop();
+        }
+
+        /// <summary>
+        /// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。
+        /// </summary>
+        public void StopXYThDrivePreserveSteeringState()
+        {
+            _chassis.StopXYThDrivePreserveSteeringState();
+        }
+
+        /// <summary>
+        /// 停车并将所有舵轮转到指定的车体角度。
+        /// 只调整舵轮角度，不产生车辆线速度。
+        /// </summary>
+        public bool PrepareParallelDirection(
+            double directionRadians)
+        {
+            EnsureBodyFrameIsActive();
+            var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
+                                    RadiansToDegrees);
+
+#pragma warning disable CS0612, CS0618
+            var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+            // 没有舵轮时不能认为预对齐成功。
+            if (wheels.Count == 0)
+            {
+                return false;
+            }
+
+            // 先检查所有舵轮能否到达目标机械角度。
+            foreach (var wheel in wheels)
+            {
+                if (targetDegrees < wheel.AngleLowerLimit ||
+                    targetDegrees > wheel.AngleUpperLimit)
+                {
+                    return false;
+                }
+            }
+
+            // 模式切换前立即停止驱动轮。
+            _chassis.PredefinedDriveStop();
+            // 检查完成后再统一下发，避免只转动一部分舵轮。
+            foreach (var wheel in wheels)
+            {
+                wheel.WriteAngle(targetDegrees);
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 检查所有舵轮是否已经对准给定方向。
+        /// </summary>
+        public bool AreParallelWheelsAligned(
+            double directionRadians,
+            double toleranceRadians)
+        {
+            if (double.IsNaN(toleranceRadians) ||
+        double.IsInfinity(toleranceRadians) ||
+        toleranceRadians < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(toleranceRadians),
+                    "舵轮到位容差必须是非负有限值。");
+            }
+
+            EnsureBodyFrameIsActive();
+            var targetDegrees = (float)(
+                FrameTransform2D.NormalizeAngle(directionRadians) *
+                180.0 / Math.PI);
+
+            var toleranceDegrees = (float)(
+                Math.Abs(toleranceRadians) *
+                180.0 / Math.PI);
+
+#pragma warning disable CS0612, CS0618
+            var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+            foreach (var wheel in wheels)
+            {
+                var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
+
+                if (Math.Abs(angleErrorDegrees) >
+                    toleranceDegrees)
+                {
+                    return false;
+                }
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 停车并将舵轮预对齐到原地自转方向。
+        /// 返回是否成功生成舵轮目标。
+        /// </summary>
+        public bool PrepareSpin(
+            TimeSpan? interval = null,
+            double alignmentToleranceDegrees = 2.0)
+        {
+            ValidateFinite(
+                alignmentToleranceDegrees,
+                nameof(alignmentToleranceDegrees));
+
+            if (alignmentToleranceDegrees < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(alignmentToleranceDegrees),
+                    "自转舵轮到位容差必须是非负有限值。");
+            }
+
+            EnsureBodyFrameIsActive();
+
+            var success =
+                _chassis.PrepareRotateWheels(
+                    alignmentToleranceDegrees:
+                        (float)alignmentToleranceDegrees);
+
+            if (!success)
+            {
+                _chassis.PredefinedDriveStop();
+            }
+            return success;
+        }
+
+        /// <summary>
+        /// 将已到位的自转舵角和轮速方向一次性交接给XYTh，
+        /// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。
+        /// </summary>
+        public bool AdoptPreparedSpinForXYTh(
+            double toleranceRadians =
+                2.0 * Math.PI / 180.0)
+        {
+            if (double.IsNaN(toleranceRadians) ||
+                double.IsInfinity(toleranceRadians) ||
+                toleranceRadians < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(toleranceRadians),
+                    "自转状态交接容差必须是非负有限值。");
+            }
+
+            EnsureBodyFrameIsActive();
+
+            var success =
+                _chassis
+                    .AdoptPreparedRotateWheelsForXYTh(
+                        (float)(
+                            toleranceRadians *
+                            RadiansToDegrees));
+
+            if (!success)
+            {
+                _chassis.PredefinedDriveStop();
+            }
+
+            return success;
+        }
+        /// <summary>
+        /// 所有舵轮是否已对齐到原地自转方向。
+        /// </summary>
+        public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
+
+
+
+    }
+}

ClumsyPilot/Shared/Mathematics/AngleMath.cs

@@ -0,0 +1,138 @@
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供与坐标系无关的角度归一化、角度差和单位转换功能。
+    /// </summary>
+    public static class AngleMath
+    {
+        public const double TwoPi = 2.0 * Math.PI;
+
+        /// <summary>
+        /// 将弧度归一化到[-π, π)区间。
+        /// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。
+        /// </summary>
+        public static double NormalizeRadians(double angleRadians)
+        {
+            EnsureFinite(angleRadians, nameof(angleRadians));
+
+            var normalized = angleRadians % TwoPi;
+
+            if (normalized >= Math.PI)
+            {
+                normalized -= TwoPi;
+            }
+            else if (normalized < -Math.PI)
+            {
+                normalized += TwoPi;
+            }
+
+            return normalized == 0.0 ? 0.0 : normalized;
+        }
+
+        /// <summary>
+        /// 将角度归一化到[-180°, 180°)区间。
+        /// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。
+        /// </summary>
+        public static double NormalizeDegrees(double angleDegrees)
+        {
+            EnsureFinite(angleDegrees, nameof(angleDegrees));
+
+            var normalized = angleDegrees % 360.0;
+
+            if (normalized >= 180.0)
+            {
+                normalized -= 360.0;
+            }
+            else if (normalized < -180.0)
+            {
+                normalized += 360.0;
+            }
+
+            return normalized == 0.0 ? 0.0 : normalized;
+        }
+
+        /// <summary>
+        /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。
+        /// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。
+        /// </summary>
+        public static double ShortestDifferenceRadians(
+            double targetRadians,
+            double currentRadians)
+        {
+            EnsureFinite(targetRadians, nameof(targetRadians));
+            EnsureFinite(currentRadians, nameof(currentRadians));
+
+            return NormalizeRadians(targetRadians - currentRadians);
+        }
+
+        /// <summary>
+        /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。
+        /// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。
+        /// </summary>
+        public static double ShortestDifferenceDegrees(
+            double targetDegrees,
+            double currentDegrees)
+        {
+            EnsureFinite(targetDegrees, nameof(targetDegrees));
+            EnsureFinite(currentDegrees, nameof(currentDegrees));
+
+            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>
+        public static double DegreesToRadians(double angleDegrees)
+        {
+            EnsureFinite(angleDegrees, nameof(angleDegrees));
+            return angleDegrees * Math.PI / 180.0;
+        }
+
+        /// <summary>
+        /// 将角度从弧度转换为度，不进行归一化。
+        /// </summary>
+        public static double RadiansToDegrees(double angleRadians)
+        {
+            EnsureFinite(angleRadians, nameof(angleRadians));
+            return angleRadians * 180.0 / Math.PI;
+        }
+
+        /// <summary>
+        /// 验证角度是可用于计算的有限数值。
+        /// </summary>
+        private static void EnsureFinite(double angle, string parameterName)
+        {
+            if (double.IsNaN(angle) || double.IsInfinity(angle))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "角度必须是有限数值。");
+            }
+        }
+    }
+}

ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs

@@ -0,0 +1,165 @@
+// 车体、运动、车队坐标系之间的转换
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
+    /// 坐标系采用X向前、Y向左、逆时针为正的右手系。
+    /// </summary>
+    public static class FrameTransform2D
+    {
+        /// <summary>
+        /// 将角度归一化到[-π, π)范围。
+        /// </summary>
+        public static double NormalizeAngle(double angleRadians)
+        {
+            return AngleMath.NormalizeRadians(angleRadians);
+        }
+
+        /// <summary>
+        /// 计算从current到target的最短角度差。
+        /// 返回正值表示逆时针旋转。
+        /// </summary>
+        public static double ShortestAngleDifference(
+            double targetRadians,
+            double currentRadians)
+        {
+            return AngleMath.ShortestDifferenceRadians(
+                targetRadians,
+                currentRadians);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的点变换到目标坐标系。
+        /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
+        /// </summary>
+        public static Point2D TransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                sourcePoseInTarget.XMeters +
+                cos * pointInSource.XMeters -
+                sin * pointInSource.YMeters,
+
+                sourcePoseInTarget.YMeters +
+                sin * pointInSource.XMeters +
+                cos * pointInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 将目标坐标系中的点反向变换到源坐标系。
+        /// </summary>
+        public static Point2D InverseTransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInTarget)
+        {
+            var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
+
+            var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
+
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * dx + sin * dy,
+                -sin * dx + cos * dy);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的向量旋转到目标坐标系。
+        /// 向量没有位置，因此不叠加平移量。
+        /// </summary>
+        public static Point2D TransformVector(
+            Pose2D sourcePoseInTarget,
+            Point2D vectorInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * vectorInSource.XMeters -
+                sin * vectorInSource.YMeters,
+
+                sin * vectorInSource.XMeters +
+                cos * vectorInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 组合两级坐标变换。
+        /// parentFromMiddle表示middle在parent中的位姿；
+        /// middleFromChild表示child在middle中的位姿；
+        /// 返回child在parent中的位姿。
+        /// </summary>
+        public static Pose2D Compose(
+            Pose2D parentFromMiddle,
+            Pose2D middleFromChild)
+        {
+            var childPositionInParent = TransformPoint(
+                parentFromMiddle,
+                middleFromChild.Position);
+
+            return new Pose2D(
+                childPositionInParent.XMeters,
+                childPositionInParent.YMeters,
+                NormalizeAngle(
+                    parentFromMiddle.YawRadians +
+                    middleFromChild.YawRadians));
+        }
+
+        /// <summary>
+        /// 对坐标变换求逆。
+        /// 输入child在parent中的位姿，返回parent在child中的位姿。
+        /// </summary>
+        public static Pose2D Inverse(Pose2D childPoseInParent)
+        {
+            var cos = Math.Cos(childPoseInParent.YawRadians);
+            var sin = Math.Sin(childPoseInParent.YawRadians);
+
+            return new Pose2D(
+                -cos * childPoseInParent.XMeters -
+                sin * childPoseInParent.YMeters,
+
+                sin * childPoseInParent.XMeters -
+                cos * childPoseInParent.YMeters,
+
+                NormalizeAngle(
+                    -childPoseInParent.YawRadians));
+        }
+
+
+        /// <summary>
+        /// 将源坐标系中的位姿变换到目标坐标系。
+        /// </summary>
+        public static Pose2D TransformPose(
+            Pose2D sourcePoseInTarget,
+            Pose2D poseInSource)
+        {
+            return Compose(sourcePoseInTarget, poseInSource);
+        }
+
+        /// <summary>
+        /// 转换同一物理参考点处的速度表达坐标系。
+        /// 只旋转线速度，角速度保持不变。
+        /// </summary>
+        public static Twist2D TransformTwistAtSamePoint(
+            Pose2D sourcePoseInTarget,
+            Twist2D twistInSource)
+        {
+            var linearVelocityInTarget = TransformVector(
+                sourcePoseInTarget,
+                new Point2D(
+                    twistInSource.VxMetersPerSecond,
+                    twistInSource.VyMetersPerSecond));
+
+            return new Twist2D(
+                linearVelocityInTarget.XMeters,
+                linearVelocityInTarget.YMeters,
+                twistInSource.OmegaRadiansPerSecond);
+        }
+    }
+}

ClumsyPilot/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,
+                    "插值参数必须是有限数值。");
+            }
+        }
+    }
+}

ClumsyPilot/Shared/Models/ChassisCommand.cs

@@ -0,0 +1,183 @@
+// 纯数据层：只描述坐标、速度和命令
+// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
+// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
+// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
+// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 二维坐标点，X、Y单位均为米。
+    /// </summary>
+    public readonly struct Point2D
+    {
+        public Point2D(double xMeters, double yMeters)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public static Point2D Zero => new Point2D(0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 二维局部坐标系在父坐标系中的位姿。
+    /// 位置单位为米，朝向单位为弧度，逆时针为正。
+    /// 具体父子关系由变量名称说明，例如RadarPoseInBody。
+    /// </summary>
+    public readonly struct Pose2D
+    {
+        public Pose2D(
+            double xMeters,
+            double yMeters,
+            double yawRadians)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+            YawRadians = yawRadians;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public double YawRadians { get; }
+
+        public Point2D Position =>
+            new Point2D(XMeters, YMeters);
+
+        public static Pose2D Identity =>
+            new Pose2D(0.0, 0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 二维刚体速度。
+    /// 线速度单位为m/s，角速度单位为rad/s。
+    /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
+    /// </summary>
+    public readonly struct Twist2D
+    {
+        public Twist2D(
+            double vxMetersPerSecond,
+            double vyMetersPerSecond,
+            double omegaRadiansPerSecond)
+        {
+            VxMetersPerSecond = vxMetersPerSecond;
+            VyMetersPerSecond = vyMetersPerSecond;
+            OmegaRadiansPerSecond = omegaRadiansPerSecond;
+        }
+
+        public double VxMetersPerSecond { get; }
+
+        public double VyMetersPerSecond { get; }
+
+        public double OmegaRadiansPerSecond { get; }
+
+        public static Twist2D Zero =>
+            new Twist2D(0.0, 0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 发送给单辆车的车体坐标系速度命令。
+    /// </summary>
+    public readonly struct ChassisCommand
+    {
+        public ChassisCommand(
+            int vehicleId,
+            Twist2D bodyTwist)
+        {
+            VehicleId = vehicleId;
+            BodyTwist = bodyTwist;
+        }
+
+        public int VehicleId { get; }
+
+        /// <summary>
+        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
+        /// </summary>
+        public Twist2D BodyTwist { get; }
+
+        /// <summary>
+        /// 创建指定车辆的停止命令。
+        /// </summary>
+        public static ChassisCommand Stop(int vehicleId)
+        {
+            return new ChassisCommand(
+                vehicleId,
+                Twist2D.Zero);
+        }
+    }
+
+    /// <summary>
+    /// 单辆车的车体坐标系在车队坐标系中的位姿。
+    /// </summary>
+    public readonly struct VehicleLayout
+    {
+        public VehicleLayout(
+            int vehicleId,
+            Pose2D poseInFleet)
+        {
+            VehicleId = vehicleId;
+            PoseInFleet = poseInFleet;
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInFleet { get; }
+    }
+
+    /// <summary>
+    /// 车队整体运动命令，速度分量均在车队坐标系中表达。
+    /// </summary>
+    public readonly struct FleetMotionCommand
+    {
+        public FleetMotionCommand(
+            Point2D referencePointInFleet,
+            Twist2D twistAtReferencePoint)
+        {
+            ReferencePointInFleet = referencePointInFleet;
+            TwistAtReferencePoint = twistAtReferencePoint;
+        }
+
+        /// <summary>
+        /// 速度命令对应的参考点，也可作为自定义旋转中心。
+        /// </summary>
+        public Point2D ReferencePointInFleet { get; }
+
+        /// <summary>
+        /// 参考点处的车队速度。
+        /// </summary>
+        public Twist2D TwistAtReferencePoint { get; }
+
+        /// <summary>
+        /// 创建绕指定中心原地旋转的车队命令。
+        /// </summary>
+        public static FleetMotionCommand RotateAround(
+            Point2D rotationCenterInFleet,
+            double omegaRadiansPerSecond)
+        {
+            return new FleetMotionCommand(
+                rotationCenterInFleet,
+                new Twist2D(
+                    0.0,
+                    0.0,
+                    omegaRadiansPerSecond));
+        }
+
+        /// <summary>
+        /// 创建车队停止命令。
+        /// </summary>
+        public static FleetMotionCommand Stop()
+        {
+            return new FleetMotionCommand(
+                Point2D.Zero,
+                Twist2D.Zero);
+        }
+    }
+
+
+}

ClumsyPilot/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 = AcceptPoseAfterReset(
+                            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);
+        }
+    }
+}

ClumsyPilot/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,
+                    "滤波输入必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/StateEstimation/IVehicleStateProvider.cs

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

ClumsyPilot/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);
+        }
+    }
+}

ClumsyPilot/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);
+        }
+    }
+}

ClumsyPilot/Trajectory/LegacyTrackAdapter.cs

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

ClumsyPilot/Trajectory/Trajectory2D.cs

@@ -0,0 +1,260 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Linq;
+using MyParking.Shared;
+
+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>
+        public TrajectoryPoint SampleAtArcLength(
+            double arcLengthMeters)
+        {
+            EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+            {
+                return StartPoint;
+            }
+
+            if (arcLengthMeters >= TotalLengthMeters)
+            {
+                return EndPoint;
+            }
+
+            var segmentStartIndex =
+                FindSegmentStartIndex(
+                    arcLengthMeters);
+            var segmentStart =
+                _points[segmentStartIndex];
+            var segmentEnd =
+                _points[segmentStartIndex + 1];
+            var interpolationRatio =
+                (arcLengthMeters -
+                 segmentStart.ArcLengthMeters) /
+                (segmentEnd.ArcLengthMeters -
+                 segmentStart.ArcLengthMeters);
+
+            return new TrajectoryPoint(
+                arcLengthMeters,
+                new Pose2D(
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.XMeters,
+                        segmentEnd.PoseInWorld.XMeters,
+                        interpolationRatio),
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.YMeters,
+                        segmentEnd.PoseInWorld.YMeters,
+                        interpolationRatio),
+                    AngleMath.LerpRadians(
+                        segmentStart.PoseInWorld.YawRadians,
+                        segmentEnd.PoseInWorld.YawRadians,
+                        interpolationRatio)),
+                InterpolationMath.Lerp(
+                    segmentStart.CurvaturePerMeter,
+                    segmentEnd.CurvaturePerMeter,
+                    interpolationRatio),
+                InterpolationMath.Lerp(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    interpolationRatio));
+        }
+
+        /// <summary>
+        /// 使用二分查找获取包含指定累计弧长的线段起点索引。
+        /// </summary>
+        private int FindSegmentStartIndex(
+            double arcLengthMeters)
+        {
+            var lowerIndex = 0;
+            var upperIndex = _points.Length - 1;
+
+            while (upperIndex - lowerIndex > 1)
+            {
+                var middleIndex =
+                    lowerIndex +
+                    (upperIndex - lowerIndex) / 2;
+
+                if (_points[middleIndex].ArcLengthMeters <=
+                    arcLengthMeters)
+                {
+                    lowerIndex = middleIndex;
+                }
+                else
+                {
+                    upperIndex = middleIndex;
+                }
+            }
+
+            return lowerIndex;
+        }
+
+        /// <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,
+                    "轨迹弧长必须是有限值。");
+            }
+        }
+    }
+}

ClumsyPilot/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,
+                    "轨迹累计弧长不能为负数。");
+            }
+        }
+    }
+}

ClumsyPilot/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,
+                    "轨迹投影距离不能为负数。");
+            }
+        }
+    }
+}

ClumsyPilot/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,
+                    "用于轨迹投影的车体位姿必须是有限值。");
+            }
+        }
+    }
+}