feat: 发布 EM 轨迹规划首个版本

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/CorridorConfiguration.cs

@@ -1,13 +1,34 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs

@@ -1,15 +1,42 @@
 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
@@ -17,8 +44,8 @@ public sealed partial class EmPlannerConfiguration
             Scheduling = new SchedulingConfiguration
             {
                 ReplanPeriodSeconds = 0.20d,
-                TimeHorizonSeconds = 6d,                //窗口的规划时间长度。单位s
-                DistanceHorizonMeters = 500d,           //最大探索s里程距离，单位m
+                TimeHorizonSeconds = 6d,                // 单次规划的时间窗口，单位 s。
+                DistanceHorizonMeters = 500d,           // 单次规划的最大前向探索距离，单位 m。
                 OutputTimeStepSeconds = 0.05d,
                 SolverTimeoutSeconds = 0.10d,
                 HandoffLookaheadSeconds = 0.30d,
@@ -103,6 +130,9 @@ public sealed partial class EmPlannerConfiguration
         };
     }
 
+    /// <summary>
+    /// 深复制当前配置树；输入为当前实例状态，输出与当前实例不共享非空嵌套对象的可修改快照，原本为 <c>null</c> 的子配置保持为 <c>null</c> 且不产生失败状态。
+    /// </summary>
     internal EmPlannerConfiguration Copy()
     {
         return new EmPlannerConfiguration

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/FrenetConfiguration.cs

@@ -1,11 +1,26 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralConfiguration.cs

@@ -1,13 +1,34 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralWeights.cs

@@ -1,16 +1,46 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs

@@ -1,20 +1,62 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalWeights.cs

@@ -1,13 +1,34 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs

@@ -1,19 +1,58 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SolverConfiguration.cs

@@ -1,16 +1,46 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs

@@ -1,12 +1,30 @@
 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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs

@@ -2,8 +2,14 @@ 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))

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs

@@ -1,10 +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

@@ -1,8 +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

@@ -1,8 +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

@@ -1,12 +1,136 @@
 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,
@@ -21,7 +145,10 @@ public sealed class EmPlanningRequest
         string referencePathId,
         string previousTrajectoryId,
         EmMotionModel motionModel,
-        EmPlanningScope planningScope)
+        EmPlanningScope planningScope,
+        TimeSpan? cycleDeadlineRemaining = null,
+        CancellationToken callerCancellationToken = default,
+        CancellationToken cycleDeadlineToken = default)
     {
         ReferencePath = referencePath;
         Map = map;
@@ -37,25 +164,166 @@ public sealed class EmPlanningRequest
         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;
+        }
+    }
 }
 
-// The request owns this DTO contract; Task 2 adds its request-bound settings.
+/// <summary>
+/// 为请求契约保留的 <see cref="EmPlannerConfiguration"/> 部分声明；实际配置成员由其他同名分部提供。
+/// </summary>
 public sealed partial class EmPlannerConfiguration
 {
 }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs

@@ -2,8 +2,14 @@ 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))
@@ -20,9 +26,18 @@ public sealed class EmPlanningResult
         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

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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs

@@ -1,26 +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

@@ -1,8 +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

@@ -4,8 +4,14 @@ 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)
@@ -27,7 +33,13 @@ public sealed class EmTrajectory
         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

@@ -3,8 +3,14 @@ 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,
@@ -52,16 +58,63 @@ public sealed class EmTrajectoryMetadata
         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

@@ -3,8 +3,14 @@ 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,
@@ -62,23 +68,88 @@ public sealed class EmTrajectoryPoint
         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

@@ -3,8 +3,14 @@ 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,
@@ -19,13 +25,28 @@ public sealed class VehicleMotionState
         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

@@ -2,9 +2,17 @@ using System;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Connected free lateral interval at one exact reference-S station.</summary>
+/// <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) ||
@@ -17,11 +25,30 @@ public sealed class LateralInterval
         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

@@ -4,9 +4,16 @@ using System.Collections.ObjectModel;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Immutable lateral hard bounds for one already-selected topological corridor.</summary>
+/// <summary>
+/// 一个已选择拓扑走廊在离散参考站上的不可变横向硬边界集合。
+/// 站点按方向段局部 S（m）非递减保存，调用方只能沿每个站点的种子连通自由区间优化。
+/// </summary>
 public sealed class StaticCorridor
 {
+    /// <summary>
+    /// 从站点序列创建走廊的防御性只读副本。
+    /// 参数：stations 不能为空且至少含一个按 ReferenceS（m）非递减的非空区间；违反这些约束会被拒绝。
+    /// </summary>
     public StaticCorridor(IReadOnlyList<LateralInterval> stations)
     {
         if (stations == null || stations.Count == 0)
@@ -25,5 +32,8 @@ public sealed class StaticCorridor
         Stations = new ReadOnlyCollection<LateralInterval>(copy);
     }
 
+    /// <summary>
+    /// 按方向段局部参考弧长 S（m）排序的横向硬边界只读列表。
+    /// </summary>
     public IReadOnlyList<LateralInterval> Stations { get; }
 }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridorBuilder.cs

@@ -6,24 +6,49 @@ using MultiWheelC.TrajectoryPlanning.Mapping;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Builds only the seed-connected static free-space corridor of a direction segment.</summary>
+/// <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)
@@ -82,6 +107,10 @@ public sealed class StaticCorridorBuilder
         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)
     {
@@ -115,6 +144,10 @@ public sealed class StaticCorridorBuilder
         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)
     {
@@ -139,6 +172,10 @@ public sealed class StaticCorridorBuilder
         return true;
     }
 
+    /// <summary>
+    /// 生成包含起止锚点的纵向采样站。
+    /// 参数：起止 S 和 spacing 单位均为 m；返回：起点、严格内部等距站以及不同于起点超过 1e-12 m 的终点。
+    /// </summary>
     private static IEnumerable<double> CreateStations(double startReferenceS, double endReferenceS, double spacing)
     {
         yield return startReferenceS;
@@ -148,6 +185,10 @@ public sealed class StaticCorridorBuilder
             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)
     {
@@ -190,6 +231,10 @@ public sealed class StaticCorridorBuilder
         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>();
@@ -202,6 +247,10 @@ public sealed class StaticCorridorBuilder
         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)
     {
@@ -214,6 +263,10 @@ public sealed class StaticCorridorBuilder
             configuration.AdditionalClearanceReserveMeters, out _);
     }
 
+    /// <summary>
+    /// 用保守障碍距离和四个扩张车体角点快速确认明显净空。
+    /// 参数：pose 为世界 X/Y（m）和 rad，车辆尺寸及额外储备为 m；返回：圆形下界安全且四角均在图内时为 true，否则交由精确检查器。
+    /// </summary>
     private static bool IsObviouslyClear(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
         double additionalClearanceReserveMeters)
     {
@@ -237,6 +290,10 @@ public sealed class StaticCorridorBuilder
         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)
@@ -257,6 +314,10 @@ public sealed class StaticCorridorBuilder
         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++)
@@ -265,25 +326,48 @@ public sealed class StaticCorridorBuilder
         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/Facade/EmPlanningService.cs

@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
+using System.Globalization;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
@@ -9,28 +10,65 @@ 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)
     {
-        if (cancellationToken.IsCancellationRequested)
+        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.");
@@ -59,7 +97,11 @@ public sealed class EmPlanningService : IEmPlanningService
                     "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();
@@ -84,18 +126,31 @@ public sealed class EmPlanningService : IEmPlanningService
                 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, configuration, previousSeed);
-            TimeSpan totalSolveBudget = TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds);
+                request.Vehicle, lateralConfiguration, previousSeed);
+            TimeSpan totalSolveBudget = lateralBudget;
             var solveBudgetStopwatch = Stopwatch.StartNew();
-            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
+            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);
@@ -119,9 +174,15 @@ public sealed class EmPlanningService : IEmPlanningService
                 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.");
             }
@@ -136,13 +197,22 @@ public sealed class EmPlanningService : IEmPlanningService
                 return Failure(envelopeStatus, request, envelopeReason);
             EmitDebug(request, "PathS speed envelope succeeded");
 
-            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
+            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,
@@ -151,10 +221,15 @@ public sealed class EmPlanningService : IEmPlanningService
                 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)
@@ -167,8 +242,13 @@ public sealed class EmPlanningService : IEmPlanningService
                 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.");
 
@@ -176,11 +256,25 @@ public sealed class EmPlanningService : IEmPlanningService
                 longitudinal.Status == EmPlanningStatus.SuccessWithFallback
                 ? EmPlanningStatus.SuccessWithFallback
                 : EmPlanningStatus.Success;
-            return new EmPlanningResult(finalStatus, trajectory,
-                DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType + ";publication=validated");
+            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)
@@ -193,6 +287,71 @@ public sealed class EmPlanningService : IEmPlanningService
         }
     }
 
+    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 ||

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjection.cs

@@ -2,9 +2,17 @@ using System;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>World pose projected onto one bounded direction segment.</summary>
+/// <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)
     {
@@ -20,12 +28,35 @@ public sealed class FrenetProjection
         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

@@ -4,11 +4,21 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Deterministically projects a world pose only inside the supplied direction segment and S window.</summary>
+/// <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)
     {
@@ -16,6 +26,10 @@ public sealed class FrenetProjector
             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)
     {
@@ -63,6 +77,10 @@ public sealed class FrenetProjector
         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)
     {
@@ -84,12 +102,20 @@ public sealed class FrenetProjector
         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)
     {
@@ -101,13 +127,25 @@ public sealed class FrenetProjector
             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;
@@ -115,10 +153,25 @@ public sealed class FrenetProjector
             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;
@@ -129,3 +182,7 @@ public sealed class FrenetProjector
         }
     }
 }
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetReferencePoint.cs

@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
+/// <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)
@@ -33,23 +41,69 @@ public sealed class FrenetReferencePoint
         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>Unwrapped direction of travel, used internally for geometry.</summary>
+    /// <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))

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetTransform.cs

@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
+/// <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)
     {
@@ -33,11 +41,19 @@ public static class FrenetTransform
         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

@@ -4,11 +4,22 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
+/// <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)
@@ -45,6 +56,10 @@ public static class ReferencePathInterpolator
         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,
@@ -52,11 +67,19 @@ public static class ReferencePathInterpolator
             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/Longitudinal/LongitudinalConstraintBuilder.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Globalization;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -16,7 +17,15 @@ public sealed class LongitudinalConstraintBuilder
     public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
         out QuadraticProgram problem, out string failureReason)
     {
-        return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason);
+        return TryBuildCore(input, speedLimit, iterate, false, null, null, 0d, out problem, out failureReason);
+    }
+
+    internal bool TryBuildTrusted(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        LongitudinalCandidate anchor, LongitudinalEnvelopeTrustRegion trustRegion, double strictTolerance,
+        out QuadraticProgram problem, out string failureReason)
+    {
+        return TryBuildCore(input, speedLimit, anchor, false, trustRegion, anchor, strictTolerance,
+            out problem, out failureReason);
     }
 
     /// <summary>Builds the bounded full-scope feasibility projection before objective optimization.</summary>
@@ -38,12 +47,14 @@ public sealed class LongitudinalConstraintBuilder
             failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning.";
             return false;
         }
-        return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem,
-            out failureReason);
+        return TryBuildCore(input, speedLimit, linearizationIterate, true, null, null, 0d,
+            out problem, out failureReason);
     }
 
     private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
