随着城市化进程加速, 高层建筑数量急剧增加, 而高层建筑火灾事故给现有的消防救援技术与设备带来了新的挑战^[1-2]. 现有的消防举高设备通常无法触及高于18层的起火楼层, 消防官兵负重登楼代价大^[3-4], 建筑内部环境复杂, 充满高温烟尘, 人员疏散极其困难^[5]. 此时, 基于多无人机 (Multi-unmanned aerial vehicles, Multi-UAVs) 协同控制技术的高层消防救援方法, 在高层建筑消防灭火中的应用愈发广泛^[6-8]. 多无人机可通过破窗, 迅速进入高层火灾现场, 尝试扑灭初期火灾^[9]; 在室内飞行时实现自主避障、自主探测火点及被困人员; 可通过扬声器、照明灯等辅助设备协助被困人员撤离^[10]. 多无人机协同控制现已成为控制领域的研究热点^[11]. 在高层火灾现场的室内环境进行多无人机协同路径规划、协同救援的研究, 其难点在于规划空间小、避障裕度低、限制条件多^[12]等.

无人机路径规划算法主要有可视图法、栅格法、快速遍历随机树法(Rapidly-exploring random tree, RRT)等^[13]. 其中, RRT算法因不需要对环境进行精确建模, 搜索性较强, 以及对时变环境适应性良好等优势^[14], 在路径规划领域中有着广泛的应用. 对于RRT算法及其改进型算法, 文献[15]在前人的研究成果上对RRT算法进行了总结, 主要包括双向RRT (Bidirectional RRT, Bi-RRT)、偏向目标RRT (RRT-Goalbiasing)以及RRT-Connect算法, 并以此为基础提出了基于人工势场法的改进RRT算法, 一定程度上提高了算法的路径规划效率, 减少了路径规划时间. 文献[16-17]研究了动态步长RRT路径规划算法, 提高了随机树扩展的效率. 然而, 上述改进算法为提高到达终点的速度, 倾向于采用降低搜索广度的方法对RRT算法进行优化, 这样的优化对于复杂的高层室内火灾环境来讲是难以适用的. 有学者^[18-21]曾提出在确定的位置上增加少量随机树以解决此问题, 但该方法更倾向于通过人工干预的形式选择辅助随机树根节点的位置, 不适合根据高层火灾室内环境变化频繁重规划. 针对以上问题, 本文提出一种改进的插入随机中间树的多树RRT算法, 下称RRT森林算法.

考虑路径的可行性, 现有的RRT算法普遍存在路径与障碍物距离过近的问题.目前常见的解决方法是把地图进行膨胀处理再进行路径规划^[22], 但对于需要实时进行路径规划的复杂高层消防环境, 对地图进行预处理会降低规划效率. 文献[23]提出一种人工势场优化的RRT算法, 但这种方法需要计算地图中的势场, 需要反复计算微分, 运算效率仍不高. 因此, 研究一种能确保无人机与障碍物之间保持安全距离的碰撞检测程序十分有意义.

此外, RRT算法得到的路径通常不是最优路径^[15], 而是一条有冗余点的路径. 在文献[24]中, Jeong等提出Quick-RRT^* 算法, 在每一次产生子节点时都在给定的深度范围内, 寻找无障碍物相隔的父节点进行连接, 减少冗余点的产生, 但其路径受制于RRT生成的节点, 易出现不合理的拐角. 针对这一问题, 本文提出两次动态规划(Dynamic programming, DP)对路径进行优化的方法, 采取“规划−插值−规划”的策略取得可行性更高的路径.

基于对上述国内外研究现状的调研与分析, 综合考虑高层消防多无人机路径规划时间紧迫、环境复杂、需要频繁对未执行的局部路径进行重规划^[25], 以及需要同时规划多条路径的特性, 本文提出RRT森林算法, 并通过改进的障碍物接近检测方法, 以及动态规划的两次优化, 提高算法的可行性. 主要工作归纳如下.

1) 提出RRT森林算法. 通过提升随机树森林的搜索广度, 提升算法搜索效率. 与文献[18-21]不同, RRT森林算法以树间连接优先的策略, 降低搜索后期节点数量对遍历效率的影响, 从而适应高层火灾救援任务中频繁的路径重规划; 同时, RRT森林算法通过多树连接进行寻路的设计, 实现多起点多终点的多无人机协同路径规划.

2) 提出障碍物接近检测方法. 通过检测包络线控制点以及包络线内部的随机点, 考虑无人机的体积, 使得路径更远离障碍物.

3) 提出基于动态规划的冗余点移除方法. 通过对路径中的节点以最优的方式重新连接, 删除其中的冗余点, 分离多无人机重合的路径, 并得到节点稀疏且路程最短的路径.

本文算法流程如图1所示. 第1节分析了基本RRT算法及双向RRT算法的优缺点; 第2节和第3节分别阐述了RRT森林算法和基于动态规划的冗余点移除方法; 第4节为RRT森林算法的仿真研究; 第5节为结论.

图 1  基于RRT森林算法的多无人机路径规划方法流程图

Fig. 1  Workflow of multi-UAVs path planning approach based on RRT-forest

下载: 全尺寸图片 幻灯片

1.   RRT算法及其改进

为满足高层消防多无人机协同路径规划中的时间紧迫性要求, 本文提出RRT森林算法. RRT森林算法通过增加随机树的数量达到提升搜索广度的目的, 从而提升算法在复杂环境下的路径搜索效率. 在阐述RRT森林算法之前, 首先对基本RRT算法与双向RRT算法进行简单的描述.

1.1   基本RRT算法

