245 lines
12 KiB
Markdown
245 lines
12 KiB
Markdown
# EM 弯曲路径纵向包络信赖域设计
|
||
|
||
## 状态
|
||
|
||
本设计已经用户确认。它解决静止起步、完整前进方向段在弯曲参考路径上反复退回低速严格种子的问题。
|
||
|
||
## 背景与证据
|
||
|
||
纯本地离线测试复用了 EM 闭环 MovementTest 的规划前半段：粗路径规划、路径平滑、EM 规划和轨迹发布。测试在发布后立即结束，不调用控制器、底盘或硬件。
|
||
|
||
已确认的结果如下：
|
||
|
||
- 15 m 无障碍直线能够发布约 `0.934 m/s` 的轨迹。
|
||
- 单圆、单矩形、双圆和双矩形绕障均能生成只前进参考路径，但纵向规划进入 `SuccessWithFallback`，发布峰值约为 `0.170–0.184 m/s`。
|
||
- 这些绕障路径的最大绝对曲率约为 `0.373–0.414 1/m`，路径中仍有足够长的直线段，因此低峰值不是 `1.0 m/s` 命令上限直接造成的。
|
||
- OSQP 的被拒绝候选曾出现负速度、真实 PathS 速度包络超限，以及终点附近的运动状态与线性化位置不一致。
|
||
- 当前重线性化会组合被拒绝候选的 `S` 与旧迭代的 `U/A/J`。组合后的状态不再满足精确离散动力学，后续速度包络直线还可能被外推到其原始分段之外，最终使 QP 硬约束失去共同可行解。
|
||
- 三次针对终点吸附、包络兼容和热启动的局部实验均未解决全部绕障场景，实验改动已经撤回。
|
||
- 近起点急弯会被最终 `CurvatureRateLimitExceeded` 校验拒绝。这是有效的安全拒绝，不能通过放宽限制绕过。
|
||
|
||
## 目标
|
||
|
||
- 15 m 直线继续发布接近 `1.0 m/s` 的轨迹，峰值不得低于 `0.90 m/s`。
|
||
- 可行的只前进绕障场景必须稳定发布峰值高于 `0.20 m/s`、不超过 `1.0 m/s` 的轨迹。
|
||
- 消除由包络重线性化自身制造的纵向 QP `primal infeasible` 回退。
|
||
- 每个场景的一次性规划总耗时必须小于 5 秒。
|
||
- 所有发布轨迹继续满足现有速度、加速度、jerk、停车距离、曲率变化率、碰撞和终点语义。
|
||
|
||
## 非目标与改动边界
|
||
|
||
- 不要求绕障弯道达到 `1.0 m/s`；弯道速度仍由真实物理包络决定。
|
||
- 不扩大 `Validation.KinematicTolerance`，不跳过发布校验，不降低横向加速度、曲率变化率或停车约束。
|
||
- 不修改 `Control`、`Shared`、状态估计、底盘接口和实车通信。
|
||
- 不引入新的非线性求解器，不替换 OSQP。
|
||
- 本轮只处理纵向包络迭代；近起点急弯的几何或平滑问题作为独立负向安全场景保留。
|
||
|
||
## 方案选择
|
||
|
||
采用“严格可行锚点 + 分段信赖域”的顺序凸化方案。
|
||
|
||
未选择固定 PathS 的交替优化，因为它容易过度保守且收敛速度不确定。未选择非线性规划或动态规划重写，因为依赖、部署和实时性风险明显更高。
|
||
|
||
## 核心不变量
|
||
|
||
每次交给 OSQP 的 QP 都必须包含上一条严格可行锚点。具体要求如下：
|
||
|
||
1. 锚点的时间节点与本次 QP 布局完全一致。
|
||
2. 锚点代入所有 QP 线性等式和不等式后的最大残差不超过配置的严格残差容差。
|
||
3. 速度包络仿射行只在其原始 PathS 分段内有效；QP 同时对对应 `S` 添加该分段的上下界。
|
||
4. 被拒绝候选不能替换锚点，不能用于下一轮包络线性化，也不能作为下一轮普通热启动。
|
||
5. 只有完整通过严格纵向物理校验的候选才能成为新锚点。
|
||
6. 最终发布仍需通过世界空间轨迹校验。
|
||
|
||
如果 QP 构造后锚点预检失败，则不得调用 OSQP。该情况按规划器内部不变量故障记录诊断，并保留最后一条严格可行轨迹作为回退。
|
||
|
||
## 组件设计
|
||
|
||
### 严格可行锚点
|
||
|
||
`SequentialLongitudinalOptimizer` 保存当前最佳严格候选，而不是分别保存可任意拼接的 `S` 与 `U/A/J`。
|
||
|
||
初始锚点来自现有静止起步严格 seed 或完整方向段可行性投影。初始候选必须先通过 `LongitudinalSolutionValidator`。如果它不能满足新增 QP 锚点预检，则可以作为安全发布回退，但不能用于启动目标优化 QP。
|
||
|
||
每次接受新候选时，完整替换 `S/U/A/J` 快照。禁止从不同候选混合状态分量。
|
||
|
||
### 分段速度包络信赖域
|
||
|
||
新增内部组件 `LongitudinalEnvelopeTrustRegionBuilder`。它消费：
|
||
|
||
- `PathSpeedLimit`；
|
||
- 当前严格锚点；
|
||
- 终点停车保持索引；
|
||
- 当前收缩比例。
|
||
|
||
对每个非起点时间节点，构造器定位锚点 `S` 所在的一个 PathS 速度包络分段，并输出：
|
||
|
||
- 该节点允许的 `S` 下界和上界；
|
||
- 分段内精确的仿射速度约束 `U <= slope * S + intercept`；
|
||
- 锚点包含性诊断。
|
||
|
||
第一版信赖域不得跨越多个速度包络分段。这样仿射约束在整个允许区间内与原始分段线性包络完全一致，不需要依赖无限外推。
|
||
|
||
锚点恰好位于包络站点时：
|
||
|
||
- 终点停车保持节点选择终点侧分段并保持精确 `S/U/A = stop/0/0`；
|
||
- 其他节点根据参考调度相对锚点的前进方向选择相邻分段；
|
||
- 两侧都可用或参考差接近零时，选择可用宽度更大的分段；
|
||
- 无论选择哪一侧，站点处的速度上限相同，锚点必须满足包络行。
|
||
|
||
### 信赖域收缩
|
||
|
||
初始区间为选中包络分段与全局 PathS 边界的交集。失败恢复使用全局收缩比例：
|
||
|
||
- 初始比例 `1.0`；
|
||
- 第一次失败后 `0.5`；
|
||
- 第二次失败后 `0.25`；
|
||
- 第三次失败后 `0.125`；
|
||
- 以锚点为中心收缩，同时保持在原始包络分段内；
|
||
- 任一活动节点的有效区间宽度小于 `0.001 m` 时停止继续收缩。
|
||
|
||
起点、终点停车保持段和其他已有精确边界不参与自由收缩。
|
||
|
||
### QP 约束构造
|
||
|
||
`LongitudinalConstraintBuilder` 接收信赖域快照。现有 `S` 全局边界收紧为逐节点信赖域边界，速度包络行使用同一快照中的斜率和截距。
|
||
|
||
其他约束保持不变：
|
||
|
||
- 精确常 jerk 离散动力学；
|
||
- 初始 `S/U/A`；
|
||
- 速度、加速度和 jerk 上下界；
|
||
- PathS 非递减；
|
||
- 低速减速度释放包络；
|
||
- 终点停车保持段。
|
||
|
||
构造完成后，在求解前以锚点的完整 primal 向量计算所有约束残差。最大残差超过 `Solver.StrictResidualTolerance` 时返回明确的行号、约束类别和物理单位残差，不调用求解器。
|
||
|
||
### 顺序求解状态机
|
||
|
||
一次纵向规划按以下顺序执行：
|
||
|
||
1. 创建并严格验证初始锚点。
|
||
2. 围绕锚点建立分段信赖域和 QP。
|
||
3. 预检锚点是否属于 QP 可行域。
|
||
4. 使用普通配置调用 OSQP。
|
||
5. 对结果执行快速迭代校验。
|
||
6. 快速校验通过后执行严格纵向校验。
|
||
7. 严格校验和目标接受门通过时，完整更新锚点并进入下一外层迭代。
|
||
8. 候选失败时对同一个 QP 高精度重求一次；高精度候选只作为本次重求热启动，不得跨 QP 复用。
|
||
9. 高精度重求仍失败时，回到当前严格锚点并收短信赖域。
|
||
10. 收缩耗尽、共享预算不足、取消或求解器不可用时结束，并按既有语义返回成功、回退或失败状态。
|
||
|
||
保留现有最多 5 次有效外层锚点更新。普通求解、高精度重求和收缩重试合计最多调用 OSQP 12 次，并且每次都只能使用当前剩余共享求解预算。
|
||
|
||
### 候选接受门
|
||
|
||
候选成为锚点前必须同时满足：
|
||
|
||
- 严格纵向物理校验通过；
|
||
- 有限且满足精确终点停车保持；
|
||
- 当前 QP 目标值不劣于锚点目标值。允许的数值差为 `1e-9 * max(1, abs(anchorObjective))`；
|
||
- 未收到取消请求；
|
||
- 总预算尚未耗尽。
|
||
|
||
最大状态变化和相对目标改善均不超过现有严格收敛容差时提前收敛。
|
||
|
||
## 两级校验
|
||
|
||
### 快速迭代校验
|
||
|
||
普通临时候选先检查：
|
||
|
||
- 求解状态和残差是否有限；
|
||
- QP 线性等式和不等式残差；
|
||
- `S/U/A/J` 基本物理范围；
|
||
- 信赖域边界；
|
||
- 时间节点和变量布局。
|
||
|
||
快速校验失败的候选不会进入完整世界空间检查。
|
||
|
||
### 严格锚点与发布校验
|
||
|
||
候选准备成为锚点时，必须执行现有 `LongitudinalSolutionValidator`，检查：
|
||
|
||
- 精确常 jerk 动力学；
|
||
- 初始状态；
|
||
- 速度、加速度、jerk 和 PathS 单调性；
|
||
- 真实 PathS 速度包络；
|
||
- jerk 限制下的逐节点停车余量；
|
||
- 终点 `S/U/A` 停车保持。
|
||
|
||
最终组装轨迹继续执行现有世界空间验证，包括曲率变化率、碰撞、终端位置和终端姿态。严格校验使用现有物理容差，不要求浮点零误差，也不得扩大容差掩盖求解问题。
|
||
|
||
## 结果与诊断语义
|
||
|
||
- 至少一个目标优化候选成为锚点并最终发布时，返回 `Success`。
|
||
- 只有严格初始 seed 能发布时，返回 `SuccessWithFallback`，诊断保留具体阶段、OSQP 状态、信赖域比例、求解次数和最后拒绝原因。
|
||
- 严格初始候选也不存在时，保留现有失败状态。
|
||
- 取消不得伪装为成功回退。
|
||
- 求解器报告不可行但锚点预检通过时，诊断必须标记为求解器/数值异常；锚点预检失败时，诊断必须标记为规划器不变量故障。
|
||
|
||
## 性能预算
|
||
|
||
- 使用 `EmPlanningService` 分配给纵向阶段的剩余共享预算，不创建额外独立 5 秒窗口。
|
||
- 每次 OSQP 调用都使用调用前重新计算的剩余时间。
|
||
- 最多 5 次有效锚点更新、3 级收缩和 12 次 OSQP 调用。
|
||
- 任何限制先到即停止，并返回最后一条严格可行锚点或既有失败状态。
|
||
|
||
## 测试设计
|
||
|
||
### 单元测试
|
||
|
||
- 锚点代入新 QP 后，所有约束最大残差不得超过严格残差容差。
|
||
- 包络仿射行的 `S` 上下界必须处于同一原始 PathS 分段。
|
||
- 包络站点边界选择必须确定且包含锚点。
|
||
- 被拒绝候选不得替换锚点或成为下一 QP 热启动。
|
||
- 普通求解失败后只能高精度重求同一 QP；跨 QP 必须回到严格锚点。
|
||
- 收缩比例依次为 `1.0/0.5/0.25/0.125`，最小宽度为 `0.001 m`。
|
||
- OSQP 调用次数不得超过 12，预算耗尽时不得启动新求解。
|
||
- 目标值变差的候选不得成为新锚点。
|
||
|
||
### 正向离线场景
|
||
|
||
1. 15 m 直线：`Success`，峰值至少 `0.90 m/s`，不出现纵向不可行回退，总耗时小于 5 秒。
|
||
2. 单圆绕障：只前进、成功发布、峰值大于 `0.20 m/s` 且不超过 `1.0 m/s`、终点速度为零、总耗时小于 5 秒。
|
||
3. 单矩形绕障：同上。
|
||
4. 双圆交错绕障：同上。
|
||
5. 双矩形交错绕障：同上。
|
||
6. 新增较缓的近起点绕障：同上。
|
||
|
||
这些场景不得出现由纵向 QP 不可行导致的 `SuccessWithFallback`。如果其他独立阶段产生回退，测试必须通过结构化诊断明确区分。
|
||
|
||
### 负向安全场景
|
||
|
||
保留当前过急的近起点弯道。该场景必须被 `CurvatureRateLimitExceeded` 或更早的等价安全门拒绝，不得为满足速度验收而发布。
|
||
|
||
### 回归与输出
|
||
|
||
执行：
|
||
|
||
- `em-closed-loop-offline`；
|
||
- `em-speed-profile`；
|
||
- `longitudinal-integration`；
|
||
- 现有 EM 闭环 MovementTest 静态检查；
|
||
- 工程编译；
|
||
- `git diff --check`。
|
||
|
||
每个离线场景输出粗路径与平滑耗时、EM 发布时间、总耗时、峰值速度、OSQP 调用次数、信赖域收缩次数、最终状态和回退原因。
|
||
|
||
## 预计文件范围
|
||
|
||
- 新增：`ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalEnvelopeTrustRegion.cs`
|
||
- 修改：`ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`
|
||
- 修改：`ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`
|
||
- 修改：纵向和离线验证宿主中的相关测试文件
|
||
|
||
若实现证明需要修改上述范围之外的规划器生产文件，必须先说明原因并重新确认范围。控制器、`Shared`、状态估计和底盘文件始终不在允许改动范围内。
|
||
|
||
## 风险与控制
|
||
|
||
- 单分段信赖域可能使某些节点每轮只能移动较短距离。通过 5 次有效锚点更新、提前收敛和 12 次硬求解上限控制速度与耗时；达不到验收时先报告证据，不通过扩大安全容差解决。
|
||
- 高精度重求可能增加单轮耗时。它只在普通候选失败后执行一次，并受剩余共享预算约束。
|
||
- 严格锚点预检可能揭示现有 QP 辅助约束与物理验证器不一致。此时应单独修复约束契约，不得绕过预检。
|
||
- 负向急弯场景继续拒绝是预期行为，不与正向绕障速度目标混淆。
|