基于狭窄通道路径树的快速航路规划算法*

2022-06-28冉华明

电讯技术 2022年6期

冉华明

(中国电子科技集团公司航空电子信息系统技术重点实验室,成都 610036)

0 引言

无人机在城市环境中进行低空飞行时所处的环境十分复杂，存在大量密集不规则障碍等，这些障碍之间可能存在狭小的缝隙，合理地利用这些缝隙规划无人机的低空飞行航路，在保证无人机安全的同时缩短无人机的航程，可有效提升无人机在城市环境中执行各类任务的效能和安全性[1-3]。

无人机航路规划是指无人机在特定规划环境下规划制定从初始位置到目标位置满足安全性、时效性等性能指标的最优飞行路径。目前常用的航路规划算法主要有遗传算法、蚁群算法、快速扩展随机树(Rapidly-exploring Random Tree,RRT)算法和A*算法等[4-11]。遗传算法[5-6]通过模拟自然界生物交叉遗传、个体变异、优胜劣汰等进化过程进行航路规划搜索，不需要确定的模型，算法鲁棒性强，但在进行复杂航路规划时存在编码困难复杂、搜索时间长、收敛性能差等问题。蚁群算法[7-8]具有正反馈、分布计算等优点，但算法初始信息素设定和信息素更新规则难以确定。RRT算法[9-10]通过随机采样避免了对空间的建模过程，同时可以避免对一些非完整航路规划约束进行建模，适合在复杂约束环境下的航路规划，但RRT算法不一定得到最优航路，同时随机搜索会消耗较大的无效计算资源。A*算法[11-12]是一种启发式搜索算法，需要将整个规划环境栅格化，而栅格的颗粒度对航路整体的精度和搜索计算时间有着很大的影响，较小的颗粒度往往会得到较高精度的航路，但其计算时间却会大大增加，因此需要选择一个合适的栅格颗粒度来权衡航路精度与计算时间的需求冲突，这就使得该方法一般只能找到较优解，而非最优解。

在复杂环境中，由于障碍位置、形状大小等因素，各障碍之间可能存在一定的缝隙，即狭窄通道。当前常用的航路规划方法在求解存在狭窄通道场景下的航路规划问题时还存在一些不足之处。

一是计算时间长。由于狭窄通道的宽度很窄，长度较长，A*算法需要很小的栅格颗粒度才能将狭窄通道与周围的障碍环境区分开，这将大大增加航路搜索的时间；RRT算法在狭窄通道扩展时很容易扩展到障碍范围内，需耗费大量时间才能得到一条通过狭窄通道的随机扩展树。

二是规划效果差。当规划环境还存在除了狭窄通道之外的其他通道时，航路规划算法往往会得到一条未穿过狭窄通道的航路，而该航路的性能比穿过狭窄通道的航路往往要差很多；当复杂环境只存在狭窄通道时，航路规划算法可能会搜索不到穿过狭窄通道的航路，导致航路规划失败。

Lien等[13]采用中轴线采样方法，对随机路标图(Probabilistic Roadmap,PRM)算法进行了改进，解决其难以覆盖狭窄通道的问题，但该方法只能实现直线型狭窄通道环境下的航路规划，难以实现多个障碍之间存在复杂狭窄通道环境下的航路规划。钟建冬、Wei等人[14-16]提出了一种星形试验法，实现对狭窄通道的识别能力，并提出了三树RRT算法和混合概率路标图法，实现机器人在狭窄通道路径环境中的路径规划，但该算法存在狭窄通道识别收敛慢、无法利用多个狭窄通道的问题。

本文在第1节中参考Voronoi法[17]求解复杂环境中各障碍之间的狭窄通道，基于此构建穿过所有障碍之间可行缝隙的狭窄通道路径树；在第2节中对双向RRT算法进行了改进，结合狭窄通道路径树构建复杂环境下的快速航路规划模型，快速有效地规划出复杂环境中的航路；最后在第3节中通过仿真对算法进行了验证。

1 复杂环境预处理

航路规划环境中各种障碍之间存在狭窄通道，通过复杂航路规划环境预处理，获取能充分利用障碍间的狭窄通道的狭窄通道路径树，为快速有效规划出穿越各狭窄缝隙的航路奠定基础，如图1所示。

图1 狭窄通道示意图

1.1 邻近障碍间狭窄通道

通过判断邻近障碍之间的空间位置关系，确定和计算所有障碍之间的狭窄通道路径。

设复杂环境中第i个障碍的顶点为[xi1,yi1;…;xin,yin;…;xiNi,yiNi]，其中，Ni表示i个障碍的顶点数量，xin,yin表示第i个障碍的第n个顶点的横坐标和纵坐标。

设障碍i的第n条边为[xin,yin;xin+1,yin+1]，障碍j的第m条边为[xjm,yjm;xjm+1,yjm+1]，则这两条边的交点计算公式为[18]

(1a)

(1b)

式中：ti、tj表示交点距离两条边起点的比例。则障碍i的第n条与障碍j的第m条边相交的判断条件为[18]