-        bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason)
+        bool useScheduleReferenceObjective, LongitudinalEnvelopeTrustRegion trustRegion,
+        LongitudinalCandidate trustedAnchor, double strictTolerance, out QuadraticProgram problem,
+        out string failureReason)
     {
         problem = null;
         failureReason = string.Empty;
@@ -63,6 +74,15 @@ public sealed class LongitudinalConstraintBuilder
             {
                 throw new ArgumentException("The ST iterate does not match the configured knot layout.");
             }
+            if ((trustRegion == null) != (trustedAnchor == null))
+                throw new ArgumentException("Trusted QP construction requires both a trust region and anchor.");
+            if (trustRegion != null && (trustRegion.MinimumPathS.Count != layout.KnotCount ||
+                trustRegion.MaximumPathS.Count != layout.KnotCount ||
+                trustRegion.SpeedSlope.Count != layout.KnotCount ||
+                trustRegion.SpeedIntercept.Count != layout.KnotCount))
+            {
+                throw new ArgumentException("The trust region does not match the configured knot layout.");
+            }
             if (!PathSpeedLimitBuilder.TryGetLimits(input, out double directionMaximum, out double maximumAcceleration,
                     out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
             {
@@ -84,12 +104,14 @@ public sealed class LongitudinalConstraintBuilder
                 _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
             int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount);
             int stationaryKnotCount = layout.KnotCount - stabilizationStart;
-            int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
+            int expectedRows = 10 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
             var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
             var lower = new List<double>(expectedRows);
             var upper = new List<double>(expectedRows);
             int row = 0;
-            AddVariableBounds(input, speedLimit, iterate, layout, maximumAcceleration, maximumDeceleration, maximumJerk,
+            AddVariableBounds(input, speedLimit, iterate, trustRegion, layout, maximumAcceleration,
+                maximumDeceleration, maximumJerk, constraints, lower, upper, ref row);
+            AddLowSpeedDecelerationReleaseEnvelope(layout, iterate, maximumJerk,
                 constraints, lower, upper, ref row);
             AddMonotonicProgress(layout, constraints, lower, upper, ref row);
             AddExactDynamics(expectedTimes, layout, constraints, lower, upper, ref row);
@@ -99,6 +121,20 @@ public sealed class LongitudinalConstraintBuilder
             if (row != expectedRows)
                 throw new InvalidOperationException("ST constraint row accounting is inconsistent.");
             problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
+            if (trustedAnchor != null)
+            {
+                LongitudinalQpAuditResult audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, trustedAnchor,
+                    strictTolerance, layout, stabilizationStart);
+                if (!audit.IsFeasible)
+                {
+                    problem = null;
+                    failureReason = "Planner invariant failure: strict anchor is outside trusted QP" +
+                        ";row=" + audit.WorstRow + ";category=" + audit.Category +
+                        ";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) +
+                        audit.Unit + ";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
+                    return false;
+                }
+            }
             return true;
         }
         catch (ArgumentException exception)
@@ -145,7 +181,8 @@ public sealed class LongitudinalConstraintBuilder
     }
 
     private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
+        LongitudinalCandidate iterate, LongitudinalEnvelopeTrustRegion trustRegion,
+        LongitudinalVariableLayout layout, double maximumAcceleration,
         double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
         IList<double> upper, ref int row)
     {
@@ -153,14 +190,32 @@ public sealed class LongitudinalConstraintBuilder
         {
             if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
                 throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds.");
-            AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
+            if (trustRegion == null)
+                AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
+            else
+                AddSingleVariableRow(constraints, lower, upper, layout.S(index),
+                    trustRegion.MinimumPathS[index], trustRegion.MaximumPathS[index], ref row);
             double maximumSpeed = index == 0
                 ? input.DirectionMaximumSpeedMetersPerSecond
                 : input.DirectionMaximumSpeedMetersPerSecond;
             AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
             if (index > 0)
-                AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
-                    constraints, lower, upper, ref row);
+            {
+                if (trustRegion == null)
+                {
+                    AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
+                        constraints, lower, upper, ref row);
+                }
+                else
+                {
+                    AddRow(constraints, lower, upper, row, new[]
+                    {
+                        new Coefficient(layout.U(index), 1d),
+                        new Coefficient(layout.S(index), -trustRegion.SpeedSlope[index]),
+                    }, -QuadraticProgram.MaximumFiniteBound, trustRegion.SpeedIntercept[index]);
+                    row++;
+                }
+            }
             AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
                 ref row);
         }
@@ -209,6 +264,34 @@ public sealed class LongitudinalConstraintBuilder
         }
     }
 
