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

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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