自动驾驶---决策规划中的动态规划

智能汽车人

1650人浏览 · 2025-07-21 07:15:00

智能汽车人 · 2025-07-21 07:15:00 发布

1 前言

在早期的规控算法中动态规划（Dynamic Programming，DP）是非常流行的一种算法，比如大家熟知的Apollo的EM planner，早些年讲解EM Planner的文章有很多，笔者在本篇博客就不赘述了。

但是随着对算力以及性能的需求，EM Planner（双层DP + QP）算法逐渐退出量产的历史舞台，取而代之的是Decision + QP，在近两三年也逐渐开始被Model取代。

2 DP应用

动态规划是一种用于解决优化问题的算法策略。它的核心思想是将一个复杂的问题分解为一系列相互关联的子问题，并通过求解子问题来构建原问题的最优解。在自动驾驶领域，动态规划算法被广泛应用于路径规划和轨迹规划，以实现安全、高效、舒适的行驶。

2.1 LeetCode

在LeetCode中有专题是针对动态规划的，刷过题朋友们会比较熟悉。下面笔者举两个例子来简单说明动态规划的使用，这两也是面试刷题中动态规划比较经典的两个题目。

（1）动态规划（DP）算法来解决经典的斐波那契数列问题

斐波那契数列问题可以很好地体现动态规划中通过记录子问题的解来避免重复计算，从而高效求解问题的思想，代码如下：

#include <iostream>
#include <vector>

// 使用动态规划计算斐波那契数列的第n项
int fibonacciDP(int n) {
    // 创建一个vector来存储已经计算过的斐波那契数，初始化为0，大小为n + 1
    std::vector<int> dp(n + 1, 0);

    // 初始化边界条件，斐波那契数列的第0项和第1项都为1
    dp[0] = 0;
    dp[1] = 1;

    // 从第2项开始，根据斐波那契数列的递推公式dp[i] = dp[i - 1] + dp[i - 2]来计算
    for (int i = 2; i <= n; ++i) {
        dp[i] = dp[i - 1] + dp[i - 2];
    }

    return dp[n];
}

int main() {
    int n = 10;  // 这里可以修改n的值来计算不同位置的斐波那契数
    int result = fibonacciDP(n);
    std::cout << "斐波那契数列的第 " << n << " 项是: " << result << std::endl;
    return 0;
}

（2）路径规划 DP 示例

假设我们有一个简单的二维网格地图，车辆（或机器人等移动体）只能向右或向下移动，目标是从左上角起点移动到右下角终点，求最短路径长度（这里路径长度可以简单假设每移动一格距离为 1）。以下是示例代码：

#include <iostream>
#include <vector>

// 使用动态规划计算在二维网格地图中从左上角到右下角的最短路径长度
int shortestPathDP(int m, int n) {
    // 创建二维动态规划数组，用于存储每个位置到起点的最短路径长度，初始化为较大值
    std::vector<std::vector<int>> dp(m, std::vector<int>(n, INT_MAX));

    // 初始化起点的最短路径长度为0
    dp[0][0] = 0;

    // 初始化第一行和第一列的最短路径长度，因为只能向右或向下移动，所以是依次累加
    for (int i = 1; i < m; ++i) {
        dp[i][0] = dp[i - 1][0] + 1;
    }
    for (int j = 1; j < n; ++j) {
        dp[0][j] = dp[0][j - 1] + 1;
    }

    // 填充其余位置的最短路径长度，根据动态规划思想，当前位置的最短路径长度是
    // 上方位置和左方位置的最短路径长度中的较小值加1（因为移动一格距离为1）
    for (int i = 1; i < m; ++i) {
        for (int j = 1; j < n; ++j) {
            dp[i][j] = std::min(dp[i - 1][j], dp[i][j - 1]) + 1;
        }
    }

    return dp[m - 1][n - 1];
}

int main() {
    int m = 5;  // 地图的行数，可以根据实际情况修改
    int n = 5;  // 地图的列数，可以根据实际情况修改
    int result = shortestPathDP(m, n);
    std::cout << "从左上角到右下角的最短路径长度是: " << result << std::endl;
    return 0;
}

2.2 决策规划

在自动驾驶领域，许多决策和规划问题都具有复杂的时空约束和多个相互关联的因素。例如，车辆的路径规划需要考虑道路条件、交通规则、其他车辆和行人的位置以及车辆自身的动力学特性等诸多因素。动态规划算法提供了一种有效的方法来处理这些复杂的情况，通过将路径规划问题分解为一系列小的决策阶段，在每个阶段寻找最优的决策，从而实现全局最优或者接近最优的路径规划。

在自动驾驶规划模块中比较常见的DP，一般用于路径规划和速度规划的初始解的生成，为后续的QP做好准备。

（1）路径规划问题

车辆需要在复杂的交通环境中找到一条从起点到终点的安全、高效的行驶路径。传统的路径规划方法可能在处理复杂的交通场景和动态变化的环境时遇到困难。例如，基于几何方法的路径规划可能无法很好地考虑车辆的动力学限制和实时交通信息。
DP 算法可以根据地图信息（如道路拓扑结构、障碍物位置等）以及车辆的运动学和动力学模型，通过对行驶路径的离散化处理，将路径规划问题转化为一个多阶段决策问题。每个阶段对应车辆在一定时间或距离间隔内的行驶决策，如选择车道、转弯或直行等。

