chore: 停止跟踪日报和开发规格文档

.gitignore

@@ -54,3 +54,14 @@ appsettings.Development.json
 # OS files
 Thumbs.db
 .DS_Store
+
+
+# Local / unfinished workspace artifacts
+/TrajPlanner/
+/.agents/
+/.sdd-worktrees/
+/.superpowers/
+/.task8-sweep/
+/dailywork_report/
+/docs/
+/ClumsyPilot/tests/

ClumsyPilot/ClumsyPilot.csproj

@@ -8,7 +8,35 @@
 
   <ItemGroup>
     <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
+    <PackageReference Include="System.Drawing.Common" Version="10.0.10"
+                      GeneratePathProperty="true" />
     <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
+    <PackageReference Include="StbImageWriteSharp" Version="1.16.7"
+                      GeneratePathProperty="true" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Compile Remove="TrajectoryPlanningVisualization\**\*.cs" />
+    <Compile Remove="tests\TrajectoryPlanningVisualizationVerificationHost\**\*.cs" />
+    <Compile Remove="tests\PathSmoothingPngVerificationHost\**\*.cs" />
+    <Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\Trajplanner_output\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\Trajplanner_guide\**\*.cs" />
+    <Compile Remove="Shared\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
+             Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
+    <!-- 嵌套验证项目的旧输出不能参与主插件的程序集解析。 -->
+    <None Remove="**\bin\**\*" />
+    <None Remove="**\obj\**\*" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Compile Include="..\Shared\**\*.cs"
+             Link="Shared\%(RecursiveDir)%(Filename)%(Extension)" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="TrajectoryPlanningVisualization\TrajectoryPlanningVisualization.csproj" />
   </ItemGroup>
 
   <ItemGroup>
@@ -31,5 +59,26 @@
       <HintPath>ref\RefFundamentalLib.dll</HintPath>
     </Reference>
   </ItemGroup>
-	
+
+  <ItemGroup>
+    <None Update="ThirdParty\OSQP\win-x64\osqp.dll"
+          Link="osqp.dll" CopyToOutputDirectory="PreserveNewest" />
+    <None Update="ThirdParty\OSQP\LICENSE"
+          Link="licenses\OSQP-LICENSE.txt" CopyToOutputDirectory="PreserveNewest" />
+    <None Update="ThirdParty\OSQP\NOTICE"
+          Link="licenses\OSQP-NOTICE.txt" CopyToOutputDirectory="PreserveNewest" />
+    <None Update="ThirdParty\OSQP\VERSION"
+          Link="licenses\OSQP-VERSION.txt" CopyToOutputDirectory="PreserveNewest" />
+  </ItemGroup>
+
+  <Target Name="DeployManagedPngRuntime" AfterTargets="Build">
+    <Copy SourceFiles="$(PkgStbImageWriteSharp)\lib\netstandard2.0\StbImageWriteSharp.dll"
+          DestinationFiles="$(TargetDir)StbImageWriteSharp.dll" />
+  </Target>
+
+  <Target Name="DeploySystemDrawingRuntime" AfterTargets="Build">
+    <Copy SourceFiles="$(PkgSystem_Drawing_Common)\lib\netstandard2.0\System.Drawing.Common.dll"
+          DestinationFiles="$(TargetDir)System.Drawing.Common.dll" />
+  </Target>
+
 </Project>

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/ParkrobTrajplanner/CoarsePath/Contracts/CoarsePathPoint.cs

@@ -0,0 +1,63 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>最终粗路径上的一个稠密连续点；长度、净空和位置使用 m，航向使用 rad，曲率使用 1/m。</summary>
+public sealed class CoarsePathPoint
+{
+    /// <summary>
+    /// 创建粗路径点。
+    /// 参数：xMeters、yMeters 为世界坐标 m；headingRadians 与 unwrappedHeadingRadians 为航向 rad；arcLengthMeters 和 bodyClearanceMeters 为 m；vehicleCurvaturePerMeter 为 1/m。
+    /// </summary>
+    public CoarsePathPoint(
+        double xMeters,
+        double yMeters,
+        double headingRadians,
+        double unwrappedHeadingRadians,
+        double arcLengthMeters,
+        TravelDirection direction,
+        double vehicleCurvaturePerMeter,
+        double bodyClearanceMeters,
+        bool isGearSwitchPoint,
+        CoarsePathPointSource source)
+    {
+        X = xMeters;
+        Y = yMeters;
+        Heading = headingRadians;
+        UnwrappedHeading = unwrappedHeadingRadians;
+        ArcLength = arcLengthMeters;
+        Direction = direction;
+        VehicleCurvature = vehicleCurvaturePerMeter;
+        BodyClearance = bodyClearanceMeters;
+        IsGearSwitchPoint = isGearSwitchPoint;
+        Source = source;
+    }
+
+    /// <summary>世界 X 坐标，单位 m。</summary>
+    public double X { get; }
+
+    /// <summary>世界 Y 坐标，单位 m。</summary>
+    public double Y { get; }
+
+    /// <summary>归一化后可用于几何查询的车头航向，单位 rad。</summary>
+    public double Heading { get; }
+
+    /// <summary>跨越 ±π 后仍连续的车头航向，单位 rad。</summary>
+    public double UnwrappedHeading { get; }
+
+    /// <summary>从路径起点累计的弧长，单位 m；必须非负且不递减。</summary>
+    public double ArcLength { get; }
+
+    /// <summary>从上一点运动到当前点所在方向段的行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>车辆在此点采用的恒曲率原语曲率，单位 1/m。</summary>
+    public double VehicleCurvature { get; }
+
+    /// <summary>扩大车体到障碍物的保守净空下界，单位 m。</summary>
+    public double BodyClearance { get; }
+
+    /// <summary>此点是否为新方向段开始的换向点。</summary>
+    public bool IsGearSwitchPoint { get; }
+
+    /// <summary>此点由起点、普通原语或终点截断产生的来源。</summary>
+    public CoarsePathPointSource Source { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/CoarsePathPointSource.cs

@@ -0,0 +1,14 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>粗路径点在搜索与原语重建中的产生来源。</summary>
+public enum CoarsePathPointSource
+{
+    /// <summary>请求提供的起始位姿。</summary>
+    Start,
+
+    /// <summary>未截断恒曲率运动原语的内部积分点。</summary>
+    MotionPrimitive,
+
+    /// <summary>首次满足目标容差而在原语内部截断的终点积分点。</summary>
+    GoalTruncation,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/GoalDirectionConstraint.cs

@@ -0,0 +1,14 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>终点候选最后一个连续运动段的方向约束。</summary>
+public enum GoalDirectionConstraint
+{
+    /// <summary>不限制进入终点的运动方向。</summary>
+    Any,
+
+    /// <summary>必须以前进方向进入终点。</summary>
+    Forward,
+
+    /// <summary>必须以倒车方向进入终点。</summary>
+    Reverse,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/HybridAStarConfiguration.cs

@@ -0,0 +1,83 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// Hybrid A* 粗路径搜索的可调配置。
+/// 长度和距离使用 m，航向使用 rad，曲率使用 1/m；各项有效范围由规划器在执行前校验。
+/// </summary>
+public sealed class HybridAStarConfiguration
+{
+    /// <summary>创建采用 P0 固定安全边界和代价权重的默认配置。</summary>
+    public HybridAStarConfiguration()
+    {
+        PrimitiveLengthMeters = 0.50d;
+        IntegrationStepMeters = 0.05d;
+        MaximumCollisionCheckStepMeters = 0.025d;
+        HeadingResolutionRadians = Math.PI / 36d;
+        CurvatureLevelCount = 5;
+        GoalPositionToleranceMeters = 0.15d;
+        GoalHeadingToleranceRadians = Math.PI / 36d;
+        MaximumExpandedNodes = 200000;
+        SearchTimeout = TimeSpan.FromSeconds(5d);
+        HeuristicWeight = 1d;
+        ReverseCostMultiplier = 1.5d;
+        GearSwitchPenaltyMeters = 1d;
+        CurvatureMagnitudeWeight = 0.10d;
+        CurvatureChangePenaltyMetersPerLevel = 0.05d;
+        ClearanceCostWeight = 0.20d;
+        ClearanceCostDistanceMeters = 0.50d;
+        AllowReverse = true;
+    }
+
+    /// <summary>单个恒曲率原语的最大行驶长度，单位 m。</summary>
+    public double PrimitiveLengthMeters { get; set; }
+
+    /// <summary>原语内部输出积分点之间允许的最大弧长，单位 m。</summary>
+    public double IntegrationStepMeters { get; set; }
+
+    /// <summary>连续碰撞检查允许的最大车辆中心位移，单位 m；实际值还受地图分辨率限制。</summary>
+    public double MaximumCollisionCheckStepMeters { get; set; }
+
+    /// <summary>搜索状态离散所使用的航向格宽，单位 rad。</summary>
+    public double HeadingResolutionRadians { get; set; }
+
+    /// <summary>从最大负曲率到最大正曲率的离散曲率等级数。</summary>
+    public int CurvatureLevelCount { get; set; }
+
+    /// <summary>终点位置允许的欧氏距离误差，单位 m。</summary>
+    public double GoalPositionToleranceMeters { get; set; }
+
+    /// <summary>终点车头航向允许的最小环形角度误差，单位 rad。</summary>
+    public double GoalHeadingToleranceRadians { get; set; }
+
+    /// <summary>单次搜索允许扩展的最大节点数。</summary>
+    public int MaximumExpandedNodes { get; set; }
+
+    /// <summary>单次搜索允许消耗的最长时间；超出后返回 <see cref="PlanningStatus.SearchTimeout"/>。</summary>
+    public TimeSpan SearchTimeout { get; set; }
+
+    /// <summary>二维绕障启发式的权重；1 表示不额外放大。</summary>
+    public double HeuristicWeight { get; set; }
+
+    /// <summary>倒车原语长度代价相对前进原语的倍率。</summary>
+    public double ReverseCostMultiplier { get; set; }
+
+    /// <summary>相邻原语发生换向时增加的等效距离代价，单位 m。</summary>
+    public double GearSwitchPenaltyMeters { get; set; }
+
+    /// <summary>曲率绝对值对应的无量纲代价权重。</summary>
+    public double CurvatureMagnitudeWeight { get; set; }
+
+    /// <summary>相邻曲率等级每变化一级增加的等效距离代价，单位 m。</summary>
+    public double CurvatureChangePenaltyMetersPerLevel { get; set; }
+
+    /// <summary>车体保守净空不足时增加的无量纲代价权重。</summary>
+    public double ClearanceCostWeight { get; set; }
+
+    /// <summary>计算净空代价时视为足够安全的车体保守净空，单位 m。</summary>
+    public double ClearanceCostDistanceMeters { get; set; }
+
+    /// <summary>是否允许生成倒车原语；false 时搜索只生成前进原语。</summary>
+    public bool AllowReverse { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PathSegment.cs

@@ -0,0 +1,37 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>粗路径中方向一致的一段连续点范围；索引两端均包含在段内。</summary>
+public sealed class PathSegment
+{
+    /// <summary>
+    /// 创建方向分段。
+    /// 参数：segmentIndex 为从零开始的段序号；startIndex 与 endIndex 为 <see cref="PlanningResult.Path"/> 的包含式索引；两个换向标记描述段首或段尾是否位于换向对。
+    /// </summary>
+    public PathSegment(int segmentIndex, TravelDirection direction, int startIndex, int endIndex, bool startsAtGearSwitch, bool endsAtGearSwitch)
+    {
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        StartIndex = startIndex;
+        EndIndex = endIndex;
+        StartsAtGearSwitch = startsAtGearSwitch;
+        EndsAtGearSwitch = endsAtGearSwitch;
+    }
+
+    /// <summary>从零开始的方向段序号。</summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>此段所有连续运动点对应的行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>此段在 <see cref="PlanningResult.Path"/> 中的起始索引，包含该点。</summary>
+    public int StartIndex { get; }
+
+    /// <summary>此段在 <see cref="PlanningResult.Path"/> 中的结束索引，包含该点。</summary>
+    public int EndIndex { get; }
+
+    /// <summary>此段首点是否为换向后保留的新方向点。</summary>
+    public bool StartsAtGearSwitch { get; }
+
+    /// <summary>此段尾点是否紧邻下一方向段的换向对。</summary>
+    public bool EndsAtGearSwitch { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningDiagnostics.cs

@@ -0,0 +1,65 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>一次规划的只读统计与终止说明；长度和净空单位为 m。</summary>
+public sealed class PlanningDiagnostics
+{
+    /// <summary>
+    /// 创建规划统计快照。
+    /// 参数中的节点数量均为非负计数；pathLengthMeters 和 minimumBodyClearanceMeters 单位为 m；elapsed 为总耗时；pathSearchElapsed 为地图和起终点预检通过后的路径产出耗时；terminationReason 为可读终止说明，可为 null。
+    /// </summary>
+    public PlanningDiagnostics(
+        int expandedNodeCount = 0,
+        int generatedNodeCount = 0,
+        int reopenedNodeCount = 0,
+        int staleOpenListEntryCount = 0,
+        int peakOpenListCount = 0,
+        double pathLengthMeters = 0d,
+        double minimumBodyClearanceMeters = 0d,
+        TimeSpan elapsed = default(TimeSpan),
+        string terminationReason = null,
+        TimeSpan pathSearchElapsed = default(TimeSpan))
+    {
+        ExpandedNodeCount = expandedNodeCount;
+        GeneratedNodeCount = generatedNodeCount;
+        ReopenedNodeCount = reopenedNodeCount;
+        StaleOpenListEntryCount = staleOpenListEntryCount;
+        PeakOpenListCount = peakOpenListCount;
+        PathLengthMeters = pathLengthMeters;
+        MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
+        Elapsed = elapsed;
+        PathSearchElapsed = pathSearchElapsed;
+        TerminationReason = terminationReason ?? string.Empty;
+    }
+
+    /// <summary>从 Open List 取出并真正扩展的节点数量。</summary>
+    public int ExpandedNodeCount { get; }
+
+    /// <summary>生成并尝试加入搜索状态的节点数量。</summary>
+    public int GeneratedNodeCount { get; }
+
+    /// <summary>以严格更小代价到达同一离散键而重新打开的节点数量。</summary>
+    public int ReopenedNodeCount { get; }
+
+    /// <summary>从 Open List 取出后因已有更优条目而丢弃的陈旧堆条目数量。</summary>
+    public int StaleOpenListEntryCount { get; }
+
+    /// <summary>搜索期间 Open List 同时容纳的最大有效或待丢弃条目数量。</summary>
+    public int PeakOpenListCount { get; }
+
+    /// <summary>成功路径的累计弧长，单位 m；失败结果通常为 0。</summary>
+    public double PathLengthMeters { get; }
+
+    /// <summary>成功路径所有点中车体保守净空下界的最小值，单位 m；失败结果通常为 0。</summary>
+    public double MinimumBodyClearanceMeters { get; }
+
+    /// <summary>从规划入口到返回结果的总耗时。</summary>
+    public TimeSpan Elapsed { get; }
+
+    /// <summary>地图和起终点预检通过后，二维启发式、Hybrid A*、回溯、装配和最终复核的耗时；不含建图，搜索前失败时为零。</summary>
+    public TimeSpan PathSearchElapsed { get; }
+
+    /// <summary>面向调用方的终止原因；成功时可为空字符串。</summary>
+    public string TerminationReason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningRequest.cs

@@ -0,0 +1,41 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 已建规划地图上的一次 Hybrid A* 请求。
+/// 本对象只接受不可变 <see cref="PlanningGridMap"/> 快照，不包含地图构建器、传感器、UI 或调试对象。
+/// </summary>
+public sealed class PlanningRequest
+{
+    /// <summary>创建空规划请求。调用规划器前必须提供地图、起点、终点、车辆和配置。</summary>
+    public PlanningRequest()
+    {
+        StartVehicleCurvature = 0d;
+        GoalDirection = GoalDirectionConstraint.Any;
+    }
+
+    /// <summary>本次搜索唯一允许查询的不可变规划地图快照；位置查询单位为 m。</summary>
+    public PlanningGridMap Map { get; set; }
+
+    /// <summary>车辆几何中心的起始位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Start { get; set; }
+
+    /// <summary>车辆几何中心的目标位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Goal { get; set; }
+
+    /// <summary>车辆几何、余量和曲率限制；尺寸单位 m，曲率单位 1/m。</summary>
+    public VehicleParameters Vehicle { get; set; }
+
+    /// <summary>搜索步长、离散、终点容差、代价和资源上限配置。</summary>
+    public HybridAStarConfiguration Configuration { get; set; }
+
+    /// <summary>车辆起步时的转向曲率，单位 1/m；默认值为 0。</summary>
+    public double StartVehicleCurvature { get; set; }
+
+    /// <summary>起步方向约束；null 表示可从前进或倒车开始。</summary>
+    public TravelDirection? StartDirection { get; set; }
+
+    /// <summary>目标进入方向约束；默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
+    public GoalDirectionConstraint GoalDirection { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningResult.cs

@@ -0,0 +1,67 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 一次粗路径规划的最终不可变结果。
+/// 只有 <see cref="Status"/> 为 <see cref="PlanningStatus.Success"/> 时才携带非空路径与方向分段；其他状态始终返回空只读集合。
+/// </summary>
+public sealed class PlanningResult
+{
+    private static readonly IReadOnlyList<CoarsePathPoint> EmptyPath = new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
+    private static readonly IReadOnlyList<PathSegment> EmptySegments = new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
+
+    private PlanningResult(PlanningStatus status, PlanningDiagnostics diagnostics, IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments)
+    {
+        Status = status;
+        Diagnostics = diagnostics ?? new PlanningDiagnostics(terminationReason: "未提供诊断信息。");
+        Path = path;
+        Segments = segments;
+    }
+
+    /// <summary>规划最终状态；只有 <see cref="PlanningStatus.Success"/> 可以发布路径。</summary>
+    public PlanningStatus Status { get; }
+
+    /// <summary>节点、耗时、路径长度、净空和终止原因统计；始终非空。</summary>
+    public PlanningDiagnostics Diagnostics { get; }
+
+    /// <summary>成功时的稠密粗路径；失败时为不可修改的空集合。</summary>
+    public IReadOnlyList<CoarsePathPoint> Path { get; }
+
+    /// <summary>成功时覆盖 <see cref="Path"/> 的包含式方向分段；失败时为不可修改的空集合。</summary>
+    public IReadOnlyList<PathSegment> Segments { get; }
+
+    /// <summary>
+    /// 创建成功结果。
+    /// 参数：path 与 segments 必须均为非空；diagnostics 为本次规划的统计快照。参数不符合要求时抛出 <see cref="ArgumentException"/>，防止以成功状态发布不完整路径。
+    /// </summary>
+    public static PlanningResult Success(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, PlanningDiagnostics diagnostics)
+    {
+        if (path == null || path.Count == 0)
+            throw new ArgumentException("Successful planning results require a non-empty path.", nameof(path));
+        if (segments == null || segments.Count == 0)
+            throw new ArgumentException("Successful planning results require non-empty segments.", nameof(segments));
+        return new PlanningResult(PlanningStatus.Success, diagnostics, CopyReadOnly(path), CopyReadOnly(segments));
+    }
+
+    /// <summary>
+    /// 创建失败、取消或资源受限结果。
+    /// 参数：status 不能为 <see cref="PlanningStatus.Success"/>；diagnostics 会原样保留。返回结果的路径与分段始终为空只读集合。
+    /// </summary>
+    public static PlanningResult Failure(PlanningStatus status, PlanningDiagnostics diagnostics)
+    {
+        if (status == PlanningStatus.Success)
+            throw new ArgumentException("Use Success to create a successful planning result.", nameof(status));
+        return new PlanningResult(status, diagnostics, EmptyPath, EmptySegments);
+    }
+
+    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningStatus.cs

@@ -0,0 +1,56 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>粗路径规划的最终状态；除 <see cref="Success"/> 外均不发布路径或方向分段。</summary>
+public enum PlanningStatus
+{
+    /// <summary>已得到并通过最终连续碰撞复核的完整粗路径。</summary>
+    Success,
+
+    /// <summary>在搜索扩展检查点收到取消请求。</summary>
+    Cancelled,
+
+    /// <summary>请求对象或其必要成员为空，或包含不符合基本契约的值。</summary>
+    InvalidRequest,
+
+    /// <summary>请求提供的地图对象不满足规划器的结构要求。</summary>
+    InvalidMap,
+
+    /// <summary>地图快照尚未准备好参与规划；应查看地图的阻止原因。</summary>
+    MapNotReady,
+
+    /// <summary>车辆尺寸、安全余量或曲率限制无效。</summary>
+    InvalidVehicleParameters,
+
+    /// <summary>原语、离散、代价、容差或资源限制配置无效。</summary>
+    InvalidCurvatureConfiguration,
+
+    /// <summary>起始车辆几何中心或扩大车体不在地图范围内。</summary>
+    StartOutsideMap,
+
+    /// <summary>起始扩大车体与地图障碍物相交或擦边。</summary>
+    StartInCollision,
+
+    /// <summary>目标车辆几何中心或扩大车体不在地图范围内。</summary>
+    GoalOutsideMap,
+
+    /// <summary>目标扩大车体与地图障碍物相交或擦边。</summary>
+    GoalInCollision,
+
+    /// <summary>搜索达到 <see cref="HybridAStarConfiguration.SearchTimeout"/> 限制。</summary>
+    SearchTimeout,
+
+    /// <summary>搜索达到 <see cref="HybridAStarConfiguration.MaximumExpandedNodes"/> 限制。</summary>
+    SearchNodeLimitExceeded,
+
+    /// <summary>Open List 已耗尽，或二维启发式证明目标不可达。</summary>
+    NoFeasiblePath,
+
+    /// <summary>搜索成功节点无法按父链回溯为完整路径。</summary>
+    BacktrackingFailed,
+
+    /// <summary>回溯路径未通过连续碰撞、终点或输出不变量复核。</summary>
+    FinalValidationFailed,
+
+    /// <summary>规划内部发生未预期错误；不会发布部分路径。</summary>
+    InternalError,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/Pose2D.cs

@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 粗路径规划使用的二维连续位姿。
+/// 位置以世界坐标 m 表示，航向以 rad 表示；此值对象不在构造时归一化航向，调用方可保留展开航向。
+/// </summary>
+public sealed class Pose2D
+{
+    /// <summary>
+    /// 创建二维位姿。
+    /// 参数：xMeters、yMeters 为世界坐标，单位 m；headingRadians 为车头航向，单位 rad。
+    /// </summary>
+    public Pose2D(double xMeters, double yMeters, double headingRadians)
+    {
+        X = xMeters;
+        Y = yMeters;
+        Heading = headingRadians;
+    }
+
+    /// <summary>世界 X 坐标，单位 m。</summary>
+    public double X { get; }
+
+    /// <summary>世界 Y 坐标，单位 m。</summary>
+    public double Y { get; }
+
+    /// <summary>车头航向，单位 rad；可以是已展开的连续航向。</summary>
+    public double Heading { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/TravelDirection.cs

@@ -0,0 +1,11 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>连续运动段的行驶方向。</summary>
+public enum TravelDirection
+{
+    /// <summary>沿车辆车头方向前进。</summary>
+    Forward,
+
+    /// <summary>与车辆车头方向相反地倒车。</summary>
+    Reverse,
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/VehicleParameters.cs

@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 车辆几何与运动学参数。
+/// 所有几何尺寸均以车辆几何中心为 <see cref="Pose2D"/> 参考点，单位为 m；曲率单位为 1/m。
+/// </summary>
+public sealed class VehicleParameters
+{
+    /// <summary>创建空车辆参数。调用规划器前必须填写有效的几何尺寸和至少一种曲率限制。</summary>
+    public VehicleParameters()
+    {
+    }
+
+    /// <summary>车辆本体长度，单位 m；不含 <see cref="SafetyMarginMeters"/>。</summary>
+    public double LengthMeters { get; set; }
+
+    /// <summary>车辆本体宽度，单位 m；不含 <see cref="SafetyMarginMeters"/>。</summary>
+    public double WidthMeters { get; set; }
+
+    /// <summary>碰撞检查时加在车体四周的安全余量，单位 m；不会写入地图。</summary>
+    public double SafetyMarginMeters { get; set; }
+
+    /// <summary>车辆允许的最大绝对曲率，单位 1/m；null 表示由 <see cref="MinimumTurningRadiusMeters"/> 提供限制。</summary>
+    public double? MaximumCurvaturePerMeter { get; set; }
+
+    /// <summary>车辆允许的最小转弯半径，单位 m；null 表示由 <see cref="MaximumCurvaturePerMeter"/> 提供限制。</summary>
+    public double? MinimumTurningRadiusMeters { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningJob.cs

@@ -0,0 +1,45 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次从地图输入到 Hybrid A* 粗路径输出的完整业务请求。
+/// 地图边界、分辨率和障碍物几何位于 <see cref="MapRequest"/> 中并使用 mm；位姿和车辆几何使用 m，航向使用 rad，曲率使用 1/m。
+/// </summary>
+public sealed class CoarsePathPlanningJob
+{
+    /// <summary>创建默认目标方向、起步曲率和关闭调试旁路的业务请求。</summary>
+    public CoarsePathPlanningJob()
+    {
+        StartVehicleCurvature = 0d;
+        GoalDirection = GoalDirectionConstraint.Any;
+        DebugOptions = new PlanningDebugOptions();
+    }
+
+    /// <summary>本次唯一建图输入；边界、分辨率和障碍物几何均遵循 Map 模块的 mm 契约。</summary>
+    public PlanningMapRequest MapRequest { get; set; }
+
+    /// <summary>车辆几何中心的起始位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Start { get; set; }
+
+    /// <summary>车辆几何中心的目标位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Goal { get; set; }
+
+    /// <summary>车辆尺寸、安全余量和曲率限制；尺寸单位 m，曲率单位 1/m。</summary>
+    public VehicleParameters Vehicle { get; set; }
+
+    /// <summary>原语、离散、容差、代价和资源上限配置。</summary>
+    public HybridAStarConfiguration Configuration { get; set; }
+
+    /// <summary>车辆起步曲率，单位 1/m；默认值为 0。</summary>
+    public double StartVehicleCurvature { get; set; }
+
+    /// <summary>起步方向约束；null 表示可从前进或倒车开始。</summary>
+    public TravelDirection? StartDirection { get; set; }
+
+    /// <summary>目标进入方向约束；默认值为 <see cref="GoalDirectionConstraint.Any"/>。</summary>
+    public GoalDirectionConstraint GoalDirection { get; set; }
+
+    /// <summary>可选调试旁路配置；默认关闭且使用空接收器，不参与地图或路径计算。</summary>
+    public PlanningDebugOptions DebugOptions { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningJobResult.cs

@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次粗路径业务编排的不可变结果。
+/// 始终同时保留地图构建结果和规划结果；调试旁路消息只用于诊断，不会修改地图或规划内容。
+/// </summary>
+public sealed class CoarsePathPlanningJobResult
+{
+    /// <summary>
+    /// 创建业务编排结果。
+    /// 参数：mapResult 和 planningResult 均不能为空；debugDiagnostics 可为 null，返回时会转换为只读字符串集合。
+    /// </summary>
+    public CoarsePathPlanningJobResult(PlanningMapBuildResult mapResult, PlanningResult planningResult,
+        IReadOnlyList<string> debugDiagnostics = null)
+    {
+        MapResult = mapResult ?? throw new ArgumentNullException(nameof(mapResult));
+        PlanningResult = planningResult ?? throw new ArgumentNullException(nameof(planningResult));
+        DebugDiagnostics = CopyDiagnostics(debugDiagnostics);
+    }
+
+    /// <summary>本次调用的地图创建结果；失败时读取 <see cref="PlanningMapBuildResult.FailureReason"/>。</summary>
+    public PlanningMapBuildResult MapResult { get; }
+
+    /// <summary>本次调用的粗路径结果；地图创建失败时为无路径的失败状态。</summary>
+    public PlanningResult PlanningResult { get; }
+
+    /// <summary>调试旁路的非致命诊断信息；为空时表示未启用、未发生异常或无额外调试消息。</summary>
+    public IReadOnlyList<string> DebugDiagnostics { get; }
+
+    private static IReadOnlyList<string> CopyDiagnostics(IReadOnlyList<string> source)
+    {
+        if (source == null || source.Count == 0)
+            return new ReadOnlyCollection<string>(new List<string>());
+
+        var copy = new List<string>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index] ?? string.Empty);
+        return new ReadOnlyCollection<string>(copy);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningService.cs

@@ -0,0 +1,108 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 从 <see cref="PlanningMapRequest"/> 到 Hybrid A* 粗路径的一次调用服务。
+/// 服务生命周期内长期持有同一个地图工厂和规划器，以保留地图缓存并避免将 UI、传感器或调试依赖带入规划核心。
+/// </summary>
+public sealed class CoarsePathPlanningService
+{
+    private readonly PlanningMapFactory _mapFactory;
+    private readonly HybridAStarPlanner _planner;
+
+    /// <summary>创建长期复用默认地图工厂与 Hybrid A* 规划器的服务。</summary>
+    public CoarsePathPlanningService()
+        : this(new PlanningMapFactory(), new HybridAStarPlanner())
+    {
+    }
+
+    internal CoarsePathPlanningService(PlanningMapFactory mapFactory, HybridAStarPlanner planner)
+    {
+        _mapFactory = mapFactory ?? throw new ArgumentNullException(nameof(mapFactory));
+        _planner = planner ?? throw new ArgumentNullException(nameof(planner));
+    }
+
+    /// <summary>
+    /// 按固定顺序创建地图并执行一次 Hybrid A* 粗路径规划。
+    /// 参数：job 的地图输入使用 mm，位姿和车辆尺寸使用 m，航向使用 rad，曲率使用 1/m；cancellationToken 会传递给搜索阶段。
+    /// 返回：始终同时保留地图创建结果和规划结果；地图创建失败时不会启动搜索，并返回 <see cref="PlanningStatus.InvalidMap"/> 的空路径结果。
+    /// </summary>
+    public CoarsePathPlanningJobResult Plan(CoarsePathPlanningJob job,
+        CancellationToken cancellationToken = default(CancellationToken))
+    {
+        PlanningOperationBudget budget = CreateBudget(job, cancellationToken);
+        PlanningMapBuildResult mapResult = _mapFactory.Create(job == null ? null : job.MapRequest, budget);
+        if (!mapResult.Succeeded || mapResult.Map == null)
+        {
+            PlanningResult mapFailure = PlanningResult.Failure(MapFailureStatus(mapResult),
+                new PlanningDiagnostics(elapsed: budget.Elapsed, terminationReason: BuildMapFailureReason(mapResult)));
+            return PublishDebug(job, mapResult, mapFailure);
+        }
+
+        var request = new PlanningRequest
+        {
+            Map = mapResult.Map,
+            Start = job.Start,
+            Goal = job.Goal,
+            Vehicle = job.Vehicle,
+            Configuration = job.Configuration,
+            StartVehicleCurvature = job.StartVehicleCurvature,
+            StartDirection = job.StartDirection,
+            GoalDirection = job.GoalDirection,
+        };
+        PlanningResult planningResult = _planner.Plan(request, budget);
+        return PublishDebug(job, mapResult, planningResult);
+    }
+
+    private static PlanningOperationBudget CreateBudget(CoarsePathPlanningJob job, CancellationToken cancellationToken)
+    {
+        HybridAStarConfiguration configuration = job == null ? null : job.Configuration;
+        return configuration != null && configuration.SearchTimeout >= TimeSpan.Zero
+            ? new PlanningOperationBudget(cancellationToken, configuration.SearchTimeout)
+            : PlanningOperationBudget.Unlimited(cancellationToken);
+    }
+
+    private static PlanningStatus MapFailureStatus(PlanningMapBuildResult mapResult)
+    {
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled) return PlanningStatus.Cancelled;
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut) return PlanningStatus.SearchTimeout;
+        return PlanningStatus.InvalidMap;
+    }
+
+    private static CoarsePathPlanningJobResult PublishDebug(CoarsePathPlanningJob job, PlanningMapBuildResult mapResult,
+        PlanningResult planningResult)
+    {
+        var diagnostics = new List<string>();
+        PlanningDebugOptions options = job == null ? null : job.DebugOptions;
+        if (options != null && options.Enabled)
+        {
+            IPlanningDebugSink sink = options.Sink ?? NullPlanningDebugSink.Instance;
+            try
+            {
+                sink.Publish(mapResult, planningResult);
+            }
+            catch (Exception exception)
+            {
+                diagnostics.Add("调试旁路发布失败：" + exception.GetType().Name + "。" + exception.Message);
+            }
+        }
+
+        return new CoarsePathPlanningJobResult(mapResult, planningResult, diagnostics);
+    }
+
+    private static string BuildMapFailureReason(PlanningMapBuildResult mapResult)
+    {
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.Cancelled)
+            return "规划地图创建已取消，未启动 Hybrid A* 搜索。";
+        if (mapResult != null && mapResult.Status == PlanningMapBuildStatus.TimedOut)
+            return "规划地图创建已超时，未启动 Hybrid A* 搜索。";
+        string reason = mapResult == null ? string.Empty : mapResult.FailureReason;
+        return string.IsNullOrEmpty(reason) ? "规划地图创建失败，未启动 Hybrid A* 搜索。" :
+            "规划地图创建失败，未启动 Hybrid A* 搜索：" + reason;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/IPlanningDebugSink.cs

@@ -0,0 +1,29 @@
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 接收一次地图构建和粗路径规划完成后的旁路调试数据。
+/// 实现不得修改输入对象；服务会隔离实现抛出的异常，调试失败不会改变规划结果。
+/// </summary>
+public interface IPlanningDebugSink
+{
+    /// <summary>
+    /// 发布本次编排得到的地图和规划结果。
+    /// 参数：mapResult 为地图创建结果；planningResult 为对应的完整或失败规划结果；二者均不可由接收方修改。
+    /// </summary>
+    void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult);
+}
+
+internal sealed class NullPlanningDebugSink : IPlanningDebugSink
+{
+    internal static readonly NullPlanningDebugSink Instance = new NullPlanningDebugSink();
+
+    private NullPlanningDebugSink()
+    {
+    }
+
+    public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
+    {
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/PlanningDebugOptions.cs

@@ -0,0 +1,23 @@
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+
+/// <summary>
+/// 一次粗路径编排的可选调试旁路配置。
+/// 调试开关和接收器不参与地图输入指纹、缓存键、搜索状态或路径结果。
+/// </summary>
+public sealed class PlanningDebugOptions
+{
+    /// <summary>创建默认关闭并使用空接收器的调试配置。</summary>
+    public PlanningDebugOptions()
+    {
+        Enabled = false;
+        Sink = NullPlanningDebugSink.Instance;
+    }
+
+    /// <summary>是否在本次编排结束后向 <see cref="Sink"/> 发布旁路数据；默认值为 false。</summary>
+    public bool Enabled { get; set; }
+
+    /// <summary>
+    /// 接收地图和规划结果的旁路对象；默认为空实现。赋值为 null 时服务仍会使用空实现，且不会影响主流程。
+    /// </summary>
+    public IPlanningDebugSink Sink { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs

@@ -0,0 +1,264 @@
+using System;
+using System.Diagnostics;
+using System.Globalization;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+/// <summary>
+/// 已建 <see cref="PlanningGridMap"/> 上 Hybrid A* 粗路径规划的下层门面。
+/// 本类不建图、不读取 UI 或传感器；只有路径经回溯、装配和最终连续复核后才发布成功结果。
+/// </summary>
+public sealed class HybridAStarPlanner
+{
+    private readonly HybridAStarSearch _search;
+    private readonly PathBacktracker _backtracker;
+    private readonly CoarsePathAssembler _assembler;
+    private readonly CoarsePathValidator _validator;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认搜索、回溯、装配和最终复核组件的规划器。</summary>
+    public HybridAStarPlanner()
+        : this(new HybridAStarSearch(), new PathBacktracker(), new CoarsePathAssembler(), new CoarsePathValidator(),
+            new FootprintCollisionChecker())
+    {
+    }
+
+    internal HybridAStarPlanner(HybridAStarSearch search, PathBacktracker backtracker, CoarsePathAssembler assembler,
+        CoarsePathValidator validator, FootprintCollisionChecker collisionChecker)
+    {
+        _search = search ?? throw new ArgumentNullException(nameof(search));
+        _backtracker = backtracker ?? throw new ArgumentNullException(nameof(backtracker));
+        _assembler = assembler ?? throw new ArgumentNullException(nameof(assembler));
+        _validator = validator ?? throw new ArgumentNullException(nameof(validator));
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 在请求提供的不可变规划地图上执行一次 Hybrid A* 粗路径规划。
+    /// 参数：request 的位置单位为 m、航向单位为 rad、曲率单位为 1/m；cancellationToken 会在搜索扩展检查点取消。
+    /// 返回：输入、边界、碰撞、搜索、回溯或最终复核失败均返回空路径；只有 <see cref="PlanningStatus.Success"/> 携带完整路径和分段。
+    /// </summary>
+    public PlanningResult Plan(PlanningRequest request, CancellationToken cancellationToken = default(CancellationToken))
+    {
+        PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
+            ? new PlanningOperationBudget(cancellationToken, request.Configuration.SearchTimeout)
+            : PlanningOperationBudget.Unlimited(cancellationToken);
+        return Plan(request, budget);
+    }
+
+    /// <summary>使用门面传入的共享预算执行预检、搜索和最终路径复核。</summary>
+    internal PlanningResult Plan(PlanningRequest request, PlanningOperationBudget budget)
+    {
+        Stopwatch pathSearchStopwatch = null;
+        try
+        {
+            if (budget == null) throw new ArgumentNullException(nameof(budget));
+            PlanningOperationStopReason stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return Failure(ToPlanningStatus(stopReason), budget, "规划在开始前已停止。", null);
+
+            PlanningStatus preflightStatus = ValidatePreflight(request, out string preflightReason);
+            if (preflightStatus != PlanningStatus.Success)
+                return Failure(preflightStatus, budget, preflightReason, null);
+
+            if (!IsFootprintInsideMap(request.Start, request.Map, request.Vehicle))
+                return Failure(PlanningStatus.StartOutsideMap, budget, "起始扩大车体不完全位于地图边界内。", null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Start, request.Map, request.Vehicle, 0d, out _))
+                return Failure(PlanningStatus.StartInCollision, budget, "起始扩大车体与障碍物相交或擦边。", null);
+            if (!IsFootprintInsideMap(request.Goal, request.Map, request.Vehicle))
+                return Failure(PlanningStatus.GoalOutsideMap, budget, "目标扩大车体不完全位于地图边界内。", null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Goal, request.Map, request.Vehicle, 0d, out _))
+                return Failure(PlanningStatus.GoalInCollision, budget, "目标扩大车体与障碍物相交或擦边。", null);
+
+            stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return Failure(ToPlanningStatus(stopReason), budget, "规划在搜索前已停止。", null);
+            pathSearchStopwatch = Stopwatch.StartNew();
+            HybridAStarSearchResult searchResult = _search.Search(request, budget);
+            if (searchResult == null)
+                return Failure(PlanningStatus.InternalError, budget, "搜索器未返回结果。", null, pathSearchStopwatch);
+            if (searchResult.Status != PlanningStatus.Success)
+                return Failure(searchResult.Status, budget,
+                    BuildSearchFailureReason(searchResult, request.Configuration), searchResult, pathSearchStopwatch);
+
+            if (!_backtracker.TryBacktrack(searchResult, request, out BacktrackedPath backtrackedPath, out string backtrackingReason))
+                return Failure(PlanningStatus.BacktrackingFailed, budget, backtrackingReason, searchResult, pathSearchStopwatch);
+            if (!_assembler.TryAssemble(backtrackedPath, request, out var path, out var segments, out string assemblyReason))
+                return Failure(PlanningStatus.FinalValidationFailed, budget, assemblyReason, searchResult, pathSearchStopwatch);
+            if (!_validator.TryValidate(path, segments, request, out double minimumClearanceMeters, out string validationReason))
+                return Failure(PlanningStatus.FinalValidationFailed, budget, validationReason, searchResult, pathSearchStopwatch);
+
+            double pathLengthMeters = path[path.Count - 1].ArcLength;
+            return PlanningResult.Success(path, segments, CreateDiagnostics(searchResult, budget.Elapsed, pathLengthMeters,
+                minimumClearanceMeters, string.Empty, pathSearchStopwatch.Elapsed));
+        }
+        catch (Exception exception)
+        {
+            string reason = "规划内部错误：" + exception.GetType().Name +
+                (string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
+            return Failure(PlanningStatus.InternalError, budget ?? PlanningOperationBudget.Unlimited(CancellationToken.None),
+                reason, null, pathSearchStopwatch);
+        }
+    }
+
+    private static PlanningStatus ValidatePreflight(PlanningRequest request, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
+            !IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
+            !IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
+        {
+            failureReason = "规划请求缺少必要对象或包含非法数值。";
+            return PlanningStatus.InvalidRequest;
+        }
+
+        PlanningGridMap map = request.Map;
+        if (map.Bounds == null || map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters))
+        {
+            failureReason = "规划地图结构无效。";
+            return PlanningStatus.InvalidMap;
+        }
+        if (!map.PlanningReady)
+        {
+            failureReason = string.IsNullOrEmpty(map.PlanningBlockReason) ? "规划地图尚未就绪。" : map.PlanningBlockReason;
+            return PlanningStatus.MapNotReady;
+        }
+
+        VehicleParameters vehicle = request.Vehicle;
+        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
+            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+        {
+            failureReason = "车辆尺寸、安全余量或曲率限制无效。";
+            return PlanningStatus.InvalidVehicleParameters;
+        }
+
+        HybridAStarConfiguration configuration = request.Configuration;
+        if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
+            (request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
+        {
+            failureReason = "Hybrid A* 曲率、离散、代价或资源配置无效。";
+            return PlanningStatus.InvalidCurvatureConfiguration;
+        }
+
+        return PlanningStatus.Success;
+    }
+
+    private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
+    {
+        return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
+            configuration.HeadingResolutionRadians <= 2d * Math.PI && configuration.CurvatureLevelCount >= 3 &&
+            configuration.CurvatureLevelCount % 2 == 1 && NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) &&
+            configuration.GoalPositionToleranceMeters >= 0d && NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) &&
+            configuration.GoalHeadingToleranceRadians >= 0d && configuration.MaximumExpandedNodes >= 0 &&
+            configuration.SearchTimeout >= TimeSpan.Zero && NumericGuard.IsFinite(configuration.HeuristicWeight) &&
+            configuration.HeuristicWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
+            NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) &&
+            configuration.CurvatureChangePenaltyMetersPerLevel >= 0d && NumericGuard.IsFinite(configuration.ClearanceCostWeight) &&
+            configuration.ClearanceCostWeight >= 0d && NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
+    }
+
+    private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
+    {
+        if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
+
+        double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
+        double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
+        double longitudinalX = Math.Cos(pose.Heading);
+        double longitudinalY = Math.Sin(pose.Heading);
+        double lateralX = -longitudinalY;
+        double lateralY = longitudinalX;
+        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
+        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
+        {
+            double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
+            double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
+            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
+        }
+
+        return true;
+    }
+
+    private static string BuildSearchFailureReason(HybridAStarSearchResult searchResult,
+        HybridAStarConfiguration configuration)
+    {
+        string reason = string.IsNullOrEmpty(searchResult.TerminationReason)
+            ? "Hybrid A* 搜索以 " + searchResult.Status + " 状态终止。"
+            : searchResult.TerminationReason;
+        string resourceLimit = string.Empty;
+        if (searchResult.Status == PlanningStatus.SearchTimeout)
+        {
+            resourceLimit = "总预算=" + configuration.SearchTimeout.TotalSeconds.ToString(
+                "F3", CultureInfo.InvariantCulture) + "s；";
+        }
+        else if (searchResult.Status == PlanningStatus.SearchNodeLimitExceeded)
+        {
+            resourceLimit = "节点上限=" + configuration.MaximumExpandedNodes.ToString(
+                CultureInfo.InvariantCulture) + "；";
+        }
+
+        return reason + resourceLimit +
+            "扩展=" + searchResult.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "生成=" + searchResult.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "重开=" + searchResult.ReopenedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "陈旧条目=" + searchResult.StaleOpenListEntryCount.ToString(CultureInfo.InvariantCulture) + "，" +
+            "Open List峰值=" + searchResult.PeakOpenListCount.ToString(CultureInfo.InvariantCulture) + "。";
+    }
+
+    private static PlanningResult Failure(PlanningStatus status, PlanningOperationBudget budget, string reason,
+        HybridAStarSearchResult searchResult, Stopwatch pathSearchStopwatch = null)
+    {
+        TimeSpan pathSearchElapsed = pathSearchStopwatch == null ? TimeSpan.Zero : pathSearchStopwatch.Elapsed;
+        return PlanningResult.Failure(status, CreateDiagnostics(searchResult, budget.Elapsed, 0d, 0d,
+            reason, pathSearchElapsed));
+    }
+
+    private static PlanningDiagnostics CreateDiagnostics(HybridAStarSearchResult searchResult, TimeSpan elapsed,
+        double pathLengthMeters, double minimumClearanceMeters, string reason, TimeSpan pathSearchElapsed)
+    {
+        return new PlanningDiagnostics(
+            searchResult == null ? 0 : searchResult.ExpandedNodeCount,
+            searchResult == null ? 0 : searchResult.GeneratedNodeCount,
+            searchResult == null ? 0 : searchResult.ReopenedNodeCount,
+            searchResult == null ? 0 : searchResult.StaleOpenListEntryCount,
+            searchResult == null ? 0 : searchResult.PeakOpenListCount,
+            pathLengthMeters,
+            minimumClearanceMeters,
+            elapsed,
+            reason,
+            pathSearchElapsed);
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+
+    private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
+    {
+        if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
+        if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
+        throw new ArgumentOutOfRangeException(nameof(stopReason));
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs

@@ -0,0 +1,216 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+
+/// <summary>
+/// 将回溯原语装配为调用方可消费的稠密路径和包含式方向分段。
+/// 装配器保留原语边界的换向双点，其余相邻重复位姿会被删除。
+/// </summary>
+public sealed class CoarsePathAssembler
+{
+    private const double DuplicateTolerance = 1e-8d;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认连续车体检查器的路径装配器。</summary>
+    public CoarsePathAssembler()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定连续车体检查器的路径装配器。</summary>
+    public CoarsePathAssembler(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 从已回溯的恒曲率原语构造稠密路径。
+    /// 参数：backtrackedPath 提供原语顺序；request 提供地图和车辆；path、segments 为成功时的只读输出。
+    /// 返回：首点可通过连续车体检查、每个原语积分点数据一致且方向分段完整覆盖时为 true；否则返回 false 且输出为空。
+    /// </summary>
+    public bool TryAssemble(BacktrackedPath backtrackedPath, PlanningRequest request,
+        out IReadOnlyList<CoarsePathPoint> path, out IReadOnlyList<PathSegment> segments, out string failureReason)
+    {
+        path = EmptyPath();
+        segments = EmptySegments();
+        failureReason = string.Empty;
+        if (backtrackedPath == null || request == null || request.Map == null || request.Vehicle == null ||
+            !IsFinitePose(backtrackedPath.Start) || !IsTravelDirection(backtrackedPath.StartDirection) ||
+            !NumericGuard.IsFinite(backtrackedPath.StartCurvaturePerMeter))
+        {
+            failureReason = "路径装配输入无效。";
+            return false;
+        }
+
+        if (!_collisionChecker.IsPoseCollisionFree(backtrackedPath.Start, request.Map, request.Vehicle, 0d, out double startClearanceMeters))
+        {
+            failureReason = "回溯路径起点未通过连续车体检查。";
+            return false;
+        }
+
+        var points = new List<CoarsePathPoint>();
+        TravelDirection currentDirection = backtrackedPath.Primitives.Count > 0
+            ? backtrackedPath.Primitives[0].Direction
+            : backtrackedPath.StartDirection;
+        double currentCurvature = request.StartVehicleCurvature;
+        double normalizedStartHeading = AngleMath.NormalizeRadians(backtrackedPath.Start.Heading);
+        if (!NumericGuard.IsFinite(normalizedStartHeading))
+        {
+            failureReason = "回溯路径起点航向无效。";
+            return false;
+        }
+
+        points.Add(new CoarsePathPoint(backtrackedPath.Start.X, backtrackedPath.Start.Y, normalizedStartHeading,
+            backtrackedPath.Start.Heading, 0d, currentDirection, currentCurvature, startClearanceMeters, false,
+            CoarsePathPointSource.Start));
+
+        for (int primitiveIndex = 0; primitiveIndex < backtrackedPath.Primitives.Count; primitiveIndex++)
+        {
+            MotionPrimitive primitive = backtrackedPath.Primitives[primitiveIndex];
+            if (!IsValidPrimitive(primitive))
+            {
+                failureReason = "回溯路径包含无效原语。";
+                return false;
+            }
+
+            CoarsePathPoint lastPoint = points[points.Count - 1];
+            if (primitive.Direction != lastPoint.Direction)
+            {
+                // 换向处的旧方向终点和新方向起点必须共存，二者位置、航向、弧长完全相同。
+                points.Add(new CoarsePathPoint(lastPoint.X, lastPoint.Y, lastPoint.Heading, lastPoint.UnwrappedHeading,
+                    lastPoint.ArcLength, primitive.Direction, primitive.CurvaturePerMeter, lastPoint.BodyClearance,
+                    true, CoarsePathPointSource.MotionPrimitive));
+            }
+
+            Pose2D previousPose = primitive.Start;
+            for (int pointIndex = 0; pointIndex < primitive.Points.Count; pointIndex++)
+            {
+                Pose2D pose = primitive.Points[pointIndex];
+                double bodyClearanceMeters = primitive.BodyClearancesMeters[pointIndex];
+                if (!IsFinitePose(pose) || !IsValidClearance(bodyClearanceMeters))
+                {
+                    failureReason = "原语积分点或净空无效。";
+                    return false;
+                }
+
+                CoarsePathPoint previousPoint = points[points.Count - 1];
+                double arcIncrementMeters = CalculateArcIncrement(previousPose, pose, primitive.CurvaturePerMeter);
+                if (!NumericGuard.IsFinite(arcIncrementMeters) || arcIncrementMeters <= 0d)
+                {
+                    failureReason = "原语积分点未产生正弧长。";
+                    return false;
+                }
+
+                if (IsSamePose(previousPoint, pose))
+                {
+                    // 非换向情况下不允许重复采样点泄露到对外路径。
+                    previousPose = pose;
+                    continue;
+                }
+
+                double normalizedHeading = AngleMath.NormalizeRadians(pose.Heading);
+                double headingDelta = AngleMath.ShortestSignedDifference(previousPoint.Heading, normalizedHeading);
+                if (!NumericGuard.IsFinite(normalizedHeading) || !NumericGuard.IsFinite(headingDelta))
+                {
+                    failureReason = "原语积分点航向无法展开。";
+                    return false;
+                }
+
+                CoarsePathPointSource source = primitive.IsGoalTruncation && pointIndex == primitive.Points.Count - 1
+                    ? CoarsePathPointSource.GoalTruncation
+                    : CoarsePathPointSource.MotionPrimitive;
+                points.Add(new CoarsePathPoint(pose.X, pose.Y, normalizedHeading,
+                    previousPoint.UnwrappedHeading + headingDelta, previousPoint.ArcLength + arcIncrementMeters,
+                    primitive.Direction, primitive.CurvaturePerMeter, bodyClearanceMeters, false, source));
+                previousPose = pose;
+            }
+        }
+
+        if (points.Count == 0)
+        {
+            failureReason = "路径装配未产生起点。";
+            return false;
+        }
+
+        path = new ReadOnlyCollection<CoarsePathPoint>(points);
+        segments = BuildSegments(points);
+        return true;
+    }
+
+    private static IReadOnlyList<PathSegment> BuildSegments(IReadOnlyList<CoarsePathPoint> points)
+    {
+        var segments = new List<PathSegment>();
+        int startIndex = 0;
+        TravelDirection direction = points[0].Direction;
+        for (int index = 1; index < points.Count; index++)
+        {
+            if (points[index].Direction == direction) continue;
+            segments.Add(new PathSegment(segments.Count, direction, startIndex, index - 1,
+                points[startIndex].IsGearSwitchPoint, true));
+            startIndex = index;
+            direction = points[index].Direction;
+        }
+
+        segments.Add(new PathSegment(segments.Count, direction, startIndex, points.Count - 1,
+            points[startIndex].IsGearSwitchPoint, false));
+        return new ReadOnlyCollection<PathSegment>(segments);
+    }
+
+    private static double CalculateArcIncrement(Pose2D from, Pose2D to, double curvaturePerMeter)
+    {
+        if (!IsFinitePose(from) || !IsFinitePose(to) || !NumericGuard.IsFinite(curvaturePerMeter)) return double.NaN;
+        if (Math.Abs(curvaturePerMeter) < 1e-12d)
+        {
+            double deltaX = to.X - from.X;
+            double deltaY = to.Y - from.Y;
+            return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        }
+
+        double headingDelta = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
+        return Math.Abs(headingDelta / curvaturePerMeter);
+    }
+
+    private static bool IsValidPrimitive(MotionPrimitive primitive)
+    {
+        return primitive != null && primitive.Start != null && primitive.Points != null && primitive.BodyClearancesMeters != null &&
+            primitive.Points.Count == primitive.BodyClearancesMeters.Count && primitive.Points.Count > 0 &&
+            IsTravelDirection(primitive.Direction) && NumericGuard.IsFinite(primitive.CurvaturePerMeter) &&
+            NumericGuard.IsPositiveFinite(primitive.ActualLengthMeters);
+    }
+
+    private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
+    {
+        return Math.Abs(point.X - pose.X) <= DuplicateTolerance && Math.Abs(point.Y - pose.Y) <= DuplicateTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= DuplicateTolerance;
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsValidClearance(double clearanceMeters)
+    {
+        return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
+    }
+
+    private static IReadOnlyList<CoarsePathPoint> EmptyPath()
+    {
+        return new ReadOnlyCollection<CoarsePathPoint>(new List<CoarsePathPoint>());
+    }
+
+    private static IReadOnlyList<PathSegment> EmptySegments()
+    {
+        return new ReadOnlyCollection<PathSegment>(new List<PathSegment>());
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathValidator.cs

@@ -0,0 +1,245 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+
+/// <summary>
+/// 对准备发布的粗路径执行独立的连续安全与输出契约复核。
+/// 复核失败的路径不得被包装为 <see cref="PlanningStatus.Success"/>。
+/// </summary>
+public sealed class CoarsePathValidator
+{
+    private const double NumericTolerance = 1e-6d;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认连续车体检查器的最终路径复核器。</summary>
+    public CoarsePathValidator()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定连续车体检查器的最终路径复核器。</summary>
+    public CoarsePathValidator(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 复核路径数值、曲率、连续扫掠碰撞、终点约束、累计弧长和方向分段。
+    /// 参数：path 与 segments 为待发布输出；request 必须是生成该路径的请求；minimumBodyClearanceMeters 返回沿途的保守净空下界。
+    /// 返回：所有规则通过时为 true；否则返回 false、写入失败原因，调用方必须丢弃 path 和 segments。
+    /// </summary>
+    public bool TryValidate(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments,
+        PlanningRequest request, out double minimumBodyClearanceMeters, out string failureReason)
+    {
+        minimumBodyClearanceMeters = 0d;
+        failureReason = string.Empty;
+        if (path == null || segments == null || request == null || request.Map == null || request.Vehicle == null ||
+            request.Configuration == null || path.Count == 0 || segments.Count == 0)
+        {
+            failureReason = "最终路径或复核请求为空。";
+            return false;
+        }
+
+        if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvaturePerMeter))
+        {
+            failureReason = "车辆曲率约束无效。";
+            return false;
+        }
+
+        CoarsePathPoint first = path[0];
+        if (!IsValidPoint(first) || first.Source != CoarsePathPointSource.Start || first.IsGearSwitchPoint ||
+            Math.Abs(first.ArcLength) > NumericTolerance || !IsSamePose(first, request.Start))
+        {
+            failureReason = "最终路径首点不符合起点契约。";
+            return false;
+        }
+
+        double minimumClearance = double.PositiveInfinity;
+        for (int index = 0; index < path.Count; index++)
+        {
+            CoarsePathPoint current = path[index];
+            if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvaturePerMeter + NumericTolerance)
+            {
+                failureReason = "最终路径包含非法数值或超限曲率。";
+                return false;
+            }
+
+            var currentPose = new Pose2D(current.X, current.Y, current.Heading);
+            if (!_collisionChecker.IsPoseCollisionFree(currentPose, request.Map, request.Vehicle, 0d, out double poseClearanceMeters) ||
+                IsClearanceOverclaimed(current.BodyClearance, poseClearanceMeters))
+            {
+                failureReason = "最终路径点未通过连续车体碰撞复核。";
+                return false;
+            }
+
+            minimumClearance = Math.Min(minimumClearance, current.BodyClearance);
+            if (index == 0) continue;
+
+            CoarsePathPoint previous = path[index - 1];
+            if (current.ArcLength + NumericTolerance < previous.ArcLength ||
+                !IsUnwrappedHeadingContinuous(previous, current))
+            {
+                failureReason = "最终路径的弧长或展开航向不连续。";
+                return false;
+            }
+
+            bool isDuplicate = IsDuplicatePoseAndArcLength(previous, current);
+            if (isDuplicate)
+            {
+                if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
+                {
+                    failureReason = "相邻重复点不是合法换向对。";
+                    return false;
+                }
+            }
+            else
+            {
+                if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + NumericTolerance ||
+                    !IsArcIncrementConsistent(previous, current))
+                {
+                    failureReason = "非换向路径点的弧长增量不一致。";
+                    return false;
+                }
+            }
+
+            var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
+            if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, request.Map, request.Vehicle,
+                request.Configuration.MaximumCollisionCheckStepMeters, out double sweptClearanceMeters))
+            {
+                failureReason = "最终路径相邻点之间的车体扫掠碰撞复核失败。";
+                return false;
+            }
+
+            minimumClearance = Math.Min(minimumClearance, sweptClearanceMeters);
+        }
+
+        CoarsePathPoint last = path[path.Count - 1];
+        if (!GoalToleranceChecker.IsSatisfied(new Pose2D(last.X, last.Y, last.Heading), request.Goal, request.Configuration,
+            last.Direction, request.GoalDirection) || (path.Count > 1 && last.Source != CoarsePathPointSource.GoalTruncation))
+        {
+            failureReason = "最终路径末点未满足终点容差、方向或来源契约。";
+            return false;
+        }
+
+        if (!AreSegmentsValid(path, segments, out failureReason)) return false;
+        minimumBodyClearanceMeters = minimumClearance;
+        return true;
+    }
+
+    private static bool AreSegmentsValid(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string failureReason)
+    {
+        failureReason = string.Empty;
+        int expectedStartIndex = 0;
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            PathSegment segment = segments[segmentIndex];
+            if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
+                segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
+                segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
+            {
+                failureReason = "方向分段索引或起始换向标记无效。";
+                return false;
+            }
+
+            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
+            {
+                if (path[pointIndex].Direction != segment.Direction)
+                {
+                    failureReason = "方向分段包含不同方向的路径点。";
+                    return false;
+                }
+            }
+
+            bool hasNextSegment = segmentIndex + 1 < segments.Count;
+            bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
+                path[segment.EndIndex + 1].IsGearSwitchPoint;
+            if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
+            {
+                failureReason = "方向分段末尾换向标记无效。";
+                return false;
+            }
+
+            expectedStartIndex = segment.EndIndex + 1;
+        }
+
+        if (expectedStartIndex != path.Count)
+        {
+            failureReason = "方向分段未完整覆盖路径点。";
+            return false;
+        }
+
+        return true;
+    }
+
+    private static bool IsValidPoint(CoarsePathPoint point)
+    {
+        if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
+            !NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
+            !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
+            !NumericGuard.IsFinite(point.VehicleCurvature) || !IsTravelDirection(point.Direction) ||
+            !IsValidClearance(point.BodyClearance))
+            return false;
+
+        return Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= NumericTolerance;
+    }
+
+    private static bool IsSamePose(CoarsePathPoint point, Pose2D pose)
+    {
+        return point != null && pose != null && Math.Abs(point.X - pose.X) <= NumericTolerance &&
+            Math.Abs(point.Y - pose.Y) <= NumericTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, pose.Heading)) <= NumericTolerance;
+    }
+
+    private static bool IsUnwrappedHeadingContinuous(CoarsePathPoint previous, CoarsePathPoint current)
+    {
+        double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
+        return NumericGuard.IsFinite(expectedDelta) &&
+            Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= NumericTolerance;
+    }
+
+    private static bool IsDuplicatePoseAndArcLength(CoarsePathPoint previous, CoarsePathPoint current)
+    {
+        return Math.Abs(previous.X - current.X) <= NumericTolerance && Math.Abs(previous.Y - current.Y) <= NumericTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= NumericTolerance &&
+            Math.Abs(previous.ArcLength - current.ArcLength) <= NumericTolerance;
+    }
+
+    private static bool IsArcIncrementConsistent(CoarsePathPoint previous, CoarsePathPoint current)
+    {
+        double expectedIncrement;
+        if (Math.Abs(current.VehicleCurvature) < 1e-12d)
+        {
+            double deltaX = current.X - previous.X;
+            double deltaY = current.Y - previous.Y;
+            expectedIncrement = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        }
+        else
+        {
+            double headingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
+            expectedIncrement = Math.Abs(headingDelta / current.VehicleCurvature);
+        }
+
+        return NumericGuard.IsFinite(expectedIncrement) && expectedIncrement > 0d &&
+            Math.Abs((current.ArcLength - previous.ArcLength) - expectedIncrement) <= NumericTolerance;
+    }
+
+    private static bool IsClearanceOverclaimed(double reportedClearanceMeters, double checkedClearanceMeters)
+    {
+        if (double.IsPositiveInfinity(reportedClearanceMeters)) return !double.IsPositiveInfinity(checkedClearanceMeters);
+        return reportedClearanceMeters > checkedClearanceMeters + NumericTolerance;
+    }
+
+    private static bool IsValidClearance(double clearanceMeters)
+    {
+        return !double.IsNaN(clearanceMeters) && clearanceMeters >= 0d;
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/PathBacktracker.cs

@@ -0,0 +1,167 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;
+
+/// <summary>
+/// 成功搜索父链重新生成后的原语序列。
+/// 起点位置使用 m/rad，起始曲率使用 1/m；原语集合按从起点到终点的顺序排列。
+/// </summary>
+public sealed class BacktrackedPath
+{
+    internal BacktrackedPath(Pose2D start, TravelDirection startDirection, double startCurvaturePerMeter,
+        IReadOnlyList<MotionPrimitive> primitives)
+    {
+        Start = start;
+        StartDirection = startDirection;
+        StartCurvaturePerMeter = startCurvaturePerMeter;
+        Primitives = primitives;
+    }
+
+    /// <summary>原始请求中的起始车辆中心位姿；位置单位 m，航向单位 rad。</summary>
+    public Pose2D Start { get; }
+
+    /// <summary>成功父链根节点记录的起步方向。</summary>
+    public TravelDirection StartDirection { get; }
+
+    /// <summary>成功父链根节点离散后的起始曲率，单位 1/m。</summary>
+    public double StartCurvaturePerMeter { get; }
+
+    /// <summary>按起点到终点顺序重新积分的恒曲率原语；每项都不包含自身起点。</summary>
+    public IReadOnlyList<MotionPrimitive> Primitives { get; }
+}
+
+/// <summary>
+/// 仅依据成功节点的父索引和原语描述，确定性地重建稠密原语序列。
+/// 不复用搜索期保存的积分点，以保证输出与当前解析积分、扫掠检查规则一致。
+/// </summary>
+public sealed class PathBacktracker
+{
+    private const double PoseComparisonTolerance = 1e-7d;
+    private const double LengthComparisonTolerance = 1e-7d;
+    private readonly MotionPrimitiveGenerator _primitiveGenerator;
+
+    /// <summary>创建使用默认解析积分器的回溯器。</summary>
+    public PathBacktracker()
+        : this(new MotionPrimitiveGenerator())
+    {
+    }
+
+    /// <summary>创建使用指定原语生成器的回溯器。</summary>
+    public PathBacktracker(MotionPrimitiveGenerator primitiveGenerator)
+    {
+        _primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
+    }
+
+    /// <summary>
+    /// 依据搜索成功节点重建从起点到终点的原语序列。
+    /// 参数：searchResult 必须是携带成功节点索引的搜索结果；request 必须仍指向执行搜索的同一不可变地图和配置。
+    /// 返回：父链无环、索引连续且每条原语按同一解析规则可重建时为 true；失败时 path 为 null 并返回可读原因。
+    /// </summary>
+    public bool TryBacktrack(HybridAStarSearchResult searchResult, PlanningRequest request,
+        out BacktrackedPath path, out string failureReason)
+    {
+        path = null;
+        failureReason = string.Empty;
+        if (searchResult == null || request == null || searchResult.Status != PlanningStatus.Success ||
+            !searchResult.SuccessNodeIndex.HasValue || searchResult.Nodes == null)
+        {
+            failureReason = "搜索结果不含可回溯的成功节点。";
+            return false;
+        }
+
+        int currentIndex = searchResult.SuccessNodeIndex.Value;
+        var reverseNodes = new List<HybridAStarNode>();
+        var visited = new HashSet<int>();
+        while (currentIndex >= 0)
+        {
+            if (currentIndex >= searchResult.Nodes.Count || !visited.Add(currentIndex))
+            {
+                failureReason = "搜索父链索引越界或存在环。";
+                return false;
+            }
+
+            HybridAStarNode current = searchResult.Nodes[currentIndex];
+            if (current == null || current.NodeIndex != currentIndex || current.Pose == null)
+            {
+                failureReason = "搜索节点索引或连续位姿不一致。";
+                return false;
+            }
+
+            reverseNodes.Add(current);
+            currentIndex = current.ParentNodeIndex;
+        }
+
+        if (reverseNodes.Count == 0)
+        {
+            failureReason = "搜索父链为空。";
+            return false;
+        }
+
+        reverseNodes.Reverse();
+        HybridAStarNode root = reverseNodes[0];
+        if (root.ParentNodeIndex != -1 || root.IncomingPrimitive != null || !IsFinitePose(root.Pose))
+        {
+            failureReason = "搜索父链根节点无效。";
+            return false;
+        }
+
+        var primitives = new List<MotionPrimitive>(Math.Max(0, reverseNodes.Count - 1));
+        Pose2D previousPose = root.Pose;
+        for (int index = 1; index < reverseNodes.Count; index++)
+        {
+            HybridAStarNode node = reverseNodes[index];
+            MotionPrimitive descriptor = node.IncomingPrimitive;
+            if (node.ParentNodeIndex != reverseNodes[index - 1].NodeIndex || descriptor == null ||
+                !IsFinitePose(node.Pose) || !NumericGuard.IsFinite(descriptor.CurvaturePerMeter) ||
+                !IsTravelDirection(descriptor.Direction) || descriptor.ActualLengthMeters <= 0d)
+            {
+                failureReason = "搜索父链中的原语描述无效。";
+                return false;
+            }
+
+            // 只把恒曲率、方向和长度描述作为真源，再走一遍相同的解析积分与连续碰撞检查。
+            MotionPrimitive rebuilt = _primitiveGenerator.Generate(previousPose, descriptor.CurvaturePerMeter, descriptor.Direction, request);
+            if (rebuilt == null || !IsEquivalent(descriptor, rebuilt) || !IsSamePose(rebuilt.End, node.Pose))
+            {
+                failureReason = "搜索原语无法按当前解析规则确定性重建。";
+                return false;
+            }
+
+            primitives.Add(rebuilt);
+            previousPose = rebuilt.End;
+        }
+
+        path = new BacktrackedPath(root.Pose, root.Direction, root.CurvaturePerMeter,
+            new ReadOnlyCollection<MotionPrimitive>(primitives));
+        return true;
+    }
+
+    private static bool IsEquivalent(MotionPrimitive expected, MotionPrimitive actual)
+    {
+        return expected.Direction == actual.Direction && expected.IsGoalTruncation == actual.IsGoalTruncation &&
+            Math.Abs(expected.CurvaturePerMeter - actual.CurvaturePerMeter) <= PoseComparisonTolerance &&
+            Math.Abs(expected.ActualLengthMeters - actual.ActualLengthMeters) <= LengthComparisonTolerance;
+    }
+
+    private static bool IsSamePose(Pose2D first, Pose2D second)
+    {
+        return IsFinitePose(first) && IsFinitePose(second) &&
+            Math.Abs(first.X - second.X) <= PoseComparisonTolerance &&
+            Math.Abs(first.Y - second.Y) <= PoseComparisonTolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(first.Heading, second.Heading)) <= PoseComparisonTolerance;
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md

@@ -0,0 +1,387 @@
+# CoarsePath 粗路径规划（P0/P1）
+
+`CoarsePath` 在 `Map` 提供的不可变 `PlanningGridMap` 上执行 Hybrid A*，输出已经过连续碰撞、终点和输出不变量复核的粗路径。它只处理“能否安全地从当前车辆几何中心到达目标”的几何搜索；不读取传感器或定位，不绘制 UI，也不向底盘发送任何命令。
+
+地图障碍物来源、世界坐标栅格化、距离场和缓存细节由 [Map/README.md](../Map/README.md) 说明。本模块唯一建议的业务调用入口是：
+
+```csharp
+CoarsePathPlanningService.Plan(job, cancellationToken)
+```
+
+## 模块说明（Module Overview）
+
+| 模块 | 负责内容 | 不负责内容 |
+| --- | --- | --- |
+| `Map` | 外部障碍物快照、占据栅格、保守障碍距离、地图缓存 | 车辆足迹、运动原语、路径搜索、控制 |
+| `CoarsePath` | 车辆扩大足迹、连续碰撞检查、前进/倒车原语、Hybrid A*、路径复核与方向分段 | 传感器读取、定位读取、速度规划、路径跟踪、底盘命令 |
+| `Facade` | 将建图、搜索、取消、总预算和可选调试旁路编排为一次调用 | 修改地图内容、写入 AMR 占据、执行轨迹 |
+| `Test` | 固定回归场景、P1 手动测试入口与规划结果可视化 | 真实作业地图、实时重规划或车辆控制 |
+
+安全余量只由 `VehicleParameters.SafetyMarginMeters` 扩大车辆矩形。它不会回写到地图障碍物，因此同一个 `PlanningGridMap` 可由不同车辆参数重复使用。
+
+## 文件结构（File Structure）
+
+```text
+CoarsePath/
+├── README.md                                      # 本模块说明：结构、数据流、调用与测试
+├── HybridAStarPlanner.cs                          # 公开规划门面后的核心编排：校验、搜索、回溯和复核
+├── Contracts/
+│   ├── Pose2D.cs                                  # 车辆几何中心位姿：m / rad
+│   ├── PlanningRequest.cs                         # 已有 PlanningGridMap 上的一次内部搜索请求
+│   ├── PlanningResult.cs                          # 不可变规划结果：成功路径或空结果
+│   ├── PlanningStatus.cs                          # 成功、取消、无解、输入和资源限制状态
+│   ├── CoarsePathPoint.cs                         # 稠密路径点、方向、曲率、净空和换向标记
+│   ├── PathSegment.cs                             # 前进或倒车的包含式路径索引段
+│   ├── VehicleParameters.cs                       # 车体尺寸、安全余量和曲率限制
+│   ├── HybridAStarConfiguration.cs                # 原语、离散、代价、容差和资源上限
+│   └── GoalDirectionConstraint.cs                 # 目标进入方向约束
+├── Vehicle/
+│   ├── VehicleKinematics.cs                       # 恒曲率车辆运动学积分
+│   ├── VehicleFootprint.cs                        # 扩大后的车辆矩形几何
+│   ├── OrientedRectangleCellIntersection.cs       # 旋转矩形与占据格相交判定
+│   └── FootprintCollisionChecker.cs               # 连续扫掠的足迹碰撞检查
+├── Search/
+│   ├── BinaryMinHeap.cs                           # 可更新优先级的 Open List
+│   ├── GridDijkstraHeuristic.cs                   # 二维栅格可达性和距离启发式
+│   ├── GoalToleranceChecker.cs                    # 目标位置、航向和方向约束判定
+│   ├── MotionPrimitive.cs                         # 单个前进或倒车恒曲率原语
+│   ├── MotionPrimitiveGenerator.cs                # 原语离散与连续积分点生成
+│   ├── SearchCostCalculator.cs                    # 长度、倒车、换向、曲率和净空代价
+│   ├── HybridAStarNode.cs                         # 搜索节点与父链信息
+│   ├── HybridAStarNodeKey.cs                      # 离散状态键
+│   └── HybridAStarSearch.cs                       # Hybrid A* 主搜索循环
+├── Output/
+│   ├── PathBacktracker.cs                         # 从终点节点安全回溯父链
+│   ├── CoarsePathAssembler.cs                     # 组装稠密路径和方向段
+│   └── CoarsePathValidator.cs                     # 对最终输出重新进行连续复核
+├── Facade/
+│   ├── CoarsePathPlanningJob.cs                   # 一次完整业务输入：地图请求、位姿、车辆、配置
+│   ├── CoarsePathPlanningJobResult.cs             # 同时包含地图结果和规划结果的不可变输出
+│   ├── CoarsePathPlanningService.cs               # 唯一业务调用门面
+│   ├── PlanningDebugOptions.cs                    # 可选调试旁路配置
+│   └── IPlanningDebugSink.cs                      # 调试旁路接收器契约
+└── Test/
+    ├── CoarsePathScenarioFactory.cs               # 六个固定场景和手动目标演示请求工厂
+    └── MovementTest.CoarsePathTest.cs              # 七个 Clumsy 入口、后台取消和 Painter 绘制
+```
+
+## 规划数据流（Planning Data Flow）
+
+```text
+CoarsePathPlanningJob
+        │  MapRequest 使用 mm；Pose2D/车辆使用 m、rad
+        ▼
+CoarsePathPlanningService.Plan(job, cancellationToken)
+        │
+        ├── PlanningMapFactory.Create(job.MapRequest)
+        │       │
+        │       ├── 失败、取消或超时
+        │       │       └── MapResult + 空路径 PlanningResult，搜索不启动
+        │       │
+        │       └── 成功：不可变 PlanningGridMap
+        ▼
+PlanningRequest
+        │
+        ▼
+HybridAStarPlanner
+        ├── 车辆扩大足迹与连续碰撞检查
+        ├── GridDijkstraHeuristic + Hybrid A* 搜索
+        ├── PathBacktracker + CoarsePathAssembler
+        └── CoarsePathValidator 最终复核
+        ▼
+PlanningResult + MapResult
+        ▼
+CoarsePathPlanningJobResult
+```
+
+调用方只创建 `CoarsePathPlanningJob` 并消费 `CoarsePathPlanningJobResult`。`PlanningRequest`、`HybridAStarPlanner`、原语和碰撞检查器属于模块内部协作对象，不应由 UI、传感器或 MovementTest 直接拼接。
+
+## 构建状态与停止（Build Status and Stop）
+
+必须一起处理 `MapResult` 和 `PlanningResult`。`MapResult.Status` 的类型是 `PlanningMapBuildStatus`；地图失败时，门面返回对应的空路径结果，并且不会启动 Hybrid A*。
+
+| 情况 | `MapResult` | `PlanningResult` | 调用方处理 |
+| --- | --- | --- | --- |
+| 地图和搜索成功 | `Success` 且 `Map` 非空 | `Success`，发布完整路径和方向段 | 消费粗路径；后续模块仍需自行进行平滑、速度和控制 |
+| 地图输入/来源失败 | `Failed` | `InvalidMap` | 读取 `FailureReason`，修复地图输入 |
+| 调用被取消 | `Cancelled` | `Cancelled` | 不重试为普通无解；不会发布地图或部分路径 |
+| 总预算耗尽 | `TimedOut` | `SearchTimeout` | 根据上层策略调整预算或稍后重试 |
+| 搜索无解 | 地图成功 | `NoFeasiblePath` | 当前地图、车体和运动约束下无可行路径 |
+| 节点或搜索资源受限 | 地图成功 | `SearchNodeLimitExceeded` 或 `SearchTimeout` | 读取诊断后调整配置或上层策略 |
+
+除 `PlanningStatus.Success` 外，`PlanningResult.Path` 与 `PlanningResult.Segments` 始终为空。取消、超时、无解、输入错误和最终复核失败都不能作为“部分可执行路径”使用。
+
+### 总预算与取消
+
+`HybridAStarConfiguration.SearchTimeout` 是从门面开始计时的一次总预算，依次覆盖建图、距离场、二维启发式和 Hybrid A*。同一个 `CancellationToken` 会沿调用链传递，取消优先于超时。
+
+### 总耗时与路径搜索耗时
+
+`PlanningDiagnostics.Elapsed` 是从 `CoarsePathPlanningService.Plan` 开始的总耗时，包含地图来源、缓存、栅格化、距离场和路径规划。`PathSearchElapsed`（路径搜索耗时）从地图和起终点预检通过后开始，包含二维启发式、Hybrid A*、回溯、装配、方向分段和最终复核；搜索开始前失败时为零。
+
+## 坐标与单位（Coordinates and Units）
+
+| 数据 | 单位 | 说明 |
+| --- | --- | --- |
+| `PlanningMapRequest.Bounds`、分辨率、障碍物几何 | mm | 来自 Map 的世界坐标；边界采用 `[min, max)` |
+| `Pose2D.X`、`Pose2D.Y`、路径位置、车辆尺寸、安全余量、弧长 | m | CoarsePath 的连续世界坐标和长度 |
+| `Pose2D.Heading`、航向容差 | rad | 核心一律使用弧度 |
+| 曲率、起步曲率 | 1/m | 最大曲率或最小转弯半径至少提供一个 |
+| `PlanningGridMap` 世界查询参数 | m | 越界位置按占据处理，净距为 0 |
+| P1 的 AMR/手动目标 X/Y | mm | 仅在 UI 边界读取，进入核心前除以 1000 |
+| P1 的 AMR/手动目标航向 | deg | 仅在 UI 边界转换为 `deg * PI / 180 -> rad` |
+
+起点和终点都表示车辆**几何中心**。若上游定位参考点是雷达、天线或其他安装点，必须先在上游应用安装外参；不要在 CoarsePath 内猜测偏移。车辆外扩由 `VehicleParameters.SafetyMarginMeters` 表达，不要把余量写入 Map 障碍物。
+
+## 最小调用示例（Minimal Call Example）
+
+以下示例明确允许空图，因而只适合算法或单位演示。真实作业必须通过 `IMapObstacleSource` 提供有效障碍物快照；如何构造来源请阅读 [Map/README.md](../Map/README.md)。
+
+```csharp
+using System;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+var service = new CoarsePathPlanningService(); // 长期持有，保留地图缓存
+
+var job = new CoarsePathPlanningJob
+{
+    MapRequest = new PlanningMapRequest
+    {
+        Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
+        ResolutionMm = 50f,
+        ObstacleSources = Array.Empty<IMapObstacleSource>(),
+        AllowExplicitEmptyMap = true, // 仅演示时明确允许
+    },
+    Start = new Pose2D(1d, 1d, 0d),
+    Goal = new Pose2D(3d, 1d, 0d),
+    Vehicle = new VehicleParameters
+    {
+        LengthMeters = 0.80d,
+        WidthMeters = 0.60d,
+        SafetyMarginMeters = 0.05d,
+        MaximumCurvaturePerMeter = 1d / 1.20d,
+    },
+    Configuration = new HybridAStarConfiguration(),
+    GoalDirection = GoalDirectionConstraint.Forward,
+};
+
+CoarsePathPlanningJobResult result =
+    service.Plan(job, CancellationToken.None);
+
+if (!result.MapResult.Succeeded)
+    throw new InvalidOperationException(result.MapResult.FailureReason);
+
+if (result.PlanningResult.Status != PlanningStatus.Success)
+    throw new InvalidOperationException(
+        result.PlanningResult.Diagnostics.TerminationReason);
+
+foreach (CoarsePathPoint point in result.PlanningResult.Path)
+    Console.WriteLine(point.X + "," + point.Y + "," + point.Heading);
+```
+
+## 缓存与 SourceVersion（Cache and SourceVersion）
+
+`CoarsePathPlanningService` 在生命周期内长期持有 `PlanningMapFactory`，因此重复调用时能够复用地图缓存。不要每次规划都新建服务，否则会失去缓存收益。
+
+| 缓存层级 | 条件 | 结果 |
+| --- | --- | --- |
+| `Input` | 边界、分辨率、空图策略、来源 ID、`SourceVersion`、必需性和来源结果相同 | 返回同一个不可变 `PlanningGridMap` |
+| `Occupancy` | 输入版本变化，但最终占据栅格相同 | 复用占据/距离数组，生成新的快照元数据 |
+| `None` | 占据内容变化 | 重建规划快照和距离场 |
+
+障碍来源内容改变时，调用方必须递增该来源的 `SourceVersion`。仅修改起终点、车辆、搜索配置、调试开关或调试接收器不会改变地图输入；改变障碍物却不递增版本则可能错误复用旧快照。
+
+## 详细使用指南（Detailed Usage Guide）
+
+本节说明调用方如何从一个地图输入得到可消费的粗路径。所有业务调用都通过 `CoarsePathPlanningService.Plan(job, cancellationToken)` 完成。
+
+### 第 1 步：长期持有服务
+
+服务持有地图工厂和规划器，应该作为规划业务、任务执行器或上层服务的长期字段，而不是在每次调用中创建：
+
+```csharp
+private readonly CoarsePathPlanningService _coarsePathService =
+    new CoarsePathPlanningService();
+```
+
+### 第 2 步：准备地图请求
+
+创建 `PlanningMapRequest`，其边界、分辨率和障碍物仍使用 mm。使用手工圆形/矩形或 TwoLeg 快照时，应先按 [Map/README.md](../Map/README.md) 将它们包装为 `IMapObstacleSource`，并为内容变化递增 `SourceVersion`。
+
+```csharp
+var mapRequest = new PlanningMapRequest
+{
+    Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
+    ResolutionMm = 50f,
+    ObstacleSources = sources,
+    AllowExplicitEmptyMap = false,
+};
+```
+
+`AllowExplicitEmptyMap = true` 只在调用方明确确认空地图安全时使用。未提供有效障碍物且未显式允许空图时，地图不会进入规划。
+
+### 第 3 步：填写起点、终点和方向约束
+
+将车辆几何中心的世界 X/Y 从 mm 转为 m，并将航向转换为 rad 后创建 `Pose2D`。`StartDirection = null` 表示允许从前进或倒车开始；`GoalDirection` 可以限制最终进入目标的方向。
+
+```csharp
+var start = new Pose2D(startXmm / 1000d, startYmm / 1000d,
+    startHeadingDeg * Math.PI / 180d);
+var goal = new Pose2D(goalXmm / 1000d, goalYmm / 1000d,
+    goalHeadingDeg * Math.PI / 180d);
+```
+
+### 第 4 步：填写车辆参数
+
+车辆尺寸和安全余量全部为 m。曲率限制可填写 `MaximumCurvaturePerMeter`，或填写 `MinimumTurningRadiusMeters`；至少必须提供一个有效限制。
+
+```csharp
+var vehicle = new VehicleParameters
+{
+    LengthMeters = 0.80d,
+    WidthMeters = 0.60d,
+    SafetyMarginMeters = 0.05d,
+    MaximumCurvaturePerMeter = 1d / 1.20d,
+};
+```
+
+### 第 5 步：调整搜索配置
+
+默认 `HybridAStarConfiguration` 包含原语长度、积分步长、航向离散、终点容差、代价、节点上限和总超时。若业务需要覆盖默认值，应同时理解安全影响：`MaximumCollisionCheckStepMeters` 不能以牺牲连续碰撞检查精度为代价随意增大。
+
+```csharp
+var configuration = new HybridAStarConfiguration
+{
+    SearchTimeout = TimeSpan.FromSeconds(5d),
+    MaximumExpandedNodes = 200000,
+};
+```
+
+### 第 6 步：调用并消费成功结果
+
+只有 `Success` 可以发布完整路径。`Path` 是稠密点序列；`Segments` 是覆盖整条路径的前进/倒车包含式索引段，可供后续的速度规划或显示模块消费。
+
+```csharp
+var job = new CoarsePathPlanningJob
+{
+    MapRequest = mapRequest,
+    Start = start,
+    Goal = goal,
+    Vehicle = vehicle,
+    Configuration = configuration,
+    GoalDirection = GoalDirectionConstraint.Any,
+};
+
+CoarsePathPlanningJobResult result = _coarsePathService.Plan(job, cancellationToken);
+if (!result.MapResult.Succeeded)
+    ReportMapFailure(result.MapResult.FailureReason);
+else if (result.PlanningResult.Status == PlanningStatus.Success)
+    ConsumeCoarsePath(result.PlanningResult.Path, result.PlanningResult.Segments);
+else
+    ReportPlanningFailure(result.PlanningResult.Diagnostics.TerminationReason);
+```
+
+`CoarsePathPoint.IsGearSwitchPoint` 为 `true` 表示该点是新方向段开始处。换向位置会保留一对位置、航向与弧长相同、方向不同的相邻点；`UnwrappedHeading` 用于跨越 `-pi/pi` 时保持显示连续。
+
+## P1 手动测试与可视化（P1 Manual Tests and Visualization）
+
+P1 在 `Test/MovementTest.CoarsePathTest.cs` 提供只读测试入口。它们共用一个长期存活的 `CoarsePathPlanningService`，只提交规划并绘制结果；不发送底盘、速度或转向命令。
+
+| MovementTest 名称 | 场景 | 预期 |
+| --- | --- | --- |
+| `粗路径规划-显式空图` | 显式允许的空图 | 前进直达成功 |
+| `粗路径规划-单矩形绕行` | 中央矩形阻断直线 | 成功绕障 |
+| `粗路径规划-多来源障碍` | 手工圆形、矩形和 TwoLeg 快照 | 证明多来源经过同一门面 |
+| `粗路径规划-缓存命中` | 重复相同地图输入 | 后续调用显示 `Input` 缓存命中 |
+| `粗路径规划-倒车换向` | 前进起步、倒车到达 | 成功路径含 `IsGearSwitchPoint` |
+| `粗路径规划-无解` | 贯穿地图的障碍带 | 返回 `NoFeasiblePath` 且不发布路径 |
+| `粗路径规划` | 当前 AMR 位姿、人工终点和可选人工障碍物 | 验证手动障碍物、边界、路径与可视化 |
+
+### 固定案例的实时 AMR 锚定
+
+`粗路径规划-显式空图`、`粗路径规划-单矩形绕行`、`粗路径规划-多来源障碍`、`粗路径规划-缓存命中`、`粗路径规划-倒车换向` 和 `粗路径规划-无解` 是六个固定案例。它们不再以写死的世界起点运行：共享运行器在前台仅调用一次 `DetourInterface.getCartLocation()`，校验并冻结本次 AMR 的世界 `X(mm)`、`Y(mm)` 与航向 `deg`，再创建本次规划请求；后台规划期间不会再次读取定位。
+
+`Create(CoarsePathTestScenario scenario, double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)`
+
+该入口把基准案例的起点映射为冻结的 AMR 位姿，并以相同的 `ΔX/ΔY` 平移地图边界、目标、圆形/矩形障碍，以及 TwoLeg 的检测原点。因此，固定案例始终在当前 AMR 附近保留原有的相对几何关系。起点航向严格使用冻结的 AMR 航向；终点航向保持基准案例的“终点航向减起点航向”差值，叠加到当前 AMR 航向后规范化到 `[-pi, pi]`。TwoLeg 只平移检测原点，`DetectionHeadingRadians` 不会因 AMR 航向发生旋转。
+
+`粗路径规划-缓存命中` 只有两次运行冻结到相同的 AMR `X/Y`、从而形成相同的平移后地图输入时，才作为缓存命中场景；AMR 位置移动后，地图输入正常未命中并重建快照。仅 AMR 航向变化不会改变固定案例的地图输入，地图缓存仍可命中。若定位读取为空、抛出异常，或 `X`、`Y`、航向含有 `NaN`/无穷值，运行器不会提交后台规划，状态与 Toast 会显示以“AMR 位姿不可用”开头的诊断原因。
+
+手动 `粗路径规划` 的人工终点、障碍物和超时输入流程保持不变；它不套用固定案例的整体平移规则。
+
+### AMR 位姿、手动终点与障碍物
+
+`CoarsePathPlanningTest` 启动时读取一次 `DetourInterface.getCartLocation()`，将当前 AMR 世界位姿冻结为起点；随后依次输入目标世界 `X(mm)`、`Y(mm)`、航向 `deg`，以及障碍物数量 `0-20`。每个障碍物再依次输入类型和几何参数：
+
+| 类型输入 | 形状 | 输入参数（全部为 mm） |
+| --- | --- | --- |
+| `1` | 圆形 | 几何中心 `X`、`Y` 与半径；半径必须大于 0 |
+| `2` | 轴对齐矩形 | 几何中心 `X`、`Y`、X 向长度、Y 向宽度；两个尺寸必须大于 0 |
+
+矩形只支持 `AxisAlignedRectangle`，不提供旋转角；其中心和长宽由 `ManualCoarsePathObstacle.AxisAlignedRectangle` 表达。圆形由 `ManualCoarsePathObstacle.Circle` 表达。所有无穷、NaN、非数字或不合法尺寸都会在输入阶段拒绝。
+
+`CoarsePathScenarioFactory.CreateManualObstacleDemo` 在唯一边界完成转换：
+
+```text
+AMR/目标 X、Y：mm / 1000 -> m
+AMR/目标航向：deg * PI / 180 -> rad
+```
+
+当数量为 0 时，入口使用显式空图；`CreateManualGoalDemo` 保留为同一零障碍物场景的兼容帮助方法。数量大于 0 时，工厂将 `ManualCoarsePathObstacle` 快照封装成来源 ID 为 `manual-user-input` 的地图输入，并为每次手动快照分配新的 `SourceVersion`，避免错误复用地图缓存。规划边界覆盖起点、终点和每个障碍物的完整轮廓，再增加 8000 mm 余量并按 50 mm 对齐。
+
+手动入口还要求输入一次“粗路径规划总超时”，单位为秒，只接受 `TimeSpan` 可表示范围内的有限正数秒；`0`、负数、NaN、Infinity 或溢出值都会在启动规划前拒绝。该值只覆盖本次 `CoarsePathPlanningJob.Configuration.SearchTimeout`，不会改变固定场景或全局默认值。
+
+此入口使用固定演示车辆：长 `0.80 m`、宽 `0.60 m`、四周安全余量 `0.05 m`、最小转弯半径 `1.20 m`。这些值不是从现场 AMR 配置读取的，判断现场可行性前必须确认车辆参数一致。
+
+`getCartLocation` 在无有效定位时可能阻塞，因此应在定位准备完成的测试环境使用。手动障碍物是测试输入，不能替代现场障碍物来源；零障碍物的显式空图也绝不代表现场不存在障碍物。
+
+### 后台执行、停止与图层
+
+每次测试启动时会创建独立的 `CancellationTokenSource`，以 `Task.Run` 调用门面，并先取消旧会话。`Test()` 不等待任务，也不读取 `Task.Result`；`TestStop` 取消当前令牌、使会话失效并清空图层。已取消任务完成后不会覆盖新会话，也不会显示部分路径。
+
+专用世界坐标 Painter 图层为 `CoarsePathPlanningV1`。它直接读取本次 `PlanningGridMap` 的 `Bounds`、`ResolutionMm`、`SnapshotId` 和 `IsOccupied(row, col)`，因此边界、抽稀网格和占据格与实际规划快照一致，而不是重新绘制原始障碍物。
+
+状态图层显示规划状态、总耗时、`PathSearchElapsed`（路径搜索耗时）、扩展/生成节点数、Open List 峰值、失败原因和固定演示车辆参数。Toast 同时显示两种耗时，并在失败时附加 `TerminationReason`，因此超时、节点上限、无解、碰撞和内部错误不会只显示成泛化失败。
+
+| 颜色 | 可视化元素 |
+| --- | --- |
+| 灰白 | 地图边界与栅格网络 |
+| 暗红 | 占据格 |
+| 绿色 | 起点、前进路径和方向箭头 |
+| 橙色 | 终点、航向和位置容差圈 |
+| 天蓝 | 倒车路径和方向箭头 |
+| 紫色 | 换向点 |
+| 金色 | 已纳入安全余量的车辆检查框 |
+
+自动化已检查 UI 入口的后台、取消和数据来源结构。仍需在实际 Clumsy 界面手动运行“粗路径规划-单矩形绕行”和“粗路径规划”，确认图层交互显示与停止按钮效果。
+
+## 常见错误（Common Errors）
+
+| 现象 | 原因 | 处理 |
+| --- | --- | --- |
+| 起点、终点或障碍物位置相差 1000 倍 | 将 mm 直接传给 `Pose2D` 或把 m 传给地图输入 | Map 输入使用 mm；`Pose2D`、车辆和路径使用 m |
+| 路径朝向错误或旋转异常 | 将 P1 的 deg 直接当作核心 rad | 在 UI/上层边界执行 `deg * PI / 180`，核心只保存 rad |
+| 障碍物已经变化却复用旧地图 | 内容变更后没有递增 `SourceVersion` | 每次来源快照内容变化后增加对应版本号 |
+| 地图创建成功但规划被阻止 | 没有有效障碍物且未显式允许空图 | 提供有效来源；仅在确认安全的演示中设置 `AllowExplicitEmptyMap = true` |
+| 无解、取消或超时后仍尝试使用路径 | 没有检查 `PlanningStatus.Success` | 仅成功时消费 `Path` 和 `Segments`；其他状态读取诊断 |
+| 将粗路径直接下发给车辆 | 粗路径不包含速度、时间、执行控制或实时安全闭环 | 在后续阶段增加平滑、时间参数化、跟踪和独立安全控制 |
+| P1 手动终点表现为空场地安全 | 手动入口使用显式空图演示 | 真实作业必须提供真实障碍物快照，不能复用空图语义 |
+
+## 第一版限制（First-Version Limits）
+
+当前 P0/P1 已提供安全、确定性的粗路径核心和手动结果可视化，但不包含：
+
+- 路径平滑或曲率连续优化；
+- Reeds-Shepp 或 Dubins 精确终点连接；
+- 速度、加速度、时间标注、时间轨迹和路径跟踪控制；
+- 底盘命令、避障闭环、现场传感器采集或实时重规划调度；
+- 横移、蟹行或其他非前进/倒车运动原语；
+- 原地旋转；
+- 真实作业地图接入、交互式场景编辑和 Release 性能/资源/确定性基准。
+
+当前只生成汽车式恒曲率前进/倒车原语，并允许在原语边界换向；未实现的 Reeds-Shepp、横移、蟹行和原地旋转是整个粗规划核心的第一版能力边界，不是 MovementTest 单独关闭。
+
+因此，调用方只能把 `Success` 结果视作后续模块的粗路径输入，不能把它当作可直接下发的时间轨迹。

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/BinaryMinHeap.cs

@@ -0,0 +1,146 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 为 Hybrid A* Open List 提供确定性优先级的二叉最小堆。
+/// 排序严格依次比较 F、H、较大的 G 和插入序号；F、H、G 必须为有限且非负的等效米代价。
+/// </summary>
+/// <typeparam name="T">与一组搜索代价关联的节点或条目类型。</typeparam>
+public sealed class BinaryMinHeap<T>
+{
+    private readonly List<HeapEntry> _entries = new List<HeapEntry>();
+    private long _nextInsertionSequence;
+
+    /// <summary>创建空的确定性 Open List 堆。</summary>
+    public BinaryMinHeap()
+    {
+    }
+
+    /// <summary>当前堆内尚未出队的条目数量。</summary>
+    public int Count { get { return _entries.Count; } }
+
+    /// <summary>
+    /// 将一个条目和其搜索排序代价压入堆。
+    /// 参数：item 为关联条目；f、h、g 均为有限且非负的等效米代价。
+    /// 失败：任一代价无效、item 为 null（仅引用类型）或插入序号耗尽时抛出异常。
+    /// </summary>
+    public void Push(T item, double f, double h, double g)
+    {
+        if (ReferenceEquals(item, null)) throw new ArgumentNullException(nameof(item));
+        ValidateCost(f, nameof(f));
+        ValidateCost(h, nameof(h));
+        ValidateCost(g, nameof(g));
+        if (_nextInsertionSequence == long.MaxValue)
+            throw new InvalidOperationException("The binary heap insertion sequence has been exhausted.");
+
+        var entry = new HeapEntry(item, f, h, g, _nextInsertionSequence);
+        _nextInsertionSequence++;
+        _entries.Add(entry);
+        SiftUp(_entries.Count - 1);
+    }
+
+    /// <summary>
+    /// 弹出当前排序最优的条目。
+    /// 返回：按 F、H、较大 G 和插入序号排序后的最小条目；空堆时抛出 <see cref="InvalidOperationException"/>。
+    /// </summary>
+    public T Pop()
+    {
+        if (_entries.Count == 0) throw new InvalidOperationException("The binary heap is empty.");
+
+        HeapEntry result = _entries[0];
+        int lastIndex = _entries.Count - 1;
+        if (lastIndex == 0)
+        {
+            _entries.RemoveAt(0);
+            return result.Item;
+        }
+
+        _entries[0] = _entries[lastIndex];
+        _entries.RemoveAt(lastIndex);
+        SiftDown(0);
+        return result.Item;
+    }
+
+    /// <summary>清空尚未出队的条目；后续插入序号继续单调递增以保持整个实例内的确定性。</summary>
+    public void Clear()
+    {
+        _entries.Clear();
+    }
+
+    private void SiftUp(int index)
+    {
+        while (index > 0)
+        {
+            int parentIndex = (index - 1) / 2;
+            if (Compare(_entries[index], _entries[parentIndex]) >= 0) return;
+            Swap(index, parentIndex);
+            index = parentIndex;
+        }
+    }
+
+    private void SiftDown(int index)
+    {
+        while (true)
+        {
+            int leftChildIndex = index * 2 + 1;
+            if (leftChildIndex >= _entries.Count) return;
+
+            int bestChildIndex = leftChildIndex;
+            int rightChildIndex = leftChildIndex + 1;
+            if (rightChildIndex < _entries.Count && Compare(_entries[rightChildIndex], _entries[leftChildIndex]) < 0)
+                bestChildIndex = rightChildIndex;
+
+            if (Compare(_entries[bestChildIndex], _entries[index]) >= 0) return;
+            Swap(index, bestChildIndex);
+            index = bestChildIndex;
+        }
+    }
+
+    private void Swap(int firstIndex, int secondIndex)
+    {
+        HeapEntry temporary = _entries[firstIndex];
+        _entries[firstIndex] = _entries[secondIndex];
+        _entries[secondIndex] = temporary;
+    }
+
+    private static int Compare(HeapEntry left, HeapEntry right)
+    {
+        int comparison = left.F.CompareTo(right.F);
+        if (comparison != 0) return comparison;
+
+        comparison = left.H.CompareTo(right.H);
+        if (comparison != 0) return comparison;
+
+        comparison = right.G.CompareTo(left.G);
+        if (comparison != 0) return comparison;
+
+        return left.InsertionSequence.CompareTo(right.InsertionSequence);
+    }
+
+    private static void ValidateCost(double value, string parameterName)
+    {
+        if (!NumericGuard.IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Search costs must be finite and non-negative.");
+    }
+
+    private sealed class HeapEntry
+    {
+        public HeapEntry(T item, double f, double h, double g, long insertionSequence)
+        {
+            Item = item;
+            F = f;
+            H = h;
+            G = g;
+            InsertionSequence = insertionSequence;
+        }
+
+        public T Item { get; }
+        public double F { get; }
+        public double H { get; }
+        public double G { get; }
+        public long InsertionSequence { get; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/GoalToleranceChecker.cs

@@ -0,0 +1,61 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>按位置、航向和进入方向约束判定连续位姿是否可以作为目标候选。</summary>
+public static class GoalToleranceChecker
+{
+    /// <summary>
+    /// 判断一个位姿是否满足目标容差和进入方向约束。
+    /// 参数：pose 与 goal 使用世界 m/rad；configuration 提供位置 m 和航向 rad 容差；direction 为候选末段方向；goalDirection 为目标进入方向约束。
+    /// 返回：输入有限、位置距离和最小环形航向误差均不超过容差且方向匹配时为 true；无效输入保守地返回 false。
+    /// </summary>
+    public static bool IsSatisfied(
+        Pose2D pose,
+        Pose2D goal,
+        HybridAStarConfiguration configuration,
+        TravelDirection direction,
+        GoalDirectionConstraint goalDirection)
+    {
+        if (!IsFinitePose(pose) || !IsFinitePose(goal) || configuration == null ||
+            !NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) ||
+            !NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) ||
+            configuration.GoalPositionToleranceMeters < 0d || configuration.GoalHeadingToleranceRadians < 0d ||
+            !IsTravelDirection(direction) || !IsGoalDirection(goalDirection))
+            return false;
+
+        if (!MatchesDirection(direction, goalDirection)) return false;
+
+        double deltaX = pose.X - goal.X;
+        double deltaY = pose.Y - goal.Y;
+        double positionDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        if (!NumericGuard.IsFinite(positionDistanceMeters) || positionDistanceMeters > configuration.GoalPositionToleranceMeters)
+            return false;
+
+        double headingDifferenceRadians = Math.Abs(AngleMath.ShortestSignedDifference(pose.Heading, goal.Heading));
+        return NumericGuard.IsFinite(headingDifferenceRadians) && headingDifferenceRadians <= configuration.GoalHeadingToleranceRadians;
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+
+    private static bool MatchesDirection(TravelDirection direction, GoalDirectionConstraint constraint)
+    {
+        return constraint == GoalDirectionConstraint.Any ||
+            (constraint == GoalDirectionConstraint.Forward && direction == TravelDirection.Forward) ||
+            (constraint == GoalDirectionConstraint.Reverse && direction == TravelDirection.Reverse);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/GridDijkstraHeuristic.cs

@@ -0,0 +1,123 @@
+using System;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 基于不可变栅格地图的目标反向八邻域 Dijkstra 启发式。
+/// 距离单位为 m；对角移动仅在两个对应正交邻格都未占据时允许，以避免从障碍夹角穿越。
+/// </summary>
+public sealed class GridDijkstraHeuristic
+{
+    private readonly PlanningGridMap _map;
+    private readonly double[] _costs;
+
+    /// <summary>
+    /// 从目标栅格预计算所有可达自由格到目标的二维最短距离。
+    /// 参数：map 必须是已就绪的不可变地图；goalRow、goalCol 为地图内且未占据的目标格索引。
+    /// 失败：地图为空、未就绪或目标格无效时抛出异常。
+    /// </summary>
+    public GridDijkstraHeuristic(PlanningGridMap map, int goalRow, int goalCol)
+    {
+        if (!TryCreate(map, goalRow, goalCol, PlanningOperationBudget.Unlimited(CancellationToken.None),
+            out GridDijkstraHeuristic heuristic, out _))
+            throw new InvalidOperationException("Unbounded Dijkstra construction unexpectedly stopped.");
+        _map = heuristic._map;
+        _costs = heuristic._costs;
+    }
+
+    private GridDijkstraHeuristic(PlanningGridMap map, double[] costs)
+    {
+        _map = map;
+        _costs = costs;
+    }
+
+    /// <summary>使用共享预算创建完整二维启发式；停止时不返回部分成本数组。</summary>
+    internal static bool TryCreate(PlanningGridMap map, int goalRow, int goalCol, PlanningOperationBudget budget,
+        out GridDijkstraHeuristic heuristic, out PlanningOperationStopReason stopReason)
+    {
+        if (map == null) throw new ArgumentNullException(nameof(map));
+        if (!map.PlanningReady) throw new ArgumentException("The planning map must be ready.", nameof(map));
+        if (goalRow < 0 || goalRow >= map.Rows || goalCol < 0 || goalCol >= map.Cols)
+            throw new ArgumentOutOfRangeException(nameof(goalRow));
+        if (map.IsOccupied(goalRow, goalCol))
+            throw new ArgumentException("The goal grid cell must be free.", nameof(goalRow));
+        if (budget == null) throw new ArgumentNullException(nameof(budget));
+
+        heuristic = null;
+        stopReason = budget.GetStopReason();
+        if (stopReason != PlanningOperationStopReason.None) return false;
+        var costs = new double[checked(map.Rows * map.Cols)];
+        int workItemCount = 0;
+        for (int index = 0; index < costs.Length; index++)
+        {
+            stopReason = budget.CheckEvery(ref workItemCount);
+            if (stopReason != PlanningOperationStopReason.None) return false;
+            costs[index] = double.PositiveInfinity;
+        }
+        if (!TryBuild(map, costs, goalRow, goalCol, budget, ref workItemCount, out stopReason)) return false;
+        heuristic = new GridDijkstraHeuristic(map, costs);
+        stopReason = PlanningOperationStopReason.None;
+        return true;
+    }
+
+    /// <summary>
+    /// 查询指定栅格到构造时目标格的二维最短距离。
+    /// 参数：row、col 为从零开始的栅格索引。
+    /// 返回：单位 m 的有限最短距离；自由格不可达或索引越界时返回正无穷。
+    /// </summary>
+    public double GetCost(int row, int col)
+    {
+        return row < 0 || row >= _map.Rows || col < 0 || col >= _map.Cols
+            ? double.PositiveInfinity
+            : _costs[row * _map.Cols + col];
+    }
+
+    private static bool TryBuild(PlanningGridMap map, double[] costs, int goalRow, int goalCol,
+        PlanningOperationBudget budget, ref int workItemCount, out PlanningOperationStopReason stopReason)
+    {
+        var openList = new BinaryMinHeap<int>();
+        int goalIndex = goalRow * map.Cols + goalCol;
+        costs[goalIndex] = 0d;
+        openList.Push(goalIndex, 0d, 0d, 0d);
+
+        while (openList.Count > 0)
+        {
+            stopReason = budget.CheckEvery(ref workItemCount);
+            if (stopReason != PlanningOperationStopReason.None) return false;
+            int currentIndex = openList.Pop();
+            int currentRow = currentIndex / map.Cols;
+            int currentCol = currentIndex % map.Cols;
+            double currentCost = costs[currentIndex];
+
+            for (int rowOffset = -1; rowOffset <= 1; rowOffset++)
+            for (int colOffset = -1; colOffset <= 1; colOffset++)
+            {
+                stopReason = budget.CheckEvery(ref workItemCount);
+                if (stopReason != PlanningOperationStopReason.None) return false;
+                if (rowOffset == 0 && colOffset == 0) continue;
+
+                int nextRow = currentRow + rowOffset;
+                int nextCol = currentCol + colOffset;
+                if (nextRow < 0 || nextRow >= map.Rows || nextCol < 0 || nextCol >= map.Cols || map.IsOccupied(nextRow, nextCol))
+                    continue;
+
+                bool isDiagonal = rowOffset != 0 && colOffset != 0;
+                if (isDiagonal && (map.IsOccupied(currentRow + rowOffset, currentCol) || map.IsOccupied(currentRow, currentCol + colOffset)))
+                    continue;
+
+                double stepCost = isDiagonal ? Math.Sqrt(2d) * map.ResolutionMeters : map.ResolutionMeters;
+                double candidateCost = currentCost + stepCost;
+                int nextIndex = nextRow * map.Cols + nextCol;
+                if (candidateCost >= costs[nextIndex]) continue;
+
+                costs[nextIndex] = candidateCost;
+                openList.Push(nextIndex, candidateCost, 0d, 0d);
+            }
+        }
+        stopReason = PlanningOperationStopReason.None;
+        return true;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarNode.cs

@@ -0,0 +1,99 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// Hybrid A* 运行期节点。
+/// 节点保留连续位姿和其离散闭集键；搜索过程中不会修改已创建节点，改进代价时会追加新节点并使旧 Open List 条目失效。
+/// </summary>
+public sealed class HybridAStarNode
+{
+    /// <summary>
+    /// 创建一个搜索节点。
+    /// 参数：nodeIndex 为本次搜索内稳定索引；key 为离散状态；pose 为连续车辆中心位姿；parentNodeIndex 为父节点索引，根节点使用 -1；
+    /// incomingPrimitive 为父节点到本节点的原语，根节点为 null；curvaturePerMeter、gCostMeters 和 hCostMeters 均使用规划器的标准单位。
+    /// </summary>
+    public HybridAStarNode(
+        int nodeIndex,
+        HybridAStarNodeKey key,
+        Pose2D pose,
+        int parentNodeIndex,
+        MotionPrimitive incomingPrimitive,
+        double curvaturePerMeter,
+        double gCostMeters,
+        double hCostMeters)
+        : this(nodeIndex, key, pose, parentNodeIndex, incomingPrimitive, curvaturePerMeter, gCostMeters, hCostMeters, false)
+    {
+    }
+
+    /// <summary>
+    /// 创建一个搜索节点，并显式指定它是否为原语内部命中的终点候选。
+    /// 参数：isGoalCandidate 为 true 时，本节点不参与普通离散状态的最优 G 值支配；它仍必须在从 Open List 出队后重新通过连续终点和碰撞检查才能成功。
+    /// </summary>
+    public HybridAStarNode(
+        int nodeIndex,
+        HybridAStarNodeKey key,
+        Pose2D pose,
+        int parentNodeIndex,
+        MotionPrimitive incomingPrimitive,
+        double curvaturePerMeter,
+        double gCostMeters,
+        double hCostMeters,
+        bool isGoalCandidate)
+    {
+        if (key == null) throw new ArgumentNullException(nameof(key));
+        if (pose == null) throw new ArgumentNullException(nameof(pose));
+
+        NodeIndex = nodeIndex;
+        Key = key;
+        Pose = pose;
+        ParentNodeIndex = parentNodeIndex;
+        IncomingPrimitive = incomingPrimitive;
+        CurvaturePerMeter = curvaturePerMeter;
+        GCostMeters = gCostMeters;
+        HCostMeters = hCostMeters;
+        IsGoalCandidate = isGoalCandidate;
+    }
+
+    /// <summary>本次搜索节点数组中的稳定索引。</summary>
+    public int NodeIndex { get; }
+
+    /// <summary>本节点用于 Open/Closed 状态管理的离散键。</summary>
+    public HybridAStarNodeKey Key { get; }
+
+    /// <summary>未量化的连续车辆中心位姿。</summary>
+    public Pose2D Pose { get; }
+
+    /// <summary>父节点稳定索引；根节点为 -1。</summary>
+    public int ParentNodeIndex { get; }
+
+    /// <summary>由父节点驶入本节点的已连续碰撞检查原语；根节点为 null。</summary>
+    public MotionPrimitive IncomingPrimitive { get; }
+
+    /// <summary>与 <see cref="IncomingPrimitive"/> 含义相同的原语别名。</summary>
+    public MotionPrimitive Primitive { get { return IncomingPrimitive; } }
+
+    /// <summary>当前末段采用的车辆曲率，单位 1/m。</summary>
+    public double CurvaturePerMeter { get; }
+
+    /// <summary>当前节点的累计等效米代价。</summary>
+    public double GCostMeters { get; }
+
+    /// <summary>当前节点的二维启发式等效米代价。</summary>
+    public double HCostMeters { get; }
+
+    /// <summary>
+    /// 本节点是否由原语内部首次满足终点条件而生成。
+    /// 终点候选必须保留连续位姿，不能因同一离散键的普通低代价节点而被 Open List 准入规则压制。
+    /// </summary>
+    public bool IsGoalCandidate { get; }
+
+    /// <summary>Open List 排序使用的总等效米代价。</summary>
+    public double FCostMeters { get { return GCostMeters + HCostMeters; } }
+
+    /// <summary>本节点末段的行驶方向。</summary>
+    public TravelDirection Direction { get { return Key.Direction; } }
+
+    /// <summary>本节点末段的曲率等级索引。</summary>
+    public int CurvatureLevelIndex { get { return Key.CurvatureLevelIndex; } }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarNodeKey.cs

@@ -0,0 +1,67 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// Hybrid A* 闭集使用的离散状态键。
+/// 位置使用地图行列索引；航向、行驶方向和曲率等级共同保留车辆运动学状态，避免把同一栅格中的不同可达姿态错误合并。
+/// </summary>
+public sealed class HybridAStarNodeKey : IEquatable<HybridAStarNodeKey>
+{
+    /// <summary>
+    /// 创建一个离散 Hybrid A* 状态键。
+    /// 参数：row、column 为零开始的地图行列；headingIndex 为航向桶；direction 为末段行驶方向；curvatureLevelIndex 为末段曲率等级。
+    /// </summary>
+    public HybridAStarNodeKey(int row, int column, int headingIndex, TravelDirection direction, int curvatureLevelIndex)
+    {
+        Row = row;
+        Column = column;
+        HeadingIndex = headingIndex;
+        Direction = direction;
+        CurvatureLevelIndex = curvatureLevelIndex;
+    }
+
+    /// <summary>车辆中心所在的零开始地图行索引。</summary>
+    public int Row { get; }
+
+    /// <summary>车辆中心所在的零开始地图列索引。</summary>
+    public int Column { get; }
+
+    /// <summary>与 <see cref="Column"/> 含义相同的列索引别名。</summary>
+    public int Col { get { return Column; } }
+
+    /// <summary>根据配置航向分辨率量化后的航向桶索引。</summary>
+    public int HeadingIndex { get; }
+
+    /// <summary>到达当前节点的最后一段行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>到达当前节点的最后一段曲率等级索引。</summary>
+    public int CurvatureLevelIndex { get; }
+
+    /// <summary>判断另一个键是否表示完全相同的离散搜索状态。</summary>
+    public bool Equals(HybridAStarNodeKey other)
+    {
+        return other != null && Row == other.Row && Column == other.Column && HeadingIndex == other.HeadingIndex &&
+            Direction == other.Direction && CurvatureLevelIndex == other.CurvatureLevelIndex;
+    }
+
+    /// <summary>判断另一个对象是否表示完全相同的离散搜索状态。</summary>
+    public override bool Equals(object obj)
+    {
+        return Equals(obj as HybridAStarNodeKey);
+    }
+
+    /// <summary>返回用于闭集字典的稳定哈希值。</summary>
+    public override int GetHashCode()
+    {
+        unchecked
+        {
+            int hashCode = Row;
+            hashCode = hashCode * 397 ^ Column;
+            hashCode = hashCode * 397 ^ HeadingIndex;
+            hashCode = hashCode * 397 ^ (int)Direction;
+            return hashCode * 397 ^ CurvatureLevelIndex;
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs

@@ -0,0 +1,542 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 单次 Hybrid A* 搜索的只读结果。
+/// 即使失败也会保留已经创建的运行期节点，供上层记录诊断；仅 <see cref="PlanningStatus.Success"/> 时 <see cref="SuccessNodeIndex"/> 有值。
+/// </summary>
+public sealed class HybridAStarSearchResult
+{
+    internal HybridAStarSearchResult(
+        PlanningStatus status,
+        IEnumerable<HybridAStarNode> nodes,
+        int expandedNodeCount,
+        int generatedNodeCount,
+        int reopenedNodeCount,
+        int staleOpenListEntryCount,
+        int peakOpenListCount,
+        int? successNodeIndex,
+        string terminationReason)
+    {
+        Status = status;
+        Nodes = new ReadOnlyCollection<HybridAStarNode>(new List<HybridAStarNode>(nodes ?? Array.Empty<HybridAStarNode>()));
+        ExpandedNodeCount = expandedNodeCount;
+        GeneratedNodeCount = generatedNodeCount;
+        ReopenedNodeCount = reopenedNodeCount;
+        StaleOpenListEntryCount = staleOpenListEntryCount;
+        PeakOpenListCount = peakOpenListCount;
+        SuccessNodeIndex = status == PlanningStatus.Success ? successNodeIndex : null;
+        TerminationReason = status == PlanningStatus.Success ? string.Empty : terminationReason ?? string.Empty;
+    }
+
+    /// <summary>搜索终止状态。</summary>
+    public PlanningStatus Status { get; }
+
+    /// <summary>本次搜索已经创建的全部节点；索引与 <see cref="HybridAStarNode.NodeIndex"/> 一致。</summary>
+    public IReadOnlyList<HybridAStarNode> Nodes { get; }
+
+    /// <summary>实际从 Open List 弹出并扩展的节点数量。</summary>
+    public int ExpandedNodeCount { get; }
+
+    /// <summary>已进入 Open List 的节点数量，包含根节点和因改进代价追加的节点。</summary>
+    public int GeneratedNodeCount { get; }
+
+    /// <summary>更优路径到达已关闭离散状态、并重新放回 Open List 的次数。</summary>
+    public int ReopenedNodeCount { get; }
+
+    /// <summary>从 Open List 弹出后因已有更优普通状态而被丢弃的陈旧条目数量。</summary>
+    public int StaleOpenListEntryCount { get; }
+
+    /// <summary>搜索期间 Open List 持有的最大条目数，包含等待惰性丢弃的旧条目。</summary>
+    public int PeakOpenListCount { get; }
+
+    /// <summary>成功时最后一个从 Open List 弹出且满足终点条件的节点索引；失败时为 null。</summary>
+    public int? SuccessNodeIndex { get; }
+
+    /// <summary>搜索边界记录的原始终止原因；成功时为空字符串，失败时非空。</summary>
+    public string TerminationReason { get; }
+}
+
+/// <summary>
+/// 在不可变 <see cref="PlanningGridMap"/> 上执行前进/倒车恒曲率原语的 Hybrid A* 搜索。
+/// 搜索只消费已准备好的地图快照；所有候选原语均先完成连续扩大车体碰撞检查，再参与 Open List 排序。
+/// </summary>
+public sealed class HybridAStarSearch
+{
+    private const double CostImprovementToleranceMeters = 1e-9d;
+    private readonly MotionPrimitiveGenerator _primitiveGenerator;
+    private readonly SearchCostCalculator _costCalculator;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认原语、代价和碰撞检查实现的搜索器。</summary>
+    public HybridAStarSearch()
+        : this(new MotionPrimitiveGenerator(), new SearchCostCalculator(), new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定协作对象的搜索器，便于在不引入地图或 UI 依赖的情况下测试搜索过程。</summary>
+    public HybridAStarSearch(
+        MotionPrimitiveGenerator primitiveGenerator,
+        SearchCostCalculator costCalculator,
+        FootprintCollisionChecker collisionChecker)
+    {
+        _primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
+        _costCalculator = costCalculator ?? throw new ArgumentNullException(nameof(costCalculator));
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 执行一次不可变地图上的 Hybrid A* 搜索。
+    /// 参数：request 提供地图、起终点、车辆和搜索配置；cancellationToken 在每次节点扩展前检查。
+    /// 返回：成功仅在满足目标约束的候选已从 Open List 弹出时报告；任一失败状态均不报告成功节点。
+    /// </summary>
+    public HybridAStarSearchResult Search(PlanningRequest request, CancellationToken cancellationToken)
+    {
+        TimeSpan timeout = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
+            ? request.Configuration.SearchTimeout
+            : TimeSpan.Zero;
+        PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
+            ? new PlanningOperationBudget(cancellationToken, timeout)
+            : PlanningOperationBudget.Unlimited(cancellationToken);
+        return Search(request, budget);
+    }
+
+    /// <summary>使用门面传入的共享预算执行搜索；预算从整次规划开始计时。</summary>
+    internal HybridAStarSearchResult Search(PlanningRequest request, PlanningOperationBudget budget)
+    {
+        var nodes = new List<HybridAStarNode>();
+        int expandedNodeCount = 0;
+        int generatedNodeCount = 0;
+        int reopenedNodeCount = 0;
+        int staleOpenListEntryCount = 0;
+        int peakOpenListCount = 0;
+
+        try
+        {
+            if (budget == null) throw new ArgumentNullException(nameof(budget));
+            PlanningOperationStopReason stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            PlanningStatus validationStatus = ValidateRequest(request);
+            if (validationStatus != PlanningStatus.Success)
+                return CreateResult(validationStatus, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            PlanningGridMap map = request.Map;
+            HybridAStarConfiguration configuration = request.Configuration;
+            VehicleParameters vehicle = request.Vehicle;
+
+            if (!IsFootprintInsideMap(request.Start, map, vehicle))
+                return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Start, map, vehicle, 0d, out _))
+                return CreateResult(PlanningStatus.StartInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!IsFootprintInsideMap(request.Goal, map, vehicle))
+                return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!_collisionChecker.IsPoseCollisionFree(request.Goal, map, vehicle, 0d, out _))
+                return CreateResult(PlanningStatus.GoalInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            stopReason = budget.GetStopReason();
+            if (stopReason != PlanningOperationStopReason.None)
+                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (configuration.MaximumExpandedNodes == 0)
+                return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            if (!map.TryWorldToGrid(request.Goal.X, request.Goal.Y, out int goalRow, out int goalColumn))
+                return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!GridDijkstraHeuristic.TryCreate(map, goalRow, goalColumn, budget, out GridDijkstraHeuristic heuristic, out stopReason))
+                return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+            if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+                return CreateResult(PlanningStatus.InvalidVehicleParameters, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            IReadOnlyList<double> curvatureLevels = _primitiveGenerator.GetCurvatureLevels(vehicle, configuration);
+            if (curvatureLevels.Count == 0)
+                return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            int headingBinCount = GetHeadingBinCount(configuration.HeadingResolutionRadians);
+            int startCurvatureLevelIndex = GetNearestCurvatureLevelIndex(curvatureLevels, request.StartVehicleCurvature);
+            if (startCurvatureLevelIndex < 0)
+                return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            if (!map.TryWorldToGrid(request.Start.X, request.Start.Y, out int startRow, out int startColumn))
+                return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null);
+
+            var openList = new BinaryMinHeap<int>();
+            var bestStates = new Dictionary<HybridAStarNodeKey, NodeLabel>();
+            foreach (TravelDirection startDirection in GetStartDirections(request, configuration))
+            {
+                int headingIndex = AngleMath.ToHeadingIndex(request.Start.Heading, configuration.HeadingResolutionRadians, headingBinCount);
+                var key = new HybridAStarNodeKey(startRow, startColumn, headingIndex, startDirection, startCurvatureLevelIndex);
+                if (bestStates.ContainsKey(key)) continue;
+
+                double hCostMeters = GetHeuristicCost(heuristic, startRow, startColumn, configuration);
+                if (double.IsPositiveInfinity(hCostMeters)) continue;
+                var node = new HybridAStarNode(nodes.Count, key, request.Start, -1, null,
+                    curvatureLevels[startCurvatureLevelIndex], 0d, hCostMeters);
+                nodes.Add(node);
+                bestStates.Add(key, new NodeLabel(node.NodeIndex, 0d, false));
+                openList.Push(node.NodeIndex, node.FCostMeters, node.HCostMeters, node.GCostMeters);
+                peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
+                generatedNodeCount++;
+            }
+
+            if (openList.Count == 0)
+                return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                    staleOpenListEntryCount, peakOpenListCount, null,
+                    "二维启发式标记起点不可达目标，或起始方向无法进入 Open List。");
+
+            while (openList.Count > 0)
+            {
+                stopReason = budget.GetStopReason();
+                if (stopReason != PlanningOperationStopReason.None)
+                    return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, null);
+                if (expandedNodeCount >= configuration.MaximumExpandedNodes)
+                    return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, null);
+
+                int nodeIndex = openList.Pop();
+                HybridAStarNode current = nodes[nodeIndex];
+                NodeLabel currentLabel = null;
+                if (!current.IsGoalCandidate)
+                {
+                    if (!bestStates.TryGetValue(current.Key, out currentLabel) || currentLabel.NodeIndex != nodeIndex || currentLabel.IsClosed)
+                    {
+                        staleOpenListEntryCount++;
+                        continue;
+                    }
+
+                    currentLabel.IsClosed = true;
+                }
+
+                expandedNodeCount++;
+                if (current.IsGoalCandidate)
+                {
+                    if (!GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection) ||
+                        !_collisionChecker.IsPoseCollisionFree(current.Pose, map, vehicle, 0d, out _))
+                        continue;
+
+                    return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
+                }
+
+                if (GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection))
+                    return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                        staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
+
+                for (int curvatureLevelIndex = 0; curvatureLevelIndex < curvatureLevels.Count; curvatureLevelIndex++)
+                {
+                    stopReason = budget.GetStopReason();
+                    if (stopReason != PlanningOperationStopReason.None)
+                        return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                            staleOpenListEntryCount, peakOpenListCount, null);
+                    if (!MotionPrimitiveGenerator.AreCurvatureLevelsAdjacent(current.CurvatureLevelIndex, curvatureLevelIndex)) continue;
+
+                    foreach (TravelDirection direction in GetSuccessorDirections(configuration))
+                    {
+                        MotionPrimitive primitive = _primitiveGenerator.Generate(current.Pose, curvatureLevels[curvatureLevelIndex], direction, request);
+                        if (primitive == null || primitive.ActualLengthMeters <= 0d) continue;
+
+                        if (!map.TryWorldToGrid(primitive.End.X, primitive.End.Y, out int row, out int column)) continue;
+                        double hCostMeters = GetHeuristicCost(heuristic, row, column, configuration);
+                        if (double.IsPositiveInfinity(hCostMeters)) continue;
+
+                        double bodyClearanceMeters = GetMinimumBodyClearance(primitive);
+                        bool isGearSwitch = current.ParentNodeIndex >= 0 && current.Direction != direction;
+                        int curvatureLevelDelta = curvatureLevelIndex - current.CurvatureLevelIndex;
+                        double incrementalCostMeters = _costCalculator.Calculate(
+                            primitive.ActualLengthMeters,
+                            direction,
+                            isGearSwitch,
+                            curvatureLevels[curvatureLevelIndex],
+                            maximumCurvaturePerMeter,
+                            curvatureLevelDelta,
+                            bodyClearanceMeters,
+                            configuration);
+                        double gCostMeters = current.GCostMeters + incrementalCostMeters;
+                        if (!NumericGuard.IsFinite(gCostMeters) || gCostMeters < 0d) continue;
+
+                        int headingIndex = AngleMath.ToHeadingIndex(primitive.End.Heading, configuration.HeadingResolutionRadians, headingBinCount);
+                        var key = new HybridAStarNodeKey(row, column, headingIndex, direction, curvatureLevelIndex);
+                        bestStates.TryGetValue(key, out NodeLabel existingLabel);
+                        var successor = new HybridAStarNode(nodes.Count, key, primitive.End, current.NodeIndex, primitive,
+                            curvatureLevels[curvatureLevelIndex], gCostMeters, hCostMeters, primitive.IsGoalTruncation);
+                        if (existingLabel != null && !ShouldEnqueueSuccessor(successor, existingLabel.GCostMeters)) continue;
+
+                        if (!successor.IsGoalCandidate && existingLabel != null && existingLabel.IsClosed) reopenedNodeCount++;
+                        nodes.Add(successor);
+                        if (!successor.IsGoalCandidate)
+                            bestStates[key] = new NodeLabel(successor.NodeIndex, gCostMeters, false);
+                        openList.Push(successor.NodeIndex, successor.FCostMeters, successor.HCostMeters, successor.GCostMeters);
+                        peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
+                        generatedNodeCount++;
+                    }
+                }
+            }
+
+            return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                staleOpenListEntryCount, peakOpenListCount, null,
+                "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。");
+        }
+        catch (Exception exception)
+        {
+            string reason = "Hybrid A* 搜索内部错误：" + exception.GetType().Name +
+                (string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
+            return CreateResult(PlanningStatus.InternalError, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+                staleOpenListEntryCount, peakOpenListCount, null, reason);
+        }
+    }
+
+    private static HybridAStarSearchResult CreateResult(
+        PlanningStatus status,
+        IEnumerable<HybridAStarNode> nodes,
+        int expandedNodeCount,
+        int generatedNodeCount,
+        int reopenedNodeCount,
+        int staleOpenListEntryCount,
+        int peakOpenListCount,
+        int? successNodeIndex,
+        string terminationReason = null)
+    {
+        string reason = status == PlanningStatus.Success
+            ? string.Empty
+            : terminationReason ?? GetDefaultTerminationReason(status);
+        return new HybridAStarSearchResult(status, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
+            staleOpenListEntryCount, peakOpenListCount, successNodeIndex, reason);
+    }
+
+    private static string GetDefaultTerminationReason(PlanningStatus status)
+    {
+        switch (status)
+        {
+            case PlanningStatus.Cancelled:
+                return "Hybrid A* 搜索已取消。";
+            case PlanningStatus.InvalidRequest:
+                return "Hybrid A* 搜索请求缺少必要对象或包含非法数值。";
+            case PlanningStatus.InvalidMap:
+                return "Hybrid A* 搜索地图结构无效。";
+            case PlanningStatus.MapNotReady:
+                return "Hybrid A* 搜索地图尚未准备好。";
+            case PlanningStatus.InvalidVehicleParameters:
+                return "Hybrid A* 搜索车辆参数无效。";
+            case PlanningStatus.InvalidCurvatureConfiguration:
+                return "Hybrid A* 搜索曲率、离散、代价或资源配置无效。";
+            case PlanningStatus.StartOutsideMap:
+                return "Hybrid A* 搜索起始扩大车体不完全位于地图内。";
+            case PlanningStatus.StartInCollision:
+                return "Hybrid A* 搜索起始扩大车体与障碍物相交或擦边。";
+            case PlanningStatus.GoalOutsideMap:
+                return "Hybrid A* 搜索目标扩大车体不完全位于地图内。";
+            case PlanningStatus.GoalInCollision:
+                return "Hybrid A* 搜索目标扩大车体与障碍物相交或擦边。";
+            case PlanningStatus.SearchTimeout:
+                return "Hybrid A* 搜索使用的总规划预算已耗尽。";
+            case PlanningStatus.SearchNodeLimitExceeded:
+                return "Hybrid A* 搜索达到扩展节点上限。";
+            case PlanningStatus.NoFeasiblePath:
+                return "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。";
+            case PlanningStatus.BacktrackingFailed:
+                return "Hybrid A* 成功节点无法回溯为完整父链。";
+            case PlanningStatus.FinalValidationFailed:
+                return "Hybrid A* 路径未通过最终复核。";
+            case PlanningStatus.InternalError:
+                return "Hybrid A* 搜索发生未预期内部错误。";
+            default:
+                return "Hybrid A* 搜索以未识别状态终止：" + status + "。";
+        }
+    }
+
+    private static PlanningStatus ValidateRequest(PlanningRequest request)
+    {
+        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
+            !IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
+            !IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
+            return PlanningStatus.InvalidRequest;
+
+        PlanningGridMap map = request.Map;
+        if (map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters) || map.Bounds == null)
+            return PlanningStatus.InvalidMap;
+        if (!map.PlanningReady) return PlanningStatus.MapNotReady;
+
+        VehicleParameters vehicle = request.Vehicle;
+        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
+            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+            return PlanningStatus.InvalidVehicleParameters;
+
+        HybridAStarConfiguration configuration = request.Configuration;
+        if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
+            (request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
+            return PlanningStatus.InvalidCurvatureConfiguration;
+
+        return PlanningStatus.Success;
+    }
+
+    private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
+    {
+        return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
+            NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
+            configuration.HeadingResolutionRadians <= 2d * Math.PI &&
+            configuration.CurvatureLevelCount >= 3 && configuration.CurvatureLevelCount % 2 == 1 &&
+            NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) && configuration.GoalPositionToleranceMeters >= 0d &&
+            NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) && configuration.GoalHeadingToleranceRadians >= 0d &&
+            configuration.MaximumExpandedNodes >= 0 && configuration.SearchTimeout >= TimeSpan.Zero &&
+            NumericGuard.IsFinite(configuration.HeuristicWeight) && configuration.HeuristicWeight >= 0d &&
+            NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
+            NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
+            NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) && configuration.CurvatureChangePenaltyMetersPerLevel >= 0d &&
+            NumericGuard.IsFinite(configuration.ClearanceCostWeight) && configuration.ClearanceCostWeight >= 0d &&
+            NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
+    }
+
+    private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
+    {
+        if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
+
+        double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
+        double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
+        double longitudinalX = Math.Cos(pose.Heading);
+        double longitudinalY = Math.Sin(pose.Heading);
+        double lateralX = -longitudinalY;
+        double lateralY = longitudinalX;
+        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
+        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
+        {
+            double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
+            double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
+            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
+        }
+        return true;
+    }
+
+    private static IEnumerable<TravelDirection> GetStartDirections(PlanningRequest request, HybridAStarConfiguration configuration)
+    {
+        if (request.StartDirection.HasValue)
+        {
+            yield return request.StartDirection.Value;
+            yield break;
+        }
+
+        yield return TravelDirection.Forward;
+        if (configuration.AllowReverse) yield return TravelDirection.Reverse;
+    }
+
+    private static IEnumerable<TravelDirection> GetSuccessorDirections(HybridAStarConfiguration configuration)
+    {
+        yield return TravelDirection.Forward;
+        if (configuration.AllowReverse) yield return TravelDirection.Reverse;
+    }
+
+    private static int GetHeadingBinCount(double headingResolutionRadians)
+    {
+        double rawBinCount = Math.Ceiling(2d * Math.PI / headingResolutionRadians);
+        if (!NumericGuard.IsFinite(rawBinCount) || rawBinCount < 1d || rawBinCount > int.MaxValue)
+            throw new ArgumentOutOfRangeException(nameof(headingResolutionRadians));
+        return (int)rawBinCount;
+    }
+
+    private static int GetNearestCurvatureLevelIndex(IReadOnlyList<double> curvatureLevels, double curvaturePerMeter)
+    {
+        int bestIndex = -1;
+        double smallestDifference = double.PositiveInfinity;
+        for (int index = 0; index < curvatureLevels.Count; index++)
+        {
+            double difference = Math.Abs(curvatureLevels[index] - curvaturePerMeter);
+            if (difference >= smallestDifference) continue;
+            smallestDifference = difference;
+            bestIndex = index;
+        }
+        return bestIndex;
+    }
+
+    private static double GetMinimumBodyClearance(MotionPrimitive primitive)
+    {
+        double minimum = double.PositiveInfinity;
+        foreach (double clearanceMeters in primitive.BodyClearancesMeters)
+        {
+            if (double.IsNaN(clearanceMeters) || clearanceMeters < 0d) return 0d;
+            minimum = Math.Min(minimum, clearanceMeters);
+        }
+        return minimum;
+    }
+
+    private static double GetHeuristicCost(GridDijkstraHeuristic heuristic, int row, int column, HybridAStarConfiguration configuration)
+    {
+        double gridCostMeters = heuristic.GetCost(row, column);
+        if (double.IsPositiveInfinity(gridCostMeters)) return gridCostMeters;
+        double weightedCostMeters = gridCostMeters * configuration.HeuristicWeight;
+        return NumericGuard.IsFinite(weightedCostMeters) && weightedCostMeters >= 0d
+            ? weightedCostMeters
+            : double.PositiveInfinity;
+    }
+
+    /// <summary>
+    /// 判断后继是否应进入 Open List。
+    /// 参数：successor 是已完成连续碰撞检查的后继；bestKnownGCostMeters 是相同离散键普通状态的当前最优 G 值。
+    /// 返回：普通状态仅在严格改善最优 G 值时入队；终点候选始终入队，以保留其未量化的连续终点位姿。
+    /// </summary>
+    private static bool ShouldEnqueueSuccessor(HybridAStarNode successor, double bestKnownGCostMeters)
+    {
+        if (successor == null || !NumericGuard.IsFinite(bestKnownGCostMeters)) return false;
+        return successor.IsGoalCandidate || successor.GCostMeters < bestKnownGCostMeters - CostImprovementToleranceMeters;
+    }
+
+    private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
+    {
+        if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
+        if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
+        throw new ArgumentOutOfRangeException(nameof(stopReason));
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+
+    private sealed class NodeLabel
+    {
+        public NodeLabel(int nodeIndex, double gCostMeters, bool isClosed)
+        {
+            NodeIndex = nodeIndex;
+            GCostMeters = gCostMeters;
+            IsClosed = isClosed;
+        }
+
+        public int NodeIndex { get; }
+        public double GCostMeters { get; }
+        public bool IsClosed { get; set; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/MotionPrimitive.cs

@@ -0,0 +1,69 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 一条经连续碰撞检查的恒曲率运动原语。
+/// 位置和长度单位为 m，航向单位为 rad，曲率单位为 1/m；<see cref="Points"/> 不包含起点，只包含按积分顺序产生的后续点。
+/// </summary>
+public sealed class MotionPrimitive
+{
+    /// <summary>
+    /// 创建不可变运动原语。
+    /// 参数：start 为原语起点；direction 为行驶方向；curvaturePerMeter 为恒定曲率；actualLengthMeters 为已实际行驶长度；
+    /// points 与 bodyClearancesMeters 按同一索引保存内部积分位姿和对应的车体净空；isGoalTruncation 表示末点是否首次命中目标容差。
+    /// </summary>
+    public MotionPrimitive(
+        Pose2D start,
+        TravelDirection direction,
+        double curvaturePerMeter,
+        double actualLengthMeters,
+        IEnumerable<Pose2D> points,
+        IEnumerable<double> bodyClearancesMeters,
+        bool isGoalTruncation)
+    {
+        if (start == null) throw new ArgumentNullException(nameof(start));
+        if (points == null) throw new ArgumentNullException(nameof(points));
+        if (bodyClearancesMeters == null) throw new ArgumentNullException(nameof(bodyClearancesMeters));
+
+        var copiedPoints = new List<Pose2D>(points);
+        var copiedClearances = new List<double>(bodyClearancesMeters);
+        if (copiedPoints.Count != copiedClearances.Count)
+            throw new ArgumentException("The point and clearance counts must match.", nameof(bodyClearancesMeters));
+
+        Start = start;
+        Direction = direction;
+        CurvaturePerMeter = curvaturePerMeter;
+        ActualLengthMeters = actualLengthMeters;
+        Points = new ReadOnlyCollection<Pose2D>(copiedPoints);
+        BodyClearancesMeters = new ReadOnlyCollection<double>(copiedClearances);
+        End = copiedPoints.Count == 0 ? start : copiedPoints[copiedPoints.Count - 1];
+        IsGoalTruncation = isGoalTruncation;
+    }
+
+    /// <summary>原语起始车辆几何中心位姿。</summary>
+    public Pose2D Start { get; }
+
+    /// <summary>原语最后一个积分点；零长度终点候选时等于 <see cref="Start"/>。</summary>
+    public Pose2D End { get; }
+
+    /// <summary>原语对应的行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>原语全程采用的恒定车辆曲率，单位 1/m。</summary>
+    public double CurvaturePerMeter { get; }
+
+    /// <summary>从 <see cref="Start"/> 到 <see cref="End"/> 的实际行驶弧长，单位 m。</summary>
+    public double ActualLengthMeters { get; }
+
+    /// <summary>不含起点的连续积分位姿，只读且按行驶顺序排列。</summary>
+    public IReadOnlyList<Pose2D> Points { get; }
+
+    /// <summary>与 <see cref="Points"/> 一一对应的扩大车体保守净空下界，单位 m。</summary>
+    public IReadOnlyList<double> BodyClearancesMeters { get; }
+
+    /// <summary>末点是否因首次满足目标位置、航向和方向约束而截断。</summary>
+    public bool IsGoalTruncation { get; }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/MotionPrimitiveGenerator.cs

@@ -0,0 +1,173 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 生成经解析积分和连续碰撞检查的前进或倒车恒曲率原语。
+/// 每个积分点均依次经过有限数值检查、与前一点之间的扫掠碰撞检查和终点容差检查。
+/// </summary>
+public sealed class MotionPrimitiveGenerator
+{
+    private const double StraightCurvatureThreshold = 1e-12d;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认车辆连续碰撞检查器的原语生成器。</summary>
+    public MotionPrimitiveGenerator()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定连续车辆碰撞检查器的原语生成器。</summary>
+    public MotionPrimitiveGenerator(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 使用完整规划请求生成一条原语。
+    /// 参数：start 为当前连续位姿；curvaturePerMeter 为候选恒定曲率；direction 为前进或倒车；request 提供地图、车辆、配置和目标。
+    /// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；否则返回最大长度不超过配置上限的原语。
+    /// </summary>
+    public MotionPrimitive Generate(Pose2D start, double curvaturePerMeter, TravelDirection direction, PlanningRequest request)
+    {
+        if (request == null) return null;
+        return Generate(start, curvaturePerMeter, direction, request.Map, request.Vehicle, request.Configuration, request.Goal, request.GoalDirection);
+    }
+
+    /// <summary>
+    /// 使用显式地图、车辆、配置和目标生成一条原语。
+    /// 参数：所有位置使用 m/rad，curvaturePerMeter 使用 1/m；goalDirection 限制末段允许的进入方向。
+    /// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；首次命中目标时返回 <see cref="MotionPrimitive.IsGoalTruncation"/> 为 true 的截断原语。
+    /// </summary>
+    public MotionPrimitive Generate(
+        Pose2D start,
+        double curvaturePerMeter,
+        TravelDirection direction,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        HybridAStarConfiguration configuration,
+        Pose2D goal,
+        GoalDirectionConstraint goalDirection)
+    {
+        if (!IsValidInput(start, curvaturePerMeter, direction, map, vehicle, configuration, goal, goalDirection)) return null;
+
+        if (GoalToleranceChecker.IsSatisfied(start, goal, configuration, direction, goalDirection))
+            return new MotionPrimitive(start, direction, curvaturePerMeter, 0d, Array.Empty<Pose2D>(), Array.Empty<double>(), true);
+
+        double pointStepMeters = Math.Min(configuration.IntegrationStepMeters,
+            Math.Min(configuration.MaximumCollisionCheckStepMeters, map.ResolutionMeters / 2d));
+        if (!NumericGuard.IsPositiveFinite(pointStepMeters)) return null;
+
+        var points = new List<Pose2D>();
+        var bodyClearancesMeters = new List<double>();
+        Pose2D previous = start;
+        double actualLengthMeters = 0d;
+        while (actualLengthMeters < configuration.PrimitiveLengthMeters)
+        {
+            double remainingLengthMeters = configuration.PrimitiveLengthMeters - actualLengthMeters;
+            double stepMeters = Math.Min(pointStepMeters, remainingLengthMeters);
+            if (!NumericGuard.IsPositiveFinite(stepMeters)) return null;
+
+            Pose2D next = Integrate(previous, curvaturePerMeter, direction, stepMeters);
+            if (!IsFinitePose(next)) return null;
+            if (!_collisionChecker.IsSweptMotionCollisionFree(previous, next, map, vehicle, stepMeters, out double bodyClearanceMeters)) return null;
+
+            actualLengthMeters += stepMeters;
+            points.Add(next);
+            bodyClearancesMeters.Add(bodyClearanceMeters);
+            if (GoalToleranceChecker.IsSatisfied(next, goal, configuration, direction, goalDirection))
+                return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, true);
+
+            previous = next;
+        }
+
+        return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, false);
+    }
+
+    /// <summary>
+    /// 获取从最大负曲率到最大正曲率均匀分布的候选曲率等级。
+    /// 参数：vehicle 提供保守最大曲率；configuration 的曲率等级数必须为不小于 3 的奇数。
+    /// 返回：输入无效时为空只读列表；有效时长度等于配置等级数且中间等级恒为零曲率。
+    /// </summary>
+    public IReadOnlyList<double> GetCurvatureLevels(VehicleParameters vehicle, HybridAStarConfiguration configuration)
+    {
+        if (configuration == null || configuration.CurvatureLevelCount < 3 || configuration.CurvatureLevelCount % 2 == 0 ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+            return Array.Empty<double>();
+
+        var levels = new double[configuration.CurvatureLevelCount];
+        double increment = 2d * maximumCurvaturePerMeter / (levels.Length - 1d);
+        for (int index = 0; index < levels.Length; index++) levels[index] = -maximumCurvaturePerMeter + increment * index;
+        levels[levels.Length / 2] = 0d;
+        return levels;
+    }
+
+    /// <summary>
+    /// 判断两个曲率等级是否允许在相邻原语间直接切换。
+    /// 参数：previousLevelIndex 与 nextLevelIndex 为从零开始的等级索引。
+    /// 返回：两个索引均非负且最多相差一个等级时为 true。
+    /// </summary>
+    public static bool AreCurvatureLevelsAdjacent(int previousLevelIndex, int nextLevelIndex)
+    {
+        return previousLevelIndex >= 0 && nextLevelIndex >= 0 && Math.Abs(previousLevelIndex - nextLevelIndex) <= 1;
+    }
+
+    private static bool IsValidInput(
+        Pose2D start,
+        double curvaturePerMeter,
+        TravelDirection direction,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        HybridAStarConfiguration configuration,
+        Pose2D goal,
+        GoalDirectionConstraint goalDirection)
+    {
+        if (!IsFinitePose(start) || !IsFinitePose(goal) || map == null || vehicle == null || configuration == null ||
+            !NumericGuard.IsFinite(curvaturePerMeter) || !IsTravelDirection(direction) || !IsGoalDirection(goalDirection) ||
+            !NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) ||
+            !NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) ||
+            !NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
+            !NumericGuard.IsPositiveFinite(map.ResolutionMeters) ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
+            return false;
+
+        return Math.Abs(curvaturePerMeter) <= maximumCurvaturePerMeter;
+    }
+
+    private static Pose2D Integrate(Pose2D previous, double curvaturePerMeter, TravelDirection direction, double stepMeters)
+    {
+        double signedDistanceMeters = direction == TravelDirection.Forward ? stepMeters : -stepMeters;
+        double nextHeadingRadians = AngleMath.NormalizeRadians(previous.Heading + curvaturePerMeter * signedDistanceMeters);
+        if (Math.Abs(curvaturePerMeter) < StraightCurvatureThreshold)
+        {
+            return new Pose2D(
+                previous.X + signedDistanceMeters * Math.Cos(previous.Heading),
+                previous.Y + signedDistanceMeters * Math.Sin(previous.Heading),
+                nextHeadingRadians);
+        }
+
+        return new Pose2D(
+            previous.X + (Math.Sin(nextHeadingRadians) - Math.Sin(previous.Heading)) / curvaturePerMeter,
+            previous.Y - (Math.Cos(nextHeadingRadians) - Math.Cos(previous.Heading)) / curvaturePerMeter,
+            nextHeadingRadians);
+    }
+
+    private static bool IsFinitePose(Pose2D pose)
+    {
+        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
+    }
+
+    private static bool IsTravelDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static bool IsGoalDirection(GoalDirectionConstraint direction)
+    {
+        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/SearchCostCalculator.cs

@@ -0,0 +1,98 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 将运动原语的长度、方向、曲率和净空转换为统一的等效米搜索代价。
+/// 此类型不修改搜索状态；换向标记必须由调用方依据相邻原语的方向关系提供。
+/// </summary>
+public sealed class SearchCostCalculator
+{
+    /// <summary>创建使用调用时配置参数的等效米代价计算器。</summary>
+    public SearchCostCalculator()
+    {
+    }
+
+    /// <summary>
+    /// 计算一条运动原语的等效米增量代价。
+    /// 参数：lengthMeters 为非负弧长 m；direction 为原语方向；isGearSwitch 表示该原语前是否换向；
+    /// curvaturePerMeter 与 maximumCurvaturePerMeter 的单位为 1/m；curvatureLevelDelta 为相邻曲率等级差；
+    /// bodyClearanceMeters 为非负车体保守净空 m，可为正无穷；configuration 提供非负权重和惩罚。
+    /// 返回：有限且非负的等效米代价。
+    /// 失败：任一数值、方向或权重无效时抛出 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    public double Calculate(
+        double lengthMeters,
+        TravelDirection direction,
+        bool isGearSwitch,
+        double curvaturePerMeter,
+        double maximumCurvaturePerMeter,
+        int curvatureLevelDelta,
+        double bodyClearanceMeters,
+        HybridAStarConfiguration configuration)
+    {
+        ValidateInput(lengthMeters, direction, curvaturePerMeter, maximumCurvaturePerMeter, bodyClearanceMeters, configuration);
+
+        double directionMultiplier = direction == TravelDirection.Reverse ? configuration.ReverseCostMultiplier : 1d;
+        double normalizedCurvature = Math.Abs(curvaturePerMeter / maximumCurvaturePerMeter);
+        double clearanceDeficit = GetClearanceDeficit(bodyClearanceMeters, configuration.ClearanceCostDistanceMeters);
+        double levelDelta = Math.Abs((double)curvatureLevelDelta);
+        double motionCost = lengthMeters * directionMultiplier *
+            (1d + configuration.CurvatureMagnitudeWeight * normalizedCurvature +
+                configuration.ClearanceCostWeight * clearanceDeficit);
+        double gearSwitchCost = isGearSwitch ? configuration.GearSwitchPenaltyMeters : 0d;
+        double curvatureChangeCost = configuration.CurvatureChangePenaltyMetersPerLevel * levelDelta;
+        double totalCost = motionCost + gearSwitchCost + curvatureChangeCost;
+        if (!NumericGuard.IsFinite(totalCost) || totalCost < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lengthMeters), "The calculated cost must remain finite and non-negative.");
+
+        return totalCost;
+    }
+
+    private static void ValidateInput(
+        double lengthMeters,
+        TravelDirection direction,
+        double curvaturePerMeter,
+        double maximumCurvaturePerMeter,
+        double bodyClearanceMeters,
+        HybridAStarConfiguration configuration)
+    {
+        if (!NumericGuard.IsFinite(lengthMeters) || lengthMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lengthMeters));
+        if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!NumericGuard.IsFinite(curvaturePerMeter))
+            throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
+        if (!NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter))
+            throw new ArgumentOutOfRangeException(nameof(maximumCurvaturePerMeter));
+        if (Math.Abs(curvaturePerMeter) > maximumCurvaturePerMeter)
+            throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
+        if (double.IsNaN(bodyClearanceMeters) || bodyClearanceMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(bodyClearanceMeters));
+        if (configuration == null) throw new ArgumentNullException(nameof(configuration));
+
+        ValidateNonNegativeFinite(configuration.HeuristicWeight, nameof(configuration.HeuristicWeight));
+        if (!NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier))
+            throw new ArgumentOutOfRangeException(nameof(configuration.ReverseCostMultiplier));
+        ValidateNonNegativeFinite(configuration.GearSwitchPenaltyMeters, nameof(configuration.GearSwitchPenaltyMeters));
+        ValidateNonNegativeFinite(configuration.CurvatureMagnitudeWeight, nameof(configuration.CurvatureMagnitudeWeight));
+        ValidateNonNegativeFinite(configuration.CurvatureChangePenaltyMetersPerLevel, nameof(configuration.CurvatureChangePenaltyMetersPerLevel));
+        ValidateNonNegativeFinite(configuration.ClearanceCostWeight, nameof(configuration.ClearanceCostWeight));
+        if (!NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
+            throw new ArgumentOutOfRangeException(nameof(configuration.ClearanceCostDistanceMeters));
+    }
+
+    private static void ValidateNonNegativeFinite(double value, string parameterName)
+    {
+        if (!NumericGuard.IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(parameterName);
+    }
+
+    private static double GetClearanceDeficit(double bodyClearanceMeters, double clearanceCostDistanceMeters)
+    {
+        if (double.IsPositiveInfinity(bodyClearanceMeters)) return 0d;
+        double deficit = 1d - bodyClearanceMeters / clearanceCostDistanceMeters;
+        return deficit > 0d ? deficit : 0d;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs

@@ -0,0 +1,507 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
+
+/// <summary>Clumsy 粗路径手动测试可选择的固定场景。</summary>
+public enum CoarsePathTestScenario
+{
+    /// <summary>明确允许的空地图直达场景。</summary>
+    ExplicitEmpty,
+
+    /// <summary>由中央矩形阻断直线的绕行场景。</summary>
+    RectangleDetour,
+
+    /// <summary>同时包含手工圆形、矩形与 TwoLeg 快照的多来源场景。</summary>
+    ManualAndTwoLeg,
+
+    /// <summary>与矩形绕行输入完全一致，用于在同一服务中验证输入缓存命中。</summary>
+    CacheHit,
+
+    /// <summary>起步前进、终点倒车进入的换向场景。</summary>
+    ReverseGearSwitch,
+
+    /// <summary>由贯穿边界的障碍带分隔起终点的无解场景。</summary>
+    NoFeasiblePath,
+}
+
+/// <summary>手动障碍物输入支持的世界几何类型。</summary>
+public enum ManualCoarsePathObstacleKind
+{
+    /// <summary>由世界中心和半径定义的圆形障碍物。</summary>
+    Circle,
+
+    /// <summary>由世界中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
+    AxisAlignedRectangle,
+}
+
+/// <summary>
+/// 手动粗路径测试的不可变障碍物输入。
+/// 所有中心和尺寸均使用世界 mm；矩形始终与世界坐标轴平行，不包含旋转角。
+/// </summary>
+public sealed class ManualCoarsePathObstacle
+{
+    private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
+        double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
+    {
+        Kind = kind;
+        CenterXMillimeters = centerXMillimeters;
+        CenterYMillimeters = centerYMillimeters;
+        SizeXMillimeters = sizeXMillimeters;
+        SizeYMillimeters = sizeYMillimeters;
+    }
+
+    /// <summary>障碍物的支持几何类型。</summary>
+    public ManualCoarsePathObstacleKind Kind { get; }
+
+    /// <summary>几何中心世界 X 坐标，单位 mm。</summary>
+    public double CenterXMillimeters { get; }
+
+    /// <summary>几何中心世界 Y 坐标，单位 mm。</summary>
+    public double CenterYMillimeters { get; }
+
+    /// <summary>圆形时为半径，矩形时为 X 方向长度；单位 mm。</summary>
+    public double SizeXMillimeters { get; }
+
+    /// <summary>圆形时为半径，矩形时为 Y 方向宽度；单位 mm。</summary>
+    public double SizeYMillimeters { get; }
+
+    /// <summary>创建圆形障碍物。参数：圆心和半径均使用世界 mm，半径必须为有限正数。</summary>
+    public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
+        double radiusMillimeters)
+    {
+        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
+        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
+        EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
+        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
+            centerYMillimeters, radiusMillimeters, radiusMillimeters);
+    }
+
+    /// <summary>
+    /// 创建轴对齐矩形障碍物。
+    /// 参数：中心、X 方向长度和 Y 方向宽度均使用世界 mm；两个尺寸必须为有限正数。
+    /// </summary>
+    public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
+        double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
+    {
+        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
+        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
+        EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
+        EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
+        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
+            centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
+    }
+
+    private static void EnsureFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
+    }
+
+    private static void EnsurePositiveFinite(double value, string parameterName)
+    {
+        EnsureFinite(value, parameterName);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
+    }
+}
+
+/// <summary>
+/// Clumsy 粗路径测试的纯输入工厂。
+/// 固定场景不读取 UI、传感器、定位或时钟；传入 AMR 位姿的手动入口仅在此处完成世界 mm/deg 到核心 m/rad 的转换。
+/// </summary>
+public static class CoarsePathScenarioFactory
+{
+    private const float MapXMinMillimeters = 0f;
+    private const float MapXMaxMillimeters = 6000f;
+    private const float MapYMinMillimeters = 0f;
+    private const float MapYMaxMillimeters = 4000f;
+    private const float ResolutionMillimeters = 50f;
+    private const double MillimetersPerMeter = 1000d;
+    private const double DegreesToRadians = Math.PI / 180d;
+    private const double ManualMapPaddingMillimeters = 8000d;
+    private const int MaximumManualObstacleCount = 20;
+
+    /// <summary>
+    /// 创建一个新的固定测试业务请求。
+    /// 返回：每次调用都返回独立的可变请求对象，供调用方安全地传入同一个长期存活的规划服务。
+    /// </summary>
+    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
+    {
+        return CreateCore(scenario, null);
+    }
+
+    /// <summary>
+    /// 创建以当前 AMR 世界位姿为起点的固定测试业务请求。
+    /// 参数：X/Y 使用世界 mm，航向使用 deg；地图、目标和障碍物仅随 AMR 坐标平移，TwoLeg 朝向保持不变。
+    /// </summary>
+    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario,
+        double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)
+    {
+        return CreateCore(scenario,
+            new FixedScenarioAnchor(amrXMillimeters, amrYMillimeters, amrHeadingDegrees));
+    }
+
+    private static CoarsePathPlanningJob CreateCore(CoarsePathTestScenario scenario, FixedScenarioAnchor anchor)
+    {
+        switch (scenario)
+        {
+            case CoarsePathTestScenario.ExplicitEmpty:
+                return CreateExplicitEmpty(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.RectangleDetour:
+            case CoarsePathTestScenario.CacheHit:
+                return CreateRectangleDetour(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.ManualAndTwoLeg:
+                return CreateManualAndTwoLeg(FixedScenarioTransform.From(1000d, 1000d, 0d, anchor));
+            case CoarsePathTestScenario.ReverseGearSwitch:
+                return CreateReverseGearSwitch(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.NoFeasiblePath:
+                return CreateNoFeasiblePath(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            default:
+                throw new ArgumentOutOfRangeException(nameof(scenario));
+        }
+    }
+
+    /// <summary>
+    /// 创建传入 AMR 世界位姿和手动世界终点的空图演示请求。
+    /// 参数：X/Y 使用世界 mm，航向使用 deg；返回请求中的 <see cref="Pose2D"/> 使用世界 m/rad。
+    /// 注意：这是坐标、路径和取消流程的演示空图，不能表示现场不存在障碍物。
+    /// </summary>
+    public static CoarsePathPlanningJob CreateManualGoalDemo(
+        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
+        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
+    {
+        return CreateManualObstacleDemo(startXMillimeters, startYMillimeters, startHeadingDegrees,
+            goalXMillimeters, goalYMillimeters, goalHeadingDegrees,
+            Array.Empty<ManualCoarsePathObstacle>(), 0L);
+    }
+
+    /// <summary>
+    /// 创建传入 AMR 世界位姿、手动世界终点和手动障碍物快照的测试请求。
+    /// 参数：位姿 X/Y、障碍物中心和尺寸使用世界 mm，航向使用 deg；返回的 <see cref="Pose2D"/> 使用 m/rad。
+    /// 障碍物非空时 obstacleSnapshotVersion 必须为正数，以避免长期服务错误复用旧地图；零障碍物才创建显式空图演示。
+    /// </summary>
+    public static CoarsePathPlanningJob CreateManualObstacleDemo(
+        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
+        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
+        IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
+    {
+        EnsureFinite(startXMillimeters, nameof(startXMillimeters));
+        EnsureFinite(startYMillimeters, nameof(startYMillimeters));
+        EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
+        EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
+        EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
+        EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));
+
+        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
+        if (obstacles.Count > MaximumManualObstacleCount)
+            throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle count exceeds the supported limit.");
+        if (obstacles.Count != 0 && obstacleSnapshotVersion <= 0L)
+            throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
+
+        return CreateJob(
+            CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters,
+                obstacles, obstacleSnapshotVersion),
+            ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
+            ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees),
+            null,
+            GoalDirectionConstraint.Any);
+    }
+
+    private static CoarsePathPlanningJob CreateExplicitEmpty(FixedScenarioTransform transform)
+    {
+        return CreateJob(
+            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
+            transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d),
+            null,
+            GoalDirectionConstraint.Forward);
+    }
+
+    private static CoarsePathPlanningJob CreateRectangleDetour(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
+            {
+                new AxisAlignedRectangleObstacle(transform.X(2700f), transform.X(3300f),
+                    transform.Y(1200f), transform.Y(2800f)),
+            }),
+        };
+        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
+        // 固定绕行场景保留最优启发式；30 秒覆盖较慢测试环境，UI 仍可随时取消。
+        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
+        return job;
+    }
+
+    private static CoarsePathPlanningJob CreateManualAndTwoLeg(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 2L, true, new IMapObstacle[]
+            {
+                new CircleObstacle(transform.X(2400f), transform.Y(1300f), 220f),
+                new AxisAlignedRectangleObstacle(transform.X(3000f), transform.X(3600f),
+                    transform.Y(2000f), transform.Y(2600f)),
+            }),
+            new TwoLegObstacleSource("two-leg", 1L, true, new TwoLegProjectionInput(true,
+                transform.X(3900f), transform.Y(2500f), 0d,
+                    -180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
+        };
+        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 1d, 0d),
+            transform.Pose(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
+        // 多来源场景的最优绕行会受机器负载影响；放宽演示总预算但保留全部碰撞与目标判定。
+        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(15d);
+        return job;
+    }
+
+    private static CoarsePathPlanningJob CreateReverseGearSwitch(FixedScenarioTransform transform)
+    {
+        return CreateJob(
+            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
+            transform.Pose(1d, 2d, 0d),
+            transform.Pose(4d, 2d, 0d),
+            TravelDirection.Forward,
+            GoalDirectionConstraint.Reverse);
+    }
+
+    private static CoarsePathPlanningJob CreateNoFeasiblePath(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 3L, true, new IMapObstacle[]
+            {
+                new AxisAlignedRectangleObstacle(transform.X(2900f), transform.X(3100f),
+                    transform.Y(0f), transform.Y(4000f)),
+            }),
+        };
+        return CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d),
+            null, GoalDirectionConstraint.Forward);
+    }
+
+    private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
+        TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
+    {
+        return new CoarsePathPlanningJob
+        {
+            MapRequest = mapRequest,
+            Start = start,
+            Goal = goal,
+            Vehicle = new VehicleParameters
+            {
+                LengthMeters = 0.80d,
+                WidthMeters = 0.60d,
+                SafetyMarginMeters = 0.05d,
+                MaximumCurvaturePerMeter = 1d / 1.20d,
+            },
+            Configuration = new HybridAStarConfiguration(),
+            StartDirection = startDirection,
+            GoalDirection = goalDirection,
+        };
+    }
+
+    private static PlanningMapRequest CreateMap(bool allowExplicitEmptyMap, IReadOnlyList<IMapObstacleSource> sources,
+        FixedScenarioTransform transform)
+    {
+        return new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(transform.X(MapXMinMillimeters), transform.X(MapXMaxMillimeters),
+                transform.Y(MapYMinMillimeters), transform.Y(MapYMaxMillimeters)),
+            ResolutionMm = ResolutionMillimeters,
+            ObstacleSources = sources,
+            AllowExplicitEmptyMap = allowExplicitEmptyMap,
+        };
+    }
+
+    private sealed class FixedScenarioAnchor
+    {
+        public FixedScenarioAnchor(double xMillimeters, double yMillimeters, double headingDegrees)
+        {
+            EnsureFinite(xMillimeters, nameof(xMillimeters));
+            EnsureFinite(yMillimeters, nameof(yMillimeters));
+            EnsureFinite(headingDegrees, nameof(headingDegrees));
+            XMillimeters = xMillimeters;
+            YMillimeters = yMillimeters;
+            HeadingRadians = NormalizeRadians((headingDegrees % 360d) * DegreesToRadians);
+        }
+
+        public double XMillimeters { get; }
+        public double YMillimeters { get; }
+        public double HeadingRadians { get; }
+    }
+
+    private sealed class FixedScenarioTransform
+    {
+        private FixedScenarioTransform(double deltaXMillimeters, double deltaYMillimeters, double headingDeltaRadians)
+        {
+            DeltaXMillimeters = deltaXMillimeters;
+            DeltaYMillimeters = deltaYMillimeters;
+            HeadingDeltaRadians = headingDeltaRadians;
+        }
+
+        public double DeltaXMillimeters { get; }
+        public double DeltaYMillimeters { get; }
+        public double HeadingDeltaRadians { get; }
+
+        public static FixedScenarioTransform From(double baselineStartXMillimeters,
+            double baselineStartYMillimeters, double baselineStartHeadingRadians, FixedScenarioAnchor anchor)
+        {
+            if (anchor == null) return new FixedScenarioTransform(0d, 0d, 0d);
+            return new FixedScenarioTransform(anchor.XMillimeters - baselineStartXMillimeters,
+                anchor.YMillimeters - baselineStartYMillimeters,
+                NormalizeRadians(anchor.HeadingRadians - baselineStartHeadingRadians));
+        }
+
+        public float X(float value)
+        {
+            return ToFiniteFloat(value + DeltaXMillimeters, nameof(value));
+        }
+
+        public float Y(float value)
+        {
+            return ToFiniteFloat(value + DeltaYMillimeters, nameof(value));
+        }
+
+        public Pose2D Pose(double xMeters, double yMeters, double headingRadians)
+        {
+            return new Pose2D((xMeters * MillimetersPerMeter + DeltaXMillimeters) / MillimetersPerMeter,
+                (yMeters * MillimetersPerMeter + DeltaYMillimeters) / MillimetersPerMeter,
+                NormalizeRadians(headingRadians + HeadingDeltaRadians));
+        }
+    }
+
+    private static PlanningMapRequest CreateManualDemoMap(double startXMillimeters, double startYMillimeters,
+        double goalXMillimeters, double goalYMillimeters, IReadOnlyList<ManualCoarsePathObstacle> obstacles,
+        long obstacleSnapshotVersion)
+    {
+        double minimumX = Math.Min(startXMillimeters, goalXMillimeters);
+        double maximumX = Math.Max(startXMillimeters, goalXMillimeters);
+        double minimumY = Math.Min(startYMillimeters, goalYMillimeters);
+        double maximumY = Math.Max(startYMillimeters, goalYMillimeters);
+        for (int index = 0; index < obstacles.Count; index++)
+        {
+            ManualCoarsePathObstacle obstacle = obstacles[index] ??
+                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
+            double halfX;
+            double halfY;
+            switch (obstacle.Kind)
+            {
+                case ManualCoarsePathObstacleKind.Circle:
+                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
+                    halfX = obstacle.SizeXMillimeters;
+                    halfY = obstacle.SizeYMillimeters;
+                    break;
+                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
+                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
+                    EnsurePositiveFinite(obstacle.SizeYMillimeters, nameof(obstacles));
+                    halfX = obstacle.SizeXMillimeters / 2d;
+                    halfY = obstacle.SizeYMillimeters / 2d;
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
+            }
+
+            minimumX = Math.Min(minimumX, obstacle.CenterXMillimeters - halfX);
+            maximumX = Math.Max(maximumX, obstacle.CenterXMillimeters + halfX);
+            minimumY = Math.Min(minimumY, obstacle.CenterYMillimeters - halfY);
+            maximumY = Math.Max(maximumY, obstacle.CenterYMillimeters + halfY);
+        }
+
+        float xMin = ToGridLowerBound(minimumX - ManualMapPaddingMillimeters);
+        float xMax = ToGridUpperBound(maximumX + ManualMapPaddingMillimeters);
+        float yMin = ToGridLowerBound(minimumY - ManualMapPaddingMillimeters);
+        float yMax = ToGridUpperBound(maximumY + ManualMapPaddingMillimeters);
+        bool isExplicitEmptyMap = obstacles.Count == 0;
+        return new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(xMin, xMax, yMin, yMax),
+            ResolutionMm = ResolutionMillimeters,
+            ObstacleSources = isExplicitEmptyMap ? Array.Empty<IMapObstacleSource>() :
+                new IMapObstacleSource[]
+                {
+                    new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true,
+                        ConvertManualObstacles(obstacles)),
+                },
+            AllowExplicitEmptyMap = isExplicitEmptyMap,
+        };
+    }
+
+    private static IMapObstacle[] ConvertManualObstacles(IReadOnlyList<ManualCoarsePathObstacle> obstacles)
+    {
+        var result = new IMapObstacle[obstacles.Count];
+        for (int index = 0; index < obstacles.Count; index++)
+        {
+            ManualCoarsePathObstacle obstacle = obstacles[index] ??
+                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
+            switch (obstacle.Kind)
+            {
+                case ManualCoarsePathObstacleKind.Circle:
+                    result[index] = new CircleObstacle(ToFiniteFloat(obstacle.CenterXMillimeters, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.SizeXMillimeters, nameof(obstacles)));
+                    break;
+                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
+                    double halfX = obstacle.SizeXMillimeters / 2d;
+                    double halfY = obstacle.SizeYMillimeters / 2d;
+                    result[index] = new AxisAlignedRectangleObstacle(
+                        ToFiniteFloat(obstacle.CenterXMillimeters - halfX, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterXMillimeters + halfX, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters - halfY, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters + halfY, nameof(obstacles)));
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
+            }
+        }
+        return result;
+    }
+
+    private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
+    {
+        return new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
+            headingDegrees * DegreesToRadians);
+    }
+
+    private static double NormalizeRadians(double angle)
+    {
+        double normalized = angle % (2d * Math.PI);
+        if (normalized <= -Math.PI) return normalized + 2d * Math.PI;
+        return normalized > Math.PI ? normalized - 2d * Math.PI : normalized;
+    }
+
+    private static float ToGridLowerBound(double millimeters)
+    {
+        double rounded = Math.Floor(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
+        return ToFiniteFloat(rounded, nameof(millimeters));
+    }
+
+    private static float ToGridUpperBound(double millimeters)
+    {
+        double rounded = Math.Ceiling(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
+        return ToFiniteFloat(rounded, nameof(millimeters));
+    }
+
+    private static float ToFiniteFloat(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
+            throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
+        return (float)value;
+    }
+
+    private static void EnsureFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
+    }
+
+    private static void EnsurePositiveFinite(double value, string parameterName)
+    {
+        EnsureFinite(value, parameterName);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs

@@ -0,0 +1,628 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Globalization;
+using System.Threading;
+using System.Threading.Tasks;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Pilot;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC;
+
+/// <summary>
+/// 显式空图的粗路径规划测试入口。
+/// 只负责创建纯规划请求；规划、取消与可视化均由共享执行器处理，不会向底盘发送任何命令。
+/// </summary>
+[MovementTest(name = "粗路径规划-显式空图")]
+public sealed class CoarsePathExplicitEmptyTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ExplicitEmpty, "显式空图");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 单矩形绕行的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-单矩形绕行")]
+// TODO：#在该测试下发生了红色矩形栅格碰撞但依然规划成功，需要进一步核实与确认
+public sealed class CoarsePathRectangleDetourTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.RectangleDetour, "单矩形绕行");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 手工圆形、矩形和 TwoLeg 快照组合的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-多来源障碍")]
+public sealed class CoarsePathManualAndTwoLegTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ManualAndTwoLeg, "多来源障碍");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 重复输入地图缓存命中的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-缓存命中")]
+public sealed class CoarsePathCacheHitTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.CacheHit, "缓存命中");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 前进起步、倒车到达并显示换向点的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-倒车换向")]
+public sealed class CoarsePathReverseGearSwitchTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ReverseGearSwitch, "倒车换向");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 障碍带完全隔开起终点的无解粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-无解")]
+public sealed class CoarsePathNoFeasiblePathTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.NoFeasiblePath, "无解障碍带");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 使用当前 AMR 车身几何中心位姿和人工终点的粗路径规划演示入口。
+/// 输入的 X/Y 使用世界 mm、航向使用 deg；进入规划核心前由场景工厂一次性转换为 m/rad。
+/// </summary>
+[MovementTest(name = "粗路径规划")]
+public sealed class CoarsePathPlanningTest : MovementTest
+{
+    private const int MaximumManualObstacleCount = 20;
+    private static long _nextManualObstacleSnapshotVersion;
+
+    /// <summary>
+    /// 读取一次 AMR 当前世界位姿、手动终点和障碍物快照后启动规划。
+    /// 注意：getCartLocation 在无定位时可能阻塞；全部输入会在启动后台任务前冻结，不会被规划线程重复读取。
+    /// </summary>
+    public override void Test()
+    {
+        try
+        {
+            var amrPose = DetourInterface.getCartLocation();
+            double goalXmm = ReadFiniteInput("粗路径终点 X（世界 mm）");
+            double goalYmm = ReadFiniteInput("粗路径终点 Y（世界 mm）");
+            double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
+            TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
+            IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
+            long snapshotVersion = obstacles.Count == 0 ? 0L :
+                Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
+            CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
+                amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
+                obstacles, snapshotVersion);
+            job.Configuration.SearchTimeout = searchTimeout;
+            CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
+        }
+        catch (Exception exception)
+        {
+            CoarsePathPlanningTestRunner.ShowInputFailure(exception);
+        }
+    }
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+
+    private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
+    {
+        int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
+        var obstacles = new List<ManualCoarsePathObstacle>(count);
+        for (int index = 0; index < count; index++)
+        {
+            string label = "障碍物 " + (index + 1);
+            int kind = ReadBoundedIntegerInput(label + " 类型（1圆形，2矩形）", 1, 2);
+            double centerXmm = ReadFiniteInput(label + " 中心 X（世界 mm）");
+            double centerYmm = ReadFiniteInput(label + " 中心 Y（世界 mm）");
+            if (kind == 1)
+            {
+                double radiusMm = ReadPositiveFiniteInput(label + " 半径 r（mm）");
+                obstacles.Add(ManualCoarsePathObstacle.Circle(centerXmm, centerYmm, radiusMm));
+            }
+            else
+            {
+                double lengthXmm = ReadPositiveFiniteInput(label + " X方向长度（mm）");
+                double widthYmm = ReadPositiveFiniteInput(label + " Y方向宽度（mm）");
+                obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerXmm, centerYmm,
+                    lengthXmm, widthYmm));
+            }
+        }
+        return obstacles;
+    }
+
+    private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
+    {
+        object raw = UI.GetInput(prompt);
+        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
+        int value;
+        if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
+            !int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
+            throw new ArgumentException("输入必须是整数：" + prompt);
+        if (value < minimum || value > maximum)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
+        return value;
+    }
+
+    private static double ReadPositiveFiniteInput(string prompt)
+    {
+        double value = ReadFiniteInput(prompt);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
+        return value;
+    }
+
+    private static TimeSpan ReadPositiveTimeoutInput(string prompt)
+    {
+        double timeoutSeconds = ReadFiniteInput(prompt);
+        if (timeoutSeconds <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
+        try
+        {
+            return TimeSpan.FromSeconds(timeoutSeconds);
+        }
+        catch (OverflowException)
+        {
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
+        }
+    }
+
+    private static double ReadFiniteInput(string prompt)
+    {
+        object raw = UI.GetInput(prompt);
+        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
+        double value;
+        if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
+            !double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
+            throw new ArgumentException("输入必须是有限数字：" + prompt);
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentException("输入必须是有限数字：" + prompt);
+        return value;
+    }
+}
+
+/// <summary>
+/// 粗路径 MovementTest 的共享后台会话、停止和绘制实现。
+/// 同一时刻只允许一个会话绘制；启动新会话或停止时会取消旧会话，但不会等待旧任务退出。
+/// </summary>
+internal static class CoarsePathPlanningTestRunner
+{
+    private const string PainterLayerName = "CoarsePathPlanningV1";
+    private const float MillimetersPerMeter = 1000f;
+    private const int MaximumVisibleGridLines = 100;
+
+    private static readonly object SessionSync = new object();
+    private static readonly CoarsePathPlanningService PlanningService = new CoarsePathPlanningService();
+    private static readonly Painter Painter = UI.GetPainter(PainterLayerName, true);
+
+    private static CancellationTokenSource _activeCancellation;
+    private static Task<CoarsePathPlanningJobResult> _activeTask;
+    private static long _nextSessionId;
+    private static long _activeSessionId;
+
+    /// <summary>
+    /// 固定场景启动时冻结的 AMR 位姿。规划后台不会重新读取定位，确保输入一致。
+    /// </summary>
+    private sealed class AmrPoseSnapshot
+    {
+        public AmrPoseSnapshot(double xMillimeters, double yMillimeters, double headingDegrees)
+        {
+            EnsureFiniteAmrValue(xMillimeters, "X");
+            EnsureFiniteAmrValue(yMillimeters, "Y");
+            EnsureFiniteAmrValue(headingDegrees, "航向");
+            XMillimeters = xMillimeters;
+            YMillimeters = yMillimeters;
+            HeadingDegrees = headingDegrees;
+        }
+
+        public double XMillimeters { get; }
+        public double YMillimeters { get; }
+        public double HeadingDegrees { get; }
+
+        public string DisplayText
+        {
+            get
+            {
+                return "AMR 起点：X=" + XMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
+                    " mm，Y=" + YMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
+                    " mm，航向=" + HeadingDegrees.ToString("F1", CultureInfo.InvariantCulture) + " deg";
+            }
+        }
+    }
+
+    /// <summary>
+    /// 创建指定固定场景并将其提交给共享后台服务。
+    /// </summary>
+    internal static void RunScenario(CoarsePathTestScenario scenario, string scenarioName)
+    {
+        try
+        {
+            var pose = DetourInterface.getCartLocation();
+            if (ReferenceEquals(pose, null)) throw new ArgumentException("AMR 位姿为空。");
+            var snapshot = new AmrPoseSnapshot(pose.x, pose.y, pose.th);
+            CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenario,
+                snapshot.XMillimeters, snapshot.YMillimeters, snapshot.HeadingDegrees);
+            Run(scenarioName, job, snapshot);
+        }
+        catch (Exception exception)
+        {
+            ShowInputFailure(new ArgumentException("AMR 位姿不可用：" + exception.Message, exception));
+        }
+    }
+
+    /// <summary>
+    /// 提交已经冻结输入的一次规划请求。调用立即返回，结果只会由对应会话的完成回调绘制。
+    /// </summary>
+    internal static void Run(string scenarioName, CoarsePathPlanningJob job)
+    {
+        Run(scenarioName, job, null);
+    }
+
+    private static void Run(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
+    {
+        if (job == null) throw new ArgumentNullException(nameof(job));
+
+        var cancellation = new CancellationTokenSource();
+        CancellationTokenSource previousCancellation;
+        long sessionId;
+        lock (SessionSync)
+        {
+            previousCancellation = _activeCancellation;
+            _activeCancellation = cancellation;
+            _activeTask = null;
+            sessionId = ++_nextSessionId;
+            _activeSessionId = sessionId;
+        }
+
+        // 先发布新会话编号，再取消旧任务，避免旧完成回调覆盖新画面。
+        if (previousCancellation != null) previousCancellation.Cancel();
+        Painter.Clear();
+        DrawPending(scenarioName, job, amrPose);
+
+        Task<CoarsePathPlanningJobResult> task = Task.Run(() => PlanningService.Plan(job, cancellation.Token));
+        lock (SessionSync)
+        {
+            if (_activeSessionId == sessionId) _activeTask = task;
+        }
+
+        _ = task.ContinueWith(completed => Finish(sessionId, scenarioName, job, amrPose, cancellation, completed),
+            TaskScheduler.Default);
+    }
+
+    /// <summary>
+    /// 取消当前会话并清空专用图层；不等待后台任务结束。
+    /// 已取消任务的完成回调只释放资源，不再记录或绘制结果。
+    /// </summary>
+    internal static void Stop()
+    {
+        CancellationTokenSource cancellation;
+        lock (SessionSync)
+        {
+            cancellation = _activeCancellation;
+            _activeCancellation = null;
+            _activeTask = null;
+            _activeSessionId = 0;
+        }
+
+        if (cancellation != null) cancellation.Cancel();
+        Painter.Clear();
+        Hedingben.ToastText("粗路径规划已请求停止。", PainterLayerName);
+    }
+
+    /// <summary>
+    /// 显示输入读取或校验失败，且不改变现有规划任务。
+    /// </summary>
+    internal static void ShowInputFailure(Exception exception)
+    {
+        string message = exception == null ? "未知输入错误。" : exception.Message;
+        Painter.DrawText(Color.LightYellow, "粗路径规划未启动：" + message, 0f, 0f);
+        Hedingben.ToastText("粗路径规划未启动：" + message, PainterLayerName);
+    }
+
+    private static void Finish(long sessionId, string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
+        CancellationTokenSource cancellation, Task<CoarsePathPlanningJobResult> completed)
+    {
+        try
+        {
+            CoarsePathPlanningJobResult result = completed.GetAwaiter().GetResult();
+            bool isCurrent;
+            lock (SessionSync)
+            {
+                isCurrent = _activeSessionId == sessionId;
+                if (isCurrent)
+                {
+                    _activeTask = null;
+                    _activeCancellation = null;
+                }
+            }
+
+            if (!isCurrent) return;
+            DrawResult(scenarioName, job, amrPose, result);
+            Hedingben.ToastText(BuildToastMessage(scenarioName, result), PainterLayerName);
+        }
+        catch (Exception exception)
+        {
+            bool isCurrent;
+            lock (SessionSync)
+            {
+                isCurrent = _activeSessionId == sessionId;
+                if (isCurrent)
+                {
+                    _activeTask = null;
+                    _activeCancellation = null;
+                }
+            }
+
+            if (isCurrent)
+                Hedingben.ToastText("粗路径规划任务异常：" + exception.GetType().Name + "。" + exception.Message,
+                    PainterLayerName);
+        }
+        finally
+        {
+            cancellation.Dispose();
+        }
+    }
+
+    private static void DrawPending(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
+    {
+        DrawPose(job.Start, Color.LimeGreen, "起点");
+        DrawPose(job.Goal, Color.Orange, "终点");
+        Painter.DrawText(Color.LightGray, "场景：" + scenarioName + "（规划中）", 0f, 0f);
+        if (amrPose != null) Painter.DrawText(Color.LightGray, amrPose.DisplayText, 0f, -120f);
+    }
+
+    private static void DrawResult(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
+        CoarsePathPlanningJobResult result)
+    {
+        Painter.Clear();
+        PlanningGridMap map = result.MapResult.Map;
+        if (map != null) DrawMap(map);
+
+        DrawPose(job.Start, Color.LimeGreen, "起点");
+        DrawGoal(job.Goal, job.Configuration, Color.Orange);
+        if (result.PlanningResult.Status == PlanningStatus.Success)
+            DrawPath(result.PlanningResult, job.Vehicle);
+
+        DrawLegend(map);
+        DrawStatus(scenarioName, job, result, map, amrPose);
+    }
+
+    private static void DrawMap(PlanningGridMap map)
+    {
+        float xMin = map.Bounds.XMin;
+        float xMax = map.Bounds.XMax;
+        float yMin = map.Bounds.YMin;
+        float yMax = map.Bounds.YMax;
+        float resolution = map.ResolutionMm;
+        int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) / (double)MaximumVisibleGridLines));
+
+        // 真实 ResolutionMm 决定网格位置，gridStride 只影响显示抽稀。
+        for (int col = 0; col <= map.Cols; col += gridStride)
+        {
+            float x = Math.Min(xMax, xMin + col * resolution);
+            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
+        }
+        for (int row = 0; row <= map.Rows; row += gridStride)
+        {
+            float y = Math.Min(yMax, yMin + row * resolution);
+            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
+        }
+
+        DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
+        if (xMin <= 0f && 0f < xMax) Painter.DrawLine(Color.DimGray, 0f, yMin, 0f, yMax, width: 2);
+        if (yMin <= 0f && 0f < yMax) Painter.DrawLine(Color.DimGray, xMin, 0f, xMax, 0f, width: 2);
+
+        for (int row = 0; row < map.Rows; row++)
+        {
+            for (int col = 0; col < map.Cols; col++)
+            {
+                if (!map.IsOccupied(row, col)) continue;
+                float x = xMin + col * resolution + resolution / 2f;
+                float y = yMin + row * resolution;
+                Painter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + resolution,
+                    width: Math.Max(1, (int)Math.Round(resolution)));
+            }
+        }
+    }
+
+    private static void DrawPose(Pose2D pose, Color color, string label)
+    {
+        if (pose == null) return;
+        float x = ToMillimeters(pose.X);
+        float y = ToMillimeters(pose.Y);
+        Painter.DrawCircle(color, x, y, 80f);
+        DrawHeadingArrow(x, y, pose.Heading, color, 260f);
+        Painter.DrawText(color, label, x + 100f, y + 100f);
+    }
+
+    private static void DrawGoal(Pose2D goal, HybridAStarConfiguration configuration, Color color)
+    {
+        DrawPose(goal, color, "终点");
+        if (goal == null || configuration == null) return;
+        Painter.DrawCircle(Color.FromArgb(150, color), ToMillimeters(goal.X), ToMillimeters(goal.Y),
+            ToMillimeters(configuration.GoalPositionToleranceMeters));
+    }
+
+    private static void DrawPath(PlanningResult planningResult, VehicleParameters vehicle)
+    {
+        if (planningResult.Path == null || planningResult.Path.Count == 0) return;
+
+        int frameStride = Math.Max(1, planningResult.Path.Count / 10);
+        for (int index = 1; index < planningResult.Path.Count; index++)
+        {
+            CoarsePathPoint previous = planningResult.Path[index - 1];
+            CoarsePathPoint current = planningResult.Path[index];
+            Color color = current.Direction == TravelDirection.Forward ? Color.LimeGreen : Color.DeepSkyBlue;
+            Painter.DrawLine(color, ToMillimeters(previous.X), ToMillimeters(previous.Y),
+                ToMillimeters(current.X), ToMillimeters(current.Y), width: 4);
+
+            if (index % frameStride == 0 || current.IsGearSwitchPoint || index == planningResult.Path.Count - 1)
+                DrawVehicleFrame(current, vehicle);
+            if (index % Math.Max(1, frameStride / 2) == 0)
+                DrawHeadingArrow(ToMillimeters(current.X), ToMillimeters(current.Y),
+                    current.Heading + (current.Direction == TravelDirection.Reverse ? Math.PI : 0d), color, 140f);
+            if (!current.IsGearSwitchPoint) continue;
+
+            float x = ToMillimeters(current.X);
+            float y = ToMillimeters(current.Y);
+            Painter.DrawCircle(Color.MediumPurple, x, y, 100f);
+            Painter.DrawText(Color.MediumPurple, "换向", x + 110f, y - 110f);
+        }
+
+        DrawVehicleFrame(planningResult.Path[0], vehicle);
+    }
+
+    private static void DrawVehicleFrame(CoarsePathPoint point, VehicleParameters vehicle)
+    {
+        if (point == null || vehicle == null) return;
+        float halfLength = ToMillimeters(vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters);
+        float halfWidth = ToMillimeters(vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters);
+        float centerX = ToMillimeters(point.X);
+        float centerY = ToMillimeters(point.Y);
+        double cos = Math.Cos(point.Heading);
+        double sin = Math.Sin(point.Heading);
+
+        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, halfWidth, out float frontLeftX, out float frontLeftY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, -halfWidth, out float frontRightX, out float frontRightY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, -halfWidth, out float rearRightX, out float rearRightY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, halfWidth, out float rearLeftX, out float rearLeftY);
+        Painter.DrawLine(Color.Gold, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 2);
+        Painter.DrawLine(Color.Gold, frontRightX, frontRightY, rearRightX, rearRightY, width: 2);
+        Painter.DrawLine(Color.Gold, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 2);
+        Painter.DrawLine(Color.Gold, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 2);
+    }
+
+    private static void TransformVehicleCorner(float centerX, float centerY, double cos, double sin,
+        float longitudinal, float lateral, out float x, out float y)
+    {
+        x = centerX + (float)(cos * longitudinal - sin * lateral);
+        y = centerY + (float)(sin * longitudinal + cos * lateral);
+    }
+
+    private static void DrawHeadingArrow(float x, float y, double headingRadians, Color color, float length)
+    {
+        float endX = x + (float)Math.Cos(headingRadians) * length;
+        float endY = y + (float)Math.Sin(headingRadians) * length;
+        Painter.DrawLine(color, x, y, endX, endY, endArrow: true, width: 3);
+    }
+
+    private static void DrawLegend(PlanningGridMap map)
+    {
+        float x = map == null ? 0f : map.Bounds.XMin + 150f;
+        float y = map == null ? 250f : map.Bounds.YMax - 180f;
+        Painter.DrawText(Color.White, "图例", x, y);
+        DrawLegendItem(x, y - 130f, Color.Gainsboro, "边界 / 栅格");
+        DrawLegendItem(x, y - 260f, Color.Firebrick, "占据格");
+        DrawLegendItem(x, y - 390f, Color.LimeGreen, "起点 / 前进");
+        DrawLegendItem(x, y - 520f, Color.Orange, "终点 / 容差");
+        DrawLegendItem(x, y - 650f, Color.DeepSkyBlue, "倒车");
+        DrawLegendItem(x, y - 780f, Color.MediumPurple, "换向");
+        DrawLegendItem(x, y - 910f, Color.Gold, "扩大车体检查框");
+    }
+
+    private static void DrawLegendItem(float x, float y, Color color, string text)
+    {
+        Painter.DrawLine(color, x, y, x + 90f, y, width: 5);
+        Painter.DrawText(color, text, x + 120f, y - 30f);
+    }
+
+    private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult result, PlanningGridMap map, AmrPoseSnapshot amrPose)
+    {
+        float x = map == null ? 0f : map.Bounds.XMin + 150f;
+        float y = map == null ? -250f : map.Bounds.YMin + 150f;
+        string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
+        string resolution = map == null ? "无" : map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
+        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
+        string reason = diagnostics.TerminationReason ?? string.Empty;
+        string turningRadius = "无";
+        if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(job.Vehicle, out double maximumCurvaturePerMeter))
+            turningRadius = (1d / maximumCurvaturePerMeter).ToString("F2", CultureInfo.InvariantCulture) + " m";
+        Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
+        float detailOffset = 0f;
+        if (amrPose != null)
+        {
+            Painter.DrawText(Color.White, amrPose.DisplayText, x, y + 120f);
+            detailOffset = 120f;
+        }
+        Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" + result.MapResult.CacheHit + "，快照：" + snapshot,
+            x, y + 120f + detailOffset);
+        Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" + result.PlanningResult.Status + "，总耗时：" +
+            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms，路径搜索：" +
+            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms", x, y + 240f + detailOffset);
+        Painter.DrawText(Color.White, "节点：扩展=" + diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) +
+            "，生成=" + diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
+            diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f + detailOffset);
+        if (job != null && job.Vehicle != null)
+        {
+            Painter.DrawText(Color.White, "演示车辆：长=" + job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) +
+                " m，宽=" + job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
+                job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，最小转弯半径=" +
+                turningRadius, x, y + 480f + detailOffset);
+        }
+        if (!string.IsNullOrEmpty(reason))
+            Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f + detailOffset);
+    }
+
+    private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
+    {
+        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
+        string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status + "，规划=" +
+            result.PlanningResult.Status + "，总耗时=" +
+            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms，路径搜索=" +
+            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
+        if (result.PlanningResult.Status != PlanningStatus.Success && !string.IsNullOrEmpty(diagnostics.TerminationReason))
+            message += "，原因=" + diagnostics.TerminationReason;
+        return message + "。";
+    }
+
+    private static void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
+    {
+        Painter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
+        Painter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
+        Painter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
+        Painter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
+    }
+
+    private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
+
+    private static void EnsureFiniteAmrValue(double value, string name)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentException(name + " 必须是有限数。");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/FootprintCollisionChecker.cs

@@ -0,0 +1,160 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>
+/// 对以车辆几何中心表示的扩大矩形执行连续碰撞检查。
+/// 地图查询使用 m；任何地图外车辆部分、占据格相交或擦边均按碰撞处理。
+/// </summary>
+public sealed class FootprintCollisionChecker
+{
+    /// <summary>创建连续车辆碰撞检查器。</summary>
+    public FootprintCollisionChecker()
+    {
+    }
+
+    /// <summary>
+    /// 判断单个车辆位姿是否无碰撞。
+    /// 参数：pose 为车辆几何中心的世界 m/rad 位姿；map 为不可变规划地图；vehicle 为车辆尺寸；
+    /// additionalMarginMeters 为临时额外安全余量，单位 m；bodyClearanceMeters 输出不含该临时余量的保守车体净空下界，单位 m。
+    /// 返回：扩大车辆矩形完整位于地图内且不与任何占据格相交或擦边时为 true；无效输入保守地返回 false。
+    /// </summary>
+    public bool IsPoseCollisionFree(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
+        double additionalMarginMeters, out double bodyClearanceMeters)
+    {
+        bodyClearanceMeters = 0d;
+        if (map == null || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d ||
+            !VehicleFootprint.TryCreate(pose, vehicle, 0d, out VehicleFootprint bodyFootprint) ||
+            !VehicleFootprint.TryCreate(pose, vehicle, additionalMarginMeters, out VehicleFootprint checkedFootprint))
+            return false;
+
+        if (!AreCornersInsideMap(checkedFootprint, map)) return false;
+
+        double centerDistanceMeters = map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y);
+        bodyClearanceMeters = GetBodyClearance(centerDistanceMeters, bodyFootprint.CircumscribedRadiusMeters);
+        if (centerDistanceMeters > checkedFootprint.CircumscribedRadiusMeters) return true;
+
+        return !IntersectsOccupiedCell(checkedFootprint, map);
+    }
+
+    /// <summary>
+    /// 判断两个位姿之间的平移和转向扫掠是否无碰撞。
+    /// 参数：from、to 为世界 m/rad 位姿；maximumCenterStepMeters 为允许的最大中心采样间距，单位 m；
+    /// minimumBodyClearanceMeters 输出沿途不含临时扫掠余量的保守车体净空下界，单位 m。
+    /// 返回：端点和每个分段扫掠均无碰撞时为 true；无效输入、地图外或任一中间碰撞时返回 false。
+    /// </summary>
+    public bool IsSweptMotionCollisionFree(Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
+        double maximumCenterStepMeters, out double minimumBodyClearanceMeters)
+    {
+        minimumBodyClearanceMeters = 0d;
+        if (map == null || from == null || to == null || !NumericGuard.IsFinite(maximumCenterStepMeters) || maximumCenterStepMeters <= 0d ||
+            !NumericGuard.IsFinite(from.X) || !NumericGuard.IsFinite(from.Y) || !NumericGuard.IsFinite(from.Heading) ||
+            !NumericGuard.IsFinite(to.X) || !NumericGuard.IsFinite(to.Y) || !NumericGuard.IsFinite(to.Heading) ||
+            !VehicleFootprint.TryCreate(from, vehicle, 0d, out VehicleFootprint bodyFootprint))
+            return false;
+
+        double allowedStepMeters = Math.Min(maximumCenterStepMeters, map.ResolutionMeters / 2d);
+        if (!NumericGuard.IsPositiveFinite(allowedStepMeters)) return false;
+
+        if (!IsPoseCollisionFree(from, map, vehicle, 0d, out double fromClearanceMeters)) return false;
+        minimumBodyClearanceMeters = fromClearanceMeters;
+
+        double deltaX = to.X - from.X;
+        double deltaY = to.Y - from.Y;
+        double centerDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        if (!NumericGuard.IsFinite(centerDistanceMeters)) return false;
+        double headingDeltaRadians = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
+        if (!NumericGuard.IsFinite(headingDeltaRadians)) return false;
+        double rawSegmentCount = Math.Ceiling(centerDistanceMeters / allowedStepMeters);
+        if (!NumericGuard.IsFinite(rawSegmentCount) || rawSegmentCount > int.MaxValue) return false;
+        int segmentCount = Math.Max(1, (int)rawSegmentCount);
+
+        Pose2D previousPose = from;
+        for (int segment = 1; segment <= segmentCount; segment++)
+        {
+            double endFraction = (double)segment / segmentCount;
+            double middleFraction = ((double)segment - 0.5d) / segmentCount;
+            var currentPose = new Pose2D(
+                from.X + deltaX * endFraction,
+                from.Y + deltaY * endFraction,
+                from.Heading + headingDeltaRadians * endFraction);
+            var middlePose = new Pose2D(
+                from.X + deltaX * middleFraction,
+                from.Y + deltaY * middleFraction,
+                from.Heading + headingDeltaRadians * middleFraction);
+            double segmentDeltaX = currentPose.X - previousPose.X;
+            double segmentDeltaY = currentPose.Y - previousPose.Y;
+            double segmentCenterDisplacementMeters = Math.Sqrt(segmentDeltaX * segmentDeltaX + segmentDeltaY * segmentDeltaY);
+            double segmentHeadingDeltaRadians = currentPose.Heading - previousPose.Heading;
+            double temporaryMarginMeters = 0.5d * (segmentCenterDisplacementMeters +
+                bodyFootprint.CircumscribedRadiusMeters * Math.Abs(segmentHeadingDeltaRadians));
+            if (!NumericGuard.IsFinite(temporaryMarginMeters) ||
+                !IsPoseCollisionFree(middlePose, map, vehicle, temporaryMarginMeters, out double middleClearanceMeters))
+                return false;
+            minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, middleClearanceMeters);
+            previousPose = currentPose;
+        }
+
+        if (!IsPoseCollisionFree(to, map, vehicle, 0d, out double toClearanceMeters)) return false;
+        minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, toClearanceMeters);
+        return true;
+    }
+
+    private static bool AreCornersInsideMap(VehicleFootprint footprint, PlanningGridMap map)
+    {
+        for (int index = 0; index < 4; index++)
+        {
+            footprint.GetCorner(index, out double cornerX, out double cornerY);
+            if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
+        }
+        return true;
+    }
+
+    private static double GetBodyClearance(double centerDistanceMeters, double bodyRadiusMeters)
+    {
+        if (double.IsPositiveInfinity(centerDistanceMeters)) return double.PositiveInfinity;
+        if (!NumericGuard.IsFinite(centerDistanceMeters) || !NumericGuard.IsFinite(bodyRadiusMeters)) return 0d;
+        return Math.Max(0d, centerDistanceMeters - bodyRadiusMeters);
+    }
+
+    private static bool IntersectsOccupiedCell(VehicleFootprint footprint, PlanningGridMap map)
+    {
+        GetCellRange(map, footprint.MinX, footprint.MaxX, footprint.MinY, footprint.MaxY,
+            out int firstRow, out int lastRow, out int firstCol, out int lastCol);
+        for (int row = firstRow; row <= lastRow; row++)
+        for (int col = firstCol; col <= lastCol; col++)
+        {
+            if (!map.IsOccupied(row, col)) continue;
+            GetCellBoundsMeters(map, row, col, out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY);
+            if (OrientedRectangleCellIntersection.Intersects(footprint, cellMinX, cellMaxX, cellMinY, cellMaxY)) return true;
+        }
+        return false;
+    }
+
+    private static void GetCellRange(PlanningGridMap map, double minX, double maxX, double minY, double maxY,
+        out int firstRow, out int lastRow, out int firstCol, out int lastCol)
+    {
+        double minimumMapX = map.Bounds.XMin / 1000d;
+        double minimumMapY = map.Bounds.YMin / 1000d;
+        firstCol = Clamp((int)Math.Floor((minX - minimumMapX) / map.ResolutionMeters) - 1, 0, map.Cols - 1);
+        lastCol = Clamp((int)Math.Floor((maxX - minimumMapX) / map.ResolutionMeters), 0, map.Cols - 1);
+        firstRow = Clamp((int)Math.Floor((minY - minimumMapY) / map.ResolutionMeters) - 1, 0, map.Rows - 1);
+        lastRow = Clamp((int)Math.Floor((maxY - minimumMapY) / map.ResolutionMeters), 0, map.Rows - 1);
+    }
+
+    private static void GetCellBoundsMeters(PlanningGridMap map, int row, int col,
+        out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY)
+    {
+        cellMinX = map.Bounds.XMin / 1000d + col * map.ResolutionMeters;
+        cellMinY = map.Bounds.YMin / 1000d + row * map.ResolutionMeters;
+        cellMaxX = Math.Min(map.Bounds.XMax / 1000d, cellMinX + map.ResolutionMeters);
+        cellMaxY = Math.Min(map.Bounds.YMax / 1000d, cellMinY + map.ResolutionMeters);
+    }
+
+    private static int Clamp(int value, int minimum, int maximum)
+    {
+        return value < minimum ? minimum : value > maximum ? maximum : value;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/OrientedRectangleCellIntersection.cs

@@ -0,0 +1,33 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
+internal static class OrientedRectangleCellIntersection
+{
+    /// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
+    public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
+    {
+        if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
+        double cellCenterX = (cellMinX + cellMaxX) / 2d;
+        double cellCenterY = (cellMinY + cellMaxY) / 2d;
+        double cellHalfX = (cellMaxX - cellMinX) / 2d;
+        double cellHalfY = (cellMaxY - cellMinY) / 2d;
+
+        return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
+            !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
+    }
+
+    private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
+        double axisX, double axisY)
+    {
+        double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
+        double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
+        double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
+            rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
+        double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
+        return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleFootprint.cs

@@ -0,0 +1,90 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>以车辆几何中心为原点的扩大旋转矩形。</summary>
+internal sealed class VehicleFootprint
+{
+    private VehicleFootprint(Pose2D pose, double halfLengthMeters, double halfWidthMeters)
+    {
+        CenterX = pose.X;
+        CenterY = pose.Y;
+        HalfLengthMeters = halfLengthMeters;
+        HalfWidthMeters = halfWidthMeters;
+        AxisLongitudinalX = Math.Cos(pose.Heading);
+        AxisLongitudinalY = Math.Sin(pose.Heading);
+        AxisLateralX = -AxisLongitudinalY;
+        AxisLateralY = AxisLongitudinalX;
+        CircumscribedRadiusMeters = Math.Sqrt(halfLengthMeters * halfLengthMeters + halfWidthMeters * halfWidthMeters);
+
+        double minX = double.PositiveInfinity;
+        double maxX = double.NegativeInfinity;
+        double minY = double.PositiveInfinity;
+        double maxY = double.NegativeInfinity;
+        for (int index = 0; index < 4; index++)
+        {
+            GetCorner(index, out double x, out double y);
+            minX = Math.Min(minX, x);
+            maxX = Math.Max(maxX, x);
+            minY = Math.Min(minY, y);
+            maxY = Math.Max(maxY, y);
+        }
+        MinX = minX;
+        MaxX = maxX;
+        MinY = minY;
+        MaxY = maxY;
+    }
+
+    public double CenterX { get; }
+    public double CenterY { get; }
+    public double HalfLengthMeters { get; }
+    public double HalfWidthMeters { get; }
+    public double AxisLongitudinalX { get; }
+    public double AxisLongitudinalY { get; }
+    public double AxisLateralX { get; }
+    public double AxisLateralY { get; }
+    public double CircumscribedRadiusMeters { get; }
+    public double MinX { get; }
+    public double MaxX { get; }
+    public double MinY { get; }
+    public double MaxY { get; }
+
+    /// <summary>创建包含车辆安全余量和临时扫掠余量的矩形。</summary>
+    public static bool TryCreate(Pose2D pose, VehicleParameters vehicle, double additionalMarginMeters, out VehicleFootprint footprint)
+    {
+        footprint = null;
+        if (pose == null || vehicle == null || !NumericGuard.IsFinite(pose.X) || !NumericGuard.IsFinite(pose.Y) ||
+            !NumericGuard.IsFinite(pose.Heading) || !NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
+            !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) || !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) ||
+            vehicle.SafetyMarginMeters < 0d || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d)
+            return false;
+
+        double totalMarginMeters = vehicle.SafetyMarginMeters + additionalMarginMeters;
+        if (!NumericGuard.IsFinite(totalMarginMeters)) return false;
+        double halfLengthMeters = vehicle.LengthMeters / 2d + totalMarginMeters;
+        double halfWidthMeters = vehicle.WidthMeters / 2d + totalMarginMeters;
+        if (!NumericGuard.IsPositiveFinite(halfLengthMeters) || !NumericGuard.IsPositiveFinite(halfWidthMeters)) return false;
+
+        footprint = new VehicleFootprint(pose, halfLengthMeters, halfWidthMeters);
+        return NumericGuard.IsFinite(footprint.CircumscribedRadiusMeters) && NumericGuard.IsFinite(footprint.MinX) &&
+            NumericGuard.IsFinite(footprint.MaxX) && NumericGuard.IsFinite(footprint.MinY) && NumericGuard.IsFinite(footprint.MaxY);
+    }
+
+    /// <summary>获取指定角点。索引按逆时针顺序为 0 到 3。</summary>
+    public void GetCorner(int index, out double x, out double y)
+    {
+        double longitudinalSign;
+        double lateralSign;
+        switch (index)
+        {
+            case 0: longitudinalSign = 1d; lateralSign = 1d; break;
+            case 1: longitudinalSign = -1d; lateralSign = 1d; break;
+            case 2: longitudinalSign = -1d; lateralSign = -1d; break;
+            case 3: longitudinalSign = 1d; lateralSign = -1d; break;
+            default: throw new ArgumentOutOfRangeException(nameof(index));
+        }
+        x = CenterX + longitudinalSign * HalfLengthMeters * AxisLongitudinalX + lateralSign * HalfWidthMeters * AxisLateralX;
+        y = CenterY + longitudinalSign * HalfLengthMeters * AxisLongitudinalY + lateralSign * HalfWidthMeters * AxisLateralY;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleKinematics.cs

@@ -0,0 +1,40 @@
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>
+/// 从车辆参数提取运动学限制的辅助方法。
+/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
+/// </summary>
+public static class VehicleKinematics
+{
+    /// <summary>
+    /// 尝试获取车辆允许的最大绝对曲率。
+    /// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
+    /// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
+    /// </summary>
+    public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
+    {
+        maximumCurvaturePerMeter = 0d;
+        if (vehicle == null) return false;
+
+        bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
+        bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
+        if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
+        if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
+        if (!hasMaximumCurvature && !hasMinimumRadius) return false;
+
+        if (hasMaximumCurvature && hasMinimumRadius)
+        {
+            maximumCurvaturePerMeter = System.Math.Min(
+                vehicle.MaximumCurvaturePerMeter.Value,
+                1d / vehicle.MinimumTurningRadiusMeters.Value);
+            return true;
+        }
+
+        maximumCurvaturePerMeter = hasMaximumCurvature
+            ? vehicle.MaximumCurvaturePerMeter.Value
+            : 1d / vehicle.MinimumTurningRadiusMeters.Value;
+        return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/CorridorConfiguration.cs

@@ -0,0 +1,43 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 静态走廊采样、偏移和净空配置；距离单位均为 m。
+/// </summary>
+public sealed class CorridorConfiguration
+{
+    /// <summary>
+    /// 沿参考弧长的走廊采样间距；默认值 0.10 m，必须为正有限值，见 <see cref="EmPlannerConfiguration.CreateDefault"/>。
+    /// </summary>
+    public double LongitudinalSampleSpacingMeters { get; set; }
+    /// <summary>
+    /// 每个采样站横向搜索的间距；默认值 0.025 m，必须为正有限值，见 <see cref="EmPlannerConfiguration.CreateDefault"/>。
+    /// </summary>
+    public double LateralSampleSpacingMeters { get; set; }
+    /// <summary>
+    /// 相对参考线允许搜索的最大横向偏移；单位 m，默认值 0.30，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralOffsetMeters { get; set; }
+    /// <summary>
+    /// 碰撞检测之外预留的附加净空；单位 m，默认值 0.02，必须为非负有限值。
+    /// </summary>
+    public double AdditionalClearanceReserveMeters { get; set; }
+    /// <summary>
+    /// 足迹碰撞检测的最大行进步长；单位 m，默认值 0.025，必须为正有限值。
+    /// </summary>
+    public double MaximumCollisionCheckStepMeters { get; set; }
+
+    /// <summary>
+    /// 复制当前走廊配置；输入为当前五个标量，输出为无共享可变状态的可修改快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal CorridorConfiguration Copy()
+    {
+        return new CorridorConfiguration
+        {
+            LongitudinalSampleSpacingMeters = LongitudinalSampleSpacingMeters,
+            LateralSampleSpacingMeters = LateralSampleSpacingMeters,
+            MaximumLateralOffsetMeters = MaximumLateralOffsetMeters,
+            AdditionalClearanceReserveMeters = AdditionalClearanceReserveMeters,
+            MaximumCollisionCheckStepMeters = MaximumCollisionCheckStepMeters,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs

@@ -0,0 +1,149 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// EM 规划器的完整可变配置树；所有子配置均须非空，建议通过 <see cref="CreateDefault"/> 创建协调且彼此独立的默认快照。
+/// </summary>
+public sealed partial class EmPlannerConfiguration
+{
+    /// <summary>
+    /// 重规划节奏、时间窗口和资源上限配置；无直接单位，必须为非空的 <see cref="SchedulingConfiguration"/>，默认值为新的调度配置对象。
+    /// </summary>
+    public SchedulingConfiguration Scheduling { get; set; }
+    /// <summary>
+    /// 世界坐标至 Frenet 坐标的投影容差配置；无直接单位，必须为非空的 <see cref="FrenetConfiguration"/>，默认值为新的 Frenet 配置对象。
+    /// </summary>
+    public FrenetConfiguration Frenet { get; set; }
+    /// <summary>
+    /// 静态无碰撞走廊的采样与净空配置；无直接单位，必须为非空的 <see cref="CorridorConfiguration"/>，默认值为新的走廊配置对象。
+    /// </summary>
+    public CorridorConfiguration Corridor { get; set; }
+    /// <summary>
+    /// LS 横向优化的约束与权重配置；无直接单位，必须为非空的 <see cref="LateralConfiguration"/>，默认值为新的横向配置对象。
+    /// </summary>
+    public LateralConfiguration Lateral { get; set; }
+    /// <summary>
+    /// ST 纵向优化的速度、舒适性与权重配置；无直接单位，必须为非空的 <see cref="LongitudinalConfiguration"/>，默认值为新的纵向配置对象。
+    /// </summary>
+    public LongitudinalConfiguration Longitudinal { get; set; }
+    /// <summary>
+    /// QP 求解器的迭代次数、容差和运行选项；无直接单位，必须为非空的 <see cref="SolverConfiguration"/>，默认值为新的求解器配置对象。
+    /// </summary>
+    public SolverConfiguration Solver { get; set; }
+    /// <summary>
+    /// 发布前几何、运动学和终端姿态校验容差；无直接单位，必须为非空的 <see cref="ValidationConfiguration"/>，默认值为新的校验配置对象。
+    /// </summary>
+    public ValidationConfiguration Validation { get; set; }
+
+    /// <summary>
+    /// 创建用于低速泊车的全套默认配置；每次调用均构造新的子配置和权重对象，因此调用方可修改返回值而不影响其他默认快照。
+    /// </summary>
+    public static EmPlannerConfiguration CreateDefault()
+    {
+        return new EmPlannerConfiguration
+        {
+            Scheduling = new SchedulingConfiguration
+            {
+                ReplanPeriodSeconds = 0.20d,
+                TimeHorizonSeconds = 6d,                // 单次规划的时间窗口，单位 s。
+                DistanceHorizonMeters = 500d,           // 单次规划的最大前向探索距离，单位 m。
+                OutputTimeStepSeconds = 0.05d,
+                SolverTimeoutSeconds = 0.10d,
+                HandoffLookaheadSeconds = 0.30d,
+                MaximumVehicleStateAgeSeconds = 0.20d,
+                MaximumOptimizationTimeStepSeconds = 0.20d,
+                MaximumOptimizationSpatialStepMeters = 0.10d,
+                MaximumOptimizationKnotCount = 401,
+                MaximumPublishedSampleCount = 5001,
+            },
+            Corridor = new CorridorConfiguration
+            {
+                LongitudinalSampleSpacingMeters = 0.10d,
+                LateralSampleSpacingMeters = 0.025d,
+                MaximumLateralOffsetMeters = 0.30d,
+                AdditionalClearanceReserveMeters = 0.02d,
+                MaximumCollisionCheckStepMeters = 0.025d,
+            },
+            Frenet = new FrenetConfiguration
+            {
+                MaximumProjectionDistanceMeters = 0.50d,
+                MinimumFrenetDenominator = 0.20d,
+                BoundaryAnchorToleranceMeters = 1e-8d,
+            },
+            Lateral = new LateralConfiguration
+            {
+                MaximumLateralStepPerIterationMeters = 0.05d,
+                MaximumLateralSlope = 0.50d,
+                MaximumLateralSecondDerivativePerMeter = 1d,
+                MaximumLateralThirdDerivativePerSquareMeter = 2d,
+                Weights = new LateralWeights
+                {
+                    ReferenceOffset = 10d,
+                    HeadingDeviation = 1d,
+                    SecondDerivative = 5d,
+                    ThirdDerivative = 10d,
+                    Curvature = 5d,
+                    CurvatureVariation = 20d,
+                    PreviousTrajectory = 5d,
+                    RollingTerminal = 10d,
+                },
+            },
+            Longitudinal = new LongitudinalConfiguration
+            {
+                MaximumForwardSpeedMetersPerSecond = 1d,
+                MaximumReverseSpeedMetersPerSecond = 0.5d,
+                DesiredForwardSpeedMetersPerSecond = 1d,
+                DesiredReverseSpeedMetersPerSecond = 0.5d,
+                MaximumAccelerationMetersPerSecondSquared = 0.20d,
+                MaximumDecelerationMetersPerSecondSquared = 0.30d,
+                MaximumJerkMetersPerSecondCubed = 0.50d,
+                MaximumLateralAccelerationMetersPerSecondSquared = 0.20d,
+                MaximumCurvatureRatePerMeterPerSecond = 0.50d,
+                StopSpeedToleranceMetersPerSecond = 0.01d,
+                ZeroSpeedHoldSeconds = 0.20d,
+                Weights = new LongitudinalWeights
+                {
+                    ReferenceSpeed = 10d,
+                    Acceleration = 1d,
+                    Jerk = 10d,
+                    PreviousTrajectory = 5d,
+                    TerminalAcceleration = 1d,
+                },
+            },
+            Solver = new SolverConfiguration
+            {
+                MaximumOuterIterations = 5,
+                MaximumOsqpIterations = 4000,
+                AbsoluteTolerance = 1e-5d,
+                RelativeTolerance = 1e-5d,
+                StrictResidualTolerance = 1e-5d,
+                WarmStart = true,
+                Polish = true,
+                NativeVerbose = false,
+            },
+            Validation = new ValidationConfiguration
+            {
+                SpatialToleranceMeters = 1e-8d,
+                KinematicTolerance = 1e-5d,
+                TerminalPositionToleranceMeters = 0d,
+                TerminalYawToleranceRadians = 0d,
+            },
+        };
+    }
+
+    /// <summary>
+    /// 深复制当前配置树；输入为当前实例状态，输出与当前实例不共享非空嵌套对象的可修改快照，原本为 <c>null</c> 的子配置保持为 <c>null</c> 且不产生失败状态。
+    /// </summary>
+    internal EmPlannerConfiguration Copy()
+    {
+        return new EmPlannerConfiguration
+        {
+            Scheduling = Scheduling == null ? null : Scheduling.Copy(),
+            Frenet = Frenet == null ? null : Frenet.Copy(),
+            Corridor = Corridor == null ? null : Corridor.Copy(),
+            Lateral = Lateral == null ? null : Lateral.Copy(),
+            Longitudinal = Longitudinal == null ? null : Longitudinal.Copy(),
+            Solver = Solver == null ? null : Solver.Copy(),
+            Validation = Validation == null ? null : Validation.Copy(),
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/FrenetConfiguration.cs

@@ -0,0 +1,33 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 世界坐标与 Frenet 参考之间投影和边界判定的容差配置。
+/// </summary>
+public sealed class FrenetConfiguration
+{
+    /// <summary>
+    /// 世界姿态投影到方向段所允许的最大欧氏距离；单位 m，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumProjectionDistanceMeters { get; set; }
+    /// <summary>
+    /// Frenet 重建中 <c>1-kappa*l</c> 的最小绝对安全余量；无量纲，默认值 0.20，必须为大于 0 且小于 1 的有限值。
+    /// </summary>
+    public double MinimumFrenetDenominator { get; set; }
+    /// <summary>
+    /// 判断投影是否锚定在段边界的距离容差；单位 m，默认值 1e-8，必须为正有限值。
+    /// </summary>
+    public double BoundaryAnchorToleranceMeters { get; set; }
+
+    /// <summary>
+    /// 复制当前 Frenet 配置；输入为当前三个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal FrenetConfiguration Copy()
+    {
+        return new FrenetConfiguration
+        {
+            MaximumProjectionDistanceMeters = MaximumProjectionDistanceMeters,
+            MinimumFrenetDenominator = MinimumFrenetDenominator,
+            BoundaryAnchorToleranceMeters = BoundaryAnchorToleranceMeters,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralConfiguration.cs

@@ -0,0 +1,43 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// LS 横向优化的离散、收敛、曲率和边界限制配置。
+/// </summary>
+public sealed class LateralConfiguration
+{
+    /// <summary>
+    /// 相邻外层迭代横向解之间允许的最大偏移量；单位 m，默认值 0.05，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralStepPerIterationMeters { get; set; }
+    /// <summary>
+    /// 横向偏移对弧长的一阶导数上限；无量纲，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralSlope { get; set; }
+    /// <summary>
+    /// 横向偏移对弧长的二阶导数上限；单位 1/m，默认值 1，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralSecondDerivativePerMeter { get; set; }
+    /// <summary>
+    /// 横向偏移对弧长的三阶导数上限；单位 1/m²，默认值 2，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralThirdDerivativePerSquareMeter { get; set; }
+    /// <summary>
+    /// 横向 QP 目标函数的权重集合；无直接单位，必须非空且其每项为非负有限值，默认值为新的 <see cref="LateralWeights"/> 对象。
+    /// </summary>
+    public LateralWeights Weights { get; set; }
+
+    /// <summary>
+    /// 复制当前横向配置；输入为当前标量和权重引用，输出为不共享非空权重对象的可修改快照，权重为 <c>null</c> 时保持为空且不产生失败状态。
+    /// </summary>
+    internal LateralConfiguration Copy()
+    {
+        return new LateralConfiguration
+        {
+            MaximumLateralStepPerIterationMeters = MaximumLateralStepPerIterationMeters,
+            MaximumLateralSlope = MaximumLateralSlope,
+            MaximumLateralSecondDerivativePerMeter = MaximumLateralSecondDerivativePerMeter,
+            MaximumLateralThirdDerivativePerSquareMeter = MaximumLateralThirdDerivativePerSquareMeter,
+            Weights = Weights == null ? null : Weights.Copy(),
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralWeights.cs

@@ -0,0 +1,58 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 横向 QP 目标函数的权重快照；权重只影响偏好，不放宽安全约束。
+/// </summary>
+public sealed class LateralWeights
+{
+    /// <summary>
+    /// 惩罚偏离参考线横向偏移的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double ReferenceOffset { get; set; }
+    /// <summary>
+    /// 惩罚横向偏移一阶导数的权重；无量纲，默认值 1，必须为非负有限值。
+    /// </summary>
+    public double HeadingDeviation { get; set; }
+    /// <summary>
+    /// 惩罚横向偏移二阶导数的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double SecondDerivative { get; set; }
+    /// <summary>
+    /// 惩罚横向偏移三阶导数的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double ThirdDerivative { get; set; }
+    /// <summary>
+    /// 惩罚由横向解导出的车辆曲率的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double Curvature { get; set; }
+    /// <summary>
+    /// 惩罚相邻站曲率变化的权重；无量纲，默认值 20，必须为非负有限值。
+    /// </summary>
+    public double CurvatureVariation { get; set; }
+    /// <summary>
+    /// 惩罚偏离上一轮横向轨迹的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double PreviousTrajectory { get; set; }
+    /// <summary>
+    /// 滚动规划末端横向状态的稳定权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double RollingTerminal { get; set; }
+
+    /// <summary>
+    /// 复制当前横向权重；输入为当前八个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal LateralWeights Copy()
+    {
+        return new LateralWeights
+        {
+            ReferenceOffset = ReferenceOffset,
+            HeadingDeviation = HeadingDeviation,
+            SecondDerivative = SecondDerivative,
+            ThirdDerivative = ThirdDerivative,
+            Curvature = Curvature,
+            CurvatureVariation = CurvatureVariation,
+            PreviousTrajectory = PreviousTrajectory,
+            RollingTerminal = RollingTerminal,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs

@@ -0,0 +1,78 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// ST 纵向优化的速度、舒适性和终端保持限制配置；所有数值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 提供低速泊车默认值。
+/// </summary>
+public sealed class LongitudinalConfiguration
+{
+    /// <summary>
+    /// 前进方向允许的最大速度；单位 m/s，默认值 1，必须为正有限值。
+    /// </summary>
+    public double MaximumForwardSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 倒车方向允许的最大速度；单位 m/s，默认值 0.5，必须为正有限值。
+    /// </summary>
+    public double MaximumReverseSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 前进方向的目标巡航速度；单位 m/s，默认值 1，必须为正有限值且不得超过 <see cref="MaximumForwardSpeedMetersPerSecond"/>。
+    /// </summary>
+    public double DesiredForwardSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 倒车方向的目标巡航速度；单位 m/s，默认值 0.5，必须为正有限值且不得超过 <see cref="MaximumReverseSpeedMetersPerSecond"/>。
+    /// </summary>
+    public double DesiredReverseSpeedMetersPerSecond { get; set; }
+    /// <summary>
+    /// 允许的最大纵向加速度；单位 m/s²，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumAccelerationMetersPerSecondSquared { get; set; }
+    /// <summary>
+    /// 允许的最大纵向减速度幅值；单位 m/s²，默认值 0.30，必须为正有限值。
+    /// </summary>
+    public double MaximumDecelerationMetersPerSecondSquared { get; set; }
+    /// <summary>
+    /// 允许的最大纵向加加速度幅值；单位 m/s³，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumJerkMetersPerSecondCubed { get; set; }
+    /// <summary>
+    /// 由速度和曲率共同施加的最大横向加速度；单位 m/s²，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumLateralAccelerationMetersPerSecondSquared { get; set; }
+    /// <summary>
+    /// 车辆曲率随时间变化的上限；单位 1/(m·s)，默认值 0.50，必须为正有限值。
+    /// </summary>
+    public double MaximumCurvatureRatePerMeterPerSecond { get; set; }
+    /// <summary>
+    /// 将进度速度视为停止的容差；单位 m/s，默认值 0.01，必须为非负有限值。
+    /// </summary>
+    public double StopSpeedToleranceMetersPerSecond { get; set; }
+    /// <summary>
+    /// 到达零速度后需保持的时长；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double ZeroSpeedHoldSeconds { get; set; }
+    /// <summary>
+    /// 纵向 QP 目标函数的权重集合；无直接单位，必须非空且其每项为非负有限值，默认值为新的 <see cref="LongitudinalWeights"/> 对象。
+    /// </summary>
+    public LongitudinalWeights Weights { get; set; }
+
+    /// <summary>
+    /// 复制当前纵向配置；输入为当前标量和权重引用，输出为不共享非空权重对象的可修改快照，权重为 <c>null</c> 时保持为空且不产生失败状态。
+    /// </summary>
+    internal LongitudinalConfiguration Copy()
+    {
+        return new LongitudinalConfiguration
+        {
+            MaximumForwardSpeedMetersPerSecond = MaximumForwardSpeedMetersPerSecond,
+            MaximumReverseSpeedMetersPerSecond = MaximumReverseSpeedMetersPerSecond,
+            DesiredForwardSpeedMetersPerSecond = DesiredForwardSpeedMetersPerSecond,
+            DesiredReverseSpeedMetersPerSecond = DesiredReverseSpeedMetersPerSecond,
+            MaximumAccelerationMetersPerSecondSquared = MaximumAccelerationMetersPerSecondSquared,
+            MaximumDecelerationMetersPerSecondSquared = MaximumDecelerationMetersPerSecondSquared,
+            MaximumJerkMetersPerSecondCubed = MaximumJerkMetersPerSecondCubed,
+            MaximumLateralAccelerationMetersPerSecondSquared = MaximumLateralAccelerationMetersPerSecondSquared,
+            MaximumCurvatureRatePerMeterPerSecond = MaximumCurvatureRatePerMeterPerSecond,
+            StopSpeedToleranceMetersPerSecond = StopSpeedToleranceMetersPerSecond,
+            ZeroSpeedHoldSeconds = ZeroSpeedHoldSeconds,
+            Weights = Weights == null ? null : Weights.Copy(),
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalWeights.cs

@@ -0,0 +1,43 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 纵向 QP 目标函数的权重快照；权重只改变偏好而不放宽硬约束。
+/// </summary>
+public sealed class LongitudinalWeights
+{
+    /// <summary>
+    /// 惩罚偏离参考速度的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double ReferenceSpeed { get; set; }
+    /// <summary>
+    /// 惩罚纵向加速度的权重；无量纲，默认值 1，必须为非负有限值。
+    /// </summary>
+    public double Acceleration { get; set; }
+    /// <summary>
+    /// 惩罚纵向加加速度的权重；无量纲，默认值 10，必须为非负有限值。
+    /// </summary>
+    public double Jerk { get; set; }
+    /// <summary>
+    /// 惩罚偏离上一条轨迹速度解的权重；无量纲，默认值 5，必须为非负有限值。
+    /// </summary>
+    public double PreviousTrajectory { get; set; }
+    /// <summary>
+    /// 惩罚末端加速度的权重；无量纲，默认值 1，必须为非负有限值。
+    /// </summary>
+    public double TerminalAcceleration { get; set; }
+
+    /// <summary>
+    /// 复制当前纵向权重；输入为当前五个标量，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal LongitudinalWeights Copy()
+    {
+        return new LongitudinalWeights
+        {
+            ReferenceSpeed = ReferenceSpeed,
+            Acceleration = Acceleration,
+            Jerk = Jerk,
+            PreviousTrajectory = PreviousTrajectory,
+            TerminalAcceleration = TerminalAcceleration,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs

@@ -0,0 +1,73 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// EM 规划的触发节奏、预测窗口、离散上限和发布样本上限配置；默认值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 为低速泊车协调。
+/// </summary>
+public sealed class SchedulingConfiguration
+{
+    /// <summary>
+    /// 两次规划触发之间的周期；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double ReplanPeriodSeconds { get; set; }
+    /// <summary>
+    /// 单次优化覆盖的预测时间窗口；单位 s，默认值 6，必须为正有限值。
+    /// </summary>
+    public double TimeHorizonSeconds { get; set; }
+    /// <summary>
+    /// 单次优化覆盖的最大参考线距离；单位 m，默认值 500，必须为正有限值；滚动规划时还须覆盖制动距离及一个重规划周期内的行程。
+    /// </summary>
+    public double DistanceHorizonMeters { get; set; }
+    /// <summary>
+    /// 发布轨迹相邻样本的时间间隔；单位 s，默认值 0.05，必须为正有限值。
+    /// </summary>
+    public double OutputTimeStepSeconds { get; set; }
+    /// <summary>
+    /// 单次求解允许使用的超时预算；单位 s，默认值 0.10，必须为正有限值。
+    /// </summary>
+    public double SolverTimeoutSeconds { get; set; }
+    /// <summary>
+    /// 与已发布轨迹交接时向前查看的时长；单位 s，默认值 0.30，必须为正有限值。
+    /// </summary>
+    public double HandoffLookaheadSeconds { get; set; }
+    /// <summary>
+    /// 接受车辆状态输入的最大时效；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumVehicleStateAgeSeconds { get; set; }
+    /// <summary>
+    /// 纵向优化结点允许的最大时间间距；单位 s，默认值 0.20，必须为正有限值。
+    /// </summary>
+    public double MaximumOptimizationTimeStepSeconds { get; set; }
+    /// <summary>
+    /// 横向或走廊优化结点允许的最大空间间距；单位 m，默认值 0.10，必须为正有限值。
+    /// </summary>
+    public double MaximumOptimizationSpatialStepMeters { get; set; }
+    /// <summary>
+    /// 一次优化允许的最大结点数量；单位为结点数，默认值 401，必须为不小于 3 的整数。
+    /// </summary>
+    public int MaximumOptimizationKnotCount { get; set; }
+    /// <summary>
+    /// 一条发布轨迹允许的最大样本数量；单位为样本数，默认值 5001，必须为不小于 2 的整数。
+    /// </summary>
+    public int MaximumPublishedSampleCount { get; set; }
+
+    /// <summary>
+    /// 复制当前调度配置；输入为当前标量限制，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal SchedulingConfiguration Copy()
+    {
+        return new SchedulingConfiguration
+        {
+            ReplanPeriodSeconds = ReplanPeriodSeconds,
+            TimeHorizonSeconds = TimeHorizonSeconds,
+            DistanceHorizonMeters = DistanceHorizonMeters,
+            OutputTimeStepSeconds = OutputTimeStepSeconds,
+            SolverTimeoutSeconds = SolverTimeoutSeconds,
+            HandoffLookaheadSeconds = HandoffLookaheadSeconds,
+            MaximumVehicleStateAgeSeconds = MaximumVehicleStateAgeSeconds,
+            MaximumOptimizationTimeStepSeconds = MaximumOptimizationTimeStepSeconds,
+            MaximumOptimizationSpatialStepMeters = MaximumOptimizationSpatialStepMeters,
+            MaximumOptimizationKnotCount = MaximumOptimizationKnotCount,
+            MaximumPublishedSampleCount = MaximumPublishedSampleCount,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SolverConfiguration.cs

@@ -0,0 +1,58 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// LS/ST QP 求解器的迭代预算、收敛容差和运行选项。
+/// </summary>
+public sealed class SolverConfiguration
+{
+    /// <summary>
+    /// 外层顺序凸化最大迭代次数；默认值 5，必须为正整数。
+    /// </summary>
+    public int MaximumOuterIterations { get; set; }
+    /// <summary>
+    /// 单次 OSQP 求解的最大迭代次数；默认值 4000，必须为正整数。
+    /// </summary>
+    public int MaximumOsqpIterations { get; set; }
+    /// <summary>
+    /// 求解器绝对残差容差；默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double AbsoluteTolerance { get; set; }
+    /// <summary>
+    /// 求解器相对残差容差；默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double RelativeTolerance { get; set; }
+    /// <summary>
+    /// 发布前接受解的严格残差容差；默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double StrictResidualTolerance { get; set; }
+    /// <summary>
+    /// 是否将上一轮可用解作为初值；无单位，默认值 <c>true</c>，仅接受布尔值。
+    /// </summary>
+    public bool WarmStart { get; set; }
+    /// <summary>
+    /// 是否请求 OSQP 进行结果修正；无单位，默认值 <c>true</c>，仅接受布尔值。
+    /// </summary>
+    public bool Polish { get; set; }
+    /// <summary>
+    /// 是否启用原生求解器诊断输出；无单位，默认值 <c>false</c>，仅接受布尔值。
+    /// </summary>
+    public bool NativeVerbose { get; set; }
+
+    /// <summary>
+    /// 复制当前求解器配置；输入为当前迭代预算、容差和布尔选项，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal SolverConfiguration Copy()
+    {
+        return new SolverConfiguration
+        {
+            MaximumOuterIterations = MaximumOuterIterations,
+            MaximumOsqpIterations = MaximumOsqpIterations,
+            AbsoluteTolerance = AbsoluteTolerance,
+            RelativeTolerance = RelativeTolerance,
+            StrictResidualTolerance = StrictResidualTolerance,
+            WarmStart = WarmStart,
+            Polish = Polish,
+            NativeVerbose = NativeVerbose,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs

@@ -0,0 +1,38 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 发布前轨迹几何、运动学和终端误差的验收容差配置；默认值由 <see cref="EmPlannerConfiguration.CreateDefault"/> 提供。
+/// </summary>
+public sealed class ValidationConfiguration
+{
+    /// <summary>
+    /// 几何位置、弧长和采样比较使用的空间容差；单位 m，默认值 1e-8，必须为正有限值。
+    /// </summary>
+    public double SpatialToleranceMeters { get; set; }
+    /// <summary>
+    /// 速度、加速度和曲率等无量纲相对比较的容差；无量纲，默认值 1e-5，必须为正有限值。
+    /// </summary>
+    public double KinematicTolerance { get; set; }
+    /// <summary>
+    /// 精确终止模式下终端位置允许的误差；单位 m，默认值 0，必须为非负有限值。
+    /// </summary>
+    public double TerminalPositionToleranceMeters { get; set; }
+    /// <summary>
+    /// 精确终止模式下终端航向允许的误差；单位 rad，默认值 0，必须为 [0, π] 内的有限值。
+    /// </summary>
+    public double TerminalYawToleranceRadians { get; set; }
+
+    /// <summary>
+    /// 复制当前校验配置；输入为当前四个容差，输出为无共享可变状态的标量快照，不对数值作校验且不产生失败状态。
+    /// </summary>
+    internal ValidationConfiguration Copy()
+    {
+        return new ValidationConfiguration
+        {
+            SpatialToleranceMeters = SpatialToleranceMeters,
+            KinematicTolerance = KinematicTolerance,
+            TerminalPositionToleranceMeters = TerminalPositionToleranceMeters,
+            TerminalYawToleranceRadians = TerminalYawToleranceRadians,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs

@@ -0,0 +1,18 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 验证规划契约中必须为有限数的标量输入。
+/// </summary>
+internal static class ContractNumeric
+{
+    /// <summary>
+    /// 拒绝 NaN 或无穷数值。<paramref name="value"/> 的单位由调用方语境决定；<paramref name="parameterName"/> 原样传给异常构造器，方法本身不验证它。
+    /// </summary>
+    public static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "A finite value is required.");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs

@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 表示 EM 规划中参考段终端或滚动窗口终端的边界类型。
+/// </summary>
+public enum EmBoundaryType
+{
+    /// <summary>
+    /// 非边界采样点；不携带终端或换挡边界语义。
+    /// </summary>
+    None,
+    /// <summary>
+    /// 滚动窗口因安全约束形成的停车终点，而非方向段的真实终点。
+    /// </summary>
+    RollingSafetyStop,
+    /// <summary>
+    /// 到达换挡位置前的真实停车边界。
+    /// </summary>
+    GearSwitchApproach,
+    /// <summary>
+    /// 换挡后新方向段开始时的离开边界。
+    /// </summary>
+    GearSwitchDeparture,
+    /// <summary>
+    /// 整条参考路径或当前任务目标的真实终点边界。
+    /// </summary>
+    Goal,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs

@@ -0,0 +1,20 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 指定纵向规划面对当前窗口末端时采用的速度终端语义。
+/// </summary>
+public enum EmLongitudinalMode
+{
+    /// <summary>
+    /// 普通滚动窗口末端；保持连续行驶，不要求在本窗口内停止。
+    /// </summary>
+    RollingContinuation,
+    /// <summary>
+    /// 将真实停车边界纳入窗口，但在可达性不足时仅以可停车方式接近。
+    /// </summary>
+    ApproachStopBoundary,
+    /// <summary>
+    /// 要求在当前窗口内到达真实边界并保持零速。
+    /// </summary>
+    ExactStopAtBoundary,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmMotionModel.cs

@@ -0,0 +1,20 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 请求允许的车辆运动模型；服务依据此值选择或拒绝相应规划分支。
+/// </summary>
+public enum EmMotionModel
+{
+    /// <summary>
+    /// 常规非完整约束车辆，可前进或倒车但不能侧移。
+    /// </summary>
+    NonholonomicForwardReverse,
+    /// <summary>
+    /// 蟹行平移模型；当前 EM 管线不支持时返回明确状态。
+    /// </summary>
+    CrabTranslation,
+    /// <summary>
+    /// 原地旋转模型；当前 EM 管线不支持时返回明确状态。
+    /// </summary>
+    InPlaceRotation,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs

@@ -0,0 +1,329 @@
+using System;
+using System.Diagnostics;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal interface IEmPlanningPublicationAuthorization
+{
+    TimeSpan Remaining { get; }
+
+    bool IsExpired { get; }
+
+    EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
+        CancellationToken coordinatorCallerCancellationToken, Action publish);
+}
+
+internal enum EmPlanningPublicationDecision
+{
+    Published = 0,
+    CallerCancelled = 1,
+    DeadlineExpired = 2,
+}
+
+internal sealed class EmPlanningRequestPublicationAuthorization : IEmPlanningPublicationAuthorization
+{
+    private readonly object gate = new object();
+    private readonly TimeSpan? initialRemaining;
+    private readonly Stopwatch stopwatch;
+    private readonly CancellationToken deadlineToken;
+    private bool callerCancelled;
+    private bool deadlineExpired;
+
+    public EmPlanningRequestPublicationAuthorization(TimeSpan? remaining, CancellationToken deadlineToken)
+    {
+        initialRemaining = remaining;
+        this.deadlineToken = deadlineToken;
+        stopwatch = remaining.HasValue ? Stopwatch.StartNew() : null;
+        deadlineExpired = remaining.HasValue && remaining.Value <= TimeSpan.Zero ||
+            deadlineToken.IsCancellationRequested;
+    }
+
+    public TimeSpan Remaining
+    {
+        get
+        {
+            lock (gate)
+            {
+                ObserveDeadlineInsideGate();
+                return ComputeRemainingInsideGate();
+            }
+        }
+    }
+
+    public bool IsExpired
+    {
+        get
+        {
+            lock (gate)
+            {
+                ObserveDeadlineInsideGate();
+                return deadlineExpired;
+            }
+        }
+    }
+
+    public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
+        CancellationToken coordinatorCallerCancellationToken, Action publish)
+    {
+        if (publish == null)
+            throw new ArgumentNullException(nameof(publish));
+
+        using CancellationTokenRegistration requestCallerRegistration =
+            requestCallerCancellationToken.Register(ObserveCallerCancellation);
+        using CancellationTokenRegistration coordinatorCallerRegistration =
+            coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
+        using CancellationTokenRegistration deadlineRegistration =
+            deadlineToken.Register(ObserveDeadlineCancellation);
+        lock (gate)
+        {
+            if (requestCallerCancellationToken.IsCancellationRequested ||
+                coordinatorCallerCancellationToken.IsCancellationRequested)
+            {
+                callerCancelled = true;
+            }
+            ObserveDeadlineInsideGate();
+            if (callerCancelled)
+                return EmPlanningPublicationDecision.CallerCancelled;
+            if (deadlineExpired)
+                return EmPlanningPublicationDecision.DeadlineExpired;
+            publish();
+            return EmPlanningPublicationDecision.Published;
+        }
+    }
+
+    private void ObserveCallerCancellation()
+    {
+        lock (gate)
+            callerCancelled = true;
+    }
+
+    private void ObserveDeadlineCancellation()
+    {
+        lock (gate)
+            deadlineExpired = true;
+    }
+
+    private void ObserveDeadlineInsideGate()
+    {
+        if (deadlineToken.IsCancellationRequested || ComputeRemainingInsideGate() <= TimeSpan.Zero)
+            deadlineExpired = true;
+    }
+
+    private TimeSpan ComputeRemainingInsideGate()
+    {
+        if (!initialRemaining.HasValue)
+            return deadlineExpired ? TimeSpan.Zero : TimeSpan.MaxValue;
+        TimeSpan remaining = initialRemaining.Value - stopwatch.Elapsed;
+        return remaining > TimeSpan.Zero && !deadlineExpired ? remaining : TimeSpan.Zero;
+    }
+}
+
+/// <summary>
+/// EM 单次规划的只读输入引用快照，绑定平滑参考路径、地图、车辆状态、目标方向段和输出身份。
+/// 坐标使用 m，航向使用 rad，时间使用 UTC；调用方必须在进入服务前保证这些输入属于同一业务版本。
+/// </summary>
+public sealed class EmPlanningRequest
+{
+    /// <summary>
+    /// 保存一次规划所需的引用快照和发布身份。构造器只赋值而不校验、复制或取得对象所有权；调用方仍拥有传入引用，服务随后负责验证并复制配置。
+    /// </summary>
+    public EmPlanningRequest(
+        PathSmoothingResult referencePath,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        VehicleMotionState vehicleState,
+        EmPlannerConfiguration configuration,
+        int segmentIndex,
+        EmTrajectory previousTrajectory,
+        DateTimeOffset requestedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        string outputTrajectoryId,
+        string referencePathId,
+        string previousTrajectoryId,
+        EmMotionModel motionModel,
+        EmPlanningScope planningScope,
+        TimeSpan? cycleDeadlineRemaining = null,
+        CancellationToken callerCancellationToken = default,
+        CancellationToken cycleDeadlineToken = default)
+    {
+        ReferencePath = referencePath;
+        Map = map;
+        Vehicle = vehicle;
+        VehicleState = vehicleState;
+        Configuration = configuration;
+        SegmentIndex = segmentIndex;
+        PreviousTrajectory = previousTrajectory;
+        RequestedAtUtc = requestedAtUtc;
+        EffectiveAtUtc = effectiveAtUtc;
+        OutputTrajectoryId = outputTrajectoryId;
+        ReferencePathId = referencePathId;
+        PreviousTrajectoryId = previousTrajectoryId;
+        MotionModel = motionModel;
+        PlanningScope = planningScope;
+        CycleDeadlineRemaining = cycleDeadlineRemaining;
+        CallerCancellationToken = callerCancellationToken;
+        CycleDeadlineToken = cycleDeadlineToken;
+        PublicationAuthorization = new EmPlanningRequestPublicationAuthorization(
+            cycleDeadlineRemaining, cycleDeadlineToken);
+    }
+
+    internal EmPlanningRequest(
+        PathSmoothingResult referencePath,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        VehicleMotionState vehicleState,
+        EmPlannerConfiguration configuration,
+        int segmentIndex,
+        EmTrajectory previousTrajectory,
+        DateTimeOffset requestedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        string outputTrajectoryId,
+        string referencePathId,
+        string previousTrajectoryId,
+        EmMotionModel motionModel,
+        EmPlanningScope planningScope,
+        TimeSpan? cycleDeadlineRemaining,
+        CancellationToken callerCancellationToken,
+        CancellationToken cycleDeadlineToken,
+        IEmPlanningPublicationAuthorization publicationAuthorization)
+        : this(referencePath, map, vehicle, vehicleState, configuration, segmentIndex, previousTrajectory,
+            requestedAtUtc, effectiveAtUtc, outputTrajectoryId, referencePathId, previousTrajectoryId,
+            motionModel, planningScope, cycleDeadlineRemaining, callerCancellationToken, cycleDeadlineToken)
+    {
+        PublicationAuthorization = publicationAuthorization ??
+            new EmPlanningRequestPublicationAuthorization(cycleDeadlineRemaining, cycleDeadlineToken);
+    }
+
+    /// <summary>
+    /// 平滑后的参考路径结果引用；服务要求其为可消费的成功结果，且调用方负责保持其与其他输入版本一致。
+    /// </summary>
+    public PathSmoothingResult ReferencePath { get; }
+
+    /// <summary>
+    /// 规划使用的栅格地图快照引用；地图坐标单位和快照身份由地图契约定义，构造器不检查 null 或就绪状态。
+    /// </summary>
+    public PlanningGridMap Map { get; }
+
+    /// <summary>
+    /// 车辆几何与运动限制引用；调用方拥有该对象，服务在请求验证后使用其约束。
+    /// </summary>
+    public VehicleParameters Vehicle { get; }
+
+    /// <summary>
+    /// 一次性捕获的车辆运动状态；其位置为世界坐标 m、航向为 rad，时效和数值有效性由服务验证。
+    /// </summary>
+    public VehicleMotionState VehicleState { get; }
+
+    /// <summary>
+    /// 本次规划的配置引用；请求不深拷贝它，验证通过后服务取得配置副本供本次规划使用。
+    /// </summary>
+    public EmPlannerConfiguration Configuration { get; }
+
+    /// <summary>
+    /// 参考路径中待规划的方向段索引；必须由服务验证为有效的非负范围。
+    /// </summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>
+    /// 可选的上一条已发布轨迹引用，用于衔接或诊断；构造器允许为 null，调用方保有其所有权。
+    /// </summary>
+    public EmTrajectory PreviousTrajectory { get; }
+
+    /// <summary>
+    /// 本次规划请求发起的 UTC 时刻；用于车辆状态时效判断和生成轨迹元数据，构造器不验证其时间关系。
+    /// </summary>
+    public DateTimeOffset RequestedAtUtc { get; }
+
+    /// <summary>
+    /// 成功发布轨迹开始生效的 UTC 时刻；由下游消费者依据其调度语义使用。
+    /// </summary>
+    public DateTimeOffset EffectiveAtUtc { get; }
+
+    /// <summary>
+    /// 成功轨迹的发布标识；构造器不校验，轨迹元数据构造时要求为非空白字符串。
+    /// </summary>
+    public string OutputTrajectoryId { get; }
+
+    /// <summary>
+    /// 输入参考路径的业务标识，用于成功轨迹的来源追踪；构造器不校验其 null 或空白值。
+    /// </summary>
+    public string ReferencePathId { get; }
+
+    /// <summary>
+    /// 上一条轨迹的业务标识；允许为 null，成功轨迹元数据会将 null 规范化为空字符串。
+    /// </summary>
+    public string PreviousTrajectoryId { get; }
+
+    /// <summary>
+    /// 请求的车辆运动模型；不支持或无效的枚举值由服务转换为失败状态。
+    /// </summary>
+    public EmMotionModel MotionModel { get; }
+
+    /// <summary>
+    /// 本次请求覆盖滚动窗口或整个方向段的范围；服务负责验证其枚举值和资源约束。
+    /// </summary>
+    public EmPlanningScope PlanningScope { get; }
+
+    /// <summary>Optional remaining wall-clock budget for the complete outer planning cycle.</summary>
+    public TimeSpan? CycleDeadlineRemaining { get; }
+
+    /// <summary>Dynamic caller-cancellation origin used to preserve terminal-status precedence.</summary>
+    public CancellationToken CallerCancellationToken { get; }
+
+    /// <summary>Dynamic shared-cycle deadline origin checked again at atomic publication.</summary>
+    public CancellationToken CycleDeadlineToken { get; }
+
+    internal IEmPlanningPublicationAuthorization PublicationAuthorization { get; }
+
+    internal bool IsCycleDeadlineExpired()
+    {
+        try
+        {
+            return PublicationAuthorization.IsExpired;
+        }
+        catch
+        {
+            return true;
+        }
+    }
+
+    internal TimeSpan? DynamicCycleDeadlineRemaining()
+    {
+        try
+        {
+            TimeSpan remaining = PublicationAuthorization.Remaining;
+            return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
+        }
+        catch
+        {
+            return TimeSpan.Zero;
+        }
+    }
+
+    internal EmPlanningPublicationDecision TryAuthorizePublication(
+        CancellationToken coordinatorCallerCancellationToken, Action publish)
+    {
+        if (publish == null)
+            throw new ArgumentNullException(nameof(publish));
+        try
+        {
+            return PublicationAuthorization.TryPublish(CallerCancellationToken,
+                coordinatorCallerCancellationToken, publish);
+        }
+        catch
+        {
+            return EmPlanningPublicationDecision.DeadlineExpired;
+        }
+    }
+}
+
+/// <summary>
+/// 为请求契约保留的 <see cref="EmPlannerConfiguration"/> 部分声明；实际配置成员由其他同名分部提供。
+/// </summary>
+public sealed partial class EmPlannerConfiguration
+{
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs

@@ -0,0 +1,43 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 一次 EM 规划的不可变结果。只有成功状态才携带可发布的完整轨迹；其余状态仅提供诊断，不能作为部分执行轨迹消费。
+/// </summary>
+public sealed class EmPlanningResult
+{
+    /// <summary>
+    /// 创建规划结果并强制成功状态与轨迹发布的一致性：<see cref="EmPlanningStatus.Success"/> 和 <see cref="EmPlanningStatus.SuccessWithFallback"/> 必须提供非 null 轨迹，其他状态必须提供 null 轨迹；null 失败原因规范化为空字符串。
+    /// </summary>
+    public EmPlanningResult(EmPlanningStatus status, EmTrajectory trajectory, string failureReason)
+    {
+        if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+
+        bool isSuccess = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+        if (isSuccess && trajectory == null)
+            throw new ArgumentException("Successful results require a trajectory.", nameof(trajectory));
+        if (!isSuccess && trajectory != null)
+            throw new ArgumentException("Only successful results may contain a trajectory.", nameof(trajectory));
+
+        Status = status;
+        Trajectory = trajectory;
+        FailureReason = failureReason ?? string.Empty;
+    }
+
+    /// <summary>
+    /// 本次规划的最终状态；消费者必须先判断其是否为成功状态，再访问可发布轨迹。
+    /// </summary>
+    public EmPlanningStatus Status { get; }
+
+    /// <summary>
+    /// 仅在成功或成功降级状态下存在的完整可发布轨迹；失败、取消和无效输入结果始终为 null，消费者不得把失败结果当作部分轨迹执行。
+    /// </summary>
+    public EmTrajectory Trajectory { get; }
+
+    /// <summary>
+    /// 面向诊断的失败或降级原因；null 输入已规范化为空字符串，不替代 <see cref="Status"/> 的机器可读状态。
+    /// </summary>
+    public string FailureReason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningScope.cs

@@ -0,0 +1,16 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 定义一次 EM 规划覆盖当前滚动窗口还是完整方向段。
+/// </summary>
+public enum EmPlanningScope
+{
+    /// <summary>
+    /// 仅覆盖有限的滚动规划窗口；段末之外保留给后续重规划。
+    /// </summary>
+    RollingHorizon,
+    /// <summary>
+    /// 覆盖选定方向段直至真实停车边界，并受完整段资源上限保护。
+    /// </summary>
+    FullDirectionSegment,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs

@@ -0,0 +1,96 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 表示 EM 规划请求的最终处理状态；仅两个成功状态允许发布轨迹，其余状态要求消费者保留现有安全策略并读取诊断。
+/// </summary>
+public enum EmPlanningStatus
+{
+    /// <summary>
+    /// 所有规划和验证阶段成功，结果可按其生效时间发布。
+    /// </summary>
+    Success,
+    /// <summary>
+    /// 已发布经过验证的降级或回退轨迹；消费者仍可使用轨迹，并应记录诊断原因。
+    /// </summary>
+    SuccessWithFallback,
+    /// <summary>
+    /// 请求对象或其成员未满足服务输入要求。
+    /// </summary>
+    InvalidInput,
+    /// <summary>
+    /// 请求的车辆运动模型未被当前 EM 管线支持。
+    /// </summary>
+    UnsupportedMotionMode,
+    /// <summary>
+    /// 车辆状态采集时刻相对于请求时刻过旧。
+    /// </summary>
+    StaleVehicleState,
+    /// <summary>
+    /// 车辆状态的行驶方向与选定参考路径方向段不一致。
+    /// </summary>
+    StateDirectionMismatch,
+    /// <summary>
+    /// 参考路径结果、方向段或其状态不能用于规划。
+    /// </summary>
+    InvalidReferencePath,
+    /// <summary>
+    /// 无法将车辆状态投影到选定参考路径或方向段。
+    /// </summary>
+    ProjectionFailed,
+    /// <summary>
+    /// 地图和车辆约束下未能构造可行的行驶走廊。
+    /// </summary>
+    CorridorInfeasible,
+    /// <summary>
+    /// 横向优化未得到满足约束的候选解。
+    /// </summary>
+    LateralInfeasible,
+    /// <summary>
+    /// 纵向优化未得到满足时空与动力学约束的候选解。
+    /// </summary>
+    LongitudinalInfeasible,
+    /// <summary>
+    /// 当前速度、距离或限制不足以在所需边界前完成停车。
+    /// </summary>
+    StoppingDistanceInsufficient,
+    /// <summary>
+    /// 所需二次规划求解器不可用。
+    /// </summary>
+    SolverUnavailable,
+    /// <summary>
+    /// 求解在共享时间预算或迭代限制内未完成。
+    /// </summary>
+    SolverTimedOut,
+    /// <summary>
+    /// 调用方在可发布结果生成前取消了请求。
+    /// </summary>
+    Cancelled,
+    /// <summary>
+    /// 候选轨迹未通过最终世界空间、动力学或终端语义验证。
+    /// </summary>
+    ValidationFailed,
+    /// <summary>
+    /// 请求在处理期间被更新版本的规划工作替代。
+    /// </summary>
+    Superseded,
+    /// <summary>
+    /// 候选轨迹相对于当前状态未形成足够的有效进展。
+    /// </summary>
+    NoProgress,
+    /// <summary>
+    /// 轨迹末端位姿未满足要求的真实终端位姿。
+    /// </summary>
+    TerminalPoseMismatch,
+    /// <summary>
+    /// 完整方向段规划超过配置的时间、采样或求解资源上限。
+    /// </summary>
+    FullSegmentResourceLimitExceeded,
+    /// <summary>
+    /// 未被其他状态细分的规划失败。
+    /// </summary>
+    Failed,
+    /// <summary>
+    /// The outer planning cycle exhausted its shared bootstrap-to-publication deadline.
+    /// </summary>
+    CycleDeadlineExpired,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTerminalType.cs

@@ -0,0 +1,20 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 表示已发布轨迹末端所对应的规划终端语义。
+/// </summary>
+public enum EmTerminalType
+{
+    /// <summary>
+    /// 因滚动窗口安全约束形成的停车终端，不代表方向段完成。
+    /// </summary>
+    RollingSafetyStop,
+    /// <summary>
+    /// 用于在方向改变前后执行换挡的终端。
+    /// </summary>
+    GearSwitch,
+    /// <summary>
+    /// 最终任务目标的终端。
+    /// </summary>
+    Goal,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectory.cs

@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 供成功规划结果发布的时间参数化轨迹；本类型只施加结构约束，世界空间复核由上游服务完成，点序列、元数据和终端语义在发布后保持不可变。
+/// </summary>
+public sealed class EmTrajectory
+{
+    /// <summary>
+    /// 创建可发布的轨迹并复制点列表容器。元数据和每个点对象按引用保存、调用方仍拥有它们；传入列表可随后修改而不影响 <see cref="Points"/>，且 null 或空列表会抛出异常。
+    /// </summary>
+    public EmTrajectory(EmTrajectoryMetadata metadata, IReadOnlyList<EmTrajectoryPoint> points)
+    {
+        if (metadata == null)
+            throw new ArgumentNullException(nameof(metadata));
+        if (points == null)
+            throw new ArgumentNullException(nameof(points));
+        if (points.Count == 0)
+            throw new ArgumentException("A published trajectory requires at least one point.", nameof(points));
+
+        var copy = new List<EmTrajectoryPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++)
+        {
+            if (points[index] == null)
+                throw new ArgumentException("Trajectory points cannot contain null values.", nameof(points));
+            copy.Add(points[index]);
+        }
+
+        Metadata = metadata;
+        Points = new ReadOnlyCollection<EmTrajectoryPoint>(copy);
+    }
+
+    /// <summary>
+    /// 轨迹的不可变发布元数据引用；构造时必须非 null，未在本类中深拷贝。
+    /// </summary>
+    public EmTrajectoryMetadata Metadata { get; }
+
+    /// <summary>
+    /// 按调用方提供顺序保存的只读点列表；列表容器为构造时复制的快照，至少包含一个非 null 点，时间单调性由上游验证保证而非本构造器检查。
+    /// </summary>
+    public IReadOnlyList<EmTrajectoryPoint> Points { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs

@@ -0,0 +1,120 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 轨迹发布身份、来源版本、有效时间、方向段和终端边界的不可变元数据。
+/// </summary>
+public sealed class EmTrajectoryMetadata
+{
+    /// <summary>
+    /// 创建轨迹的发布和来源元数据。验证标识、序列号、索引和枚举值；<paramref name="previousTrajectoryId"/> 可为 null 并会规范化为空字符串，两个 UTC 时刻的先后关系不在此处验证。
+    /// </summary>
+    public EmTrajectoryMetadata(
+        string trajectoryId,
+        DateTimeOffset generatedAtUtc,
+        DateTimeOffset effectiveAtUtc,
+        long mapSnapshotId,
+        string referencePathId,
+        long vehicleStateSequenceId,
+        string previousTrajectoryId,
+        int segmentIndex,
+        TravelDirection direction,
+        EmTerminalType terminalType,
+        EmLongitudinalMode longitudinalMode,
+        EmPlanningScope planningScope)
+    {
+        if (string.IsNullOrWhiteSpace(trajectoryId))
+            throw new ArgumentException("A trajectory ID is required.", nameof(trajectoryId));
+        if (mapSnapshotId < 0)
+            throw new ArgumentOutOfRangeException(nameof(mapSnapshotId));
+        if (string.IsNullOrWhiteSpace(referencePathId))
+            throw new ArgumentException("A reference path ID is required.", nameof(referencePathId));
+        if (vehicleStateSequenceId < 0)
+            throw new ArgumentOutOfRangeException(nameof(vehicleStateSequenceId));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), longitudinalMode))
+            throw new ArgumentOutOfRangeException(nameof(longitudinalMode));
+        if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
+            throw new ArgumentOutOfRangeException(nameof(planningScope));
+
+        TrajectoryId = trajectoryId;
+        GeneratedAtUtc = generatedAtUtc;
+        EffectiveAtUtc = effectiveAtUtc;
+        MapSnapshotId = mapSnapshotId;
+        ReferencePathId = referencePathId;
+        VehicleStateSequenceId = vehicleStateSequenceId;
+        PreviousTrajectoryId = previousTrajectoryId ?? string.Empty;
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        TerminalType = terminalType;
+        LongitudinalMode = longitudinalMode;
+        PlanningScope = planningScope;
+    }
+
+    /// <summary>
+    /// 非空白的本次发布轨迹标识，由消费者用于去重、替换和追踪。
+    /// </summary>
+    public string TrajectoryId { get; }
+
+    /// <summary>
+    /// 生成此元数据的 UTC 时刻；值原样保存，构造器不与生效时刻比较。
+    /// </summary>
+    public DateTimeOffset GeneratedAtUtc { get; }
+
+    /// <summary>
+    /// 轨迹计划开始生效的 UTC 时刻；执行消费者负责按其调度策略解释。
+    /// </summary>
+    public DateTimeOffset EffectiveAtUtc { get; }
+
+    /// <summary>
+    /// 地图快照的非负版本标识；用于确认轨迹依赖的环境版本。
+    /// </summary>
+    public long MapSnapshotId { get; }
+
+    /// <summary>
+    /// 非空白的输入参考路径标识；用于关联轨迹的几何来源。
+    /// </summary>
+    public string ReferencePathId { get; }
+
+    /// <summary>
+    /// 车辆状态的非负序列版本；用于判断轨迹是否基于当前状态快照。
+    /// </summary>
+    public long VehicleStateSequenceId { get; }
+
+    /// <summary>
+    /// 前一轨迹的可选标识；null 输入已规范化为空字符串，空字符串表示没有可关联的前轨迹标识。
+    /// </summary>
+    public string PreviousTrajectoryId { get; }
+
+    /// <summary>
+    /// 参考路径中此轨迹所属的非负方向段索引。
+    /// </summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>
+    /// 所属方向段的已验证行驶方向。
+    /// </summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>
+    /// 轨迹最后一个终端的已验证业务类型。
+    /// </summary>
+    public EmTerminalType TerminalType { get; }
+
+    /// <summary>
+    /// 生成轨迹时采用的已验证纵向终端速度语义。
+    /// </summary>
+    public EmLongitudinalMode LongitudinalMode { get; }
+
+    /// <summary>
+    /// 生成轨迹时采用的已验证规划覆盖范围。
+    /// </summary>
+    public EmPlanningScope PlanningScope { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryPoint.cs

@@ -0,0 +1,155 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 轨迹中的单个时间样本。世界位置单位 m、航向单位 rad、速度单位 m/s、曲率单位 1/m。
+/// </summary>
+public sealed class EmTrajectoryPoint
+{
+    /// <summary>
+    /// 创建一个轨迹时间样本。所有浮点输入必须有限；时间、段内弧长和路径弧长必须非负，段索引和枚举值必须有效。派生的绝对速度、世界速度分量和偏航角速度由有符号纵向速度、航向和曲率计算。
+    /// </summary>
+    public EmTrajectoryPoint(
+        double x,
+        double y,
+        double yaw,
+        double signedLongitudinalVelocity,
+        double timeFromStart,
+        double vehicleCurvature,
+        int segmentIndex,
+        double segmentLocalS,
+        double pathS,
+        TravelDirection direction,
+        EmBoundaryType boundaryType,
+        double longitudinalAcceleration,
+        double longitudinalJerk)
+    {
+        ContractNumeric.RequireFinite(x, nameof(x));
+        ContractNumeric.RequireFinite(y, nameof(y));
+        ContractNumeric.RequireFinite(yaw, nameof(yaw));
+        ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity));
+        ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart));
+        ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS));
+        ContractNumeric.RequireFinite(pathS, nameof(pathS));
+        ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration));
+        ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (timeFromStart < 0d)
+            throw new ArgumentOutOfRangeException(nameof(timeFromStart));
+        if (segmentLocalS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
+            throw new ArgumentOutOfRangeException(nameof(boundaryType));
+
+        X = x;
+        Y = y;
+        Yaw = yaw;
+        SignedLongitudinalVelocity = signedLongitudinalVelocity;
+        Speed = Math.Abs(signedLongitudinalVelocity);
+        VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
+        VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
+        YawRate = signedLongitudinalVelocity * vehicleCurvature;
+        TimeFromStart = timeFromStart;
+        VehicleCurvature = vehicleCurvature;
+        SegmentIndex = segmentIndex;
+        SegmentLocalS = segmentLocalS;
+        PathS = pathS;
+        Direction = direction;
+        BoundaryType = boundaryType;
+        LongitudinalAcceleration = longitudinalAcceleration;
+        LongitudinalJerk = longitudinalJerk;
+    }
+
+    /// <summary>
+    /// 世界坐标系 X 位置，单位 m。
+    /// </summary>
+    public double X { get; }
+
+    /// <summary>
+    /// 世界坐标系 Y 位置，单位 m。
+    /// </summary>
+    public double Y { get; }
+
+    /// <summary>
+    /// 车辆在世界坐标系中的航向，单位 rad；构造器仅要求有限，不归一化角度。
+    /// </summary>
+    public double Yaw { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的有符号纵向速度，单位 m/s；符号表示前进或倒车。
+    /// </summary>
+    public double SignedLongitudinalVelocity { get; }
+
+    /// <summary>
+    /// 有符号纵向速度的绝对值，单位 m/s。
+    /// </summary>
+    public double Speed { get; }
+
+    /// <summary>
+    /// 由有符号纵向速度和航向导出的世界坐标 X 速度分量，单位 m/s。
+    /// </summary>
+    public double VelocityX { get; }
+
+    /// <summary>
+    /// 由有符号纵向速度和航向导出的世界坐标 Y 速度分量，单位 m/s。
+    /// </summary>
+    public double VelocityY { get; }
+
+    /// <summary>
+    /// 由有符号纵向速度乘车辆曲率导出的偏航角速度，单位 rad/s。
+    /// </summary>
+    public double YawRate { get; }
+
+    /// <summary>
+    /// 从本条轨迹开始执行起累计的非负时间，单位 s。
+    /// </summary>
+    public double TimeFromStart { get; }
+
+    /// <summary>
+    /// 车辆路径曲率，单位 1/m；符号约定由上游几何计算定义。
+    /// </summary>
+    public double VehicleCurvature { get; }
+
+    /// <summary>
+    /// 此点所属的非负方向段索引。
+    /// </summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>
+    /// 从该方向段开始沿参考路径累计的非负弧长，单位 m。
+    /// </summary>
+    public double SegmentLocalS { get; }
+
+    /// <summary>
+    /// 从整条参考路径开始累计的非负弧长，单位 m。
+    /// </summary>
+    public double PathS { get; }
+
+    /// <summary>
+    /// 此点的已验证行驶方向。
+    /// </summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>
+    /// 此点的已验证边界语义；普通内部点使用 <see cref="EmBoundaryType.None"/>。
+    /// </summary>
+    public EmBoundaryType BoundaryType { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的纵向加速度，单位 m/s²；仅供同程序集的轨迹验证和执行逻辑读取。
+    /// </summary>
+    internal double LongitudinalAcceleration { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的纵向加加速度，单位 m/s³；仅供同程序集的轨迹验证和执行逻辑读取。
+    /// </summary>
+    internal double LongitudinalJerk { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs

@@ -0,0 +1,52 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 调用方一次性捕获的车辆运动状态；EMPlanner 只消费此快照，不直接读取定位、轮速或系统时钟。
+/// </summary>
+public sealed class VehicleMotionState
+{
+    /// <summary>
+    /// 保存调用方捕获的车辆状态。构造器不校验 null、数值有限性、UTC 时效或序列号；服务在接受请求前负责验证，传入 <paramref name="pose"/> 的所有权仍归调用方。
+    /// </summary>
+    public VehicleMotionState(
+        Pose2D pose,
+        double signedLongitudinalSpeedMetersPerSecond,
+        double? longitudinalAccelerationMetersPerSecondSquared,
+        DateTimeOffset capturedAtUtc,
+        long sequenceId)
+    {
+        Pose = pose;
+        SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond;
+        LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared;
+        CapturedAtUtc = capturedAtUtc;
+        SequenceId = sequenceId;
+    }
+
+    /// <summary>
+    /// 车辆世界位姿引用，其中位置单位为 m、航向单位为 rad；构造器允许 null，但服务要求有效位姿。
+    /// </summary>
+    public Pose2D Pose { get; }
+
+    /// <summary>
+    /// 沿车辆前向轴的有符号纵向速度，单位 m/s；正负方向约定由上游状态生产者负责，服务要求其为有限数。
+    /// </summary>
+    public double SignedLongitudinalSpeedMetersPerSecond { get; }
+
+    /// <summary>
+    /// 可选的沿车辆前向轴纵向加速度，单位 m/s²；null 表示采集方未提供该量，非 null 值必须由服务验证为有限数。
+    /// </summary>
+    public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
+
+    /// <summary>
+    /// 采集此状态的 UTC 时刻；服务用它相对请求发起时刻判断状态是否过期。
+    /// </summary>
+    public DateTimeOffset CapturedAtUtc { get; }
+
+    /// <summary>
+    /// 调用方提供的状态快照序列版本；服务要求其非负，成功轨迹将其写入元数据供消费者关联。
+    /// </summary>
+    public long SequenceId { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/LateralInterval.cs

@@ -0,0 +1,56 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 一个精确参考站 S 处与种子横向位置连通的自由横向区间。
+/// 参考站和横向偏移均以 m 计，横向正负号遵循该方向段的行驶坐标系；构造时拒绝非有限值、反向边界，或超出边界 1e-12 m 的种子。
+/// </summary>
+public sealed class LateralInterval
+{
+    /// <summary>
+    /// 创建一个固定参考站上的闭合自由横向区间。
+    /// 参数：referenceS、minimumL、maximumL 与 seedL 均为 m；seedL 必须在闭区间内（容许 1e-12 m 数值误差）。
+    /// 返回：保存已验证边界的不可变区间；非法数值或不连通种子会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    public LateralInterval(double referenceS, double minimumL, double maximumL, double seedL)
+    {
+        if (!IsFinite(referenceS) || !IsFinite(minimumL) || !IsFinite(maximumL) || !IsFinite(seedL) ||
+            minimumL > maximumL || seedL < minimumL - 1e-12d || seedL > maximumL + 1e-12d)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+
+        ReferenceS = referenceS;
+        MinimumL = minimumL;
+        MaximumL = maximumL;
+        SeedL = seedL;
+    }
+
+    /// <summary>
+    /// 本区间所属方向段局部参考弧长 S，单位 m。
+    /// </summary>
+    public double ReferenceS { get; }
+
+    /// <summary>
+    /// 可通行横向偏移闭区间的下界，单位 m。
+    /// </summary>
+    public double MinimumL { get; }
+
+    /// <summary>
+    /// 可通行横向偏移闭区间的上界，单位 m。
+    /// </summary>
+    public double MaximumL { get; }
+
+    /// <summary>
+    /// 用于保持拓扑连通性的种子横向偏移，单位 m。
+    /// </summary>
+    public double SeedL { get; }
+
+    /// <summary>
+    /// 判定标量能否参与走廊边界计算。
+    /// 参数：value 为无单位或 m 制实数；返回：仅非 NaN 且非无穷大时为 true。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridor.cs

@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 一个已选择拓扑走廊在离散参考站上的不可变横向硬边界集合。
+/// 站点按方向段局部 S（m）非递减保存，调用方只能沿每个站点的种子连通自由区间优化。
+/// </summary>
+public sealed class StaticCorridor
+{
+    /// <summary>
+    /// 从站点序列创建走廊的防御性只读副本。
+    /// 参数：stations 不能为空且至少含一个按 ReferenceS（m）非递减的非空区间；违反这些约束会被拒绝。
+    /// </summary>
+    public StaticCorridor(IReadOnlyList<LateralInterval> stations)
+    {
+        if (stations == null || stations.Count == 0)
+            throw new ArgumentException("A static corridor requires stations.", nameof(stations));
+
+        var copy = new List<LateralInterval>(stations.Count);
+        double previousS = double.NegativeInfinity;
+        for (int index = 0; index < stations.Count; index++)
+        {
+            LateralInterval station = stations[index];
+            if (station == null || station.ReferenceS < previousS)
+                throw new ArgumentException("Static corridor stations must be non-null and sorted.", nameof(stations));
+            copy.Add(station);
+            previousS = station.ReferenceS;
+        }
+        Stations = new ReadOnlyCollection<LateralInterval>(copy);
+    }
+
+    /// <summary>
+    /// 按方向段局部参考弧长 S（m）排序的横向硬边界只读列表。
+    /// </summary>
+    public IReadOnlyList<LateralInterval> Stations { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridorBuilder.cs

@@ -0,0 +1,373 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 为单一方向段构建仅与种子轨迹横向连通的静态自由空间走廊。
+/// 采样站和横向偏移使用 Frenet S/L（m），碰撞在世界 X/Y（m）车体几何中验证；任何无效、碰撞或断连站都会拒绝整个走廊。
+/// </summary>
+public sealed class StaticCorridorBuilder
+{
+    /// <summary>
+    /// 横向边界、采样去重和相邻区间重叠判定使用的数值容差，单位 m。
+    /// </summary>
+    private const double Epsilon = 1e-12d;
+
+    /// <summary>
+    /// Frenet 重建时拒绝 1-κL 接近零的最小正分母，无量纲。
+    /// </summary>
+    private const double ReconstructionDenominator = 1e-12d;
+
+    /// <summary>
+    /// 对未被保守净空快速放行的位置执行精确车体碰撞复核的依赖项。
+    /// </summary>
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>
+    /// 创建使用默认连续车体碰撞检查器的静态走廊构建器。
+    /// </summary>
+    public StaticCorridorBuilder()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>
+    /// 创建使用指定车体碰撞检查器的静态走廊构建器。
+    /// 参数：collisionChecker 不可为空；该检查器在世界坐标中按车辆尺寸和安全裕度验证采样位姿。
+    /// </summary>
+    public StaticCorridorBuilder(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 从 S 起止锚点、种子轨迹和栅格地图构建种子连通的横向硬边界。
+    /// 参数：startReferenceS/endReferenceS、种子 S/L 和配置采样尺度均为 m，map 使用世界 X/Y 栅格，vehicle 提供 m 制尺寸；区间限定在一个方向段内。
+    /// 返回：每站均存在含种子的自由区间且相邻区间在 1e-12 m 内重叠时返回 true；否则 corridor 为 null 并写入拒绝原因。
+    /// </summary>
+    public bool TryBuild(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
+        IReadOnlyList<FrenetProjection> seed, PlanningGridMap map, VehicleParameters vehicle,
+        CorridorConfiguration configuration, out StaticCorridor corridor, out string failureReason)
+    {
+        corridor = null;
+        failureReason = string.Empty;
+        if (!TryValidateInput(segment, startReferenceS, endReferenceS, map, vehicle, configuration, out failureReason))
+            return false;
+        if (!TryReadSeeds(seed, segment, out List<SeedSample> seeds, out failureReason))
+            return false;
+
+        var stations = new List<LateralInterval>();
+        LateralInterval previous = null;
+        foreach (double referenceS in CreateStations(startReferenceS, endReferenceS,
+                     configuration.LongitudinalSampleSpacingMeters))
+        {
+            FrenetReferencePoint reference;
+            try
+            {
+                reference = ReferencePathInterpolator.Interpolate(segment, referenceS);
+            }
+            catch (ArgumentException)
+            {
+                failureReason = "Reference interpolation failed at S=" + referenceS + ".";
+                return false;
+            }
+
+            double seedL = GetSeedL(seeds, referenceS);
+            if (seedL < -configuration.MaximumLateralOffsetMeters - Epsilon ||
+                seedL > configuration.MaximumLateralOffsetMeters + Epsilon)
+            {
+                failureReason = "Seed lateral offset is outside configured bounds at S=" + referenceS + ".";
+                return false;
+            }
+
+            if (!TrySelectSeedConnectedInterval(reference, seedL, map, vehicle, configuration,
+                    out double minimumL, out double maximumL))
+            {
+                failureReason = "Seed-connected free interval disappeared at S=" + referenceS + ".";
+                return false;
+            }
+
+            var selected = new LateralInterval(referenceS, minimumL, maximumL, seedL);
+            if (previous != null && Math.Max(previous.MinimumL, selected.MinimumL) >
+                Math.Min(previous.MaximumL, selected.MaximumL) + Epsilon)
+            {
+                failureReason = "Seed-connected corridor loses overlap at S=" + referenceS + ".";
+                return false;
+            }
+
+            stations.Add(selected);
+            previous = selected;
+        }
+
+        corridor = new StaticCorridor(stations);
+        return true;
+    }
+
+    /// <summary>
+    /// 验证构建走廊所需的段、S 范围、地图、车辆和采样配置。
+    /// 参数：S 锚点和配置距离均为 m，车辆尺寸为 m；返回：地图未就绪、非有限量、越段锚点或非正采样尺度时为 false 并说明原因。
+    /// </summary>
+    private static bool TryValidateInput(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
+        PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (segment == null || map == null || vehicle == null || configuration == null || !map.PlanningReady)
+        {
+            failureReason = "A planning-ready map, vehicle, segment, and corridor configuration are required.";
+            return false;
+        }
+        if (!IsFinite(startReferenceS) || !IsFinite(endReferenceS) || startReferenceS < 0d ||
+            endReferenceS < startReferenceS || endReferenceS > segment.LengthMeters + Epsilon)
+        {
+            failureReason = "Requested corridor S anchors are invalid.";
+            return false;
+        }
+        if (!IsPositiveFinite(configuration.LongitudinalSampleSpacingMeters) ||
+            !IsPositiveFinite(configuration.LateralSampleSpacingMeters) ||
+            !IsFinite(configuration.MaximumLateralOffsetMeters) || configuration.MaximumLateralOffsetMeters < 0d ||
+            !IsFinite(configuration.AdditionalClearanceReserveMeters) || configuration.AdditionalClearanceReserveMeters < 0d ||
+            !IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters))
+        {
+            failureReason = "Corridor configuration is invalid.";
+            return false;
+        }
+        if (!IsPositiveFinite(vehicle.LengthMeters) || !IsPositiveFinite(vehicle.WidthMeters) ||
+            !IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d)
+        {
+            failureReason = "Vehicle footprint dimensions are invalid.";
+            return false;
+        }
+        return true;
+    }
+
+    /// <summary>
+    /// 验证并按局部参考 S 排序输入种子投影。
+    /// 参数：input 可为 null（表示中心线 L=0 种子），各投影 S/L 为 m；返回：种子属于当前段且横向量有限时为 true，否则为 false。
+    /// </summary>
+    private static bool TryReadSeeds(IReadOnlyList<FrenetProjection> input, DirectionSegmentView segment,
+        out List<SeedSample> seeds, out string failureReason)
+    {
+        seeds = new List<SeedSample>();
+        failureReason = string.Empty;
+        if (input == null)
+            return true;
+
+        for (int index = 0; index < input.Count; index++)
+        {
+            FrenetProjection projection = input[index];
+            if (projection == null || projection.ReferencePoint == null ||
+                projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon ||
+                !IsFinite(projection.LateralOffset))
+            {
+                failureReason = "Corridor seeds must belong to the selected direction segment.";
+                return false;
+            }
+            seeds.Add(new SeedSample(projection.ReferenceS, projection.LateralOffset));
+        }
+        seeds.Sort((left, right) => left.ReferenceS.CompareTo(right.ReferenceS));
+        return true;
+    }
+
+    /// <summary>
+    /// 生成包含起止锚点的纵向采样站。
+    /// 参数：起止 S 和 spacing 单位均为 m；返回：起点、严格内部等距站以及不同于起点超过 1e-12 m 的终点。
+    /// </summary>
+    private static IEnumerable<double> CreateStations(double startReferenceS, double endReferenceS, double spacing)
+    {
+        yield return startReferenceS;
+        for (double candidate = startReferenceS + spacing; candidate < endReferenceS - Epsilon; candidate += spacing)
+            yield return candidate;
+        if (endReferenceS > startReferenceS + Epsilon)
+            yield return endReferenceS;
+    }
+
+    /// <summary>
+    /// 找出横向离散样本中包含种子的连续无碰撞区间。
+    /// 参数：reference 为世界几何，seedL 及配置横向距离为 m；返回：种子样本自由时给出 [minimumL, maximumL]，碰撞或断连时返回 false。
+    /// </summary>
+    private bool TrySelectSeedConnectedInterval(FrenetReferencePoint reference, double seedL, PlanningGridMap map,
+        VehicleParameters vehicle, CorridorConfiguration configuration, out double minimumL, out double maximumL)
+    {
+        minimumL = 0d;
+        maximumL = 0d;
+        List<double> samples = CreateLateralSamples(seedL, configuration.MaximumLateralOffsetMeters,
+            configuration.LateralSampleSpacingMeters);
+        int index = 0;
+        while (index < samples.Count)
+        {
+            if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
+            {
+                index++;
+                continue;
+            }
+
+            double groupMinimum = samples[index];
+            double groupMaximum = samples[index];
+            bool containsSeed = Math.Abs(samples[index] - seedL) <= Epsilon;
+            index++;
+            while (index < samples.Count && samples[index] - groupMaximum <= configuration.LateralSampleSpacingMeters + Epsilon)
+            {
+                if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
+                {
+                    index++;
+                    break;
+                }
+                groupMaximum = samples[index];
+                containsSeed |= Math.Abs(samples[index] - seedL) <= Epsilon;
+                index++;
+            }
+
+            if (containsSeed)
+            {
+                minimumL = groupMinimum;
+                maximumL = groupMaximum;
+                return true;
+            }
+        }
+        return false;
+    }
+
+    /// <summary>
+    /// 在配置横向范围内生成有序且包含种子的唯一 L 采样值。
+    /// 参数：seedL、maximumOffset 和 spacing 为 m；返回：包含两端和种子、以 1e-12 m 去重的升序列表。
+    /// </summary>
+    private static List<double> CreateLateralSamples(double seedL, double maximumOffset, double spacing)
+    {
+        var samples = new List<double>();
+        for (double candidate = -maximumOffset; candidate < maximumOffset - Epsilon; candidate += spacing)
+            AddSortedUnique(samples, candidate);
+        AddSortedUnique(samples, maximumOffset);
+        AddSortedUnique(samples, -maximumOffset);
+        AddSortedUnique(samples, seedL);
+        samples.Sort();
+        return samples;
+    }
+
+    /// <summary>
+    /// 重建给定 L 的世界车体位姿并检查其是否无碰撞。
+    /// 参数：lateralOffset 为 m，reference 使用世界 X/Y（m）和 rad 航向，车辆与储备净空为 m；返回：重建奇异、越图或碰撞均为 false。
+    /// </summary>
+    private bool IsCollisionFree(FrenetReferencePoint reference, double lateralOffset, PlanningGridMap map,
+        VehicleParameters vehicle, CorridorConfiguration configuration)
+    {
+        if (!FrenetTransform.TryReconstruct(reference, lateralOffset, 0d, ReconstructionDenominator, out Pose2D pose))
+            return false;
+
+        if (IsObviouslyClear(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters))
+            return true;
+        return _collisionChecker.IsPoseCollisionFree(pose, map, vehicle,
+            configuration.AdditionalClearanceReserveMeters, out _);
+    }
+
+    /// <summary>
+    /// 用保守障碍距离和四个扩张车体角点快速确认明显净空。
+    /// 参数：pose 为世界 X/Y（m）和 rad，车辆尺寸及额外储备为 m；返回：圆形下界安全且四角均在图内时为 true，否则交由精确检查器。
+    /// </summary>
+    private static bool IsObviouslyClear(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
+        double additionalClearanceReserveMeters)
+    {
+        double totalMargin = vehicle.SafetyMarginMeters + additionalClearanceReserveMeters;
+        double halfLength = vehicle.LengthMeters / 2d + totalMargin;
+        double halfWidth = vehicle.WidthMeters / 2d + totalMargin;
+        double radius = Math.Sqrt(halfLength * halfLength + halfWidth * halfWidth);
+        if (!IsFinite(radius) || map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y) <= radius)
+            return false;
+
+        double cosine = Math.Cos(pose.Heading);
+        double sine = Math.Sin(pose.Heading);
+        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
+        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
+        {
+            double x = pose.X + longitudinalSign * halfLength * cosine - lateralSign * halfWidth * sine;
+            double y = pose.Y + longitudinalSign * halfLength * sine + lateralSign * halfWidth * cosine;
+            if (!map.TryWorldToGrid(x, y, out _, out _))
+                return false;
+        }
+        return true;
+    }
+
+    /// <summary>
+    /// 以相邻种子 S 线性插值得到采样站的横向种子。
+    /// 参数：seeds 已按局部 S（m）升序，referenceS 为 m；返回：区间外保持端点 L，重合 S 间隔不超过 1e-12 m 时取上端 L。
+    /// </summary>
+    private static double GetSeedL(IReadOnlyList<SeedSample> seeds, double referenceS)
+    {
+        if (seeds.Count == 0)
+            return 0d;
+        if (referenceS <= seeds[0].ReferenceS)
+            return seeds[0].LateralOffset;
+        for (int index = 1; index < seeds.Count; index++)
+        {
+            SeedSample upper = seeds[index];
+            if (referenceS <= upper.ReferenceS)
+            {
+                SeedSample lower = seeds[index - 1];
+                double span = upper.ReferenceS - lower.ReferenceS;
+                return span <= Epsilon ? upper.LateralOffset : lower.LateralOffset +
+                    (upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
+            }
+        }
+        return seeds[seeds.Count - 1].LateralOffset;
+    }
+
+    /// <summary>
+    /// 向样本列表加入未在 1e-12 m 容差内出现过的横向值。
+    /// 参数：samples 保存 m 制 L 值，value 为待加入 L（m）；返回：重复近似值被拒绝，唯一值追加后由调用方排序。
+    /// </summary>
+    private static void AddSortedUnique(List<double> samples, double value)
+    {
+        for (int index = 0; index < samples.Count; index++)
+            if (Math.Abs(samples[index] - value) <= Epsilon)
+                return;
+        samples.Add(value);
+    }
+
+    /// <summary>
+    /// 判定采样步长或车辆尺寸是否为正的有限值。
+    /// 参数：value 为对应单位的标量；返回：仅有限且严格大于零时为 true。
+    /// </summary>
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    /// <summary>
+    /// 判定走廊计算输入是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 和无穷时为 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    /// <summary>
+    /// 走廊种子在一个局部参考站上的轻量不可变记录。
+    /// ReferenceS 与 LateralOffset 均以 m 计，仅用于站间种子插值。
+    /// </summary>
+    private sealed class SeedSample
+    {
+        /// <summary>
+        /// 创建一个种子记录。
+        /// 参数：referenceS 为段局部 S（m），lateralOffset 为行驶坐标系中的 L（m）。
+        /// </summary>
+        public SeedSample(double referenceS, double lateralOffset)
+        {
+            ReferenceS = referenceS;
+            LateralOffset = lateralOffset;
+        }
+
+        /// <summary>
+        /// 种子所在方向段局部参考弧长 S，单位 m。
+        /// </summary>
+        public double ReferenceS { get; }
+
+        /// <summary>
+        /// 种子相对中心线的横向偏移 L，单位 m。
+        /// </summary>
+        public double LateralOffset { get; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlannerDebugOptions.cs

@@ -0,0 +1,13 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlannerDebugOptions
+{
+    public bool EnableSummary { get; set; }
+    public bool EnableProjectionTrace { get; set; }
+    public bool EnableCorridorTrace { get; set; }
+    public bool EnableLateralSolverTrace { get; set; }
+    public bool EnableLongitudinalSolverTrace { get; set; }
+    public bool EnableTrajectoryDump { get; set; }
+    public bool EnableVisualization { get; set; }
+    public IEmPlannerDebugSink Sink { get; set; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlanningDiagnostics.cs

@@ -0,0 +1,30 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlanningDiagnostics
+{
+    private readonly List<string> _debugSinkFailures = new List<string>();
+
+    public IReadOnlyList<string> DebugSinkFailures
+    {
+        get { return new ReadOnlyCollection<string>(new List<string>(_debugSinkFailures)); }
+    }
+
+    public void WriteDebug(EmPlannerDebugOptions options, string message)
+    {
+        if (options == null || options.Sink == null)
+            return;
+
+        try
+        {
+            options.Sink.Write(message ?? string.Empty);
+        }
+        catch (Exception exception)
+        {
+            _debugSinkFailures.Add(exception.GetType().FullName + ": " + exception.Message);
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/IEmPlannerDebugSink.cs

@@ -0,0 +1,6 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public interface IEmPlannerDebugSink
+{
+    void Write(string message);
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -0,0 +1,455 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories.</summary>
+public sealed class EmPlanningService : IEmPlanningService
+{
+    private static readonly TimeSpan MaximumCycleDeadlineRemaining =
+        TimeSpan.FromMilliseconds(int.MaxValue);
+    private readonly IQpSolver qpSolver;
+    private readonly IEmPlannerDebugSink defaultDebugSink;
+    private readonly Func<TimeSpan> cycleElapsedForTesting;
+
+    public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
+        : this(qpSolver, defaultDebugSink, null)
+    {
+    }
+
+    internal EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink,
+        Func<TimeSpan> cycleElapsedForTesting)
+    {
+        this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
+        this.defaultDebugSink = defaultDebugSink;
+        this.cycleElapsedForTesting = cycleElapsedForTesting;
+    }
+
+    public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
+    {
+        Stopwatch cycleStopwatch = cycleElapsedForTesting == null ? Stopwatch.StartNew() : null;
+        Func<TimeSpan> cycleElapsed = cycleElapsedForTesting ?? (() => cycleStopwatch.Elapsed);
+        if (CallerCancellationRequested(request, cancellationToken))
+            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
+        if (request?.CycleDeadlineRemaining is TimeSpan suppliedRemaining &&
+            (suppliedRemaining < TimeSpan.Zero || suppliedRemaining > MaximumCycleDeadlineRemaining))
+        {
+            return Failure(EmPlanningStatus.InvalidInput, request,
+                "CycleDeadlineRemaining must be between zero and " +
+                MaximumCycleDeadlineRemaining.TotalMilliseconds + " milliseconds.");
+        }
+        if (DeadlineExpired(request, cycleElapsed))
+            return CycleDeadlineFailure(request, "request", CycleRemaining(request, cycleElapsed));
+
+        EmPlanningRequestValidationResult requestValidation = EmPlanningRequestValidator.Validate(request);
+        if (!requestValidation.IsValid)
+            return Failure(requestValidation.Status, request, requestValidation.FailureReason);
+        EmPlannerConfiguration configuration = requestValidation.Snapshot.Configuration;
+        EmitDebug(request, "request/config validation succeeded");
+        if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                "request-validation", out EmPlanningResult validationFailure))
+        {
+            return validationFailure;
+        }
+        TimeSpan expirationRemaining = CycleRemaining(request, cycleElapsed);
+        using var cycleExpiration = request.CycleDeadlineRemaining.HasValue
+            ? new CancellationTokenSource(expirationRemaining)
+            : null;
+        using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
+            cancellationToken, request.CallerCancellationToken, request.CycleDeadlineToken,
+            cycleExpiration?.Token ?? CancellationToken.None);
+        CancellationToken planningCancellationToken = linkedCancellation.Token;
+
+        try
+        {
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "projection", out EmPlanningResult deadlineFailure))
+                return deadlineFailure;
+            IReadOnlyList<DirectionSegmentView> segments = ReferencePathSegmenter.Create(request.ReferencePath);
+            if (request.SegmentIndex < 0 || request.SegmentIndex >= segments.Count)
+                return Failure(EmPlanningStatus.InvalidReferencePath, request, "The requested direction segment is unavailable.");
+            DirectionSegmentView segment = segments[request.SegmentIndex];
+            if (!HasCompatibleStateDirection(request.VehicleState, segment.Direction,
+                    configuration.Longitudinal.StopSpeedToleranceMetersPerSecond))
+            {
+                return Failure(EmPlanningStatus.StateDirectionMismatch, request,
+                    "Vehicle signed speed contradicts the selected direction segment.");
+            }
+            EmitDebug(request, "direction-segment selection succeeded");
+
+            var projector = new FrenetProjector();
+            double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
+                : segment.LengthMeters;
+            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
+                    configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
+            {
+                return Failure(EmPlanningStatus.ProjectionFailed, request,
+                    "Vehicle pose could not be projected at an admissible start of the selected direction segment.");
+            }
+            if (Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
+            {
+                return Failure(EmPlanningStatus.ProjectionFailed, request,
+                    "Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
+            }
+            EmitDebug(request, "bounded ego projection succeeded");
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "projection", out deadlineFailure))
+                return deadlineFailure;
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
+                return deadlineFailure;
+            double initialProgressSpeed = Math.Abs(request.VehicleState.SignedLongitudinalSpeedMetersPerSecond);
+            double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
+            var horizonSelector = new PlanningHorizonSelector();
+            EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
+                initialAcceleration, request.PlanningScope, configuration, out PlanningHorizonSelection horizon,
+                out string horizonReason);
+            if (horizonStatus != EmPlanningStatus.Success)
+                return Failure(horizonStatus, request, horizonReason);
+            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
+            EmitDebug(request, "exact horizon and terminal selection succeeded");
+
+            IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
+                startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
+            var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
+            for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
+            EmitDebug(request, "previous-trajectory seed projection completed");
+
+            var corridorBuilder = new StaticCorridorBuilder();
+            if (!corridorBuilder.TryBuild(segment, startProjection.ReferenceS, slice.TerminalBoundary.SegmentLocalS, corridorSeed,
+                    request.Map, request.Vehicle, configuration.Corridor, out StaticCorridor corridor, out string corridorReason))
+            {
+                return Failure(EmPlanningStatus.CorridorInfeasible, request, corridorReason);
+            }
+            EmitDebug(request, "static connected corridor succeeded");
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
+                return deadlineFailure;
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
+                return deadlineFailure;
+            TimeSpan lateralBudget = SmallerBudget(
+                TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds),
+                CycleRemaining(request, cycleElapsed));
+            EmPlannerConfiguration lateralConfiguration = configuration.Copy();
+            lateralConfiguration.Scheduling.SolverTimeoutSeconds = lateralBudget.TotalSeconds;
+            var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
+                request.Vehicle, lateralConfiguration, previousSeed);
+            TimeSpan totalSolveBudget = lateralBudget;
+            var solveBudgetStopwatch = Stopwatch.StartNew();
+            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, planningCancellationToken);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
+                return deadlineFailure;
+            if (!IsSuccess(lateral.Status))
+                return Failure(lateral.Status, request, lateral.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
+            EmitDebug(request, "LS optimization and validation succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
+                return deadlineFailure;
+            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
+                initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
+                out string envelopeReason);
+            if (envelopeStatus != EmPlanningStatus.Success)
+                return Failure(envelopeStatus, request, envelopeReason);
+            LongitudinalKnotSchedule knotSchedule;
+            if (request.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            {
+                EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit,
+                    initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration,
+                    out knotSchedule, out string scheduleReason);
+                if (scheduleStatus != EmPlanningStatus.Success)
+                    return Failure(scheduleStatus, request, scheduleReason);
+            }
+            else
+            {
+                knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
+                    configuration.Scheduling.OutputTimeStepSeconds);
+            }
+            LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
+                new LongitudinalPreviousTrajectorySeedBuilder().Build(
+                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
+                    segment.SegmentIndex, segment.Direction);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
+                return deadlineFailure;
+            TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
+            remainingSolveBudget = SmallerBudget(remainingSolveBudget, CycleRemaining(request, cycleElapsed));
+            if (remainingSolveBudget <= TimeSpan.Zero)
+            {
+                if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                        "longitudinal", out deadlineFailure))
+                    return deadlineFailure;
+                return Failure(EmPlanningStatus.SolverTimedOut, request,
+                    "LS/ST optimization exhausted the shared solve budget before ST optimization.");
+            }
+            EmPlannerConfiguration longitudinalConfiguration = configuration.Copy();
+            longitudinalConfiguration.Scheduling.SolverTimeoutSeconds = remainingSolveBudget.TotalSeconds;
+            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
+                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, longitudinalConfiguration,
+                request.PlanningScope, knotSchedule,
+                previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
+            envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
+            if (envelopeStatus != EmPlanningStatus.Success)
+                return Failure(envelopeStatus, request, envelopeReason);
+            EmitDebug(request, "PathS speed envelope succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "longitudinal", out deadlineFailure))
+                return deadlineFailure;
+            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(
+                longitudinalInput, planningCancellationToken);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "longitudinal", out deadlineFailure))
+                return deadlineFailure;
+            if (!IsSuccess(longitudinal.Status))
+                return Failure(longitudinal.Status, request, longitudinal.FailureReason);
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
+            EmitDebug(request, "ST optimization and validation succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
+                return deadlineFailure;
+            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
+                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
+                segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
+                request.PlanningScope);
+            EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(configuration).TryAssemble(lateral.Path,
+                longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
+            if (assemblyStatus != EmPlanningStatus.Success)
+                return Failure(assemblyStatus, request, assemblyFailure);
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
+                return deadlineFailure;
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
+            EmitDebug(request, "trajectory assembly succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "world-validation", out deadlineFailure))
+                return deadlineFailure;
+            EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
+            Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary)
+                ? TerminalPose(lateral.Path)
+                : null;
+            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
+                request.Vehicle, configuration, segment.SegmentIndex, segment.LengthMeters, longitudinalInput.PathUpperBoundS,
+                terminalPose, terminalBoundary);
+            if (!publication.IsValid)
+            {
+                return Failure(MapPublicationFailure(publication.Failure), request,
+                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
+            }
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "world-validation", out deadlineFailure))
+                return deadlineFailure;
+            EmitDebug(request, "world-space publication validation succeeded");
+
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
+                return deadlineFailure;
+            if (cancellationToken.IsCancellationRequested)
+                return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");
+
+            EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
+                longitudinal.Status == EmPlanningStatus.SuccessWithFallback
+                ? EmPlanningStatus.SuccessWithFallback
+                : EmPlanningStatus.Success;
+            string publicationDiagnostic = DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType +
+                ";publication=validated";
+            if (lateral.Status == EmPlanningStatus.SuccessWithFallback)
+                publicationDiagnostic += ";lateralFallback=" + lateral.FailureReason;
+            if (!string.IsNullOrWhiteSpace(longitudinal.FailureReason))
+            {
+                publicationDiagnostic += longitudinal.Status == EmPlanningStatus.SuccessWithFallback
+                    ? ";longitudinalFallback=" + longitudinal.FailureReason
+                    : ";longitudinal=" + longitudinal.FailureReason;
+            }
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
+                return deadlineFailure;
+            return new EmPlanningResult(finalStatus, trajectory, publicationDiagnostic);
+        }
+        catch (OperationCanceledException)
+        {
+            if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
+                    "operation", out EmPlanningResult deadlineFailure))
+                return deadlineFailure;
+            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled.");
+        }
+        catch (ArgumentException exception)
+        {
+            return Failure(EmPlanningStatus.Failed, request, exception.Message);
+        }
+        catch (InvalidOperationException exception)
+        {
+            return Failure(EmPlanningStatus.Failed, request, exception.Message);
+        }
+    }
+
+    private static bool TryTerminalFailure(EmPlanningRequest request, CancellationToken cancellationToken,
+        Func<TimeSpan> elapsed, string phase, out EmPlanningResult failure)
+    {
+        if (CallerCancellationRequested(request, cancellationToken))
+        {
+            failure = Failure(EmPlanningStatus.Cancelled, request,
+                "callerCancellation=true;phase=" + phase);
+            return true;
+        }
+        TimeSpan remaining = CycleRemaining(request, elapsed);
+        if (!DeadlineExpired(request, elapsed))
+        {
+            failure = null;
+            return false;
+        }
+        failure = CycleDeadlineFailure(request, phase, remaining);
+        return true;
+    }
+
+    private static bool CallerCancellationRequested(EmPlanningRequest request,
+        CancellationToken cancellationToken)
+    {
+        if (request?.CallerCancellationToken.IsCancellationRequested == true)
+            return true;
+        if (!cancellationToken.IsCancellationRequested)
+            return false;
+        return request == null || !request.CycleDeadlineToken.IsCancellationRequested ||
+            !request.CallerCancellationToken.CanBeCanceled;
+    }
+
+    private static bool DeadlineExpired(EmPlanningRequest request, Func<TimeSpan> elapsed)
+    {
+        return request != null && (request.CycleDeadlineToken.IsCancellationRequested ||
+            request.IsCycleDeadlineExpired() ||
+            request.CycleDeadlineRemaining.HasValue && CycleRemaining(request, elapsed) <= TimeSpan.Zero);
+    }
+
+    private static TimeSpan CycleRemaining(EmPlanningRequest request, Func<TimeSpan> elapsed)
+    {
+        TimeSpan snapshotRemaining = TimeSpan.MaxValue;
+        if (request.CycleDeadlineRemaining.HasValue)
+        {
+            snapshotRemaining = request.CycleDeadlineRemaining.Value - elapsed();
+            if (snapshotRemaining < TimeSpan.Zero)
+                snapshotRemaining = TimeSpan.Zero;
+        }
+        TimeSpan? dynamicRemaining = request.DynamicCycleDeadlineRemaining();
+        if (!dynamicRemaining.HasValue)
+            return snapshotRemaining;
+        return dynamicRemaining.Value < snapshotRemaining ? dynamicRemaining.Value : snapshotRemaining;
+    }
+
+    private static TimeSpan SmallerBudget(TimeSpan configured, TimeSpan cycleRemaining)
+    {
+        return configured < cycleRemaining ? configured : cycleRemaining;
+    }
+
+    private static EmPlanningResult CycleDeadlineFailure(EmPlanningRequest request, string phase,
+        TimeSpan remaining)
+    {
+        return Failure(EmPlanningStatus.CycleDeadlineExpired, request,
+            "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
+            remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
+    }
+
+    private void EmitDebug(EmPlanningRequest request, string message)
+    {
+        if (defaultDebugSink == null || request == null || request.Configuration == null || request.Configuration.Solver == null ||
+            !request.Configuration.Solver.NativeVerbose)
+        {
+            return;
+        }
+        try
+        {
+            defaultDebugSink.Write(message ?? string.Empty);
+        }
+        catch
+        {
+            // Debug output is deliberately isolated from pure planning results.
+        }
+    }
+
+    private static IReadOnlyList<FrenetProjection> ProjectPreviousTrajectorySeed(EmTrajectory previousTrajectory,
+        DirectionSegmentView segment, double minimumReferenceS, double maximumReferenceS, double maximumDistanceMeters)
+    {
+        var projected = new List<FrenetProjection>();
+        if (previousTrajectory == null || previousTrajectory.Metadata.SegmentIndex != segment.SegmentIndex ||
+            previousTrajectory.Metadata.Direction != segment.Direction)
+        {
+            return projected;
+        }
+
+        var projector = new FrenetProjector();
+        double seedReferenceS = minimumReferenceS;
+        for (int index = 0; index < previousTrajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = previousTrajectory.Points[index];
+            if (point == null || point.TimeFromStart <= 0d || point.Direction != segment.Direction)
+                continue;
+            if (projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, minimumReferenceS,
+                    maximumReferenceS, maximumDistanceMeters, seedReferenceS, out FrenetProjection projection))
+            {
+                projected.Add(projection);
+                seedReferenceS = projection.ReferenceS;
+            }
+        }
+        return projected;
+    }
+
+    private static bool HasCompatibleStateDirection(VehicleMotionState state, TravelDirection direction, double stopTolerance)
+    {
+        if (state == null || double.IsNaN(stopTolerance) || double.IsInfinity(stopTolerance) || stopTolerance < 0d)
+            return false;
+        if (Math.Abs(state.SignedLongitudinalSpeedMetersPerSecond) <= stopTolerance)
+            return true;
+        return direction == TravelDirection.Forward
+            ? state.SignedLongitudinalSpeedMetersPerSecond > 0d
+            : state.SignedLongitudinalSpeedMetersPerSecond < 0d;
+    }
+
+    private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward
+            ? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond
+            : configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond;
+    }
+
+    private static bool IsSuccess(EmPlanningStatus status)
+    {
+        return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+    }
+
+    internal static EmPlanningStatus MapPublicationFailure(EmTrajectoryValidationFailure failure)
+    {
+        return failure == EmTrajectoryValidationFailure.TerminalPoseMismatch
+            ? EmPlanningStatus.TerminalPoseMismatch
+            : EmPlanningStatus.ValidationFailed;
+    }
+
+    private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType)
+    {
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
+    }
+
+    private static Pose2D TerminalPose(LateralPath path)
+    {
+        LateralPathPoint terminal = path.Points[path.Points.Count - 1];
+        return new Pose2D(terminal.X, terminal.Y, terminal.VehicleYaw);
+    }
+
+    private static EmPlanningResult Failure(EmPlanningStatus status, EmPlanningRequest request, string reason)
+    {
+        return new EmPlanningResult(status, null, DiagnosticsPrefix(request) + ";reason=" + (reason ?? string.Empty));
+    }
+
+    private static string DiagnosticsPrefix(EmPlanningRequest request)
+    {
+        if (request == null)
+            return "map=;reference=;state=;previous=;segment=";
+        return "map=" + (request.Map == null ? string.Empty : request.Map.SnapshotId.ToString()) +
+            ";reference=" + (request.ReferencePathId ?? string.Empty) +
+            ";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
+            ";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
+            ";segment=" + request.SegmentIndex + ";scope=" + request.PlanningScope;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs

@@ -0,0 +1,9 @@
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Pure, one-shot EM planning boundary with no scheduler, UI, hardware, or clock dependency.</summary>
+public interface IEmPlanningService
+{
+    EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken);
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjection.cs

@@ -0,0 +1,64 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 世界车辆位姿投影到单一受限方向段后的 Frenet 描述。
+/// S 与 L 单位均为 m，航向误差为 rad，距离以 m² 保存；对象不代表跨越换向边界的投影。
+/// </summary>
+public sealed class FrenetProjection
+{
+    /// <summary>
+    /// 从参考点和相对量创建不可变投影结果。
+    /// 参数：lateralOffset 为沿行驶方向左法线的 L（m），headingError 为行驶航向差（rad），squaredDistanceMeters 为非负 m²。
+    /// 返回：携带参考 S 的投影；空参考点、非有限值或负平方距离会被拒绝。
+    /// </summary>
+    public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
+        double squaredDistanceMeters)
+    {
+        if (referencePoint == null)
+            throw new ArgumentNullException(nameof(referencePoint));
+        if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lateralOffset));
+
+        ReferencePoint = referencePoint;
+        ReferenceS = referencePoint.ReferenceS;
+        LateralOffset = lateralOffset;
+        HeadingError = headingError;
+        SquaredDistanceMeters = squaredDistanceMeters;
+    }
+
+    /// <summary>
+    /// 投影命中的插值参考点，其坐标为世界 X/Y（m）。
+    /// </summary>
+    public FrenetReferencePoint ReferencePoint { get; }
+
+    /// <summary>
+    /// 命中点在当前方向段的局部参考弧长 S，单位 m。
+    /// </summary>
+    public double ReferenceS { get; }
+
+    /// <summary>
+    /// 世界位姿相对参考行驶方向左法线的横向偏移 L，单位 m。
+    /// </summary>
+    public double LateralOffset { get; }
+
+    /// <summary>
+    /// 车辆行驶航向相对参考行驶航向的归一化误差，单位 rad。
+    /// </summary>
+    public double HeadingError { get; }
+
+    /// <summary>
+    /// 世界位置与命中参考位置的欧氏距离平方，单位 m²。
+    /// </summary>
+    public double SquaredDistanceMeters { get; }
+
+    /// <summary>
+    /// 判定投影标量是否有限。
+    /// 参数：value 为任意实数；返回：NaN 和正负无穷均返回 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjector.cs

@@ -0,0 +1,188 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 在指定方向段及局部 S 窗口内确定性地投影世界位姿。
+/// 投影不跨越换向边界；世界 X/Y 与距离为 m，航向为 rad，并以种子 S 消除等距候选的拓扑歧义。
+/// </summary>
+public sealed class FrenetProjector
+{
+    /// <summary>
+    /// 等距候选比较和零长度线段识别使用的 S/距离平方数值容差 1e-14。
+    /// </summary>
+    private const double TieTolerance = 1e-14d;
+
+    /// <summary>
+    /// 在窗口内投影世界位姿，并以窗口起点作为等距候选的种子 S。
+    /// 参数：窗口与 maximumDistanceMeters 均为当前段局部 m 制距离；返回：命中距离不超过阈值时返回 true，否则 projection 为 null。
+    /// </summary>
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
+    {
+        return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters,
+            minimumReferenceS, out projection);
+    }
+
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
+    {
+        projection = null;
+        if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) ||
+            !IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) ||
+            !IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d)
+            return false;
+
+        double lowerBound = Math.Max(0d, minimumReferenceS);
+        double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS);
+        if (lowerBound > upperBound)
+            return false;
+
+        Candidate best = null;
+        for (int index = 0; index + 1 < segment.Points.Count; index++)
+        {
+            double startS = Math.Max(lowerBound, segment.Points[index].ArcLength);
+            double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength);
+            if (startS > endS)
+                continue;
+
+            FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS);
+            FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS);
+            ConsiderLine(worldPose, start, end, seedReferenceS, ref best);
+        }
+
+        if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance)
+        {
+            FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound);
+            ConsiderPoint(worldPose, point, seedReferenceS, ref best);
+        }
+
+        if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters)
+            return false;
+
+        FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS);
+        double dx = worldPose.X - reference.X;
+        double dy = worldPose.Y - reference.Y;
+        double travelYaw = reference.TravelYaw;
+        double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw);
+        double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction);
+        double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw);
+        projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters);
+        return true;
+    }
+
+    /// <summary>
+    /// 考察被 S 窗口裁剪后的参考折线边，并将世界点的正交投影加入最优候选比较。
+    /// 参数：端点为世界 X/Y（m）和局部 S（m），seedReferenceS 为 m；零长度（不超过 1e-14）边退化为两个端点比较。
+    /// </summary>
+    private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
+        double seedReferenceS, ref Candidate best)
+    {
+        double dx = end.X - start.X;
+        double dy = end.Y - start.Y;
+        double lengthSquared = dx * dx + dy * dy;
+        if (lengthSquared <= TieTolerance)
+        {
+            ConsiderPoint(worldPose, start, seedReferenceS, ref best);
+            ConsiderPoint(worldPose, end, seedReferenceS, ref best);
+            return;
+        }
+
+        double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared;
+        fraction = Math.Max(0d, Math.Min(1d, fraction));
+        double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction;
+        double x = start.X + dx * fraction;
+        double y = start.Y + dy * fraction;
+        Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
+    }
+
+    /// <summary>
+    /// 将一个离散参考点作为投影候选参与比较。
+    /// 参数：point 的位置为世界 m 制坐标，seedReferenceS 为局部 m 制种子；结果通过 best 原位更新。
+    /// </summary>
+    private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
+        ref Candidate best)
+    {
+        Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
+    }
+
+    /// <summary>
+    /// 依据世界平面平方距离登记候选，并按确定性优先级替换当前最佳值。
+    /// 参数：referenceS、x、y 与 seedReferenceS 分别为局部 S（m）和世界坐标（m）；平方距离由内部计算，单位 m²。
+    /// </summary>
+    private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
+        ref Candidate best)
+    {
+        double dx = worldPose.X - x;
+        double dy = worldPose.Y - y;
+        double squaredDistance = dx * dx + dy * dy;
+        var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS));
+        if (best == null || candidate.IsPreferredTo(best))
+            best = candidate;
+    }
+
+    /// <summary>
+    /// 判定投影计算的标量是否有限。
+    /// 参数：value 为任意实数；返回：NaN 与无穷均返回 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    /// <summary>
+    /// 保存一个待比较的折线投影候选。
+    /// ReferenceS 和 SeedDistance 使用 m，SquaredDistanceMeters 使用 m²，优先级由距离、种子距离及较小 S 依次决定。
+    /// </summary>
+    private sealed class Candidate
+    {
+        /// <summary>
+        /// 创建投影候选。
+        /// 参数：referenceS 与 seedDistance 为 m，squaredDistanceMeters 为 m²；调用方仅传入有限的已计算值。
+        /// </summary>
+        public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
+        {
+            ReferenceS = referenceS;
+            SquaredDistanceMeters = squaredDistanceMeters;
+            SeedDistance = seedDistance;
+        }
+
+        /// <summary>
+        /// 候选在当前方向段的局部参考弧长 S，单位 m。
+        /// </summary>
+        public double ReferenceS { get; }
+
+        /// <summary>
+        /// 候选世界位置到车辆位置的距离平方，单位 m²。
+        /// </summary>
+        public double SquaredDistanceMeters { get; }
+
+        /// <summary>
+        /// 候选 S 与调用方种子 S 的绝对距离，单位 m。
+        /// </summary>
+        public double SeedDistance { get; }
+
+        /// <summary>
+        /// 比较两个候选的稳定优先级。
+        /// 参数：other 为非空候选；返回：平方距离差超过 1e-14 时取较小者，随后取较近种子，仍相等时取较小 S。
+        /// </summary>
+        public bool IsPreferredTo(Candidate other)
+        {
+            if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
+            if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false;
+            if (SeedDistance < other.SeedDistance - TieTolerance) return true;
+            if (SeedDistance > other.SeedDistance + TieTolerance) return false;
+            return ReferenceS < other.ReferenceS;
+        }
+    }
+}
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetReferencePoint.cs

@@ -0,0 +1,112 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 单一方向段内的不可变插值参考样本，连接世界几何与 Frenet 坐标。
+/// X/Y 与参考 S 为 m，航向为 rad，曲率为 1/m、曲率导数为 1/m²；倒车段仍以车辆航向保存几何。
+/// </summary>
+public sealed class FrenetReferencePoint
+{
+    /// <summary>
+    /// 创建已验证的方向段参考样本。
+    /// 参数：位置采用世界 X/Y（m），referenceS 为段局部弧长（m），航向为 rad，曲率量遵循 1/m 与 1/m²；所有数值必须有限。
+    /// 返回：车辆航向会规范到 [-π, π]；任一非有限输入会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
+        TravelDirection direction, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative, double bodyClearance)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        RequireFinite(bodyClearance, nameof(bodyClearance));
+
+        ReferenceS = referenceS;
+        X = x;
+        Y = y;
+        VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
+        UnwrappedVehicleYaw = unwrappedVehicleYaw;
+        Direction = direction;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+        BodyClearance = bodyClearance;
+    }
+
+    /// <summary>
+    /// 样本在当前方向段的局部参考弧长 S，单位 m。
+    /// </summary>
+    public double ReferenceS { get; }
+
+    /// <summary>
+    /// 参考中心线点的世界 X 坐标，单位 m。
+    /// </summary>
+    public double X { get; }
+
+    /// <summary>
+    /// 参考中心线点的世界 Y 坐标，单位 m。
+    /// </summary>
+    public double Y { get; }
+
+    /// <summary>
+    /// 归一化后的车辆车身航向，单位 rad。
+    /// </summary>
+    public double VehicleYaw { get; }
+
+    /// <summary>
+    /// 连续展开的车辆车身航向，单位 rad，供跨点几何插值使用。
+    /// </summary>
+    public double UnwrappedVehicleYaw { get; }
+
+    /// <summary>
+    /// 该参考样本的行驶方向，决定 Frenet 横向正负号和行驶航向。
+    /// </summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>
+    /// 中心线几何曲率，单位 1/m。
+    /// </summary>
+    public double GeometricCurvature { get; }
+
+    /// <summary>
+    /// 满足车辆模型后的车辆曲率，单位 1/m。
+    /// </summary>
+    public double VehicleCurvature { get; }
+
+    /// <summary>
+    /// 车辆曲率相对弧长的导数，单位 1/m²。
+    /// </summary>
+    public double VehicleCurvatureDerivative { get; }
+
+    /// <summary>
+    /// 参考点记录的车体净空或可用裕度，单位 m。
+    /// </summary>
+    public double BodyClearance { get; }
+
+    /// <summary>
+    /// 获取与当前行驶方向一致的连续航向。
+    /// 返回：前进时为车辆展开航向，倒车时加 π；单位 rad，仅供 Frenet 几何计算，不重新归一化。
+    /// </summary>
+    public double TravelYaw
+    {
+        get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
+    }
+
+    /// <summary>
+    /// 拒绝不能安全保存为参考几何的数值。
+    /// 参数：value 是待验证的任意单位标量，name 是异常参数名；NaN 或无穷会引发 <see cref="ArgumentOutOfRangeException"/>。
+    /// </summary>
+    private static void RequireFinite(double value, string name)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(name);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetTransform.cs

@@ -0,0 +1,61 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 在世界坐标与 Frenet 横向约定间转换，并始终以实际行驶方向确定 L 的正负。
+/// 世界位置使用 X/Y（m）、航向使用 rad；倒车时车辆车身航向与行驶航向相差 π。
+/// </summary>
+public static class FrenetTransform
+{
+    /// <summary>
+    /// 将参考点及其横向状态重建为世界车辆中心位姿。
+    /// 参数：lateralOffset 为 L（m），lateralDerivative 为 dL/dS（无量纲），minimumFrenetDenominator 为正的奇异性下界；参考点采用世界 X/Y（m）与 rad 航向。
+    /// 返回：当 1-κL 有限且不小于下界、重建位姿有限时返回 true；空参考点、非法输入或接近 Frenet 奇异点时返回 false。
+    /// </summary>
+    public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
+        double minimumFrenetDenominator, out Pose2D pose)
+    {
+        pose = null;
+        if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
+            !IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
+            return false;
+
+        double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
+        if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
+            return false;
+
+        double travelYaw = referencePoint.TravelYaw;
+        double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
+        double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
+        double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
+        double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
+            ? AngleMath.NormalizeRadians(optimizedTravelYaw)
+            : AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
+        if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
+            return false;
+
+        pose = new Pose2D(x, y, vehicleYaw);
+        return true;
+    }
+
+    /// <summary>
+    /// 从车辆车身航向取得对应实际行驶的连续航向。
+    /// 参数：vehicleYaw 为 rad，direction 指定前进或倒车；返回：前进原样、倒车加 π，结果不归一化。
+    /// </summary>
+    internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
+    }
+
+    /// <summary>
+    /// 判定重建中间量是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 或正负无穷时为 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/ReferencePathInterpolator.cs

@@ -0,0 +1,87 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>
+/// 在恰好一个方向段内按局部参考弧长插值参考几何。
+/// 输入和输出 S、X/Y 与净空均为 m，航向为 rad，曲率为 1/m；插值不会越过方向段边界。
+/// </summary>
+public static class ReferencePathInterpolator
+{
+    /// <summary>
+    /// 接受端点轻微 S 超差并判断精确节点的数值容差，单位 m。
+    /// </summary>
+    private const double Epsilon = 1e-12d;
+
+    /// <summary>
+    /// 为给定局部 S 生成方向一致的 Frenet 参考样本。
+    /// 参数：segment 不可为空，referenceS 为 m；[-1e-12, Length+1e-12] 内的端点超差会夹紧到边界。
+    /// 返回：精确点直接转换，区间内对位置、展开航向和几何量线性插值；超界或退化跨度会引发异常。
+    /// </summary>
+    public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
+    {
+        if (segment == null)
+            throw new ArgumentNullException(nameof(segment));
+        if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+
+        double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS));
+        for (int index = 0; index < segment.Points.Count; index++)
+        {
+            SmoothedPathPoint upper = segment.Points[index];
+            if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon)
+                return FromPoint(upper);
+            if (upper.ArcLength > clampedS)
+            {
+                SmoothedPathPoint lower = segment.Points[index - 1];
+                double span = upper.ArcLength - lower.ArcLength;
+                if (span <= Epsilon)
+                    throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment));
+                double fraction = (clampedS - lower.ArcLength) / span;
+                return new FrenetReferencePoint(
+                    clampedS,
+                    Lerp(lower.X, upper.X, fraction),
+                    Lerp(lower.Y, upper.Y, fraction),
+                    AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)),
+                    Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction),
+                    segment.Direction,
+                    Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
+                    Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
+                    Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
+                    Lerp(lower.BodyClearance, upper.BodyClearance, fraction));
+            }
+        }
+        return FromPoint(segment.Points[segment.Points.Count - 1]);
+    }
+
+    /// <summary>
+    /// 将原始平滑路径点转换为同一局部 S 的 Frenet 参考样本。
+    /// 参数：point 已携带世界 X/Y（m）、航向（rad）和曲率量；返回：保持其方向及所有几何量的不可变副本。
+    /// </summary>
+    private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
+    {
+        return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
+            point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
+            point.BodyClearance);
+    }
+
+    /// <summary>
+    /// 在线性标量区间内计算插值值。
+    /// 参数：lower、upper 为同单位端点，fraction 为无单位比例；返回：同单位的未夹紧线性结果。
+    /// </summary>
+    private static double Lerp(double lower, double upper, double fraction)
+    {
+        return lower + (upper - lower) * fraction;
+    }
+
+    /// <summary>
+    /// 判定插值输入是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 或无穷时为 false。
+    /// </summary>
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCandidate.cs

@@ -0,0 +1,114 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>One discrete lateral iterate expressed against exact reference-S stations.</summary>
+public sealed class LateralCandidate
+{
+    public LateralCandidate(IReadOnlyList<double> referenceStations, IReadOnlyList<double> l, IReadOnlyList<double> dl,
+        IReadOnlyList<double> ddl, IReadOnlyList<double> dddl)
+    {
+        ReferenceStations = CopyStations(referenceStations);
+        int stationCount = ReferenceStations.Count;
+        L = CopyValues(l, stationCount, nameof(l));
+        DL = CopyValues(dl, stationCount, nameof(dl));
+        DDL = CopyValues(ddl, stationCount, nameof(ddl));
+        DDDL = CopyValues(dddl, stationCount - 1, nameof(dddl));
+    }
+
+    public IReadOnlyList<double> ReferenceStations { get; }
+
+    public IReadOnlyList<double> L { get; }
+
+    public IReadOnlyList<double> DL { get; }
+
+    public IReadOnlyList<double> DDL { get; }
+
+    public IReadOnlyList<double> DDDL { get; }
+
+    public static LateralCandidate Integrate(IReadOnlyList<double> referenceStations, double initialL, double initialDL,
+        double initialDDL, IReadOnlyList<double> dddl)
+    {
+        IReadOnlyList<double> stations = CopyStations(referenceStations);
+        if (!IsFinite(initialL) || !IsFinite(initialDL) || !IsFinite(initialDDL))
+            throw new ArgumentOutOfRangeException(nameof(initialL));
+        IReadOnlyList<double> copiedJerk = CopyValues(dddl, stations.Count - 1, nameof(dddl));
+
+        var l = new double[stations.Count];
+        var dl = new double[stations.Count];
+        var ddl = new double[stations.Count];
+        l[0] = initialL;
+        dl[0] = initialDL;
+        ddl[0] = initialDDL;
+        for (int index = 0; index < copiedJerk.Count; index++)
+        {
+            double ds = stations[index + 1] - stations[index];
+            double jerk = copiedJerk[index];
+            ddl[index + 1] = ddl[index] + ds * jerk;
+            dl[index + 1] = dl[index] + ds * ddl[index] + 0.5d * ds * ds * jerk;
+            l[index + 1] = l[index] + ds * dl[index] + 0.5d * ds * ds * ddl[index] +
+                ds * ds * ds * jerk / 6d;
+        }
+        return new LateralCandidate(stations, l, dl, ddl, copiedJerk);
+    }
+
+    public bool SatisfiesExactDiscreteDynamics(double tolerance)
+    {
+        if (!IsFinite(tolerance) || tolerance < 0d)
+            throw new ArgumentOutOfRangeException(nameof(tolerance));
+
+        for (int index = 0; index < DDDL.Count; index++)
+        {
+            double ds = ReferenceStations[index + 1] - ReferenceStations[index];
+            double jerk = DDDL[index];
+            if (Math.Abs(DDL[index + 1] - (DDL[index] + ds * jerk)) > tolerance ||
+                Math.Abs(DL[index + 1] - (DL[index] + ds * DDL[index] + 0.5d * ds * ds * jerk)) > tolerance ||
+                Math.Abs(L[index + 1] - (L[index] + ds * DL[index] + 0.5d * ds * ds * DDL[index] +
+                    ds * ds * ds * jerk / 6d)) > tolerance)
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static IReadOnlyList<double> CopyStations(IReadOnlyList<double> values)
+    {
+        if (values == null || values.Count < 2)
+            throw new ArgumentException("At least two reference-S stations are required.", nameof(values));
+
+        var copy = new List<double>(values.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < values.Count; index++)
+        {
+            double value = values[index];
+            if (!IsFinite(value) || value <= previous)
+                throw new ArgumentException("Reference-S stations must be finite and strictly increasing.", nameof(values));
+            copy.Add(value);
+            previous = value;
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> values, int expectedCount, string parameterName)
+    {
+        if (values == null || values.Count != expectedCount)
+            throw new ArgumentException("Lateral value count does not match the station layout.", parameterName);
+
+        var copy = new List<double>(values.Count);
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (!IsFinite(values[index]))
+                throw new ArgumentOutOfRangeException(parameterName);
+            copy.Add(values[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs

@@ -0,0 +1,224 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Assembles one lateral SQP QP with exact dynamics and finite hard bounds.</summary>
+public sealed class LateralConstraintBuilder
+{
+    private const double Epsilon = 1e-12d;
+    private readonly LateralObjectiveBuilder _objectiveBuilder;
+
+    public LateralConstraintBuilder(LateralObjectiveBuilder objectiveBuilder)
+    {
+        _objectiveBuilder = objectiveBuilder ?? throw new ArgumentNullException(nameof(objectiveBuilder));
+    }
+
+    public bool TryBuild(LateralPlanningInput input, LateralCandidate linearization, out QuadraticProgram problem,
+        out string failureReason)
+    {
+        problem = null;
+        failureReason = string.Empty;
+        if (input == null || linearization == null)
+        {
+            failureReason = "Lateral input and linearization are required.";
+            return false;
+        }
+        if (!TryValidateCandidateStations(input, linearization, out failureReason))
+            return false;
+
+        try
+        {
+            var layout = new LateralVariableLayout(input.ReferenceStations.Count);
+            var hessian = new SparseTripletBuilder(layout.VariableCount, layout.VariableCount, true);
+            var linearCost = new double[layout.VariableCount];
+            _objectiveBuilder.AddTerms(input, layout, linearization, hessian, linearCost);
+
+            int terminalRows = input.TerminalType == EmTerminalType.RollingSafetyStop ? 0 : 2;
+            var constraints = new SparseTripletBuilder(8 * layout.StationCount - 2 + terminalRows, layout.VariableCount);
+            var lower = new List<double>();
+            var upper = new List<double>();
+            int row = 0;
+
+            if (!TryAddLateralBounds(input, layout, linearization, constraints, lower, upper, ref row, out failureReason))
+                return false;
+            AddDerivativeBounds(input, layout, constraints, lower, upper, ref row);
+            AddCurvatureBounds(input, layout, linearization, constraints, lower, upper, ref row);
+            AddStartConstraints(input, layout, constraints, lower, upper, ref row);
+            AddExactDynamics(input.ReferenceStations, layout, constraints, lower, upper, ref row);
+            if (input.TerminalType != EmTerminalType.RollingSafetyStop)
+                AddTerminalConstraints(layout, constraints, lower, upper, ref row);
+
+            if (row != 8 * layout.StationCount - 2 + terminalRows)
+                throw new InvalidOperationException("Lateral constraint row accounting is inconsistent.");
+            problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
+            return true;
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+    }
+
+    private static bool TryValidateCandidateStations(LateralPlanningInput input, LateralCandidate candidate,
+        out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
+        {
+            failureReason = "Linearization station count does not match the lateral input.";
+            return false;
+        }
+        for (int index = 0; index < input.ReferenceStations.Count; index++)
+        {
+            if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > Epsilon)
+            {
+                failureReason = "Linearization stations do not match the lateral input.";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static bool TryAddLateralBounds(LateralPlanningInput input, LateralVariableLayout layout,
+        LateralCandidate linearization, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
+        ref int row, out string failureReason)
+    {
+        failureReason = string.Empty;
+        double maximumOffset = RequireNonnegative(input.Configuration.Corridor.MaximumLateralOffsetMeters,
+            "maximum lateral offset");
+        double trustRegion = RequirePositive(input.Configuration.Lateral.MaximumLateralStepPerIterationMeters,
+            "lateral trust region");
+        double minimumDenominator = RequirePositive(input.Configuration.Frenet.MinimumFrenetDenominator,
+            "minimum Frenet denominator");
+
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            LateralInterval corridor = input.Corridor.Stations[station];
+            double minimum = Math.Max(corridor.MinimumL, Math.Max(-maximumOffset, linearization.L[station] - trustRegion));
+            double maximum = Math.Min(corridor.MaximumL, Math.Min(maximumOffset, linearization.L[station] + trustRegion));
+            double referenceCurvature = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                input.ReferenceStations[station]).GeometricCurvature;
+            if (referenceCurvature > 0d)
+                maximum = Math.Min(maximum, (1d - minimumDenominator) / referenceCurvature);
+            else if (referenceCurvature < 0d)
+                minimum = Math.Max(minimum, (1d - minimumDenominator) / referenceCurvature);
+
+            if (!IsFinite(minimum) || !IsFinite(maximum) || minimum > maximum + Epsilon)
+            {
+                failureReason = "The lateral corridor, offset, trust-region, and Frenet denominator bounds do not intersect at station " + station + ".";
+                return false;
+            }
+            AddSingleVariableRow(constraints, lower, upper, ref row, layout.L(station), minimum, maximum);
+        }
+        return true;
+    }
+
+    private static void AddDerivativeBounds(LateralPlanningInput input, LateralVariableLayout layout,
+        SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
+    {
+        double slope = RequirePositive(input.Configuration.Lateral.MaximumLateralSlope, "maximum lateral slope");
+        double second = RequirePositive(input.Configuration.Lateral.MaximumLateralSecondDerivativePerMeter,
+            "maximum lateral second derivative");
+        double third = RequirePositive(input.Configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter,
+            "maximum lateral third derivative");
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            AddSingleVariableRow(constraints, lower, upper, ref row, layout.DL(station), -slope, slope);
+            AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDL(station), -second, second);
+        }
+        for (int interval = 0; interval < layout.StationCount - 1; interval++)
+            AddSingleVariableRow(constraints, lower, upper, ref row, layout.DDDL(interval), -third, third);
+    }
+
+    private static void AddCurvatureBounds(LateralPlanningInput input, LateralVariableLayout layout,
+        LateralCandidate linearization, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
+        ref int row)
+    {
+        double maximumCurvature = LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle);
+        IReadOnlyList<LateralCurvatureLinearization> affines = LateralCurvatureLinearization.Create(input, layout,
+            linearization);
+        for (int station = 0; station < affines.Count; station++)
+        {
+            LateralCurvatureLinearization affine = affines[station];
+            AddRow(constraints, lower, upper, ref row, -maximumCurvature - affine.Constant,
+                maximumCurvature - affine.Constant, affine.VariableIndices, affine.Gradient);
+        }
+    }
+
+    private static void AddStartConstraints(LateralPlanningInput input, LateralVariableLayout layout,
+        SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
+    {
+        double denominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature *
+            input.StartProjection.LateralOffset;
+        double startSlope = denominator * Math.Tan(input.StartProjection.HeadingError);
+        if (!IsFinite(startSlope))
+            throw new ArgumentException("The start lateral slope is non-finite.", nameof(input));
+        AddSingleVariableRow(constraints, lower, upper, ref row, layout.L(0), input.StartProjection.LateralOffset,
+            input.StartProjection.LateralOffset);
+        AddSingleVariableRow(constraints, lower, upper, ref row, layout.DL(0), startSlope, startSlope);
+    }
+
+    private static void AddExactDynamics(IReadOnlyList<double> stations, LateralVariableLayout layout,
+        SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
+    {
+        for (int interval = 0; interval < layout.StationCount - 1; interval++)
+        {
+            double ds = stations[interval + 1] - stations[interval];
+            AddRow(constraints, lower, upper, ref row, 0d, 0d,
+                new[] { layout.DDL(interval), layout.DDL(interval + 1), layout.DDDL(interval) },
+                new[] { -1d, 1d, -ds });
+            AddRow(constraints, lower, upper, ref row, 0d, 0d,
+                new[] { layout.DL(interval), layout.DL(interval + 1), layout.DDL(interval), layout.DDDL(interval) },
+                new[] { -1d, 1d, -ds, -0.5d * ds * ds });
+            AddRow(constraints, lower, upper, ref row, 0d, 0d,
+                new[] { layout.L(interval), layout.L(interval + 1), layout.DL(interval), layout.DDL(interval), layout.DDDL(interval) },
+                new[] { -1d, 1d, -ds, -0.5d * ds * ds, -ds * ds * ds / 6d });
+        }
+    }
+
+    private static void AddTerminalConstraints(LateralVariableLayout layout, SparseTripletBuilder constraints,
+        IList<double> lower, IList<double> upper, ref int row)
+    {
+        AddSingleVariableRow(constraints, lower, upper, ref row, layout.L(layout.StationCount - 1), 0d, 0d);
+        AddSingleVariableRow(constraints, lower, upper, ref row, layout.DL(layout.StationCount - 1), 0d, 0d);
+    }
+
+    private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
+        ref int row, int variable, double minimum, double maximum)
+    {
+        AddRow(constraints, lower, upper, ref row, minimum, maximum, new[] { variable }, new[] { 1d });
+    }
+
+    private static void AddRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row,
+        double minimum, double maximum, IReadOnlyList<int> variables, IReadOnlyList<double> coefficients)
+    {
+        if (!IsFinite(minimum) || !IsFinite(maximum) || minimum > maximum || variables.Count != coefficients.Count)
+            throw new ArgumentException("Lateral constraint bounds are invalid.");
+        for (int index = 0; index < variables.Count; index++)
+            constraints.Add(row, variables[index], coefficients[index]);
+        lower.Add(minimum);
+        upper.Add(maximum);
+        row++;
+    }
+
+    private static double RequirePositive(double value, string name)
+    {
+        if (!IsFinite(value) || value <= 0d)
+            throw new ArgumentOutOfRangeException(name);
+        return value;
+    }
+
+    private static double RequireNonnegative(double value, string name)
+    {
+        if (!IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(name);
+        return value;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCurvatureLinearization.cs

@@ -0,0 +1,101 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Shared affine vehicle-curvature model used by the LS objective and hard QP constraints.</summary>
+internal sealed class LateralCurvatureLinearization
+{
+    private LateralCurvatureLinearization(int[] variableIndices, double[] gradient, double constant)
+    {
+        VariableIndices = variableIndices;
+        Gradient = gradient;
+        Constant = constant;
+    }
+
+    internal IReadOnlyList<int> VariableIndices { get; }
+
+    internal IReadOnlyList<double> Gradient { get; }
+
+    internal double Constant { get; }
+
+    internal static IReadOnlyList<LateralCurvatureLinearization> Create(LateralPlanningInput input,
+        LateralVariableLayout layout, LateralCandidate linearization)
+    {
+        if (input == null) throw new ArgumentNullException(nameof(input));
+        if (layout == null) throw new ArgumentNullException(nameof(layout));
+        if (linearization == null) throw new ArgumentNullException(nameof(linearization));
+        if (layout.StationCount != input.ReferenceStations.Count ||
+            linearization.ReferenceStations.Count != layout.StationCount)
+        {
+            throw new ArgumentException("Curvature linearization stations must match the lateral layout.", nameof(linearization));
+        }
+
+        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
+        var affines = new List<LateralCurvatureLinearization>(layout.StationCount);
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                input.ReferenceStations[station]);
+            double l = linearization.L[station];
+            double dl = linearization.DL[station];
+            double ddl = linearization.DDL[station];
+            double referenceCurvature = reference.GeometricCurvature;
+            double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
+            double a = 1d - referenceCurvature * l;
+            double denominatorSquared = a * a + dl * dl;
+            if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
+                throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
+
+            double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
+            double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
+            double numerator = a * a * referenceCurvature + a * ddl +
+                referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
+            double geometricCurvature = numerator / denominatorPow3Over2;
+            double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
+                referenceCurvature * ddl + referenceCurvatureDerivative * dl;
+            double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
+            double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
+            double dDenominatorSquaredDSlope = 2d * dl;
+            double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
+                1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
+            double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
+                1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
+            double dGeometricDSecondDerivative = a / denominatorPow3Over2;
+            double vehicleCurvature = directionSign * geometricCurvature;
+            double[] gradient =
+            {
+                directionSign * dGeometricDLateral,
+                directionSign * dGeometricDSlope,
+                directionSign * dGeometricDSecondDerivative,
+            };
+            double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
+            if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
+                !IsFinite(gradient[1]) || !IsFinite(gradient[2]))
+            {
+                throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
+            }
+            affines.Add(new LateralCurvatureLinearization(
+                new[] { layout.L(station), layout.DL(station), layout.DDL(station) }, gradient, constant));
+        }
+        return affines;
+    }
+
+    internal static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
+    {
+        if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
+        double maximum = vehicle.MaximumCurvaturePerMeter ??
+            (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d
+                ? 1d / vehicle.MinimumTurningRadiusMeters.Value
+                : double.NaN);
+        if (!IsFinite(maximum) || maximum <= 0d)
+            throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
+        return maximum;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs

@@ -0,0 +1,201 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Reconstructs world geometry and actual path arc length from a lateral candidate.</summary>
+public sealed class LateralGeometryEvaluator
+{
+    public bool TryEvaluate(LateralPlanningInput input, LateralCandidate candidate, out LateralPath path,
+        out string failureReason)
+    {
+        path = null;
+        failureReason = string.Empty;
+        if (!HasMatchingStations(input, candidate, out failureReason))
+            return false;
+
+        try
+        {
+            List<GeometrySample> samples = Reconstruct(input, candidate, out failureReason);
+            if (samples == null)
+                return false;
+            CalculateActualPathSAndCurvatureDerivative(samples, out failureReason);
+            if (failureReason.Length != 0)
+                return false;
+
+            var points = new List<LateralPathPoint>(samples.Count);
+            for (int index = 0; index < samples.Count; index++)
+            {
+                GeometrySample sample = samples[index];
+                double dddl = candidate.DDDL[Math.Min(index, candidate.DDDL.Count - 1)];
+                points.Add(new LateralPathPoint(sample.ReferenceS, sample.PathS, sample.L, sample.DL, sample.DDL, dddl,
+                    sample.X, sample.Y, sample.VehicleYaw, sample.GeometricCurvature, sample.VehicleCurvature,
+                    sample.VehicleCurvatureDerivative));
+            }
+            path = new LateralPath(points, false);
+            return true;
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+    }
+
+    private static List<GeometrySample> Reconstruct(LateralPlanningInput input, LateralCandidate candidate,
+        out string failureReason)
+    {
+        failureReason = string.Empty;
+        double minimumDenominator = input.Configuration.Frenet.MinimumFrenetDenominator;
+        if (!IsFinite(minimumDenominator) || minimumDenominator <= 0d)
+        {
+            failureReason = "The minimum Frenet denominator is invalid.";
+            return null;
+        }
+
+        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
+        var samples = new List<GeometrySample>(candidate.ReferenceStations.Count);
+        for (int index = 0; index < candidate.ReferenceStations.Count; index++)
+        {
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                candidate.ReferenceStations[index]);
+            double l = candidate.L[index];
+            double dl = candidate.DL[index];
+            double ddl = candidate.DDL[index];
+            double denominator = 1d - reference.GeometricCurvature * l;
+            if (!IsFinite(denominator) || denominator < minimumDenominator)
+            {
+                failureReason = "Frenet denominator is below the hard minimum at station " + index + ".";
+                return null;
+            }
+
+            double travelYaw = reference.TravelYaw + Math.Atan2(dl, denominator);
+            double vehicleYaw = input.ReferenceSegment.Direction == TravelDirection.Forward
+                ? AngleMath.NormalizeRadians(travelYaw)
+                : AngleMath.NormalizeRadians(travelYaw + Math.PI);
+            double x = reference.X - l * Math.Sin(reference.TravelYaw);
+            double y = reference.Y + l * Math.Cos(reference.TravelYaw);
+            double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
+                directionSign * reference.VehicleCurvatureDerivative);
+            double vehicleCurvature = directionSign * geometricCurvature;
+            if (!IsFinite(travelYaw) || !IsFinite(vehicleYaw) || !IsFinite(x) || !IsFinite(y) ||
+                !IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature))
+            {
+                failureReason = "Reconstructed lateral geometry is non-finite at station " + index + ".";
+                return null;
+            }
+            samples.Add(new GeometrySample(candidate.ReferenceStations[index], l, dl, ddl, x, y, travelYaw, vehicleYaw,
+                geometricCurvature, vehicleCurvature));
+        }
+        return samples;
+    }
+
+    private static void CalculateActualPathSAndCurvatureDerivative(IReadOnlyList<GeometrySample> samples,
+        out string failureReason)
+    {
+        failureReason = string.Empty;
+        samples[0].PathS = 0d;
+        for (int index = 1; index < samples.Count; index++)
+        {
+            double dx = samples[index].X - samples[index - 1].X;
+            double dy = samples[index].Y - samples[index - 1].Y;
+            double chord = Math.Sqrt(dx * dx + dy * dy);
+            if (!IsFinite(chord) || chord <= 0d)
+            {
+                failureReason = "Reconstructed path S is not strictly increasing at station " + index + ".";
+                return;
+            }
+            samples[index].PathS = samples[index - 1].PathS + chord;
+        }
+        for (int index = 0; index < samples.Count; index++)
+        {
+            int lower = index == 0 ? 0 : index - 1;
+            int upper = index == samples.Count - 1 ? samples.Count - 1 : index + 1;
+            double span = samples[upper].PathS - samples[lower].PathS;
+            if (!IsFinite(span) || span <= 0d)
+            {
+                failureReason = "Path-S curvature derivative span is invalid at station " + index + ".";
+                return;
+            }
+            double derivative = (samples[upper].VehicleCurvature - samples[lower].VehicleCurvature) / span;
+            if (!IsFinite(derivative))
+            {
+                failureReason = "Vehicle curvature derivative is non-finite at station " + index + ".";
+                return;
+            }
+            samples[index].VehicleCurvatureDerivative = derivative;
+        }
+    }
+
+    private static bool HasMatchingStations(LateralPlanningInput input, LateralCandidate candidate, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (input == null || candidate == null)
+        {
+            failureReason = "Lateral input and candidate are required.";
+            return false;
+        }
+        if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
+        {
+            failureReason = "Candidate station count does not match the lateral input.";
+            return false;
+        }
+        for (int index = 0; index < input.ReferenceStations.Count; index++)
+        {
+            if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > 1e-12d)
+            {
+                failureReason = "Candidate stations do not match the lateral input.";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    internal static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
+        double referenceCurvatureDerivative)
+    {
+        double a = 1d - reference.GeometricCurvature * l;
+        double denominatorSquared = a * a + dl * dl;
+        double numerator = a * a * reference.GeometricCurvature + a * ddl +
+            referenceCurvatureDerivative * l * dl + 2d * reference.GeometricCurvature * dl * dl;
+        return numerator / (denominatorSquared * Math.Sqrt(denominatorSquared));
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    private sealed class GeometrySample
+    {
+        public GeometrySample(double referenceS, double l, double dl, double ddl, double x, double y, double travelYaw,
+            double vehicleYaw, double geometricCurvature, double vehicleCurvature)
+        {
+            ReferenceS = referenceS;
+            L = l;
+            DL = dl;
+            DDL = ddl;
+            X = x;
+            Y = y;
+            TravelYaw = travelYaw;
+            VehicleYaw = vehicleYaw;
+            GeometricCurvature = geometricCurvature;
+            VehicleCurvature = vehicleCurvature;
+        }
+
+        public double ReferenceS { get; }
+        public double L { get; }
+        public double DL { get; }
+        public double DDL { get; }
+        public double X { get; }
+        public double Y { get; }
+        public double TravelYaw { get; }
+        public double VehicleYaw { get; }
+        public double GeometricCurvature { get; }
+        public double VehicleCurvature { get; }
+        public double PathS { get; set; }
+        public double VehicleCurvatureDerivative { get; set; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs

@@ -0,0 +1,174 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Builds normalized squared-residual costs in OSQP's 0.5*x'P*x + q'x convention.</summary>
+public sealed class LateralObjectiveBuilder
+{
+    public void AddTerms(LateralPlanningInput input, LateralVariableLayout layout, LateralCandidate linearization,
+        SparseTripletBuilder hessian, IList<double> linearCost)
+    {
+        if (input == null)
+            throw new ArgumentNullException(nameof(input));
+        if (layout == null)
+            throw new ArgumentNullException(nameof(layout));
+        if (linearization == null)
+            throw new ArgumentNullException(nameof(linearization));
+        if (hessian == null)
+            throw new ArgumentNullException(nameof(hessian));
+        if (linearCost == null || linearCost.Count != layout.VariableCount)
+            throw new ArgumentException("Linear cost must match the lateral layout.", nameof(linearCost));
+
+        LateralConfiguration lateral = input.Configuration.Lateral ?? throw new ArgumentException("Missing lateral configuration.");
+        LateralWeights weights = lateral.Weights ?? throw new ArgumentException("Missing lateral weights.");
+        double lateralScale = RequirePositive(input.Configuration.Corridor.MaximumLateralOffsetMeters, "lateral scale");
+        double slopeScale = RequirePositive(lateral.MaximumLateralSlope, "slope scale");
+        double secondDerivativeScale = RequirePositive(lateral.MaximumLateralSecondDerivativePerMeter, "second-derivative scale");
+        double thirdDerivativeScale = RequirePositive(lateral.MaximumLateralThirdDerivativePerSquareMeter, "third-derivative scale");
+        double curvatureScale = RequirePositive(LateralCurvatureLinearization.GetMaximumVehicleCurvature(input.Vehicle),
+            "curvature scale");
+        double curvatureVariationScale = GetCurvatureVariationScale(input);
+
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            AddSquaredResidual(hessian, linearCost, new[] { layout.L(station) }, new[] { 1d }, 0d,
+                weights.ReferenceOffset, lateralScale);
+            AddSquaredResidual(hessian, linearCost, new[] { layout.DL(station) }, new[] { 1d }, 0d,
+                weights.HeadingDeviation, slopeScale);
+            AddSquaredResidual(hessian, linearCost, new[] { layout.DDL(station) }, new[] { 1d }, 0d,
+                weights.SecondDerivative, secondDerivativeScale);
+        }
+        for (int interval = 0; interval < layout.StationCount - 1; interval++)
+        {
+            AddSquaredResidual(hessian, linearCost, new[] { layout.DDDL(interval) }, new[] { 1d }, 0d,
+                weights.ThirdDerivative, thirdDerivativeScale);
+        }
+
+        AddPreviousTrajectoryTerms(input, layout, hessian, linearCost, weights.PreviousTrajectory, lateralScale);
+        IReadOnlyList<LateralCurvatureLinearization> curvature = LateralCurvatureLinearization.Create(input, layout,
+            linearization);
+        for (int station = 0; station < curvature.Count; station++)
+        {
+            AddSquaredResidual(hessian, linearCost, curvature[station].VariableIndices, curvature[station].Gradient,
+                curvature[station].Constant, weights.Curvature, curvatureScale);
+        }
+        AddCurvatureVariationTerms(input.ReferenceStations, curvature, hessian, linearCost, weights.CurvatureVariation,
+            curvatureVariationScale);
+        if (input.TerminalType == EmTerminalType.RollingSafetyStop)
+        {
+            AddSquaredResidual(hessian, linearCost, new[] { layout.L(layout.StationCount - 1) }, new[] { 1d }, 0d,
+                weights.RollingTerminal, lateralScale);
+        }
+    }
+
+    private static void AddPreviousTrajectoryTerms(LateralPlanningInput input, LateralVariableLayout layout,
+        SparseTripletBuilder hessian, IList<double> linearCost, double weight, double lateralScale)
+    {
+        if (input.PreviousTrajectorySeed.Count == 0)
+            return;
+
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            double previousL = InterpolatePreviousL(input.PreviousTrajectorySeed, input.ReferenceStations[station]);
+            AddSquaredResidual(hessian, linearCost, new[] { layout.L(station) }, new[] { 1d }, -previousL,
+                weight, lateralScale);
+        }
+    }
+
+    private static void AddCurvatureVariationTerms(IReadOnlyList<double> stations,
+        IReadOnlyList<LateralCurvatureLinearization> curvature,
+        SparseTripletBuilder hessian, IList<double> linearCost, double weight, double scale)
+    {
+        for (int station = 0; station < curvature.Count; station++)
+        {
+            int lower = station == 0 ? 0 : station - 1;
+            int upper = station == curvature.Count - 1 ? curvature.Count - 1 : station + 1;
+            double ds = stations[upper] - stations[lower];
+            if (!IsFinite(ds) || ds <= 0d)
+                throw new ArgumentException("Curvature variation requires strictly increasing stations.", nameof(stations));
+            LateralCurvatureLinearization left = curvature[lower];
+            LateralCurvatureLinearization right = curvature[upper];
+            var indices = new int[left.VariableIndices.Count + right.VariableIndices.Count];
+            var gradient = new double[indices.Length];
+            for (int index = 0; index < left.VariableIndices.Count; index++)
+            {
+                indices[index] = left.VariableIndices[index];
+                gradient[index] = -left.Gradient[index] / ds;
+                indices[left.VariableIndices.Count + index] = right.VariableIndices[index];
+                gradient[left.VariableIndices.Count + index] = right.Gradient[index] / ds;
+            }
+            AddSquaredResidual(hessian, linearCost, indices, gradient, (right.Constant - left.Constant) / ds, weight, scale);
+        }
+    }
+
+    private static void AddSquaredResidual(SparseTripletBuilder hessian, IList<double> linearCost,
+        IReadOnlyList<int> indices, IReadOnlyList<double> gradient, double constant, double weight, double scale)
+    {
+        if (indices.Count != gradient.Count || indices.Count == 0 || !IsFinite(constant))
+            throw new ArgumentException("Affine residual is invalid.");
+        if (!IsFinite(weight) || weight < 0d)
+            throw new ArgumentOutOfRangeException(nameof(weight));
+        double coefficient = 2d * weight / (scale * scale);
+        for (int left = 0; left < indices.Count; left++)
+        {
+            if (!IsFinite(gradient[left]) || indices[left] < 0 || indices[left] >= linearCost.Count)
+                throw new ArgumentOutOfRangeException(nameof(gradient));
+            linearCost[indices[left]] += coefficient * constant * gradient[left];
+            for (int right = left; right < indices.Count; right++)
+            {
+                if (!IsFinite(gradient[right]) || indices[right] < 0 || indices[right] >= linearCost.Count)
+                    throw new ArgumentOutOfRangeException(nameof(gradient));
+                int row = Math.Min(indices[left], indices[right]);
+                int column = Math.Max(indices[left], indices[right]);
+                hessian.Add(row, column, coefficient * gradient[left] * gradient[right]);
+            }
+        }
+    }
+
+    private static double InterpolatePreviousL(IReadOnlyList<FrenetProjection> seed, double referenceS)
+    {
+        if (referenceS <= seed[0].ReferenceS)
+            return seed[0].LateralOffset;
+        for (int index = 1; index < seed.Count; index++)
+        {
+            if (referenceS <= seed[index].ReferenceS)
+            {
+                FrenetProjection lower = seed[index - 1];
+                FrenetProjection upper = seed[index];
+                double span = upper.ReferenceS - lower.ReferenceS;
+                if (span <= 0d)
+                    return upper.LateralOffset;
+                return lower.LateralOffset + (upper.LateralOffset - lower.LateralOffset) *
+                    (referenceS - lower.ReferenceS) / span;
+            }
+        }
+        return seed[seed.Count - 1].LateralOffset;
+    }
+
+    private static double GetCurvatureVariationScale(LateralPlanningInput input)
+    {
+        double maximum = 0d;
+        for (int station = 0; station < input.ReferenceStations.Count; station++)
+        {
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                input.ReferenceStations[station]);
+            maximum = Math.Max(maximum, Math.Abs(reference.VehicleCurvatureDerivative));
+        }
+        return Math.Max(1d, maximum);
+    }
+
+    private static double RequirePositive(double value, string name)
+    {
+        if (!IsFinite(value) || value <= 0d)
+            throw new ArgumentOutOfRangeException(name);
+        return value;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPath.cs

@@ -0,0 +1,29 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable reconstructed lateral path, marked only after independent validation.</summary>
+public sealed class LateralPath
+{
+    public LateralPath(IReadOnlyList<LateralPathPoint> points, bool independentlyValidated)
+    {
+        if (points == null)
+            throw new ArgumentNullException(nameof(points));
+
+        var copy = new List<LateralPathPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++)
+        {
+            if (points[index] == null)
+                throw new ArgumentException("Lateral path points cannot contain null values.", nameof(points));
+            copy.Add(points[index]);
+        }
+        Points = new ReadOnlyCollection<LateralPathPoint>(copy);
+        IsIndependentlyValidated = independentlyValidated;
+    }
+
+    public IReadOnlyList<LateralPathPoint> Points { get; }
+
+    public bool IsIndependentlyValidated { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPathPoint.cs

@@ -0,0 +1,61 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable world-space sample reconstructed from one lateral candidate.</summary>
+public sealed class LateralPathPoint
+{
+    public LateralPathPoint(double referenceS, double pathS, double l, double dl, double ddl, double dddl,
+        double x, double y, double vehicleYaw, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(pathS, nameof(pathS));
+        RequireFinite(l, nameof(l));
+        RequireFinite(dl, nameof(dl));
+        RequireFinite(ddl, nameof(ddl));
+        RequireFinite(dddl, nameof(dddl));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        if (referenceS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+
+        ReferenceS = referenceS;
+        PathS = pathS;
+        L = l;
+        DL = dl;
+        DDL = ddl;
+        DDDL = dddl;
+        X = x;
+        Y = y;
+        VehicleYaw = vehicleYaw;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+    }
+
+    public double ReferenceS { get; }
+    public double PathS { get; }
+    public double L { get; }
+    public double DL { get; }
+    public double DDL { get; }
+    public double DDDL { get; }
+    public double X { get; }
+    public double Y { get; }
+    public double VehicleYaw { get; }
+    public double GeometricCurvature { get; }
+    public double VehicleCurvature { get; }
+    public double VehicleCurvatureDerivative { get; }
+
+    private static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs

@@ -0,0 +1,20 @@
+using System;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Public lateral-planning entry point backed by the solver-neutral SQP optimizer.</summary>
+public sealed class LateralPlanner
+{
+    private readonly SequentialConvexOptimizer _optimizer;
+
+    public LateralPlanner(IQpSolver qpSolver)
+    {
+        _optimizer = new SequentialConvexOptimizer(qpSolver ?? throw new ArgumentNullException(nameof(qpSolver)));
+    }
+
+    public LateralPlanningResult Plan(LateralPlanningInput input, CancellationToken cancellationToken)
+    {
+        return _optimizer.Optimize(input, cancellationToken);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs

@@ -0,0 +1,140 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable inputs for one lateral solve over a single direction segment.</summary>
+public sealed class LateralPlanningInput
+{
+    private const double Epsilon = 1e-12d;
+
+    public LateralPlanningInput(DirectionSegmentView referenceSegment, StaticCorridor corridor,
+        FrenetProjection startProjection, EmTerminalType terminalType, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, IReadOnlyList<FrenetProjection> previousTrajectorySeed)
+    {
+        if (referenceSegment == null)
+            throw new ArgumentNullException(nameof(referenceSegment));
+        if (corridor == null)
+            throw new ArgumentNullException(nameof(corridor));
+        if (startProjection == null)
+            throw new ArgumentNullException(nameof(startProjection));
+        if (vehicle == null)
+            throw new ArgumentNullException(nameof(vehicle));
+        if (configuration == null)
+            throw new ArgumentNullException(nameof(configuration));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+
+        ReferenceSegment = referenceSegment;
+        Corridor = CopyCorridor(corridor);
+        ReferenceStations = CopyStationValues(Corridor.Stations);
+        ValidateCorridorMatchesInput(referenceSegment, Corridor.Stations, startProjection);
+        StartProjection = startProjection;
+        TerminalType = terminalType;
+        Vehicle = CopyVehicle(vehicle);
+        Configuration = configuration.Copy();
+        PreviousTrajectorySeed = CopySeed(previousTrajectorySeed, referenceSegment);
+    }
+
+    public DirectionSegmentView ReferenceSegment { get; }
+
+    public StaticCorridor Corridor { get; }
+
+    public IReadOnlyList<double> ReferenceStations { get; }
+
+    public FrenetProjection StartProjection { get; }
+
+    public EmTerminalType TerminalType { get; }
+
+    public VehicleParameters Vehicle { get; }
+
+    public EmPlannerConfiguration Configuration { get; }
+
+    public IReadOnlyList<FrenetProjection> PreviousTrajectorySeed { get; }
+
+    private static StaticCorridor CopyCorridor(StaticCorridor source)
+    {
+        var stations = new List<LateralInterval>(source.Stations.Count);
+        for (int index = 0; index < source.Stations.Count; index++)
+        {
+            LateralInterval station = source.Stations[index];
+            if (station == null)
+                throw new ArgumentException("Corridor stations cannot be null.", nameof(source));
+            stations.Add(new LateralInterval(station.ReferenceS, station.MinimumL, station.MaximumL, station.SeedL));
+        }
+        return new StaticCorridor(stations);
+    }
+
+    private static IReadOnlyList<double> CopyStationValues(IReadOnlyList<LateralInterval> stations)
+    {
+        if (stations.Count < 2)
+            throw new ArgumentException("A lateral planning input requires at least two corridor stations.", nameof(stations));
+
+        var copy = new List<double>(stations.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < stations.Count; index++)
+        {
+            double referenceS = stations[index].ReferenceS;
+            if (referenceS <= previous)
+                throw new ArgumentException("Corridor reference-S stations must be strictly increasing.", nameof(stations));
+            copy.Add(referenceS);
+            previous = referenceS;
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static void ValidateCorridorMatchesInput(DirectionSegmentView segment, IReadOnlyList<LateralInterval> stations,
+        FrenetProjection start)
+    {
+        if (start.ReferencePoint == null || start.ReferencePoint.Direction != segment.Direction ||
+            start.ReferenceS < -Epsilon || start.ReferenceS > segment.LengthMeters + Epsilon)
+        {
+            throw new ArgumentException("Start projection must belong to the selected direction segment.", nameof(start));
+        }
+        if (Math.Abs(stations[0].ReferenceS - start.ReferenceS) > Epsilon)
+            throw new ArgumentException("The first corridor station must match the start projection reference S.", nameof(stations));
+        if (start.LateralOffset < stations[0].MinimumL - Epsilon || start.LateralOffset > stations[0].MaximumL + Epsilon)
+            throw new ArgumentException("Start projection is outside the first hard corridor interval.", nameof(start));
+
+        for (int index = 0; index < stations.Count; index++)
+        {
+            if (stations[index].ReferenceS < -Epsilon || stations[index].ReferenceS > segment.LengthMeters + Epsilon)
+                throw new ArgumentException("Corridor stations must lie inside the selected direction segment.", nameof(stations));
+        }
+    }
+
+    private static IReadOnlyList<FrenetProjection> CopySeed(IReadOnlyList<FrenetProjection> seed,
+        DirectionSegmentView segment)
+    {
+        var copy = new List<FrenetProjection>(seed == null ? 0 : seed.Count);
+        if (seed != null)
+        {
+            for (int index = 0; index < seed.Count; index++)
+            {
+                FrenetProjection projection = seed[index];
+                if (projection == null || projection.ReferencePoint.Direction != segment.Direction ||
+                    projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon)
+                {
+                    throw new ArgumentException("Previous lateral seed must belong to the selected direction segment.", nameof(seed));
+                }
+                copy.Add(projection);
+            }
+        }
+        return new ReadOnlyCollection<FrenetProjection>(copy);
+    }
+
+    private static VehicleParameters CopyVehicle(VehicleParameters vehicle)
+    {
+        return new VehicleParameters
+        {
+            LengthMeters = vehicle.LengthMeters,
+            WidthMeters = vehicle.WidthMeters,
+            SafetyMarginMeters = vehicle.SafetyMarginMeters,
+            MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
+            MinimumTurningRadiusMeters = vehicle.MinimumTurningRadiusMeters,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningResult.cs

@@ -0,0 +1,29 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Result of the lateral stage; only independently validated paths may be successful.</summary>
+public sealed class LateralPlanningResult
+{
+    public LateralPlanningResult(EmPlanningStatus status, LateralPath path, string failureReason)
+    {
+        if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+
+        bool successful = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+        if (successful && (path == null || path.Points.Count == 0 || !path.IsIndependentlyValidated))
+            throw new ArgumentException("Successful lateral results require a non-empty independently validated path.", nameof(path));
+        if (!successful && path != null)
+            throw new ArgumentException("Failed lateral results cannot contain a path.", nameof(path));
+
+        Status = status;
+        Path = path;
+        FailureReason = failureReason ?? string.Empty;
+    }
+
+    public EmPlanningStatus Status { get; }
+
+    public LateralPath Path { get; }
+
+    public string FailureReason { get; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs

@@ -0,0 +1,318 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Independently recomputes and verifies lateral world geometry before a path may be published.</summary>
+public sealed class LateralSolutionValidator
+{
+    public bool TryValidate(LateralPlanningInput input, LateralCandidate candidate, LateralPath path,
+        out LateralPath validatedPath, out string failureReason)
+    {
+        validatedPath = null;
+        failureReason = string.Empty;
+        if (input == null || candidate == null || path == null)
+        {
+            failureReason = "Lateral input, candidate, and reconstructed path are required.";
+            return false;
+        }
+        if (path.Points.Count != input.ReferenceStations.Count || candidate.ReferenceStations.Count != path.Points.Count)
+        {
+            failureReason = "Lateral path point count does not match the candidate stations.";
+            return false;
+        }
+
+        try
+        {
+            double spatialTolerance = RequireNonnegative(input.Configuration.Validation.SpatialToleranceMeters,
+                "spatial tolerance");
+            double kinematicTolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance,
+                "kinematic tolerance");
+            double residualTolerance = RequireNonnegative(input.Configuration.Solver.StrictResidualTolerance,
+                "strict residual tolerance");
+            if (!ValidateCandidateConstraints(input, candidate, spatialTolerance, kinematicTolerance, residualTolerance,
+                    out failureReason))
+            {
+                return false;
+            }
+
+            List<ExpectedSample> expected = ReconstructIndependently(input, candidate, out failureReason);
+            if (expected == null)
+                return false;
+            if (!ComparePath(path, candidate, input.ReferenceStations, expected, spatialTolerance, kinematicTolerance,
+                    out failureReason))
+            {
+                return false;
+            }
+
+            validatedPath = new LateralPath(path.Points, true);
+            return true;
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+    }
+
+    private static bool ValidateCandidateConstraints(LateralPlanningInput input, LateralCandidate candidate,
+        double spatialTolerance, double kinematicTolerance, double residualTolerance, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
+        {
+            failureReason = "Candidate station count does not match the input.";
+            return false;
+        }
+        if (!candidate.SatisfiesExactDiscreteDynamics(residualTolerance))
+        {
+            failureReason = "Candidate violates exact lateral dynamics.";
+            return false;
+        }
+
+        LateralConfiguration lateral = input.Configuration.Lateral;
+        double maximumCurvature = GetMaximumVehicleCurvature(input.Vehicle);
+        for (int index = 0; index < input.ReferenceStations.Count; index++)
+        {
+            if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > spatialTolerance)
+            {
+                failureReason = "Candidate reference-S does not match the input at station " + index + ".";
+                return false;
+            }
+            LateralInterval corridor = input.Corridor.Stations[index];
+            double l = candidate.L[index];
+            double dl = candidate.DL[index];
+            double ddl = candidate.DDL[index];
+            if (!IsFinite(l) || !IsFinite(dl) || !IsFinite(ddl) || l < corridor.MinimumL - spatialTolerance ||
+                l > corridor.MaximumL + spatialTolerance || Math.Abs(l) > input.Configuration.Corridor.MaximumLateralOffsetMeters + spatialTolerance ||
+                Math.Abs(dl) > lateral.MaximumLateralSlope + kinematicTolerance ||
+                Math.Abs(ddl) > lateral.MaximumLateralSecondDerivativePerMeter + kinematicTolerance)
+            {
+                failureReason = "Candidate violates lateral corridor or derivative limits at station " + index + ".";
+                return false;
+            }
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                input.ReferenceStations[index]);
+            double denominator = 1d - reference.GeometricCurvature * l;
+            if (!IsFinite(denominator) || denominator < input.Configuration.Frenet.MinimumFrenetDenominator - kinematicTolerance)
+            {
+                failureReason = "Candidate violates the Frenet denominator at station " + index + ".";
+                return false;
+            }
+            double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
+                (input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d) * reference.VehicleCurvatureDerivative);
+            double vehicleCurvature = (input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d) *
+                geometricCurvature;
+            if (!IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature) ||
+                Math.Abs(vehicleCurvature) > maximumCurvature + kinematicTolerance)
+            {
+                failureReason = "Candidate violates vehicle curvature at station " + index + ".";
+                return false;
+            }
+        }
+        for (int index = 0; index < candidate.DDDL.Count; index++)
+        {
+            if (!IsFinite(candidate.DDDL[index]) ||
+                Math.Abs(candidate.DDDL[index]) > lateral.MaximumLateralThirdDerivativePerSquareMeter + kinematicTolerance)
+            {
+                failureReason = "Candidate violates third-derivative limits at interval " + index + ".";
+                return false;
+            }
+        }
+
+        double startDenominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature *
+            input.StartProjection.LateralOffset;
+        double expectedStartDL = startDenominator * Math.Tan(input.StartProjection.HeadingError);
+        if (!IsFinite(expectedStartDL) || Math.Abs(candidate.L[0] - input.StartProjection.LateralOffset) > spatialTolerance ||
+            Math.Abs(candidate.DL[0] - expectedStartDL) > kinematicTolerance)
+        {
+            failureReason = "Candidate violates the lateral start state.";
+            return false;
+        }
+        if (input.TerminalType != EmTerminalType.RollingSafetyStop &&
+            (Math.Abs(candidate.L[candidate.L.Count - 1]) > spatialTolerance ||
+             Math.Abs(candidate.DL[candidate.DL.Count - 1]) > kinematicTolerance))
+        {
+            failureReason = "Candidate violates the exact terminal lateral state.";
+            return false;
+        }
+        return true;
+    }
+
+    private static List<ExpectedSample> ReconstructIndependently(LateralPlanningInput input, LateralCandidate candidate,
+        out string failureReason)
+    {
+        failureReason = string.Empty;
+        double minimumDenominator = input.Configuration.Frenet.MinimumFrenetDenominator;
+        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
+        var samples = new List<ExpectedSample>(candidate.ReferenceStations.Count);
+        for (int index = 0; index < candidate.ReferenceStations.Count; index++)
+        {
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                candidate.ReferenceStations[index]);
+            double l = candidate.L[index];
+            double dl = candidate.DL[index];
+            double ddl = candidate.DDL[index];
+            double denominator = 1d - reference.GeometricCurvature * l;
+            if (!IsFinite(denominator) || denominator < minimumDenominator)
+            {
+                failureReason = "Independent reconstruction found a Frenet denominator violation at station " + index + ".";
+                return null;
+            }
+            double travelYaw = reference.TravelYaw + Math.Atan2(dl, denominator);
+            double vehicleYaw = input.ReferenceSegment.Direction == TravelDirection.Forward
+                ? AngleMath.NormalizeRadians(travelYaw)
+                : AngleMath.NormalizeRadians(travelYaw + Math.PI);
+            double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
+                directionSign * reference.VehicleCurvatureDerivative);
+            double vehicleCurvature = directionSign * geometricCurvature;
+            double x = reference.X - l * Math.Sin(reference.TravelYaw);
+            double y = reference.Y + l * Math.Cos(reference.TravelYaw);
+            if (!IsFinite(x) || !IsFinite(y) || !IsFinite(travelYaw) || !IsFinite(vehicleYaw) ||
+                !IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature))
+            {
+                failureReason = "Independent reconstruction produced non-finite geometry at station " + index + ".";
+                return null;
+            }
+            samples.Add(new ExpectedSample(candidate.ReferenceStations[index], x, y, vehicleYaw, geometricCurvature,
+                vehicleCurvature));
+        }
+
+        samples[0].PathS = 0d;
+        for (int index = 1; index < samples.Count; index++)
+        {
+            double dx = samples[index].X - samples[index - 1].X;
+            double dy = samples[index].Y - samples[index - 1].Y;
+            double chord = Math.Sqrt(dx * dx + dy * dy);
+            if (!IsFinite(chord) || chord <= 0d)
+            {
+                failureReason = "Independent reconstruction found non-increasing PathS at station " + index + ".";
+                return null;
+            }
+            samples[index].PathS = samples[index - 1].PathS + chord;
+        }
+        for (int index = 0; index < samples.Count; index++)
+        {
+            int lower = index == 0 ? 0 : index - 1;
+            int upper = index == samples.Count - 1 ? samples.Count - 1 : index + 1;
+            double span = samples[upper].PathS - samples[lower].PathS;
+            if (!IsFinite(span) || span <= 0d)
+            {
+                failureReason = "Independent curvature derivative span is invalid at station " + index + ".";
+                return null;
+            }
+            samples[index].VehicleCurvatureDerivative =
+                (samples[upper].VehicleCurvature - samples[lower].VehicleCurvature) / span;
+        }
+        return samples;
+    }
+
+    private static bool ComparePath(LateralPath path, LateralCandidate candidate, IReadOnlyList<double> stations,
+        IReadOnlyList<ExpectedSample> expected, double spatialTolerance, double kinematicTolerance,
+        out string failureReason)
+    {
+        failureReason = string.Empty;
+        for (int index = 0; index < path.Points.Count; index++)
+        {
+            LateralPathPoint actual = path.Points[index];
+            ExpectedSample sample = expected[index];
+            double dddl = candidate.DDDL[Math.Min(index, candidate.DDDL.Count - 1)];
+            if (!AreClose(actual.ReferenceS, stations[index], spatialTolerance) ||
+                !AreClose(actual.PathS, sample.PathS, spatialTolerance) ||
+                !AreClose(actual.L, candidate.L[index], spatialTolerance) ||
+                !AreClose(actual.DL, candidate.DL[index], kinematicTolerance) ||
+                !AreClose(actual.DDL, candidate.DDL[index], kinematicTolerance) ||
+                !AreClose(actual.DDDL, dddl, kinematicTolerance) ||
+                !AreClose(actual.X, sample.X, spatialTolerance) || !AreClose(actual.Y, sample.Y, spatialTolerance) ||
+                Math.Abs(AngleMath.NormalizeRadians(actual.VehicleYaw - sample.VehicleYaw)) > kinematicTolerance ||
+                !AreClose(actual.GeometricCurvature, sample.GeometricCurvature, kinematicTolerance) ||
+                !AreClose(actual.VehicleCurvature, sample.VehicleCurvature, kinematicTolerance) ||
+                !AreClose(actual.VehicleCurvatureDerivative, sample.VehicleCurvatureDerivative, kinematicTolerance))
+            {
+                failureReason = "Independent lateral geometry validation failed at station " + index + ".";
+                return false;
+            }
+            if (index == 0 && Math.Abs(actual.PathS) > spatialTolerance)
+            {
+                failureReason = "PathS must start at zero.";
+                return false;
+            }
+            if (index > 0 && actual.PathS <= path.Points[index - 1].PathS + spatialTolerance)
+            {
+                failureReason = "PathS must be strictly increasing.";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
+        double referenceCurvatureDerivative)
+    {
+        double a = 1d - reference.GeometricCurvature * l;
+        double denominatorSquared = a * a + dl * dl;
+        double numerator = a * a * reference.GeometricCurvature + a * ddl +
+            referenceCurvatureDerivative * l * dl + 2d * reference.GeometricCurvature * dl * dl;
+        return numerator / (denominatorSquared * Math.Sqrt(denominatorSquared));
+    }
+
+    private static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
+    {
+        if (vehicle == null)
+            throw new ArgumentNullException(nameof(vehicle));
+        if (vehicle.MaximumCurvaturePerMeter.HasValue)
+            return RequirePositive(vehicle.MaximumCurvaturePerMeter.Value, "vehicle maximum curvature");
+        if (vehicle.MinimumTurningRadiusMeters.HasValue)
+            return 1d / RequirePositive(vehicle.MinimumTurningRadiusMeters.Value, "vehicle minimum turning radius");
+        throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
+    }
+
+    private static bool AreClose(double actual, double expected, double tolerance)
+    {
+        return IsFinite(actual) && IsFinite(expected) && Math.Abs(actual - expected) <= tolerance;
+    }
+
+    private static double RequirePositive(double value, string name)
+    {
+        if (!IsFinite(value) || value <= 0d)
+            throw new ArgumentOutOfRangeException(name);
+        return value;
+    }
+
+    private static double RequireNonnegative(double value, string name)
+    {
+        if (!IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(name);
+        return value;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    private sealed class ExpectedSample
+    {
+        public ExpectedSample(double referenceS, double x, double y, double vehicleYaw, double geometricCurvature,
+            double vehicleCurvature)
+        {
+            ReferenceS = referenceS;
+            X = x;
+            Y = y;
+            VehicleYaw = vehicleYaw;
+            GeometricCurvature = geometricCurvature;
+            VehicleCurvature = vehicleCurvature;
+        }
+
+        public double ReferenceS { get; }
+        public double X { get; }
+        public double Y { get; }
+        public double VehicleYaw { get; }
+        public double GeometricCurvature { get; }
+        public double VehicleCurvature { get; }
+        public double PathS { get; set; }
+        public double VehicleCurvatureDerivative { get; set; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralVariableLayout.cs

@@ -0,0 +1,63 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministic variable ranges for one lateral QP discretized on N stations.</summary>
+public sealed class LateralVariableLayout
+{
+    public LateralVariableLayout(int stationCount)
+    {
+        if (stationCount < 2)
+            throw new ArgumentOutOfRangeException(nameof(stationCount), "At least two reference-S stations are required.");
+
+        StationCount = stationCount;
+        LStart = 0;
+        DLStart = stationCount;
+        DDLStart = 2 * stationCount;
+        DDDLStart = 3 * stationCount;
+        VariableCount = 4 * stationCount - 1;
+    }
+
+    public int StationCount { get; }
+
+    public int LStart { get; }
+
+    public int DLStart { get; }
+
+    public int DDLStart { get; }
+
+    public int DDDLStart { get; }
+
+    public int VariableCount { get; }
+
+    public int L(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return LStart + stationIndex;
+    }
+
+    public int DL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DLStart + stationIndex;
+    }
+
+    public int DDL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DDLStart + stationIndex;
+    }
+
+    public int DDDL(int intervalIndex)
+    {
+        if (intervalIndex < 0 || intervalIndex >= StationCount - 1)
+            throw new ArgumentOutOfRangeException(nameof(intervalIndex));
+        return DDDLStart + intervalIndex;
+    }
+
+    private void RequireStationIndex(int stationIndex)
+    {
+        if (stationIndex < 0 || stationIndex >= StationCount)
+            throw new ArgumentOutOfRangeException(nameof(stationIndex));
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs

@@ -0,0 +1,321 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Runs bounded lateral SQP iterations and retains only independently validated candidates.</summary>
+public sealed class SequentialConvexOptimizer
+{
+    private const double StationTolerance = 1e-12d;
+    private readonly IQpSolver _qpSolver;
+    private readonly LateralConstraintBuilder _constraintBuilder;
+    private readonly LateralGeometryEvaluator _geometryEvaluator;
+    private readonly LateralSolutionValidator _solutionValidator;
+
+    public SequentialConvexOptimizer(IQpSolver qpSolver)
+        : this(qpSolver, new LateralConstraintBuilder(new LateralObjectiveBuilder()), new LateralGeometryEvaluator(),
+            new LateralSolutionValidator())
+    {
+    }
+
+    internal SequentialConvexOptimizer(IQpSolver qpSolver, LateralConstraintBuilder constraintBuilder,
+        LateralGeometryEvaluator geometryEvaluator, LateralSolutionValidator solutionValidator)
+    {
+        _qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
+        _constraintBuilder = constraintBuilder ?? throw new ArgumentNullException(nameof(constraintBuilder));
+        _geometryEvaluator = geometryEvaluator ?? throw new ArgumentNullException(nameof(geometryEvaluator));
+        _solutionValidator = solutionValidator ?? throw new ArgumentNullException(nameof(solutionValidator));
+    }
+
+    public LateralPlanningResult Optimize(LateralPlanningInput input, CancellationToken cancellationToken)
+    {
+        if (input == null)
+            return Failed(EmPlanningStatus.InvalidInput, "Lateral planning input is required.");
+
+        if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
+                out string configurationFailure))
+        {
+            return Failed(EmPlanningStatus.InvalidInput, configurationFailure);
+        }
+
+        LateralCandidate iterate = CreateInitialIterate(input);
+        var warmStart = new double[new LateralVariableLayout(input.ReferenceStations.Count).VariableCount];
+        LateralPath lastValidatedPath = null;
+        double previousObjective = 0d;
+        bool hasPreviousObjective = false;
+        var stopwatch = Stopwatch.StartNew();
+
+        for (int iteration = 0; iteration < input.Configuration.Solver.MaximumOuterIterations; iteration++)
+        {
+            if (cancellationToken.IsCancellationRequested)
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Cancelled, "Lateral SQP was cancelled.");
+
+            TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
+            if (remainingBudget <= TimeSpan.Zero)
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverTimedOut,
+                    "Lateral SQP exhausted its solve budget.");
+
+            if (!_constraintBuilder.TryBuild(input, iterate, out QuadraticProgram problem, out string failureReason))
+            {
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.LateralInfeasible,
+                    "Lateral SQP constraints are infeasible: " + failureReason);
+            }
+
+            QpSolveResult solved = _qpSolver.Solve(problem,
+                new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
+                    remainingBudget, settings.EnableWarmStart, settings.EnablePolishing, settings.EnableNativeVerboseOutput),
+                warmStart, cancellationToken);
+
+            if (cancellationToken.IsCancellationRequested)
+                return Failed(EmPlanningStatus.Cancelled, "Lateral SQP was cancelled after the QP solve.");
+            if (solved == null)
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed, "The lateral QP solver returned no result.");
+
+            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
+            {
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverTimedOut,
+                    "The lateral QP solver timed out: " + solved.Diagnostic);
+            }
+            if (solved.Status == QpSolveStatus.Cancelled)
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Cancelled,
+                    "The lateral QP solver was cancelled: " + solved.Diagnostic);
+            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
+            {
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.LateralInfeasible,
+                    "The lateral QP solver reported infeasibility: " + solved.Diagnostic);
+            }
+            if (solved.Status == QpSolveStatus.SolverUnavailable)
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverUnavailable,
+                    "The lateral QP solver is unavailable: " + solved.Diagnostic);
+            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
+            {
+                return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed,
+                    "The lateral QP solver failed: " + solved.Diagnostic);
+            }
+            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, input.Configuration.Solver.StrictResidualTolerance))
+            {
+                continue;
+            }
+            if (!TryCreateCandidate(input.ReferenceStations, solved.Primal, out LateralCandidate candidate))
+                continue;
+            if (!_geometryEvaluator.TryEvaluate(input, candidate, out LateralPath evaluatedPath, out _))
+                continue;
+
+            // A geometrically evaluable QP solution that remains inside the static
+            // corridor is the next SQP linearization point, even when the independent
+            // validator rejects it.  Otherwise the next outer pass rebuilds the identical
+            // convex subproblem and cannot correct that result.  An out-of-corridor vector
+            // must not become an iterate: it can make the following trust region infeasible.
+            LateralCandidate previousIterate = iterate;
+            if (RespectsStaticCorridor(input, candidate))
+            {
+                iterate = candidate;
+                warmStart = CopyValues(solved.Primal);
+            }
+
+            if (!_solutionValidator.TryValidate(input, candidate, evaluatedPath, out LateralPath validatedPath, out _))
+                continue;
+
+            double maximumLateralChange = MaximumLateralChange(previousIterate, candidate);
+            double relativeObjectiveImprovement = hasPreviousObjective
+                ? RelativeObjectiveImprovement(previousObjective, solved.Objective)
+                : double.PositiveInfinity;
+            lastValidatedPath = CopyPath(validatedPath);
+            previousObjective = solved.Objective;
+            hasPreviousObjective = true;
+
+            if (maximumLateralChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
+                return new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
+        }
+
+        return lastValidatedPath == null
+            ? Failed(EmPlanningStatus.LateralInfeasible, "No independently validated lateral candidate was found.")
+            : new LateralPlanningResult(
+                EmPlanningStatus.SuccessWithFallback,
+                lastValidatedPath,
+                "Lateral SQP reached its outer-iteration limit before strict convergence.");
+    }
+
+    private static bool TryCreateSettings(LateralPlanningInput input, out QpSolverSettings settings, out TimeSpan totalBudget,
+        out double convergenceTolerance, out string failureReason)
+    {
+        settings = null;
+        totalBudget = TimeSpan.Zero;
+        convergenceTolerance = 0d;
+        failureReason = string.Empty;
+        SolverConfiguration solver = input.Configuration.Solver;
+        SchedulingConfiguration scheduling = input.Configuration.Scheduling;
+        if (solver == null || scheduling == null || solver.MaximumOuterIterations <= 0 ||
+            !IsPositiveFinite(solver.AbsoluteTolerance) || !IsPositiveFinite(solver.RelativeTolerance) ||
+            !IsPositiveFinite(solver.StrictResidualTolerance) || !IsPositiveFinite(scheduling.SolverTimeoutSeconds))
+        {
+            failureReason = "The lateral SQP solver configuration is invalid.";
+            return false;
+        }
+        try
+        {
+            totalBudget = TimeSpan.FromSeconds(scheduling.SolverTimeoutSeconds);
+            settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
+                totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
+            convergenceTolerance = solver.StrictResidualTolerance;
+            return true;
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+    }
+
+    private static LateralCandidate CreateInitialIterate(LateralPlanningInput input)
+    {
+        int stationCount = input.ReferenceStations.Count;
+        var l = new double[stationCount];
+        var dl = new double[stationCount];
+        var ddl = new double[stationCount];
+        var dddl = new double[stationCount - 1];
+        bool coversAllStations = input.PreviousTrajectorySeed.Count >= 2 &&
+            input.PreviousTrajectorySeed[0].ReferenceS <= input.ReferenceStations[0] + StationTolerance &&
+            input.PreviousTrajectorySeed[input.PreviousTrajectorySeed.Count - 1].ReferenceS >=
+            input.ReferenceStations[stationCount - 1] - StationTolerance;
+
+        for (int index = 0; index < stationCount; index++)
+        {
+            LateralInterval corridor = input.Corridor.Stations[index];
+            l[index] = coversAllStations
+                ? InterpolateSeedL(input.PreviousTrajectorySeed, input.ReferenceStations[index])
+                : Clamp(0d, corridor.MinimumL, corridor.MaximumL);
+        }
+
+        double startDenominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature *
+            input.StartProjection.LateralOffset;
+        l[0] = input.StartProjection.LateralOffset;
+        dl[0] = startDenominator * Math.Tan(input.StartProjection.HeadingError);
+        return new LateralCandidate(input.ReferenceStations, l, dl, ddl, dddl);
+    }
+
+    private static bool TryCreateCandidate(IReadOnlyList<double> stations, IReadOnlyList<double> primal,
+        out LateralCandidate candidate)
+    {
+        candidate = null;
+        if (primal == null)
+            return false;
+        try
+        {
+            var layout = new LateralVariableLayout(stations.Count);
+            if (primal.Count != layout.VariableCount)
+                return false;
+            var l = new double[layout.StationCount];
+            var dl = new double[layout.StationCount];
+            var ddl = new double[layout.StationCount];
+            var dddl = new double[layout.StationCount - 1];
+            for (int index = 0; index < layout.StationCount; index++)
+            {
+                l[index] = primal[layout.L(index)];
+                dl[index] = primal[layout.DL(index)];
+                ddl[index] = primal[layout.DDL(index)];
+            }
+            for (int index = 0; index < dddl.Length; index++)
+                dddl[index] = primal[layout.DDDL(index)];
+            candidate = new LateralCandidate(stations, l, dl, ddl, dddl);
+            return true;
+        }
+        catch (ArgumentException)
+        {
+            return false;
+        }
+    }
+
+    private static bool HasStrictResiduals(QpSolveResult result, double tolerance)
+    {
+        return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d &&
+            result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
+    }
+
+    private static double MaximumLateralChange(LateralCandidate previous, LateralCandidate current)
+    {
+        double maximum = 0d;
+        for (int index = 0; index < previous.L.Count; index++)
+            maximum = Math.Max(maximum, Math.Abs(current.L[index] - previous.L[index]));
+        return maximum;
+    }
+
+    private static bool RespectsStaticCorridor(LateralPlanningInput input, LateralCandidate candidate)
+    {
+        for (int index = 0; index < candidate.L.Count; index++)
+        {
+            LateralInterval interval = input.Corridor.Stations[index];
+            if (candidate.L[index] < interval.MinimumL || candidate.L[index] > interval.MaximumL)
+                return false;
+        }
+        return true;
+    }
+
+    private static double RelativeObjectiveImprovement(double previous, double current)
+    {
+        return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
+    }
+
+    private static LateralPlanningResult FallbackOrFailure(LateralPath path, EmPlanningStatus failureStatus, string failureReason)
+    {
+        return failureStatus == EmPlanningStatus.Cancelled || path == null
+            ? Failed(failureStatus, failureReason)
+            : new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, path, failureReason);
+    }
+
+    private static LateralPlanningResult Failed(EmPlanningStatus status, string reason)
+    {
+        return new LateralPlanningResult(status, null, reason);
+    }
+
+    private static LateralPath CopyPath(LateralPath source)
+    {
+        var points = new List<LateralPathPoint>(source.Points.Count);
+        for (int index = 0; index < source.Points.Count; index++)
+        {
+            LateralPathPoint point = source.Points[index];
+            points.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
+                point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
+                point.VehicleCurvatureDerivative));
+        }
+        return new LateralPath(points, true);
+    }
+
+    private static double[] CopyValues(IReadOnlyList<double> source)
+    {
+        var copy = new double[source.Count];
+        for (int index = 0; index < source.Count; index++)
+            copy[index] = source[index];
+        return copy;
+    }
+
+    private static double InterpolateSeedL(IReadOnlyList<FrenetProjection> seed, double referenceS)
+    {
+        if (referenceS <= seed[0].ReferenceS)
+            return seed[0].LateralOffset;
+        for (int index = 1; index < seed.Count; index++)
+        {
+            if (referenceS <= seed[index].ReferenceS)
+            {
+                FrenetProjection lower = seed[index - 1];
+                FrenetProjection upper = seed[index];
+                double span = upper.ReferenceS - lower.ReferenceS;
+                return span <= StationTolerance ? upper.LateralOffset : lower.LateralOffset +
+                    (upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
+            }
+        }
+        return seed[seed.Count - 1].LateralOffset;
+    }
+
+    private static double Clamp(double value, double minimum, double maximum)
+    {
+        return Math.Max(minimum, Math.Min(maximum, value));
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
+    }
+}