自动驾驶局部路径规划算法:图搜索、优化与采样的原理及实战

在自动驾驶系统中，规划（Planning）模块扮演着承上启下的关键角色——它接收环境感知的信息，生成可执行的轨迹，并下发给车辆控制模块执行。

广义的规划可以分为3个部分：

• 路由寻径（Routing）：即全局路径规划，类似于传统导航，结合高精地图中的车道、交通规则等信息，规划出从起点到终点的宏观路径。

• 行为决策（Behavioral Decision）：在行驶过程中决定车辆的具体行为，例如跟车、等待行人、路口交互、变道超车等。

• 运动规划（Motion Planning）：又称局部路径规划，负责生成未来一段时间内车辆期望行驶的轨迹（包含路径和速度信息），需满足车辆运动学/动力学约束、避障要求、舒适性指标等。

运动规划的本质是一个多目标数学优化问题，其核心目标包括：

• 安全性：避免与静态或动态障碍物碰撞，对于动态障碍物还需考虑其未来运动的不确定性，降低碰撞风险。

• 稳定性：轨迹必须符合车辆物理特性，确保控制器能够稳定跟踪。

• 舒适性：通过平滑的加/减速度和转向，提升乘客的乘坐体验。

• 时效性：在保证安全的前提下尽可能高效地到达目的地。

同时，规划过程需考虑多种物理约束，如加/减速度极限、非完整性约束（车辆只有两个控制自由度）、动力学约束（最大曲率、横摆角速度）等。在实际工程中，针对特定场景可适当简化模型。

本文详解当前主流的局部路径规划算法，包括基于图搜索、曲线拟合、数学优化、人工势场及采样等方法及应用。

一、基于图搜索的算法

图搜索算法将环境离散化为栅格或节点图，通过搜索算法寻找从起点到终点的最优路径。典型代表有Dijkstra和A*算法。

1.1 Dijkstra算法

Dijkstra是一种广度优先搜索算法，它以起点为中心向外层层扩展，直到扩展到目标点。该算法能够保证找到最短路径，成功率极高，但计算效率较低，尤其在高维栅格图中搜索速度较慢。

基本步骤：

1. 初始化集合S（已确定最短路径的节点）和U（未确定节点），S中仅包含起点s。

2. 从U中选出距离起点最近的节点u，将其加入S，并更新U中所有与u相邻节点的距离（若新路径更短）。

3. 重复步骤2，直到U为空或目标点被加入S。

1.2 A*算法

A*算法在Dijkstra的基础上引入了启发式函数（通常为当前节点到目标点的估计代价），形成深度优先的启发式搜索，大幅提升了搜索效率。其代价函数为：

基本步骤：

1. 将起点加入Open List。

2. 重复以下过程：

• 若不可通过或已在Close List中，忽略。

• 若不在Open List中，将其加入Open List，并设置当前节点为其父节点，计算G,H,F G、H、F。

• 若已在Open List中，检查经由当前节点到达该节点的路径是否更优（即GG值更小），若是，则更新其父节点和FF值。

• 从Open List中选出FF值最小的节点作为当前节点，移入Close List。

• 对当前节点的每个邻接节点：

3. 当Open List为空或目标点被加入Close List时，从目标点回溯父节点至起点，得到最优路径。

对比：Dijkstra保证全局最优但速度慢，A*在合理启发函数下能快速找到较优路径，是实际导航系统（如手机地图）的基础算法。

应用：在自动驾驶中，路由寻径模块常采用A或其变种（如分层A、D* Lite）进行全局路径规划。而运动规划则需在全局路径基础上进一步细化轨迹，通常需要结合其他方法。

二、基于曲线拟合的算法

曲线拟合方法通过参数化曲线（如多项式、贝塞尔曲线等）生成平滑的局部路径，常用于路径平滑或特定场景（如泊车、换道）的轨迹生成。其核心目标是保证路径的几何连续性（如位置、方向、曲率连续），以提升车辆跟踪的可行性。

常见曲线类型：

• 余弦曲线：加速度在起止点取最大值，舒适性较差。

• 多项式曲线（如五次多项式）：能够同时约束起止点的位置、速度和加速度，实现曲率连续变化，应用广泛。

• 圆弧及公切线：路径由直线和圆弧拼接而成，但曲率在连接点处突变，车辆需停车转向才能精确跟踪。

• 贝塞尔曲线：通过控制点调节曲线形状，可实现曲率变化率连续，常用于路径平滑。

• B样条曲线：具备局部支撑性，可灵活调整局部形状，同时保证高阶连续性。

• 微分平坦：利用系统微分平坦特性，将轨迹规划问题转化为低维空间中的曲线设计。

三、基于数学优化的算法

数学优化方法将轨迹规划建模为带约束的优化问题，通过求解最优控制问题得到轨迹。典型代表是百度Apollo的EM Planner和Lattice Planner。

3.1 EM Planner（ Expectation-Maximization Planner）

EM Planner是Apollo中一种“轻决策重规划”的典型实现，它将路径规划和速度规划分层处理，并在SL（纵向-横向）坐标系和ST（纵向-时间）坐标系下分别进行决策和优化。

核心流程：

1. 参考线生成：路由模块提供全局参考线，作为规划的基础。

2. 路径规划（SL坐标系）：

