2. 速度规划输入输出：数据从哪里来，结果到哪里去
速度规划这个模块，说白了就是“看菜下饭”。你给它什么原料，它给你做出什么菜。我刚开始带项目的时候，有个同事把障碍物轨迹的坐标系搞错了，结果规划出来的速度曲线直接把车往墙上怼。嗯，从那以后，我对输入数据的检查就格外严格。
今天我们就来拆解一下，速度规划到底吃进去什么，又吐出什么。你理解清楚了这个接口，后面写代码才不会跑偏。
2.1 输入数据：三驾马车
速度规划的输入，核心就三样东西：参考线、障碍物轨迹、限速信息。缺一个，规划就没法做。
核心原则：输入数据的质量，直接决定速度规划的上限。垃圾进，垃圾出，这个道理在自动驾驶里尤其残酷。
2.1.1 参考线（Reference Line）
参考线是车辆行驶的“骨架”。它通常来自全局路径规划，经过平滑处理后，变成一条由离散点组成的路径。每个点都包含位置、朝向、曲率等信息。
我个人习惯把参考线理解成“铁轨”。车辆必须沿着这条线走，速度规划的任务就是决定在这条铁轨上什么时候加速、什么时候刹车。
| 参考线属性 | 说明 | 对速度规划的影响 |
|---|---|---|
| 位置 (x, y) | 路径点的地理坐标 | 决定车辆在哪条车道上 |
| 朝向 (theta) | 路径点的航向角 | 影响转弯时的速度限制 |
| 曲率 (kappa) | 路径的弯曲程度 | 曲率越大，过弯速度必须越低 |
| 累积弧长 (s) | 从起点到当前点的路径长度 | 速度规划的“横坐标” |
我的经验：参考线的曲率一定要做滤波。我曾经遇到过原始路径曲率跳变太大，导致速度规划在弯道入口急刹车，乘客差点从座位上飞出去。加一个低通滤波器，世界就和平了。
2.1.2 障碍物轨迹（Obstacle Trajectory）
障碍物轨迹是预测模块给出来的。它告诉我们：周围的车、人、自行车，未来几秒钟会怎么动。
这里要注意，障碍物轨迹不是简单的“一个点”，而是一串带时间戳的状态序列。每个时间点都包含位置、速度、加速度、朝向，甚至还有不确定性方差。
为什么需要这些？你想想看，如果前车在减速，你还在匀速跟车，那肯定要追尾。速度规划必须根据障碍物的未来轨迹，提前做出反应。
- 静态障碍物：轨迹是一条直线（位置不变），速度为零。处理起来相对简单，绕开或停车就行。
- 动态障碍物：轨迹是时空曲线。比如旁边车道有车要加塞，它的轨迹会斜着插到你的车道里。这时候速度规划就得“让”或者“抢”。
避坑指南：我曾经遇到过预测轨迹的时间戳和速度规划的时间戳对不齐的情况。预测模块用的是系统时间，速度规划用的是规划周期时间，差了50毫秒。结果就是，速度规划以为前车还在10米外，实际上已经到5米了。所以，时间同步是输入数据的第一道关。
2.1.3 限速信息（Speed Limit）
限速信息来源很多：高精地图的静态限速、交通标志识别的动态限速、还有因为道路曲率或天气条件产生的临时限速。
速度规划需要把这些限速融合成一个“速度上限曲线”。说白了，就是告诉车辆：在任何一段路上，你最快不能超过多少。
| 限速类型 | 来源 | 特点 |
|---|---|---|
| 法定限速 | 高精地图 | 固定值，比如60 km/h |
| 动态限速 | 视觉/雷达识别 | 临时施工、事故区域限速 |
| 曲率限速 | 根据参考线曲率计算 | 过弯时自动降低，保证侧向加速度不超标 |
| 舒适性限速 | 策略层设定 | 比如最大加速度不超过2 m/s² |
2.2 输出数据：速度规划的“答卷”
输入处理完了，速度规划就要交作业了。输出主要三样：速度曲线、加速度曲线、时间戳。
2.2.1 速度曲线（Velocity Profile）
速度曲线是核心输出。它是一条以时间或弧长为横轴，速度为纵轴的曲线。每个点都告诉底层控制：在某个时刻或某个位置，你应该跑多快。
我习惯把速度曲线输出成离散点序列，每个点包含 (s, v, t)。s是弧长位置，v是目标速度，t是到达时间。这样控制模块拿到后，直接插值就能用。
// 速度曲线输出示例（伪代码）
struct SpeedPoint {
double s; // 弧长位置 [m]
double v; // 目标速度 [m/s]
double t; // 到达时间 [s]
};
std::vector<SpeedPoint> speed_profile;
// 填充数据...
speed_profile.push_back({0.0, 0.0, 0.0});
speed_profile.push_back({10.0, 5.0, 2.0});
speed_profile.push_back({20.0, 10.0, 4.0});
2.2.2 加速度曲线（Acceleration Profile）
加速度曲线是速度曲线的“导数”。它告诉控制模块：在每个时刻，你应该用多大的加速度去执行。
为什么需要单独输出加速度？因为速度规划不仅要规划速度，还要规划“怎么加速”。如果速度曲线很平滑，但加速度跳变剧烈，车辆就会一耸一耸的，乘客体验极差。
我个人习惯在输出速度曲线后，再计算一阶导数得到加速度曲线，然后做一次平滑滤波。这样能保证加速度变化率（加加速度）也在舒适范围内。
小技巧：加速度曲线里如果出现超过 ±3 m/s² 的值，基本可以判定规划有问题。正常舒适驾驶，加速度应该控制在 ±2 m/s² 以内。急刹车除外，但那是安全模块的活。
2.2.3 时间戳（Timestamp）
时间戳是速度规划的“时钟”。每个速度点和加速度点，都必须绑定一个时间戳。这样控制模块才能知道：什么时候该执行哪个指令。
时间戳通常有两种对齐方式：
- 绝对时间戳：和系统时钟对齐。比如规划周期是100ms，那么时间戳就是 0.1s, 0.2s, 0.3s...
- 相对时间戳：以当前规划时刻为0。比如未来5秒的规划，时间戳就是 0.0s, 0.1s, 0.2s... 到 5.0s。
我建议在工程中使用相对时间戳。原因很简单：绝对时间戳容易受系统时钟漂移影响，而相对时间戳只关心“从现在开始往后多久”，更稳定。
2.3 核心逻辑：一张图看懂
下面这张图，把输入输出和它们之间的关系串起来了。你盯着看两分钟，整个速度规划的接口就清楚了。
2.4 输入输出对齐：一个都不能少
最后，我总结一个检查清单。每次你写完速度规划的接口，对着这个清单过一遍，能省掉很多调试时间。
| 检查项 | 常见问题 | 我的建议 |
|---|---|---|
| 参考线曲率是否平滑 | 曲率跳变导致急刹 | 加滑动窗口滤波 |
| 障碍物轨迹时间戳是否对齐 | 时间偏差导致碰撞风险 | 统一使用相对时间戳 |
| 限速信息是否取最小值 | 多个限速源冲突 | 取交集，即最严格限速 |
| 速度曲线是否单调递增 | 速度回退导致控制异常 | 后处理做单调性检查 |
| 加速度曲线是否超限 | 舒适性差 | 限制 ±2 m/s² |