303 lines
14 KiB
Markdown
303 lines
14 KiB
Markdown
# 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 调用；
|
||
- 规划完成到控制启动之间是否存在额外软件延迟。
|
||
|
||
只有上述问题被数据回答且安全验收通过，才进入后续闭环部署。
|