无人机路径规划算法_路径规划算法总结

本文来自知乎网友@搬砖的旺财，地平线机器人算法工程师。作者根据自己本科和硕士阶段的学习经历，整理归纳了所接触过的规划算法。

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 可视图法

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

图5 可视图

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

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

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

图6 顶点可见性判断

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

2.1.2 Dijkstra算法

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

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

详情请参考：路径规划——Dijkstra算法：https://zhuanlan.zhihu.com/p/51112799

2.1.3 A*算法

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

详情请参考：路径规划——A*算法：https://zhuanlan.zhihu.com/p/51099376

2.2 RRT算法

快速搜索随机