总结 ｜ 六大路径规划算法

点击上方“小白学视觉”，选择加"星标"或“置顶”

仅作学术分享，不代表本公众号立场，侵权联系删除

转载于：https://zhuanlan.zhihu.com/p/51372134，?知乎ID:搬砖的旺财

1.自主机器人近距离操作运动规划体系

在研究自主运动规划问题之前，首先需建立相对较为完整的自主运动规划体系，再由该体系作为指导，对自主运动规划的各项具体问题进行深入研究。本节将根据自主机器人的思维方式、运动形式、任务行为等特点，建立与之相适应的自主运动规划体系。并按照机器人的数量与规模，将自主运动规划分为单个机器人的运动规划与多机器人协同运动规划两类规划体系。

运动规划系统是自主控制系统中主控单元的核心部分，因此有必要先研究自主控制系统和其主控单元的体系结构问题。

自主控制技术研究至今，先后出现了多种体系结构形式，目前被广泛应用于实践的是分布式体系结构，其各个功能模块作为相对独立的单元参与整个体系。随着人工智能技术的不断发展，基于多Agent的分布式体系结构逐渐成为了主流，各功能模块作为独立的智能体参与整个自主控制过程，该体系结构应用的基本形式如图1所示。一方面，主控单元与测控介入处理、姿态控制系统、轨道控制系统、热控系统、能源系统、数传、有效载荷控制等功能子系统相互独立为智能体，由总线相连；另一方面，主控单元为整个系统提供整体规划，以及协调、管理各子系统Agent的行为。测控介入处理Agent保证地面系统对整个系统任意层面的控制介入能力，可接受上行的使命级任务、具体的飞行规划和底层的控制指令；各子系统Agent存储本分系统的各种知识和控制算法，自主完成主控单元发送的任务规划，并将执行和本身的健康等信息传回主控单元，作为主控单元Agent运行管理和调整计划的依据。

图1 基于多Agent的分布式自主控制系统体系结构基本形式示意图

主控单元Agent采用主流的分层递阶式结构，这种结构层次鲜明，并且十分利于实现，其基本结构如图2所示。主控单元由任务生成与调度、运动行为规划和控制指令生成三层基本结构组成，由任务生成与调度层获得基本的飞行任务，经过运动行为规划层获得具体的行为规划，再由控制指令生成层得到最终的模块控制指令，发送给其它功能Agent。各功能Agent发送状态信息给主控单元的状态检测系统，状态检测系统将任务执行情况和子系统状态反馈回任务生成与调度层，以便根据具体情况对任务进行规划调整。当遇到突发情况时，还可启用重规划模块，它可根据当时情况迅速做出反应快速生成行为规划，用以指导控制指令生成层得到紧急情况的控制指令。

此外，地面控制系统在三个层次上都分别具有介入能力。图2中，点划线内是主控单元全部模块，虚线内为运动规划系统，包括运动行为规划模块和重规划模块，这也是运动规划系统的主要功能。

图2? 主控单元基本结构示意图

明确了自主控制系统与其主控单元的基本结构，以及运动规划系统在主控单元中的基本功能，便可建立运动规划系统的体系结构。运动规划系统的体系结构如图3所示，该系统由规划器和重规划器两大执行单元组成，分别承担对飞行任务的一般规划和对突发事件紧急处理的运动规划。当然，这两部分也可理解为离线规划与在线规划两种，离线规划一般解决平时按部就班的飞行任务，在线规划一般解决突然下达的飞行任务。除规划器以外，系统还配有知识域模块，用以利用特定语言描述相关知识。知识域包括行为域和模型域两个部分，行为域用来存储服务系统一般的运动行为描述和紧急情况下的一些运动行为方面的处理方法（如急停、转向等），模型域用来存储规划所需模型知识，包括环境模型、组装体模型、组装任务对象模型和任务模型等等。

图3 运动规划系统体系结构示意图

