229 lines
14 KiB
Markdown
229 lines
14 KiB
Markdown
# TrajectoryExecution 滚动执行
|
||
|
||
`TrajectoryExecution` 位于纯 `EMPlanner` 服务之后。它接收调用方冻结的状态和时间，负责滚动周期的 latest-wins 协调、上一条已发布轨迹的安全交接、不可变轨迹采样、换向停稳状态机，以及控制器中立的 `TrajectoryControlCommand` 输出。
|
||
|
||
```text
|
||
caller snapshots -> EmPlanningCoordinator -> immutable published EmTrajectory
|
||
-> TrajectoryExecutor
|
||
-> TrajectoryControlCommand
|
||
-> future hardware adapter
|
||
```
|
||
|
||
它不读取定位、轮速、UI、硬件、系统时钟或当前工作目录，也不参与 LS/ST 优化。纯单次规划的请求、结果、坐标、OSQP 和插件打包说明见 [EMPlanner README](../EMPlanner/README.md)。
|
||
|
||
## 模块说明（Module Overview）
|
||
|
||
| 组件 | 负责内容 | 不负责内容 |
|
||
| --- | --- | --- |
|
||
| `EmPlanningCoordinator` | caller-clocked 周期判断、取消上一周期、当前身份判定与原子发布 | 读取状态、修改轨迹、控制硬件 |
|
||
| `TrajectoryHandoffSelector` | 仅在安全追踪且未跨边界时从已发布轨迹取未来样本 | 跨段/跨方向插值或替换调用方状态 |
|
||
| `TrajectoryExecutor` | 按调用方时间选择轨迹点、驱动换向状态机、生成执行状态 | 越过末点外推、重新规划或直接下发底盘 |
|
||
| `GearSwitchStateMachine` | 零速停稳、一次换向请求和确认后继续 | 猜测换向是否已经由硬件完成 |
|
||
| `TrajectoryControlAdapter` | 将执行状态映射为纵向速度与 yaw rate 命令 | 横移、蟹行、原地旋转或硬件字段映射 |
|
||
| 调用方 / 未来硬件适配器 | 捕获状态、选择地图和路径版本、确认方向、执行具体设备动作 | 将这些外部依赖反向注入 EM 核心 |
|
||
|
||
`IVehicleStateProvider.Capture()` 只定义执行边界中的状态快照契约。实现者可以在模块外读取定位或硬件，但返回后必须是不可变 `VehicleMotionState`；EMPlanner 和执行器不会直接引用该实现。
|
||
|
||
## 文件结构（File Structure）
|
||
|
||
```text
|
||
TrajectoryExecution/
|
||
├── README.md
|
||
├── EmPlanningCoordinator.cs # latest-wins 异步周期、取消和完整轨迹发布
|
||
├── IEmPlanningCycleSink.cs # 可选完成事件；异常不会改变周期结果
|
||
├── IVehicleStateProvider.cs # 调用方拥有的车辆状态快照边界
|
||
├── PlanningCycleIdentity.cs # 地图/路径/状态/上一轨迹/方向段的身份绑定
|
||
├── PlanningCycleInput.cs # 一次 caller-clocked 周期的请求、身份和时间
|
||
├── PlanningCycleResult.cs # 周期版本、结果、是否发布和诊断
|
||
├── TrajectoryHandoffSelector.cs # 同段、同方向的未来样本交接或测量状态回退
|
||
├── TrajectorySampler.cs # 同质轨迹区间内的时间插值
|
||
├── TrajectoryExecutor.cs # 轨迹点选择、末点钳制和执行状态更新
|
||
├── TrajectoryExecutionState.cs # 选中点、换向状态和安全语义
|
||
├── GearSwitchState.cs # 执行状态枚举
|
||
├── GearSwitchStateMachine.cs # 零速停稳与方向确认状态机
|
||
├── TrajectoryControlAdapter.cs # 执行状态到通用控制命令的映射
|
||
└── TrajectoryControlCommand.cs # 不可变控制器中立输出
|
||
```
|
||
|
||
## 滚动执行数据流（Rolling Execution Data Flow）
|
||
|
||
```text
|
||
caller captures VehicleMotionState, map/path versions and clock value
|
||
│
|
||
▼
|
||
PlanningCycleInput(request, now)
|
||
│
|
||
├── EmPlanningCoordinator.ShouldStartCycle(now)
|
||
└── EmPlanningCoordinator.PlanLatestAsync(input, cancellationToken)
|
||
│
|
||
├── cancel older in-flight cycle
|
||
├── bind PlanningCycleIdentity and monotonically increasing version
|
||
├── call pure IEmPlanningService.Plan
|
||
└── publish only a current, successful, complete EmTrajectory
|
||
▼
|
||
caller reads PublishedTrajectory
|
||
▼
|
||
TrajectoryExecutor.UpdateCommand(now, measuredState, trajectory,
|
||
desiredDirection, currentDirection, directionConfirmed)
|
||
▼
|
||
TrajectoryControlCommand -> future hardware adapter
|
||
```
|
||
|
||
`PlanningCycleIdentity` 绑定 `MapSnapshotId`、`ReferencePathId`、车辆状态 `SequenceId`、`PreviousTrajectoryId` 和 `SegmentIndex`。新周期会取消先前周期；即使旧周期随后完成，只要版本或完整身份不再当前，它的结果就变为 `Superseded`，不能发布。
|
||
|
||
规划失败也不会清除或替换上一条完整、已发布轨迹。调用方可继续执行其精确零速安全尾段；这不是允许忽略失败，而是避免以空或部分结果制造新的执行跳变。
|
||
|
||
## 周期、交接与停止（Cycle, Handoff and Stop）
|
||
|
||
### 周期频率与发布
|
||
|
||
默认重规划周期为 `0.20 s`，由 `EmPlannerConfiguration.Scheduling.ReplanPeriodSeconds` 提供。`ShouldStartCycle` 和 `PlanLatestAsync` 均只使用调用方传入的 `DateTimeOffset`；协调器不读取墙上时钟。
|
||
|
||
`PlanLatestAsync` 仅在下列条件同时满足时设置 `PublishedTrajectory`：
|
||
|
||
- 此周期版本和完整 `PlanningCycleIdentity` 仍是最新；
|
||
- `IEmPlanningService.Plan` 返回 `Success` 或 `SuccessWithFallback`；
|
||
- 结果携带非空、完整的 `EmTrajectory`。
|
||
|
||
可选 `IEmPlanningCycleSink` 只能观察 `PlanningCycleResult`。接收器抛出异常会追加诊断，但不会回滚已确定的周期结果或发布状态。
|
||
|
||
### 安全交接
|
||
|
||
`TrajectoryHandoffSelector.Select` 只有在以下条件均成立时才使用上一条已发布轨迹的未来样本：
|
||
|
||
- 轨迹已生效且未超过配置的最大状态年龄；
|
||
- 轨迹与预期的 `SegmentIndex` 和 `TravelDirection` 完全一致；
|
||
- 当前测量状态与轨迹当前样本的位置、航向和带符号速度都在配置容差内；
|
||
- 交接 lookahead 未超出轨迹末点，且从当前样本到交接样本之间没有换向或终端边界。
|
||
|
||
任何条件不满足时，结果固定回退到调用方提供的 `VehicleMotionState`，并记录 `TrajectoryHandoffRejectionReason`。它不会跨方向段插值，也不会把不安全的旧轨迹当作种子。
|
||
|
||
### 采样和终端
|
||
|
||
`TrajectorySampler` 只在同一段、同一方向、同一边界类型的相邻点之间插值。`TrajectoryExecutor` 对轨迹开始前选择首点，对末点之后选择末点；它绝不向轨迹末点之后外推。`Goal` 与 `RollingSafetyStop` 都进入完成状态，输出零速度、零 yaw rate 和制动保持。
|
||
|
||
## 换向状态机（Gear-Switch State Machine）
|
||
|
||
精确 `GearSwitchApproach` 边界使用如下状态序列：
|
||
|
||
```text
|
||
Following -> ApproachingGearSwitch -> HoldingZero
|
||
-> RequestingDirectionChange -> AwaitingDirectionConfirmation -> Following
|
||
```
|
||
|
||
| 状态 | 条件和动作 | 命令语义 |
|
||
| --- | --- | --- |
|
||
| `Following` | 所需方向与当前方向一致 | 跟随已采样的纵向速度和 yaw rate |
|
||
| `ApproachingGearSwitch` | 所需方向不同但尚未到精确边界 | 仍跟随接近轨迹 |
|
||
| `HoldingZero` | 到达边界；测量速度需持续低于停车容差 | 零速度、零 yaw rate、保持制动 |
|
||
| `RequestingDirectionChange` | 达到配置的零速 dwell（默认 `0.20 s`） | 只发出一次 `RequestDirectionChange`，仍制动 |
|
||
| `AwaitingDirectionConfirmation` | 已发请求但调用方尚未确认 | 零速度、零 yaw rate、保持制动 |
|
||
| `Completed` | `Goal` 或 `RollingSafetyStop` | 零速度、零 yaw rate、保持制动且完成 |
|
||
|
||
方向变化必须由调用方更新 `currentDirection` 并设置 `directionConfirmed`。执行层不猜测硬件换向是否成功，也不会在停稳前释放非零速度。
|
||
|
||
## 通用控制命令（Generic Control Command）
|
||
|
||
`TrajectoryExecutor.UpdateCommand` 先更新纯执行状态，再通过 `TrajectoryControlAdapter` 创建不可变 `TrajectoryControlCommand`：
|
||
|
||
| 字段 | 单位 / 语义 |
|
||
| --- | --- |
|
||
| `SignedLongitudinalVelocity` | m/s；正数前进，负数倒车；保持或完成时强制为 0 |
|
||
| `YawRate` | rad/s；常规跟随时来自轨迹点；保持或完成时强制为 0 |
|
||
| `Direction` | 当前轨迹点的 `TravelDirection` |
|
||
| `RequestDirectionChange` | 仅在换向请求那个更新周期为 `true` |
|
||
| `HoldBrake` | 停稳、等待确认或已完成时为 `true` |
|
||
| `IsTrajectoryComplete` | 到达 `Goal` 或 `RollingSafetyStop` 后为 `true` |
|
||
|
||
此命令刻意不包含横向车体速度、蟹行、原地旋转、UI 对象或硬件对象。未来硬件适配器只能在独立确认字段语义后将这些通用量映射到具体底盘协议。
|
||
|
||
## 最小调用示例（Minimal Call Example）
|
||
|
||
下例假设调用方已经创建 `request`，并已从外部捕获 `measuredState`、当前方向和当前时间：
|
||
|
||
```csharp
|
||
using System;
|
||
using System.Threading;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
var planningService = new EmPlanningService(new OsqpNativeSolver());
|
||
var coordinator = new EmPlanningCoordinator(planningService);
|
||
var executor = new TrajectoryExecutor(request.Configuration);
|
||
var now = capturedNowUtc;
|
||
|
||
if (coordinator.ShouldStartCycle(now))
|
||
{
|
||
var cycle = new PlanningCycleInput(request, now);
|
||
PlanningCycleResult result = coordinator.PlanLatestAsync(cycle, CancellationToken.None)
|
||
.GetAwaiter().GetResult();
|
||
if (!result.Published)
|
||
ReportPlanningDiagnostic(result.Diagnostic);
|
||
}
|
||
|
||
EmTrajectory published = coordinator.PublishedTrajectory;
|
||
if (published != null)
|
||
{
|
||
TrajectoryControlCommand command = executor.UpdateCommand(
|
||
now, measuredState, published,
|
||
desiredDirection, currentDirection, directionConfirmed);
|
||
SendToFutureHardwareAdapter(command);
|
||
}
|
||
```
|
||
|
||
`ReportPlanningDiagnostic` 和 `SendToFutureHardwareAdapter` 是调用方边界的示意名称，不是本模块 API。调用方必须保证 `request` 与本次捕获的地图、路径和状态版本一致。
|
||
|
||
## 详细使用指南（Detailed Usage Guide）
|
||
|
||
### 第 1 步：调用方捕获和冻结输入
|
||
|
||
在规划边界外通过 `IVehicleStateProvider` 的实现或其他集成代码捕获 `VehicleMotionState`，再准备相同版本的地图、平滑路径、车辆参数和配置，构造 `EmPlanningRequest` 与 `PlanningCycleInput`。不要让协调器或执行器自行读取定位、轮速或 UI。
|
||
|
||
### 第 2 步：按调用方时钟发起周期
|
||
|
||
使用同一个调用方时钟值检查 `ShouldStartCycle(now)` 并构造 `PlanningCycleInput(request, now)`。将取消令牌传给 `EmPlanningCoordinator.PlanLatestAsync`；新周期会自动取消旧周期，但调用方仍应管理自身任务生命周期和应用退出时的取消。
|
||
|
||
### 第 3 步：只消费已发布的完整轨迹
|
||
|
||
读取 `PublishedTrajectory` 前无需持有内部锁。只有协调器确认周期为当前且结果成功时该属性才更新。失败、取消和过期结果不覆盖上一条轨迹；调用方应记录 `PlanningCycleResult`，并继续以自己的安全策略决定是否保持或停止。
|
||
|
||
### 第 4 步：采样、换向并下发通用命令
|
||
|
||
每个控制更新调用 `TrajectoryExecutor.UpdateCommand`，并提供当前测量状态、目标方向、已确认方向和确认标志。收到 `RequestDirectionChange` 时，硬件层负责发起真实换向；只有成功后才把确认状态回传。`HoldBrake` 为真时必须保持零纵向速度和零 yaw rate。
|
||
|
||
## 验证命令与部署（Verification and Deployment）
|
||
|
||
从仓库根目录运行：
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- plugin-package
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
|
||
```
|
||
|
||
`coordinator` 覆盖周期、过期抑制和安全交接；`executor` 覆盖前进/倒车换向、停稳、确认和通用命令；`plugin-package` 验证 Windows x64 插件树。`em-all` 是完整首版门禁。`ClumsyPilot.dll`、`osqp.dll` 和许可证树的发布命令、哈希与部署位置见 [EMPlanner README](../EMPlanner/README.md) 的“Windows x64 插件发布”章节。
|
||
|
||
## 常见错误（Common Errors）
|
||
|
||
| 现象 | 原因 | 处理 |
|
||
| --- | --- | --- |
|
||
| 旧周期覆盖了新地图或新状态的结果 | 没有经协调器的当前身份检查直接发布结果 | 只消费 `PublishedTrajectory` 和 `PlanningCycleResult.Published` |
|
||
| 交接后车辆跳变 | 忽略追踪、年龄、方向段或边界拒绝条件 | 使用 `TrajectoryHandoffSelection`；拒绝时从测量状态重置 |
|
||
| 轨迹结束后仍输出移动命令 | 自己外推或忽略末点 | 每个周期使用 `UpdateCommand`；末点只返回零速 tail/完成状态 |
|
||
| 还未停稳就切换方向 | 忽略 `HoldBrake` 或重复发送请求 | 等待 `RequestDirectionChange`，保持制动，待确认后继续 |
|
||
| 将 `YawRate` 当成转向角 | 混淆通用命令与具体底盘协议 | 在未来硬件适配器中按底盘模型转换 |
|
||
| 从任意目录运行历史横向夹具失败 | 夹具以仓库根目录定位固定 OSQP DLL | 按本文和 EMPlanner README 的命令从仓库根目录运行 |
|
||
|
||
## 第一版限制（First-Version Limits）
|
||
|
||
当前首版已经提供静态环境下的滚动协调、轨迹交接、前进/倒车换向、通用控制命令和 Windows x64 插件部署边界；它不包含：
|
||
|
||
- 动态障碍物预测、时空占用、跟车、让行、超车或动态路径重选；
|
||
- UI、定位、传感器、轮速、底盘、电机或硬件协议实现；
|
||
- 横移、蟹行、横向车体速度或原地旋转；
|
||
- 真实硬件闭环安全认证、性能基准或现场故障恢复策略；
|
||
- 对 `EmPlanningService` 的异步化、全局调度或外部状态读取。
|
||
|
||
因此，`TrajectoryControlCommand` 是明确但控制器中立的执行意图；只有未来硬件适配器才能把它变为特定设备动作。
|