+    internal static void CalculateLowSpeedDecelerationReleaseTangent(
+        double anchorAcceleration, double maximumJerk,
+        out double accelerationCoefficient, out double lowerBound)
+    {
+        if (!IsFinite(anchorAcceleration) || !IsFinite(maximumJerk) || maximumJerk <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(anchorAcceleration));
+        double a0 = Math.Min(0d, anchorAcceleration);
+        accelerationCoefficient = -a0 / maximumJerk;
+        lowerBound = -(a0 * a0) / (2d * maximumJerk);
+    }
+
+    private static void AddLowSpeedDecelerationReleaseEnvelope(LongitudinalVariableLayout layout,
+        LongitudinalCandidate iterate, double maximumJerk, SparseTripletBuilder constraints,
+        IList<double> lower, IList<double> upper, ref int row)
+    {
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            CalculateLowSpeedDecelerationReleaseTangent(iterate.A[index], maximumJerk,
+                out double accelerationCoefficient, out double lowerBound);
+            AddRow(constraints, lower, upper, row, new[]
+            {
+                new Coefficient(layout.U(index), 1d),
+                new Coefficient(layout.A(index), accelerationCoefficient),
+            }, lowerBound, QuadraticProgram.MaximumFiniteBound);
+            row++;
+        }
+    }
+
     private static void AddExactDynamics(IReadOnlyList<double> times, LongitudinalVariableLayout layout,
         SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
     {

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalContinuousProfileValidator.cs

@@ -0,0 +1,136 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Proves the constant-jerk profile used by publication is continuously forward-progressing.</summary>
+internal static class LongitudinalContinuousProfileValidator
+{
+    public static bool TryValidate(LongitudinalCandidate candidate, double tolerance, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (candidate == null)
+        {
+            failureReason = "A longitudinal candidate is required.";
+            return false;
+        }
+        if (!IsFinite(tolerance) || tolerance < 0d)
+        {
+            failureReason = "A finite nonnegative continuous-profile tolerance is required.";
+            return false;
+        }
+
+        double highWater = candidate.S[0];
+        for (int interval = 0; interval < candidate.J.Count; interval++)
+        {
+            double duration = candidate.KnotTimes[interval + 1] - candidate.KnotTimes[interval];
+            double initialS = candidate.S[interval];
+            double initialU = candidate.U[interval];
+            double initialA = candidate.A[interval];
+            double jerk = candidate.J[interval];
+            var evaluationTimes = new List<double>(5) { 0d, duration };
+            if (jerk != 0d)
+                AddIfInside(evaluationTimes, -initialA / jerk, duration);
+            AddSpeedRoots(evaluationTimes, initialU, initialA, jerk, duration);
+            evaluationTimes.Sort();
+
+            double previousTime = double.NegativeInfinity;
+            for (int point = 0; point < evaluationTimes.Count; point++)
+            {
+                double localTime = evaluationTimes[point];
+                if (localTime == previousTime)
+                    continue;
+                previousTime = localTime;
+                Evaluate(initialS, initialU, initialA, jerk, localTime,
+                    out double progress, out double speed, out double acceleration);
+                double regression = highWater - progress;
+                if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) ||
+                    speed < -tolerance || regression > tolerance)
+                {
+                    string kind = !IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration)
+                        ? "non-finite"
+                        : speed < -tolerance ? "negative-speed" : "PathS-regression";
+                    failureReason = FormatFailure(kind, interval, localTime, progress, speed,
+                        acceleration, jerk, regression, string.Empty);
+                    return false;
+                }
+                if (progress > highWater)
+                    highWater = progress;
+            }
+
+            Evaluate(initialS, initialU, initialA, jerk, duration,
+                out double integratedS, out double integratedU, out double integratedA);
+            double sMismatch = Math.Abs(integratedS - candidate.S[interval + 1]);
+            double uMismatch = Math.Abs(integratedU - candidate.U[interval + 1]);
+            double aMismatch = Math.Abs(integratedA - candidate.A[interval + 1]);
+            if (!IsFinite(sMismatch) || !IsFinite(uMismatch) || !IsFinite(aMismatch) ||
+                sMismatch > tolerance || uMismatch > tolerance || aMismatch > tolerance)
+            {
+                string detail = ";endpointMismatchS=" + Invariant(sMismatch) +
+                    ";endpointMismatchU=" + Invariant(uMismatch) +
+                    ";endpointMismatchA=" + Invariant(aMismatch);
+                failureReason = FormatFailure("endpoint-mismatch", interval, duration, integratedS,
+                    integratedU, integratedA, jerk, highWater - integratedS, detail);
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static void AddSpeedRoots(ICollection<double> times, double initialU, double initialA,
+        double jerk, double duration)
+    {
+        if (jerk == 0d)
+        {
+            if (initialA != 0d)
+                AddIfInside(times, -initialU / initialA, duration);
+            return;
+        }
+
+        double discriminant = initialA * initialA - 2d * jerk * initialU;
+        if (!IsFinite(discriminant) || discriminant < 0d)
+            return;
+        double rootTerm = Math.Sqrt(discriminant);
+        AddIfInside(times, (-initialA - rootTerm) / jerk, duration);
+        AddIfInside(times, (-initialA + rootTerm) / jerk, duration);
+    }
+
+    private static void AddIfInside(ICollection<double> times, double localTime, double duration)
+    {
+        if (IsFinite(localTime) && localTime >= 0d && localTime <= duration)
+            times.Add(localTime);
+    }
+
+    private static void Evaluate(double initialS, double initialU, double initialA, double jerk,
+        double localTime, out double progress, out double speed, out double acceleration)
+    {
+        acceleration = initialA + jerk * localTime;
+        speed = initialU + initialA * localTime + 0.5d * jerk * localTime * localTime;
+        progress = initialS + initialU * localTime + 0.5d * initialA * localTime * localTime +
+            jerk * localTime * localTime * localTime / 6d;
+    }
+
+    private static string FormatFailure(string kind, int interval, double localTime, double progress,
+        double speed, double acceleration, double jerk, double regression, string detail)
+    {
+        return "Continuous ST profile rejected: kind=" + kind +
+            ";interval=" + interval.ToString(CultureInfo.InvariantCulture) +
+            ";localTime=" + Invariant(localTime) +
+            ";S=" + Invariant(progress) +
+            ";U=" + Invariant(speed) +
+            ";A=" + Invariant(acceleration) +
+            ";J=" + Invariant(jerk) +
+            ";regression=" + Invariant(regression) + detail + ".";
+    }
+
+    private static string Invariant(double value)
+    {
+        return value.ToString("R", CultureInfo.InvariantCulture);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalEnvelopeTrustRegion.cs

@@ -0,0 +1,274 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalEnvelopeTrustRegion
+{
+    internal LongitudinalEnvelopeTrustRegion(IReadOnlyList<double> minimumPathS, IReadOnlyList<double> maximumPathS,
+        IReadOnlyList<double> speedSlope, IReadOnlyList<double> speedIntercept,
+        IReadOnlyList<int> envelopeSegmentIndex, double scale)
+    {
+        MinimumPathS = Copy(minimumPathS, nameof(minimumPathS));
+        MaximumPathS = Copy(maximumPathS, nameof(maximumPathS));
+        SpeedSlope = Copy(speedSlope, nameof(speedSlope));
+        SpeedIntercept = Copy(speedIntercept, nameof(speedIntercept));
+        EnvelopeSegmentIndex = Copy(envelopeSegmentIndex, nameof(envelopeSegmentIndex));
+        Scale = scale;
+    }
+
+    internal IReadOnlyList<double> MinimumPathS { get; }
+
+    internal IReadOnlyList<double> MaximumPathS { get; }
+
+    internal IReadOnlyList<double> SpeedSlope { get; }
+
+    internal IReadOnlyList<double> SpeedIntercept { get; }
+
+    internal IReadOnlyList<int> EnvelopeSegmentIndex { get; }
+
+    internal double Scale { get; }
+
+    internal bool CanShrinkTo(double nextScale, double minimumActiveWidthMeters, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (!IsFinite(nextScale) || nextScale <= 0d || nextScale >= Scale)
+        {
+            failureReason = "Next trust-region scale must be finite, positive, and smaller than the current scale.";
+            return false;
+        }
+        if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
+        {
+            failureReason = "Minimum active width must be finite and positive.";
+            return false;
+        }
+        double ratio = nextScale / Scale;
+        for (int index = 0; index < MinimumPathS.Count; index++)
+        {
+            double currentWidth = MaximumPathS[index] - MinimumPathS[index];
+            if (currentWidth > 0d && currentWidth * ratio < minimumActiveWidthMeters)
+            {
+                failureReason = "The next trust-region scale would fall below the minimum width at knot " + index + ".";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static IReadOnlyList<double> Copy(IReadOnlyList<double> source, string parameterName)
+    {
+        if (source == null)
+            throw new ArgumentNullException(parameterName);
+        var copy = new List<double>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<int> Copy(IReadOnlyList<int> source, string parameterName)
+    {
+        if (source == null)
+            throw new ArgumentNullException(parameterName);
+        var copy = new List<int>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<int>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class LongitudinalEnvelopeTrustRegionBuilder
+{
+    private const double ScheduleProgressTolerance = 1e-12d;
+    private const double StationSelectionTolerance = 1e-12d;
+
+    internal bool TryBuild(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
+        IReadOnlyList<double> referencePathS, int terminalHoldStartIndex, double scale,
+        double minimumActiveWidthMeters, out LongitudinalEnvelopeTrustRegion region,
+        out string failureReason)
+    {
+        region = null;
+        failureReason = string.Empty;
+        if (speedLimit == null || anchor == null || referencePathS == null)
+        {
+            failureReason = "Trust region inputs must be present.";
+            return false;
+        }
+        if (!IsSupportedScale(scale))
+        {
+            failureReason = "Trust-region scale must be one of 1, 0.5, 0.25, or 0.125.";
+            return false;
+        }
+        if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
+        {
+            failureReason = "Minimum active width must be finite and positive.";
+            return false;
+        }
+        if (referencePathS.Count != anchor.S.Count)
+        {
+            failureReason = "Reference PathS count must match the anchor knot count.";
+            return false;
+        }
+        if (terminalHoldStartIndex == -1)
+            terminalHoldStartIndex = anchor.S.Count;
+        if (terminalHoldStartIndex < 1 || terminalHoldStartIndex > anchor.S.Count)
+        {
+            failureReason = "Terminal-hold start index is outside the anchor knot range.";
+            return false;
+        }
+        if (!TryValidateAnchorAndReference(speedLimit, anchor, referencePathS, out failureReason))
+            return false;
+
+        int knotCount = anchor.S.Count;
+        var minimumPathS = new double[knotCount];
+        var maximumPathS = new double[knotCount];
+        var speedSlope = new double[knotCount];
+        var speedIntercept = new double[knotCount];
+        var segmentIndex = new int[knotCount];
+        for (int index = 0; index < knotCount; index++)
+        {
+            double anchorS = anchor.S[index];
+            bool fixedKnot = index == 0 || index >= terminalHoldStartIndex;
+            int segment = SelectSegment(speedLimit, anchorS, referencePathS, index, fixedKnot);
+            FindMaximalExactAffineRun(speedLimit, segment, out int firstSegment, out int lastSegment,
+                out double slope, out double intercept);
+            double lower = speedLimit.PathS[firstSegment];
+            double upper = speedLimit.PathS[lastSegment + 1];
+            if (anchorS < lower && lower - anchorS <= StationSelectionTolerance)
+                lower = anchorS;
+            if (anchorS > upper && anchorS - upper <= StationSelectionTolerance)
+                upper = anchorS;
+            double trustedLower = fixedKnot ? anchorS : anchorS - scale * (anchorS - lower);
+            double trustedUpper = fixedKnot ? anchorS : anchorS + scale * (upper - anchorS);
+            if (!fixedKnot && scale < 1d && trustedUpper - trustedLower < minimumActiveWidthMeters)
+            {
+                failureReason = "Active trust-region interval is narrower than the minimum width at knot " + index + ".";
+                return false;
+            }
+
+            minimumPathS[index] = trustedLower;
+            maximumPathS[index] = trustedUpper;
+            speedSlope[index] = slope;
+            speedIntercept[index] = intercept;
+            segmentIndex[index] = segment;
+        }
+
+        region = new LongitudinalEnvelopeTrustRegion(minimumPathS, maximumPathS, speedSlope, speedIntercept,
+            segmentIndex, scale);
+        return true;
+    }
+
+    private static bool TryValidateAnchorAndReference(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
+        IReadOnlyList<double> referencePathS, out string failureReason)
+    {
+        if (anchor.S[0] != 0d)
+        {
+            failureReason = "The anchor must begin at exact PathS zero.";
+            return false;
+        }
+
+        double minimumPathS = speedLimit.PathS[0];
+        double maximumPathS = speedLimit.PathS[speedLimit.PathS.Count - 1];
+        double previousAnchorS = double.NegativeInfinity;
+        for (int index = 0; index < anchor.S.Count; index++)
+        {
+            double anchorS = anchor.S[index];
+            double referenceS = referencePathS[index];
+            if (!IsFinite(anchorS) || anchorS < minimumPathS || anchorS > maximumPathS)
+            {
+                failureReason = "Anchor PathS is outside the speed-limit range at knot " + index + ".";
+                return false;
+            }
+            if (anchorS < previousAnchorS)
+            {
+                failureReason = "Anchor PathS must be nondecreasing.";
+                return false;
+            }
+            if (!IsFinite(referenceS))
+            {
+                failureReason = "Reference PathS must be finite.";
+                return false;
+            }
+            previousAnchorS = anchorS;
+        }
+
+        failureReason = string.Empty;
+        return true;
+    }
+
+    private static int SelectSegment(PathSpeedLimit speedLimit, double anchorS, IReadOnlyList<double> referencePathS,
+        int knotIndex, bool terminalHold)
+    {
+        int lastSegment = speedLimit.PathS.Count - 2;
+        if (Math.Abs(anchorS - speedLimit.PathS[0]) <= StationSelectionTolerance)
+            return 0;
+        if (Math.Abs(anchorS - speedLimit.PathS[speedLimit.PathS.Count - 1]) <= StationSelectionTolerance)
+            return lastSegment;
+        for (int index = 1; index < speedLimit.PathS.Count - 1; index++)
+        {
+            if (Math.Abs(anchorS - speedLimit.PathS[index]) <= StationSelectionTolerance)
+            {
+                if (terminalHold)
+                    return index;
+                double scheduleDelta = referencePathS[knotIndex] - referencePathS[knotIndex - 1];
+                if (scheduleDelta > ScheduleProgressTolerance)
+                    return index;
+                if (scheduleDelta < -ScheduleProgressTolerance)
+                    return index - 1;
+                double leftWidth = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
+                double rightWidth = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
+                return rightWidth >= leftWidth ? index : index - 1;
+            }
+            if (anchorS < speedLimit.PathS[index])
+                return index - 1;
+        }
+        return lastSegment;
+    }
+
+    private static void GetAffineLine(PathSpeedLimit limit, int segment,
+        out double slope, out double intercept)
+    {
+        double lower = limit.PathS[segment];
+        double upper = limit.PathS[segment + 1];
+        slope = (limit.MaximumSpeedMetersPerSecond[segment + 1] -
+            limit.MaximumSpeedMetersPerSecond[segment]) / (upper - lower);
+        intercept = limit.MaximumSpeedMetersPerSecond[segment] - slope * lower;
+    }
+
+    private static void FindMaximalExactAffineRun(PathSpeedLimit limit, int selectedSegment,
+        out int firstSegment, out int lastSegment, out double slope, out double intercept)
+    {
+        GetAffineLine(limit, selectedSegment, out slope, out intercept);
+        firstSegment = selectedSegment;
+        while (firstSegment > 0)
+        {
+            GetAffineLine(limit, firstSegment - 1, out double candidateSlope, out double candidateIntercept);
+            if (candidateSlope != slope || candidateIntercept != intercept)
+                break;
+            firstSegment--;
+        }
+        lastSegment = selectedSegment;
+        while (lastSegment < limit.PathS.Count - 2)
+        {
+            GetAffineLine(limit, lastSegment + 1, out double candidateSlope, out double candidateIntercept);
+            if (candidateSlope != slope || candidateIntercept != intercept)
+                break;
+            lastSegment++;
+        }
+    }
+
+    private static bool IsSupportedScale(double scale)
+    {
+        return scale == 1d || scale == 0.5d || scale == 0.25d || scale == 0.125d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalQpFeasibilityAudit.cs

@@ -0,0 +1,217 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalQpAuditResult
+{
+    internal LongitudinalQpAuditResult(bool isFeasible, double maximumResidual, int worstRow,
+        string category, string unit)
+    {
+        IsFeasible = isFeasible;
+        MaximumResidual = maximumResidual;
+        WorstRow = worstRow;
+        Category = category;
+        Unit = unit;
+    }
+
+    internal bool IsFeasible { get; }
+
+    internal double MaximumResidual { get; }
+
+    internal int WorstRow { get; }
+
+    internal string Category { get; }
+
+    internal string Unit { get; }
+}
+
+internal static class LongitudinalQpFeasibilityAudit
+{
+    internal static LongitudinalQpAuditResult Evaluate(QuadraticProgram problem,
+        LongitudinalCandidate candidate, double tolerance, LongitudinalVariableLayout layout,
+        int stabilizationStart)
+    {
+        if (problem == null)
+            throw new ArgumentNullException(nameof(problem));
+        if (candidate == null)
+            throw new ArgumentNullException(nameof(candidate));
+        if (layout == null)
+            throw new ArgumentNullException(nameof(layout));
+        if (problem.VariableCount != layout.VariableCount || candidate.S.Count != layout.KnotCount ||
+            candidate.U.Count != layout.KnotCount || candidate.A.Count != layout.KnotCount ||
+            candidate.J.Count != layout.KnotCount - 1)
+        {
+            throw new ArgumentException("The QP, candidate, and longitudinal layout must have matching dimensions.");
+        }
+
+        double[] primal = ToPrimal(candidate, layout);
+        var activity = new double[problem.ConstraintCount];
+        bool allFinite = IsFinite(tolerance);
+        SparseCscMatrix matrix = problem.ConstraintMatrix;
+        for (int column = 0; column < matrix.ColumnCount; column++)
+        {
+            double value = primal[column];
+            allFinite &= IsFinite(value);
+            for (int entry = matrix.ColumnPointers[column]; entry < matrix.ColumnPointers[column + 1]; entry++)
+                activity[matrix.RowIndices[entry]] += matrix.Values[entry] * value;
+        }
+
+        double maximumResidual = 0d;
+        int worstRow = problem.ConstraintCount == 0 ? -1 : 0;
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            double residual;
+            if (!IsFinite(activity[row]))
+            {
+                allFinite = false;
+                residual = double.PositiveInfinity;
+            }
+            else
+            {
+                residual = Math.Max(0d, Math.Max(
+                    problem.LowerBounds[row] - activity[row],
+                    activity[row] - problem.UpperBounds[row]));
+            }
+            if (row == 0 || residual > maximumResidual)
+            {
+                maximumResidual = residual;
+                worstRow = row;
+            }
+        }
+
+        DescribeRow(worstRow, layout.KnotCount, stabilizationStart, out string category, out string unit);
+        return new LongitudinalQpAuditResult(allFinite && maximumResidual <= tolerance,
+            maximumResidual, worstRow, category, unit);
+    }
+
+    private static double[] ToPrimal(LongitudinalCandidate candidate, LongitudinalVariableLayout layout)
+    {
+        var primal = new double[layout.VariableCount];
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            primal[layout.S(index)] = candidate.S[index];
+            primal[layout.U(index)] = candidate.U[index];
+            primal[layout.A(index)] = candidate.A[index];
+        }
+        for (int index = 0; index < layout.KnotCount - 1; index++)
+            primal[layout.J(index)] = candidate.J[index];
+        return primal;
+    }
+
+    private static void DescribeRow(int targetRow, int knotCount, int stabilizationStart,
+        out string category, out string unit)
+    {
+        int row = 0;
+        for (int index = 0; index < knotCount; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "PathS trust";
+                unit = "m";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "speed";
+                unit = "m/s";
+                return;
+            }
+            if (index > 0 && targetRow == row++)
+            {
+                category = "speed envelope";
+                unit = "m/s";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "acceleration";
+                unit = "m/s^2";
+                return;
+            }
+        }
+
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "jerk";
+                unit = "m/s^3";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "low-speed deceleration release";
+                unit = "m/s";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "monotonic progress";
+                unit = "m";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m/s^2";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m/s";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m";
+                return;
+            }
+        }
+
+        string[] stateUnits = { "m", "m/s", "m/s^2" };
+        for (int state = 0; state < stateUnits.Length; state++)
+        {
+            if (targetRow == row++)
+            {
+                category = "exact start";
+                unit = stateUnits[state];
+                return;
+            }
+        }
+
+        int stopKnotCount = stabilizationStart >= 0 && stabilizationStart < knotCount
+            ? knotCount - stabilizationStart
+            : 0;
+        for (int index = 0; index < stopKnotCount; index++)
+        {
+            for (int state = 0; state < stateUnits.Length; state++)
+            {
+                if (targetRow == row++)
+                {
+                    category = "exact stop";
+                    unit = stateUnits[state];
+                    return;
+                }
+            }
+        }
+
+        category = "row accounting";
+        unit = string.Empty;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Globalization;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -48,6 +49,8 @@ public sealed class LongitudinalSolutionValidator
                 failureReason = "ST candidate violates exact constant-jerk dynamics.";
                 return false;
             }
+            if (!LongitudinalContinuousProfileValidator.TryValidate(candidate, tolerance, out failureReason))
+                return false;
             if (!AreClose(candidate.S[0], 0d, tolerance) ||
                 !AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
                 !AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
@@ -56,6 +59,9 @@ public sealed class LongitudinalSolutionValidator
                 return false;
             }
 
+            var canonicalS = new double[candidate.S.Count];
+            var canonicalU = new double[candidate.U.Count];
+            var canonicalA = new double[candidate.A.Count];
             for (int index = 0; index < candidate.S.Count; index++)
             {
                 double progress = candidate.S[index];
@@ -83,6 +89,11 @@ public sealed class LongitudinalSolutionValidator
                     failureReason = "ST candidate PathS decreases at knot " + index + ".";
                     return false;
                 }
+                canonicalS[index] = progress;
+                canonicalU[index] = speed < 0d ? 0d : speed;
+                canonicalA[index] = speed < 0d && acceleration < 0d && acceleration >= -tolerance
+                    ? 0d
+                    : acceleration;
             }
             for (int index = 0; index < candidate.J.Count; index++)
             {
@@ -122,25 +133,29 @@ public sealed class LongitudinalSolutionValidator
             {
                 for (int index = 0; index < candidate.S.Count; index++)
                 {
-                    if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
-                            maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
-                        candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
+                    bool hasStop = JerkLimitedStoppingMath.TryCalculate(canonicalU[index], canonicalA[index],
+                        maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop,
+                        out string stoppingFailure);
+                    double stopDistance = hasStop ? stop.DistanceMeters : double.NaN;
+                    double margin = hasStop
+                        ? input.StopBoundaryPathS - canonicalS[index] - stopDistance
+                        : double.NaN;
+                    if (!hasStop || canonicalS[index] + stopDistance > input.StopBoundaryPathS + tolerance)
                     {
-                        failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
+                        failureReason = "ST candidate leaves the jerk-limited stoppable set: knot=" +
+                            index.ToString(CultureInfo.InvariantCulture) +
+                            ";S=" + Invariant(canonicalS[index]) +
+                            ";U=" + Invariant(canonicalU[index]) +
+                            ";A=" + Invariant(canonicalA[index]) +
+                            ";stopDistance=" + Invariant(stopDistance) +
+                            ";stopBoundary=" + Invariant(input.StopBoundaryPathS) +
+                            ";margin=" + Invariant(margin) +
+                            (hasStop ? string.Empty : ";stoppingReason=" + stoppingFailure) + ".";
                         return false;
                     }
                 }
             }
 
-            var canonicalS = new double[candidate.S.Count];
-            var canonicalU = new double[candidate.U.Count];
-            var canonicalA = new double[candidate.A.Count];
-            for (int index = 0; index < candidate.S.Count; index++)
-            {
-                canonicalS[index] = candidate.S[index];
-                canonicalU[index] = candidate.U[index];
-                canonicalA[index] = candidate.A[index];
-            }
             canonicalS[0] = 0d;
             canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
             canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
@@ -202,6 +217,11 @@ public sealed class LongitudinalSolutionValidator
         return Math.Abs(actual - expected) <= tolerance;
     }
 
+    private static string Invariant(double value)
+    {
+        return value.ToString("R", CultureInfo.InvariantCulture);
+    }
+
     private static double RequireNonnegative(double value, string parameterName)
     {
         if (!IsFinite(value) || value < 0d)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolveTrace.cs

@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalSolveTrace
+{
+    private const int MaximumEntries = 12;
+    private readonly List<string> _entries = new List<string>(MaximumEntries);
+
+    internal void Add(string phase, int callOrdinal, int anchorUpdateIndex, double trustScale,
+        TimeSpan remainingBudget, TimeSpan remainingAfterReserve, TimeSpan elapsed, QpSolveResult result,
+        double anchorObjective, double candidateObjective, string rejection)
+    {
+        if (_entries.Count >= MaximumEntries)
+            throw new InvalidOperationException("Longitudinal solve trace exceeded the twelve-call cap.");
+        string status = result == null ? "null" : result.Status.ToString();
+        int iterations = result == null ? -1 : result.Iterations;
+        double primal = result == null ? double.NaN : result.PrimalResidual;
+        double dual = result == null ? double.NaN : result.DualResidual;
+        _entries.Add("phase:" + phase +
+            ",call:" + callOrdinal.ToString(CultureInfo.InvariantCulture) +
+            ",anchor:" + anchorUpdateIndex.ToString(CultureInfo.InvariantCulture) +
+            ",scale:" + trustScale.ToString("R", CultureInfo.InvariantCulture) +
+            ",budgetMs:" + remainingBudget.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
+            ",remainingAfterReserveMs:" + remainingAfterReserve.TotalMilliseconds.ToString(
+                "F3", CultureInfo.InvariantCulture) +
+            ",elapsedMs:" + elapsed.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
+            ",status:" + status +
+            ",iterations:" + iterations.ToString(CultureInfo.InvariantCulture) +
+            ",primal:" + primal.ToString("R", CultureInfo.InvariantCulture) +
+            ",dual:" + dual.ToString("R", CultureInfo.InvariantCulture) +
+            ",anchorObj:" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
+            ",candidateObj:" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) +
+            ",rejection:" + Sanitize(rejection));
+    }
+
+    internal string Format() => string.Join("|", _entries);
+
+    private static string Sanitize(string text)
+    {
+        if (string.IsNullOrEmpty(text))
+            return "none";
+        return text.Replace(';', '/').Replace('|', '/').Replace('\r', '/').Replace('\n', '/');
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -10,13 +10,21 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 /// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
 public sealed class SequentialLongitudinalOptimizer
 {
-    private const int MaximumEnvelopeIterations = 5;
-    private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
-    private const double OrdinaryTerminalProbeFraction = 0.5d;
+    private const int MaximumAcceptedAnchorUpdates = 5;
+    private const int MaximumQpSolveCalls = 12;
+    private const double MinimumTrustRegionWidthMeters = 0.001d;
+    private const double ObjectiveAcceptanceRelativeTolerance = 1e-9d;
+    private const double HighPrecisionRetryTolerance = 1e-7d;
+    private const double StaticStartSeedBudgetFraction = 0.10d;
+    private static readonly TimeSpan MaximumStaticStartSeedBudget = TimeSpan.FromMilliseconds(250d);
+    private static readonly TimeSpan PublicationReserve = TimeSpan.FromMilliseconds(250d);
+    private static readonly double[] TrustRegionScales = { 1d, 0.5d, 0.25d, 0.125d };
     private readonly IQpSolver _qpSolver;
     private readonly PathSpeedLimitBuilder _speedLimitBuilder;
     private readonly LongitudinalConstraintBuilder _constraintBuilder;
     private readonly LongitudinalSolutionValidator _solutionValidator;
+    private readonly LongitudinalEnvelopeTrustRegionBuilder _trustRegionBuilder =
+        new LongitudinalEnvelopeTrustRegionBuilder();
 
     public SequentialLongitudinalOptimizer(IQpSolver qpSolver)
         : this(qpSolver, new PathSpeedLimitBuilder(), new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()),
@@ -65,133 +73,418 @@ public sealed class SequentialLongitudinalOptimizer
             return Failed(speedStatus, speedFailure);
 
         var stopwatch = Stopwatch.StartNew();
-        LongitudinalCandidate iterate;
-        LongitudinalCandidate lastStrictCandidate = null;
-        int remainingObjectiveIterations = iterationLimit;
+        var solveTrace = new LongitudinalSolveTrace();
+        LongitudinalCandidate initialCandidate;
+        int projectionSolveCount = 0;
         if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
             input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
         {
             if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
-                    iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount,
-                    out EmPlanningStatus projectionStatus, out string projectionFailure))
+                    iterationLimit, stopwatch, solveTrace, cancellationToken, out initialCandidate,
+                    out int usedProjectionSolveCount, out EmPlanningStatus projectionStatus,
+                    out string projectionFailure))
             {
-                return Failed(projectionStatus, projectionFailure);
-            }
-            lastStrictCandidate = CopyCandidate(iterate);
-            remainingObjectiveIterations -= projectionSolveCount;
-            if (remainingObjectiveIterations <= 0)
-            {
-                return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate,
-                    "The strict initial feasibility projection consumed the configured outer-iteration budget.");
+                string projectionTrace = solveTrace.Format();
+                return Failed(projectionStatus, projectionFailure +
+                    (string.IsNullOrEmpty(projectionTrace) ? string.Empty : ";solveTrace=" + projectionTrace));
             }
+            projectionSolveCount = usedProjectionSolveCount;
         }
         else
         {
-            iterate = CreateInitialIterate(input, speedLimit);
-            if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
-                lastStrictCandidate = null;
+            LongitudinalCandidate seed = CreateInitialIterate(input, speedLimit);
+            if (!_solutionValidator.TryValidate(input, speedLimit, seed, out initialCandidate,
+                    out EmPlanningStatus initializationStatus, out string initializationFailure))
+            {
+                return Failed(initializationStatus, "No strictly validated longitudinal candidate was found. " +
+                    initializationFailure);
+            }
         }
-        double[] warmStart = ToPrimal(iterate);
-        bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
-        string lastCandidateRejection = string.Empty;
-        bool hasPreviousObjective = false;
-        double previousObjective = 0d;
 
-        for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++)
+        LongitudinalCandidate anchor = CopyCandidate(initialCandidate);
+        int stabilizationStart = GetStabilizationStart(input);
+        int qpSolveCount = projectionSolveCount;
+        int trustShrinkCount = 0;
+        int acceptedAnchorCount = 0;
+        int acceptedUpdateLimit = Math.Min(MaximumAcceptedAnchorUpdates, iterationLimit);
+        double finalTrustScale = 1d;
+        string lastRejection = string.Empty;
+
+        while (acceptedAnchorCount < acceptedUpdateLimit && qpSolveCount < MaximumQpSolveCalls)
         {
+            bool promoted = false;
+            for (int scaleIndex = 0; scaleIndex < TrustRegionScales.Length; scaleIndex++)
+            {
+                double scale = TrustRegionScales[scaleIndex];
+                finalTrustScale = scale;
+                if (cancellationToken.IsCancellationRequested || totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
+                {
+                    return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+                        finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
+                }
+                if (!_trustRegionBuilder.TryBuild(speedLimit, anchor, input.KnotSchedule.ReferencePathS,
+                        stabilizationStart, scale, MinimumTrustRegionWidthMeters,
+                        out LongitudinalEnvelopeTrustRegion region, out string regionFailure))
+                {
+                    lastRejection = regionFailure;
+                    break;
+                }
+                if (!_constraintBuilder.TryBuildTrusted(input, speedLimit, anchor, region,
+                        convergenceTolerance, out QuadraticProgram problem, out string buildFailure))
+                {
+                    lastRejection = buildFailure;
+                    break;
+                }
+                TrustedSolveAttempt attempt = SolveTrustedProblem(problem, input, speedLimit, anchor, settings,
+                    totalBudget, stopwatch, convergenceTolerance, stabilizationStart,
+                    MaximumQpSolveCalls - qpSolveCount, solveTrace, qpSolveCount, acceptedAnchorCount,
+                    scale, acceptedAnchorCount > 0, cancellationToken);
+                qpSolveCount += attempt.SolveCount;
+                lastRejection = attempt.FailureReason;
+                if (attempt.Status == EmPlanningStatus.Cancelled)
+                {
+                    return Failed(EmPlanningStatus.Cancelled, CreateRunDiagnostic(qpSolveCount,
+                        trustShrinkCount, acceptedAnchorCount, finalTrustScale, lastRejection, solveTrace));
+                }
+                if (attempt.Accepted)
+                {
+                    anchor = CopyCandidate(attempt.Candidate);
+                    acceptedAnchorCount++;
+                    promoted = true;
+                    break;
+                }
+                if (attempt.Status != EmPlanningStatus.SuccessWithFallback)
+                {
+                    return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+                        finalTrustScale, false, lastRejection, solveTrace);
+                }
+                if (scaleIndex + 1 < TrustRegionScales.Length)
+                {
+                    double nextScale = TrustRegionScales[scaleIndex + 1];
+                    if (!region.CanShrinkTo(nextScale, MinimumTrustRegionWidthMeters, out string shrinkFailure))
+                    {
+                        lastRejection = shrinkFailure;
+                        break;
+                    }
+                    trustShrinkCount++;
+                }
+            }
+            if (!promoted)
+                break;
+        }
+
+        return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+            finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
+    }
+
+    private TrustedSolveAttempt SolveTrustedProblem(QuadraticProgram problem,
+        LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
+        QpSolverSettings settings, TimeSpan totalBudget, Stopwatch stopwatch, double strictTolerance,
+        int stabilizationStart, int remainingCallCount, LongitudinalSolveTrace solveTrace,
+        int solveOrdinalOffset, int anchorUpdateIndex, double trustScale,
+        bool optionalImprovement, CancellationToken cancellationToken)
+    {
+        int solveCount = 0;
+        double anchorObjective = EvaluateObjective(problem, ToPrimal(anchor));
+        if (!IsFinite(anchorObjective))
+        {
+            return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
+                "The strict anchor objective is non-finite for the trusted QP.");
+        }
+
+        IReadOnlyList<double> warmStart = ToPrimal(anchor);
+        string lastFailure = string.Empty;
+        for (int attemptIndex = 0; attemptIndex < 2; attemptIndex++)
+        {
+            if (solveCount >= remainingCallCount)
+            {
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    string.IsNullOrWhiteSpace(lastFailure) ? "The longitudinal QP solve-call cap was reached." : lastFailure);
+            }
             if (cancellationToken.IsCancellationRequested)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
+            {
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    "Longitudinal optimization was cancelled before the trusted QP solve.");
+            }
             TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
             if (remainingBudget <= TimeSpan.Zero)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
-                    "Longitudinal optimization exhausted its solve budget.");
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    "Longitudinal optimization exhausted its shared solve budget.");
             }
