docs: design longitudinal envelope contract repair

2026-08-11 07:38:14 +08:00
parent 34f99526e3
commit dcd6f7d569
1 changed files with 201 additions and 0 deletions
@@ -0,0 +1,201 @@
+# 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 次调用的紧凑字段，不输出完整向量。
+- 粗路径和负例调整可能掩盖纵向缺陷：保留原失败证据，并在独立测试中验证各自阶段。