欧阳子路，王鸿东*，黄一，杨楷文，易宏

1 上海交通大学海洋智能装备与系统教育部重点实验室，上海200240 2 上海交通大学海洋工程国家重点实验室，上海200240

0 引 言

近年来，随着世界各国对海洋安全防护、海洋资源勘探与开发以及无人军事作战系统的重视，水面无人艇（USV）因其自主程度高、机动性强等特点，已成为国内外研究的热点。随着无人艇技术的深入研究与军民两用的发展需求，无人艇编队的概念应运而生。相比单艘无人艇，无人艇编队在执行任务时具有效率高、容错性强及覆盖范围广的特点，在实际工程应用中有着重要的意义。

路径规划是无人艇编队的核心技术之一，用于在保持无人艇编队形状稳定的同时对障碍物进行避碰。目前，国内外已有专家学者对无人运载器编队路径规划问题展开研究并取得了一定的成果。Tam 等［1］提出了一种符合COLREGS 规则的多艇协同路径规划算法，仿真结果表明，在各种交通场景下，算法的输出是有效且一致的。Ni等［2］提出了一种虚拟力场的算法，通过引入面积比参数，增加了模糊控制模块来解决动态环境中的避障问题，并通过仿真实验演示了该方法的效率。Geng等［3］采用改进的粒子群算法提高了路径搜索的效率，在仿真实验中验证了所提方法在生成优化路径时的高效性。王跃午［4］提出了一种基于可变快速行进平方法的无人艇编队路径规划技术，该方法将可行区域看作各向异性，并认为某点的安全性与该点距障碍物的距离线性相关，具有实时性好的特点。包昕幼［5］在对常见的全覆盖路径规划进行分析后，采用往返式覆盖算法作为无人艇编队的覆盖方法，同时针对未覆盖区域采用改进的A-star 算法，通过Matlab 仿真实验验证了融合算法的可行性。苏青［6］对基于双层模糊逻辑的多机器人路径规划与协调避碰系统进行了研究，利用模糊控制的鲁棒性，提高了算法效率与可行性。从上述国内外研究情况来看，解决无人艇编队路径规划问题的思路主要为拓展演进经典算法，提高算法的实时性与规划效率。

受前人研究的启发，本文将提出一种基于改进快速搜索随机树（rapidly exploring random tree，RRT）算法的无人艇编队路径规划技术。首先，针对无人艇编队形状稳定问题，在RRT 算法扩展环节提出一种非严格保形修正向量与非严格保形控制圆区域，使搜索树有朝着严格保形坐标点生长的趋势；接着，针对突发障碍物与非严格保形规划点碰撞的问题，在RRT 算法碰撞检测环节提出可调节避碰圆区域与障碍物修正向量，使无人艇安全避碰并最大程度地保持队形稳定；最后，通过仿真实验验证该算法在无人艇编队路径规划问题中的可行性。

1 无人艇编队路径规划环境模型建立

无人艇编队在自主航行过程中通过智能感知系统实时定位并探测障碍物信息，本文所解决的无人艇编队路径规划问题如图1 所示。领航艇L以速度V 按预定航线第一象限角平分线航行，若干跟随艇为F1，F2，…，Fn，无人艇智能感知系统探测到突发障碍物区域B1和B2。对于该种情形，本文基于RRT 算法设计一种编队路径规划算法，使无人艇编队在航行过程中保持队形稳定，并对影响无人艇按规划航迹点航行的突发障碍物进行自动规避，最大程度地保持编队形状稳定。

图1 无人艇编队路径规划环境情形Fig.1 The situation of USV formation path planning

2 无人艇编队保形规划技术研究

2.1 经典RRT 算法

经典RRT 算法由LaValle 于1998 年提出［7］，现有的研究表明，RRT 算法与A*算法、随机路线图（PRM）算法、混合整数线性规划（MILP）算法相比，收敛速度显著加快，具有更高的优化效能［8-11］，因此在无人机、无人车等运载器上得到了广泛的应用［12-14］。经典RRT 算法构造方式是以给定的初始点为随机树根节点，根据当前环境快速有效地搜索可行域空间，通过产生随机点，将搜索导向空白区域并添加随机树节点直至目标点，最后通过反向搜索得到路径点。

