自动驾驶路径规划五大常用算法

谈思汽车 2024-01-28 10:30

【应用手册】TI 全新MCU及C29内核的能源设施应用方案 【免注册下载】创新MCU提升高压系统的实时性能

智能汽车安全新媒体

无人驾驶系统的核心可分为四个部分：感知、定位、规划和控制。规划承接环境感知，并下启车辆控制，是实现无人驾驶的关键技术之一。

规划是指无人车为了到达某一目的地而做出决策和计划的过程，其规划出来的轨迹是带速度信息的路径，对于无人驾驶车辆而言，这个过程包括从起始地到达目的地，要避开障碍物，同时要不断优化行车路线和轨迹行为，以保证车辆安全舒适到达目的地。

根据这两点要求可将路径规划分为全局路径规划和局部路径规划，全局路径规划为静态规划，局部路径规划为动态规划。全局路径规划需要掌握所有的环境信息，根据环境地图的所有信息进行路径规划；局部路径规划只需要与传感器实时采集环境信息，了解环境地图信息，然后确定出所在地图的位置及其局部的障碍物分布情况，从而可以选出从当前结点到某一子目标结点的最优路径。

常用路径规划算法大致可分为以下几类：

最常用的传统经典算法

1.Dijkstra算法

Dijkstra算法是由计算机科学家Edsger W. Dijkstra在1956年提出的。Dijkstra算法用来寻找图形中节点之间的最短路径的算法。采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。Dijkstra算法在扩展的过程中，都是取出未访问结点中距离该点距离最小的结点，然后利用该结点去更新其他结点的距离值。

优点：如果最优路径存在，那么一定能找到最优路径。

缺点：有权图中可能是负边；扩展的节点很多，效率低。

2.A*算法

A* 算法发表于1968年，A* 算法是将Dijkstra算法与广度优先搜索算法（BFS, Breath First Search）二者结合而成，通过借助启发式函数的作用，能够使该算法能够更快的找到最优路径。A*算法是静态路网中求解最短路径最有效的直接搜索方法。

贪婪的最佳优先搜索算法与Dijkstra有着类似的工作方式，但是使用的是以每个节点到达终点的距离作为优先级，Dijkstra是以每个节点距离起点的移动代价进行优先级排序的，优先选择代价最小的作为下一个遍历的节点。最佳优先搜索算法比Dijkstra算法运行更快。

Dijkstra算法不同之处在于，A* 算法是一个“启发式”算法，它已经有了一些我们告诉它的先验知识，如“朝着终点的方向走更可能走到”。它不仅关注已走过的路径，还会对未走过的点或状态进行预测。因此A* 算法相较于Dijkstra而言，调整了进行BFS的顺序，少搜索了哪些“不太可能经过的点”，更快地找到目标点的最短路径。另外一点，由于H选取的不同，A * 算法找到的路径可能并不是最短的，但是牺牲准确率带来的是效率的提升。

优点：利用启发式函数，搜索范围小，提高了搜索效率；如果最优路径存在，那么一定能找到最优路径。

缺点：A*算法不适用于动态环境；不适用于高维空间，计算量大；目标点不可达时会造成大量性能消耗。

**3.D*算法**

D* 算法(Dynamic A Star)是卡耐基梅隆机器人中心的Stentz在1994年提出的主要用于机器人探路，并且美国火星探测器上就是应用的D* 路径规划算法。A* 算法适用于在静态路网中寻路，在环境变化后，往往需要replan，由于A* 不能有效利用上次计算的信息，故计算效率较低。D* 算法由于储存了空间中每个点到终点的最短路径信息，故在重规划时效率大大提升。A* 是正向搜索，而D* 特点是反向搜索，即从目标点开始搜索过程。在初次遍历时候，与Dijkstra算法一致，它将每个节点的信息都保存下来。

优点：适用于动态环境的路径规划，搜索效率高

缺点：不适用于高维空间，计算量大

人工势场法

