docs: describe EM planner foundation

2026-08-03 22:52:49 +08:00
parent 2d252ff309
commit aa62d6b227
1 changed files with 84 additions and 0 deletions
@@ -0,0 +1,84 @@
+# EM Planner Foundation
+
+## 当前范围
+
+EM Planner 位于既有的空间路径之后：
+
+```text
+PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing -> EM Planner -> TrajectoryExecution
+```
+
+本 Foundation 只消费一个已验证的 `PathSmoothingResult` 方向段、同版本的
+`PlanningGridMap`、车辆几何和请求快照。它不读取硬件、UI、时钟或当前工作目录。
+每次计算只处理一个 `DirectionSegmentView`，因此不会跨越换向边界。
+
+此门禁尚未实现 OSQP、LS 横向优化、ST 纵向优化、滚动协调、轨迹发布或动态障碍物。
+
+## 坐标与符号
+
+世界长度使用 m，时间使用 s，航向使用 rad，曲率使用 `1/m`。车体 `+x` 指向车头，
+车体 `+y` 指向车体左侧，正航向为逆时针。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)
+```
+
+`ReferenceS` 始终沿实际运动方向递增；`l > 0` 永远代表运动方向左侧。
+例如倒车时车头朝 `-PI + 0.01`，`travelYaw` 为 `0.01`。此时 `l > 0` 在
+运动方向左侧、也就是车体右侧；不得在倒车分支额外翻转 `l`。重建要求
+`1 - referenceK*l >= MinimumFrenetDenominator`，否则拒绝奇异 Frenet 几何。
+
+参考航向的插值采用未归一化航向；只有公开的 vehicle yaw 被归一化到 `[-PI, PI)`。
+投影只检查请求的同一方向段和 S 区间：它对线段候选做夹紧投影，按距离、种子 S
+距离和较小 S 的顺序确定性地破平局，绝不搜索另一方向段或另一条回环分支。
+
+## 精确边界与静态走廊
+
+换向配对点可拥有相同的世界位姿和源弧长，但仍有不同身份：前一段末端是
+`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
+```