（2）速度规划问题

车辆需要根据路况、交通信号和周围车辆的速度等因素合理地调整速度。DP 算法可以用于在给定的路径上确定最优的速度曲线，以满足安全、舒适和高效的行驶要求。
例如，在接近交通信号灯或前方车辆时，需要考虑如何平稳地减速；在路况良好且没有交通限制的情况下，如何选择合适的加速策略以提高行驶效率。DP 算法通过考虑速度的变化范围、加速度限制以及与其他交通参与者的交互，来优化速度规划。

3 DP算法

本小节主要介绍在自动驾驶规划模块中，DP算法的详细原理和实现步骤，举例自动驾驶规划模块的局部规划。

3.1 算法步骤

（1）状态定义

首先需要定义系统的状态。状态通常包括车辆的位置、速度、加速度等信息。例如，在路径规划场景下，状态可以表示为 $s=(s,s',s'',d,d',d'')$ ，其中 $(s,d)$ 是车辆在车道坐标系下的二维位置坐标， $s'$ 是纵向速度， $s''$ 是纵向加速度， $d'$ 是横向速度， $d''$ 是横向加速度。
状态的定义要能够完整地描述车辆在行驶过程中的关键信息，并且状态空间的大小和离散化程度会影响算法的计算复杂度和精度。

（2）状态转移方程

状态转移方程描述了系统从一个状态到另一个状态的演变规律。在自动驾驶中，这取决于车辆的动力学模型。例如，根据牛顿运动定律，车辆的速度和位置可以通过以下简单的离散时间状态转移方程来描述：
- $v_{t + 1}=v_t + a_t\Delta t$ （速度更新方程，其中是时间步长）
- $x_{t + 1}=x_t + v_t\Delta t+\frac{1}{2}a_t(\Delta t)^2$ （位置更新方程）
对于更复杂的情况，如考虑车辆的转向、轮胎摩擦等因素，状态转移方程会更加复杂。这些方程定义了在每个决策阶段（如每个时间步长）车辆状态如何变化，是 DP 算法的核心部分之一。

目前很多DP算法在状态转移部分采取的原则：从当前纵向位置的一个横向点，只能转移到下一个纵向位置上的相邻横向点。

（3）决策变量和动作空间

决策变量是指在每个状态下可以采取的行动。在自动驾驶中，决策变量可能包括车辆的转向角度、加速度的调整等。
例如，车辆的转向角度可能有一个范围限制，如 $[-\theta_{max},\theta_{max}]$ ，加速度的范围可能是 $[-a_{max},a_{max}]$ 。在每个状态下，算法需要从这个动作空间中选择一个最优的动作，以引导车辆朝着目标状态前进。

（4）代价函数

代价函数用于评估每个决策的优劣。在自动驾驶中，代价函数通常考虑多个因素，如行驶距离、行驶时间、安全性（与障碍物或其他车辆的距离）、舒适性（加速度的变化率等）。
例如，一个简单的代价函数可以表示为： $J = \alpha d+\beta t+\gamma \sum_{i}d_{obs,i}+\delta \sum_{j}|a_j - a_{j - 1}|$ ，其中是行驶距离， $t$ 是行驶时间， $d_{obs,i}$ 是与第 $i$ 个障碍物的距离， $a_j$ 是第 $j$ 个时间步长的加速度， $\alpha,\beta,\gamma,\delta$ 是权重系数，用于平衡不同因素在代价函数中的重要性。

算法小结：

问题离散化：根据车辆的宽度、位置、车道宽度、车辆速度以及撒点的最大步长、最小步长、最小长度、最大长度等规则，在道路上生成一系列的采样点。这些采样点构成了路径规划的搜索空间。
初始化阶段：确定初始状态，即车辆的起始位置、速度等信息。
迭代求解阶段: 对于每个采样点，计算从其相邻的前驱点到达该点的代价。
最优解提取阶段: 在计算完所有采样点的代价后，从目标点开始，根据每个点的最优前驱信息，逐步回溯到起始点，从而得到从起始点到目标点的最优路径。

3.2 算法特点

优势
- 全局优化能力：能够考虑整个行驶过程中的各种因素，通过将问题分解为子问题并逐步求解，有机会找到全局最优或接近最优的解决方案。例如，在路径规划中可以综合考虑全程的道路条件和交通情况。
- 灵活性：可以很容易地将不同的约束条件（如交通规则、车辆动力学限制）和优化目标（如最短时间、最小能耗）纳入代价函数和状态转移方程中，从而适应不同的自动驾驶场景和需求。
局限
- 计算复杂度高：随着状态空间的增大和决策阶段的增多，计算量会呈指数级增长。这在复杂的交通场景或高分辨率的地图情况下可能导致计算时间过长，无法满足实时性要求。例如，在城市复杂交通环境中，车辆可能的状态数量众多，使用 DP 算法进行详细的规划可能会使计算资源耗尽。
- 模型依赖：DP 算法的性能依赖于准确的车辆动力学模型和环境模型。如果模型与实际情况有偏差，例如对道路摩擦力估计错误或者对其他车辆的行为预测不准确，可能会导致规划结果不理想甚至不安全。