多智能体系统的群体体系结构一般分为集中式、分散式两种基本结构，分散式结构又可以进一步分为分层式和分布式结构。集中式结构通常由一个主控单元掌握全部环境和受控机器人信息，运用规划算法对任务进行分解，并分配给各受控机器人，组织它们完成任务。其优点是理论条理清晰，实现较为直观；缺点是容错性、灵活性和对环境的适应性较差，与各受控机器人存在通讯瓶颈问题。相对于集中式结构，分散式结构无法得到全局最优解，但它凭借着可靠性、灵活性和较强的环境适应性越来越受到广泛的青睐。分散式结构中的分布式结构没有主控单元，各智能体地位平等，通过各智能体间的通讯和信息交流达到协商的目的，实现最终的决策，但该结构容易片面强调个体，导致占用资源过多，且难于得到磋商结果。分层式结构介乎于集中式和分布式之间，存在主控单元，但并不是由主控单元掌控一切，各智能体也具备一定的自主性，上下级之间按照一定的规则，通过信息流形成完整的整体，共同完成协同任务。

多自主机器人系统应采用分层式结构，以保证整个系统既适于统一领导，又满足系统灵活、快速的需求。多自主机器人协同规划体系结构如图4所示，按照分层式结构建立两种工作模式：事先的离线规划由主控单元负责，首先获得协同任务，经过规划器得到具体的行为运动规划，并分发给各分系统执行单元，相关的知识域中主要是用于描述各分系统协商规则的协商域，主控单元从外界获取环境信息，从各分系统获取状态信息；当遇到突发事件或紧急任务变更以及主控单元停止工作时，各分系统采用分布式结构，单独规划各自运动行为，并从各自的知识域中获取协商方式，外界环境信息由主控单元发送和自我感知相结合获得（主控单元停止工作时，仅靠自我感知获取信息），其它机器人信息的传输由机器人间的数据链实现。

图4 多自主机器人协同规划体系结构示意图

2.路径规划研究

当给定了某一特定的任务之后，如何规划机器人的运动方式将至关重要。机器人的规划包括两部分内容：基座移动到适合操作的位置和转动手臂关节完成操作。包括三个问题：基座点到点运动规划；关节空间规划；综合规划。

本章研究几种常用的运动规划算法：图搜索法、RRT算法、人工势场法、BUG算法。并对部分算法的自身缺陷进行了一些改进。

图搜索法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的可行路径。主要分成深度优先和广度优先两个方向。深度优先算法优先扩展搜索深度大的节点，可以快速的得到一条可行路径，但是深度优先算法得到的第一条路径往往是较长的路径。广度优先算法优先扩展深度小的节点，呈波状的搜索方式。广度优先算法搜索到的第一条路径就是最短路径。

可视图法由Lozano-Perez和Wesley于1979年提出，是机器人全局运动规划的经典算法。可视图法中，机器人用点来描述，障碍物用多边形描述。将起始点??、目标点??和多边形障碍物的各顶点(设??是所有障碍物的顶点构成的集合)进行组合连接，要求起始点和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物，即直线是“可视的”。给图中的边赋权值，构造可见图??。其中点集??，??为所有弧段即可见边的集合。然后釆用某种优化算法搜索从起始点??到目标点??的最优路径，那么根据累加和比较这些直线的距离就可以获得从起始点到目标点的最短路径。

图5 可视图

由此可见，利用可视图法规划避障路径主要在于构建可视图，而构建可视图的关键在于障碍物各顶点之间可见性的判断。判断时主要分为两种情况，同一障碍物各顶点之间可见性的判断以及不同障碍物之间顶点可见性的判断。

1. 同一障碍物中，相邻顶点可见(通常不考虑凹多边形障碍物中不相邻顶点也有可能可见的情况)，不相邻顶点不可见，权值赋为??。

2. 不同障碍物之间顶点可见性的判断则转化为判断顶点连线是否会与其它顶点连线相交的几何问题。如下图虚线所示，、?分别是障碍物?、?的顶点，但??与??连线与障碍物其它顶点连线相交，故?、?之间不可见；而实线所示的??与??连线不与障碍物其它顶点连线相交，故??、??之间可见。

?图6 顶点可见性判断