-            if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
+            bool hasOptionalSolveBudget = TryGetOptionalSolveBudget(remainingBudget,
+                out TimeSpan remainingAfterReserve);
+            if (optionalImprovement && !hasOptionalSolveBudget)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
-                    "Longitudinal constraints are infeasible: " + buildFailure);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    CreatePublicationReserveSkipDiagnostic(remainingAfterReserve));
+            }
+            TimeSpan solveBudget = optionalImprovement ? remainingAfterReserve : remainingBudget;
+
+            bool highPrecision = attemptIndex == 1;
+            double absoluteTolerance = highPrecision
+                ? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
+                : settings.AbsoluteTolerance;
+            double relativeTolerance = highPrecision
+                ? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
+                : settings.RelativeTolerance;
+            var solveStopwatch = Stopwatch.StartNew();
+            QpSolveResult solved = _qpSolver.Solve(problem,
+                new QpSolverSettings(settings.MaximumIterations, absoluteTolerance, relativeTolerance,
+                    solveBudget, settings.EnableWarmStart, settings.EnablePolishing,
+                    settings.EnableNativeVerboseOutput), warmStart, cancellationToken);
+            solveStopwatch.Stop();
+            solveCount++;
+            double candidateObjective = double.NaN;
+            void AddSolveTrace(string rejection)
+            {
+                solveTrace.Add(attemptIndex == 0 ? "normal" : "retry",
+                    solveOrdinalOffset + solveCount, anchorUpdateIndex, trustScale,
+                    solveBudget, remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
+                    candidateObjective, rejection);
             }
 
