feat: integrate trajectory tracking controller runtime

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,
+                    "纵向控制器速度误差死区必须是非负有限值。");
+            }
+        }
+    }
+}