240 lines
16 KiB
Markdown
240 lines
16 KiB
Markdown
# EM Planner 轨迹规划
|
||
|
||
`EMPlanner` 位于空间路径之后，针对一个已验证的前进或倒车方向段生成带时间、速度、曲率和终端语义的不可变 `EmTrajectory`。它执行静态走廊构建、LS 横向优化、ST 纵向优化、轨迹装配和独立世界空间复核；不读取 UI、定位、轮速、硬件、系统时钟或当前工作目录。
|
||
|
||
```text
|
||
PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing
|
||
-> EMPlanner (pure one-shot plan) -> TrajectoryExecution
|
||
```
|
||
|
||
滚动调度、过期结果抑制、旧轨迹交接、换向执行和通用控制命令不属于本模块；它们由 [TrajectoryExecution](../TrajectoryExecution/README.md) 负责。唯一建议的业务入口是 `EmPlanningService.Plan`：
|
||
|
||
```csharp
|
||
EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
||
```
|
||
|
||
## 模块说明（Module Overview）
|
||
|
||
| 模块 | 负责内容 | 不负责内容 |
|
||
| --- | --- | --- |
|
||
| `CoarsePath` / `PathSmoothing` | 生成并验证带方向段的连续空间参考路径 | EM 走廊、时间参数化、轨迹执行 |
|
||
| `EMPlanner` | 快照校验、方向段投影、静态走廊、LS/ST、轨迹装配和世界空间发布复核 | 调度、UI、定位/轮速读取、硬件命令、动态障碍行为 |
|
||
| `Optimization` | 求解器中立的 `IQpSolver` / `QuadraticProgram` 契约和 OSQP 后端 | 将 OSQP P/Invoke 泄漏到 LS/ST 规划器 |
|
||
| `TrajectoryExecution` | 滚动协调、最新结果发布、旧轨迹交接、换向状态机和通用控制命令 | 修改 EM 优化结果、直接驱动硬件 |
|
||
| `EMPlannerVerificationHost` | 固定回归场景、求解器与滚动执行验证 | 实时地图、UI 或车辆控制 |
|
||
|
||
每次 `Plan` 只消费一个 `DirectionSegmentView`。换向边界由相邻方向段的身份区分，即使两个锚点具有相同世界位姿，也绝不让一次规划跨越该边界。
|
||
|
||
## 文件结构（File Structure）
|
||
|
||
```text
|
||
EMPlanner/
|
||
├── README.md
|
||
├── Configuration/
|
||
│ ├── EmPlannerConfiguration.cs # 不可变快照前的配置根与默认值
|
||
│ ├── SchedulingConfiguration.cs # 0.20 s 重规划、6 s / 5 m 视界和输出时间步长
|
||
│ ├── CorridorConfiguration.cs # 走廊采样、偏移和净空
|
||
│ ├── FrenetConfiguration.cs # 投影距离、分母和边界锚点容差
|
||
│ ├── LateralConfiguration.cs / LateralWeights.cs
|
||
│ ├── LongitudinalConfiguration.cs / LongitudinalWeights.cs
|
||
│ ├── SolverConfiguration.cs # OSQP 迭代、残差与 warm start 设置
|
||
│ └── ValidationConfiguration.cs # 独立复核容差
|
||
├── Contracts/
|
||
│ ├── EmPlanningRequest.cs / EmPlanningResult.cs / EmPlanningStatus.cs
|
||
│ ├── EmTrajectory.cs / EmTrajectoryMetadata.cs / EmTrajectoryPoint.cs
|
||
│ ├── VehicleMotionState.cs # 位姿、带符号纵向速度、采样时间和序列号
|
||
│ └── EmMotionModel.cs / EmTerminalType.cs / EmBoundaryType.cs
|
||
├── Segmentation/ # 方向段、精确边界、视界和切片
|
||
├── Frenet/ # 投影、插值和前进/倒车重建
|
||
├── Corridor/ # 静态、种子连通的可行走廊
|
||
├── Lateral/ # LS 变量、QP、SQP、几何和独立候选复核
|
||
├── Longitudinal/ # 实际 PathS 速度包络、ST QP 和复核
|
||
├── Trajectory/ # LS/ST 合成、插值和零速 hold tail
|
||
├── Validation/ # 请求校验与世界空间轨迹发布复核
|
||
├── Optimization/ # 稀疏 QP 契约及 OSQP 绝对路径加载后端
|
||
├── Diagnostics/ # 可选且与规划结果隔离的诊断旁路
|
||
└── Facade/
|
||
├── IEmPlanningService.cs # 纯单次规划边界
|
||
└── EmPlanningService.cs # 固定处理顺序的业务门面
|
||
```
|
||
|
||
`Lateral` 与 `Longitudinal` 只能依赖 `IQpSolver`、`QuadraticProgram`、`QpSolverSettings` 和 `QpSolveResult`。OSQP 原生加载、生命周期和状态映射都封装在 `Optimization` 后端，不能向 LS/ST 引入 P/Invoke 或当前工作目录依赖。
|
||
|
||
## 规划数据流（Planning Data Flow）
|
||
|
||
```text
|
||
EmPlanningRequest（调用方冻结的输入）
|
||
│
|
||
├── 请求与配置校验、复制快照
|
||
├── PathSmoothingResult -> DirectionSegmentView
|
||
├── 车辆状态有界 Frenet 投影
|
||
├── 精确规划视界与终端选择
|
||
├── 上一条轨迹的同段 Frenet 种子投影
|
||
├── StaticCorridorBuilder（静态、种子连通走廊）
|
||
├── LateralPlanner（LS SQP + 独立横向几何复核）
|
||
├── PathSpeedLimitBuilder（实际 PathS 速度包络）
|
||
├── LongitudinalPlanner（ST + 严格纵向复核）
|
||
├── EmTrajectoryAssembler（完整时间轨迹和零速尾段）
|
||
└── EmTrajectoryValidator（世界空间、足迹扫掠和冗余字段复核）
|
||
▼
|
||
EmPlanningResult（完整轨迹或空轨迹 + 可审计诊断）
|
||
```
|
||
|
||
LS 的变量是 `l`、`dl`、`ddl` 和区间 `dddl`；它对静态走廊、导数、Frenet 分母和线性化车辆曲率使用硬约束。ST 只消费 LS 已复核的实际、严格递增 `PathS`，并对进度、速度、加速度、jerk 和终端 `PathS` / 零速度施加硬约束。两者均只保留经独立复核的候选；超时、取消或求解失败绝不发布未经验证的原始求解向量。
|
||
|
||
## 结果、状态与停止（Result Status and Stop）
|
||
|
||
`EmPlanningResult` 将结果和失败语义绑定：只有 `Success` 与 `SuccessWithFallback` 才携带非空 `Trajectory`；其余状态始终携带空轨迹。调用方不能把任何失败状态理解为“可执行的部分轨迹”。
|
||
|
||
| 情况 | `EmPlanningStatus` | `Trajectory` | 调用方处理 |
|
||
| --- | --- | --- | --- |
|
||
| 所有优化和发布复核成功 | `Success` | 完整、不可变轨迹 | 可交给执行层或保存为下一轮种子 |
|
||
| 后续迭代失败但保留已独立验证候选 | `SuccessWithFallback` | 完整、不可变 fallback 轨迹 | 可消费，并记录诊断 |
|
||
| 输入、状态、路径或投影无效 | `InvalidInput`、`StaleVehicleState`、`StateDirectionMismatch`、`InvalidReferencePath`、`ProjectionFailed` | 空 | 刷新快照或修复上游输入 |
|
||
| 静态走廊、LS、ST 或停车条件不可行 | `CorridorInfeasible`、`LateralInfeasible`、`LongitudinalInfeasible`、`StoppingDistanceInsufficient` | 空 | 不发布；由上层决定重试、停车或重新选路 |
|
||
| 求解器不可用、超时、取消或发布复核失败 | `SolverUnavailable`、`SolverTimedOut`、`Cancelled`、`ValidationFailed`、`Failed` | 空 | 读取 `FailureReason`；不得使用中间轨迹 |
|
||
| 已被更新周期替代 | `Superseded` | 空 | 由滚动协调器忽略旧结果 |
|
||
|
||
`FailureReason` 始终以 `map=...;reference=...;state=...;previous=...;segment=...` 开头，供调用方追踪地图、参考路径、状态、上一轨迹和方向段绑定。
|
||
|
||
## 坐标、单位与方向（Coordinates and Direction）
|
||
|
||
| 数据 | 单位 / 约定 | 说明 |
|
||
| --- | --- | --- |
|
||
| 世界 `X`、`Y`、车辆尺寸、`ReferenceS`、`PathS` | m | `PathS` 是 LS 重建后的真实几何弧长，非参考弧长替代品 |
|
||
| `Yaw`、航向误差 | rad | 公开车辆航向规范化到 `[-PI, PI)` |
|
||
| 车辆曲率 | `1/m` | `YawRate = SignedLongitudinalVelocity * VehicleCurvature` |
|
||
| 时间 | s / `DateTimeOffset` | 输出 `TimeFromStart` 严格递增；请求时间由调用方冻结 |
|
||
| `SignedLongitudinalVelocity` | m/s | 前进为正、倒车为负；是权威速度字段 |
|
||
| `Speed`、`VelocityX/Y` | m/s | 分别由绝对速度和车辆航向从权威速度推导 |
|
||
|
||
Frenet 的切向量永远沿实际行驶方向，而非倒车时的车头方向：
|
||
|
||
```text
|
||
travelYaw = Forward ? vehicleYaw : Normalize(vehicleYaw + PI)
|
||
l > 0 = 实际行驶方向左侧
|
||
x = referenceX - l * sin(travelYaw)
|
||
y = referenceY + l * cos(travelYaw)
|
||
vehicleYaw = Reverse ? Normalize(optimizedTravelYaw + PI) : optimizedTravelYaw
|
||
```
|
||
|
||
因此倒车不需要再次翻转横向偏移或世界速度；选择倒车方向段并提供负的带符号纵向速度即可。
|
||
|
||
## 最小调用示例（Minimal Call Example）
|
||
|
||
调用方必须先在模块边界外获取并冻结平滑路径、同版本地图、车辆参数和车辆状态。下例中这些对象均已准备好：
|
||
|
||
```csharp
|
||
using System;
|
||
using System.Threading;
|
||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
var service = new EmPlanningService(new OsqpNativeSolver());
|
||
var configuration = EmPlannerConfiguration.CreateDefault();
|
||
var capturedAt = DateTimeOffset.UtcNow;
|
||
var state = new VehicleMotionState(
|
||
capturedVehiclePose,
|
||
signedLongitudinalSpeedMetersPerSecond: 0d,
|
||
longitudinalAccelerationMetersPerSecondSquared: null,
|
||
capturedAtUtc: capturedAt,
|
||
sequenceId: 42L);
|
||
|
||
var request = new EmPlanningRequest(
|
||
publishedSmoothingResult, planningMap, vehicle, state, configuration,
|
||
segmentIndex: 0, previousTrajectory: null,
|
||
requestedAtUtc: capturedAt, effectiveAtUtc: capturedAt,
|
||
outputTrajectoryId: "trajectory-42", referencePathId: "path-17", previousTrajectoryId: "",
|
||
motionModel: EmMotionModel.NonholonomicForwardReverse);
|
||
|
||
EmPlanningResult result = service.Plan(request, CancellationToken.None);
|
||
if (result.Status != EmPlanningStatus.Success && result.Status != EmPlanningStatus.SuccessWithFallback)
|
||
throw new InvalidOperationException(result.FailureReason);
|
||
|
||
EmTrajectory trajectory = result.Trajectory;
|
||
```
|
||
|
||
该服务不负责周期调用、版本淘汰、轨迹采样或控制命令。需要滚动运行时，将成功结果交给 [TrajectoryExecution](../TrajectoryExecution/README.md)，并由调用方管理状态捕获和周期。
|
||
|
||
## 详细使用指南（Detailed Usage Guide）
|
||
|
||
### 第 1 步：冻结一致的请求快照
|
||
|
||
`EmPlanningRequest` 必须包含同一版本的 `PathSmoothingResult` 和 `PlanningGridMap`、当前 `VehicleParameters`、不可变 `VehicleMotionState`、完整 `EmPlannerConfiguration`、方向段索引、时间、ID 和运动模型。不要在 `Plan` 进行期间修改这些对象或从 UI/硬件重新读取值。
|
||
|
||
车辆状态中的速度带有符号：正数表示前进，负数表示倒车；速度接近配置的停车容差时按停车处理。`SequenceId`、地图快照 ID、参考路径 ID 和上一轨迹 ID 是诊断及滚动执行身份的一部分。
|
||
|
||
### 第 2 步：选择方向段与规划终端
|
||
|
||
一次请求只选择一个平滑路径方向段。服务从状态投影处开始，按配置的 `6.0 s` / `5.0 m` 视界选择精确终端：未到方向段边界时为 `RollingSafetyStop`，到换向边界前为 `GearSwitch`，最终段终点为 `Goal`。成功轨迹在终端精确停车，并以 `0.05 s` 间隔提供 `0.20 s` 同姿态、零速度 hold tail。
|
||
|
||
上一条轨迹仅能作为同方向、同方向段、位于当前视界内的 Frenet 种子。种子用于保持横向连续性，不能让规划跨过换向边界或绕过静态走廊连通性检查。
|
||
|
||
### 第 3 步：处理成功和 fallback
|
||
|
||
`SuccessWithFallback` 仍表示轨迹已通过完整独立复核；它不是“尽力而为”的未验证输出。调用方可安全消费其 `EmTrajectory`，同时记录诊断以监控求解器或迭代问题。任何其他状态都没有可消费轨迹。
|
||
|
||
`EmTrajectoryPoint` 保留世界位姿、权威带符号速度、推导速度字段、yaw rate、曲率、时间、方向段、边界类型和内部纵向导数。装配器与验证器分别计算和复核这些冗余关系，并对点间以最大 `0.025 m` 中心步长执行车辆足迹扫掠检查。
|
||
|
||
### 第 4 步：保持服务纯净
|
||
|
||
`EmPlanningService` 是同步、单次且可取消的函数边界。它不创建后台周期、不比较并发周期、不发布全局当前轨迹、不读取时钟，也不调用底盘。调度与执行职责在 [TrajectoryExecution](../TrajectoryExecution/README.md)；硬件字段映射必须留给未来、经独立确认语义的适配器。
|
||
|
||
## 验证命令（Verification Commands）
|
||
|
||
从仓库根目录运行：
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
|
||
```
|
||
|
||
`em-core-all` 覆盖纵向模型、纵向集成、完整轨迹和纯 `EmPlanningService`。`em-all` 在此基础上覆盖 Foundation、OSQP、LS、ST、轨迹发布、滚动协调、执行器、插件打包和端到端安全尾段。若只排查特定边界，也可使用现有 `lateral-all`、`optimization`、`osqp`、`coordinator` 或 `executor` 入口；所有命令都必须从仓库根目录运行。
|
||
|
||
## Windows x64 插件发布（Windows x64 Plugin Package）
|
||
|
||
构建后的插件需将托管程序集、固定 OSQP 运行时和许可证一起部署。使用已构建的 `ClumsyPilot.dll` 与显式的非仓库根目录目标：
|
||
|
||
```powershell
|
||
& .\ClumsyPilot\scripts\Publish-ClumsyPilotPlugin.ps1 `
|
||
-ManagedDll .\ClumsyPilot\bin\Debug\netstandard2.0\ClumsyPilot.dll `
|
||
-OutputDirectory C:\deploy\ParkingRobot
|
||
```
|
||
|
||
发布树固定为：
|
||
|
||
```text
|
||
plugins/
|
||
├── ClumsyPilot.dll
|
||
├── osqp.dll
|
||
└── licenses/
|
||
├── OSQP-LICENSE.txt
|
||
├── OSQP-NOTICE.txt
|
||
└── OSQP-VERSION.txt
|
||
```
|
||
|
||
发布器会检查 64 位 PowerShell、原生 DLL 的 x64 PE 类型和固定 SHA-256。OSQP 加载器从托管程序集目录以绝对路径加载同级 `osqp.dll`；它不依赖 `PATH` 或当前工作目录。部署和执行职责的更多说明见 [TrajectoryExecution](../TrajectoryExecution/README.md)。
|
||
|
||
## 常见错误（Common Errors）
|
||
|
||
| 现象 | 原因 | 处理 |
|
||
| --- | --- | --- |
|
||
| 倒车横向偏移或世界速度方向反了 | 又按车头方向翻转了 Frenet 符号 | 使用倒车方向段及负的 `SignedLongitudinalVelocity`；不要额外翻转 `l` |
|
||
| 用 `ReferenceS` 做 ST 距离 | 忽略 LS 重建后的几何弧长 | ST 只消费严格递增的实际 `PathS` |
|
||
| 失败后仍使用轨迹 | 忽略 `EmPlanningStatus` | 仅 `Success` / `SuccessWithFallback` 可读取 `Trajectory` |
|
||
| LS/ST 直接调用 OSQP P/Invoke | 破坏求解器中立边界 | 只通过 `IQpSolver` / `QuadraticProgram` 求解 |
|
||
| 换向段被一次规划跨越 | 将同位姿锚点按坐标去重 | 使用 `(SegmentIndex, SegmentLocalS, BoundaryType)` 身份 |
|
||
| 从子目录运行 `lateral-all` 找不到固定 DLL | 该历史测试夹具以仓库根目录为基准定位 OSQP | 按文档从仓库根目录运行，不修改夹具或核心加载逻辑 |
|
||
| 将轨迹直接写入电机或 UI 字段 | 规划服务不拥有执行/硬件边界 | 交给 `TrajectoryExecution` 的通用命令，再实现独立硬件适配器 |
|
||
|
||
## 第一版限制（First-Version Limits）
|
||
|
||
当前首版已经提供静态环境下的前进/倒车 EM 轨迹规划、独立发布复核和可供滚动执行消费的终端安全轨迹；它不包含：
|
||
|
||
- 动态障碍物预测、时空占用、跟车、让行、超车或动态重路由；
|
||
- UI、定位、传感器、轮速、底盘、电机或任何硬件协议集成；
|
||
- 横移、蟹行、侧向车体速度或原地旋转；
|
||
- 多段跨换向的一次性轨迹发布；每次只处理一个方向段；
|
||
- 滚动调度、轨迹交接、换向确认和控制命令生成；这些由 [TrajectoryExecution](../TrajectoryExecution/README.md) 实现。
|
||
|
||
因此，EMPlanner 的成功结果是经物理与碰撞复核的时间轨迹输入，而不是可直接下发给真实车辆的硬件命令。
|