3、SL图与速度规划：从纵向-横向耦合中提取速度约束
说实话，很多做规划的同学容易把SL图和速度规划当成两个独立模块来处理。我个人觉得，这是新手最容易踩的坑。SL图不仅仅是用来做路径优化的，它里面藏着大量对速度规划至关重要的约束信息。今天我们就来聊聊，怎么从SL图里把这些约束“挖”出来。
3.1 SL图到底是什么？
先简单回顾一下。SL图，全称是Station-Lateral图。S轴代表纵向位移（沿着道路中心线的距离），L轴代表横向偏移（偏离中心线的距离）。说白了，就是把车辆在道路上的位置，用“走了多远”和“偏了多少”两个维度来描述。
你想想看，我们平时做路径规划，本质上就是在SL空间里找一条合理的曲线。但这条曲线不是随便画的——它必须满足车辆的运动学约束、障碍物避让要求，还要考虑舒适性。
核心认知：SL图是路径规划的输出，但同时也是速度规划的输入。两者之间存在强耦合关系，不能割裂对待。
3.2 SL图与速度规划的耦合关系
我在项目中遇到过不少团队，路径规划和速度规划是分开做的。路径规划出一个SL轨迹，然后速度规划再基于这个轨迹去算速度。听起来没问题对吧？但实际跑起来就出问题了。
为什么会这样？因为SL图里的某些信息，直接决定了速度规划的上限。举个例子：
- 曲率约束：SL轨迹的曲率变化，直接决定了车辆能安全通过的最大速度。弯道越急，速度就得越低。
- 横向加速度约束：SL轨迹的横向偏移变化率，对应着车辆的横向加速度。这个值太大，乘客会不舒服，甚至车辆会侧滑。
- 障碍物距离约束：SL图上障碍物的位置和形状，决定了车辆在某个S位置上的可通行宽度。宽度越窄，速度就得越保守。
嗯，这里要注意：这些约束不是独立的，它们会互相影响。比如一个急弯加上一个窄通道，速度约束就会变得非常严格。
3.3 如何从SL图提取速度约束？
我个人习惯把提取过程分为三步：
- 曲率分析：从SL轨迹中计算每个点的曲率
- 横向动态分析：评估横向位移变化率
- 可通行宽度分析：基于障碍物信息计算安全裕度
下面我们逐个来看。
3.3.1 曲率约束提取
曲率是速度规划最直接的约束来源。从SL轨迹中提取曲率，本质上是对轨迹进行二阶求导。但直接求导噪声太大，我建议先做平滑处理。
// 伪代码：从SL轨迹提取曲率约束
function extractCurvatureConstraints(slTrajectory) {
// 1. 对SL轨迹进行B样条平滑
smoothedTrajectory = bsplineSmooth(slTrajectory, 3);
// 2. 计算每个点的曲率
curvatures = [];
for (i = 1; i < smoothedTrajectory.length - 1; i++) {
dx = smoothedTrajectory[i+1].s - smoothedTrajectory[i].s;
ddx = smoothedTrajectory[i+1].s - 2*smoothedTrajectory[i].s + smoothedTrajectory[i-1].s;
dy = smoothedTrajectory[i+1].l - smoothedTrajectory[i].l;
ddy = smoothedTrajectory[i+1].l - 2*smoothedTrajectory[i].l + smoothedTrajectory[i-1].l;
curvature = (dx*ddy - dy*ddx) / Math.pow(dx*dx + dy*dy, 1.5);
curvatures.push(curvature);
}
// 3. 根据曲率计算最大安全速度
maxSpeedByCurvature = [];
for (i = 0; i < curvatures.length; i++) {
maxSpeed = Math.sqrt(maxLateralAccel / Math.abs(curvatures[i]));
maxSpeedByCurvature.push(maxSpeed);
}
return maxSpeedByCurvature;
}经验之谈：我曾经在高速场景下直接用原始轨迹算曲率，结果速度规划出来的曲线抖得像筛子。后来加了B样条平滑，效果立竿见影。平滑阶数建议选3，太低平滑不够，太高会丢失细节。
3.3.2 横向动态约束提取
横向动态约束，说白了就是看车辆在横向上的运动有多剧烈。这个约束来自SL轨迹的L值随时间的变化率。
但这里有个问题：SL轨迹本身没有时间信息，只有空间信息。所以我们需要先做一个“虚拟时间映射”——假设车辆以某个参考速度行驶，然后计算横向位移的变化率。
| 横向动态指标 | 计算公式 | 典型约束值 | 影响 |
|---|---|---|---|
| 横向速度 | v_l = dl/dt ≈ dl/ds * v_ref | < 0.5 m/s | 变道时的横向移动速度 |
| 横向加速度 | a_l = d²l/dt² ≈ d²l/ds² * v_ref² | < 0.3 m/s² | 乘客舒适度关键指标 |
| 横向加加速度 | j_l = d³l/dt³ ≈ d³l/ds³ * v_ref³ | < 0.2 m/s³ | 冲击感控制 |
嗯，这里要注意：v_ref是参考速度，不是实际速度。实际速度规划完成后，需要迭代验证横向动态是否真的满足约束。
3.3.3 可通行宽度约束提取
这个约束来自SL图上障碍物的分布。每个S位置，都有一个可通行的L范围。这个范围越窄，车辆能安全通过的速度就越低。
我曾经在狭窄的施工路段吃过亏。SL图上看起来可通行宽度还有2米，但实际因为道路曲率，车辆需要更大的横向空间。后来我加了一个“曲率补偿宽度”的修正项：
// 计算考虑曲率的有效可通行宽度
function computeEffectiveWidth(slMap, s, curvature) {
// 基础可通行宽度
baseWidth = slMap.getFreeSpaceWidth(s);
// 曲率补偿：弯道需要额外宽度
curvatureCompensation = Math.abs(curvature) * vehicleLength * 0.5;
// 安全裕度
safetyMargin = 0.3; // 30cm
effectiveWidth = baseWidth - curvatureCompensation - safetyMargin;
return effectiveWidth;
}避坑指南：我曾经在提取可通行宽度时，只考虑了静态障碍物，结果动态障碍物（比如突然插入的车辆）导致速度规划完全失效。后来我改为使用“时空SL图”——在SL基础上加入时间维度，这样就能同时处理静态和动态障碍物的约束了。
3.4 多约束融合：速度规划的核心挑战
上面我们提取了三种约束：曲率约束、横向动态约束、可通行宽度约束。但速度规划不是简单地取最小值就完事了。这些约束之间会互相影响，需要做融合处理。
我个人常用的方法是“约束优先级分层”：
- 第一层：安全约束（曲率 + 可通行宽度）—— 必须满足，否则车辆会出事故
- 第二层：舒适约束（横向加速度 + 加加速度）—— 尽量满足，必要时可以放宽
- 第三层：效率约束（目标速度 + 时间要求）—— 在满足前两层的前提下，尽可能快
下面这张图展示了SL图与速度规划之间的完整耦合关系：
3.5 实战中的常见问题
最后，分享几个我在实际项目中踩过的坑：
问题1：SL轨迹抖动导致速度规划震荡
我曾经遇到过SL轨迹在直道上很平滑，但一到弯道就开始抖动。后来发现是路径规划模块在弯道处的采样密度不够。解决方案是：根据曲率自适应调整SL轨迹的采样密度——曲率大的地方多采点，曲率小的地方少采点。
问题2：横向动态约束过于保守
刚开始做的时候，我把横向加速度约束设得很严（0.2 m/s²），结果车辆在变道时速度降得很厉害，被后车疯狂按喇叭。后来我改为“动态约束”——根据场景调整约束值：高速场景严格，低速场景可以适当放宽。
我的建议：不要试图一次性把所有约束都考虑周全。先做一个基础版本，跑通流程，然后逐步增加约束的精细度。迭代开发比一次性完美设计要高效得多。
好了，关于SL图与速度规划的耦合关系，以及如何从SL图提取速度约束，我们就聊到这里。记住，SL图不仅仅是路径规划的事情，它和速度规划是“命运共同体”。理解了这个耦合关系，你才能做出真正安全、舒适、高效的自动驾驶系统。