路径规划在自动驾驶技术中起到承上启下的重要作用。路径规划在遵循道路交通规则的前提下，计算出具体运动指令，将车辆从当前位置导航到目的地。本文将介绍路径规划在自动驾驶系统中的架构以及路径规划的常见类型——基础图搜索算法、启发式算法、基于概率采样的算法以及基于人工智能的算法。

    Claudine Badue^[1]等人以圣西班牙联邦大学（UFES）开发的自动驾驶汽车（Intelligent Autonomous Robotics Automobile，IARA）为例，提出了自动驾驶汽车的自动驾驶系统的典型架构。如图所示，自动驾驶系统主要由感知系统（Perception System）和规划决策系统（Decision Making System）组成。感知系统主要由交通信号检测模块（Traffic Signalization Detector，TSD）、移动目标跟踪模块（Moving Objects Tracker，MOT）、定位与建图模块（Localizer and Mapper）等组成。规划决策系统主要由全局路径规划模块（Route Planner）、局部路径规划模块（Path Planner）、行为决策模块（Behavior Selector）、运动规划模块（Motion Planner）、自主避障模块(Obstacle Avoider)以及控制模块（Controller）组成。

    全局路径规划在此架构中主要是由全局路径规划模块完成的。该模块通过接收用户在离线地图（Offline Maps）中给出的目标点，基于由感知系统提供的感知信息，计算出从当前自动驾驶汽车的位置到最终目标点的路径 W W W。路径 W W W是一个路径点的序列，即 W = { w 1 ， w 2 , , , , w ∣ W ∣ } W=\{{w_1，w_2,,,,w_{|W|}}\} W={w1​，w2​,,,,w∣W∣​}，其中每个路径点 w i w_i wi​，是一个坐标对，在离线地图中， w i = ( x i , y i ) w_i=(x_i,y_i) wi​=(xi​,yi​)。得到路径 W W W后，将路径（Route）信息传递给下一个模块——局部路径规划模块。

    全局路径规划模块负责计算一条从自动驾驶汽车的初始位置到由用户操作员定义的最终位置的路线W，。路径W是一个路径点的序列，即 W = { w 1 ， w 2 , , , , w ∣ W ∣ } W=\{{w_1，w_2,,,,w_{|W|}}\} W={w1​，w2​,,,,w∣W∣​}，其中每个路径点 w i w_i wi​，是一个坐标对，在离线地图中， w i = ( x i , y i ) w_i=(x_i,y_i) wi​=(xi​,yi​)。
    如果我们将道路网络看做成一个带有边权值的有向图，该有向图中的顶点通过带有权值的边连接起来，可以将一般的路径规划问题转化为有向图中最短路径的求解问题。对于大型的静态路网问题，Dijkstra^[2]和A*^[3]等经典算法由于时间复杂度过大，难以取得较好的成果。
    在过去的十年中，道路网络中路径规划算法的性能有了显著的进展。新开发的算法可以以毫秒或更少的时间计算驾驶方向。道路网络中的路径规划技术在查询时间、预处理时间、空间使用和对输入变化的鲁棒性等方面提供了不同的权衡。不同的学者对路径规划算法的分类有所不同。Claudine Badue在他的文章中将全局路径规划算法分为以下几类:

• 基于目标导向的全局路径规划（Goal-directed techniques）
• 基于分割的全局路径规划（Separator-based techniques）
• 分级规划的全局路径规划（Hierarchical techniques）
• 有界跳跃的全局路径规划（Bounded-hop techniques）

    Bast^[4]等人的分类方式与Claudine作出的分类方式几乎一致。他们在上述四种算法的基础之上，不仅仅是对静态网络中两点之间的最短路径的长度的求解。最重要的是，他们还扩展到能够检索到最短的路径本身。此外，他们还在批量计算最短路径（如距离表），更现实的场景（如动态网络），或处理多个目标函数等多方面进行探索与验证。
    David González^[5]等人虽然主要考虑了运动规划方面的分类方式，但其中有些方法对于全局路径规划仍然是通用的。他们将一般的规划算法分为:

• 基于采样搜索的算法:Dijkstra 、 RRT 、 A *、 hybird A *和 Lattice 等；
• 基于曲线插值的算法: RS 曲线、 Dubins 曲线、多项式曲线、贝塞尔曲线和样条曲线等；
• 基于最优化的算法: Apollo 的 piecewise - jerk、EM Planner 等。

    Brian Paden^[24]对常见的路径规划算法从适用场合、最优性、完备性、时间复杂度以及实时性进行分析（表2-2）。

    Dijkstra算法^[2]是由荷兰计算机科学家迪杰斯特拉于1959年提出的，是从一个节点遍历其余各节点的最短路径算法，解决的是有权图中最短路径问题。迪杰斯特拉算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。

• Dijkstra算法将图上的初始点看作一个集合S，其它点看作另一个集合U。
• 根据初始点，求出其它点到初始点的距离dist[i]（若相邻，则dist[i]为边权值；若不相邻，则dist[i]为无限大）。
• 之后选取最小的dist[i]（记为dist[x]），并将此dist[i]边对应的点（记为x）加入集合S（实际上，加入集合的这个点的dist[x]值就是它到初始点的最短距离）。
• 再根据x，更新跟x相邻点y的dist[y]值:dist[y] = min{ dist[y], dist[x] +边权值weight[x][y] }，因为可能把距离调小，所以这个更新操作叫做松弛操作。
• 重复3，4两步，直到目标点也加入了集合，此时目标点所对应的dist[i]即为最短路径长度。（注:重复第三步的时候，应该从所有的dist[i]中寻找最小值，而不是只从与x点相邻的点中寻找）。

    下图展示了Dijkstra算法、双向搜索算法以及A算法的搜索空间。Dijkstra算法的搜索空间较大，双向搜索算法通过从起始点和终点同时搜索，两个搜索空间相遇之时即在x点处，结束算法，其搜索空间比Dijkstra算法小。

    双向搜索算法可以用来减少搜索空间，它同时运行s的前向搜索和t的向后搜索。一旦它们的搜索空间的交点被证明在从s到t的最短路径上包含一个顶点x，该算法就会停止。对于静态路网最短路径问题，双向搜索访问的顶点数量大约是单向方法的一半。

    Floyd算法^[6]在Dijkstra算法的基础上进行了改进，使之不仅能求得起始点到目标点的距离，还能求得图中任意一节点到另一节点的最短距离。Floyd算法的实现依靠两个邻接矩阵，其中一个临界矩阵存放各个路径的顶点，另一个临界矩阵存放各个路径的距离值，通过一行一行的更新，最终获得图中任意一节点到另一节点的最短距离。

    启发式方法一种比传统方法更快地解决问题的技术，它会在每一个分支步骤，评价可用信息并决定遵循哪个分支，得到一个最优或近似最优解。大多数搜索问题的数据量都是指数级的，而启发式方法可以将问题降低一个数量级，而且，当可评估的信息越多，搜索的指向性就越明确，从而提高搜索效率。常见的启发式方法有Astar算法和Dstar算法。

    Astar搜索方法^[3]最初是Hart等人在1968年作为经典Dijkstra算法的修改版本引入的。Astar算法通过引导搜索方向的启发式算法来改进Dijkstra算法，并通过这样做减少必要的计算次数。Astar算法的关键是建立评估函数 f ( n ) f(n) f(n)， f ( n ) = g ( n ) + h ( n ) f(n)=g(n)+h(n) f(n)=g(n)+h(n)，其中 g ( n ) g(n) g(n)表示实际成本， h ( n ) h(n) h(n)表示最佳路径的估计成本。假设在一个二维平面中的目标位置坐标为 （ x g o a l ， y g o a l ） （x_{goal}，y_{goal}） （xgoal​，ygoal​），对于一个平面中的坐标节点 （ x , y ） （x,y） （x,y）， h ( n ) h(n) h(n)的值通常取两个节点之间的欧几里德距离。但是这种方式很容易忽略了障碍物，从而低估了实际成本，也就是说存在障碍物的情况下距离目标点的距离很容易被低估。

    在现实世界中，移动机器人通常对通向目标的路径上的障碍物信息知之甚少。随着机器人在运动过程中发现新的障碍，这种知识会逐渐得到改善。因此，当接收到有关环境的新信息时，对目标的计算路径进行更新是非常重要的。换句话说，当要搜索的区域发生时，重新规划是必要的。前面介绍的Astar算法除了从头开始图搜索之外，没有为重新规划问题提供任何解决方案。

    基于采样的算法在20世纪90年代末引入，以获得路径规划问题的近似解。基于采样的方法将状态空间表示为图形，并尝试使用不同的随机采样技术找到从初始状态到最终状态的路径。基于采样的规划器遵循概率完备性原则，这意味着如果存在解决方案，就一定会找到它。但需要注意的是，这种方法不能保证解决方案的最优性。基于采样的算法是根据算法的收敛速度和平均收敛速度来判断的。根据实际实验观察和统计，基于采样的算法平均至少比多项式时间分析算法快一个数量级。目前，两个主要基于采样的算法是由Kavraki LE 1994年提出的概率路线图^[11]（PRM）和LaValle SM 1998年提出的快速随机探索树^[12]（RRT）。

    概率路线图算法包括两个步骤路线图构建和查询，即学习阶段和查询阶段。在学习阶段，对状态空间中的一定数量的随机状态进行采样，并生成连接这些状态的图表。这是一个计算昂贵的预处理步骤，但只需要执行一次。在查询阶段，输入初始和最终状态，并使用 Dijkstra或者Astar算法从图表中获得它们之间的轨迹。在多个查询的情况下，概率路线图是非常有效的。但PRM存在许多问题，所以相关学者对其做出了变种改进。PRMstar[13]引入了一种可变距离度量，以提高优化性。灵活的随时动态随机路线图^[14]（Flexible Anytime Dynamic PRM， FADPRM）将地图环境划分为理想的区域，根据边界边的优先级形成启发式算法，进行了Astar搜索，以不断改进该解决方案。

    图2-4与图2-5阐述了PRM算法的流程。首先在二值化地图中进行构型采样（大约750个点），并进行碰撞检测，筛选出无碰撞的点。再进行邻域计算，对每个周围一定邻域距离以内的点进行连接并对连线进行碰撞检测。再对所有的点用图搜索算法找到一条最短路径，最后通过路径修剪的方法，得到最终的路径。

    RRT 算法通过随机选择在非障碍物区域的点，构建单向搜索树，直到树权到达目标节点。RRT树在未占用空间中均匀扩展的属性，使得在复杂的高维环境、非完整和运动约束环境中依旧是有效的。但是生成的路径往往不是最优的并且需要进行平滑优化。相比于PRM，RRT作为一种增量算法，不需要使用一定数量的样本预计算图，对于具有动态环境和约束变化的场景更为有利。因此专家学者从规划时间、扩展节点、路径成本和路径间隙四个方面对RRT方法做出了相应地改进，如RRT-Connect、RRTstar、Bi-RRT^[15-17]等。图2-6展示了RRT、RRTstar以及RRTstarFN的算法效果。RRTstar的路径明显较RRT短，RRTstarFN既保证了路径的距离优势，同时节省了大量冗余的节点，提升了算法速度。

    RRTstarFN的核心在于在原有RRTstar的方法上，通过强制移除一些子节点为空的节点，从而减少运算时的节点数目。

    经典的方法虽然被发现是有效的，但需要更多的时间来确定可行的无碰撞路径。而且，经典的方法往往被锁定在局部最优解中，这可能远不及全局最优解。此外，存在多个障碍物的移动机器人的路径规划被发现是非确定性多项式时间困难（NP-hard）问题。因此，在动态环境情况下就会变得更加复杂。这些缺点使得传统方法在复杂环境中显得无能为力。因此，人工神经网络（ANN）、遗传算法（GA），粒子群优化算法（PSO），蚁群优化算法（ACO）和模拟退火算法（SA）等进化算法来快速求解路径规划问题。

    人工神经网络（ANN）可以表达路径规划感知到行为的映射关系。神经网络用于描述环境之间的约束，并且将能量定义为关于路径点的函数。能量水平取决于路径点的位置，而机器人朝着能量降低的方向移动。最后得到一条总能量最小的路径。虽然这条路径没有障碍，但它并不一定是最短路径或最佳路径。同时很难定量地描述移动机器人随机的工作环境，因此建立神经网络拓扑来描述移动环境也是非常困难的。另外，复杂且庞大的结构也加大了神经网络权重设置的难度和复杂度。

    遗传算法（GA）^[18]由Holland于1975年提出。在遗传算法中，问题的所有可能解都编码为形成初始种群的染色体。构建了几个基本操作∶交叉，变异和选择。生成初始种群，利用目标计算个体的适合度值，以便确定可选择的基本操作。GA虽然具有强大的搜索能力和较高的搜索效率，但容易提前收敛的问题使得它它接近问题的最优解时收敛速度降低。

    1995年，Eberhart和Kennedy^[19-20] 研究鸟类群体的活动规律，提出了粒子群优化算法（PSO）。它从随机解开始，通过不停地迭代找到最优解。然后利用适应度值对解的质量进行评价，最后与当前搜索的最优值进行比较，找出全局最优解。该算法用于求解机器人路径规划问题，具有实现简单、精度高、收敛速度快等优点。

    粒子群优化（PSO）是一种极好的基于种群的优化算法，经常应用于群体机器人协同搜索工作。考虑到PSO的全局优化特性，它很容易收敛到搜索环境中的特定位置，从而错过了学习更多信息的机会。V. Garg^[21]在PSO的基础上进行改进，提出了一种自适应探索机器人PSO（AERPSO）来解决多目标搜索问题。该方法提高了探索未探索区域的机会，并有助于利用进化速度和聚集程度进行避障。自适应惯性权重有助于增强探索。

    ACO算法由 Marco Dorigo于1992年提出。它的基本原理是每只蚂蚁在其行走的路径上释放分泌物作为参考，并且还将感知其他蚂蚁释放的分泌物，这种分泌物通常被称为信息素。在信息素的作用下，蚁群可以相互通信并选择路径。当路径上的信息素比其他路径更多时，蚁群会自发地移动到这条路径，导致信息素浓度升高，从而吸引后者的蚂蚁形成正向机制反馈。最后，整个蚁群集中在最佳路径上。

    蚁群算法不仅具有种群的全局搜索能力，而且具有个体间的协同效应。它可以找到一条更好的道路，即使环境的完整信息是未知的。然而，在算法的早期阶段，收敛速度慢，计算量大。
    在三维路径规划问题中，理想的飞行路径应该是平衡总飞行路径长度和地形威胁并在此基础之上缩短飞行时间、减少碰撞的可能性。但传统的三位路径规划算法往往不能较好地权衡这些因素，且优化后的目标函数缺乏实际约束，导致建模不准确。对此，Yuting Wan等^[22]提出了APPMS算法，该算法将路径规划任务转换为具有多约束的多目标优化任务，同时对基于飞行总路径长度和地形威胁程度的目标进行优化。此外，为了获得最优三维飞行路径，引入了一种基于改进蚁群优化的精确群智能搜索方法，该方法可以利用首选搜索方向和随机邻域搜索机制提高全局和局部搜索能力。图2-10描述了APPMS算法的流程。

    模拟退火（SA）算法由N.Metropolis 等人1953年提出。它是一种基于蒙特卡洛迭代求解的随机优化算法。SA从一定的较高初始温度开始，随着温度参数的不断减小，随机求解空间中目标函数的全局最优解，并结合概率跳跃特征。 Amylia Ait saadi^[23]提出了一种基于混沌混合优化和模拟退火的混合优化方案（CAOSA）来解决无人机的路径规划问题。与其他九种经典的元启发算法相比，其规划出的路径更短，时间相对较短，如图2-11与图2-12所示。

    通常来说,在任何水域中,鱼都能自行或尾随其他鱼找到营养物质丰富的地方。人工鱼群算法就是根据该特点,通过构造人工鱼来模仿鱼群的觅食、聚群和追尾以及鱼群之间的相互协助等自然行为,最终达到全局寻优的目的^[25]。
    人工鱼群算法通过生物界鱼类的3种自然行为进行相关迭代计算,从而获得路径规划最优解 。因此,该算法实际上也是一种仿生智能算法,每个人工鱼可以根据实时变化的环境因素自适应地调整角度进行搜索,最终使人工鱼聚集到食物源附近。因此,该算法与其他传统优化方法相比,在解决路径规划问题时能够良好地克服局部极值,并且具有一定的自适应能力。
    接下来简要说明一下人工鱼群算法的基本含义。如图2-13所示,某一人工鱼当前的活动状态为 X X X,首先假设它的最大视野范围为 V i s u a l Visual Visual。在该范围内某时刻人工鱼视点位置状态定义为 X v X_v Xv​,如果该位置的状态优于当前状态,则向该位置方向前进一步,到达下一状态 X n e x t X_{next} Xnext​,否则继续搜索视野范围内的其他位置。如果搜寻的次数越多,则能对周围的环境有一个更加全面的感知,有助于作出合理的选择。另外,针对状态较多甚至无限多的情况,并不需要全部遍历,存在一定的不确定性对于搜索全局最优其实是更有利的。

    以上这些行为是鱼类的基本行为,它们与鱼的生存状态紧密联系。从学习方式上来说,觅食行为是一种个体极值寻优的过程,属于自学过程,而聚群与追尾行为是鱼和外界环境相互交互的过程。

    人工蜂群算法其实是由蜜蜂寻找食物源的过程启发而来的。该算法在求解路径规划问题时,蜜源代表一条可行路径,而蜜源的收益度代表该问题的约束条件(如时间、路径长度、避障情况等)。整体上来说,该算法就是一个迭代的过程 。该算法的基本模型主要涉及3种类型的蜜蜂:雇佣蜂、侦察蜂和观察蜂 。雇佣蜂在其当前处于的食物源附近进行局部搜索,若发现更优食物源,立刻更新所在位置。观察蜂则辅助雇佣蜂在食物源附近邻域搜索。当陷入局部最优情况时,雇佣蜂则转换为侦察蜂的角色,重新开始搜索。

    入侵杂草算法(invasive weed optimization)是一种模拟自然界杂草繁殖过程的一种新型智能优化方法,具有易于理解、参数少、稳定性高等优点。在农耕活动中,人们总会或多或少地残留一些多余的资源,杂草利用这些资源不断繁衍后代并占据其他的土地,产生更多的种子,这些种子又会继续繁殖下去。另外,杂草根据“适者生存”的法则,对环境条件适应性好的杂草将会生存下来,反之则被淘汰。受该自然现象的启发,Mehrabian和Lucas于2016年提出了一种新的智能优化方法—入侵杂草算法 。
    在入侵杂草算法中,杂草代表路径规划问题的最优解,种群代表所有杂草的集合。在进化过程中,杂草通过不断繁殖产生种子,种子又陆陆续续发育成杂草,当种群中的杂草数量达到预先设定的最大种群规模时,则通过竞争法则来保存适应性好的杂草,淘汰适应性较差的 。