RRT算法是由Lavalle于1998年提出的一种基于随机采样的快速路径规划算法, 是一种概率完备算法^{[15, 26]}. 该算法的优点是节点扩展不需要进行预处理, 根据当前的环境信息快速搜索可行路径, 可用于无人机路径规划上.

RRT算法的基本思想是: 以起点为根节点, 通过在地图上随机采样, 并以最近的节点为父节点, 产生相应的子节点, 使随机树不断扩展. 当随机树接近终点并与终点连接, 即完成路径搜索. 随机树的采样及搜索过程如图2所示.

图 2  基本RRT算法采样及搜索过程

Fig. 2  Sampling and exploring process of basic RRT

下载: 全尺寸图片 幻灯片

基本RRT算法的优点在于不需要对环境模型进行复杂处理, 即可进行路径规划. 但传统RRT算法的缺点主要有:

1) 由于RRT算法是通过随机采样驱动寻找路径, 得到的路径曲折, 通常不是最优路径;

2) RRT算法在随机采样后需要遍历随机树上现有的节点, 之后选择最近的节点$ {\boldsymbol{v}}_\text{near} $, 方能在$ {\boldsymbol{v}}_\text{near} $附近朝着采样点$ {\boldsymbol{v}}_\text{rand} $的方向前进一小步到达$ {\boldsymbol{v}}_\text{new} $. 然而, 在复杂环境下, 路径搜索之初, 随机树上节点少, 覆盖范围小, 生成$ {\boldsymbol{v}}_\text{new} $的成功率显著降低, 随机树在小范围内重复搜索, 会导致路径规划收敛缓慢.

高层建筑火灾的室内环境往往是复杂时变环境, 需要频繁重规划路径的环境, 因此我们需要解决传统RRT算法前期搜索效率低的问题, 使之能够高效完成复杂环境下的路径搜索.

1.2   双向RRT算法

双向RRT算法与基本RRT算法不同之处在于, 以起点和终点为两个根节点, 分别建立随机树进行搜索, 直到双方节点彼此接近, 最终将两棵树最接近的子节点连接^[26]. 双向RRT两棵树连接示意图如图3所示.

图 3  双向RRT连接过程

Fig. 3  Connecting process of bidirection-RRT

下载: 全尺寸图片 幻灯片

与基本RRT算法相比, 双向RRT算法在终点处增加一随机树, 从起点和终点同时搜索路径, 在搜索速度上具有一定优势, 一定程度上增大了搜索初期的搜索广度. 但随着环境复杂度的提高, 双向RRT的两个随机树, 在搜索初期仍会被局限在小范围内, 不能满足高层消防救援中频繁进行路径重规划的需求.

2.   RRT森林算法

为提升RRT算法在高层火灾现场内部的路径搜索效率, 适应高层消防救援中频繁进行路径重规划的任务需求, 本文提出RRT森林算法, 在可行区域内随机选点生成随机树, 并与其他随机树相连接, 从而得出多组路径.

通过多树优化的RRT算法的思路并非首次提出, 受双向RRT算法增加随机树提升搜索广度的启发, 钟建冬^[18]提出可以将RRT算法从两棵随机树进一步扩展为三棵树, 直至多树. Devaurs等^[20]利用多树的特性对Transition-RRT进行优化, 通过给定根节点的方式在地图中生成多个随机树, 提升了搜索效率. 然而, 上述文献中的多树RRT算法为随机树扩展优先的形式, 这会导致在搜索连接对象时遍历量较大. 对此, 本文提出的RRT森林算法采用树间连接优先策略, 能够有效减少遍历量, 进而提升搜索效率. 另外, 针对文献[20]中导出路径需要经过所有随机树的根节点、路径合理性较差的问题, 本文通过设计一种适用于多树RRT的路径导出方法予以解决. 综上所述, 本文所提出的RRT森林算法具有以下特点: 1)随机树连接优先, 通过检查$ {\boldsymbol{v}}_\text{near} $判断是否可以与其他随机树连接, 一旦进行连接, 则不再扩展随机树; 2)利用多随机树的特点进行多起点多终点的路径规划; 3)导出路径不必经过每棵树的根节点, 因此能够取得节点更少的路径.

RRT森林算法的主要工作原理是: 在地图中增加随机点, 并以这些随机点以及原有的起点和终点为根节点, 各自生成随机树. 随着随机树的生长, 地图上可行的区域会迅速被随机树覆盖, 就像一片森林, 因此称这种算法为RRT森林算法. 基于该算法的特性, 随机树之间可以通过树枝与其他树相互连接, 最终将起点与终点连通, 从而完成路径搜索. RRT森林算法分为搜索过程、随机树之间的连接与合并过程以及路径导出过程三个部分.

2.1   RRT森林算法搜索过程

定义一个由$ k $架无人机完成的多起点多终点的高层消防多无人机协同路径规划任务, 记地图为$ {\boldsymbol{M}} $. 设需要规划$n \,(n\geq k)$条路径, 记第$ i $条路径的起点、终点分别为$ {\boldsymbol{v}}^i_\text{start} $和$ {\boldsymbol{v}}^i_\text{goal} $, 其中, $i = 1, 2,\cdots, n$, ${\boldsymbol{v}}^i_\text{start}\neq {\boldsymbol{v}}^i_\text{goal}$, 并满足以下假设:

假设 1. 考虑到多无人机路径可能出现交叉的问题, 每架无人机之间以及无人机与地面之间至少需保持$ \alpha $米距离. 无人机的高度随地面起伏而变化, 当需要改变高度时, 无人机会加强对附近无人机的监测.

