路径规划算法总结
目录
1 自主机器人近距离操作运动规划体系
········1.1 单个自主机器人的规划体系
········1.2 多自主机器人协同规划体系
2 路径规划研究
········2.1 图搜索法
················2.1.1 可视图法
················2.1.2 Dijkstra算法
················2.1.3 A*算法
········2.2 RRT算法
················2.2.1 算法步骤
················2.2.2 改进算法
········2.3 滚动在线RRT算法
················2.3.1 滚动规划
················2.3.2 滚动在线RRT算法流程
········2.4 人工势场法
················2.4.1 基本人工势场法
················2.4.2 人工势场法算法改进
········2.5 BUG算法
················2.5.1 BUG1算法
················2.5.2 BUG2算法
················2.5.3 TangentBUG算法
········2.6 增量式启发算法
················2.6.1 LPA*算法
················2.6.2 D* Lite算法
········2.7 小结
1 自主机器人近距离操作运动规划体系
在研究自主运动规划问题之前，首先需建立相对较为完整的自主运动规划体系，再由该体系作为指导，对自主运动规划的各项具体问题进行深入研究。本节将根据自主机器人的思维方式、运动形式、任务
行为等特点，建立与之相适应的自主运动规划体系。并按照机器人的数量与规模，将自主运动规划分为单个机器人的运动规划与多机器人协同运动规划两类规划体系。
1.1 单个自主机器人的规划体系
运动规划系统是自主控制系统中主控单元的核心部分，因此有必要先研究自主控制系统和其主控单元的体系结构问题。 自主控制技术研究至今，先后出现了多种体系结构形式，目前被广泛应用于实践的是分布式体系结构，其各个功能模块作为相对独立的单元参与整个体系。随着人工智能技术的不断发展，基于多Agent的分布式体系 结构逐渐成为了主流，各功能模块作为独立的智能体参与整个自主控制过程，该体系结构应用的基本形式如图1所示。一方面，主控单元与测控介入处理、姿态控制系统、轨道控制系统、热控系统、能源系统、数 传、有效载荷控制等功能子系统相互独立为智能体，由总线相连；另一方面，主控单元为整个系统提供整体规划，以及协调、管理各子系统Agent的行为。测控介入处理Agent保证地面系统对整个系统任意层面的控 制介入能力，可接受上行的使命级任务、具体的飞行规划和底层的控制指令；各子系统Agent存储本分系统的各种知识和控制算法，自主完成主控单元发送的任务规划，并将执行和本身的健康等信息传回主控单元， 作为主控单元Agent运行管理和调整计划的依据。 
图1 基于多Agent的分布式自主控制系统体系结构基本形式示意图 主控单元Agent采用主流的分层递阶式结构，这种结构层次鲜明，并且十分利于实现，其基本结构如图2所示。主控单元由任务生成与调度、运动行为规划和控制指令生成三层基本结构组成，由任务生成与调度 层获得基本的飞行任务，经过运动行为规划层获得具体的行为规划，再由控制指令生成层得到最终的模块控制指令，发送给其它功能Agent。各功能Agent发送状态信息给主控单元的状态检测系统，状态检测系统将 任务执行情况和子系统状态反馈回任务生成与调度层，以便根据具体情况对任务进行规划调整。当遇到突发情况时，还可启用重规划模块，它可根据当时情况迅速做出反应快速生成行为规划，用以指导控制指令生 成层得到紧急情况的控制指令。此外，地面控制系统在三个层次上都分别具有介入能力。图2中，点划线内是主控单元全部模块，虚线内为运动规划系统，包括运动行为规划模块和重规划模块，这也是运动规划系统 的主要功能。 
图2 主控单元基本结构示意图 明确了自主控制系统与其主控单元的基本结构，以及运动规划系统在主控单元中的基本功能，便可建立运动规划系统的体系结构。运动规划系统的体系结构如图3所示，该系统由规划器和重规划器两大执行单元 组成，分别承担对飞行任务的一般规划和对突发事件紧急处理的运动规划。当然，这两部分也可理解为离线规划与在线规划两种，离线规划一般解决平时按部就班的飞行任务，在线规划一般解决突然下达的飞行任 务。除规划器以外，系统还配有知识域模块，用以利用特定语言描述相关知识。知识域包括行为域和模型域两个部分，行为域用来存储服务系统一般的运动行为描述和紧急情况下的一些运动行为方面的处理方法 （如急停、转向等），模型域用来存储规划所需模型知识，包括环境模型、组装体模型、组装任务对象模型和任务模型等等。 
图3 运动规划系统体系结构示意图
1.2 多自主机器人协同规划体系
多智能体系统的群体体系结构一般分为集中式、分散式两种基本结构，分散式结构又可以进一步分为分层式和分布式结构。集中式结构通常由一个主控单元掌握全部环境和受控机器人信息，运用规划算法对任 务进行分解，并分配给各受控机器人，组织它们完成任务。其优点是理论条理清晰，实现较为直观；缺点是容错性、灵活性和对环境的适应性较差，与各受控机器人存在通讯瓶颈问题。相对于集中式结构，分散式 结构无法得到全局最优解，但它凭借着可靠性、灵活性和较强的环境适应性越来越受到广泛的青睐。分散式结构中的分布式结构没有主控单元，各智能体地位平等，通过各智能体间的通讯和信息交流达到协商的目 的，实现最终的决策，但该结构容易片面强调个体，导致占用资源过多，且难于得到磋商结果。分层式结构介乎于集中式和分布式之间，存在主控单元，但并不是由主控单元掌控一切，各智能体也具备一定的自主 性，上下级之间按照一定的规则，通过信息流形成完整的整体，共同完成协同任务。 多自主机器人系统应采用分层式结构，以保证整个系统既适于统一领导，又满足系统灵活、快速的需求。多自主机器人协同规划体系结构如图4所示，按照分层式结构建立两种工作模式：事先的离线规划由主 控单元负责，首先获得协同任务，经过规划器得到具体的行为运动规划，并分发给各分系统执行单元，相关的知识域中主要是用于描述各分系统协商规则的协商域，主控单元从外界获取环境信息，从各分系统获取 状态信息；当遇到突发事件或紧急任务变更以及主控单元停止工作时，各分系统采用分布式结构，单独规划各自运动行为，并从各自的知识域中获取协商方式，外界环境信息由主控单元发送和自我感知相结合获得 （主控单元停止工作时，仅靠自我感知获取信息），其它机器人信息的传输由机器人间的数据链实现。 
图4 多自主机器人协同规划体系结构示意图
2 路径规划研究
当给定了某一特定的任务之后，如何规划机器人的运动方式将至关重要。机器人的规划包括两部分内容：基座移动到适合操作的位置和转动手臂关节完成操作。包括三个问题：基座点到点运动规划；关节空间 规划；综合规划。 本章研究几种常用的运动规划算法：图搜索法、RRT算法、人工势场法、BUG算法。并对部分算法的自身缺陷进行了一些改进。
2.1 图搜索法
图搜索法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的可行路径。主要分成深度优先和广度优先两个方向。深度优先算法优先扩展搜索深度大的节点，可以快速的得到一条可行路径，但是 深度优先算法得到的第一条路径往往是较长的路径。广度优先算法优先扩展深度小的节点，呈波状的搜索方式。广度优先算法搜索到的第一条路径就是最短路径。
2.1.1 可视图法
由此可见，利用可视图法规划避障路径主要在于构建可视图，而构建可视图的关键在于障碍物各顶点之间可见性的判断。判断时主要分为两种情况，同一障碍物各顶点之间可见性的判断以及不同障碍物之间顶 点可见性的判断。 
可视图法能求得最短路径，但搜索时间长，并且缺乏灵活性，即一旦机器人的起始点和目标点发生改变，就要重新构造可视图，比较麻烦。可视图法适用于多边形障碍物，对于圆形障碍物失效。切线图法和 Voronoi图法对可视图法进行了改进。切线图法用障碍物的切线表示弧，因此是从起始点到目标点的最短路径的图，移动机器人必须几乎接近障碍物行走。其缺点是如果控制过程中产生位置误差，机器人碰撞障碍物 的可能性会很高。Voronoi图法用尽可能远离障碍物和墙壁的路径表示弧。因此，从起始点到目标点的路径将会增长，但采用这种控制方式时，即使产生位置误差，移动机器人也不会碰到障碍物。
2.1.2 Dijkstra算法
Dijkstra算法由荷兰计算机科学家艾兹赫尔·戴克斯特拉（Edsger Wybe Dijkstra）发明，通过计算初始点到自由空间内任何一点的最短距离可以得到全局最优路径。算法从初始点开始计算周围4个或者8个点与 初始点的距离，再将新计算距离的点作为计算点计算其周围点与初始点的距离，这样计算像波阵面一样在自由空间内传播，直到到达目标点。这样就可以计算得到机器人的最短路径。 Dijkstra算法是一种经典的广度优先的状态空间搜索算法，即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点。这样会大大增加计算时间和数据量。而且搜索得到的大量对于机器人运动是无 用的。 详情请参考另外一篇文章。
2.1.3 A*算法
为了解决Dijkstra算法效率低的问题，A*算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。 详情请参考：
2.2 RRT算法
快速搜索随机树（Rapidly Exploring Random Tree，RRT）算法是一种增量式采样的搜索方法，该方法在应用中不需要任何参数整定，具备良好的使用性能。它利用增量式方法构建搜索树，以逐渐提高分辨能 力，而无须设置任何分辨率参数。在极限情况，该搜索树将稠密的布满整个空间，此时搜索树由很多较短曲线或路经构成，以实现充满整个空间的目的。增量式方法构建的搜索树其导向取决于稠密采样序列，当该 序列为随机序列时，该搜索树称为快速搜索随机树（Rapidly Exploring Random Tree，RRT），而不论该序列为随机还是确定性序列，都被称为快速搜索稠密树（Rapidly Exploring Dense Trees，RDTs），这种规 划方法可处理微分等多种约束。
2.2.1 算法步骤
由于在搜索过程中考虑了机器人的动力学约束，因此生成的路径的可行性很好。但是算法的随机性导致其只具备概率完备性。
2.2.2 改进算法
LaValle等人的工作奠定了RRT方法的基础。在采样策略方面，RRTGoalBiaS方法在控制机器人随机运动的同时，以一定概率向最终目标运动；RRTGoalZoom方法分别在整个空间和目标点周围的空间进行采 样；RRTCon方法则通过加大随机步长改进规划速度。双向规划思想也被采用，衍生出RRTExtExt，RRTExtCon，RRTConCon等多种算法。 基本RRT算法收敛到终点位姿的速度可能比较慢。为了提高算法的效率和性能，需不断对该算法进行改进。如为了提高搜索效率采用双向随机搜索树(Bi~RRT)，从起始点和目标点并行生成两棵RRT，直至两棵 树相遇，算法收敛。由于这个算法相比于原始RRT有更好的收敛性，因此在目前路径规划中是很常见的。NikAMelchior提出的粒子RRT算法，考虑了地形的不确定性，保证了在不确定性环境下搜索树的扩展。 Kuffner和Lavane又提出RRT-connectlv，使得节点的扩展效率大大提高。运动规划中，距离的定义非常复杂，Pengcheng研究了在RRT生长过程中距离函数不断学习的算法以降低距离函数对环境的敏感性。考虑到 基本RRT规划器得到的路径长度一般是最优路径的1.3~1.5倍，英国的J.desmithl研究了变分法技术使其达到最优。Amna A引入KD树作为二级数据结构加速查找距离从环境中取出的随机点最近的叶节点，降低了搜 索成本。该算法在动态障碍物、高维状态空间和存在运动学、动力学等微分约束的环境中的运动规划已经得到广泛的应用。 关于改进RRT算法详情可参考：
2.3 滚动在线RRT算法
基本RRT算法倾向于遍历整个自由空间直到获得可行路径，这使其不可能用于未知或动态环境中的机器人在线运动规划。利用滚动规划的思想可以将RRT算法进行改进，使其具备在线规划能力。
2.3.1 滚动规划
机器人在未知或动态环境中运动时，只能探知其传感器范围内有限区域内的环境信息。机器人利用局部信息进行局部运动规划，并根据一定的评价准则得到局部目标。机器人到达局部目标后再次进行新的局部 规划。如此反复进行直到到达全局目标。 滚动规划算法的基本原理： 环境信息预测：在滚动的每一步，机器人根据探测到的视野内的信息、或所有已知的环境信息，建立环境模型，包括设置已知区域内的节点类型信息等； 局部滚动优化：将上述环境信息模型看成一个优化的窗口，在此基础上，根据目标点的位置和特定的优化策略计算出下一步的最优子目标，然后根据子目标和环境信息模型，选择局部规划算法，确定向子目标行进 的局部路径，并实施当前策略，即依所规划的局部路径行进若干步，窗口相应向前滚动； 反馈信息校正：根据局部最优路径，驱动机器人行走一段路径后，机器人会探测到新的未知信息，此时可以根据机器人在行走过程探测到的新信息补充或校正原来的环境模型，用于滚动后下一步的局部规划。 其中，局部子目标是在滚动窗口中寻找一个全局目标的映射，它必须避开障碍物，且满足某种优化指标。子目标的选择方法反映了全局优化的要求与局部有限信息约束的折衷，是在给定信息环境下企图实现全局优 化的自然选择。 基于滚动窗口的路径规划算法依靠实时探测到的局部环境信息，以滚动方式进行在线规划。在滚动的每一步，根据探测到的局部信息，用启发式方法生成优化子目标，在当前滚动窗口内进行局部路径规划，然后实 施当前策略(依局部规划路径移动一步)，随滚动窗口推进，不断取得新的环境信息，从而在滚动中实现优化与反馈的结合。由于规划问题压缩到滚动窗口内，与全局规划相比其计算量大大下降。 基于滚动窗口的路径规划算法的具体步骤如下： 步骤0：对起点、终点、工作环境、机器人的视野半径、步长进行初始化； 步骤1：如果终点到达，规划中止； 步骤2：对当前滚动窗口内的环境信息进行刷新； 步骤3：产生局部子目标； 步骤4：根据子目标及已知环境信息，在当前滚动窗口内规划一条优化的局部可行路径； 步骤5：依规划的局部路径行进一步，步长小于视野半径； 步骤6：返回步骤1。
2.3.2 滚动在线RRT算法流程
这里利用统计学中回归分析生成新节点，将RRT算法探索未知空间的能力进一步增强以避免因启发估价因子导致的局部极小。其思想是探索以前到过的空间是无用的，而且容易陷入局部极小。引进回归分析 (regression analysis)是考察新节点与其他节点之间关系，利用回归函数约束，使得随机树不探索以前到过的空间，因此避免了局部极小。 
上图中各个空心点是中间的父节点的可能扩展。椭圆圈起的空心点表示这个新节点不符合回归函数约束，剩下的两个未被圈起的空心节点到其父节点的距离小于该节点到随机树上任意节点的距离，这两个点可 以成为随机树的新节点。 
2.4 人工势场法
人工势场法是由Khatib提出的一种用于机器人运动规划的虚拟力方法。其基本思想是将目标和障碍物对机器人运动的影响具体化成人造势场。目标处势能低，障碍物处势能高。这种势差产生了目标对机器人的 引力和障碍物对机器人的斥力，其合力控制机器人沿势场的负梯度方向向目标点运动。人工势场法计算方便，得到的路径安全平滑，但是复杂的势场环境可能在目标点之外产生局部极小点导致机器人无法到达目 标。为了解决人工势场法的局部极小点问题，学者们提出了各种改进方法。主要分成两个方向：一个是构造合适的势函数以减小或避免局部极小点的出现；另一种是在机器人遇到局部极小点后结合其他的方法使机 器人离开局部极小点。前者一般需要全局地图信息，并且依赖于障碍物的形状。当环境复杂时难以应用。后者多利用搜索法、多势场法和沿墙行走法等方法使机器人离开局部极小点。搜索法利用最佳优先、模拟退 火、随即搜索等策略寻找比局部极小点势场值更低的点使机器人继续移动。由于未知环境中大多缺乏启发信息，搜索方法的效率很低。多势场法构造多个全局极小点相同，而局部极小点不同的势函数，在机器人陷 入某个局部极小点时，规划器就切换势函数使机器人离开该点。但是在未知的环境中这样的多个势场很难构造，而且该方法可能导致机器人在回到曾逃离的局部极小点。由于局部极小点是某个或多个障碍物的斥力 势场与引力势场共同作用产生，其位置与障碍物距离必然不远，沿墙行走法正是利用这样的远离，使机器人在遇到局部极小点后参照类似BUG算法的环绕行为绕过产生局部极小点的障碍物继续前进。这种方法可靠 性高，不依赖环境的先验信息和障碍物形状。 本节构造人工势场进行机器人平动的在线运动规划，利用一种沿墙行走法对基本的人工势场法进行改进。
2.4.1 基本人工势场法
作用在机器人上的假想引力和斥力为势函数的负梯度，因而人工势函数应该具有以下特征： 非负且连续可微； 斥力势强度距离障碍物越近其强度越大； 引力势强度离目标位置越近其强度越小。
2.4.2 人工势场法算法改进
当机器人的运行环境中包含形状复杂或者距离很近的障碍物时，可能出现势场局部极小点，导致机器人在该处停止或在其周围振动。如下图所示，当环境中出现“陷阱”形障碍物或者与目标成特定位置关系的障 碍物时，可能在人工势场中产生局部极小点（图中L点），当机器人运动到局部极小点附近时，势场的负梯度方向指向L点。机器人将在L点处停止或在其附近振动或作圆周运动。
图9 人工势场法的局部极小点 为了使机器人从局部极小点中逃离，在人工势场法的基础上引入应激行为，即增加绕行行为。当机器人遇到局部极小点时，忽略目标引力势的作用，沿着斥力势的等势面方向移动，直到机器人离开局部极小区 域。改进的算法流程如下： 根据传感器信息计算当前位置的引力和斥力； 判断是否处于绕行行为，若是，执行3；若否，执行4； 判断是否离开局部极小区域，若是，机器人沿着合力方向运动，结束绕行行为；若否，机器人沿着斥力场等势线运动，继续绕行行为； 判断是否遇到局部极小点，若是，机器人沿着斥力场等势线运动，开始绕行行为；若否，机器人沿着合力方向运动； 判断是否到达目标，若是，退出算法；若否，继续1； 使用下面的判别条件判断机器人是否遇到局部极小点。 
2.5 BUG算法
BUG算法是一种完全应激的机器人避障算法。其算法原理类似昆虫爬行的运动决策策略。在未遇到障碍物时，沿直线向目标运动；在遇到障碍物后，沿着障碍物边界绕行，并利用一定的判断准则离开障碍物继 续直行。这种应激式的算法计算简便，不需要获知全局地图和障碍物形状，具备完备性。但是其生成的路径平滑性不够好，对机器人的各种微分约束适应性比较差。
2.5.1 BUG1算法
该算法的基本思想是在没有障碍物时，沿着直线向目标运动可以得到最短的路线。当传感器检测到障碍物时，机器人绕行障碍物直到能够继续沿直线项目标运动。BUG1算法实现了最基本的向目标直行和绕行障 碍物的思想。
图11 BUG1算法中认为机器人无法到达目标点的情况 
图12 BUG1算法伪代码
2.5.2 BUG2算法
图13 BUG2算法运动规划 
图14 BUG2算法中认为机器人无法到达目标点的情况 
图15 BUG2算法伪代码 BUG1和BUG2算法提供了搜索问题的两种基本方法：比较保守的BUG1算法进行详细的搜索来获得最佳的离开点。这需要机器人环绕整个障碍物的边界。而BUG2算法使用一种投机的方法。机器人不环绕完整的 障碍物，而选择第一个可用的点作为离开点。对于一般的环境，BUG2算法的效率更高；而对于复杂形状的障碍物，保守的BUG1算法性能更优。
2.5.3 TangentBUG算法
图16 距离传感器扫描障碍物 
与其他的BUG算法一样，TangentBUG算法也有两种行为：直行（motion-to-go）和环绕障碍物（boundary-following）。
图19 
图20
2.6 增量式启发算法
2.6.1 LPA*算法
LPA*算法，即Lifelong Planning A*算法，该算法于2001年由Sven Koenig和Maxim Likhachev提出，是一种增量启发式搜索版本的A*算法，这种路径规划算法适用于随着时间改变导致有限栅格地图上的边缘 代价c(s1,s2)改变的问题，也就是随着时间改变障碍物增多或减少，网格点发生增删等，在许多场合下比再利用A*重新搜索更高效。 详情请参考： 搬砖的旺财：路径规划——Lifelong Planning A*算法zhuanlan.zhihu.com
2.6.2 D* Lite算法
D* Lite算法是以LPA*为基础，是Maxim Likhachev和Sven Koenig于2002年基于LPA*，结合A*算法思想，提出一种增量启发式算法，适用于在未知环境中的导航以及路径规划，广泛用于目前各种移动机器人和自主车辆载具，例如“机遇号”和“勇气号”火星车测试的原型导航系统。 详情请参考： 搬砖的旺财：路径规划——D* Lite算法zhuanlan.zhihu.com
关于A*、LPA*、D* Lite算法的小结，详情请参考： 搬砖的旺财：路径规划——关于A*、LPA*、D* Lite算法的小结zhuanlan.zhihu.com
搬砖的旺财：路径规划——启发式动态规划算法发展研究zhuanlan.zhihu.com
2.7 小结
本章研究了几种常用的运动规划算法。其中，人工势场法应用灵活，可以在保证安全的情况下获得一条平滑路径，并且对于动态环境可以实现实时运动控制。适合用于长距离机动且障碍物较少的情况。而基于 随机采样的搜索树方法可以在复杂约束环境中获得可行解，适合用于机械臂近距离操作。 参考文献： 【1】Choset H , Kantor G A , Thrun S . Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. MIT Press, 2005. ———————————————— 版权声明：本文为CSDN博主「Techblog of HaoWANG」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。 原文链接：https://blog.csdn.net/hhaowang/article/details/101432545