可视图法能求得最短路径，但搜索时间长，并且缺乏灵活性，即一旦机器人的起始点和目标点发生改变，就要重新构造可视图，比较麻烦。可视图法适用于多边形障碍物，对于圆形障碍物失效。切线图法和Voronoi图法对可视图法进行了改进。切线图法用障碍物的切线表示弧，因此是从起始点到目标点的最短路径的图，移动机器人必须几乎接近障碍物行走。其缺点是如果控制过程中产生位置误差，机器人碰撞障碍物的可能性会很高。Voronoi图法用尽可能远离障碍物和墙壁的路径表示弧。因此，从起始点到目标点的路径将会增长，但采用这种控制方式时，即使产生位置误差，移动机器人也不会碰到障碍物。

Dijkstra算法由荷兰计算机科学家艾兹赫尔·戴克斯特拉（Edsger Wybe Dijkstra）发明，通过计算初始点到自由空间内任何一点的最短距离可以得到全局最优路径。算法从初始点开始计算周围4个或者8个点与初始点的距离，再将新计算距离的点作为计算点计算其周围点与初始点的距离，这样计算像波阵面一样在自由空间内传播，直到到达目标点。这样就可以计算得到机器人的最短路径。

Dijkstra算法是一种经典的广度优先的状态空间搜索算法，即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点。这样会大大增加计算时间和数据量。而且搜索得到的大量对于机器人运动是无用的。

为了解决Dijkstra算法效率低的问题，A*算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。

快速搜索随机树（RRT）算法是一种增量式采样的搜索方法，该方法在应用中不需要任何参数整定，具备良好的使用性能。它利用增量式方法构建搜索树，以逐渐提高分辨能力，而无须设置任何分辨率参数。在极限情况，该搜索树将稠密的布满整个空间，此时搜索树由很多较短曲线或路经构成，以实现充满整个空间的目的。增量式方法构建的搜索树其导向取决于稠密采样序列，当该序列为随机序列时，该搜索树称为快速搜索随机树（Rapidly Exploring Random Tree，RRT），而不论该序列为随机还是确定性序列，都被称为快速搜索稠密树（Rapidly Exploring Dense Trees，RDTs），这种规划方法可处理微分等多种约束。

考虑二维和三维工作空间，环境中包含静态障碍物。初始化快速随机搜索树T，只包括根节点，即初始状态S。在自由空间中随机选取一个状态点??，遍历当前的快速随机搜索树T，找到T上距离??最近的节点??，考虑机器人的动力学约束从控制输入集??中选择输入??，从状态??开始作用，经过一个控制周期??到达新的状态??。满足??与??的控制输入??为最佳控制量。将新状态??添加到快速随机搜索树T中。按照这样得到方法不断产生新状态，直到到达目标状态G。完成搜索树构建后，从目标点开始，逐次找到父节点直到到达初始状态，即搜索树的根节点。

图7 随机树构建过程

由于在搜索过程中考虑了机器人的动力学约束，因此生成的路径的可行性很好。但是算法的随机性导致其只具备概率完备性。

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算法倾向于遍历整个自由空间直到获得可行路径，这使其不可能用于未知或动态环境中的机器人在线运动规划。利用滚动规划的思想可以将RRT算法进行改进，使其具备在线规划能力。

机器人在未知或动态环境中运动时，只能探知其传感器范围内有限区域内的环境信息。机器人利用局部信息进行局部运动规划，并根据一定的评价准则得到局部目标。机器人到达局部目标后再次进行新的局部规划。如此反复进行直到到达全局目标。

滚动规划算法的基本原理：

1. 环境信息预测：在滚动的每一步，机器人根据探测到的视野内的信息、或所有已知的环境信息，建立环境模型，包括设置已知区域内的节点类型信息等；

2. 局部滚动优化：将上述环境信息模型看成一个优化的窗口，在此基础上，根据目标点的位置和特定的优化策略计算出下一步的最优子目标，然后根据子目标和环境信息模型，选择局部规划算法，确定向子目标行进的局部路径，并实施当前策略，即依所规划的局部路径行进若干步，窗口相应向前滚动；

3. 反馈信息校正：根据局部最优路径，驱动机器人行走一段路径后，机器人会探测到新的未知信息，此时可以根据机器人在行走过程探测到的新信息补充或校正原来的环境模型，用于滚动后下一步的局部规划。

其中，局部子目标是在滚动窗口中寻找一个全局目标的映射，它必须避开障碍物，且满足某种优化指标。子目标的选择方法反映了全局优化的要求与局部有限信息约束的折衷，是在给定信息环境下企图实现全局优化的自然选择。