注 1. 若地图中某处$ {\boldsymbol{v}} \in {\boldsymbol{M}} $可行高度范围为$ [{\boldsymbol{v}}.\text{floor},{\boldsymbol{v}}.\text{ceil}] $, 暂且假设该范围足够让$ n $架无人机在不同高度同时飞行, $ k $架无人机从低到高排列时, 第$j\; (j = 1, 2,\cdots, k)$ 架无人机在$ {\boldsymbol{v}} $点的飞行高度$ {\boldsymbol{v}}^j.z $可表示为

而第$ j $架无人机设定的距地面高度则等于$ (j+1)\alpha $. 另外, 由式(1)可得到$ k $架无人机飞行所需垂直空间条件为

满足条件(2)方可保证多无人机能同时通过该点. 因此, 路径规划使用的灰度地图中, 不满足条件(2)的区域视作不可行区域.

现用RRT森林算法搜索$ n $条路径, 其过程如下:

1) 初始化随机树. 以各起点$ {\boldsymbol{v}}^i_\text{start} $和各终点$ {\boldsymbol{v}}^i_\text{goal} $, 以及在地图上可行区域内随机选取的$N_\text{Tree}$个随机点$ {\boldsymbol{v}}_\text{rand} $, 分别初始化随机树, 最终会有$ N_\text{Tree}+2n $棵随机树在进行搜索. 注意, 起点和终点在设定任务时可以重复使用, 但在初始化过程中, 每组重复点仅生成一个随机树.

2) 开始随机搜索. 搜索过程采用树间连接优先的策略. 对于随机树集合${\boldsymbol{T}}$, 在每次迭代中逐一进行搜索. 首先, 对于当前活动树$ {\boldsymbol{T}}_\text{me} $采样得到随机点$ {\boldsymbol{v}}_\text{rand} $, 并查找节点$ {\boldsymbol{v}}_\text{near}\in {\boldsymbol{T}}_\text{me} $, 使得$ {\boldsymbol{v}}_\text{rand} $与$ {\boldsymbol{v}}_\text{near} $距离最短; 其次, 先判断$ {\boldsymbol{v}}_\text{near} $在给定范围内是否有非活动随机树$ {\boldsymbol{T}}_{\overline{\text{me}}} $的节点$ {\boldsymbol{v}}_\text{connect} $, 并确定$ {\boldsymbol{v}}_\text{near} $与$ {\boldsymbol{v}}_\text{connect} $之间有无障碍物, 若存在连接无障碍的$ {\boldsymbol{v}}_\text{connect} $, 则连接$ {\boldsymbol{v}}_\text{near} $与$ {\boldsymbol{v}}_\text{connect} $, 并结束对当前树的搜索; 反之, 按照基本RRT的方式沿$ {\boldsymbol{v}}_\text{near} $到$ {\boldsymbol{v}}_\text{rand} $方向, 以步长$ s $, 尝试在$ {\boldsymbol{T}}_\text{me} $上新建一个子节点$ {\boldsymbol{v}}_\text{new} $, 并结束对当前树的搜索. RRT森林算法搜索路径分为连接和生长两种工作模式, 其工作原理如图4所示, 其中图4(a)为随机树之间的连接过程, 图4(b)为随机树的生长过程.

图 4  RRT森林算法两种工作模式

Fig. 4  Two working modes of RRT-forest algorithm

下载: 全尺寸图片 幻灯片

3) 当所有随机树均尝试进行一次搜索后, 逐一判断树${\boldsymbol{T}}_k\in {\boldsymbol{T}}$是否已经包含所有起点$ {\boldsymbol{v}}^i_\text{start} $与终点$ {\boldsymbol{v}}^i_\text{goal} $, 若已全部包含, 则搜索完成, 否则继续进行下一次迭代. 若算法一直无法达到搜索完成条件, 则迭代会在迭代次数达到最大迭代次数$ m $时停止并显示错误信息. RRT森林算法随机树搜索过程伪代码如算法1所示.

　 算法 1. RRT森林算法随机树搜索过程

1)${\bf{while}}$ $iter<$M$\ {\bf{do}}$

2)　${\bf{for}}$ ${\boldsymbol{T}}_\text{me}$ ${\bf{in}}$ ${\boldsymbol{T}}$ ${\bf{do}}$

3)　　${\boldsymbol{v}}_\text{rand}\gets $random$\left(1,2 \right) \cdot $size(${\boldsymbol{M}}$)

4)　　${\boldsymbol{v}}_\text{near}\gets $查找最近点$({\boldsymbol{T}}_\text{me},{\boldsymbol{v}}_\text{rand})$

5)　 $d\gets \lVert {\boldsymbol{v}}_\text{rand}-{\boldsymbol{v}}_\text{near} \rVert_2 $

6)　 ${\boldsymbol{v}}_\text{new}\gets {\boldsymbol{v}}_\text{near}+\frac{s}{d}({\boldsymbol{v}}_\text{rand}-{\boldsymbol{v}}_\text{near}) $

7)　 ${\boldsymbol{T}}_{\overline{\text{me}}},{\boldsymbol{v}}_\text{connect}\gets $查找其他树中最近点$({\boldsymbol{T}}_\text{me},{\boldsymbol{v}}_\text{rand})$

8)　 ${\bf{if}}$ $\exists {\boldsymbol{v}}_\text{connect}$ ${\bf{then}}$

9)　　　 ${\boldsymbol{T}}_\text{me}\gets $合并随机树$({\boldsymbol{T}}_\text{me},{\boldsymbol{v}} _\text{near},{\boldsymbol{T}}_{\overline{\text{me}}},{\boldsymbol{v}}_\text{connect})$

