# 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 调用；
- 规划完成到控制启动之间是否存在额外软件延迟。

只有上述问题被数据回答且安全验收通过，才进入后续闭环部署。