基于滚动窗口的路径规划算法依靠实时探测到的局部环境信息，以滚动方式进行在线规划。在滚动的每一步，根据探测到的局部信息，用启发式方法生成优化子目标，在当前滚动窗口内进行局部路径规划，然后实施当前策略(依局部规划路径移动一步)，随滚动窗口推进，不断取得新的环境信息，从而在滚动中实现优化与反馈的结合。由于规划问题压缩到滚动窗口内，与全局规划相比其计算量大大下降。

基于滚动窗口的路径规划算法的具体步骤如下：

• 步骤0：对起点、终点、工作环境、机器人的视野半径、步长进行初始化；

• 步骤1：如果终点到达，规划中止；

• 步骤2：对当前滚动窗口内的环境信息进行刷新；

• 步骤3：产生局部子目标；

• 步骤4：根据子目标及已知环境信息，在当前滚动窗口内规划一条优化的局部可行路径；

• 步骤5：依规划的局部路径行进一步，步长小于视野半径；

• 步骤6：返回步骤1。

在一个滚动窗口内，随机树以当前位置为起始点，构建传感器范围内的随机树。构建方法与基本RRT算法一致。为了使全局环境中随机树具有向目标方向生长的趋势，在运动规划时引入启发信息，减少随机树的随机性，提高搜索效率。

令??代表随机树中两个位姿节点间的路径代价，??代表随机树中两个位姿节点间的欧几里德距离。类似于A*算法，本算法为随机树中每个节点定义一个估价函数：??。其中??是随机节点??到树中节点??所需的路径代价。??为启发估价函数，这里取随机节点??到目标点??的距离为估价值，??。因此??表示从节点??经随机节点??到目标节点??的路径估计值。遍历滚动窗口内随机树T，取估价函数最小值的节点??，有??。这使得随机树沿着到目标节点估价值??最小的方向进行扩展。

由于在随机树生长中引入了导向目标的启发估价因子，叶节点??总是选择离目标最近的节点，这可能会使随机树遇到局部极小值问题。因此随机树生长的新节点??必须要克服这个问题，引导随机树更好的探索未知空间。

这里利用统计学中回归分析生成新节点，将RRT算法探索未知空间的能力进一步增强以避免因启发估价因子导致的局部极小。其思想是探索以前到过的空间是无用的，而且容易陷入局部极小。引进回归分析(regression analysis)是考察新节点与其他节点之间关系，利用回归函数约束，使得随机树不探索以前到过的空间，因此避免了局部极小。

新节点生成方法是遍历随机树，如果??与其父节点??的距离小于??与扩展树上其他任意节点的距离，即??，则选择该节点为随机树新生节点。下图解释了新节点的判断过程。

?图8 新节点的判断

上图中各个空心点是中间的父节点的可能扩展。椭圆圈起的空心点表示这个新节点不符合回归函数约束，剩下的两个未被圈起的空心节点到其父节点的距离小于该节点到随机树上任意节点的距离，这两个点可以成为随机树的新节点。

综上，滚动窗口内随机树构建的具体步骤如下：

1. 对滚动窗口随机树T初始化，T开始只包含初始位置S；

2. 滚动窗口自由空间中随机选择一个状态?；

3. 根据最短路径思想寻找树T中和??距离最近的节点?；

4. 选择输入??，使机器人状态由??到?；

5. 确定??是否符合回归分析，不符合则回到第4步；

6. 将??作为随机树T的一个新节点，??则被记录在连接节点??和??的边上。

滚动窗口状态空间进行K次采样后，遍历随机树，根据启发估价思想寻找滚动窗口子目标?。??是当前滚动窗口中的子树中估价函数最小的点。确定子目标后，机器人前进到子目标点，进行下一轮的滚动RRT规划。如此反复，直到到达目标点G。

人工势场法是由Khatib提出的一种用于机器人运动规划的虚拟力方法。其基本思想是将目标和障碍物对机器人运动的影响具体化成人造势场。目标处势能低，障碍物处势能高。这种势差产生了目标对机器人的引力和障碍物对机器人的斥力，其合力控制机器人沿势场的负梯度方向向目标点运动。人工势场法计算方便，得到的路径安全平滑，但是复杂的势场环境可能在目标点之外产生局部极小点导致机器人无法到达目标。

