319 lines
27 KiB
Markdown
319 lines
27 KiB
Markdown
# 轨迹规划服务调用与闭环控制器交接指南
|
||
|
||
> 本指南展示固定离线场景的完整调用顺序。代码只用于复制阅读和接口交接，不会读取实车定位，也不会向底盘发送命令。
|
||
|
||
| 边界 | 类型/方法 | 含义 |
|
||
| --- | --- | --- |
|
||
| EMplanner 正式输出 | `EmTrajectory` | 带时间、位姿、曲率、速度、加速度、jerk 与方向边界的不可变轨迹 |
|
||
| 闭环控制器正式输入 | `Trajectory2D` | 几何控制器消费的世界位姿、弧长、曲率和有符号参考速度 |
|
||
| 唯一适配入口 | `EmControlTrajectoryAdapter.Create(EmTrajectory)` | 将单方向 EM 轨迹转换为控制器轨迹 |
|
||
|
||
`EmPlanningService` 是进程内同步服务。调用方创建冻结的 `EmPlanningRequest`，调用 `Plan(request, cancellationToken)`，并仅从本次返回的成功 `EmPlanningResult` 中索取 `Trajectory`。
|
||
|
||
## 使用边界与固定场景数据流
|
||
|
||
这是离线、静态、单方向示例：没有实时定位输入，没有底盘命令输出，也不把规划成功等同于可接管车辆。完整链路如下；每个编号都对应 [EmPlannerFullPipelineDemo.cs](EmPlannerFullPipelineDemo.cs) 中同名的 `步骤 N`。
|
||
|
||
```text
|
||
PlanningGridMap → PlanningRequest → HybridAStarPlanner.Plan
|
||
→ PathSmoothingRequest → PathSmoothingService.Smooth → DirectionSegmentView
|
||
→ EmPlanningRequest → EmPlanningService.Plan → EmPlanningResult.Trajectory
|
||
→ EmControlTrajectoryAdapter.Create → Trajectory2D → TrajectoryTrackingMovement
|
||
```
|
||
|
||
路径、位姿、车辆和轨迹值使用 SI：世界位置为 m，航向为 rad，曲率为 1/m。地图输入是明确的例外：`PlanningMapRequest` / `MapBoundsMm` 的边界、`ResolutionMm` 和障碍物几何均使用世界 mm，如完整示例步骤 3 所示。`PlanningGridMap` 是该 mm 地图请求生成的冻结快照。只能在明确拥有的边界做 m/mm 转换并保留单位标注：不得把 Clumsy 的 mm 坐标直接交给 `Pose2D`，也不得把 m 值未转换就交给 mm 地图契约；单位不明时立即停止，不根据数值大小猜测。
|
||
|
||
## 步骤 1–15
|
||
|
||
以下各步均可在 [EmPlannerFullPipelineDemo.cs](EmPlannerFullPipelineDemo.cs) 的对应 `步骤 N` 直接查阅完整固定场景代码。
|
||
|
||
### 步骤 1：定义固定世界起点和终点
|
||
|
||
[对应完整示例：步骤 1](EmPlannerFullPipelineDemo.cs#L40)
|
||
|
||
**本步输入**：固定场景常量（世界坐标 m、航向 rad）。
|
||
**调用**：构造 `Pose2D` 起点和目标。
|
||
**本步输出**：供 `PlanningRequest.Start`、`PlanningRequest.Goal` 使用的位姿。
|
||
**失败处理**：数值非有限、坐标系或单位不明时停止；不得以 mm 猜测转换。
|
||
|
||
### 步骤 2：定义车辆、曲率和搜索预算
|
||
|
||
[对应完整示例：步骤 2](EmPlannerFullPipelineDemo.cs#L50)
|
||
|
||
**本步输入**：车辆几何中心尺寸、`SafetyMarginMeters` 与可验证曲率限制。
|
||
**调用**：构造 `VehicleParameters` 和 `HybridAStarConfiguration`。
|
||
**本步输出**：用于粗规划、平滑复核和 EM 走廊的同一车辆约束。
|
||
**失败处理**：长度/宽度/曲率限制无效时不请求规划；不可混用控制点半径与车体几何。
|
||
|
||
### 步骤 3：建立静态地图
|
||
|
||
[对应完整示例：步骤 3](EmPlannerFullPipelineDemo.cs#L84)
|
||
|
||
**本步输入**：固定障碍物、`MapBoundsMm` 地图边界与 `ResolutionMm` 栅格分辨率；这些地图几何值均为世界 mm。
|
||
**调用**：建立并冻结 `PlanningGridMap`。
|
||
**本步输出**：同一 `SnapshotId` 的可读地图。
|
||
**失败处理**：地图未就绪、起点或终点越界/碰撞时拒绝，不生成替代移动命令。
|
||
|
||
### 步骤 4：建立粗路径请求
|
||
|
||
[对应完整示例：步骤 4](EmPlannerFullPipelineDemo.cs#L120)
|
||
|
||
**本步输入**：步骤 1–3 的位姿、地图、车辆和 Hybrid A* 配置。
|
||
**调用**：构造 `PlanningRequest`。
|
||
**本步输出**：一次性粗路径输入快照。
|
||
**失败处理**：不允许把上一轮的地图、车辆或目标悄悄混入本次请求。
|
||
|
||
### 步骤 5：搜索粗路径
|
||
|
||
[对应完整示例：步骤 5](EmPlannerFullPipelineDemo.cs#L133)
|
||
|
||
**本步输入**：`PlanningRequest`。
|
||
**调用**：`HybridAStarPlanner.Plan(request, cancellationToken)`。
|
||
**本步输出**：仅 `PlanningStatus.Success` 时可消费的 `PlanningResult.Path` 和 `Segments`。
|
||
**失败处理**：`SearchTimeout`、节点预算、不可达、取消或校验失败均停止；失败结果的集合为空。
|
||
|
||
### 步骤 6：请求并执行 Local G2 平滑
|
||
|
||
[对应完整示例：步骤 6](EmPlannerFullPipelineDemo.cs#L144)
|
||
|
||
**本步输入**：成功粗路径的 `Path`、`Segments`、同一地图/车辆和 `PathSmoothingConfiguration`。
|
||
**调用**：`new PathSmoothingRequest(coarseResult.Path, coarseResult.Segments, map, vehicle, smoothingConfiguration)`，再调用 `new PathSmoothingService().Smooth(smoothingRequest, cancellationToken)`。
|
||
**本步输出**：可发布状态（`Complete`、`PartialImprovement`、`NotNeeded` 或 `Unchanged`）的完整平滑路径。
|
||
**失败处理**：失败、取消或无效输入的 `Path`/`Segments` 为空，不能把局部结果交给 EM。
|
||
|
||
### 步骤 7：选择一个方向段
|
||
|
||
[对应完整示例：步骤 7](EmPlannerFullPipelineDemo.cs#L184)
|
||
|
||
**本步输入**：平滑结果的单个 `SmoothedPathSegment`。
|
||
**调用**：建立/取得 `DirectionSegmentView`。
|
||
**本步输出**：局部 `S=0`、方向一致的只读参考段和精确边界。
|
||
**失败处理**：不跨 `GearSwitch` 边界插值或合并；方向段索引、方向或边界不一致即拒绝。
|
||
|
||
### 步骤 8：建立初始运动状态
|
||
|
||
[对应完整示例：步骤 8](EmPlannerFullPipelineDemo.cs#L198)
|
||
|
||
**本步输入**：固定场景的世界位姿、车体纵向有符号速度、状态时间与序列号。
|
||
**调用**：构造 `VehicleMotionState`。
|
||
**本步输出**：与地图、平滑路径同版本的起始状态。
|
||
**失败处理**：过期状态或非零速度方向与活动段不符会得到 `StaleVehicleState`/`StateDirectionMismatch`。
|
||
|
||
### 步骤 9：冻结 EM 配置
|
||
|
||
[对应完整示例：步骤 9](EmPlannerFullPipelineDemo.cs#L207)
|
||
|
||
**本步输入**：低速泊车配置树。
|
||
**调用**：`EmPlannerConfiguration.CreateDefault()` 后在请求前完成本场景修改。
|
||
**本步输出**：供服务复制和验证的一套配置。
|
||
**失败处理**：子配置或权重为空、数值无效时只接受 `InvalidInput`，不降低安全约束继续运行。
|
||
|
||
### 步骤 10：建立 EM 请求
|
||
|
||
[对应完整示例：步骤 10](EmPlannerFullPipelineDemo.cs#L225)
|
||
|
||
**本步输入**：平滑成功结果、冻结地图、车辆、状态、配置和活动 `segmentIndex`。
|
||
**调用**：构造 `EmPlanningRequest`，给出请求/生效 UTC、输出 ID、参考 ID、`EmMotionModel` 与 `FullDirectionSegment`。
|
||
**本步输出**：一次 EM 输入快照。
|
||
**失败处理**：固定演示不传跨段 `PreviousTrajectory`；身份或方向段不匹配时不调用适配器。
|
||
|
||
### 步骤 11：调用 EM 服务
|
||
|
||
[对应完整示例：步骤 11](EmPlannerFullPipelineDemo.cs#L245)
|
||
|
||
**本步输入**：`EmPlanningRequest` 和调用方 `CancellationToken`。
|
||
**调用**：`new EmPlanningService(new OsqpNativeSolver()).Plan(request, cancellationToken)`。
|
||
**本步输出**：不可变 `EmPlanningResult`。
|
||
**失败处理**：这是同步进程内调用，不应把它包装为 HTTP、后台轨迹仓库或异步下发。
|
||
|
||
### 步骤 12：判断是否可发布
|
||
|
||
[对应完整示例：步骤 12](EmPlannerFullPipelineDemo.cs#L252)
|
||
|
||
**本步输入**：`Status`、`FailureReason` 与 `Trajectory`。
|
||
**调用**：只接受 `Success` 或 `SuccessWithFallback`，再检查轨迹非空及控制点数。
|
||
**本步输出**：可适配的完整单段 `EmTrajectory`。
|
||
**失败处理**：任何其他状态、空轨迹或少于两个不同世界位置的点，保留既有安全策略并记录原因。
|
||
|
||
### 步骤 13：读取输出契约
|
||
|
||
[对应完整示例：步骤 13](EmPlannerFullPipelineDemo.cs#L264)
|
||
|
||
**本步输入**：成功 `EmTrajectory`。
|
||
**调用**：读取只读 `Metadata` 与 `Points`，用于身份、方向、边界和诊断。
|
||
**本步输出**：原始时间轨迹仍与控制轨迹并存。
|
||
**失败处理**：不得就地修改点列；不得把 `VelocityX`/`VelocityY` 当车辆命令。
|
||
|
||
### 步骤 14：适配为控制轨迹
|
||
|
||
[对应完整示例：步骤 14](EmPlannerFullPipelineDemo.cs#L304)
|
||
|
||
**本步输入**：步骤 12 的同一方向完整 `EmTrajectory`。
|
||
**调用**：`new EmControlTrajectoryAdapter().Create(emTrajectory)`。
|
||
**本步输出**：以世界位置重新从零累计弧长的 `Trajectory2D`，保留航向、车辆曲率和有符号参考速度。
|
||
**失败处理**：适配器拒绝空、单点或没有两个不同位置的轨迹；不自行补点或拼接另一方向。
|
||
|
||
### 步骤 15：交给闭环动作
|
||
|
||
[对应完整示例：步骤 15](EmPlannerFullPipelineDemo.cs#L308)
|
||
|
||
**本步输入**：`Trajectory2D`、控制器维护者拥有的 `StateProvider` 与调参。
|
||
**调用**：赋给 `TrajectoryTrackingMovement.Trajectory`（或由宿主包装为 `TrackMotionPlanSegment`）。
|
||
**本步输出**：闭环几何控制器的轨迹输入。
|
||
**失败处理**：只替换轨迹来源；保留状态提供者、控制器调参、执行保护和停止策略的所有权。
|
||
|
||
## 服务索取与适配代码
|
||
|
||
以下是唯一可交接到控制器的状态闸门：
|
||
|
||
```csharp
|
||
var planningService = new EmPlanningService(new OsqpNativeSolver());
|
||
EmPlanningResult result = planningService.Plan(request, cancellationToken);
|
||
|
||
bool succeeded = result.Status == EmPlanningStatus.Success ||
|
||
result.Status == EmPlanningStatus.SuccessWithFallback;
|
||
if (!succeeded || result.Trajectory == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"EM 规划未返回可交给控制器的完整轨迹：" + result.FailureReason);
|
||
}
|
||
|
||
EmTrajectory emTrajectory = result.Trajectory;
|
||
Trajectory2D controllerTrajectory =
|
||
new EmControlTrajectoryAdapter().Create(emTrajectory);
|
||
```
|
||
|
||
## 输出契约、单位和坐标系
|
||
|
||
### `EmPlanningResult`
|
||
|
||
| 成员 | 契约 |
|
||
| --- | --- |
|
||
| `Status` | 机器可读最终状态；仅 `Success`、`SuccessWithFallback` 可发布。 |
|
||
| `Trajectory` | 仅成功状态为完整非空不可变 `EmTrajectory`；其他状态必为 `null`，不是部分可执行轨迹。 |
|
||
| `FailureReason` | 成功降级或失败诊断；不能取代 `Status` 判定。 |
|
||
|
||
### `EmTrajectoryMetadata`
|
||
|
||
| 成员 | 契约 |
|
||
| --- | --- |
|
||
| `TrajectoryId` / `PreviousTrajectoryId` | 本次发布 ID 与可选前轨迹 ID；后者空字符串表示无关联。 |
|
||
| `GeneratedAtUtc` / `EffectiveAtUtc` | UTC 生成时刻与计划生效时刻；控制端按自己的调度语义解释。 |
|
||
| `MapSnapshotId` / `ReferencePathId` / `VehicleStateSequenceId` | 地图、参考路径和状态的来源版本，供追踪和去重。 |
|
||
| `SegmentIndex` / `Direction` | 所属单方向段与已验证前进/倒车方向。 |
|
||
| `TerminalType` / `LongitudinalMode` / `PlanningScope` | 终端业务语义、纵向终端语义和 `FullDirectionSegment`/滚动范围。 |
|
||
|
||
### `EmTrajectoryPoint`
|
||
|
||
| 字段 | 单位/坐标 | 控制相关语义 |
|
||
| --- | --- | --- |
|
||
| `TimeFromStart` | s | 自轨迹开始的非负时间。 |
|
||
| `X`, `Y`, `Yaw` | 世界 m、世界 rad | `Trajectory2D` 使用的世界位姿。 |
|
||
| `VehicleCurvature` | 1/m | 车辆路径曲率，适配器原样传给控制点。 |
|
||
| `SignedLongitudinalVelocity` / `Speed` | m/s | 前进为正、倒车为负；`Speed` 是绝对值。适配器使用前者。 |
|
||
| `VelocityX`, `VelocityY` | 世界 m/s | 由有符号速度和航向导出的预测分量。`VelocityX` 和 `VelocityY` 是世界坐标系中的预测速度分量，不是底盘纵向/横向命令；闭环测试不得把它们直接发送给车辆。 |
|
||
| `YawRate` | rad/s | `SignedLongitudinalVelocity * VehicleCurvature` 导出的预测偏航角速度。 |
|
||
| `SegmentIndex`, `SegmentLocalS`, `PathS`, `Direction`, `BoundaryType` | 索引、m、m、枚举、枚举 | 段归属、局部/全路径弧长、方向及目标/换向边界；用于保证不跨方向消费。 |
|
||
| `LongitudinalAcceleration`, `LongitudinalJerk` | m/s²、m/s³ | 沿车体前向轴的规划器内部诊断量，不是 `Trajectory2D` 输入。外部维护者不得复制对内部成员的访问；如自有构建确需该证据，必须先通过其显式公开契约暴露后再读取。 |
|
||
|
||
VelocityX 和 VelocityY 是世界坐标系中的预测速度分量，不是底盘纵向/横向命令；闭环测试不得把它们直接发送给车辆。
|
||
|
||
| 概念 | 正确解释 | 常见错误 |
|
||
| --- | --- | --- |
|
||
| m / mm | 路径、`Pose2D`、车辆和 `EmTrajectory` / `Trajectory2D` 值为 SI m；`PlanningMapRequest` / `MapBoundsMm` 边界、`ResolutionMm` 和障碍几何为世界 mm。仅在明确的接口边界转换。 | 将 Clumsy `Vector2` 的 mm 直接用于 `Pose2D`，或将 m 障碍几何直接填入 mm 地图契约。 |
|
||
| rad / deg | 所有 `Yaw`、转角、容差为 rad；`AngleMath.DegreesToRadians` 只用于初始化控制器角度。 | 将 3 或 45 当作 rad。 |
|
||
| 1/m、m/s、m/s²、m/s³、rad/s | 分别是曲率、速度、加速度、jerk、偏航角速度。 | 用曲率替代转向角，或把加速度当速度。 |
|
||
| 世界/车体 | `X/Y/Yaw`、`VelocityX/Y` 在世界系；有符号纵向速度、加速度、jerk 沿车体前向轴。 | 把世界 XY 分量作为车体纵/横向命令。 |
|
||
|
||
## 参数索引
|
||
|
||
表中“演示/当前默认”表示固定 demo 未覆盖时使用当前构造默认值；调大/调小描述主要趋势，任何变更都须重新验证安全与可行性。
|
||
|
||
### 粗路径配置
|
||
|
||
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|
||
| --- | --- | --- | --- | --- | --- |
|
||
| `VehicleParameters.LengthMeters`, `WidthMeters` | `CoarsePath/Contracts/VehicleParameters.cs` | m | 场景填入；测试车辆为 0.80 / 0.60 | 几何中心车体长宽 | 大：更保守、可行域小；小：净空风险增大。 |
|
||
| `SafetyMarginMeters` | 同上 | m | 场景填入 | 车体四周附加安全余量 | 大：更安全但更易不可达；小：相反。 |
|
||
| `MaximumCurvaturePerMeter` / `MinimumTurningRadiusMeters` | 同上 | 1/m / m | 至少其一；测试为 1/1.20 | 车辆转弯极限 | 曲率大/半径小：机动性增、与实车不符风险增；反之可行域缩小。 |
|
||
| `PrimitiveLengthMeters`, `IntegrationStepMeters`, `MaximumCollisionCheckStepMeters` | `CoarsePath/Contracts/HybridAStarConfiguration.cs` | m | 0.50 / 0.05 / 0.025 | 原语长度、内部采样、连续碰撞步长 | 长/大：更快但分辨率低；短/小：更精细、成本高。 |
|
||
| `HeadingResolutionRadians`, `CurvatureLevelCount` | 同上 | rad / 个 | π/36 / 5 | 状态航向与曲率离散度 | 分辨率更细/等级更多：质量好、节点增；反之更快粗糙。 |
|
||
| `GoalPositionToleranceMeters`, `GoalHeadingToleranceRadians` | 同上 | m / rad | 0.15 / π/36 | 终点判定容差 | 大：更易成功但末端误差大；小：更严格、可能失败。 |
|
||
| `MaximumExpandedNodes`, `SearchTimeout` | 同上 | 节点 / s | 200000 / 5 | 搜索资源预算 | 大：成功机会高、延迟高；小：更快但易资源失败。 |
|
||
| `HeuristicWeight`, `ReverseCostMultiplier`, `GearSwitchPenaltyMeters` | 同上 | 无量纲 / 倍 / m | 1 / 1.5 / 1 | 搜索偏好、倒车和换向代价 | 大：更偏启发式/少倒车/少换向；小：更全面但可能慢或频繁换向。 |
|
||
| `CurvatureMagnitudeWeight`, `CurvatureChangePenaltyMetersPerLevel` | 同上 | 无量纲 / m | 0.10 / 0.05 | 曲率与曲率变化代价 | 大：路径更平顺；小：更短但控制负担大。 |
|
||
| `ClearanceCostWeight`, `ClearanceCostDistanceMeters`, `AllowReverse` | 同上 | 无量纲 / m / bool | 0.20 / 0.50 / true | 净空偏好、安全距离、倒车开关 | 前两项大：偏好更大净空；`false`：禁止倒车且可行域缩小。 |
|
||
|
||
### Local G2 平滑配置
|
||
|
||
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|
||
| --- | --- | --- | --- | --- | --- |
|
||
| `OutputSpacingMeters`, `MaximumCollisionCheckStepMeters` | `PathSmoothing/Contracts/PathSmoothingConfiguration.cs` | m | 0.025 / 0.025 | 输出采样、扫掠复核步长 | 大：点少/检查粗；小：更细/开销高。 |
|
||
| `MinimumClearanceReserveMeters`, `CurvatureLimitRadiusToleranceMeters` | 同上 | m / m | 0 / 0.002 | 额外净空、曲率半径容差 | 净空大更保守；半径容差大更宽松。 |
|
||
| `MinimumWindowLengthMeters`, `PreferredWindowLengthMeters`, `MaximumWindowLengthMeters` | `PathSmoothing/Contracts/LocalG2QuinticOptions.cs` | m | 0.20 / 0.50 / 0.80 | 局部 G2 窗口范围 | 大：过渡更缓、影响范围大；小：更局部、改善受限。 |
|
||
| `MaximumDeviationMeters` | 同上 | m | 0.10 | 相对原路径最大偏移 | 大：改善空间大但净空风险高；小：更保守。 |
|
||
| `AbsoluteCurvatureJumpFloorPerMeter`, `CurvatureJumpRatioOfMaximum` | 同上 | 1/m / 比例 | 0.001 / 0.05 | 识别曲率跳变阈值 | 大：仅处理明显跳变；小：处理更多区域。 |
|
||
| `MinimumPeakGradientImprovementRatio`, `MaximumVariationCostRegressionRatio` | 同上 | 比例 | 0.20 / 0.02 | 接受候选的改善与退化门槛 | 前者大/后者小：更严格；反之更易接受。 |
|
||
| `MaximumCandidatesPerRegion` | 同上 | 个 | 12 | 每区域候选预算 | 大：质量机会高、耗时高；小：更快、可能错过候选。 |
|
||
|
||
### EM 配置
|
||
|
||
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|
||
| --- | --- | --- | --- | --- | --- |
|
||
| `Corridor.LongitudinalSampleSpacingMeters`, `LateralSampleSpacingMeters`, `MaximumCollisionCheckStepMeters` | `EMPlanner/Configuration/CorridorConfiguration.cs` | m | 0.10 / 0.025 / 0.025 | 走廊纵向、横向和碰撞采样 | 大：更快但走廊/检查变粗；小：更精细、结点增。 |
|
||
| `Corridor.MaximumLateralOffsetMeters`, `AdditionalClearanceReserveMeters` | 同上 | m | 0.30 / 0.02 | 可搜索横偏与额外净空 | 横偏大：绕障空间大；净空大：更安全但更易不可行。 |
|
||
| `Frenet.MaximumProjectionDistanceMeters`, `MinimumFrenetDenominator`, `BoundaryAnchorToleranceMeters` | `EMPlanner/Configuration/FrenetConfiguration.cs` | m / 无量纲 / m | 0.50 / 0.20 / 1e-8 | 投影范围、`1-k*l` 安全余量、边界锚定容差 | 前者大更容忍错位；中项大更保守；容差大更宽松。 |
|
||
| `Lateral.MaximumLateralStepPerIterationMeters`, `MaximumLateralSlope`, `MaximumLateralSecondDerivativePerMeter`, `MaximumLateralThirdDerivativePerSquareMeter` | `EMPlanner/Configuration/LateralConfiguration.cs` | m / 无 / 1/m / 1/m² | 0.05 / 0.50 / 1 / 2 | LS 横偏更新、斜率及导数硬限 | 大：可操作性增、平顺/可控性风险增；小：更保守或不可行。 |
|
||
| `Lateral.Weights.ReferenceOffset`, `HeadingDeviation`, `SecondDerivative`, `ThirdDerivative` | `EMPlanner/Configuration/LateralWeights.cs` | 无量纲 | 10 / 1 / 5 / 10 | 偏参考、航向偏差及二/三阶导偏好 | 大：更强惩罚对应量；小：更自由。 |
|
||
| `Lateral.Weights.Curvature`, `CurvatureVariation`, `PreviousTrajectory`, `RollingTerminal` | 同上 | 无量纲 | 5 / 20 / 5 / 10 | 曲率、曲率变化、上轮和滚动末端偏好 | 大：对应项更平稳/连续；小：更追求其他目标。 |
|
||
| `Longitudinal.MaximumForwardSpeedMetersPerSecond`, `MaximumReverseSpeedMetersPerSecond`, `DesiredForwardSpeedMetersPerSecond`, `DesiredReverseSpeedMetersPerSecond` | `EMPlanner/Configuration/LongitudinalConfiguration.cs` | m/s | 1 / 0.5 / 1 / 0.5 | 正反向速度硬限与目标巡航 | 大：行程快但制动/横向约束更紧；小：更保守。 |
|
||
| `MaximumAccelerationMetersPerSecondSquared`, `MaximumDecelerationMetersPerSecondSquared`, `MaximumJerkMetersPerSecondCubed` | 同上 | m/s² / m/s² / m/s³ | 0.20 / 0.30 / 0.50 | 纵向舒适性硬限 | 大：响应快但冲击大；小：平顺但易无足够制动距离。 |
|
||
| `MaximumLateralAccelerationMetersPerSecondSquared`, `MaximumCurvatureRatePerMeterPerSecond`, `StopSpeedToleranceMetersPerSecond`, `ZeroSpeedHoldSeconds` | 同上 | m/s² / 1/(m·s) / m/s / s | 0.20 / 0.50 / 0.01 / 0.20 | 横向加速、曲率率、停稳阈值和停稳保持 | 前两项大更激进；停速阈值大更易认停；保持长更保守。 |
|
||
| `Longitudinal.Weights.ReferenceSpeed`, `Acceleration`, `Jerk`, `PreviousTrajectory`, `TerminalAcceleration` | `EMPlanner/Configuration/LongitudinalWeights.cs` | 无量纲 | 10 / 1 / 10 / 5 / 1 | ST 速度、加速度、jerk、连续性和末端偏好 | 大：更强惩罚对应项；小：让位其他目标。 |
|
||
| `Solver.MaximumOuterIterations`, `MaximumOsqpIterations` | `EMPlanner/Configuration/SolverConfiguration.cs` | 次 | 5 / 4000 | 外层与 OSQP 迭代预算 | 大：更可能收敛、耗时高；小：更快、易超时/不收敛。 |
|
||
| `Solver.AbsoluteTolerance`, `RelativeTolerance`, `StrictResidualTolerance`, `WarmStart`, `Polish`, `NativeVerbose` | 同上 | 无 / bool | 1e-5 / 1e-5 / 1e-5 / true / true / false | 求解精度与运行选项 | 容差大更快但精度低；`WarmStart`/`Polish`提高收敛质量；诊断仅影响日志。 |
|
||
| `Scheduling.ReplanPeriodSeconds`, `TimeHorizonSeconds`, `DistanceHorizonMeters`, `OutputTimeStepSeconds` | `EMPlanner/Configuration/SchedulingConfiguration.cs` | s / s / m / s | 0.20 / 6 / 500 / 0.05 | 触发周期、预测时空窗口和输出间隔 | 窗口大/步长小：覆盖细、成本高；周期小：更新快。 |
|
||
| `SolverTimeoutSeconds`, `HandoffLookaheadSeconds`, `MaximumVehicleStateAgeSeconds` | 同上 | s | 0.10 / 0.30 / 0.20 | 求解预算、交接前看、状态时效 | 预算大更易完成；交接/时效大更宽容但更陈旧。 |
|
||
| `MaximumOptimizationTimeStepSeconds`, `MaximumOptimizationSpatialStepMeters`, `MaximumOptimizationKnotCount`, `MaximumPublishedSampleCount` | 同上 | s / m / 个 / 个 | 0.20 / 0.10 / 401 / 5001 | 优化离散和资源上限 | 步长大更粗；上限大质量机会高、资源高。 |
|
||
| `Validation.SpatialToleranceMeters`, `KinematicTolerance`, `TerminalPositionToleranceMeters`, `TerminalYawToleranceRadians` | `EMPlanner/Configuration/ValidationConfiguration.cs` | m / 无 / m / rad | 1e-8 / 1e-5 / 0 / 0 | 发布前几何、动力学和终端验收容差 | 大更宽松；小更严格、可能拒绝候选。 |
|
||
|
||
### 闭环控制配置
|
||
|
||
| 参数 | 源码路径 | 单位 | 演示/当前默认 | 含义 | 调大 / 调小影响 |
|
||
| --- | --- | --- | --- | --- | --- |
|
||
| `Trajectory`, `StateProvider`, `CycleObserver` | `MultiWheelC/Movements/TrajectoryTrackingMovement.cs`（外部测试树） | 类型/回调 | 调用方提供 / null / null | 轨迹、状态来源与诊断所有权 | 仅替换 `Trajectory`；不要改变状态提供者或诊断归属。 |
|
||
| `StanleyCrossTrackGainPerSecond`, `StanleyHeadingErrorGain`, `StanleyMinimumSpeedMetersPerSecond`, `StanleyUsesActualSpeed` | 同上 | 1/s / 无 / m/s / bool | 0.4 / 1 / 0.15 / true | Stanley 横偏、航向、低速保护与速度来源 | 增益大更快纠偏也易振荡；低速保护大更稳但迟钝。 |
|
||
| `MaximumCrossTrackCorrectionRadians`, `MaximumHeadingCorrectionRadians` | 同上 | rad | 10° / 10° | 横偏/航向纠正上限 | 大：纠偏更强；小：更温和、可能跟踪慢。 |
|
||
| `LongitudinalKp`, `LongitudinalKiPerSecond`, `LongitudinalKdSeconds` | 同上 | 无 / 1/s / s | 0.5 / 0 / 0 | 速度 PID 三项 | 大：响应增强，也可能超调、积分饱和或噪声放大。 |
|
||
| `MaximumIntegralCorrectionMetersPerSecond`, `LongitudinalSpeedErrorDeadbandMetersPerSecond`, `MaximumCommandSpeedMetersPerSecond` | 同上 | m/s | 0.05 / 0.025 / 0.50 | 积分限幅、死区、命令速度上限 | 限幅/上限大更激进；死区大更稳但有稳态误差。 |
|
||
| `MaximumGcpAngleRadians`, `MaximumGcpAngleRateRadiansPerSecond` | 同上 | rad / rad/s | 45° / 15°/s | GCP 角度与变化率限制 | 大：机动快但执行风险高；小：平稳但转弯跟踪受限。 |
|
||
| `FinishDistanceMeters`, `FinishSpeedMetersPerSecond`, `FinishHeadingToleranceRadians` | 同上 | m / m/s / rad | 0.03 / 0.02 / 3° | 完成位置、停速和航向阈值 | 大：更易完成、末端误差大；小：更严格。 |
|
||
| `MaximumDistanceToTrajectoryMeters`, `ExecutionTimeoutSeconds` | 同上 | m / s | 0.30 / 120 | 偏离保护与执行超时 | 大：更容忍但风险高；小：更早保护。 |
|
||
|
||
## 状态决策与换向
|
||
|
||
| 场景 | 识别 | 必须处理 |
|
||
| --- | --- | --- |
|
||
| 无效输入 | `InvalidInput`、粗路径 `Invalid*`、平滑失败 | 记录诊断，修正输入；不创建控制轨迹。 |
|
||
| 不可行 | `NoFeasiblePath`、`CorridorInfeasible`、`LateralInfeasible`、`LongitudinalInfeasible` | 停在控制输入边界，保留已有安全策略。 |
|
||
| 取消 | `Cancelled` 或调用方 token 取消 | 丢弃本次结果，不把局部轨迹交给控制器。 |
|
||
| 截止/超时 | 粗路径 `SearchTimeout`、EM `SolverTimedOut`、`CycleDeadlineExpired`、资源限制 | 不延用本次半成品；诊断预算与阶段。 |
|
||
| 空轨迹 | 成功闸门不成立或 `Trajectory == null` | 当失败处理；绝不补造直线替代移动。 |
|
||
| 方向不匹配 | `StateDirectionMismatch`，或 Metadata/点方向不同 | 只用当前活动段；先停稳再重新确认。 |
|
||
| 适配器拒绝 | 少于两个不同位置点，`Create` 抛出 | 不修改原始轨迹硬凑点；修复上游输出。 |
|
||
|
||
本 demo 是**单方向**。有效的 `FullDirectionSegment` 轨迹即使以 `EmTerminalType.GearSwitch` 结束，仍只是一个可跟踪到精确停车的安全方向段；但调用方只能在显式提供已通过安全评审的换向交接授权时执行该段。授权仅覆盖“执行当前段并在换向边界精确停车”；到站后必须由外部状态机确认已停稳和下一方向，然后单独请求并执行下一段。授权绝不允许把正、负方向点串接成一个 `Trajectory2D`、自动跨越边界或跨边界插值。
|
||
|
||
## 外部闭环测试中的替换点
|
||
|
||
当前应替换的人工来源是外部 `NewControllerTrackingTests.cs` 中的 `TestTrajectoryFactory.CreateStraight4Meters`（在直线 4 m 测试中构造 `trajectory`）。下游汇是 `TrajectoryTrackingMovement.Trajectory`。合并时只把该变量的来源替换为本指南的服务调用和适配器输出；`StateProvider`、Stanley/PID/GCP 调参、完成/偏离保护、超时与记录器均由控制器测试继续拥有。包装器和直接服务替换路径都要求调用方显式传入换向交接是否已通过安全评审和授权；未授权的 `GearSwitch` 轨迹必须在创建跟踪动作前拒绝。不要改写原有安全所有权来迁就规划器。
|
||
|
||
## 建议阅读顺序
|
||
|
||
1. 本 README；
|
||
2. [EmPlannerFullPipelineDemo.cs](EmPlannerFullPipelineDemo.cs) 的步骤 1–15；
|
||
3. `EmPlannerTrajectoryReplacementExample.cs` 的替换前后对照；
|
||
4. 可选阅读现有 [trajectory-planning-flow-demo.html](trajectory-planning-flow-demo.html)。
|
||
|
||
HTML 只是辅助流程图，不是接口规范；以本 README、源代码契约和完整示例为准。
|