346 lines
16 KiB
Markdown
346 lines
16 KiB
Markdown
# EM Planner Foundation
|
||
|
||
## 当前范围
|
||
|
||
EM Planner 位于既有的空间路径之后：
|
||
|
||
```text
|
||
PlanningGridMap -> Hybrid A* CoarsePath -> Local G2 PathSmoothing -> EM Planner -> TrajectoryExecution
|
||
```
|
||
|
||
本 Foundation 只消费一个已验证的 `PathSmoothingResult` 方向段、同版本的
|
||
`PlanningGridMap`、车辆几何和请求快照。它不读取硬件、UI、时钟或当前工作目录。
|
||
每次计算只处理一个 `DirectionSegmentView`，因此不会跨越换向边界。
|
||
|
||
此门禁已提供 solver-neutral `IQpSolver` 契约和固定 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
|
||
```
|
||
|
||
## 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);
|
||
```
|
||
|
||
`EmPlanningService` 不负责周期调度、版本淘汰、轨迹执行、换向状态机、控制适配、UI 或硬件读取。
|
||
它只使用请求提供的 `RequestedAtUtc` / `EffectiveAtUtc`，绝不读取系统时钟或当前工作目录。处理顺序固定为：
|
||
|
||
```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=<MapSnapshotId>;reference=<ReferencePathId>;state=<VehicleState.SequenceId>;
|
||
previous=<PreviousTrajectoryId>;segment=<SegmentIndex>
|
||
```
|
||
|
||
### 结果、字段和单位
|
||
|
||
`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,
|
||
segmentIndex: 0, previousTrajectory: null,
|
||
requestedAtUtc: capturedRequestTime, effectiveAtUtc: effectiveTime,
|
||
outputTrajectoryId: "traj-100", referencePathId: "path-17", previousTrajectoryId: "",
|
||
motionModel: EmMotionModel.NonholonomicForwardReverse);
|
||
|
||
EmPlanningResult forward = service.Plan(forwardRequest, cancellationToken);
|
||
```
|
||
|
||
倒车不需要额外翻转横向坐标或世界速度；选择倒车方向段并把状态带符号速度设为负即可：
|
||
|
||
```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);
|
||
|
||
EmPlanningResult reverse = service.Plan(reverseRequest, cancellationToken);
|
||
```
|
||
|
||
在仓库根目录运行完整单次服务门禁：
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||
```
|
||
|
||
成功输出依次为：
|
||
|
||
```text
|
||
PASS longitudinal-model
|
||
PASS longitudinal-integration
|
||
PASS trajectory
|
||
PASS em-planning-service
|
||
```
|