为了解决人工势场法的局部极小点问题，学者们提出了各种改进方法。主要分成两个方向：一个是构造合适的势函数以减小或避免局部极小点的出现；另一种是在机器人遇到局部极小点后结合其他的方法使机器人离开局部极小点。前者一般需要全局地图信息，并且依赖于障碍物的形状。当环境复杂时难以应用。后者多利用搜索法、多势场法和沿墙行走法等方法使机器人离开局部极小点。搜索法利用最佳优先、模拟退火、随即搜索等策略寻找比局部极小点势场值更低的点使机器人继续移动。由于未知环境中大多缺乏启发信息，搜索方法的效率很低。多势场法构造多个全局极小点相同，而局部极小点不同的势函数，在机器人陷入某个局部极小点时，规划器就切换势函数使机器人离开该点。

但是在未知的环境中这样的多个势场很难构造，而且该方法可能导致机器人在回到曾逃离的局部极小点。由于局部极小点是某个或多个障碍物的斥力势场与引力势场共同作用产生，其位置与障碍物距离必然不远，沿墙行走法正是利用这样的远离，使机器人在遇到局部极小点后参照类似BUG算法的环绕行为绕过产生局部极小点的障碍物继续前进。这种方法可靠性高，不依赖环境的先验信息和障碍物形状。

本节构造人工势场进行机器人平动的在线运动规划，利用一种沿墙行走法对基本的人工势场法进行改进。

作用在机器人上的假想引力和斥力为势函数的负梯度，因而人工势函数应该具有以下特征：

1. 非负且连续可微；

2. 斥力势强度距离障碍物越近其强度越大；

3. 引力势强度离目标位置越近其强度越小。

空间中的合势场是引力势场与斥力势场之和：?

其中，??是目标产生的引力势场；??是各个障碍物产生的斥力势场之和，即：?。

这里构造如下的引力势函数和斥力势函数：

其中，??表示引力势的相对影响；??表示第??个障碍物的斥力势的相对影响，??表示机器人当前位置，??表示目标点位置，??表示机器人距目标的距离，??的作用是在机器人距离目标较远时，削弱目标引力势的作用，??表示机器人距离第??个障碍物的距离，??表示第??个障碍物的斥力势作用范围。

?和??对势场形状的影响很大，适当的增大??能够增强引力势场的作用，有助于减少产生局部极小点的可能，并加快机器人向目标运动。??影响机器人在障碍物附近的运动特性，??比较大可以使机器人距离障碍物更远，运动路径更安全；??比较小，机器人在避开障碍物时运动比较平滑。

利用上面势函数的梯度可以计算机器人收到的假想引力和斥力：

当机器人的运行环境中包含形状复杂或者距离很近的障碍物时，可能出现势场局部极小点，导致机器人在该处停止或在其周围振动。如下图所示，当环境中出现“陷阱”形障碍物或者与目标成特定位置关系的障碍物时，可能在人工势场中产生局部极小点（图中L点），当机器人运动到局部极小点附近时，势场的负梯度方向指向L点。机器人将在L点处停止或在其附近振动或作圆周运动。

图9 人工势场法的局部极小点

为了使机器人从局部极小点中逃离，在人工势场法的基础上引入应激行为，即增加绕行行为。当机器人遇到局部极小点时，忽略目标引力势的作用，沿着斥力势的等势面方向移动，直到机器人离开局部极小区域。改进的算法流程如下：

1. 根据传感器信息计算当前位置的引力和斥力；

2. 判断是否处于绕行行为，若是，执行3；若否，执行4；

3. 判断是否离开局部极小区域，若是，机器人沿着合力方向运动，结束绕行行为；若否，机器人沿着斥力场等势线运动，继续绕行行为；

4. 判断是否遇到局部极小点，若是，机器人沿着斥力场等势线运动，开始绕行行为；若否，机器人沿着合力方向运动；

5. 判断是否到达目标，若是，退出算法；若否，继续1；

使用下面的判别条件判断机器人是否遇到局部极小点。

条件1：?

条件2：?