rank(A)=2∩0≤ti≤1∩0≤tj≤1 。

(2)

式中：rank()为矩阵的秩。

通过射线法[19]判断障碍的顶点是否在另外一个障碍内，通过式(2)判断两个障碍之间是否存在相交的边来确定障碍之间的关系。若两个障碍有边相交或者有顶点在另一个障碍范围内，则两个障碍相交或包含；否则，两个障碍相离。

将障碍的顶点和边作为障碍的对象qin，其中i表示障碍的编号，n表示该障碍上对象的编号。对于相离的两个障碍i和j，基于Voronoi图求解两个障碍之间的最短距离对象对〈qin,qjm〉，即第i个障碍上的第n个对象与第j个障碍上的第m个对象之间的距离最短[17]。若qin和qjm之间的最短连线的距离小于狭窄通道的门限值，则最短连线的垂直平分线即为两个障碍间的狭窄通道路径。邻近障碍间的狭窄通道计算流程如图2所示。

图2 邻近障碍间的狭窄通道计算流程

1.2 狭窄通道路径树

在求解完复杂航路规划环境中所有障碍之间的狭窄通道路径后，综合所有狭窄通道路径信息、障碍范围信息，通过判断所有狭窄通道路径与各障碍范围的关系，剔除被障碍覆盖的狭窄通道，计算在障碍范围外的所有狭窄通道路径集合；再通过将相交或者相互之间距离小于一定阈值的狭窄通道路径联合起来组成复杂航路规划环境中的狭窄通道路径树。

对每一条狭窄通道路径，通过射线法[19]判断其起点和终点是否在同一障碍的范围内，若在障碍范围内，则剔除该狭窄路径；否则，计算狭窄通道路径与障碍的交点信息，保留在障碍范围外的狭窄通道路径作为新的狭窄通道路径。当完成对所有狭窄通道路径的判断后，得到在障碍范围外的新的狭窄通道路径集合。

通过式(2)判断各狭窄通道路径之间是否相交，若相交，则相交的两条狭窄通道路径可构成一棵狭窄通道路径树：

对还未加入狭窄通道路径树上的狭窄通道路径，计算其与已有狭窄通道路径树的相交情况，若相交，则将该狭窄路径加入到该狭窄通道路径树上。

当存在多棵不相交的狭窄通道路径树时，计算每棵树上节点距离其他树的最近距离，若距离小于最小距离阈值，则将两棵树合并为一棵新的树。点[x,y]距离狭窄通道路径树上第i个树枝[xi1,yi1;xi2,yi2]的最短距离dmin为

(3)

求解狭窄通道路径树的流程如图3所示。

图3 狭窄通道路径树求解流程

2 快速航路规划模型

通过复杂环境预处理确定航路规划环境的狭窄通道路径树后，结合双向RRT算法，构建复杂环境下的快速航路规划模型，快速规划出复杂环境中的航路。

2.1 双向RRT算法

RRT算法通过随机搜索的方式，能够在障碍物复杂的高维非凸空间中快速搜索到一条路径[9]。双向RRT算法从航路规划的起点和终点分别生成一棵搜索树，同时不断对两棵搜索树进行扩展，当两棵搜索树相连后就形成一棵连接航路规划起点和终点的航路树，并对航路树进行删减平滑生成航路，提升了航路规划的速度[10]。

如图4所示，以航路规划的起点为起始节点ninit生成搜索树1，在航路规划环境中随机生成点nrand，并选择搜索树1上距随机点nrand最近的点nnear，从该点向随机点nrand的方向扩展一个搜索步长ε，得到一个新的节点nnew。当新的节点不在威胁范围内时，则将该节点添加到搜索树1上，通过该方法实现搜索树1的不断扩展。当搜索树新扩展的节点与另一棵树的节点的最短距离小于搜索步长时，则连接两棵树上最近的节点，生成一棵航路树。

图4 搜索树和航路树示意图

2.2 结合狭窄通道路径树的双向RRT算法

鉴于双向RRT算法搜索的随机性，其很难搜索到处于障碍之间狭窄缝隙的有效树节点，而第1节中设计的狭窄通道路径树可穿越障碍之间各狭窄缝隙，基于此设计了一种结合狭窄通道路径树的双向RRT算法，通过判断环境预处理生成的狭窄通道路径树与双向RRT算法生成的两棵搜索树之间的空间关系，将满足条件的狭窄通道路径树添加到搜索树上，减少双向RRT算法搜索位于障碍间狭窄缝隙中的树节点所耗费的时间，提升路径搜索的效率和生成的路径的效能。

在该算法中，将预处理得到的狭窄通道路径树作为一个树枝添加到搜索树上，改变了传统双向RRT算法中搜索树每次只能扩展一个树节点的模式，减少了搜索树在狭窄通道中的搜索时间，提升了两棵搜索树相连生成航路树的速度。

通过结合狭窄通道路径树的双向RRT算法进行航路规划的流程如图5所示。