Chinese Journal of Ship Research
RRT基于改进 算法的无人艇编队路径规划技术
，王鸿东，黄一，杨楷文，易宏（18）欧阳子路
12 13图 和图 分别给出了在两种编队控制器作用下控制力 F= 2 τi 的响应曲线。由图可i =1 5
知，在状态稳定阶段，有限时间编队控制器比渐近收敛控制器并不需要提供更多的控制力。
实时用于反馈且受到有界环境干扰情况下的多船编队控制问题，并实现了整个闭环系统的误差在有限时间内收敛。通过设计有限时间观测器以观测船舶的速度状态信息，并基于得到的观测值，结合齐次法设计了多船分布式编队的有限时间控制律，并根据李雅普诺夫稳定性判据以及齐次性理论证明了编队误差均能在有限时间内收敛。通过仿真，证明了本文方法的有效性，并与渐近收敛的编队控制方法进行对比分析，验证了本文方法具有更快的收敛速度、更高的控制精度以及更强的抗干扰能力。后续可以结合多船编队的避碰问题开展深入研究。
参考文献：
［ ［ ［付明玉，焦建芳. 基于领航者的船舶无源协调编队付明玉，余玲玲，焦建芳，等.控制饱和约束下的自主水面船编
，戈.丁磊，郭 一种船队编队控制的控制与决策.方法［张国庆，黄晨峰，吴晓雪，等. 考虑伺服系统增益不确定的船舶动力定位自适应有限时间控
摘 要：［目的］为了解决无人艇编队在智能航行时全局路径规划与局部自主避碰问题，提出基于改进快速搜突发障碍物与非严格保形规划点碰撞问题，在
工程应用中具有重要的意义。关键词：无人艇编队；快速搜索随机树算法；路径规划；避碰
算法扩展环
节提出一种非严格保形修正向量与非严格保形控制圆区域，使搜索树有朝着严格保形坐标点生长的趋势；针对
算法碰撞检测环节提出可调节避碰圆区域与障碍物修正向
量，使无人艇安全避碰并最大程度地保持队形稳定。［结果］结果显示，无人艇编队在该算法作用下表现出了良
好的保形性能，并能对突发障碍物进行有效的避碰。［结论］该算法效能高、稳定性强、路径规划质量高，在实际
1 200240上海交通大学 海洋智能装备与系统教育部重点实验室，上海2 200240上海交通大学 海洋工程国家重点实验室，上海
0引言
近年来，随着世界各国对海洋安全防护、海洋资源勘探与开发以及无人军事作战系统的重视，水面无人艇（USV）因其自主程度高、机动性强等特点，已成为国内外研究的热点。随着无人艇技术的深入研究与军民两用的发展需求，无人艇编队的概念应运而生。相比单艘无人艇，无人艇编队在执行任务时具有效率高、容错性强及覆盖范围广的特点，在实际工程应用中有着重要的意义。路径规划是无人艇编队的核心技术之一，用于在保持无人艇编队形状稳定的同时对障碍物进行避碰。目前，国内外已有专家学者对无人运载器编队路径规划问题展开研究并取得了一定的成Tam COLREGS果。 等［1］提出了一种符合 规则的多艇协同路径规划算法，仿真结果表明，在各种交通场景下，算法的输出是有效且一致的。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文基于 算法设计一种编队路径规划算法，使无人艇编队在航行过程中保持队形稳定，并对影响无人艇按规划航迹点航行的突发障碍物进行自动规避，最大程度地保持编队形状稳定。
2 无人艇编队保形规划技术研究2.1 经典RRT算法
RRT LaValle 1998年提出［7］，现经典 算法由 于RRT A*有的研究表明， 算法与 算法、随机路线图（PRM MILP ）算法、混合整数线性规划（ ）算法相比，收敛速度显著加快，具有更高的优化效能［8-11］，因此在无人机、无人车等运载器上得到了广泛的RRT应用［12-14］。经典 算法构造方式是以给定的初始点为随机树根节点，根据当前环境快速有效地搜索可行域空间，通过产生随机点，将搜索导向空白区域并添加随机树节点直至目标点，最后通过反向搜索得到路径点。
2.2 “非严格保形修正向量”与“非严格保形控制圆”设计
RRT经典 算法中延伸子节点的构造方式为在任务空间C内随机选择一个随机目标点；从随机树当前所有的叶节点中选出一个离该随机目标
点最近的节点，称之为临近节点；然后从临近节点的方向延伸一个步长为S的距离，从而得到该延伸子节点。该种构造方式适用于单智能体在位姿空间的全局路径规划情形，但由于无人艇编队航行有着队形保持稳定的需求，因此经典算法中延伸子节点不可控的构造方式不适用于无人艇编队全局保形规划技术，需要对其进行改进。针对此问题，本文提出一种非严格控制圆，以实现无人艇编队的非严格保形的全局路径规划，同时引进非严格保形修正向量提高保形精度，加速算法收敛。设某时刻t跟随艇智能感知系统探测到领航艇航行到位置点P（L xt，y）t ，各跟随艇的实时位置为 PW（xW，yW），将 PL代入编队位置函数Xs解算得到该时刻各跟随艇严格保形坐标点P（F xF，yF），此时， RRT本文提出的无人艇编队路径规划 算法将开始起作用，各跟随艇以自身实时位置为根节点，并行构造搜索树，实现无人艇编队的非严格保形路径规划。由于各跟随艇搜索树构造原理相同，本文为了叙述方便，以某单艘跟随艇进行算法阐述。RRT EXTEND由经典 算法的 步骤［ 7］，该时刻跟随艇延伸子节点P（n xn，yn）的计算公式为： y - yW x = x + S ´ cos(arctan( )) r n W x - x r W y - y 1 y = y + S ´ sin(arctan( )) （ ） r W n W x - xW r式中：xr，yr为该迭代过程中随机树生成的随机目标点Pr的横纵坐标；S为无人艇单步探索步长。采用向量表示为：
P= S ´ W n
PPWr
W r
（2）
式中： 为点 P 到点 P 的位移； 为点W n W n W r P 到点 P 的位移。由于经典算法中延伸子节点W r P 依赖于随机点 P 的生成而构造，为了实现 Pn n r能够以更大概率作为该跟随艇保形航路点，首先引进非严格保形修正向量 对 P 进行坐标修R n的定义如式（3）所示。在正。 作用下，延R R及其坐标不再满足式（1）和式（2），而伸子节点 P n由向量表达式（5）及式（6）给出 P n' ： nF = ω ´ S´ F| R n
PAAU= S ´
PAPP+1e-d ω = 1
PPWr
W r
= S ´ W n′ + |
AP3 （ ）
（4）
（5）
（6）
为点 P 到点 P 为点 PW n F n F W r到点 P r 的位移； 为点 P 到点 P n' 的位移； W n′ W P 为编队位置函数解算得到的该时刻下的跟随F艇严格保形坐标点；d为点 P 与该时刻下的跟随n艇严格保形坐标点P（F XF，YF）之间的欧氏距离；ω为计算过程中的过渡向量。式（3）表 RRT明，完成经典 算法下的延伸后，在原始延伸的基础上添加由 P 到 P 连线方向上n F ω作用的 ，让搜索树有朝着严格保形坐标点生R式（4）为 Sigmoid长的趋势。 函数的数学表达式， 2该函数图像如图 所示。
AP2 sigmoid由图 可知， 函数图像位于x轴正半轴部分，具有如下特性：函数值会随着输入d的增大而增大，但增大的幅度会逐渐减小，最后趋近1。表现在跟随艇保形上则是当原始延伸子节于 作用越强； R但是作用的幅度不会使向严格保形点趋近的向量的模超过单步探索步长而带来过度修正的问题。3该过程几何上如图 所示。依式（6）计算给出以后，需判定其是否当 P n'满足无人艇编队路径规划要求以进行下一步的迭RRT代，本文在经典 算法的基础上提出了非严格保形控制圆的概念。该概念的出发点为，若严格要求跟随艇从实时位置PW规划路径至该时刻下
3 无人艇编队局部自主避碰技术
无人艇编队在智能航行过程中可能会遭遇突发障碍物，传统的局部自主避障规划方法一般为进行局部修正以达到对障碍物的自主避碰。由于无人艇编队各单艇在实施避碰动作时同时具有保形需求，有较大可能出现可行区域非常狭窄的情RRT形，若仍然采用经典 算法延伸子节点的方法，会出现算法随机搜索陷入死区的情况。针对此问题，本文提出可调节避碰圆与障碍物修正向量的概念，在一定程度上放大可行区域，从而保证算法的正常运行，具体叙述如下：当无人艇编队智能感知系统探测到突发障碍物对某单艇航行产生威胁时，此时将触发本文提出的障碍物修正向量，使搜索树有着远离障碍物延伸的趋势，以提高算法的搜索效率，减少延伸失式（9）。在败的次数。障碍物修正向量 定义为的作用下，延伸子节点 P 及其坐标不再满足n
RR式（1）和式（2），而由向量表达式（11）给出 ' Pn ： OW = λ ´ S´
RV= S ´
PPPPWr
W r
P = S´ W n′
O W
V|
（9）
（10）
（11）
式中：P 为障碍物中心点；P 为点 P 到点 PW O O W O的位移；λ 为计算过程11产生的过渡向量。式（ ）表明，当无人艇编队感知到突发障碍物时，在原始延伸的基础上添加由P到 P连线方向上根据 λ动态调节的 ，让搜O W 5索树延伸有着远离突发障碍物的趋势，如图所示。
RRRT同时，本文根据前述基于 算法的无人艇编队保形规划技术，提出一种可调节避碰圆的概念，使无人艇编队不仅能实现对突发障碍物的有效避碰，而且能实现最大程度的保形。若式（11）计' '算得到的 P n' 位于突发障碍物区域，即 P n'(x n，y n)满足式（12）： （12） ' ' (x - x O)2 + ( y - y O)2 R O2 n n式中： (xO y O) 为突发障碍物中心点 P 坐标； RO O为突发障碍物区域半径。为了实现无人艇编队跟随艇对突发障碍物的避碰，引入了碰撞检测环节，式（11）计 式（8）进由 算生成 P n' 后将不代入 行判式（12）进行碰撞检测。若不满足定，而首先转入式（12），则转回式（1）重新生成随机点与计算延伸子节点进行判定，若满足，则意味着该点在障碍物区域外，不会与突发障碍物发生碰撞；同时，为了实现在避碰安全条件下最大程度地保形，规13定还需满足式（ ）才可认为该周期搜索树延伸成功。
（13） (x' - x )2 + ( y' - y )2 R 2 n F n F r式中，R 为可调节避碰圆半径，实际工程中，可根据r
4 仿真结果与分析
RRT Micro⁃为验证改进 算法的有效性，使用soft Visual Studio 2017 C++程序进开发环境编写行仿真实验。本文考虑高速无人艇避碰问题，选15 USV取文献［ ］所述 进行仿真实验，编队中各USV 20 kn，RRT 依据文献［15］航速均为 算法中S 10，在该假设条件下，无人艇编队的单步航设为 1s。行周期约为 L仿真实验中，假设领航艇 的航行轨迹为二点（0，0）引出的射维坐标系第一象限角平分线从线，领航艇初始位置为（5 2 ，5 2），为了测试算法对各种避碰环境的通用性，在无人艇编队覆盖C++随机函数生成障碍物区域的圆的地图区域由心坐标与尺寸，障碍物设定的发现时间在一个单1s步航行周期，即每 无人艇编队智能感知系统刷新当前航行态势，探测出影响正常航行的障碍10物。进行 个连续单步航行周期下的各跟随艇10对领航艇的保形路径规划仿真实验，即生成 组在突发障碍物约束条件下的无人艇编队连续航行3 USV路径规划点，编队结构选取为由 艘 组成的等腰直角三角形结构。在编队路径规划仿真实验1#跟随艇初始位置为（0，5 2），2#跟随艇中，记 2.2初始位置为（5 2，0），根据 节所述，非严格保0.2，即保形精度为跟随艇的0.2S。形控制圆中k取 2种分别进行是否引入 以及同时引入R RRT改进方式的仿真实验对比。经典 算法与改RRT 7进 算法编队路径点对比情况如图 所示。改RRT进 算法规划出的对障碍物避碰周期下的编8队路径点如图 所示。考虑改进前、后的算法均8 10为随机算法，每组对比重复进行 次生成 组连续路径点的实验，统计得到位置误差对比与算法1 RRT运行时间对比如表 所示。比较 算法改进前2后的指标，统计结果如表 所示。每个实验序号中，定义某跟随艇平均保形误差 ε 为
A
k (x - y Fi)2 + (x ni' - y ni')2 Fi 14 ε = （ ） i =1式中：x ，y 分别为第i个单步航行周期下的理Fi Fi论严格保形点横、纵坐标；x ，y 分别为第i个单ni ni
步航行周期下的算法规划出的路径点横、纵坐标； 10，该式可一定程度体n为算法运行的周期数，取现规划算法的编队保形精度。综合分析以上实验结果，可以得出以下结论： 1）引入 后，跟随艇的保形平均误差相比R RRT 7.3%，同时总计算规划时间经典 算法减小了得到了大幅降低，这归因于该向量使搜索树在每个单步航行周期内都有着向理论上的严格保形点生长延伸的趋势，从而使子延伸节点能够以更大的概率落在非严格保形控制圆区域内，从而减少了搜索树延伸失败的次数，降低了算法消耗；同时， 也使延伸子节点坐标根据与严格保形点R的欧氏距离进行了修正，从而降低了跟随艇路径规划点的保形平均误差，提高了无人艇编队的保形精度。2）引入 RRT后，总计算规划时间相比经典算法同样得到了大幅度降低，而且算法总计算规划时间方差也低于原算法，这表明引入该向量后，不仅算法效能得到了提升，稳定性也得到了大幅提升，这归因于该向量在无人艇编队智能感知系统探测到障碍物从而被触发后，使无法正常航行
AARn的某跟随艇对应的路径搜索树有着远离障碍物中心点的趋势，从而降低了延伸子节点落入障碍物区域的概率；同时，由于该情形下的可行区域一般RRT较狭窄，该向量既一定程度保留了经典 算法的随机性，又对延伸子节点坐标进行修正，指引搜3索树向第 节所述可调节避碰圆区域生长，从而提高了算法的稳定性，降低了搜索陷入死区的概率。3）结 1、表 2 7，改进算法同时融合合表 与图与 后，发现跟随艇的保形平均误差相比经典R RRT 12.4%算法减小了 ，总计算规划时间降低了76.9%，总计算规划时间方差得到了进一步降低，这表明完整改进算法兼有保形精度高、算法效能高及算法稳定好的优点，表现在既能连续规划出较稳定的编队结构的路径点，对突发障碍进行如8图 所示的避碰，同时耗时少，多次运行稳定。
AR5结语
RRT本文将 算法应用于无人艇编队路径规RRT划问题上，针对无人艇编队形状稳定问题，在算法扩展环节提出了一种非严格保形修正向量与非严格保形控制圆区域。非严格保形修正向量使搜索树有朝着严格保形坐标点生长的趋势，使延伸子节点有着更大概率得到保留；非严格保形控制圆区域能有效控制编队保形精度并调节计算量。针对突发障碍物与非严格保形规划点冲突问RRT题，在 算法碰撞检测环节提出障碍物修正向量与可调节避碰圆区域，使无人艇安全避碰并最大程度地保持队形稳定，有效处理了避碰这一航行安全性问题与保形的任务要求之间的平衡问题。本文所提算法耗时少、稳定性强、保形精度高，在实际工程中有一定的应用空间与意义。
参考文献：
［
包昕幼. 浅水区域无人探测艇编队巡航路径规划研
摘 要：［目的］参数横摇是船舶在波浪中的特殊失稳现象，现有研究认为，波浪经过船体时稳性参数的变化是
激发船体横摇的主要原因，但其力学机理并不明确。［方法］首先，基于惯性坐标下的垂荡和纵摇耦合运动方程，以及船体坐标下的横摇运动方程，建立垂荡、纵摇和横摇的混合动力学模型；然后采用所提出的摇荡耦合切片计算方法，数值计算船舶参数横摇运动，分析数值计算的横摇运动规律，并基于能量原理提出发生参数横摇的衡准。［结果］研究结果表明，船舶发生参数横摇的力学机理是，在横摇角增大过程中回复力矩系数吸收的能量
小于横摇角减小过程中回复力矩释放的能量；发生参数横摇的衡准是，回复力矩系数一阶谐波分量的相位角位
1 430033海军工程大学 舰船与海洋学院，湖北 武汉2 200235海军研究院 特种勤务研究所，上海
准对于船舶第 代完整稳性衡准的制定具有参考意义。关键词：船舶；波浪；参数横摇；力学机理
标准中，参数横摇的薄弱性衡准是采用稳性变化量的大小来衡量［6］，然而，不同类型的船舶其计算结果离散较大，这表明以稳性变化量大小作为参数横摇衡准只是权宜办法［5］。本文拟采用所提出的摇荡耦合切片计算方法［13］，克服大幅运动时参数含义的模糊性，以及会诱发更多自由度运动耦合的缺陷，数值计算船舶的参数横摇运动，由此分析发生参数横摇运动的力学机理，并基于能量原理提出船舶参数横摇的衡准。
1 坐标系及广义参数1.1 坐标系
3个坐标系分别为惯性坐标系、船体运动坐1标系和船体固定坐标系，如图 所示。
只适合小角度情况，它存在转动顺序问题，即转动顺序不同，船体姿态亦不同。在极端情况，例如当90°时，先后不同地转动横倾横倾角和纵倾角均为角和纵倾角，船体姿态会有很大的差异。为此，本文定义横倾角绕船体纵向坐标轴转动，纵倾角绕静水面横轴线（My轴）转动，具体定义如下。假定船舶静水漂浮时吃水为T ，船体绕 Mx轴线转动横摇角 φ ，再沿 Mz 轴平移吃水增量2）。这里， DT ，最后再绕 My 轴转动纵倾角θ（图称 DT ，θ 分别为广义吃水增量和广义纵倾角。根据转动关系，不难得到船体运动坐标系和船体固定坐标系的转换关系为： x =- (T cos φ +D T )sin θ + x cos θ - y sin φ sin θ+ B B z cos φ sin θ B y =- T sin φ + y cos φ + z sin φ B B z =- (T cos φ +D T )cos θ - x sin θ - y sin φ cos θ+ B B z cos φ cos θ B
1 （ ）
3 水动力计算式3.1 瞬时波面下压力分布
由势流理论，可知船体运动坐标系下小振幅
规则波升 ζ 可表达为ζ = A cos[ω t + k(x cos χ + y sin χ)] e
水下压力 p的分布式为e指数形式
p =- ρgz + ρgAekz cos[ω t + k(x cos χ + y sin χ)] = e （4） -ρgz + ρgAekz cos Θ式中：k 为波数；χ 为浪向角；t为时间；A为波幅；
ω 为遭遇频率；Θ 为计入了时间和位置的波浪相e
位。为方便起见，令 Θ = ω t + k(x cos χ + y sin χ) 。e式（4）并不适合直接做压力积分计算，为此，通常采用静水压力和拉伸修正的近似方法［14］，或不做修正。本文采用线性近似，为此，做一个变量代换： z = z1 + A cos Θ该代换的含义是，z坐标是以静水面为零点，
而 z1 坐标则是以瞬时波面为零点。将其代入式（3），忽略高阶项后，可得压力的近似表达式为（5） p =- ρ′gz1式中， ρ′ = ρ(1 - α0 cos Θ) ，为等效水的密度，其中= 4 α0 Ak ，为最大波倾角。式（ ）满足瞬时波面=0）处压力 =0，这就意味着瞬时波面下的压（ z1 p力用等效静水压力近似，等效静水的密度 ρ′ 在波＞0）较峰区域（ cos Θ 实际水的密度 ρ 小，在波谷＜0）较实际水的密度区域（ cos Θ ρ大。
3.2 入射波浪的位移力
利用高斯定理，将瞬时波面下船体湿表面的
压力积分转换成体积分： ¶p ( ¶x
F=
MFS =
S pnds =
MV ´ pnds = 6 （ ）
式中， 和 分别为作用于船体的力和绕坐标原点的力矩；s为船体湿表面的面积；V 为波浪下
为船体表面的单位外法向
矢量； 为微分湿表面 ds 到坐标原点的位置矢3分别为船体运动坐标系的 个单位坐标矢量。将压力的表达式（4）代入到上述力的计算3个公式中，不难导出瞬时波面下船体入射波浪方向的作用力为：
V ¶p + ¶y
(Ñp ´ ¶p + ¶z