-            QpSolveResult solved = _qpSolver.Solve(problem,
-                new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
-                    remainingBudget, settings.EnableWarmStart && (iteration > 0 || hasDynamicsConsistentInitialWarmStart),
-                    settings.EnablePolishing,
-                    settings.EnableNativeVerboseOutput),
-                warmStart, cancellationToken);
             if (cancellationToken.IsCancellationRequested)
-                return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled after the QP solve.");
-            if (solved == null)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
-            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
-                    "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
-                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
-                    ", dual=" + solved.DualResidual + "): " + solved.Diagnostic);
+                const string rejection = "Longitudinal optimization was cancelled after the trusted QP solve.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (solved == null)
+            {
+                const string rejection = "The longitudinal QP solver returned no result.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
+                    rejection);
             }
             if (solved.Status == QpSolveStatus.Cancelled)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
-                    "The longitudinal QP solver was cancelled: " + solved.Diagnostic);
+            {
+                string rejection = "The longitudinal QP solver was cancelled: " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
+            {
+                string rejection = "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
+                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
+                    ", dual=" + solved.DualResidual + "): " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    rejection);
+            }
             if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
-                    "The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
+                string rejection = "The preflight-feasible longitudinal QP solver reported infeasibility;" +
+                    "solverNumericalAnomaly=true;status=" + solved.NativeStatus + ": " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
+                    rejection);
             }
             if (solved.Status == QpSolveStatus.SolverUnavailable)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
-                    "The longitudinal QP solver is unavailable: " + solved.Diagnostic);
+            {
+                string rejection = "The longitudinal QP solver is unavailable: " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverUnavailable, null, solveCount, false,
+                    rejection);
+            }
             if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
-                    "The longitudinal QP solver failed: " + solved.Diagnostic);
+                string rejection = "The longitudinal QP solver failed (status=" + solved.NativeStatus + "): " +
+                    solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
+                    rejection);
             }
-            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
+
+            if (!TryCreateCandidate(anchor.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
             {
-                lastCandidateRejection = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
+                const string rejection =
+                    "The solver primal does not match the ST variable layout or contains non-finite values.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+                    rejection);
+            }
+
+            bool accepted = true;
+            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, strictTolerance))
+            {
+                accepted = false;
+                lastFailure = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
                     " (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
-                if (TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate inaccurateCandidate))
-                    warmStart = ToPrimal(inaccurateCandidate);
-                continue;
             }
-            if (!TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
+            if (accepted && !TryFastValidateCandidate(problem, candidate, strictTolerance,
+                    stabilizationStart, out lastFailure))
             {
-                lastCandidateRejection = "The solver primal does not match the ST variable layout.";
-                continue;
+                accepted = false;
             }
-            if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate validated,
+            LongitudinalCandidate validated = null;
+            if (accepted && !_solutionValidator.TryValidate(input, speedLimit, candidate, out validated,
                     out string validationFailure))
             {
-                string rejection = validationFailure + CreateEnvelopeDiagnostic(speedLimit, iterate, candidate,
-                    iteration + 1);
-                lastCandidateRejection = string.IsNullOrEmpty(lastCandidateRejection)
-                    ? rejection
-                    : lastCandidateRejection + " | " + rejection;
-                if (TryCreateEnvelopeIterate(input, iterate, candidate, out LongitudinalCandidate nextIterate))
+                accepted = false;
+                lastFailure = validationFailure;
+            }
+            if (accepted)
+            {
+                candidateObjective = EvaluateObjective(problem, ToPrimal(validated));
+                if (!IsObjectiveAccepted(anchorObjective, candidateObjective))
                 {
-                    iterate = nextIterate;
-                    warmStart = ToPrimal(candidate);
+                    accepted = false;
+                    lastFailure = "The strictly valid candidate worsens the current trusted-QP objective" +
+                        " (anchor=" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
+                        ", candidate=" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) + ").";
                 }
-                continue;
+            }
+            if (accepted && cancellationToken.IsCancellationRequested)
+            {
+                const string rejection = "Longitudinal optimization was cancelled before strict-anchor promotion.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (accepted && totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
+            {
+                const string rejection =
+                    "Longitudinal optimization exhausted its shared solve budget before strict-anchor promotion.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    rejection);
+            }
+            if (accepted)
+            {
+                AddSolveTrace(string.Empty);
+                return new TrustedSolveAttempt(EmPlanningStatus.Success, validated, solveCount, true, string.Empty);
+            }
+            if (highPrecision)
+            {
+                AddSolveTrace(lastFailure);
+                return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+                    lastFailure);
             }
 
-            double maximumChange = MaximumProgressOrSpeedChange(iterate, validated);
-            double relativeObjectiveImprovement = hasPreviousObjective
-                ? RelativeObjectiveImprovement(previousObjective, solved.Objective)
-                : double.PositiveInfinity;
-            lastStrictCandidate = CopyCandidate(validated);
-            iterate = validated;
-            warmStart = ToPrimal(validated);
-            previousObjective = solved.Objective;
-            hasPreviousObjective = true;
-            if (maximumChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
-                return new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+            AddSolveTrace(lastFailure);
+            warmStart = ToPrimal(candidate);
         }
 
