3. 路径-速度解耦法：SL与ST图概念、路径规划与速度规划分离、经典解耦流程

各位同学，今天我们来聊聊轨迹规划里一个非常经典的思想——路径-速度解耦。

说实话，我刚入行那会儿，面对的是一个纯耦合的优化问题。状态空间是位置、速度、加速度，甚至还要考虑时间。维度一高，求解器动不动就罢工。后来我接触到解耦的思路，才恍然大悟——原来可以把一个复杂问题拆成两个相对简单的问题。

你想想看，开车的时候，我们是不是先想「往哪条车道走」，再想「什么时候加速、什么时候刹车」？路径-速度解耦，本质上就是这个直觉的数学化。

3.1 SL与ST图：两个坐标系，两种视角

要理解解耦，先得搞懂两个图：SL图和ST图。

3.1.1 SL图：横向的「车道级」规划

SL图，横轴是弧长 s（沿着道路中心线走的距离），纵轴是横向偏移 l（偏离中心线的距离）。

说白了，SL图就是回答一个问题：在道路的哪个位置，我应该偏左还是偏右？

我在项目中遇到过这样的情况：道路有弯道，中心线是弯曲的。如果用笛卡尔坐标系，弯道处的横向约束会变得非常复杂。但换到SL坐标系，道路就变成了一条直线，横向偏移 l 就是简单的左右距离。这个变换，让约束处理变得极其简洁。

  核心要点： SL图将弯曲道路「拉直」，把横向避障问题简化为在一条直线上选择偏移量。

3.1.2 ST图：纵向的「时间-距离」规划

ST图，横轴是时间 t，纵轴是弧长 s。

ST图回答的是：什么时间，我应该到达道路的哪个位置？

举个例子。前方有一辆慢车，它的轨迹在ST图上是一条斜线（斜率代表速度）。你的车要超车，就必须在某个时间点之前，让 s 超过那辆慢车。ST图把「什么时候超车」这个决策，变成了一个几何问题——你的轨迹线必须从障碍物的上方或下方穿过。

我的经验： 实际工程中，ST图里的障碍物通常用「矩形」表示。矩形的宽度代表障碍物占据的时间段，高度代表它占据的路段。你的规划轨迹，就是一条从原点出发、斜率逐渐变化的曲线，要避开所有矩形。

3.2 路径规划与速度规划：为什么能分离？

你可能会问：路径和速度明明相互影响，为什么能分开做？

嗯，这里有个关键假设：路径的形状和速度的大小，在大多数场景下是弱耦合的。

比如，你决定走左侧车道超车（路径决策），至于你是以 60km/h 还是 80km/h 超车（速度决策），并不会改变「走左侧车道」这个事实。反过来，你决定以恒定速度巡航（速度决策），但具体走车道中间还是偏左（路径决策），也基本不影响速度。

当然，极限工况下（比如高速过弯），路径和速度是强耦合的——弯道越急，速度必须越低。但那是另一套方法（比如时空联合规划）要解决的问题。对于城市道路、高速公路等大多数场景，解耦已经足够好用。

避坑指南： 我曾经在一个项目中，把解耦方法用在了冰雪路面的紧急避障上。结果路径规划出来的大曲率弯道，速度规划根本来不及减速，车辆直接侧滑了。后来我加了一个「路径曲率-速度上限」的约束，才解决问题。记住：解耦不是万能的，强耦合场景需要额外处理。

3.3 经典解耦流程：三步走

好了，理论说完了，我们来看看实际怎么操作。经典解耦流程分三步：

路径规划（SL图）：在SL空间里，生成一条平滑的横向偏移曲线 l(s)。
速度规划（ST图）：在ST空间里，生成一条平滑的纵向距离曲线 s(t)。
合成轨迹：将 l(s) 和 s(t) 合并，得到笛卡尔坐标系下的轨迹 (x(t), y(t))。

下面这张图，是我自己画的解耦流程框架，你看一眼就明白了：

你看，这个流程非常清晰。路径规划只管「走哪条道」，速度规划只管「什么时候走」。最后合起来，就是一条完整的轨迹。

3.4 代码示例：SL与ST图的简单实现

光说不练假把式。我写一段伪代码，帮你理解SL图和ST图的核心逻辑：

# 路径规划：SL图
def plan_path_sl(road_boundary, static_obstacles):
    # 1. 在SL空间采样候选路径
    candidate_paths = []
    for lateral_offset in range(-2, 3):  # 偏移 -2 到 2 米
        path = generate_smooth_curve(lateral_offset)
        candidate_paths.append(path)
    
    # 2. 评估每条路径的代价
    best_path = None
    min_cost = float('inf')
    for path in candidate_paths:
        cost = 0
        cost += smoothness_cost(path)      # 平滑性
        cost += obstacle_cost(path, static_obstacles)  # 避障
        cost += boundary_cost(path, road_boundary)     # 边界约束
        if cost < min_cost:
            min_cost = cost
            best_path = path
    
    return best_path  # 返回 l(s)

# 速度规划：ST图
def plan_speed_st(dynamic_obstacles, speed_limits):
    # 1. 将动态障碍物投影到ST空间
    st_obstacles = []
    for obs in dynamic_obstacles:
        st_obs = project_to_st(obs)  # 得到 (t_start, t_end, s_start, s_end)
        st_obstacles.append(st_obs)
    
    # 2. 在ST空间搜索最优速度曲线
    # 常用方法：动态规划 + 二次规划
    speed_profile = dp_qp_solver(st_obstacles, speed_limits)
    
    return speed_profile  # 返回 s(t)

# 合成轨迹
def combine_trajectory(path_sl, speed_st):
    trajectory = []
    for t in range(0, T, dt):
        s = speed_st(t)          # 当前弧长
        l = path_sl(s)           # 当前横向偏移
        x, y = sl_to_xy(s, l)    # 转换到笛卡尔坐标
        trajectory.append((x, y, t))
    return trajectory

这段代码虽然简单，但核心思想都在里面了。实际工程中，路径规划会用更复杂的采样策略（比如基于参考线的多项式曲线），速度规划会用动态规划加二次规划的组合。但框架不变。

一个小技巧： 我在做速度规划时，习惯先做一次「粗糙的」动态规划，得到一个粗略的速度曲线。然后用二次规划做平滑。这样既保证了全局最优性，又保证了曲线的平滑性。你可以试试。

3.5 解耦法的优缺点

优点	缺点
计算效率高：两个低维问题比一个高维问题快得多	无法处理强耦合场景（如极限过弯）
工程实现简单：每个模块可以独立调试	可能丢失全局最优解
可解释性强：路径和速度的决策逻辑清晰	需要额外的反馈迭代来处理冲突
易于扩展：可以单独升级路径或速度模块	对动态障碍物的处理不够灵活

嗯，总结一下。路径-速度解耦法，是轨迹规划领域最经典、最实用的方法之一。它把「往哪走」和「什么时候走」分开处理，让问题变得简单可控。SL图和ST图，就是这两个子问题的「语言」。掌握了它们，你就掌握了自动驾驶轨迹规划的半壁江山。

可可精品资料库

📚 课程目录

3. 路径-速度解耦法：SL与ST图概念、路径规划与速度规划分离、经典解耦流程

3.1 SL与ST图：两个坐标系，两种视角

3.1.1 SL图：横向的「车道级」规划

3.1.2 ST图：纵向的「时间-距离」规划

3.2 路径规划与速度规划：为什么能分离？

3.3 经典解耦流程：三步走

3.4 代码示例：SL与ST图的简单实现

3.5 解耦法的优缺点