ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md

EM Planner 轨迹规划

EMPlanner 位于空间路径之后，针对一个已验证的前进或倒车方向段生成带时间、速度、曲率和终端语义的不可变 EmTrajectory。它执行静态走廊构建、LS 横向优化、ST 纵向优化、轨迹装配和独立世界空间复核；不读取 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 重规划、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）

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 的切向量永远沿实际行驶方向，而非倒车时的车头方向：

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

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，并由调用方管理状态捕获和周期。

详细使用指南（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；硬件字段映射必须留给未来、经独立确认语义的适配器。

验证命令（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
├── osqp.dll
└── licenses/
    ├── OSQP-LICENSE.txt
    ├── OSQP-NOTICE.txt
    └── OSQP-VERSION.txt

发布器会检查 64 位 PowerShell、原生 DLL 的 x64 PE 类型和固定 SHA-256。OSQP 加载器从托管程序集目录以绝对路径加载同级 osqp.dll；它不依赖 PATH 或当前工作目录。部署和执行职责的更多说明见 TrajectoryExecution。

常见错误（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 的成功结果是经物理与碰撞复核的时间轨迹输入，而不是可直接下发给真实车辆的硬件命令。