-        return lastStrictCandidate == null
-            ? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
-                lastCandidateRejection)
-            : new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+        return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+            lastFailure);
+    }
+
+    private static double EvaluateObjective(QuadraticProgram problem, IReadOnlyList<double> primal)
+    {
+        if (problem == null || primal == null || primal.Count != problem.VariableCount)
+            return double.NaN;
+        double objective = 0d;
+        SparseCscMatrix hessian = problem.UpperTriangularP;
+        for (int column = 0; column < hessian.ColumnCount; column++)
+        {
+            double columnValue = primal[column];
+            if (!IsFinite(columnValue))
+                return double.NaN;
+            for (int entry = hessian.ColumnPointers[column]; entry < hessian.ColumnPointers[column + 1]; entry++)
+            {
+                int row = hessian.RowIndices[entry];
+                double term = hessian.Values[entry] * primal[row] * columnValue;
+                objective += row == column ? 0.5d * term : term;
+                if (!IsFinite(objective))
+                    return double.NaN;
+            }
+            objective += problem.LinearCost[column] * columnValue;
+            if (!IsFinite(objective))
+                return double.NaN;
+        }
+        return objective;
+    }
+
+    private static bool IsObjectiveAccepted(double anchorObjective, double candidateObjective)
+    {
+        if (!IsFinite(anchorObjective) || !IsFinite(candidateObjective))
+            return false;
+        double tolerance = ObjectiveAcceptanceRelativeTolerance * Math.Max(1d, Math.Abs(anchorObjective));
+        return candidateObjective <= anchorObjective + tolerance;
+    }
+
+    private static int GetStabilizationStart(LongitudinalPlanningInput input)
+    {
+        if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
+            return input.KnotSchedule.KnotTimes.Count;
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            return input.KnotSchedule.TerminalHoldStartIndex;
+        return LongitudinalTerminalSchedule.GetStabilizationStartIndex(input.KnotSchedule.KnotTimes,
+            input.Configuration.Scheduling.OutputTimeStepSeconds);
+    }
+
+    private static bool TryFastValidateCandidate(QuadraticProgram problem, LongitudinalCandidate candidate,
+        double strictTolerance, int stabilizationStart, out string failureReason)
+    {
+        failureReason = string.Empty;
+        var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+        LongitudinalQpAuditResult audit;
+        try
+        {
+            audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, candidate, strictTolerance,
+                layout, stabilizationStart);
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = "The candidate cannot be audited against the current trusted QP: " + exception.Message;
+            return false;
+        }
+        if (audit.IsFeasible)
+            return true;
+        failureReason = "The candidate violates the current trusted QP" +
+            ";row=" + audit.WorstRow + ";category=" + audit.Category +
+            ";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) + audit.Unit +
+            ";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
+        return false;
+    }
+
+    private static string CreateRunDiagnostic(int qpSolveCount, int trustShrinkCount,
+        int acceptedAnchorCount, double finalTrustScale, string lastRejection,
+        LongitudinalSolveTrace solveTrace)
+    {
+        return "qpSolves=" + qpSolveCount +
+            ",trustShrinks=" + trustShrinkCount +
+            ",acceptedAnchors=" + acceptedAnchorCount +
+            ",trustScale=" + finalTrustScale.ToString("R", CultureInfo.InvariantCulture) +
+            (string.IsNullOrWhiteSpace(lastRejection) ? string.Empty : ";lastRejection=" + lastRejection) +
+            (string.IsNullOrEmpty(solveTrace.Format()) ? string.Empty : ";solveTrace=" + solveTrace.Format());
+    }
+
+    internal static bool TryGetOptionalSolveBudget(TimeSpan remaining, out TimeSpan solveBudget)
+    {
+        solveBudget = remaining - PublicationReserve;
+        if (solveBudget <= TimeSpan.Zero)
+        {
+            solveBudget = TimeSpan.Zero;
+            return false;
+        }
+        return true;
+    }
+
+    private static string CreatePublicationReserveSkipDiagnostic(TimeSpan remainingAfterReserve)
+    {
+        return "remainingAfterReserveMs=" + remainingAfterReserve.TotalMilliseconds.ToString(
+                "F3", CultureInfo.InvariantCulture) +
+            ";publicationReserveMs=" + PublicationReserve.TotalMilliseconds.ToString(
+                "F0", CultureInfo.InvariantCulture) +
+            ";optionalImprovement=skipped";
+    }
+
+    private static LongitudinalPlanningResult FinishFromAnchor(LongitudinalCandidate anchor,
+        int acceptedAnchorCount, int qpSolveCount, int trustShrinkCount, double finalTrustScale,
+        bool cancelled, string lastRejection, LongitudinalSolveTrace solveTrace)
+    {
+        string diagnostic = CreateRunDiagnostic(qpSolveCount, trustShrinkCount, acceptedAnchorCount,
+            finalTrustScale, lastRejection, solveTrace);
+        if (cancelled)
+            return Failed(EmPlanningStatus.Cancelled, diagnostic);
+        EmPlanningStatus status = acceptedAnchorCount > 0
+            ? EmPlanningStatus.Success
+            : EmPlanningStatus.SuccessWithFallback;
+        return new LongitudinalPlanningResult(status, CopyCandidate(anchor), diagnostic);
     }
 
     private static bool TryCreateSettings(LongitudinalPlanningInput input, out QpSolverSettings settings,
@@ -222,7 +515,7 @@ public sealed class SequentialLongitudinalOptimizer
             settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
                 totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
             convergenceTolerance = solver.StrictResidualTolerance;
-            iterationLimit = Math.Min(MaximumEnvelopeIterations, solver.MaximumOuterIterations);
+            iterationLimit = Math.Min(MaximumAcceptedAnchorUpdates, solver.MaximumOuterIterations);
             return true;
         }
         catch (ArgumentException exception)
@@ -234,8 +527,9 @@ public sealed class SequentialLongitudinalOptimizer
 
     private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
         QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
-        Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate,
-        out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason)
+        Stopwatch stopwatch, LongitudinalSolveTrace solveTrace, CancellationToken cancellationToken,
+        out LongitudinalCandidate candidate, out int projectionSolveCount,
+        out EmPlanningStatus failureStatus, out string failureReason)
     {
         candidate = null;
         projectionSolveCount = 0;
@@ -248,8 +542,9 @@ public sealed class SequentialLongitudinalOptimizer
                 input.Configuration.Validation.KinematicTolerance;
         bool staticStartSeedUsed = false;
         string staticStartSeedFailure = string.Empty;
-        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed,
-                out staticStartSeedFailure))
+        TimeSpan staticStartSeedDeadline = stopwatch.Elapsed + GetStaticStartSeedBudget(totalBudget);
+        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, stopwatch, staticStartSeedDeadline,
+                out LongitudinalCandidate staticStartSeed, out staticStartSeedFailure))
         {
             staticStartSeedUsed = true;
             candidate = staticStartSeed;
@@ -289,60 +584,93 @@ public sealed class SequentialLongitudinalOptimizer
 
             double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
                 input.Configuration.Validation.KinematicTolerance * 0.1d);
+            double anchorObjective = double.NaN;
+            var solveStopwatch = Stopwatch.StartNew();
             QpSolveResult solved = _qpSolver.Solve(problem,
                 new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance,
                     remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d),
                     settings.EnablePolishing, settings.EnableNativeVerboseOutput),
                 ToPrimal(linearizationIterate), cancellationToken);
+            solveStopwatch.Stop();
             projectionSolveCount++;
+            int projectionCallOrdinal = projectionSolveCount;
+            double candidateObjective = double.NaN;
+            void AddProjectionTrace(string rejection)
+            {
+                TryGetOptionalSolveBudget(remainingBudget, out TimeSpan remainingAfterReserve);
+                solveTrace.Add("projection", projectionCallOrdinal, 0, 1d, remainingBudget,
+                    remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
+                    candidateObjective, rejection);
+            }
             if (cancellationToken.IsCancellationRequested)
             {
+                const string rejection =
+                    "Initial full-direction feasibility projection was cancelled after the QP solve.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled after the QP solve.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved == null)
             {
+                const string rejection = "The initial full-direction feasibility solver returned no result.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Failed;
-                failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
             {
-                failureStatus = EmPlanningStatus.SolverTimedOut;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
+                string rejection = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
                     ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
-                    solved.DualResidual + "): " + solved.Diagnostic);
+                    solved.DualResidual + "): " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
+                failureStatus = EmPlanningStatus.SolverTimedOut;
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.Cancelled)
             {
+                string rejection =
+                    "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
             {
+                string rejection =
+                    "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.SolverUnavailable)
             {
+                string rejection =
+                    "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.SolverUnavailable;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
             {
+                string rejection = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Failed;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
             {
+                const string rejection =
+                    "Initial full-direction feasibility solver primal does not match the ST layout.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
@@ -350,11 +678,13 @@ public sealed class SequentialLongitudinalOptimizer
                 if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
                         out EmPlanningStatus validationStatus, out string validationFailure))
                 {
+                    AddProjectionTrace(string.Empty);
                     candidate = strict;
                     return true;
                 }
                 if (validationStatus == EmPlanningStatus.NoProgress)
                 {
+                    AddProjectionTrace(validationFailure);
                     failureStatus = validationStatus;
                     failureReason = WithStaticSeedDiagnostic(validationFailure);
                     return false;
@@ -365,8 +695,11 @@ public sealed class SequentialLongitudinalOptimizer
             if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
                     out LongitudinalCandidate nextLinearization))
             {
+                const string rejection =
+                    "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             linearizationIterate = nextLinearization;
@@ -374,6 +707,7 @@ public sealed class SequentialLongitudinalOptimizer
                 lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance.";
             else if (string.IsNullOrEmpty(lastRejection))
                 lastRejection = "Initial feasibility projection violated the strict physical validator.";
+            AddProjectionTrace(lastRejection);
         }
         failureStatus = EmPlanningStatus.LongitudinalInfeasible;
         failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
@@ -503,7 +837,7 @@ public sealed class SequentialLongitudinalOptimizer
     }
 
     private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate, out string failureReason)
+        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate, out string failureReason)
     {
         candidate = null;
         failureReason = "unknown";
@@ -515,9 +849,11 @@ public sealed class SequentialLongitudinalOptimizer
         }
 
         IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
-        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
+        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
             return true;
-        failureReason = "exactJerkSeed=failed";
+        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
+            return true;
+        failureReason = "scurveSeed=failed; exactJerkSeed=failed";
         double firstDuration = times[1] - times[0];
         double secondDuration = times[2] - times[1];
         LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
@@ -526,6 +862,8 @@ public sealed class SequentialLongitudinalOptimizer
                 configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
         for (int sample = -256; sample <= 256; sample++)
         {
+            if (HasReachedDeadline(stopwatch, deadline))
+                return false;
             if (sample == 0)
                 continue;
             double firstJerk = maximumFirstJerk * sample / 256d;
@@ -545,18 +883,13 @@ public sealed class SequentialLongitudinalOptimizer
                 return true;
             }
         }
-        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, out candidate))
-        {
-            failureReason = string.Empty;
-            return true;
-        }
-        failureReason = "exactJerkSeed=failed; sampledSeeds=failed; scurveSeeds=failed";
+        failureReason = "scurveSeed=failed; exactJerkSeed=failed; sampledSeeds=failed";
         return false;
     }
 
     private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
         IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate)
