# 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 次调用的紧凑字段，不输出完整向量。
- 粗路径和负例调整可能掩盖纵向缺陷：保留原失败证据，并在独立测试中验证各自阶段。