当条件1或者条件2出现时，就认为机器人遇到了局部极小点。条件1中??是一个很小的正数，其含义是机器人受到的虚拟合力接近0。这是最直接局部极小点判断方法。条件2中??为0,1之间某一正数，??为机器人运动过程中某一状态，??表示机器人从??到达当前位置??的总路程，条件2成立意味着机器人在运动很长路程后，位移很小。用来检测机器人在局部极小点附近发生的振动和圆周运动。

BUG算法是一种完全应激的机器人避障算法。其算法原理类似昆虫爬行的运动决策策略。在未遇到障碍物时，沿直线向目标运动；在遇到障碍物后，沿着障碍物边界绕行，并利用一定的判断准则离开障碍物继续直行。这种应激式的算法计算简便，不需要获知全局地图和障碍物形状，具备完备性。但是其生成的路径平滑性不够好，对机器人的各种微分约束适应性比较差。

该算法的基本思想是在没有障碍物时，沿着直线向目标运动可以得到最短的路线。当传感器检测到障碍物时，机器人绕行障碍物直到能够继续沿直线项目标运动。BUG1算法实现了最基本的向目标直行和绕行障碍物的思想。

假设机器人能够计算两点之间的距离，并且不考虑机器人的定位误差。初始位置和目标位置分别用??和??表示；机器人在??时刻的位置表示为??；??表示连接机器人位置??和目标点的直线。初始时，??。若没有探测到障碍物，那么机器人就沿着??向目标直行，直到到达目标点或者遇到障碍物。当遇到障碍物时，记下当前位置??。然后机器人环绕障碍物直到又一次到达??，找到环绕路线上距离目标最近的点??，并沿着障碍物边界移动到该点。随后，直线??更新，机器人继续沿直线向目标运动。如果沿这条直线运动时还会遇到该障碍物，那么机器人不能到达目标点。否则算法不断循环直到机器人到达目标点或者规划器认为机器人无法到达目标点。

图10 BUG1算法运动规划

图11 BUG1算法中认为机器人无法到达目标点的情况

图12 BUG1算法伪代码

BUG2算法也有两种运动：朝向目标的直行和沿边界绕行。与BUG1算法不同的是，BUG2算法中的直线??是连接初始点和目标点的直线，在计算过程中保持不变。当机器人在点遇到障碍物时，机器人开始绕行障碍物，如果机器人在绕行过程中在距离目标更近的点再次遇到直线??，那么就停止绕行，继续沿着直线??向目标直行。如此循环，直到机器人到达目标点??。如果机器人在绕行过程中未遇到直线??上与目标更近的??点而回到了??点，那么得出结论，机器人不能到达目标。

图13? BUG2算法运动规划

图14? BUG2算法中认为机器人无法到达目标点的情况

图15 BUG2算法伪代码

BUG1和BUG2算法提供了搜索问题的两种基本方法：比较保守的BUG1算法进行详细的搜索来获得最佳的离开点。这需要机器人环绕整个障碍物的边界。而BUG2算法使用一种投机的方法。机器人不环绕完整的障碍物，而选择第一个可用的点作为离开点。对于一般的环境，BUG2算法的效率更高；而对于复杂形状的障碍物，保守的BUG1算法性能更优。

TangentBUG算法是对BUG2算法的提高。它利用机器人上距离传感器的读数对障碍物做出提前规避，可以获得更短更平滑的机器人路径。在一个静态环境中，传感器读数??是机器人位置??和传感器扫描角度??的函数，具体点说，??是沿着??的射线以角度??到达最近障碍物的距离，

其中??。

对于某一个固定的位置??，??被传感器视野内的障碍物分割成多个连续区间。如下图所示。??出现不连续或者到达传感器最大测量范围的角度就是这些连续区间的端点。TangentBUG算法利用者些区间的端点避开工作空间中的障碍物。

图16 距离传感器扫描障碍物

对??不连续的情况做出说明（如图17所示）：点??，??，??，??，??，??，??和??与障碍物的不连续性相关；请注意，这里的射线与障碍物相切。点??是不连续的，因为障碍物边界落在传感器的范围之外。??和??，??和??，??和??，??和??之间的free space边界上的点集是连续性的间隔（加粗线部分）。

图17 不连续的示意图