+        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate)
     {
         candidate = null;
         int intervalCount = stabilizationStart;
@@ -569,6 +902,8 @@ public sealed class SequentialLongitudinalOptimizer
         {
             for (int plateau = 0; 4 * ramp + 2 * plateau <= intervalCount; plateau++)
             {
+                if (HasReachedDeadline(stopwatch, deadline))
+                    return false;
                 int cruise = intervalCount - 4 * ramp - 2 * plateau;
                 var basisAccel = new double[intervalCount];
                 var basisBrake = new double[intervalCount];
@@ -688,6 +1023,14 @@ public sealed class SequentialLongitudinalOptimizer
 
     private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
         int stabilizationStart, PathSpeedLimit speedLimit, out LongitudinalCandidate candidate)
+    {
+        return TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, Stopwatch.StartNew(),
+            TimeSpan.MaxValue, out candidate);
+    }
+
+    private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int stabilizationStart, PathSpeedLimit speedLimit, Stopwatch stopwatch, TimeSpan deadline,
+        out LongitudinalCandidate candidate)
     {
         candidate = null;
         int intervalCount = stabilizationStart;
@@ -747,6 +1090,8 @@ public sealed class SequentialLongitudinalOptimizer
         {
             for (int basisIndex = 0; basisIndex < intervalCount; basisIndex++)
             {
+                if (HasReachedDeadline(stopwatch, deadline))
+                    return false;
                 double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
                 if (direction == null)
                     continue;
@@ -755,6 +1100,8 @@ public sealed class SequentialLongitudinalOptimizer
                 double bestViolation = currentViolation;
                 for (int sample = -256; sample <= 256; sample++)
                 {
+                    if (HasReachedDeadline(stopwatch, deadline))
+                        return false;
                     double scale = maximumJerk * sample / 256d;
                     var probeJerk = new double[intervalCount];
                     for (int interval = 0; interval < intervalCount; interval++)
@@ -776,6 +1123,18 @@ public sealed class SequentialLongitudinalOptimizer
         return false;
     }
 
+    private static TimeSpan GetStaticStartSeedBudget(TimeSpan totalBudget)
+    {
+        double milliseconds = Math.Min(MaximumStaticStartSeedBudget.TotalMilliseconds,
+            Math.Max(1d, totalBudget.TotalMilliseconds * StaticStartSeedBudgetFraction));
+        return TimeSpan.FromMilliseconds(milliseconds);
+    }
+
+    private static bool HasReachedDeadline(Stopwatch stopwatch, TimeSpan deadline)
+    {
+        return stopwatch.Elapsed >= deadline;
+    }
+
     private bool TryValidateExactSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
         int stabilizationStart, PathSpeedLimit speedLimit, IReadOnlyList<double> jerk,
         out LongitudinalCandidate candidate)
@@ -1065,72 +1424,6 @@ public sealed class SequentialLongitudinalOptimizer
         return primal;
     }
 
-    private static bool TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
-        LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
-    {
-        nextIterate = null;
-        if (candidate.S.Count != previous.S.Count)
-            return false;
-        int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
-            ? candidate.S.Count
-            : input.PlanningScope == EmPlanningScope.FullDirectionSegment
-                ? input.KnotSchedule.TerminalHoldStartIndex
-                : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
-                    input.Configuration.Scheduling.OutputTimeStepSeconds);
-        var candidateProgressSamples = new double[candidate.S.Count];
-        double priorProgress = double.NegativeInfinity;
-        double priorPreviousProgress = double.NegativeInfinity;
-        for (int index = 0; index < candidate.S.Count; index++)
-        {
-            double candidateProgress = candidate.S[index];
-            double previousProgress = previous.S[index];
-            if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.PathUpperBoundS ||
-                previousProgress < priorPreviousProgress)
-            {
-                return false;
-            }
-            if (!IsFinite(candidateProgress))
-            {
-                candidateProgress = previousProgress;
-            }
-            candidateProgress = Math.Max(0d, Math.Min(input.PathUpperBoundS, candidateProgress));
-            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary && index >= stabilizationStart)
-                candidateProgress = input.StopBoundaryPathS;
-            candidateProgress = Math.Max(priorProgress, candidateProgress);
-            candidateProgressSamples[index] = candidateProgress;
-            priorProgress = candidateProgress;
-            priorPreviousProgress = previousProgress;
-        }
-        var progress = new double[candidate.S.Count];
-        double previousNextProgress = 0d;
-        for (int index = 0; index < progress.Length; index++)
-        {
-            double candidateProgress = candidateProgressSamples[index];
-            if (index == 0 || index == progress.Length - 1 || (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
-                index >= stabilizationStart) || candidateProgress >= input.PathUpperBoundS)
-            {
-                progress[index] = candidateProgress;
-            }
-            else
-            {
-                double timeStep = candidate.KnotTimes[index + 1] - candidate.KnotTimes[index];
-                double iterationAdvance = Math.Max(0d, candidateProgress - previous.S[index]);
-                double candidateSpeed = IsFinite(candidate.U[index]) ? Math.Max(0d, candidate.U[index]) : 0d;
-                double lookaheadAdvance = IsFinite(candidate.U[index])
-                    ? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
-                    : 0d;
-                double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
-                    (input.PathUpperBoundS - candidateProgress);
-                double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
-                progress[index] = candidateProgress + advance;
-            }
-            progress[index] = Math.Max(previousNextProgress, progress[index]);
-            previousNextProgress = progress[index];
-        }
-        nextIterate = new LongitudinalCandidate(candidate.KnotTimes, progress, previous.U, previous.A, previous.J);
-        return true;
-    }
-
     private static bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
         LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
     {
@@ -1161,52 +1454,27 @@ public sealed class SequentialLongitudinalOptimizer
             result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
     }
 
-    private static string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
-        LongitudinalCandidate candidate, int iteration)
+    private readonly struct TrustedSolveAttempt
     {
-        int worstIndex = -1;
-        double worstExcess = double.NegativeInfinity;
-        for (int index = 0; index < candidate.S.Count; index++)
+        internal TrustedSolveAttempt(EmPlanningStatus status, LongitudinalCandidate candidate,
+            int solveCount, bool accepted, string failureReason)
         {
-            if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
-                continue;
-            double candidateProgress = Math.Max(0d, Math.Min(speedLimit.PathUpperBoundS, candidate.S[index]));
-            double limit = speedLimit.MaximumSpeedAt(candidateProgress);
-            double excess = candidate.U[index] - limit;
-            if (excess > worstExcess)
-            {
-                worstExcess = excess;
-                worstIndex = index;
-            }
+            Status = status;
+            Candidate = candidate;
+            SolveCount = solveCount;
+            Accepted = accepted;
+            FailureReason = failureReason ?? string.Empty;
         }
-        if (worstIndex < 0)
-            return "";
-        return " Envelope iteration " + iteration + " used PathS=" + iterate.S[worstIndex] +
-            " and produced PathS=" + candidate.S[worstIndex] + " at its largest speed-envelope excess.";
-    }
 
-    private static double MaximumProgressOrSpeedChange(LongitudinalCandidate previous, LongitudinalCandidate current)
-    {
-        double maximum = 0d;
-        for (int index = 0; index < previous.S.Count; index++)
-        {
-            maximum = Math.Max(maximum, Math.Abs(current.S[index] - previous.S[index]));
-            maximum = Math.Max(maximum, Math.Abs(current.U[index] - previous.U[index]));
-        }
-        return maximum;
-    }
+        internal EmPlanningStatus Status { get; }
 
-    private static double RelativeObjectiveImprovement(double previous, double current)
-    {
-        return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
-    }
+        internal LongitudinalCandidate Candidate { get; }
 
-    private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
-        EmPlanningStatus failureStatus, string failureReason)
-    {
-        return failureStatus == EmPlanningStatus.Cancelled || candidate == null
-            ? Failed(failureStatus, failureReason)
-            : new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
+        internal int SolveCount { get; }
+
+        internal bool Accepted { get; }
+
+        internal string FailureReason { get; }
     }
 
     private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/IQpSolver.cs

