202 lines
11 KiB
Markdown
202 lines
11 KiB
Markdown
# EM 纵向包络契约二次修复设计
|
||
|
||
## 状态
|
||
|
||
本设计已获用户批准。它是在“严格可行锚点 + 分段信赖域”第一版离线验收失败后形成的二次修复，不替换第一版安全架构，而是修正其分段语义和 QP 辅助约束契约。
|
||
|
||
## 证据与根因
|
||
|
||
两次完整本地离线矩阵稳定复现以下结果：
|
||
|
||
- 15 m 直线发布峰值约 `0.558 m/s`，已接受 5 个锚点、调用 10 次 QP，但未达到 `0.90 m/s`。
|
||
- 单圆场景峰值超过 `0.20 m/s`，但总耗时约 `6.7 s`；最后一次 OSQP 调用约 3.7 万次迭代并耗尽剩余预算。
|
||
- 单矩形场景在调用 OSQP 前回退；严格物理 seed 通过 `LongitudinalSolutionValidator`，却违反 QP 的低速减速度释放辅助行约 `5.14e-4 m/s`。
|
||
- 双矩形场景在 `scale=1` 时因 knot 162 的原始包络区间窄于 `0.001 m` 而停止，峰值约 `0.198 m/s`。
|
||
- 双圆场景在粗路径 Hybrid A* 阶段约 5 秒超时，未进入 EM 或纵向规划。
|
||
- 原紧急弯负例从静止起步时发布的最终轨迹曲率时间变化率约为 `0.410 < 0.500 1/(m*s)`；严格世界校验允许发布是正确行为，原负例假设不成立。
|
||
|
||
速度包络默认按不超过 `0.10 m` 的空间步长采样。第一版信赖域把每对相邻采样站点都当作不可跨越的数学分段，使直线上的同一条水平速度上限也被切成大量小区间。五次锚点更新理想情况下最多跨约 `0.50 m`，而从 `0.558` 加速至 `0.90 m/s`，仅按 `0.20 m/s²` 加速度下限估算就需要约 `1.25 m`。
|
||
|
||
现有 QP 低速辅助行是：
|
||
|
||
```text
|
||
U + D/(2J) * A >= 0
|
||
```
|
||
|
||
当 `A < 0` 时，它要求 `U >= D*|A|/(2J)`。严格物理验证使用的 jerk 释放条件是 `U >= A²/(2J)`。因为 `|A| <= D`，前者是严格更保守的弦线，可能排除真实物理合法状态。这是 QP 辅助约束与严格 validator 的契约冲突，不是容差问题。
|
||
|
||
## 目标
|
||
|
||
- 15 m 直线继续满足峰值 `>= 0.90 m/s`。
|
||
- 五个粗路径可行的前进绕障场景满足峰值 `> 0.20 m/s`、`<= 1.00 m/s`。
|
||
- 每个一次性本地场景总耗时 `< 5 s`。
|
||
- 每个发布场景终点速度绝对值 `<= 1e-12 m/s`，只前进、速度非负、PathS 非递减。
|
||
- 保留最多 5 次接受锚点、最多 12 次 OSQP 调用和共享预算。
|
||
- 不放宽速度、加速度、jerk、停车距离、横向加速度、曲率变化率、碰撞或世界空间发布校验。
|
||
|
||
## 非目标与边界
|
||
|
||
- 不修改 `Control`、`Shared`、状态估计、底盘、硬件通信或实车部署。
|
||
- 不提高生产粗路径或 EM 超时来满足 `< 5 s` 验收。
|
||
- 不放宽 `StrictResidualTolerance`、`Validation.KinematicTolerance` 或信赖域安全审计。
|
||
- 不把物理合法但不满足保守弦线的状态重新定义为不安全。
|
||
- 不用更急的静止起步几何强行制造曲率率负例，因为规划器可以通过主动降速保持物理可行。
|
||
|
||
## 修复一：最大精确共线包络区间
|
||
|
||
`LongitudinalEnvelopeTrustRegionBuilder` 仍从锚点所在的原始 `PathSpeedLimit` 分段开始，但不再无条件把一个采样间隔当作完整信赖域。
|
||
|
||
构造器计算选中分段的仿射线：
|
||
|
||
```text
|
||
Umax(S) = slope * S + intercept
|
||
```
|
||
|
||
然后向左、向右扩展所有与该线精确相同的相邻分段，形成最大精确共线 run。第一版只实现确定性的精确比较：斜率和截距必须逐值相等；水平 plateau 因速度值完全相同而稳定合并。不得用“近似相等”或新增 epsilon 合并真实折点。
|
||
|
||
合并只改变允许的 PathS 搜索区间，不改变 `PathSpeedLimit.MaximumSpeedAt` 的函数值，不修改横向加速度、曲率率、停车速度等原始诊断数组。QP 速度行继续使用同一 run 的仿射线，因此不会发生跨真实折点外推。
|
||
|
||
站点边界选择继续保留确定性规则，但使用既有站点容差判断锚点是否位于站点，避免只有浮点逐值相等时才能选择相邻 run。
|
||
|
||
## 修复二：`0.001 m` 是收缩停止条件
|
||
|
||
`0.001 m` 的设计含义是“不要继续生成更窄的收缩信赖域”，不是“禁止求解原始物理包络中本来就很短的分段”。
|
||
|
||
- `scale=1` 的最大精确共线 run 始终可以构造，只要它有限、包含锚点并位于 PathS 全局边界内。
|
||
- 普通候选或高精度重试失败后，在进入下一收缩比例前计算新宽度。
|
||
- 如果下一比例的任一活动区间会窄于 `0.001 m`，停止继续收缩，保留当前严格锚点。
|
||
- 起点、精确终点保持节点和其他精确固定边界不参与宽度门。
|
||
|
||
这样不会放宽速度包络，只避免把采样生成的亚毫米片段误判成规划器不变量故障。
|
||
|
||
## 修复三：低速 jerk 释放的锚点切线
|
||
|
||
严格物理条件保持：
|
||
|
||
```text
|
||
U >= A²/(2J)
|
||
```
|
||
|
||
OSQP 仍只接收线性约束。对每个 knot 的当前严格锚点加速度 `A0 = min(0, anchor.A[k])`，QP 使用该凸二次函数在 `A0` 处的切线：
|
||
|
||
```text
|
||
U >= (A0/J) * A - A0²/(2J)
|
||
```
|
||
|
||
等价行形式为：
|
||
|
||
```text
|
||
U - (A0/J) * A >= -A0²/(2J)
|
||
```
|
||
|
||
该切线在锚点处与真实物理函数相等，并且对所有 A 位于真实凸函数下方，因此：
|
||
|
||
- 任何满足真实物理条件的严格锚点都不会被 QP 预检排除；
|
||
- QP 可能暂时提出位于切线之上、真实二次边界之下的候选；
|
||
- 这种候选不能成为锚点，因为现有 `LongitudinalSolutionValidator` 仍执行真实二次检查；
|
||
- 候选失败后仍按同 QP 高精度重试和信赖域收缩处理；
|
||
- 不删除锚点 QP 审计，不扩大残差容差。
|
||
|
||
初始可行性投影和普通/可信 QP 都必须使用各自完整线性化候选的 `A0`，不再使用全局最大减速度弦线。严格 validator 是唯一物理权威。
|
||
|
||
## 单圆求解阶段诊断
|
||
|
||
在改变高精度重试行为前，先为每次纵向 OSQP 调用记录：
|
||
|
||
- `phase=projection|normal|retry`；
|
||
- 调用序号、锚点更新序号和 trust scale；
|
||
- 调用前剩余预算和实际耗时；
|
||
- OSQP status、iterations、primal/dual residual；
|
||
- 普通候选拒绝原因；
|
||
- anchor 与 candidate 的目标值。
|
||
|
||
这些字段进入现有纵向诊断字符串，不引入全局状态或硬件日志。
|
||
|
||
先实施共线 run、宽度语义和切线契约，再重跑单圆。如果仍确认固定 `1e-7` retry 吞掉大部分预算，则另行设计 retry 时间片或精度目标；本设计不预先放宽严格接受残差，也不在证据不足时修改重试策略。
|
||
|
||
## 非纵向场景处理
|
||
|
||
### 双圆粗路径
|
||
|
||
双圆 `SearchTimeout` 发生在 EM 之前，必须与纵向验收分开：
|
||
|
||
1. 用完全相同 seed、车辆和地图运行 coarse-only 诊断；
|
||
2. 仅用于诊断时比较 5 秒和延长预算，记录展开/生成节点与终止原因；
|
||
3. 若延长预算成功，证明这是搜索复杂度而不是纵向缺陷；
|
||
4. 为纵向矩阵选择仍有双圆交错绕障、但在生产 5 秒预算内稳定生成粗路径的固定几何；
|
||
5. 不通过提高生产 timeout 让测试通过。
|
||
|
||
### 负向安全场景
|
||
|
||
保留两层安全证据：
|
||
|
||
- 确定性 validator 单元负例：构造 `|DeltaCurvature/DeltaTime| > 0.50` 的轨迹，必须返回 `CurvatureRateLimitExceeded`。
|
||
- 端到端负例：近起点急弯使用非零初始速度，使车辆在 jerk/减速可达范围内无法及时降到曲率率包络以下。允许由 `CurvatureRateLimitExceeded` 或更早的等价安全门拒绝，但不得发布。
|
||
|
||
离线选择固定初始速度时，只测试小集合 `0.20/0.30/0.40 m/s`，冻结能够稳定产生安全拒绝的最低值并记录证据。不得降低 `0.50` 限制或扩大容差。原静止起步紧急弯可以保留为物理合法慢行证据，但不再作为负例。
|
||
|
||
## 求解与数据流
|
||
|
||
```text
|
||
strict anchor
|
||
-> exact-collinear speed-envelope run
|
||
-> scale=1 trusted QP (允许原始短段)
|
||
-> anchor QP audit
|
||
-> normal OSQP
|
||
-> fast QP audit
|
||
-> strict physical validator (包含真实二次 jerk 释放)
|
||
-> same-QP objective gate
|
||
-> promote whole S/U/A/J anchor
|
||
|
||
candidate rejected
|
||
-> one same-QP precision retry
|
||
-> before next scale: check proposed width >= 0.001 m
|
||
-> rebuild only from unchanged strict anchor
|
||
```
|
||
|
||
## 测试设计
|
||
|
||
### 模型测试
|
||
|
||
- 多个水平采样段合并为一个最大 run，仿射速度函数逐站点完全一致。
|
||
- 真实斜率折点不能被合并或跨越。
|
||
- 锚点接近站点容差边界时确定性选择 run。
|
||
- `scale=1` 允许小于 `0.001 m` 的原始短段；下一收缩比例小于门限时停止。
|
||
- 低速切线在锚点处等于 `A0²/(2J)`，严格物理合法 anchor 通过 QP 预检。
|
||
- 位于切线与真实二次曲线之间的候选可以属于 QP，但必须被严格 validator 拒绝。
|
||
- 原全局弦线不再出现在 QP 行中。
|
||
|
||
### 集成测试
|
||
|
||
- 直线速度 profile 继续满足 `>= 0.90 m/s`。
|
||
- 单矩形不再在低速辅助行预检处零调用回退。
|
||
- 双矩形不再因 scale=1 原始亚毫米片段直接失败。
|
||
- 每个诊断能区分 projection、normal 和 retry，并保留总调用上限 12。
|
||
- 仍满足同 QP retry、严格锚点、目标门、取消、预算和世界校验既有测试。
|
||
|
||
### 离线矩阵
|
||
|
||
- 六个正向发布场景和一个真实负向安全场景连续运行两次。
|
||
- 直线 `>=0.90 m/s`；绕障严格 `>0.20 m/s`；所有发布速度 `<=1.00 m/s`。
|
||
- 总耗时 `<5 s`，终点速度绝对值 `<=1e-12 m/s`。
|
||
- 正向场景不得出现 `longitudinalFallback=`，至少一个接受锚点，QP 调用 `<=12`。
|
||
- 负向场景不得发布，必须记录曲率率或等价早期安全拒绝。
|
||
|
||
## 预计文件范围
|
||
|
||
- 修改 `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalEnvelopeTrustRegion.cs`
|
||
- 修改 `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`
|
||
- 修改 `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`（只增加阶段诊断）
|
||
- 修改纵向模型/集成、trajectory validator 和离线场景验证文件
|
||
- 不修改控制、共享、状态估计、底盘和硬件目录
|
||
|
||
若前三项纵向契约修复后单圆仍超过 5 秒，必须先提交阶段诊断证据并进行新的 retry 设计确认，不能在本设计下直接调整 timeout、残差或 OSQP 上限。
|
||
|
||
## 风险与控制
|
||
|
||
- 共线比较过宽会跨真实折点：第一版只允许精确仿射相等，不引入近似合并。
|
||
- 切线会扩大 QP 临时可行域：严格二次 validator 仍是锚点晋升和发布硬门。
|
||
- 亚毫米原始区间可能影响数值条件：只允许 scale=1 原始区间；禁止继续收缩，并依赖预检与求解残差门。
|
||
- 阶段诊断可能增长字符串：只记录最多 12 次调用的紧凑字段，不输出完整向量。
|
||
- 粗路径和负例调整可能掩盖纵向缺陷：保留原失败证据，并在独立测试中验证各自阶段。
|