与其他的BUG算法一样，TangentBUG算法也有两种行为：直行（motion-to-go）和环绕障碍物（boundary-following）。

首先机器人沿着直线向目标运动，直到它利用传感器观测到在其运动方向的前方有障碍物。不在机器人前方的障碍物对其向目标运动没有影响。比如下图中的障碍物?，障碍物??在传感器视野内，但是不阻碍机器人的运动。机器人在刚刚探测到障碍物时，传感器视野圆与障碍物边界相切。随着机器人继续向前移动，这个切点分裂成两个交点??和??（??和??），??和??之间的障碍物边界区间与机器人运动直线相交。

图18 机器人向目标运动遇到障碍物

此时，机器人不能继续向目标运动，而从两个交点中选择一个作为局部目标。为比较两个交点对于机器人向目标运动的优劣性，定义探索距离?。在没有关于障碍物的其他信息时，可以将探索距离??定义为从??经过一个交点到目标点的折线长度，即：?。

例如图19，机器人“看见”了障碍??，并选择向??移动，因为??。

当机器人位于??时，它无法知道??阻挡了从??到目标??的路径。

在图20中，当机器人位于??但目标??不同时，它具有足够的传感器信息来得出结论：??确实阻挡了从??到目标??的路径，从而朝??行驶。

因此，选择向??行驶刚开始的时候可能使得??最小化，而不是向??行驶，但是planner有效地为??分配无限大的成本代价，因为它有足够的信息来推断任何通过??的路径都不是最理想的。

图19

图20

机器人将选择探索距离短的交点作为局部目标，向之运动。随着机器人不断运动，交点不断更新，探索距离也不断减小。如下图所示。当??时，机器人还没有探测到障碍物，因而它向目标作直线运动；当??时，机器人开始探测到障碍物，并朝向障碍物探索距离近的一侧运动；当??和??时，机器人继续移动，同时更新探测区域，在这个过程中探索距离不断减小。

图21 机器人运动时不断更新局部目标和探测距离

在机器人运动过程中，探索距离不再减小时，就停止向目标运动行为，切换到环绕边界行为。此时，机器人找到了探测距离的一个极小值，并可计算已探测的障碍物边界与目标??的最近距离?。机器人按照原来的方向环绕障碍物运动，同时机器人更新当前探测的障碍物边界与目标的最近距离?。当发现??时，机器人停止障碍物环绕行为，继续向目标运动。

图22 机器人环绕障碍物运动

如上图所示，当机器人探索到障碍物上的??点后，探索距离就不再减小，即??点是机器人探索距离在障碍物边界上的局部极小点。机器人开始沿着障碍物边界进行环绕，图中虚线路径就是机器人环绕障碍物时所走的路径。当机器人探测到与目标距离相比??点更近的点时，重新开始接近目标的运动。

LPA*算法，即Lifelong Planning A*算法，该算法于2001年由Sven Koenig和Maxim Likhachev提出，是一种增量启发式搜索版本的A*算法，这种路径规划算法适用于随着时间改变导致有限栅格地图上的边缘代价c(s1,s2)改变的问题，也就是随着时间改变障碍物增多或减少，网格点发生增删等，在许多场合下比再利用A*重新搜索更高效。

D* Lite算法是以LPA*为基础，是Maxim Likhachev和Sven Koenig于2002年基于LPA*，结合A*算法思想，提出一种增量启发式算法，适用于在未知环境中的导航以及路径规划，广泛用于目前各种移动机器人和自主车辆载具，例如“机遇号”和“勇气号”火星车测试的原型导航系统。

本章研究了几种常用的运动规划算法。其中，人工势场法应用灵活，可以在保证安全的情况下获得一条平滑路径，并且对于动态环境可以实现实时运动控制。适合用于长距离机动且障碍物较少的情况。而基于随机采样的搜索树方法可以在复杂约束环境中获得可行解，适合用于机械臂近距离操作。

参考文献：

【1】Choset H , Kantor G A , Thrun S . Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. MIT Press, 2005.

下载1：OpenCV-Contrib扩展模块中文版教程

在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。

下载2：Python视觉实战项目52讲

在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。

下载3：OpenCV实战项目20讲

在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三?+?上海交大?+?视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~