• 动态规划（DP）：将道路离散化为采样点，通过动态规划搜索出一条初步路径，实现避障决策（如绕行障碍物）。

• 二次规划（QP）：以DP结果为初始值，对路径进行平滑优化，最小化路径的曲率变化、横向偏差等。

3. 速度规划（ST坐标系）：

• 将动态障碍物投影到ST图中，通过DP决策速度轮廓（如跟车、停车）。

• 通过QP平滑速度曲线，保证加速度和加加速度的舒适性。

优点：将高维问题分解为两个低维问题，计算效率高；通过DP保证可行解，通过QP提升平滑性。

3.2 Lattice Planner

Lattice Planner直接在时域内生成候选轨迹（路径+速度），通过采样横纵向偏移量生成一系列轨迹簇，然后根据代价函数（如安全、舒适、效率）选择最优轨迹。其优点是能直接处理动态障碍物，但采样密度影响计算量。

四、基于人工势场的算法

人工势场法（Artificial Potential Field, APF）将车辆在环境中的运动视为在虚拟力场中的受力运动：目标点产生“引力”，障碍物产生“斥力”，车辆在合力作用下运动至目标点。

优点：数学简洁，实时性好，适用于动态环境。
缺点：易陷入局部极小值（如U形障碍物前无法逃脱）；在复杂环境中难以保证全局最优。

改进：通过引入随机扰动（如模拟退火）或结合全局引导势场，可缓解局部极小问题。

五、基于采样的算法

基于采样的方法通过在状态空间中随机采样，构建连通图或树，以找到可行路径。典型代表是快速扩展随机树（RRT）。

RRT算法

RRT从起点开始随机扩展节点，形成一棵树状结构，直到树梢到达目标区域。

六、实际场景应用

6.1 自动泊车（APA）

自动泊车是低速场景下的典型应用，对路径精度要求高，且需满足车辆最小转弯半径约束。常用方法包括：

• 混合A（Hybrid A）：在栅格地图上进行搜索，但节点扩展时考虑车辆运动学，生成满足非完整性约束的路径，再结合Reeds-Shepp曲线连接起止点。

• 曲线拟合：使用五次多项式或贝塞尔曲线对泊车路径进行平滑，保证曲率连续。

6.2 换道场景

换道需在高速动态环境中快速决策并生成安全轨迹。常用方法：

• 采样+优化：采样不同换道时长和横向偏移，生成候选轨迹簇，利用代价函数（如碰撞风险、舒适性）选择最优。

• 优化方法：将换道轨迹规划为带约束的优化问题，如最小化加加速度，同时考虑与周围车辆的动态安全距离。

七、总结与展望

本文综述了自动驾驶运动规划中的几类主流算法：

未来，运动规划的发展趋势包括：

• 深度融合学习：利用深度强化学习处理复杂交互场景，学习驾驶策略。

• 多传感器融合与不确定性建模：考虑感知不确定性，提升规划鲁棒性。

• 车路协同：借助V2X获取更广范围的交通信息，实现全局协同优化。

• 实时性与最优性的平衡：通过并行计算、轻量化优化算法，满足实时性要求。

附录：动态规划示例代码（贝尔曼最优原理）

% 贝尔曼最优原理示例：求解从A到J的最短路径（此处为最长路径示例）clear all; clc% 定义各层节点数（A-J每列节点数）num_states = [12331]; % 定义距离矩阵（四维，分别表示起始层、起始节点、目标层、目标节点）distance = inf(num_states); % A-Bdistance(1,1,2,1) = 12; % A-Cdistance(1,1,2,2) = 7;  % B-D, B-E, B-Fdistance(2,1,3,1) = 5; distance(2,1,3,2) = 6; distance(2,1,3,3) = 9;% C-D, C-E, C-Fdistance(2,2,3,1) = 14; distance(2,2,3,2) = 10; distance(2,2,3,3) = 11;% D-G, D-H, D-Idistance(3,1,4,1) = 8; distance(3,1,4,2) = 7; distance(3,1,4,3) = 10;% E-G, E-H, E-Idistance(3,2,4,1) = 9; distance(3,2,4,2) = 7; distance(3,2,4,3) = 9;% F-G, F-H, F-Idistance(3,3,4,1) = 10; distance(3,3,4,2) = 7; distance(3,3,4,3) = 8;% G-J, H-J, I-Jdistance(4,1,5,1) = 5; distance(4,2,5,1) = 9; distance(4,3,5,1) = 8;%% 动态规划逆向求解（求最大成本路径）V = inf(5);      % 价值函数矩阵，V(k,i)表示第k层第i节点的最优成本V(5,1) = 0;      % J点成本为0fork=4:-1:1fori=1:num_states(k)        path_len = zeros(num_states(k+1),1);forj=1:num_states(k+1)            path_len(j) = distance(k,i,k+1,j) + V(k+1,j);        end        [V(k,i), ind] = max(path_len);   % 取最大值（若求最短路径则用min）    endend%% 显示最优成本disp(['Optimal cost at A: ', num2str(V(1,1))]);%% 回溯最优路径（按最大价值选择下一节点）optimal_path = zeros(5,1);fork=1:5    [~, optimal_path(k)] = max(V(k,:));enddisp('Optimal path (node index per layer):');disp(optimal_path');