2.2 “非严格保形修正向量”与“非严格保形控制圆”设计

经典RRT 算法中延伸子节点的构造方式为在任务空间C 内随机选择一个随机目标点；从随机树当前所有的叶节点中选出一个离该随机目标点最近的节点，称之为临近节点；然后从临近节点的方向延伸一个步长为S 的距离，从而得到该延伸子节点。该种构造方式适用于单智能体在位姿空间的全局路径规划情形，但由于无人艇编队航行有着队形保持稳定的需求，因此经典算法中延伸子节点不可控的构造方式不适用于无人艇编队全局保形规划技术，需要对其进行改进。针对此问题，本文提出一种非严格控制圆，以实现无人艇编队的非严格保形的全局路径规划，同时引进非严格保形修正向量提高保形精度，加速算法收敛。

设某时刻t 跟随艇智能感知系统探测到领航艇航行到位置点PL（xt，yt），各跟随艇的实时位置为PW（xW，yW），将PL代入编队位置函数Xs解算得到该时刻各跟随艇严格保形坐标点PF（xF，yF），此时，本文提出的无人艇编队路径规划RRT 算法将开始起作用，各跟随艇以自身实时位置为根节点，并行构造搜索树，实现无人艇编队的非严格保形路径规划。由于各跟随艇搜索树构造原理相同，本文为了叙述方便，以某单艘跟随艇进行算法阐述。

由经典RRT 算法的EXTEND 步骤［7］，该时刻跟随艇延伸子节点Pn（xn，yn）的计算公式为：

式中：xr，yr为该迭代过程中随机树生成的随机目标点Pr的横纵坐标；S 为无人艇单步探索步长。采用向量表示为

式中：PW，n为点PW到点Pn的位移；PW，r为点PW到点Pr的位移。由于经典算法中延伸子节点Pn依赖于随机点Pr的生成而构造，为了实现Pn能够以更大概率作为该跟随艇保形航路点，首先引进非严格保形修正向量AR对Pn进行坐标修正。AR的定义如式（3）所示。在AR作用下，延伸子节点Pn及其坐标不再满足式（1）和式（2），而由向量表达式（5）及式（6）给出：

式中：Pn，F为点Pn到点PF的位移；PW，r为点PW到点Pr的位移；PW，n′为点PW到点的位移；PF为编队位置函数解算得到的该时刻下的跟随艇严格保形坐标点；d 为点Pn与该时刻下的跟随艇严格保形坐标点PF（XF，YF）之间的欧氏距离；ω为保形修正系数；U为计算过程中的过渡向量。

式（3）表明，完成经典RRT 算法下的延伸后，在原始延伸的基础上添加由Pn到PF连线方向上ω作用的AR，让搜索树有朝着严格保形坐标点生长的趋势。式（4）为Sigmoid 函数的数学表达式，该函数图像如图2 所示。

图2 Sigmoid 函数图像Fig.2 Graph of Sigmoid function

由图2 可知，sigmoid 函数图像位于x 轴正半轴部分，具有如下特性：函数值会随着输入d 的增大而增大，但增大的幅度会逐渐减小，最后趋近于1。表现在跟随艇保形上则是当原始延伸子节点距离该时刻严格保形点越大时，AR作用越强；但是作用的幅度不会使向严格保形点趋近的向量的模超过单步探索步长而带来过度修正的问题。该过程几何上如图3 所示。

图3 非严格保形修正向量作用示意Fig.3 The action of non-strict conformal correction vector

式中，k为保形精度权值，可根据无人艇编队实际任务下的精度需求赋值。

图4 严格保形控制圆作用示意Fig.4 The action of non-strict conformal control circular area

3 无人艇编队局部自主避碰技术

