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

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

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

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

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

01、最常用的传统经典算法

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算法一致，它将每个节点的信息都保存下来。

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

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

02、人工势场法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

05、基于曲线插值方法

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

1.多项式参数化模型

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

在道路结构的约束下，由五次多项式规划的曲线无论是在纵向上还是在侧向上都能达到期望的位置，车辆能在规定的变道时间内平顺的完成变道，且轨迹曲线的曲率在起始点和终了点都能达到零的期望值。

但是，基于多项式的轨迹规划方法也存在变道时间和终了点必须预先已知的局限，对多项式中参数的确定需要有较充分的条件，对纵向车速变化的情况和实际车辆变道过程中终了点并不唯一的机动性和自适应性较差。

多项式曲线通常有三次、五次和七次多项式曲线，每个能确定的每一个期望点的维度数不一样，三次多项式是位置和速度；五次多项式是位置、速度和加速度；七次多项式是位置、速度、加速度和加加速速度。

2.贝塞尔曲线

贝塞尔曲线于1962年，由法国工程师皮埃尔·贝济埃（Pierre Bézier）所广泛发表，他运用贝塞尔曲线来为汽车的主体进行设计,贝塞尔曲线最初由保尔·德·卡斯特里奥于1959年运用德卡斯特里奥算法开发，以稳定数值的方法求出贝塞尔曲线。

基于贝塞尔缺陷的路径规划方法通过控制点的选取来改变曲线的形状，通常定义N阶贝塞尔曲线由N+1个控制点组成。

贝塞尔曲线

表达式为：

Pi、t分别是控制点i的坐标值与时间参数，上图中式子2是Bi(t)是Bernstein多项式。

因其线条光滑且曲率值小的特点而被广泛地应用于轨迹曲线规划中。 

以上几种算法简单总结：

最经典普适的Dijkstra算法，如有最优路径存在一定能找到最与路径，但是扩展的结点多，效率低下；和A*算法适合全局路径规划，利用启发式函数，搜索范围小，提高了搜索效率，但不适用于高维空间，计算量大；D*算法适用于局部路径规划，搜索效率高，但是计算量大。

图搜索法适合做全局规划，算法收敛慢，环境建模复杂，计算效率低；随机采样法适用于局部范围场景，计算速度较快，但是难以胜任复杂条件下的运动规划问题。

曲线插值法求解路径的速度较快，并且能够满足路径平滑性、可行性，但是无法充分发挥车辆的全部运动能力；人工势场法计算速度快，实时性也好，但存在局部最优解、复杂势场难以搭建的情况。

以上为几大常见规划算法分享，欢迎评论区各位工程师们一起探讨不同规划算法的使用情况。

后面也会持续分享基于不同规划算法的公式推导、建模的干货。