docs: design 5.5m EM speed diagnosis

2026-08-11 16:40:29 +08:00
parent b688555461
commit 19c05f221c
1 changed files with 302 additions and 0 deletions
@@ -0,0 +1,302 @@
+# EM 5.5 m 场地速度诊断与连续可行性修复设计
+
+日期：2026-08-11
+
+状态：总体设计已获用户口头确认，等待书面规格复核
+
+## 1. 背景与已知证据
+
+当前离线 EM 纵向规划存在三类可区分的问题：
+
+1. 原默认硬边界较保守：最大加速度 `0.20 m/s²`、最大减速度 `0.30 m/s²`、最大 jerk `0.50 m/s³`。在约 4 m 的短直线内，车辆大部分时间必然处于加速或停车阶段。
+2. `1.0 m/s` 期望速度已经正确进入速度调度和速度包络。真实 OSQP 的 15 m 直线全尺度候选可达到约 `0.902 m/s`，但常 jerk 区间内部出现约 `-4.94e-5 m/s` 的短暂负速度；连续运动学复核拒绝该候选后，优化器缩小信赖域并最终发布约 `0.893 m/s` 的严格锚点。
+3. 单矩形场景中，同类连续负速度拒绝会触发额外 QP。成功运行在五秒周期截止时间前仅剩约 `15–19 ms`，普通 bootstrap 墙钟波动即可使安全发布失败。
+
+因此，速度慢不是底盘速度上限未修改，也不是 `1.0 m/s` 参数未传入。短场地硬边界、目标函数形成的平顺偏好、以及离散 QP 与连续发布合同之间的缺口需要分别测量。
+
+## 2. 目标
+
+1. 在真实 `5.5 m × 5.5 m` 有界场地中建立直线和全程前进绕障离线场景。
+2. 将最终目标动力学边界设为：
+   - 最大前向速度与期望前向速度：`1.00 m/s`；
+   - 最大加速度：`0.45 m/s²`；
+   - 最大减速度：`0.55 m/s²`；
+   - 最大 jerk：`0.65 m/s³`；
+   - 最大横向加速度：`0.35 m/s²`。
+3. 逐层区分参考包络、seed、QP 原始候选、严格复核候选和最终发布轨迹的速度损失。
+4. 不通过滤波、钳位或放宽容差掩盖连续负速度；为失败区间添加与现有 QP 兼容的自适应连续速度约束。
+5. 给出起步、巡航、制动、求解残差、QP 调用和各规划模块耗时的可重复 CSV 与绘图证据。
+6. 离线验收完成后，为 MovementTest 增加规划到控制交接的时间戳；本设计不启动实车闭环。
+
+## 3. 非目标与边界
+
+- 不修改 `Control`、`Shared`、状态估计、底盘、硬件或合作方控制器实现。
+- 不增加五秒端到端规划预算，不增加 12 次 QP 调用上限，不放宽 `1e-5` 运动学复核容差，不移除严格 QP 审计、连续运动学复核或 jerk 限制停车复核。
+- 不在发布阶段独立滤波或钳位速度数组。
+- 不把弯道未达到 `0.90 m/s` 直接判为失败；弯道速度必须与曲率及 `0.35 m/s²` 横向加速度包络比较。
+- 本轮保持纵向目标权重、最大曲率、曲率变化率和零速保持时间不变。只有证据证明 QP 原始解本身未利用新硬边界时，才另立单变量权重设计。
+
+## 4. 5.5 m 场地与场景
+
+### 4.1 坐标和边界
+
+内部可行区域定义为：
+
+```text
+x ∈ [0.0, 5.5] m
+y ∈ [-2.75, 2.75] m
+```
+
+用厚 `0.20 m` 的矩形障碍表示四面墙：
+
+- 左墙：`x=[-0.20,0.00]`，`y=[-2.75,2.75]`；
+- 右墙：`x=[5.50,5.70]`，`y=[-2.75,2.75]`；
+- 下墙：`x=[0.00,5.50]`，`y=[-2.95,-2.75]`；
+- 上墙：`x=[0.00,5.50]`，`y=[2.75,2.95]`。
+
+车辆尺寸保持 `0.80 m × 0.60 m`，安全余量保持 `0.05 m`。所有场景只允许前进，起点和终点速度、加速度均为零。
+
+### 4.2 场景矩阵
+
+1. `square-straight`：起点 `(0.75,0.00,0)`，终点 `(4.75,0.00,0)`，无内部障碍。隔离起步、巡航和停车。
+2. `square-single-circle`：同一起终点；圆障碍中心 `(2.75,0.00)`，半径 `0.40 m`。观察单次绕障减速与重新加速。
+3. `square-single-rectangle`：同一起终点；矩形障碍 `x=[2.40,3.10]`、`y=[-0.65,0.65]`。复现连续复核、信赖域和 QP 耗时问题。
+4. `square-staggered-double-circle`：同一起终点；圆障碍 `(2.00,-0.55,r=0.34)` 与 `(3.50,0.55,r=0.34)`。观察连续弯道中的速度收放。
+
+若粗路径证明某个几何场景在现有车辆外形、最大曲率和安全余量下物理不可行，只允许调整障碍几何并记录原因，不允许通过缩小车辆或安全余量使其通过。
+
+## 5. 参数因果矩阵
+
+每个场景运行四组配置，其余配置完全相同：
+
+| 标签 | 加速度 m/s² | 减速度 m/s² | jerk m/s³ | 横向加速度 m/s² |
+|---|---:|---:|---:|---:|
+| `original` | 0.20 | 0.30 | 0.50 | 0.20 |
+| `longitudinal-bounds` | 0.45 | 0.55 | 0.50 | 0.20 |
+| `longitudinal-jerk` | 0.45 | 0.55 | 0.65 | 0.20 |
+| `final-candidate` | 0.45 | 0.55 | 0.65 | 0.35 |
+
+每个场景/配置组合在一个已预热进程内重复三次；`final-candidate` 的四个场景各增加一次新进程冷启动。预期总计 52 个场景运行。
+
+最终目标参数只有在全部硬安全验收通过后才写入默认 EM 配置。若验收失败，参数仍保留为离线覆盖值，并记录阻断证据。
+
+## 6. 分层诊断数据流
+
+每次规划必须记录以下速度层次：
+
+```text
+参考速度与物理速度包络
+        ↓
+严格初始 seed
+        ↓
+QP 原始候选
+        ↓
+QP 独立残差审计
+        ↓
+连续运动学与停车复核后的候选
+        ↓
+最终组装和发布轨迹
+```
+
+判别规则：
+
+- 参考速度或包络低：归因于曲率、横向加速度或停车边界；
+- seed 慢而 QP 原始候选快：seed 不是最终瓶颈；
+- QP 原始候选慢：进入目标权重、调度或硬约束调查；
+- QP 原始候选快但严格候选慢：归因于残差审计、连续复核、停车复核或信赖域退回；
+- 严格候选快但发布轨迹慢：归因于组装、采样或发布边界。
+
+## 7. 诊断与导出结构
+
+### 7.1 轨迹性能指标
+
+每个已发布轨迹记录：
+
+- 到达 `0.20/0.50/0.80 m/s` 的首个时间和 PathS；
+- 峰值、平均运动速度、终点速度；
+- 加速度开始、达到峰值和退出峰值的时间；
+- 制动开始时间/PathS、从 `0.50 m/s` 到零的时间、停车距离；
+- 速度高于 `0.80 m/s` 的时间和距离占比；
+- 最大纵向加速度、减速度、jerk、横向加速度、曲率和曲率变化率。
+
+### 7.2 QP 尝试指标
+
+每次 QP 调用记录：
+
+- 场景、参数标签、重复编号；
+- 调用编号、信赖域尺度、普通/高精度阶段、是否同一 QP 重试；
+- 预算、实际耗时、迭代次数、原始 primal/dual residual；
+- 独立审计最坏行、行类别和残差；
+- 原始候选峰值与目标值；
+- 连续复核种类、区间、局部时间、`S/U/A/J` 和回退量；
+- 停车边界、停车距离和带符号 margin；
+- 是否晋升为严格锚点及拒绝原因。
+
+### 7.3 规划阶段耗时
+
+使用单调时钟记录：
+
+- 状态/地图快照；
+- 粗路径；
+- 平滑；
+- 方向段选择与投影；
+- corridor；
+- 横向优化；
+- 速度包络和纵向调度；
+- 每次纵向 QP；
+- 严格验证；
+- 轨迹组装；
+- 世界坐标验证；
+- 最终发布授权。
+
+阶段 trace 必须有固定最大容量，不记录整条原始求解器向量，不得显著改变五秒预算。
+
+### 7.4 CSV 与绘图
+
+每个证据目录包含：
+
+- `trajectory-points.csv`；
+- `scenario-summary.csv`；
+- `qp-attempts.csv`；
+- `phase-timings.csv`。
+
+绘图至少包括：
+
+- 四配置速度-时间对比；
+- 加速度-时间对比；
+- jerk-时间对比；
+- 各模块耗时堆叠图；
+- 原始候选峰值、严格候选峰值、发布峰值对比。
+
+未发布场景仅写汇总和失败原因，不生成伪造的零速度曲线。
+
+## 8. 残差与连续可行性判别实验
+
+对已知失败的全尺度 QP 做不发布的影子精度实验。保持几何、目标、约束、warm start 和信赖域完全相同，只改变影子求解的终止精度；每次用精确常 jerk 方程计算连续最低速度。
+
+- 若求解精度收紧后最低速度趋向非负，数值收敛是主要子因；
+- 若最低速度稳定为负，离散节点约束缺少连续条件是主要子因。
+
+影子结果不得发布，不得改变生产容差。当前候选按 jerk 精确重积分后负速度仍存在，因此预期将确认第二种情况，但实施必须以新场景证据为准。
+
+## 9. 自适应连续速度约束
+
+### 9.1 约束形式
+
+连续复核返回结构化 `negative-speed` 失败：区间 `k`、局部时间 `τ`、`S/U/A/J` 和最低速度。下一次新 QP 在相同严格锚点和相同信赖域尺度下增加：
+
+```text
+U(k) + A(k)·τ + 0.5·J(k)·τ² ≥ 2·KinematicTolerance
+```
+
+`τ` 来自上一候选并在新 QP 中固定，因此该约束对决策变量仍是线性行。使用两倍现有容差作为行下界；即使独立 QP 审计允许最多一个容差的负残差，候选仍至少保留一个容差的非负余量。现有 `KinematicTolerance=1e-5` 不变。
+
+### 9.2 活动集流程
+
+1. 求解当前信赖域 QP，并执行现有独立 QP 审计；
+2. 执行连续运动学复核；
+3. 若首个失败为区间内部负速度且 `τ` 严格位于区间内部，添加一条连续速度 cut；
+4. 将其视为新 QP，在同一信赖域尺度重求；普通/高精度同 QP 重试规则保持不变；
+5. 对 `(interval, τ)` 做数值去重；每个失败候选最多增加一条 cut；
+6. 总求解仍受现有 12 次调用和五秒周期截止时间约束；不存在额外无界循环；
+7. 若新 QP 不可行、预算不足、cut 重复或出现其他失败类型，走现有缩窗/严格锚点安全返回逻辑；
+8. 最终候选仍必须通过完整连续复核、停车复核、目标门、组装和世界坐标验证。
+
+端点不连续、非有限值或非负速度以外的 PathS 失败不自动生成该 cut，避免用错误约束修复不同病因。
+
+### 9.3 审计和诊断
+
+新增 QP 行类别 `ContinuousSpeedCut`。独立审计必须验证实际行 residual 不超过现有严格容差。trace 记录 cut 数量、来源区间/时间、行活动值、下界和 slack。
+
+## 10. 起步与停车性能分析
+
+最终配置在无限直线、硬边界饱和且忽略目标平滑偏好时的理论参考为：
+
+- 加速度达到 `0.45 m/s²`：约 `0.692 s`；
+- 达到 `0.20 m/s`：约 `0.79 s`；
+- 达到 `0.50 m/s`：约 `1.46 s`；
+- 达到 `0.80 m/s`：约 `2.12 s`；
+- 达到 `1.00 m/s`：约 `2.91 s`，加速距离约 `1.46 m`；
+- 从 `1.00 m/s` 停车：约 `2.66 s`，停车距离约 `1.33 m`。
+
+这些是硬边界理论参考，不是强迫 QP 必须饱和的控制命令。若 `final-candidate` 的 QP 原始直线结果仍显著慢于参考，才开始目标权重单变量影子实验。第一轮保持 `ReferenceSpeed=10`、`Acceleration=1`、`Jerk=10` 不变。
+
+## 11. 验收条件
+
+### 11.1 所有发布场景的硬安全条件
+
+- 只前进且连续速度不低于现有运动学容差允许范围；
+- PathS 非下降；
+- 无碰撞并处于 5.5 m 场地边界内；
+- 终点速度和加速度为零，零速保持满足现有配置；
+- 速度、纵向加减速度、jerk、横向加速度、曲率和曲率变化率均不越界；
+- QP 调用不超过 12，所有晋升候选通过独立审计和物理复核；
+- 三次热运行及最终冷启动运行均在五秒内安全发布。
+
+### 11.2 直线最终配置性能目标
+
+- 首次达到 `0.20 m/s` 不晚于 `1.0 s`；
+- 首次达到 `0.50 m/s` 不晚于 `1.8 s`；
+- 发布峰值不低于 `0.90 m/s`；
+- 相比 `original`，起步时间和总运动时间有明确改善；
+- 停车过程不出现拖尾、重新加速或负速度。
+
+### 11.3 绕障性能判定
+
+绕障场景不强制峰值达到 `0.90 m/s`。要求发布速度与曲率/横向加速度包络一致，且不存在因连续负速度 cut 之外的未解释退回。最终配置相对原配置的峰值、平均速度、总时间和 QP 耗时均进入比较表。
+
+## 12. MovementTest 时间戳设计
+
+该部分仅在离线验收通过后实现并进行非闭环验证。MovementTest 记录：
+
+- 会话请求和状态采集开始/结束；
+- bootstrap、EM 和发布开始/结束；
+- 轨迹冻结与控制适配开始/结束；
+- 控制任务调用时刻；
+- 控制器首次回调/首次可见控制指令；
+- 最后一条控制指令；
+- 完成、取消或失败；
+- 停止请求和停止完成。
+
+同时打印：
+
+- `planningTotalMs`；
+- `planningToAdapterMs`；
+- `adapterMs`；
+- `planToControllerStartMs`；
+- `controllerStartToFirstCommandMs`；
+- `planToFirstCommandMs`；
+- `controlDurationMs`；
+- `stopLatencyMs`。
+
+时间间隔使用单调时钟；日志可附 UTC 时间用于跨模块对应。只在 MovementTest 调用边界和现有回调中记录，不修改合作方控制器、Shared、状态估计或底盘代码。本设计阶段不启动控制任务和实车闭环。
+
+## 13. 失败处理
+
+- 任一硬安全验证失败：不发布该候选，保留严格锚点或返回明确非发布结果；
+- 周期截止：不发布部分轨迹，保留上一条有效轨迹；离线首次运行无上一条轨迹时保持无轨迹；
+- 粗路径不可行：报告几何与车辆膨胀后的最小间隙，不调整车辆尺寸或安全余量；
+- 参数组未达到性能目标但满足安全：保留 CSV 和 trace，进入单变量原因分析，不将其伪装成安全失败；
+- 影子精度实验或影子 cut 候选不得进入发布流程。
+
+## 14. 测试顺序
+
+1. 先实现 5.5 m 场景、参数矩阵、性能指标和 CSV schema，并记录未修复的 RED 基线；
+2. 增加有界阶段计时与 QP 结构化 trace；
+3. 完成同一 QP 的影子精度实验，确认数值残差与模型缺口的占比；
+4. 用测试先行方式实现自适应连续速度 cut；
+5. 运行全部 52 个场景，导出修复后 CSV 和图；
+6. 只有所有硬安全验收通过后，才将最终参数写入默认 EM 配置；
+7. 最后实现 MovementTest 时间戳并仅做编译/单元/离线交接验证，不启动闭环。
+
+## 15. 预期结果
+
+该设计不假设提高边界一定会让 QP 达到理论最快速度。它要求证据明确回答：
+
+- 起步慢有多少来自硬边界、jerk 和目标平滑偏好；
+- 巡航慢有多少来自曲率/横向包络；
+- 快候选损失是否来自连续复核和信赖域退回；
+- 自适应 cut 是否在不放宽物理条件的情况下保住快候选并减少 QP 调用；
+- 规划完成到控制启动之间是否存在额外软件延迟。
+
+只有上述问题被数据回答且安全验收通过，才进入后续闭环部署。