diff --git a/docs/superpowers/specs/2026-08-11-em-longitudinal-envelope-contract-repair-design.md b/docs/superpowers/specs/2026-08-11-em-longitudinal-envelope-contract-repair-design.md new file mode 100644 index 0000000..e821322 --- /dev/null +++ b/docs/superpowers/specs/2026-08-11-em-longitudinal-envelope-contract-repair-design.md @@ -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 次调用的紧凑字段,不输出完整向量。 +- 粗路径和负例调整可能掩盖纵向缺陷:保留原失败证据,并在独立测试中验证各自阶段。