From 297186ab8a584f2119bfc27aae710ad6da74fc6a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 13:55:39 +0800 Subject: [PATCH] docs: rewrite EM Planner readme --- .../ParkrobTrajplanner/EMPlanner/README.md | 592 ++++++------------ 1 file changed, 192 insertions(+), 400 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md index 7365100..ce6f62e 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md @@ -1,423 +1,198 @@ -# EM Planner Foundation +# EM Planner 轨迹规划 -## 当前范围 - -EM Planner 位于既有的空间路径之后： +`EMPlanner` 位于空间路径之后，针对一个已验证的前进或倒车方向段生成带时间、速度、曲率和终端语义的不可变 `EmTrajectory`。它执行静态走廊构建、LS 横向优化、ST 纵向优化、轨迹装配和独立世界空间复核；不读取 UI、定位、轮速、硬件、系统时钟或当前工作目录。 ```text -PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing -> EM Planner -> TrajectoryExecution +PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing + -> EMPlanner (pure one-shot plan) -> TrajectoryExecution ``` -本 Foundation 只消费一个已验证的 `PathSmoothingResult` 方向段、同版本的 -`PlanningGridMap`、车辆几何和请求快照。它不读取硬件、UI、时钟或当前工作目录。 -每次计算只处理一个 `DirectionSegmentView`，因此不会跨越换向边界。 +滚动调度、过期结果抑制、旧轨迹交接、换向执行和通用控制命令不属于本模块；它们由 [TrajectoryExecution](../TrajectoryExecution/README.md) 负责。唯一建议的业务入口是 `EmPlanningService.Plan`： -此门禁已提供 solver-neutral `IQpSolver` 契约和固定 OSQP 后端；尚未实现 LS 横向优化、 -ST 纵向优化、滚动协调、轨迹发布或动态障碍物。 +```csharp +EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken) +``` -## 坐标与符号 +## 模块说明（Module Overview） -世界长度使用 m，时间使用 s，航向使用 rad，曲率使用 `1/m`。车体 `+x` 指向车头， -车体 `+y` 指向车体左侧，正航向为逆时针。Frenet 的基向量永远使用实际行驶方向： +| 模块 | 负责内容 | 不负责内容 | +| --- | --- | --- | +| `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) -t = (cos(travelYaw), sin(travelYaw)) -n = (-sin(travelYaw), cos(travelYaw)) - -deltaS = dx*cos(travelYaw) + dy*sin(travelYaw) -l = -dx*sin(travelYaw) + dy*cos(travelYaw) - -x = referenceX - l*sin(travelYaw) -y = referenceY + l*cos(travelYaw) -optimizedTravelYaw = travelYaw + atan2(dl, 1 - referenceK*l) -vehicleYaw = Reverse ? Normalize(optimizedTravelYaw + PI) : Normalize(optimizedTravelYaw) +l > 0 = 实际行驶方向左侧 +x = referenceX - l * sin(travelYaw) +y = referenceY + l * cos(travelYaw) +vehicleYaw = Reverse ? Normalize(optimizedTravelYaw + PI) : optimizedTravelYaw ``` -`ReferenceS` 始终沿实际运动方向递增；`l > 0` 永远代表运动方向左侧。 -例如倒车时车头朝 `-PI + 0.01`，`travelYaw` 为 `0.01`。此时 `l > 0` 在 -运动方向左侧、也就是车体右侧；不得在倒车分支额外翻转 `l`。重建要求 -`1 - referenceK*l >= MinimumFrenetDenominator`，否则拒绝奇异 Frenet 几何。 +因此倒车不需要再次翻转横向偏移或世界速度；选择倒车方向段并提供负的带符号纵向速度即可。 -参考航向的插值采用未归一化航向；只有公开的 vehicle yaw 被归一化到 `[-PI, PI)`。 -投影只检查请求的同一方向段和 S 区间：它对线段候选做夹紧投影，按距离、种子 S -距离和较小 S 的顺序确定性地破平局，绝不搜索另一方向段或另一条回环分支。 +## 最小调用示例（Minimal Call Example） -## 精确边界与静态走廊 - -换向配对点可拥有相同的世界位姿和源弧长，但仍有不同身份：前一段末端是 -`GearSwitchApproach`，下一段起点是 `GearSwitchDeparture`。边界身份是 -`(SegmentIndex, SegmentLocalS, BoundaryType)`，不按世界坐标去重。 - -静态走廊以精确请求的起止 `ReferenceS` 为首末站，并在中间按配置采样。每站从 -上一条有效轨迹的 Frenet 种子开始；没有种子时才使用 `l=0`。候选横向位置包含精确 -种子和偏移限值。距离场仅用于明显安全的快速接受；其余候选使用旋转矩形 -`FootprintCollisionChecker` 和额外净空复核。仅选择包含种子的自由区间，并要求相邻 -站的选择区间重叠。若种子连通区消失，构建失败，绝不跳到障碍物另一侧。 - -## Foundation 配置单位 - -| 配置组 | 关键单位和初值 | -| --- | --- | -| Corridor | 纵向间距 `0.10 m`、横向间距 `0.025 m`、最大偏移 `0.30 m`、额外净空 `0.02 m` | -| Frenet | 最大投影距离 `0.50 m`、最小分母 `0.20`、边界锚点容差 `1e-8 m` | -| Scheduling | 重规划周期 `0.20 s`、时间窗 `6.0 s`、距离窗 `5.0 m` | -| Longitudinal | 前进/倒车最大速度各 `0.20 m/s`；加速度 `0.20 m/s²`；减速度 `0.30 m/s²`；jerk `0.50 m/s³` | - -请求验证会复制配置快照并拒绝非有限值、非法采样间距、不可用地图、不可消费的平滑结果 -和不支持的运动模型。 - -## 可重复 Foundation 验证 - -在仓库根目录运行： - -```powershell -dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- all-foundation -``` - -成功时输出严格为： - -```text -PASS foundation -PASS segmentation -PASS frenet -PASS corridor -``` - -## OSQP 后端部署与诊断 - -横向与纵向规划器只能依赖 `IQpSolver`、`QuadraticProgram`、`QpSolverSettings` 和 -`QpSolveResult`；它们不得依赖 OSQP 的 P/Invoke 类型。当前后端固定为 OSQP `v1.0.0`， -Windows x64、double precision、int32 索引、未打包 settings 和内置 QDLDL algebra。源包布局为： - -```text -ClumsyPilot/ThirdParty/OSQP/ -├── build-win-x64.ps1 -├── LICENSE -├── NOTICE -├── VERSION -├── SHA256SUMS -└── win-x64/osqp.dll -``` - -`VERSION` 记录且构建脚本强制使用以下开关： - -```text -OSQP_ALGEBRA_BACKEND=builtin -OSQP_BUILD_SHARED_LIB=ON -OSQP_BUILD_STATIC_LIB=OFF -OSQP_BUILD_DEMO_EXE=OFF -OSQP_BUILD_UNITTESTS=OFF -OSQP_USE_FLOAT=OFF -OSQP_USE_LONG=OFF -OSQP_PACK_SETTINGS=OFF -OSQP_ENABLE_PRINTING=OFF -OSQP_CODEGEN=OFF -OSQP_ENABLE_DERIVATIVES=OFF -``` - -最终插件发布布局（由后续插件打包阶段负责复制）必须为： - -```text -plugins/ -├── ClumsyPilot.dll -├── osqp.dll -└── licenses/ - ├── OSQP-LICENSE.txt - └── OSQP-NOTICE.txt -``` - -`OsqpNativeLoader` 只从 `typeof(OsqpNativeLoader).Assembly.Location` 所在目录以绝对路径 -预加载同级 `osqp.dll`，不读取当前工作目录，也不要求用户修改 `PATH`。它拒绝非 x64 -进程、缺失或无法加载的 DLL、缺少导出及非 `1.0.0` 版本，并以结构化 -`SolverUnavailable`/`NativeError` 结果返回；不会让宿主进程因加载失败而崩溃。所有 C API -委托均采用 Cdecl，原生 verbose 强制关闭。 - -每次 `OsqpNativeSolver.Solve` 独立拥有 P/Q/A/l/u 和可选 warm-start 的固定数组、CSC -包装、settings 和 OSQP workspace。它在 `finally` 中按反向顺序释放：先 `osqp_cleanup` -workspace，再释放 settings/CSC 块，最后释放托管数组 pin；加载后的模块句柄保留至进程生命周期结束。 - -原生状态映射固定如下： - -| OSQP status value | `QpSolveStatus` | -| ---: | --- | -| 1 | `Solved` | -| 2 | `SolvedInaccurate` | -| 3, 4 | `PrimalInfeasible` | -| 5, 6 | `DualInfeasible` | -| 7 | `MaximumIterations` | -| 8 | `TimeLimit` | -| 9, 10, 11 或未知值 | `NativeError` | - -`SolvedInaccurate` 只在后续规划器完成更严格的独立残差和领域复核后才可发布。调用前取消映射 -为 `Cancelled`；加载失败映射为 `SolverUnavailable`；OSQP setup、warm-start 或 solve API -返回错误时映射为 `NativeError`。 - -在仓库根目录运行 OSQP 回归门： - -```powershell -dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- optimization -dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- osqp -``` - -`osqp` 会在临时的干净插件目录中验证缺失 DLL、损坏 DLL、绝对路径加载、并发首次加载，以及 -有界最优、等式最优、不可行和极短时限 QP。成功时包含： - -```text -PASS osqp-solve -PASS osqp-loader -``` - -## LS 横向 SQP - -LS 只对单个 `DirectionSegmentView` 的 `ReferenceS` 站点求解；`l > 0` 在前进和倒车时 -都表示行驶方向左侧。每个有 `N` 个站点的问题使用连续区间的变量布局： - -```text -l[0..N-1], dl[0..N-1], ddl[0..N-1], dddl[0..N-2] -``` - -相邻站点之间按实际 `ds = ReferenceS[i+1]-ReferenceS[i]` 精确满足三阶积分关系： - -```text -ddl[i+1] = ddl[i] + ds*dddl[i] -dl[i+1] = dl[i] + ds*ddl[i] + 0.5*ds^2*dddl[i] -l[i+1] = l[i] + ds*dl[i] + 0.5*ds^2*ddl[i] + ds^3*dddl[i]/6 -``` - -每轮 QP 将以下项目作为硬约束：静态走廊、最大横向偏移、以当前迭代为中心且半径不超过 -`0.05 m` 的 `l` 信赖域、`1-referenceK*l >= 0.20`、`dl`/`ddl`/`dddl` 上限、起始 -`l` 与 `dl`，以及线性化的车辆曲率约束。`Goal` 和 `GearSwitch` 末端额外强制 -`l_end=0`、`dl_end=0`；`RollingSafetyStop` 不添加这两个等式，而是使用软终端回归代价。 - -目标函数采用 OSQP 的 `0.5*x'P*x + q'x` 形式。所有平方残差先除以相应物理尺度的平方， -再乘权重：`l` 使用最大横向偏移，`dl`、`ddl`、`dddl` 分别使用对应导数上限，曲率使用 -车辆最大曲率，曲率变化使用 `max(1, max |dk/ds|)`。代价覆盖参考线、航向、二阶导、三阶导、 -线性化曲率、曲率变化、上一轨迹种子及滚动终端；走廊安全绝不软化为代价。 - -`SequentialConvexOptimizer` 最多运行五轮，通过 `IQpSolver` 取得完整 primal 向量并将它用作 -下一轮 warm start。每个解都先由完整 Frenet 公式重建为世界坐标，再由独立验证器复核走廊、 -起点/终端、导数、分母、曲率、有限值和严格递增弧长。只有该复核通过的深拷贝候选才能保留。 -后续超时、取消或失败不会发布未验证的最后求解器向量：若已有候选则返回 -`SuccessWithFallback`，否则返回最具体的失败状态。 - -`ReferenceS` 是 LS 的独立变量，不能被当作行驶距离。重建后以世界坐标相邻弦长重新累计 -`PathS`，因此输出 `PathS[0]=0` 且严格递增；这个实际几何 `PathS` 才是后续纵向规划可消费 -的距离契约。 - -可重复横向验证： - -```powershell -dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-all -``` - -该门禁依次验证 LS 模型、脚本化 SQP 状态机，以及在干净复制 plugin bundle 中运行的真实 OSQP -固定场景：前进/倒车直线、缓弯、静态障碍收窄的种子连通走廊、换向终端和滚动终端。每个真实 -场景运行两次，状态、点数和全部数值输出必须在 `1e-10` 内一致。 - -## ST、完整轨迹与单次服务 - -ST 只消费 LS 已复核的实际 `PathS`，而不把 `ReferenceS` 当作行驶距离。它在固定时间 knot 上 -求解非负进度速度 `u`、加速度和 jerk，并在所有终端硬约束 `PathS=terminalPathS`、`u=0`。 -速度包络取方向限速、横向加速度、曲率率和停车包络中的最小值。成功的轨迹始终包含精确零速终端， -随后按 `0.05 s` 间隔提供 `0.20 s` 的同姿态、零速度 hold tail。 - -公开门面仅提供单次、同步且可取消的调用： +调用方必须先在模块边界外获取并冻结平滑路径、同版本地图、车辆参数和车辆状态。下例中这些对象均已准备好： ```csharp -EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken); -``` +using System; +using System.Threading; +using MultiWheelC.TrajectoryPlanning.EMPlanner; -`EmPlanningService` 不负责周期调度、版本淘汰、轨迹执行、换向状态机、控制适配、UI 或硬件读取。 -它只使用请求提供的 `RequestedAtUtc` / `EffectiveAtUtc`，绝不读取系统时钟或当前工作目录。处理顺序固定为： +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); -```text -request/config validation --> direction-segment selection --> bounded ego projection --> PlanningHorizonSelector exact terminal --> previous-trajectory seed projection --> static connected corridor --> LS optimization and validation --> PathS speed envelope --> ST optimization and validation --> immutable trajectory assembly --> world-space publication validation --> immutable result publication -``` - -### 请求快照 - -`EmPlanningRequest` 的构造参数依次为：已发布的 `PathSmoothingResult`、同版本的 -`PlanningGridMap`、`VehicleParameters`、不可变 `VehicleMotionState`、`EmPlannerConfiguration`、 -当前 `SegmentIndex`、可选 `PreviousTrajectory`、`RequestedAtUtc`、`EffectiveAtUtc`、输出轨迹 ID、 -参考路径 ID、上一轨迹 ID，以及 `EmMotionModel.NonholonomicForwardReverse`。状态快照中的正带符号 -速度表示前进，负值表示倒车；绝对值不大于 `StopSpeedToleranceMetersPerSecond` 时按停车处理。 - -服务会复制配置和各规划输入，不修改请求所属对象或列表。结果诊断（`FailureReason`）总是以以下稳定 -标识开始，方便调用方审计版本绑定： - -```text -map=;reference=;state=; -previous=;segment= -``` - -### 结果、字段和单位 - -`EmPlanningResult` 只有 `Success` 或 `SuccessWithFallback` 时才携带不可变 `EmTrajectory`；所有失败状态 -都携带空轨迹。`EmTrajectory.Metadata` 包含轨迹 ID、生成/生效时间、地图 ID、参考路径 ID、状态序列、 -上一轨迹 ID、方向段、方向和终端类型。每个公开 `EmTrajectoryPoint` 字段如下： - -| 字段 | 单位 / 符号 | -| --- | --- | -| `X`, `Y` | 世界坐标 m | -| `Yaw` | 车辆车头世界航向 rad，归一化到 `[-PI, PI)` | -| `SignedLongitudinalVelocity` | 车体纵向 m/s；前进为正，倒车为负；权威速度字段 | -| `Speed` | `abs(SignedLongitudinalVelocity)`，m/s，非负 | -| `VelocityX`, `VelocityY` | 世界速度 m/s；分别等于 `signedV*cos(Yaw)`、`signedV*sin(Yaw)` | -| `YawRate` | rad/s，逆时针为正；等于 `signedV*VehicleCurvature` | -| `TimeFromStart` | 从本条轨迹生效时刻起的 s，严格递增 | -| `VehicleCurvature` | 车辆曲率 `1/m`；倒车时已按车头 yaw 符号转换 | -| `SegmentIndex`, `SegmentLocalS`, `PathS` | 当前方向段标识与局部实际进度 m；`PathS` 不递减 | -| `Direction`, `BoundaryType` | `Forward` / `Reverse` 与 `None`、`RollingSafetyStop`、`GearSwitchApproach` 或 `Goal` | - -冗余速度字段不可独立赋值；组装器从权威 `signedV` 和 `VehicleCurvature` 派生它们，发布前 -`EmTrajectoryValidator` 再独立复算。验证器还会重算有限差分加速度、jerk、曲率率，逐点调用完整 -车体姿态检查，并以最大 `0.025 m` 中心步长检查每个相邻点的扫掠运动。 - -终端类型固定为： - -| `EmTerminalType` | 含义 | -| --- | --- | -| `RollingSafetyStop` | 当前规划窗口未到分段边界时的安全停车终端 | -| `GearSwitch` | 当前方向段末端的精确停车；执行层随后拥有换向状态机 | -| `Goal` | 最后方向段末端的精确停车 | - -状态为 `Success`、`SuccessWithFallback`、`InvalidInput`、`UnsupportedMotionMode`、`StaleVehicleState`、 -`StateDirectionMismatch`、`InvalidReferencePath`、`ProjectionFailed`、`CorridorInfeasible`、 -`LateralInfeasible`、`LongitudinalInfeasible`、`StoppingDistanceInsufficient`、`SolverUnavailable`、 -`SolverTimedOut`、`Cancelled`、`ValidationFailed`、`Superseded` 和 `Failed`。除前两项外，全部状态 -均发布空轨迹和确定性诊断。 - -### 最小调用示例 - -调用方先在规划边界外获取地图、平滑路径和车辆状态快照；下面的对象均为该步骤已经准备好的不可变输入： - -```csharp -var service = new EmPlanningService(qpSolver); -var forwardRequest = new EmPlanningRequest( - publishedSmoothingResult, planningMap, vehicle, forwardState, configuration, +var request = new EmPlanningRequest( + publishedSmoothingResult, planningMap, vehicle, state, configuration, segmentIndex: 0, previousTrajectory: null, - requestedAtUtc: capturedRequestTime, effectiveAtUtc: effectiveTime, - outputTrajectoryId: "traj-100", referencePathId: "path-17", previousTrajectoryId: "", + requestedAtUtc: capturedAt, effectiveAtUtc: capturedAt, + outputTrajectoryId: "trajectory-42", referencePathId: "path-17", previousTrajectoryId: "", motionModel: EmMotionModel.NonholonomicForwardReverse); -EmPlanningResult forward = service.Plan(forwardRequest, cancellationToken); +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)，并由调用方管理状态捕获和周期。 -```csharp -var reverseState = new VehicleMotionState(reversePose, -0.05d, null, capturedRequestTime, sequenceId: 44); -var reverseRequest = new EmPlanningRequest( - publishedSmoothingResult, planningMap, vehicle, reverseState, configuration, - segmentIndex: 1, previousTrajectory: forward.Trajectory, - requestedAtUtc: capturedRequestTime, effectiveAtUtc: effectiveTime, - outputTrajectoryId: "traj-101", referencePathId: "path-17", previousTrajectoryId: "traj-100", - motionModel: EmMotionModel.NonholonomicForwardReverse); +## 详细使用指南（Detailed Usage Guide） -EmPlanningResult reverse = service.Plan(reverseRequest, cancellationToken); -``` +### 第 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` 入口；所有命令都必须从仓库根目录运行。 -```text -PASS longitudinal-model -PASS longitudinal-integration -PASS trajectory -PASS em-planning-service -``` +## Windows x64 插件发布（Windows x64 Plugin Package） -## Rolling execution ownership and deployment - -The first-version boundary has three independently testable layers: - -```text -caller snapshots -> EmPlanningCoordinator -> immutable published EmTrajectory - -> TrajectoryExecutor -> TrajectoryControlCommand -``` - -- `EmPlanningService` remains a pure, synchronous, one-shot planner. It consumes only the request snapshot and never - reads a clock, current directory, UI, localization, wheel speed, or hardware object. -- The caller owns state capture, map/reference-path version selection, the replan clock, and any hardware-specific - action after it receives a generic command. `IVehicleStateProvider.Capture()` belongs to this execution boundary and - returns a `VehicleMotionState` snapshot; it is not a planner dependency. -- `EmPlanningCoordinator` owns latest-wins cycle cancellation and atomic publication. A cycle binds - `MapSnapshotId`, `ReferencePathId`, vehicle-state `SequenceId`, `PreviousTrajectoryId`, and `SegmentIndex`; a - result publishes only when that complete identity and its version are still current. The default update cadence is - `0.20 s` (with the configured `6.0 s` / `5.0 m` planning horizons). -- `TrajectoryExecutor` only samples the immutable published trajectory with caller-provided time and measured state. - It never extrapolates beyond the final point. A failed replan leaves the last complete published trajectory in - service through its exact zero-speed safety tail. - -### Handoff and gear changes - -A normal replan may use a future sample from the prior trajectory only when it is within the configured age and -position/yaw/speed tracking tolerances, remains in the same segment and direction, and does not cross a gear boundary. -Unsafe tracking, stale data, a terminal boundary, or any segment/direction mismatch causes a deterministic reset to -the caller-supplied measured state with no trajectory seed. - -At an exact `GearSwitchApproach` boundary the executor follows this sequence: - -```text -Following -> ApproachingGearSwitch -> HoldingZero --> RequestingDirectionChange (one request) -> AwaitingDirectionConfirmation -> Following -``` - -Measured absolute speed must remain below `0.01 m/s` continuously for `0.20 s` before the single direction-change -request. Every holding, direction-confirmation, rolling-stop, and goal-completion command is zero speed and zero yaw -rate. `Goal` and `RollingSafetyStop` leave the executor completed while braking is held. - -### Trajectory telemetry and generic command - -Every `EmTrajectoryPoint` retains these fields for execution telemetry and independent validation: - -```text -X, Y, Yaw, -SignedLongitudinalVelocity, Speed, VelocityX, VelocityY, YawRate, VehicleCurvature, -TimeFromStart, -SegmentIndex, SegmentLocalS, PathS, Direction, BoundaryType, -LongitudinalAcceleration, LongitudinalJerk -``` - -`SignedLongitudinalVelocity` is authoritative: `Speed = abs(signedV)`, world `VelocityX/Y` are derived from vehicle -yaw, and `YawRate = signedV * VehicleCurvature`. Pose, world velocity, speed, and curvature stay available in -`TrajectoryExecutionState.SelectedPoint`; they are monitoring telemetry, not controller inputs. - -`TrajectoryControlAdapter` produces only the controller-neutral immutable command below: - -```text -SignedLongitudinalVelocity -YawRate -Direction -RequestDirectionChange -HoldBrake -IsTrajectoryComplete -``` - -There is intentionally no body-lateral-velocity, crab-motion, in-place-rotation, UI, or hardware field. A later -hardware adapter may map this command only after that controller's field semantics are independently confirmed. - -### Windows x64 plugin package - -The build output carries the pinned OSQP runtime and notices. Create the deployable plugin tree from a built managed -assembly with an explicit destination that is neither a drive root nor the repository root: +构建后的插件需将托管程序集、固定 OSQP 运行时和许可证一起部署。使用已构建的 `ClumsyPilot.dll` 与显式的非仓库根目录目标： ```powershell & .\ClumsyPilot\scripts\Publish-ClumsyPilotPlugin.ps1 ` @@ -425,23 +200,40 @@ assembly with an explicit destination that is neither a drive root nor the repos -OutputDirectory C:\deploy\ParkingRobot ``` -The transactional publisher verifies a 64-bit PowerShell host, the OSQP PE machine type, and the pinned -`ThirdParty/OSQP/SHA256SUMS` hash before staging and renaming exactly: +发布树固定为： ```text -plugins/ClumsyPilot.dll -plugins/osqp.dll -plugins/licenses/OSQP-LICENSE.txt -plugins/licenses/OSQP-NOTICE.txt -plugins/licenses/OSQP-VERSION.txt +plugins/ +├── ClumsyPilot.dll +├── osqp.dll +└── licenses/ + ├── OSQP-LICENSE.txt + ├── OSQP-NOTICE.txt + └── OSQP-VERSION.txt ``` -Run the complete first-version gate from the repository root: +发布器会检查 64 位 PowerShell、原生 DLL 的 x64 PE 类型和固定 SHA-256。OSQP 加载器从托管程序集目录以绝对路径加载同级 `osqp.dll`；它不依赖 `PATH` 或当前工作目录。部署和执行职责的更多说明见 [TrajectoryExecution](../TrajectoryExecution/README.md)。 -```powershell -dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all -``` +## 常见错误（Common Errors） -Dynamic-obstacle prediction, time-space occupancy, following/yielding/overtaking behavior, dynamic rerouting, -body-lateral motion, in-place rotation, UI integration, and hardware integration are explicitly deferred and are not -implemented by this first version. +| 现象 | 原因 | 处理 | +| --- | --- | --- | +| 倒车横向偏移或世界速度方向反了 | 又按车头方向翻转了 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 的成功结果是经物理与碰撞复核的时间轨迹输入，而不是可直接下发给真实车辆的硬件命令。