10)　 ${\bf{else}}$ ${\bf{if}}$ 碰撞检测$({\boldsymbol{v}}_\text{new},{\boldsymbol{v}}_\text{near})$通过${\bf{then}}$

11)　　　${\boldsymbol{T}}_\text{me}\gets $添加节点$({\boldsymbol{v}}_\text{new},{\boldsymbol{T}}_\text{me})$

12)　 ${\bf{end\ if}}$

13) ${\bf{end\ for}}$

14) ${\bf{if}}$ $\forall {\boldsymbol{v}}_\text{start}^i,{\boldsymbol{v}}_\text{goal}^i\in {\boldsymbol{T}}_k$ ${\rm{then}}$ ${\bf{break}}$

15)${\bf{end\ while}}$

2.2   多随机树的连接与合并

对比于双向RRT的连接方式, 多随机树之间的连接方式增加了合并步骤, 以防止节点连接关系混乱. 由于RRT森林算法中每棵随机树需要进行多次连接, 因此不得不考虑随机树被连成闭环的问题. 从路径规划的角度看, 一旦随机树被连接成环, 在结束搜索后, 导出路径时会陷入死循环. 而从随机树的树状结构看, 随机树连接成环将会破坏树状结构, 导致部分节点连接关系混乱.

为了避免多随机树连接成环状造成路径规划出错的问题, 在连接的同时将两棵随机树合并, 从而防止两棵树之间多次连接. 此外, 两棵随机树的合并需要重新选择父节点. 根据随机树的树状结构, 可以将任意节点定义为新的根节点, 但需要先调整新根节点到原根节点之间各段的连接关系, 再对其他节点重新建立连接关系. 随机树之间的连接过程如图4(a)所示, 具体操作如下:

1) 在找到待连接的树$ {\boldsymbol{T}}_{\overline{\text{me}}} $与当前活动的随机树$ {\boldsymbol{T}}_\text{me} $节点$ {\boldsymbol{v}}_\text{near} $最近的节点$ {\boldsymbol{v}}_\text{connect} $后, 以$ {\boldsymbol{v}}_\text{connect} $为根节点新建临时树$ {\boldsymbol{T}}_\text{temp} $;

2) 以$ {\boldsymbol{v}}_\text{near} $为子节点, $ {\boldsymbol{v}}_\text{connect} $为父节点构成连接关系并写入$ {\boldsymbol{T}}_\text{temp} $;

3) 分别以$ {\boldsymbol{v}}_\text{connect} $与$ {\boldsymbol{v}}_\text{near} $在$ {\boldsymbol{T}}_{\overline{\text{me}}} $与$ {\boldsymbol{T}}_\text{me} $中依次向上寻找父节点, 同时, 将所经过的连接关系反向后写入$ {\boldsymbol{T}}_\text{temp} $中, 直到最终分别找到两棵树的原根节点;

4)将两棵树剩余节点写入$ {\boldsymbol{T}}_\text{temp} $, 重新计算连接关系.

5)令$ {\boldsymbol{T}}_\text{me} = {\boldsymbol{T}}_\text{temp} $, 关闭非活动树$ {\boldsymbol{T}}_{\overline{\text{me}}} $, 使得$ {\boldsymbol{T}}_ {\overline{\text{me}}} $停止生长, 连接过程结束.

2.3   多路径的导出

多无人机协同执行高层消防任务时, 往往需要为多架无人机分别导出路径, 本文结合RRT森林算法的特点, 设计了一种导出RRT多路径的方法. 主要工作原理如图5和算法2所示.

图 5  多路径的导出方法

Fig. 5  Export method for multiple paths

下载: 全尺寸图片 幻灯片

由于存在多条路径, 因此每条路径是分别确定的. 而相较于文献[20]所述的方法, 本文所导出的路径可不经过全部根节点. 具体工作原理为: 对于第$ i $架无人机, 其起点为$ {\boldsymbol{v}}^i_\text{start} $, 终点为$ {\boldsymbol{v}}^i_\text{goal} $, 所在随机树被提取, 记为$ {\boldsymbol{T}}^{*} $.

定义序列$ R_\text{from} $和$ R_\text{to} $, 分别从起点$ {\boldsymbol{v}}^i_\text{start} $和终点$ {\boldsymbol{v}}^i_\text{goal} $开始, 向$ {\boldsymbol{T}}^{*} $的根节点的方向, 不断寻找父节点并记录, 直到$ R_\text{from} $和$ R_\text{to} $ 出现重复元素. 然后令$R = R_\text{from}\,\cup R_\text{to}$, 即完成路径导出. 路径导出过程的伪代码如算法2所示.

　 算法 2. 路径导出过程

1)　$R_\text{from}\gets $查找父节点$({\boldsymbol{v}}_\text{start},{\boldsymbol{T}}^{*})$

2)　$R_\text{to}\gets $查找父节点$({\boldsymbol{v}}_\text{goal},{\boldsymbol{T}}^{*})$

3)　${\bf{while}}$ ${R}_\text{from}\cap R_\text{to}=\emptyset$ ${\bf{do}}$

4)　　${\bf{if}}$ $R_\text{from}^\text{END}>1$ ${\bf{then}}$

5)　　　 $R_\text{from}\xleftarrow{\text{顺序添加元素}}$查找父节点$({\boldsymbol{T}}^{*}.{\boldsymbol{v}}_{R_\text{from}^\text{END}},{\boldsymbol{T}}^{*})$

6)　　${\bf{end\ if}}$