人工势场法是由Khatib在1985年提出的一种基于虚拟力场的局部路径规划算法，该算法的基本思想是：假设行驶目标点对车辆产生引力，而障碍物对车辆产生斥力，控制车辆沿势场中“势峰”间的“势谷”前进。其中，引力与车辆到行驶目标点的距离成正比，斥力与车辆到障碍物的距离成反比。通过求解车辆所受引力和斥力的合力作为车辆的合外力来控制车辆的行驶速度和运动方向。该方法具有易于数学表达、反应速度快、易于实现算法与环境形成闭环控制等优点，但它在求解过程中极易出现局部最优解而导致产生死锁现象。

优点：规划出的路径一般是比较平滑且安全；人工势场法是一种反馈控制策略，具有一定的鲁棒性

缺点：容易陷入局部最优的问题；距离目标点较远时，引力特别大，斥力相对较小，可能会发生碰撞；当目标点附近有障碍物时，斥力非常大，引力较小，很难到达目标点

基于图搜索的运动规划和方法

路径规划首先需要建立规划模型，利用状态空间法描述规划模型是建立非线性优化模型的关键，图搜索算法可以很好地解决该问题。

基本思想是：将车辆的初始位姿和目标位姿映射到一个状态空间，然后将状态空间离散化，并将其构成一个图，随后从图中搜索满足约束条件的最优轨迹。目前主流的方法主要包括Voronoi图、栅格地图与代价地图、Lattice状态图、驾驶通道图等。为了兼顾实时性与障碍物约束空间处理能力，一般采用Lattice和通道图方法生成安全轨迹。

图搜索算法包括Dijkstra算法和A*算法等，以及A*算法的变种。

基于随机采样的路径规划算法

随机采样方法的基本思想是在构型空间中随机采样，并筛选出满足性能需求的最优采样点，具备概率完备性，但其最大的缺点是舒适性较差，且计算效率随着障碍物数量的增长而下降。最常用的方法包括概率路标算法（PRM）以及快速搜索随机树算法（RRT）。

1.概率路线图方法（PRM）

PRM算法首先在可行使空间中进行离线采样，构造出路线网络图（Roadmap），随后根据起始点与目标点在路线网络图中进行查询，得到可行的行驶路径。整个过程分为预处理阶段和查询阶段。

1.预处理阶段：对状态空间内的安全区域均匀随机采样ｎ个点，每个采样点分别与一定距离内的邻近采样点连接，并丢弃掉与障碍物发生碰撞的轨迹，最终得到一个连通图。

2.查询阶段：对于给定的一对初始和目标状态，分别将其连接到已经构建的图中，再使用搜索算法寻找满足要求的轨迹。

优点：适用于高维空间和复杂约束的路径规划问题；搜索效率高，搜索速度快

缺点：概率完备但不是最优

2.快速扩展随机树方法（RRT）

RRT算法是适用于高维空间，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，较好地处理带有非完整约束的路径规划问题，有效地解决了高维空间和复杂约束的路径规划问题。该算法是概率完备但不是最优的算法。

基于曲线插值方法

曲线插值法的核心思想就是基于预先构造的曲线类型，根据车辆期望达到的状态（位置、速度、加速度、航向角等），将这些期望值作为边界条件代入曲线类型进行方程求解，获得曲线的相关系数。所有系数计算出后，曲线轨迹规划完成。包括多项式参数化模型、样条曲线、螺旋线、回旋曲线和贝塞尔曲线等变种参数化曲线方法。

1.多项式参数化模型

设计思想是根据车辆的初始状态和目标状态对变道轨迹进行规划，使车辆在指定的时间到达相邻车道。试图在用函数f(x,y,t)描述的函数族类中寻找一条轨迹，能充分描述车辆从起始位置过渡到目标位置整个过程的动态特性。随着多项式次数的变大，曲线的拟合效果越好，但次数的增多也会导致参数求解的运算量指数增长，通常选用五次多项式进行变道轨迹的规划。在x方向、y方向分别选用五次多项式构造变道轨迹的曲线簇，如下式所示。