11 KiB
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 低速辅助行是：
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 分段开始，但不再无条件把一个采样间隔当作完整信赖域。
构造器计算选中分段的仿射线：
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 释放的锚点切线
严格物理条件保持：
U >= A²/(2J)
OSQP 仍只接收线性约束。对每个 knot 的当前严格锚点加速度 A0 = min(0, anchor.A[k])，QP 使用该凸二次函数在 A0 处的切线：
U >= (A0/J) * A - A0²/(2J)
等价行形式为：
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 之前，必须与纵向验收分开：
- 用完全相同 seed、车辆和地图运行 coarse-only 诊断；
- 仅用于诊断时比较 5 秒和延长预算，记录展开/生成节点与终止原因；
- 若延长预算成功，证明这是搜索复杂度而不是纵向缺陷；
- 为纵向矩阵选择仍有双圆交错绕障、但在生产 5 秒预算内稳定生成粗路径的固定几何；
- 不通过提高生产 timeout 让测试通过。
负向安全场景
保留两层安全证据：
- 确定性 validator 单元负例：构造
|DeltaCurvature/DeltaTime| > 0.50的轨迹，必须返回CurvatureRateLimitExceeded。 - 端到端负例：近起点急弯使用非零初始速度，使车辆在 jerk/减速可达范围内无法及时降到曲率率包络以下。允许由
CurvatureRateLimitExceeded或更早的等价安全门拒绝，但不得发布。
离线选择固定初始速度时，只测试小集合 0.20/0.30/0.40 m/s，冻结能够稳定产生安全拒绝的最低值并记录证据。不得降低 0.50 限制或扩大容差。原静止起步紧急弯可以保留为物理合法慢行证据，但不再作为负例。
求解与数据流
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 次调用的紧凑字段，不输出完整向量。
- 粗路径和负例调整可能掩盖纵向缺陷：保留原失败证据，并在独立测试中验证各自阶段。