7)　　${\bf{if}}$ $R_\text{to}^{1}>1$ ${\bf{then}}$

8)　　　 $R_\text{to}\xleftarrow{\text{逆序添加元素}}$查找父节点$({\boldsymbol{T}}^{*}.{\boldsymbol{v}}_{R_\text{to}^{1}},{\boldsymbol{T}}^{*})$

9)　　${\bf{end\ if}}$

10)　${\bf{end\ while}}$

11)　$i_\text{from},i_\text{to}\gets $查找相同元素标号$(R_\text{from},R_\text{to})$

12)　$R\gets R_\text{from}^{1,2,\cdots,i_\text{from}}\cup R_\text{to}^{i_\text{to},i_\text{to}+1,\cdots,\text{END}}$

13)　${\bf{for}}$ $r$ ${\bf{in}}$ $R$ ${\bf{do}}$

14)　　${\boldsymbol{P}}\xleftarrow{\text{顺序添加元素}}{\boldsymbol{T}}^{*}.{\boldsymbol{v}}_{r}$

15)　${\bf{end\ for}}$

注 2. 在程序实现的过程中, $ R_\text{from} $和$ R_\text{to} $记录的是各节点在树$ {\boldsymbol{T}}^{*} $上的编码, 因此, 需要在树$ {\boldsymbol{T}}^{*} $上根据$ R $中的编码查出具体的路径坐标并组合成序列$ {\boldsymbol{P}}_i $, 即为第$ i $架无人机的路径.

2.4   改进的障碍物接近检测方法

RRT算法常用的碰撞检测是通过检测地图中某点的灰度值判断该点是否在障碍物上, 对于线段的碰撞检测, 则从起点到终点逐个点检测是否在障碍物上, 一旦发现线段上某个点在障碍物上, 则判断线段存在碰撞. 特别地, 对于变步距RRT, 可以通过碰撞检测程序反馈推荐步长进行变步距搜索, 一定程度上提高了RRT的搜索效率. 然而, 由于碰撞检测未能考虑无人机的体积, 加之地图可能存在误差, 在实际执行中, 无人机存在与障碍物剐蹭的风险. 对于这个问题, 常用的解决方法多是通过图像处理技术对图像中障碍物进行膨胀处理, 这需要花费时间对图像进行预处理, 对于需要频繁进行部分路径重规划的高层消防多无人机系统来说是难以实现的. 基于上述情况, 本文提出一种改进的障碍物接近检测方法, 其思想是将单点检测转化为一种对包络线外缘主要控制点的检测, 在考虑无人机体积的同时, 尽可能减少路径规划速率的损失.

待测点$ {\boldsymbol{v}}_\text{test} $在灰度地图$ {\boldsymbol{M}} $上判断是否可行步骤如下: 首先, 地图$ {\boldsymbol{M}} $以深色表示不可行区域, 定义安全距离$ r $; 然后, 取无人机包络线的顶点, 以及在这些顶点所围成的区域内, 根据计算机算力, 随机选取测试点$ {\boldsymbol{v}}_{i}^{*}, i = 1,2,\cdots,k $, 加上待测点$ {\boldsymbol{v}}_\text{test} $自身; 最后通过分析在$ {\boldsymbol{M}} $上这些测试点的取值, 判定待测点$ {\boldsymbol{v}}_\text{test} $是否可行. 当且仅当上述点全部位于可行区域时, 待测点$ {\boldsymbol{v}}_\text{test} $可行.

改进的障碍物接近检测图如图6所示, 假设采用的是四旋翼无人机, 图中待测点记为$ {\boldsymbol{v}}_\text{test} $, 圆表示无人机的旋翼, 浅色实线段组成无人机的框架, 正方形点表示无人机包络线的顶点, 记为${\boldsymbol{v}}_{A}$, ${\boldsymbol{v}}_{B}$, ${\boldsymbol{v}}_{C}$, ${\boldsymbol{v}}_{D}$, 连接这四个点的虚线框为无人机包络线, 中间正方形点表示无人机的中心位置, 即${\boldsymbol{v}}_{O}$, 而三个“$* $”号点则是随机选取的测试点$ {\boldsymbol{v}}_ {i}^{*}, i = 1,2,3 $. 图6 中, 虽然中心点${\boldsymbol{v}}_{O}$为可行点, 但${\boldsymbol{v}}_{B}$, ${\boldsymbol{v}}_{D}$和一个白色“$* $”号表示的随机点位于不可行区域, 因此, 本例中待测点$ {\boldsymbol{v}}_\text{test} $不可行.

图 6  改进的障碍物接近检测示意图

Fig. 6  Schematic diagram of the improved obstacle proximity detection

下载: 全尺寸图片 幻灯片

对于线段碰撞检测, 从线段的一端向另一端逐步进行上述的障碍物检测, 其中每一步间隔一个安全距离$ r $. 当检测到第$ k $步不可行时即停止检测, 并返回可行步长$ \hat{s} = r(k-1) $.

在RRT森林算法中应用此种障碍物检测方法, 可以在未经膨胀处理的地图中进行路径规划, 同时不致使无人机过于接近障碍物而发生碰撞, 提高了路径的可行性.

3.   基于动态规划的冗余点移除方法

在高层火灾所造成的未知、时变且复杂的环境中, 直接使用RRT森林算法搜索的多路径会出现以下问题: 1) 路径非最优, 会造成不必要的时间和能力开销; 2) 路径会出现重叠, 虽然每架无人机以不同的高度飞行, 但无人机仍会相互影响. 因此, 有必要移除路径中的冗余点, 并减少路径重叠, 实现路径的进一步优化.

