2026-08-03 22:52:02 +08:00
|
|
|
|
# EM Planner Foundation
|
|
|
|
|
|
|
|
|
|
|
|
## 当前范围
|
|
|
|
|
|
|
|
|
|
|
|
EM Planner 位于既有的空间路径之后：
|
|
|
|
|
|
|
|
|
|
|
|
```text
|
|
|
|
|
|
PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing -> EM Planner -> TrajectoryExecution
|
|
|
|
|
|
```
|
|
|
|
|
|
|
|
|
|
|
|
本 Foundation 只消费一个已验证的 `PathSmoothingResult` 方向段、同版本的
|
|
|
|
|
|
`PlanningGridMap`、车辆几何和请求快照。它不读取硬件、UI、时钟或当前工作目录。
|
|
|
|
|
|
每次计算只处理一个 `DirectionSegmentView`，因此不会跨越换向边界。
|
|
|
|
|
|
|
2026-08-04 00:22:36 +08:00
|
|
|
|
此门禁已提供 solver-neutral `IQpSolver` 契约和固定 OSQP 后端；尚未实现 LS 横向优化、
|
|
|
|
|
|
ST 纵向优化、滚动协调、轨迹发布或动态障碍物。
|
2026-08-03 22:52:02 +08:00
|
|
|
|
|
|
|
|
|
|
## 坐标与符号
|
|
|
|
|
|
|
|
|
|
|
|
世界长度使用 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
|
|
|
|
|
|
```
|
2026-08-04 00:22:36 +08:00
|
|
|
|
|
|
|
|
|
|
## 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
|
|
|
|
|
|
```
|