@@ -3,6 +3,9 @@ using System.Threading;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 求解器中立的二次规划边界。实现必须将数值状态转换为 <see cref="QpSolveResult"/>，而不能把原生求解器对象泄漏给规划层。
+/// </summary>
 public interface IQpSolver
 {
     QpSolveResult Solve(

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveResult.cs

@@ -5,6 +5,9 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// QP 求解后的数值和诊断快照。调用方必须结合状态判断解向量是否可用，不能将失败输出当作候选轨迹。
+/// </summary>
 public sealed class QpSolveResult
 {
     public QpSolveResult(

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QuadraticProgram.cs

@@ -5,6 +5,9 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 供横向或纵向规划器提交给 <see cref="IQpSolver"/> 的二次规划快照；矩阵维度与变量布局必须一致。
+/// </summary>
 public sealed class QuadraticProgram
 {
     public const double MaximumFiniteBound = 1e30d;

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/DirectionSegmentView.cs

@@ -6,8 +6,16 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 平滑参考路径中单一前进或倒车方向段的只读视图；EM 规划和投影不得跨越该段的精确换向边界。
+/// </summary>
 public sealed class DirectionSegmentView
 {
+    /// <summary>
+    /// 创建一个已重基到局部 S=0 的方向段只读视图。
+    /// 参数：points 的 ArcLength 为 m 且方向必须一致，边界与 sourceStartArcLength 为 m；首点与起边界必须位于局部 S=0，终边界须在 1e-12 m 内匹配末点。
+    /// 返回：防御性复制的不可变段；索引、方向、边界或弧长不一致时引发异常。
+    /// </summary>
     public DirectionSegmentView(
         int segmentIndex,
         TravelDirection direction,
@@ -48,17 +56,38 @@ public sealed class DirectionSegmentView
         SourceStartArcLength = sourceStartArcLength;
     }
 
+    /// <summary>
+    /// 在原始平滑路径方向段序列中的零基索引。
+    /// </summary>
     public int SegmentIndex { get; }
 
+    /// <summary>
+    /// 全段唯一的行驶方向；投影和优化不得在本视图内切换方向。
+    /// </summary>
     public TravelDirection Direction { get; }
 
+    /// <summary>
+    /// 局部弧长已重基、按 S（m）非递减排列的平滑参考点只读副本。
+    /// </summary>
     public IReadOnlyList<SmoothedPathPoint> Points { get; }
 
+    /// <summary>
+    /// 局部 S=0 的起始边界，含换向离开语义（若有）。
+    /// </summary>
     public ReferenceBoundary StartBoundary { get; }
 
+    /// <summary>
+    /// 局部段末端的边界，含目标或换向接近语义（若有）。
+    /// </summary>
     public ReferenceBoundary EndBoundary { get; }
 
+    /// <summary>
+    /// 本段局部 S=0 在原始平滑路径中的累计弧长，单位 m。
+    /// </summary>
     public double SourceStartArcLength { get; }
 
+    /// <summary>
+    /// 本段从局部 S=0 到终边界的长度，单位 m。
+    /// </summary>
     public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
 }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

@@ -4,9 +4,16 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
+/// <summary>
+/// 在纵横向优化前选定的参考距离终端，保证不会跨越当前方向段。
+/// 所有参考站量以当前段局部 S（m）表示；边界类型和纵向模式明确滚动延续、接近停车或精确停车语义。
+/// </summary>
 public sealed class PlanningHorizonSelection
 {
+    /// <summary>
+    /// 创建仅供本程序集发布的窗口与停车边界选择。
+    /// 参数：两个参考站均为局部 S（m），boundary/terminal/mode 为已判定枚举；hasStopBoundary 表示终端是否受真实停车边界约束。
+    /// </summary>
     internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
         EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
         double stopBoundaryReferenceS, bool hasStopBoundary)
@@ -19,25 +26,58 @@ public sealed class PlanningHorizonSelection
         HasStopBoundary = hasStopBoundary;
     }
 
+    /// <summary>
+    /// 本次优化窗口末端的局部参考弧长 S，单位 m。
+    /// </summary>
     public double WindowEndReferenceS { get; }
 
+    /// <summary>
+    /// 规划输出的终端参考弧长 S，当前与窗口末端相同，单位 m。
+    /// </summary>
     public double TerminalReferenceS => WindowEndReferenceS;
 
+    /// <summary>
+    /// 窗口末端的边界语义；中途截断时为滚动安全停车边界。
+    /// </summary>
     public EmBoundaryType WindowEndBoundaryType { get; }
 
+    /// <summary>
+    /// 供终端约束使用的目标、换向或滚动终端类型。
+    /// </summary>
     public EmTerminalType TerminalType { get; }
 
+    /// <summary>
+    /// 纵向候选应滚动延续、接近停车边界还是在边界精确停车的模式。
+    /// </summary>
     public EmLongitudinalMode LongitudinalMode { get; }
 
+    /// <summary>
+    /// 实际目标或换向停车边界的局部参考弧长 S，单位 m；无此边界时仍保存段末。
+    /// </summary>
     public double StopBoundaryReferenceS { get; }
 
+    /// <summary>
+    /// 是否存在必须在当前方向段内处理的目标或换向停车边界。
+    /// </summary>
     public bool HasStopBoundary { get; }
 }
 
+/// <summary>
+/// 根据段边界、初始纵向状态和调度窗口选择 EM 优化地平线。
+/// 输入速度为 m/s、加速度为 m/s²、距离为 m；无效输入、越界初态或不足以停车的段会以明确状态拒绝。
+/// </summary>
 public sealed class PlanningHorizonSelector
 {
+    /// <summary>
+    /// 段终点、初态限值和停车距离比较使用的局部 S/距离容差，单位 m。
+    /// </summary>
     private const double BoundaryTolerance = 1e-8d;
 
+    /// <summary>
+    /// 为当前方向段选择不跨界的参考距离窗口和纵向终端模式。
+    /// 参数：currentSegmentReferenceS 为 m，初速为 m/s，初加速度为 m/s²，configuration 提供 m、s 制限值；planningScope 必须为定义的范围枚举。
+    /// 返回：成功时给出 selection；配置/初态非法返回 InvalidInput，停车距离越段返回 StoppingDistanceInsufficient，非法全段终边界返回 InvalidReferencePath。
+    /// </summary>
     public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
         double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
         EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
@@ -138,11 +178,19 @@ public sealed class PlanningHorizonSelector
         return EmPlanningStatus.Success;
     }
 
+    /// <summary>
+    /// 判断边界是否要求车辆在当前段内停止。
+    /// 参数：boundaryType 为边界枚举；返回：Goal 或 GearSwitchApproach 时为 true，其余边界允许滚动延续。
+    /// </summary>
     private static bool IsStopBoundary(EmBoundaryType boundaryType)
     {
         return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
     }
 
+    /// <summary>
+    /// 将参考边界语义映射为纵向终端类型。
+    /// 参数：boundaryType 为边界枚举；返回：目标、任一换向边界分别映射到 Goal、GearSwitch，其余映射为 RollingSafetyStop。
+    /// </summary>
     private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
     {
         return boundaryType == EmBoundaryType.Goal
@@ -152,11 +200,19 @@ public sealed class PlanningHorizonSelector
                 : EmTerminalType.RollingSafetyStop;
     }
 
+    /// <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);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceBoundary.cs

@@ -2,8 +2,16 @@ using System;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 方向段的不可变参考边界标识，携带局部与源路径弧长以及边界语义。
+/// 两个弧长均以 m 计；相等性仅代表同一段、同一局部位置和同一类型，不以源弧长参与判等。
+/// </summary>
 public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
 {
+    /// <summary>
+    /// 创建已验证的参考边界。
+    /// 参数：segmentIndex 必须非负，segmentLocalS 与 sourceArcLength 为非负有限 m 制弧长，boundaryType 必须是已定义枚举；非法输入会引发异常。
+    /// </summary>
     public ReferenceBoundary(int segmentIndex, double segmentLocalS, EmBoundaryType boundaryType, double sourceArcLength)
     {
         if (segmentIndex < 0)
@@ -21,22 +29,46 @@ public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
         SourceArcLength = sourceArcLength;
     }
 
+    /// <summary>
+    /// 边界所属方向段在路径段序列中的零基索引。
+    /// </summary>
     public int SegmentIndex { get; }
 
+    /// <summary>
+    /// 边界相对该方向段起点的局部参考弧长 S，单位 m。
+    /// </summary>
     public double SegmentLocalS { get; }
 
+    /// <summary>
+    /// 边界的目标、换向或普通边界语义。
+    /// </summary>
     public EmBoundaryType BoundaryType { get; }
 
+    /// <summary>
+    /// 边界在未重基原始平滑路径中的累计弧长，单位 m。
+    /// </summary>
     public double SourceArcLength { get; }
 
+    /// <summary>
+    /// 比较两个边界在优化语义上是否相同。
+    /// 参数：other 可为 null；返回：仅段索引、局部 S（精确 double 比较）和边界类型都相同时为 true，源弧长不参与比较。
+    /// </summary>
     public bool Equals(ReferenceBoundary other)
     {
         return other != null && SegmentIndex == other.SegmentIndex && SegmentLocalS.Equals(other.SegmentLocalS) &&
             BoundaryType == other.BoundaryType;
     }
 
+    /// <summary>
+    /// 比较任意对象是否为同一参考边界。
+    /// 参数：obj 可为空或非边界对象；返回：仅可转换为 <see cref="ReferenceBoundary"/> 且满足强类型相等性时为 true。
+    /// </summary>
     public override bool Equals(object obj) { return Equals(obj as ReferenceBoundary); }
 
+    /// <summary>
+    /// 计算与边界相等性一致的哈希值。
+    /// 返回：由段索引、局部 S 与边界类型组成的哈希；源弧长不参与以保持与 <see cref="Equals(ReferenceBoundary)"/> 一致。
+    /// </summary>
     public override int GetHashCode()
     {
         unchecked

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceHorizonSlicer.cs

@@ -6,8 +6,15 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 当前方向段内供一次 EM 优化使用的参考路径切片，保留全局参考站与切片局部弧长的对应关系。
+/// </summary>
 public sealed class ReferenceHorizonSlice
 {
+    /// <summary>
+    /// 创建一个以当前段起点为基准的参考地平线切片。
+    /// 参数：points 为局部 S（m）有序的参考点，terminalBoundary 必须属于 segment；返回：points 的防御性只读副本，空输入或边界不匹配会被拒绝。
+    /// </summary>
     public ReferenceHorizonSlice(DirectionSegmentView segment, IReadOnlyList<SmoothedPathPoint> points,
         ReferenceBoundary terminalBoundary)
     {
@@ -25,17 +32,38 @@ public sealed class ReferenceHorizonSlice
         TerminalBoundary = terminalBoundary;
     }
 
+    /// <summary>
+    /// 切片所属且不会跨越的单一方向段。
+    /// </summary>
     public DirectionSegmentView Segment { get; }
 
+    /// <summary>
+    /// 从段起点至精确终端点的参考点只读副本，局部 ArcLength 单位为 m。
+    /// </summary>
     public IReadOnlyList<SmoothedPathPoint> Points { get; }
 
+    /// <summary>
+    /// 切片末端的精确参考边界；中途截断时具有滚动安全停车语义。
+    /// </summary>
     public ReferenceBoundary TerminalBoundary { get; }
 }
 
+/// <summary>
+/// 按局部参考距离截取单一方向段，并在请求终端精确插值参考点。
+/// 切片只包含本段坐标；S、原始弧长与 X/Y 使用 m，航向使用 rad，终端附近以 1e-12 m 判定相等。
+/// </summary>
 public static class ReferenceHorizonSlicer
 {
+    /// <summary>
+    /// 端点复制、精确节点匹配和原段终点判定使用的局部 S 容差，单位 m。
+    /// </summary>
     private const double Epsilon = 1e-12d;
 
+    /// <summary>
+    /// 将方向段截取到请求的局部终端 S，并追加该终端的精确点。
+    /// 参数：requestedEndSegmentLocalS 为非负有限 m 制局部 S；超过段长的请求夹紧到段末。
+    /// 返回：段末保留原边界，中途截断创建 RollingSafetyStop 边界；空段或非法 S 会引发异常。
+    /// </summary>
     public static ReferenceHorizonSlice Slice(DirectionSegmentView segment, double requestedEndSegmentLocalS)
     {
         if (segment == null)
@@ -60,6 +88,10 @@ public static class ReferenceHorizonSlicer
         return new ReferenceHorizonSlice(segment, points, terminal);
     }
 
+    /// <summary>
+    /// 取得给定局部 S 的精确已有点或其相邻点之间的插值点。
+    /// 参数：terminalS 为已夹紧的 m 制局部 S；返回：与节点相差不超过 1e-12 m 时复用节点，否则在正跨度内线性插值。
+    /// </summary>
     private static SmoothedPathPoint GetExactTerminalPoint(DirectionSegmentView segment, double terminalS)
     {
         for (int index = 0; index < segment.Points.Count; index++)
@@ -76,6 +108,10 @@ public static class ReferenceHorizonSlicer
         return segment.Points[segment.Points.Count - 1];
     }
 
+    /// <summary>
+    /// 在线性弧长区间内插值一个终端平滑路径点。
+    /// 参数：lower/upper 的 ArcLength 为 m，localS 为其间的 m 制位置；返回：位置、展开航向、曲率和净空线性插值，标记为 Interpolated。
+    /// </summary>
     private static SmoothedPathPoint Interpolate(SmoothedPathPoint lower, SmoothedPathPoint upper, double localS)
     {
         double interval = upper.ArcLength - lower.ArcLength;
@@ -98,6 +134,10 @@ public static class ReferenceHorizonSlicer
             SmoothedPathPointSource.Interpolated);
     }
 
+    /// <summary>
+    /// 计算同单位标量的线性插值。
+    /// 参数：lower、upper 为端点，fraction 为无单位比例；返回：未夹紧的线性结果。
+    /// </summary>
     private static double Interpolate(double lower, double upper, double fraction)
     {
         return lower + (upper - lower) * fraction;

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferencePathSegmenter.cs

@@ -5,8 +5,16 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 将已发布的平滑参考路径拆分为不会跨越换向点的局部方向段视图。
+/// 每个输出段以局部 S=0 重基（m），同时保留原始累计弧长（m）及目标/换向边界语义；索引覆盖或弧长不连续会被拒绝。
+/// </summary>
 public static class ReferencePathSegmenter
 {
+    /// <summary>
+    /// 从已验证的平滑路径及其方向分段创建局部方向段视图。
+    /// 参数：referencePath 必须含完整 Path 和 Segments，路径弧长为有限 m 制量；返回：按源段索引排列的只读视图，任何漏覆盖、方向或严格递增弧长错误均引发异常。
+    /// </summary>
     public static IReadOnlyList<DirectionSegmentView> Create(PathSmoothingResult referencePath)
     {
         if (referencePath == null || referencePath.Path == null || referencePath.Segments == null ||
@@ -65,6 +73,10 @@ public static class ReferencePathSegmenter
         return new ReadOnlyCollection<DirectionSegmentView>(result);
     }
 
+    /// <summary>
+    /// 复制一个源参考点并将其弧长重基到当前方向段局部坐标。
+    /// 参数：source 保存世界 X/Y（m）、航向（rad）和曲率量，localS 为非负段局部 m 制弧长；返回：除 ArcLength 外保持源点几何和来源不变的副本。
+    /// </summary>
     internal static SmoothedPathPoint CloneAtLocalS(SmoothedPathPoint source, double localS)
     {
         return new SmoothedPathPoint(