3.1   动态规划

动态规划是上世纪50年代美国学者Bellman提出的一种最优化规划方法^[27]. 动态规划用一个递推关系式, 使决策过程连续地转移, 并将一个多步最优控制问题转化为多个一步最优控制问题, 从而简化求解过程. 对于$ N $级决策过程, 其动态方程为

其中, $ \boldsymbol{u}\left(k \right) $为第$ k $级的容许控制; $ \boldsymbol{x}\left(k \right) $为当前状态. 利用动态规划解路径规划问题, 就是要找到决策序列$ \boldsymbol{u} ^{*} \left(k \right) $, 使得代价函数

达到最小.

本文将RRT森林算法搜索到的路径节点作为容许控制集, 通过动态规划确定在容许控制集中最优的决策序列, 从而删除其余的冗余点, 实现对多无人机路径的优化.

3.2   删除冗余点和路径优化

根据三角不等式, 任取不共线的三点$ A, B, C $, 则三点之间相互连接的线段长度, 必然满足

当且仅当$ A, B, C $共线时, 有

RRT森林算法搜索产生的较曲折的路径, 可以利用上述原理优化. 然而, 盲目删除冗余点, 未必能得到最优路径. 因此, 本文提出基于动态规划的冗余点移除方法.

设由RRT森林算法得到路径集${\boldsymbol{ P }}$, 记第$ i $条路径为$ {\boldsymbol{P}}_i $, 且${\boldsymbol{P}}_i\in {\boldsymbol{P}}.$ 而$ {\boldsymbol{P}}_i $是由 $ N_i $个节点 $ {\boldsymbol{v}}_1^i $, ${\boldsymbol{v}}_2^i,\cdots,$ ${\boldsymbol{v}}_a^i,\cdots,$ $ {\boldsymbol{v}}_b^i ,\cdots,$ $ {\boldsymbol{v}}_{N_i}^i $组成的有序数组. 通过动态规划删除RRT森林算法产生的冗余点, 对于路径点$ {\boldsymbol{v}}_a^i $, 若路径点$ {\boldsymbol{v}}_b^i $ ($ a\neq b $且$a,b = 1, 2,\cdots, N_i$) 与${\boldsymbol{v}}_a^i$之间无障碍物阻隔, 则将$ {\boldsymbol{v}}_a^i $和$ {\boldsymbol{v}}_b^i $连接起来, 计算每条边的距离, 形成用于动态规划的路线网络. 现构造指标函数, 指标函数$ J $为递推形式, 如式(7)所示.

其中, $ k\in \mathbf{R} $且$ k<i $, 且

定义两点间指标函数

对于第$ i $条原路径$ {\boldsymbol{P}}_i $, 将起点到终点之间每一个点独立作为一级, 对每一级计算指标函数, 并记录指标函数极小的上一节点索引. 计算完毕后, 从终点根据上一节点的索引往前查找, 即得优化后的路径索引序列. 根据索引序列查找$ {\boldsymbol{P}}_i $中对应点的坐标, 即得优化后的路径. 动态规划移除冗余点的伪代码见算法3.

　 算法 3. 动态规划移除冗余点

1)　$R_\text{opt}\gets [1, 1, 0]$

2)　${\bf{for}}$ $i$ ${\bf{from}}$ 2 ${\bf{to}}$ length($P.{\boldsymbol{v}}$) ${\bf{do}}$

3)　　$d_\text{M}\gets \infty$

4)　　${\bf{for}}$ $j$ ${\bf{from}}$ 1 ${\bf{to}}$ $i-1$ ${\bf{do}}$

5)　　　${\bf{if}}$ 碰撞检测$(P.{\boldsymbol{v}}_i,P.{\boldsymbol{v}}_j)$通过

　　　　${\bf{and}}$ $\lVert P.{\boldsymbol{v}}_i-P.{\boldsymbol{v}}_j \rVert <d_\text{M}$ ${\bf{then}}$

6)　　　　　　$R_\text{opt}\xleftarrow{\text{顺序添加行}}[i,j,\lVert P.{\boldsymbol{v}}_i-P.{\boldsymbol{v}}_j \rVert]$

7)　　　　　　$d_\text{M}\gets \lVert P.{\boldsymbol{v}}_i-P.{\boldsymbol{v}}_j \rVert$

8)　　　${\bf{end\ if}}$

9)　　${\bf{end\ for}}$

10)　${\bf{end\ for}}$

11)　${\bf{for}}$ $r$ ${\bf{in}}$ $R^{\cdot,1}_\text{opt}$ ${\bf{do}}$

12)　　${\boldsymbol{P}}_\text{opt}\xleftarrow{\text{顺序添加元素}}P.{\boldsymbol{v}}_{r}$

13)　${\bf{end\ for}}$

第一次动态规划是为RRT森林算法生成的路径删除冗余点, 优化后的路径显著缩短, 但仍由原路径的部分节点连接而成, 易出现不必要的绕行以及路径节点重叠, 这需要进一步优化. 而经过第一次动态规划所得的路径, 节点比较稀疏, 需要对路径进行插值处理, 得到节点较密集的路径用于第二次动态规划.

对于路径$ {\boldsymbol{P}}_i $, 经过第一次动态规划共有$ n_i $个航段, 可拟合成一个分段函数, 但由于路径中$ x $与$ y $不能唯一映射, 故采取从起点到该点的里程$ l_j $为参数, 其中$j = 1, 2,\cdots, n_i$, 将路径按参数方程的形式拟合成$ x(l) $与$ y(l) $如下:

按一定步长$\Delta l$取参数$ l $, 代入式(10)和式(11)中分别求得对应的$ x $坐标和$ y $坐标, 即得到插值后的路径$ \bar{{\boldsymbol{P}}}_i $.

对于经过插值处理后的路径$ \bar{{\boldsymbol{P}}}_i $, 再一次进行动态规划, 即可裁去$ \bar{{\boldsymbol{P}}}_i $中不合理的转角, 并解决绝大部分的路径重合问题.

注 3. 在实际高层消防任务中进行路径规划时, 所取步长$\Delta l$不宜过短, 否则会引发动态规划的维数灾难, 导致规划时间过长, 无法及时对路径进行重规划.

4.   仿真研究

本节对上述基于RRT森林算法的路径规划方法进行仿真研究. 根据障碍物的复杂程度以及高层消防任务所面临的实际环境可分为复杂环境和实际简单环境. 复杂环境以迷宫作为地图, 实际环境以实际高层住宅平面图作为地图. 又根据路径规划的起点和终点组合的数量, 分为单一路径规划和多路径协同规划. 其中, 单一路径规划主要以复杂环境下路径规划为主; 而多路径规划是在实际环境中进行3架无人机飞越5个航段的协同路径规划.

仿真研究共分为三部分: 1)对RRT森林算法与基本RRT算法、双向RRT算法在复杂环境下的搜索效率进行对比, 并给出多起点多终点路径规划表现; 2)对比RRT森林算法生成的路径经过各次动态规划优化后的效果; 3) RRT森林算法使用改进和未改进的障碍物接近检测的对比效果.

在仿真图中, 加粗的黑色线条表示结果路径, 通过不同的线型区分多路径, 浅色细实线表示RRT森林算法产生的随机树枝干. 圆点为起点, 五角星为终点, 若两者重叠, 则表示给定的中途点.

4.1   RRT森林算法与其他RRT算法搜索效率比较

为了测试RRT森林算法的路径规划效率, 本文对不同随机树数量的RRT森林算法与基本RRT和双向RRT在复杂环境下进行比较. 其中, 随机树数量取$ N_\text{Tree} = 1, 2, 5, 10, 20, 50 $, 随机树数量为1时算法相当于基本RRT, 为2时相当于双向RRT, 其余为RRT森林算法, 其他参数保持一致. 考虑到RRT算法的固有随机性, 对各种算法分别进行不少于1000次实验, 通过箱形图展示测试结果.

图7展示了基本RRT算法、双向RRT算法以及$ N_\text{Tree} = 30 $的RRT森林算法在复杂环境中的表现. 从图7(a)和图7(b)可见, 基本RRT以及双向RRT都产生了较多的无效点, 且集中在随机树的根节点附近, 表明搜索被局限在随机树现有节点附近. 而从图7(c)中可见, RRT森林算法则产生较少的无效点, 且采点比较均匀. 在可行区域内的圆点, 表示中间随机树初始根节点. 这些从随机点开始搜索的树, 从搜索初期就拓宽了RRT森林算法的搜索范围, 提升了搜索的广度, 避免了RRT算法搜索局限在某个范围内, 减少了无效搜索, 从而提升了在复杂环境中的搜索效率.

图 7  各算法在复杂环境下的运行结果

Fig. 7  Performance of algorithms in complex environment

下载: 全尺寸图片 幻灯片

图8用箱形图展示不同随机树数量的RRT森林算法及基本RRT算法、双向RRT算法的实验数据分布情况. 表1则揭示了图8(a)中箱形图的数据. 由表1结合图8(a)分析可得, 基本RRT算法在复杂环境下的搜索时间中位数为8.00 s, 双向RRT算法为2.57 s, 而RRT森林算法中, 搜索时间与随机树的数量呈先显著下降, 再略微增加的趋势, 当$ N_\text{Tree} = 20 $时, 搜索时间的中位数为0.48 s, 与基本RRT算法相比, 节约了94%的搜索时间. 另外, 根据每种算法的上四分位数与下四分位数之差, 可得搜索时间的稳定性与随机树数量呈正相关. 从图8(b)可知, 路径搜索所使用的迭代次数随着随机树数量的增加而减少. 从图8(c)可知, 生成路径的路径点数随着随机树的增多先减少后增加. 上述实验数据表明, 在RRT森林算法的运用中, 为避免路径过于复杂, 以及不必要的运算开销, 需要视环境复杂程度选择合适的随机树数量.

图 8  各算法在复杂环境中的实验数据

Fig. 8  Statistics among algorithms in complex environment

下载: 全尺寸图片 幻灯片

表 1  各算法搜索用时数据(s)

Table 1  Statistics of time used in exploring of each algorithm (s)

	基本 RRT	双向 RRT	RRT森林 (NTree = 20)
上四分位数	11.0064	3.5425	0.69081
中位数	7.9990	2.5683	0.48464
下四分位数	6.1111	1.8900	0.36128

下载: 导出CSV 

| 显示表格

注 4. 图8和表1所述的搜索用时数据, 包括取随机根节点的时间和路径搜索时间, 不包括动态规划的运算时间.

RRT森林算法基于多个随机树连接与合并, 可支持多路径规划, 因此可用于多无人机协同路径规划. 图9为RRT森林算法多路径规划的表现.

图 9  RRT森林算法多路径规划

Fig. 9  Multi-path planning by RRT-forest

下载: 全尺寸图片 幻灯片