无人艇编队在智能航行过程中可能会遭遇突发障碍物，传统的局部自主避障规划方法一般为进行局部修正以达到对障碍物的自主避碰。由于无人艇编队各单艇在实施避碰动作时同时具有保形需求，有较大可能出现可行区域非常狭窄的情形，若仍然采用经典RRT 算法延伸子节点的方法，会出现算法随机搜索陷入死区的情况。针对此问题，本文提出可调节避碰圆与障碍物修正向量的概念，在一定程度上放大可行区域，从而保证算法的正常运行，具体叙述如下：

当无人艇编队智能感知系统探测到突发障碍物对某单艇航行产生威胁时，此时将触发本文提出的障碍物修正向量，使搜索树有着远离障碍物延伸的趋势，以提高算法的搜索效率，减少延伸失败的次数。障碍物修正向量R定义为式（9）。在R的作用下，延伸子节点Pn及其坐标不再满足式（1）和式（2），而由向量表达式（11）给出：

式中：PO为障碍物中心点；PO，W为点PO到点PW的位移；λ为调节修正强度的权值；V为计算过程产生的过渡向量。式（11）表明，当无人艇编队感知到突发障碍物时，在原始延伸的基础上添加由PO到PW连线方向上根据λ动态调节的R，让搜索树延伸有着远离突发障碍物的趋势，如图5所示。

图5 障碍物修正向量作用示意Fig.5 The action of obstacle correction vector

同时，本文根据前述基于RRT 算法的无人艇编队保形规划技术，提出一种可调节避碰圆的概念，使无人艇编队不仅能实现对突发障碍物的有效避碰，而且能实现最大程度的保形。若式（11）计算得到的Pn

式中：(xO，yO)为突发障碍物中心点PO坐标；RO为突发障碍物区域半径。为了实现无人艇编队跟随艇对突发障碍物的避碰，引入了碰撞检测环节，由式（11）计算生成Pn' 后将不代入式（8）进行判定，而首先转入式（12）进行碰撞检测。若不满足式（12），则转回式（1）重新生成随机点与计算延伸子节点进行判定，若满足，则意味着该点在障碍物区域外，不会与突发障碍物发生碰撞；同时，为了实现在避碰安全条件下最大程度地保形，规定还需满足式（13）才可认为该周期搜索树延伸成功。

式中，Rr为可调节避碰圆半径，实际工程中，可根据障碍物安全区域与保形精度要求选取，如图6所示。

图6 可调节避碰圆作用示意Fig.6 The action of adjustable collision avoidance circle

4 仿真结果与分析

为验证改进RRT 算法的有效性，使用Microsoft Visual Studio 2017 开发环境编写C++程序进行仿真实验。本文考虑高速无人艇避碰问题，选取文献［15］所述USV 进行仿真实验，编队中各USV 航速均为20 kn，RRT 算法中S 依据文献［15］设为10，在该假设条件下，无人艇编队的单步航行周期约为1 s。

仿真实验中，假设领航艇L 的航行轨迹为二维坐标系第一象限角平分线从点（0，0）引出的射线，领航艇初始位置为（5 2，5 2），为了测试算法对各种避碰环境的通用性，在无人艇编队覆盖的地图区域由C++随机函数生成障碍物区域的圆心坐标与尺寸，障碍物设定的发现时间在一个单步航行周期，即每1 s 无人艇编队智能感知系统刷新当前航行态势，探测出影响正常航行的障碍物。进行10 个连续单步航行周期下的各跟随艇对领航艇的保形路径规划仿真实验，即生成10 组在突发障碍物约束条件下的无人艇编队连续航行路径规划点，编队结构选取为由3 艘USV 组成的等腰直角三角形结构。在编队路径规划仿真实验中，记1#跟随艇初始位置为（0，5 2），2#跟随艇初始位置为（5 2，0），根据2.2 节所述，非严格保形控制圆中k 取0.2，即保形精度为跟随艇的0.2S。

