3、S-T图与速度规划：S-T图的概念与构建、基于S-T图的可行域搜索、速度曲线生成的基本流程
好，我们进入第三章。这一章聊的是速度规划的核心工具——S-T图。
说实话，很多刚入行的同学觉得速度规划就是算个加速度、设个限速。但真正做起来你会发现，没有S-T图，你根本没法系统性地处理复杂场景。我在项目里踩过不少坑，后来才明白，S-T图才是把「空间约束」转化成「时间约束」的关键桥梁。
3.1 S-T图的概念与构建
先说说S-T图到底是什么。
S-T图，横轴是时间t，纵轴是沿路径的里程s。说白了，就是把车辆在路径上的位置随时间的变化画出来。
你想想看，路径规划给了你一条几何路径，但没告诉你什么时候该走到哪里。S-T图就是用来回答这个问题的——什么时间，车应该在路径的哪个位置。
核心概念：S-T图上的每一条曲线，都代表一种速度方案。曲线的斜率就是速度（ds/dt），斜率越大，速度越快。
怎么构建S-T图？我一般分三步走：
- 确定路径长度：把路径离散化成一系列点，计算每个点的累积里程s。这一步通常路径规划已经做完了。
- 确定时间轴范围：根据场景需求设定规划时域，比如5秒、8秒。我个人习惯用8秒，太长计算量太大，太短又不够应对突发情况。
- 标注障碍物轨迹：把感知模块给出的障碍物预测轨迹，投影到S-T图上。每个障碍物在S-T图上会形成一个「占据区域」。
举个例子，前车在50米处，以10m/s匀速行驶。那么在S-T图上，它就是从(0, 50)出发，斜率为10的一条直线。我们的车如果也想走这条路，就必须避开这条线——要么跟在它后面（斜率小于10），要么提前变道（但这里我们假设只有一条车道）。
我的经验：构建S-T图时，别忘了考虑自车的初始状态。初始速度、初始加速度都会影响你后续的可行域形状。我曾经因为忘了初始化速度，导致规划出的曲线一开始就急加速，差点把乘客甩出去。
3.2 基于S-T图的可行域搜索
S-T图画好了，接下来就是找「能走的路」。这就是可行域搜索。
可行域，说白了就是S-T图上没有被障碍物占据的区域。但注意，不是所有空白区域都能走——你还要考虑车辆的运动学限制。
搜索过程我习惯用动态规划来做。大致思路是：
- 离散化S-T空间：把时间轴和里程轴都离散成网格。时间步长我常用0.1秒，里程步长0.5米。太粗了精度不够，太细了计算量爆炸。
- 标记障碍物占据网格：把障碍物轨迹覆盖的网格标记为不可通行。
- 从起点开始搜索：从(0, 0)出发，逐时间步向前搜索。每个网格点，计算从上一时间步所有可能状态转移过来的代价。
- 考虑约束：速度不能超过限速，加速度不能超过舒适阈值，加加速度（jerk）也要控制。这些约束会限制你从一个网格跳到另一个网格的可行性。
注意：可行域搜索时，最容易忽略的是「时间一致性」。我曾经犯过一个错误——只考虑了当前时刻的障碍物位置，没考虑障碍物未来的运动趋势。结果规划出的路径在t=3秒时正好撞上了一个动态障碍物。嗯，从那以后我每次都会把障碍物的预测轨迹完整投影到S-T图上。
搜索完成后，你会得到一条从起点到终点的「最优路径」——在S-T图上就是一条曲线。这条曲线告诉你：每个时刻，你应该在路径的哪个位置。
3.3 速度曲线生成的基本流程
有了S-T图上的路径，速度曲线其实就已经呼之欲出了。为什么？因为S-T曲线的斜率就是速度。
但直接求导得到的速度曲线往往不平滑，没法直接用。所以我们需要一个后处理流程：
- 从S-T路径提取速度：对S-T曲线求一阶导，得到速度-时间曲线。这一步很简单，但结果通常很粗糙。
- 平滑处理：用二次规划或者样条插值对速度曲线进行平滑。我一般用三次样条，既能保证C2连续（加速度连续），又不会过度拟合。
- 校核约束：检查平滑后的速度曲线是否满足所有约束——最大速度、最大加速度、最大jerk、舒适性阈值。如果不满足，需要局部调整。
- 生成最终速度指令：把速度曲线离散化成控制模块需要的格式，通常是时间-速度对序列，或者时间-加速度对序列。
关键点：速度曲线生成不是一次性的。在实际系统中，你需要每个控制周期（比如50ms）重新规划一次。因为环境在变，障碍物在动，你的规划必须实时更新。
这里给一段伪代码，展示核心逻辑：
// 速度曲线生成伪代码
function generateSpeedProfile(s_t_path, constraints):
// 1. 提取速度
velocity = derivative(s_t_path, wrt='time')
// 2. 平滑处理
smoothed_velocity = cubic_spline_smooth(velocity, smooth_factor=0.1)
// 3. 约束校核
for each time_step:
if smoothed_velocity[time_step] > constraints.max_speed:
smoothed_velocity[time_step] = constraints.max_speed
if acceleration(time_step) > constraints.max_accel:
// 局部调整速度
adjust_velocity_around(time_step)
// 4. 生成指令序列
return generate_command_sequence(smoothed_velocity, time_step=0.05)
你看，流程其实不复杂。但真正做工程的时候，坑都在细节里。
举个例子，平滑处理时平滑因子怎么选？选太大，速度曲线太软，响应慢；选太小，曲线抖动，乘客晕车。我一般先设0.1，然后根据实车测试微调。没有标准答案，全靠经验。
避坑指南：我曾经在高速场景下，因为平滑因子设得太大，导致速度曲线对前车减速的响应延迟了0.3秒。结果AEB触发了，虽然没撞上，但被测试工程师投诉了。从那以后，我养成了一个习惯——在动态场景下，平滑因子要动态调整。障碍物靠近时，平滑因子减小，让响应更快。
最后总结一下这一章的核心：
- S-T图是把空间-时间约束可视化的工具，是速度规划的基础
- 可行域搜索是在S-T图上找出一条满足所有约束的路径
- 速度曲线生成是把S-T路径转化为可执行的速度指令
下一章我们会聊跟车策略，到时候会用到S-T图来分析跟车距离和反应时间的关系。嗯，先消化这些吧。