18 KiB
EM Planner 轨迹规划
EMPlanner 位于空间路径之后，针对一个已验证的前进或倒车方向段生成带时间、速度、曲率和终端语义的不可变 EmTrajectory。它执行静态走廊构建、LS 横向优化、ST 纵向优化、轨迹装配和独立世界空间复核，并显式支持 FullDirectionSegment 与 RollingHorizon 两种规划范围；不读取 UI、定位、轮速、硬件、系统时钟或当前工作目录。
PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing
-> EMPlanner (pure one-shot plan) -> TrajectoryExecution
滚动调度、过期结果抑制、旧轨迹交接、换向执行和通用控制命令不属于本模块；它们由 TrajectoryExecution 负责。唯一建议的业务入口是 EmPlanningService.Plan：
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）
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）
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 m 或归一化 yaw 误差超过 0 rad |
| 自适应 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 的切向量永远沿实际行驶方向，而非倒车时的车头方向：
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）
调用方必须先在模块边界外获取并冻结平滑路径、同版本地图、车辆参数和车辆状态。下例中这些对象均已准备好：
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。 - 成功终端必须停止，世界位置误差和归一化 yaw 误差均为
0（精确到达目标）。 NoProgress、TerminalPoseMismatch、FullSegmentResourceLimitExceeded失败均返回空轨迹，不降级为滚动或截断发布。DistanceHorizonMeters与TimeHorizonSeconds作为兼容字段保留，但不用于截断 full 模式。
RollingHorizon
DistanceHorizonMeters 控制 L-S 参考窗口，TimeHorizonSeconds 控制 S-T 输出时长。只有真实 Goal 或换向边界要求 ExactStopAtBoundary 零速终端；RollingContinuation 与 ApproachStopBoundary 可发布非零末端速度，不追加合成零速 hold tail。ExactStopAtBoundary 包含真实边界锚点和 QP 视界内的静止稳定区间，ZeroSpeedHoldSeconds 只在 QP 视界之后追加。
该服务不负责周期调用、版本淘汰、轨迹采样或控制命令。需要滚动运行时，将成功结果交给 TrajectoryExecution，并由调用方管理状态捕获和周期。
详细使用指南（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；硬件字段映射必须留给未来、经独立确认语义的适配器。
验证命令（Verification Commands）
从仓库根目录运行：
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 与显式的非仓库根目录目标：
& .\ClumsyPilot\scripts\Publish-ClumsyPilotPlugin.ps1 `
-ManagedDll .\ClumsyPilot\bin\Debug\netstandard2.0\ClumsyPilot.dll `
-OutputDirectory C:\deploy\ParkingRobot
发布树固定为：
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，网页
观察的开关、token URL、图表语义和停止清理见 EM trajectory observation MovementTest。
常见错误（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 实现。
因此，EMPlanner 的成功结果是经物理与碰撞复核的时间轨迹输入，而不是可直接下发给真实车辆的硬件命令。