ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md

# EM Planner 轨迹规划

`EMPlanner` 位于空间路径之后，针对一个已验证的前进或倒车方向段生成带时间、速度、曲率和终端语义的不可变 `EmTrajectory`。它执行静态走廊构建、LS 横向优化、ST 纵向优化、轨迹装配和独立世界空间复核，并显式支持 `FullDirectionSegment` 与 `RollingHorizon` 两种规划范围；不读取 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 重规划、滚动视窗、自适应 knot/发布上限和输出时间步长
│   ├── 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` | 空 | 由滚动协调器忽略旧结果 |
| 完整方向段无有效进度 | `NoProgress` | 空 | 不发布；仅真实终点容差内或受监督停车保持观察可合法零进度 |
| 完整方向段终端位姿超差 | `TerminalPoseMismatch` | 空 | 不发布；位置误差超过 `0.03 m` 或归一化 yaw 误差超过 `5°` |
| 自适应 knots 或发布样本超限 | `FullSegmentResourceLimitExceeded` | 空 | 不发布；不得截断轨迹或回退滚动模式 |

`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,
    planningScope: EmPlanningScope.FullDirectionSegment);

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;
```

## 规划范围：FullDirectionSegment 与 RollingHorizon

`EmPlanningRequest.PlanningScope` 显式选择一次 `Plan` 的边界语义；MovementTest 默认使用 `FullDirectionSegment`。

### FullDirectionSegment

- 对完整活动方向段做一次优化；`s_end` 来自已选 Local G2 的实际 `PathS`，`T_end` 由可行加速度、巡航、jerk 限速停止和零速 hold 推导，不依赖旧的距离/时间截断窗口。
- 前进期望/硬上限为 `1.0 m/s`，倒车期望/硬上限为 `0.5 m/s`。
- 成功终端必须停止，世界位置误差不超过 `0.03 m`，归一化 yaw 误差不超过 `5°`（约 `0.0873 rad`）。
- `NoProgress`、`TerminalPoseMismatch`、`FullSegmentResourceLimitExceeded` 失败均返回空轨迹，不降级为滚动或截断发布。
- `DistanceHorizonMeters` 与 `TimeHorizonSeconds` 作为兼容字段保留，但不用于截断 full 模式。

### RollingHorizon

`DistanceHorizonMeters` 控制 L-S 参考窗口，`TimeHorizonSeconds` 控制 S-T 输出时长。只有真实 `Goal` 或换向边界要求 `ExactStopAtBoundary` 零速终端；`RollingContinuation` 与 `ApproachStopBoundary` 可发布非零末端速度，不追加合成零速 hold tail。`ExactStopAtBoundary` 包含真实边界锚点和 QP 视界内的静止稳定区间，`ZeroSpeedHoldSeconds` 只在 QP 视界之后追加。

该服务不负责周期调用、版本淘汰、轨迹采样或控制命令。需要滚动运行时，将成功结果交给 [TrajectoryExecution](../TrajectoryExecution/README.md)，并由调用方管理状态捕获和周期。

## 详细使用指南（Detailed Usage Guide）

### 第 1 步：冻结一致的请求快照

`EmPlanningRequest` 必须包含同一版本的 `PathSmoothingResult` 和 `PlanningGridMap`、当前 `VehicleParameters`、不可变 `VehicleMotionState`、完整 `EmPlannerConfiguration`、方向段索引、时间、ID、运动模型和 `PlanningScope`。不要在 `Plan` 进行期间修改这些对象或从 UI/硬件重新读取值。

车辆状态中的速度带有符号：正数表示前进，负数表示倒车；速度接近配置的停车容差时按停车处理。`SequenceId`、地图快照 ID、参考路径 ID 和上一轨迹 ID 是诊断及滚动执行身份的一部分。

### 第 2 步：选择方向段与规划终端

一次请求只选择一个平滑路径方向段，并显式传入 `EmPlanningScope`。`FullDirectionSegment` 从状态投影处规划到该方向段真实终点（`GearSwitch` 或 `Goal`），`s_end` 取自 Local G2 实际 `PathS`，`T_end` 由可行加速度/巡航/停止包络推导，不再以 `TimeHorizonSeconds` 或 `DistanceHorizonMeters` 截断。`RollingHorizon` 才使用 legacy 视界选择 `RollingSafetyStop`、`GearSwitch` 或 `Goal`；精确停车边界成功后提供同姿态、零速度 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
├── TrajectoryPlanningVisualization.dll
├── osqp.dll
└── licenses/
    ├── OSQP-LICENSE.txt
    ├── OSQP-NOTICE.txt
    └── OSQP-VERSION.txt
```

发布器只从显式 `ManagedDll` 的同级目录解析 `TrajectoryPlanningVisualization.dll`，先复制完整包到 staging，
再原子替换 `plugins` 目录；它不会搜索 `PATH` 或当前工作目录。发布器会检查 64 位 PowerShell、原生 DLL 的 x64 PE
类型和固定 SHA-256，并保留既有恢复行为。OSQP 加载器从托管程序集目录以绝对路径加载同级 `osqp.dll`；它不依赖
`PATH` 或当前工作目录。部署和执行职责的更多说明见 [TrajectoryExecution](../TrajectoryExecution/README.md)，网页
观察的开关、token URL、图表语义和停止清理见 [EM trajectory observation MovementTest](../tarjplanner_movementtest/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 的成功结果是经物理与碰撞复核的时间轨迹输入，而不是可直接下发给真实车辆的硬件命令。