从图9中可见, RRT森林算法通过将多随机树连接合并, 达到同时兼顾多起点多终点的路径规划目标, 最终达到所有起点和终点都被包含在同一个随机树上, 此时只需按照起点与终点的组合寻找路径即可. 值得注意的是, 这些路径之间往往都存在公共线段, 且这些路径比较曲折, 甚至有不必要的绕行, 因此无法直接应用于高层消防无人机协同任务当中, 需要进行删除冗余点等路径优化处理.

4.2   基于动态规划的路径优化

为了测试动态规划对路径的优化效果, 本文分别针对复杂环境下的单路径和实际环境下的多路径进行优化处理.

由图10(a)可知, 未经处理的路径非常曲折, 且存在冗余点. 对路径进行第一次动态规划得到图10(b), 发现许多折线被拉直. 显然, 从起点到终点的路程已被缩短, 但仍然存在不合理的转角. 将第一次动态规划的结果进行参数方程拟合并重新采样, 再次进行动态规划, 得到图10(c). 由图10(c)可见, 在转角处能够紧贴障碍物边缘, 以取得最短的路径.

图 10  单路径优化过程

Fig. 10  Optimization of single path

下载: 全尺寸图片 幻灯片

针对多条路径的优化效果, 本文进行了实际环境下的多路径优化实验. 假设3架无人机以不同高度飞行, 且飞行过程中保持高度不变. 仿真结果如图11所示. 由图9可发现, 处理前的路径非常曲折, 且有路径重叠的现象. 进行第一次动态规划后的效果如图11(a)所示, 由图可见, 许多不合理的转角被移除, 但仍然存在个别交叉与重叠, 部分转角仍然过大. 在图11(b)中, 对各条路径进行拟合并重新采样后, 再次进行动态规划, 并且按照任务设定将应合并的路径合并. 由图可见, 5条路径被合并成3条路径并完全分开, 各段路径笔直且不存在交点, 而重叠的路径仅存在于同一无人机飞往与飞离中途点的路径中.

图 11  多路径优化过程

Fig. 11  Optimization of multi-paths

下载: 全尺寸图片 幻灯片

4.3   障碍物接近检测改进

从图10可知, 经过两次动态规划得到的路径, 部分线段是紧贴障碍物的, 在多无人机执行高层消防救援任务时, 这样的路径可行性仍然较差, 因此需要在路径搜索以及动态规划中, 使用改进的障碍物接近检测代替原碰撞检测, 实现使路径与障碍物保持一定距离的目标. 对此, 本文对障碍物接近检测与原碰撞检测进行了对比实验.

注 5. 对于迷宫地图, 假设无人机直径$r_\text{UAV} \leq 20 \;{\rm{cm}}$; 对于实际高层建筑地图, 图上1个像素代表10 mm, 假设无人机直径$r_\text{UAV}\leq 30$cm, 并为每架无人机分配不同的飞行高度, 在飞行中高度将保持不变.

从图12(a)可见, RRT森林算法以及动态规划中使用未改进的碰撞检测函数时, 得到的路径与障碍物的最小距离为0, 这意味着无人机执行这样的路径会与障碍物发生剐蹭. 而从图12(b)可见, 使用改进的障碍物接近检测后, 设置安全距离为10 cm, 得到的路径与障碍物的距离显著大于使用原碰撞检测的算法所生成的路径. 图12(b)也给出了使用改进障碍物接近检测方法的RRT森林算法所得路径与障碍物的最短距离曲线, 可知路径到障碍物最小距离均大于给定安全距离10 cm. 可见, 改进的碰撞检测方法显著提高了路径的可行性, 使得RRT森林算法在未经膨胀处理的地图中, 通过设置安全距离进行路径规划可以得到更可行的路径.

图 12  复杂环境下原碰撞检测与改进碰撞检测对比

Fig. 12  Comparison between novel and original obstacle checking

下载: 全尺寸图片 幻灯片

需要注意的是, 当安全距离设置过大时会影响路径搜索效率, 导致搜索失败率增加, 搜索时间变长, 因此需要合理设置安全距离. 在高层建筑室内, 对于无人机仍有比较大的空间, 此时可以设置更大的安全裕度. 图13为一般室内环境下, 安全距离为15 cm的规划结果及其与障碍物之间的距离曲线, 此时总规划时间大约为1 s. 而对于图12中的复杂环境, 由于空间狭小, 当安全距离取10 cm时会使得可行区域大幅减少, 随机树生长成功率降低, 规划时间会超过2 s.

图 13  实际环境下改进碰撞检测效果

Fig. 13  Result of novel obstacle checking in practical environment

下载: 全尺寸图片 幻灯片

5.   结束语

考虑到多无人机协同执行高层室内消防救援任务时的高度紧迫性, 在路径规划方面本文提出RRT森林算法, 提高了路径搜索的速度以及结果可行性. 通过在RRT算法中增加中间树, 解决了RRT算法在前期搜索范围受限的问题, 提高了复杂环境下的路径搜索效率、搜索速度及鲁棒性, 并且为多无人机进行协同路径规划提供了重要支撑. 通过设计新型碰撞检测算法, 使得路径与障碍物始终保持安全距离. 应用两次动态规划和参数方程拟合方法, 移除了路径上的冗余节点, 增强了规划结果的实用性. 实验结果表明, RRT森林算法可实现复杂环境下快速规划多条安全可行的路径, 并能满足高层消防救援中路径频繁重规划的需求.

关于高层消防多无人机室内协同路径规划, 本文提出的RRT森林算法能在较短时间内得到可行性较高的路径, 但在无人机数量较多、任务分配复杂的情况下, 在协同能力方面仍有提升空间. 因此将来的工作可以致力于提高多无人机协同路径规划的能力.