分别进行是否引入AR，R以及同时引入2 种改进方式的仿真实验对比。经典RRT 算法与改进RRT 算法编队路径点对比情况如图7 所示。改进RRT 算法规划出的对障碍物避碰周期下的编队路径点如图8 所示。考虑改进前、后的算法均为随机算法，每组对比重复进行8 次生成10 组连续路径点的实验，统计得到位置误差对比与算法运行时间对比如表1 所示。比较RRT 算法改进前后的指标，统计结果如表2 所示。每个实验序号中，定义某跟随艇平均保形误差ε为

图7 编队路径点对比Fig.7 Formation path point comparison

图8 改进RRT 算法在障碍物避碰周期下的编队路径点Fig.8 Improved RRT algorithm for formation path points in obstacle avoidance period

表1 重复实验的位置误差与算法运行时间对比Table 1 Conformal error and running time comparison for repeated experiments

表2 RRT 算法改进前后算法指标统计对比Table 2 Comparison of algorithm indexes before and after the improvement

式中：xFi，yFi分别为第i 个单步航行周期下的理论严格保形点横、纵坐标；xni，yni分别为第i个单步航行周期下的算法规划出的路径点横、纵坐标；n为算法运行的周期数，取10，该式可一定程度体现规划算法的编队保形精度。

综合分析以上实验结果，可以得出以下结论：

1）引入AR后，跟随艇的保形平均误差相比经典RRT 算法减小了7.3%，同时总计算规划时间得到了大幅降低，这归因于该向量使搜索树在每个单步航行周期内都有着向理论上的严格保形点生长延伸的趋势，从而使子延伸节点能够以更大的概率落在非严格保形控制圆区域内，从而减少了搜索树延伸失败的次数，降低了算法消耗；同时，AR也使延伸子节点坐标根据与严格保形点的欧氏距离进行了修正，从而降低了跟随艇路径规划点的保形平均误差，提高了无人艇编队的保形精度。

2）引入R后，总计算规划时间相比经典RRT算法同样得到了大幅度降低，而且算法总计算规划时间方差也低于原算法，这表明引入该向量后，不仅算法效能得到了提升，稳定性也得到了大幅提升，这归因于该向量在无人艇编队智能感知系统探测到障碍物从而被触发后，使无法正常航行的某跟随艇对应的路径搜索树有着远离障碍物中心点的趋势，从而降低了延伸子节点落入障碍物区域的概率；同时，由于该情形下的可行区域一般较狭窄，该向量既一定程度保留了经典RRT 算法的随机性，又对延伸子节点坐标进行修正，指引搜索树向第3 节所述可调节避碰圆区域生长，从而提高了算法的稳定性，降低了搜索陷入死区的概率。

3）结合表1、表2 与图7，改进算法同时融合AR与R后，发现跟随艇的保形平均误差相比经典RRT 算法减小了12.4%，总计算规划时间降低了76.9%，总计算规划时间方差得到了进一步降低，这表明完整改进算法兼有保形精度高、算法效能高及算法稳定好的优点，表现在既能连续规划出较稳定的编队结构的路径点，对突发障碍进行如图8 所示的避碰，同时耗时少，多次运行稳定。

5 结 语

本文将RRT 算法应用于无人艇编队路径规划问题上，针对无人艇编队形状稳定问题，在RRT算法扩展环节提出了一种非严格保形修正向量与非严格保形控制圆区域。非严格保形修正向量使搜索树有朝着严格保形坐标点生长的趋势，使延伸子节点有着更大概率得到保留；非严格保形控制圆区域能有效控制编队保形精度并调节计算量。针对突发障碍物与非严格保形规划点冲突问题，在RRT 算法碰撞检测环节提出障碍物修正向量与可调节避碰圆区域，使无人艇安全避碰并最大程度地保持队形稳定，有效处理了避碰这一航行安全性问题与保形的任务要求之间的平衡问题。本文所提算法耗时少、稳定性强、保形精度高，在实际工程中有一定的应用空间与意义。