Chinese Journal of Ship Research
Bi-RRT基于改进 的无人水面艇自动避碰算法
，王鸿东，王检耀，易宏欧阳子路
网络首发地址：http://kns.cnki.net/kcms/detail/42.1755.TJ.20191206.1718.002.html期刊网址：www.ship-research.com引用格式：欧阳子路，王鸿东，王检耀，等. Bi-RRT的无人水面艇自动避碰算法［J］.中国舰船研究，2019，14基于改进（6）：8-14. Ouyang Z L，Wang H D，Wang J Y，et al. Automatic collision avoidance algorithm for unmanned surface vessel based on improved Bi-RRT algorithm［J］. Chinese Journal of Ship Research，2019，14（6）：8-14.
：［目的］提出一种实现无人水面艇（USV）在高速航行时自动规避障碍物的方法。［方法］将
摘 要 的无人水面艇自动避碰算法。针对 双向搜索树（Bi-RRT）算法与速度障碍法相结合，得到基于改进Bi-RRT Bi-RRT
算法扩展操作中父节点延伸方向位于锥形碰撞区内的情况，提出避碰危险度系数与障碍物排斥向量，使父节点延伸方
向有远离障碍物中心的趋势。同时，针对算法实时性问题，提出两棵搜索树并行延伸扩展的方式，以及当父节点延伸方向位于锥形碰撞区外时触发的目标吸引向量，以加速算法收敛。［结果］结果显示，采用上述改进方法设计的算法搜索树延伸失败次数降低，规划的避碰路径短且更加平滑。［结论］该改进 Bi-RRT算法实时性强、
路径规划质量高，对实际工程应用有重要意义。关键词：无人水面艇；自主避障；双向搜索树算法；速度障碍法中图分类号：U664.82 文献标志码：A DOI：10.19693/j.issn.1673-3185. 01450收稿日期：2018 - 09 - 26 网络首发时间：2019-12-9 15:02基金项目：海洋工程国家重点实验室基金资助项目（GKZD010074）；上海市青年科技英才扬帆计划资助项目（18YF1411500）作者简介：欧阳子路，男，1996年生，硕士生。研究方向：无人艇智能航行技术。E-mail：ouyang_sjtu@sjtu.edu.cn王鸿东，男，1989年生，博士，副研究员。研究方向：舰船系统工程，海洋智能装备与系统开发。E-mail：whd302@sjtu.edu.cn
0引言
无人水面艇（USV）具有自主程度高、航速快、隐身性能好及机动性强等特点，被广泛应用在水文探测、海事巡航以及军事作战等领域。近年来，随着人工智能技术的再次兴起及自动驾驶技术的大规模应用，USV作为智能技术与传统船舶学科交叉融合的产物，已成为国内外智能装备的研究热点之一。USV自动避碰是 实现智能航行的关键技术之USV一，一定程度上反映了 智能化水平的高低［1-2］。USV国内外许多学者在 避碰方面也取得了一定的Svec 3 USV等［3］提研究成果。 出了 种考虑风浪对艇体造成影响的规避障碍物方法，分别为：考虑障碍物边界进行路径规划；考虑外界环境的影响修正规划路径并再规划；将局部边界最优规划与启A*发式 算法相结合并通过博弈树搜索位置。Kuwata等［4］采用速度障碍法并结合国际海上避碰USV规则设计了 避碰方法，该方法在避碰过程中USV根据 操纵运动特性设置了最小转向角并考虑风浪流的影响。Benjamin等［5］将行为控制框架原理、间隔规划等多重思想融入多目标区间优化方USV避碰问题。庄佳园等［6］考虑国际法中来解决 USV海上航行规则，将 碰撞分为追越、交叉相遇与3正面相遇 种局面，设计了一种符合国际海上航行规则的相对坐标系下动态规避方法，并搭建了USV USV半实物仿真平台。吴博等［7］通过分析 与障碍物之间的相对速度和相对位置的角度关系，提出了一种基于速度障碍法并考虑风浪流影响的USV操纵运动特性的自主避碰算法。张洋洋等［8］将速度障碍法和动态窗口法相结合，提出了一种无人水面艇动态避障算法。USV从上述国内外研究情况来看，解决 避碰问题的主要思路可分为两类：一类是将其转化为局部路径规划问题并与智能算法相结合；另一类是基于速度障碍法得出避碰路径的可行范围。而目前将两者结合的研究成果较为少见。受前人研究启发，本文拟将速度障碍法基本Kinodynamic思想与已成功运用于 路径规划问题［9］的双向搜索树（Bi-RRT）算法相结合，首先针对父节点延伸方向位于锥形碰撞区内的情况提出避碰危险度系数ω与障碍物排斥向量 R ，从而使搜索树向着远离障碍物的趋势延伸；然后提出一种两搜索树并行延伸的方法及当父节点延伸方向位于锥形碰撞区外时触发的目标吸引向量 A ，以提高算法的实时性并加强两搜索树朝目标点靠近的趋势；最后通过仿真实验证明改进算法的可行性与优越性。
1 无人水面艇避碰模型
USV在自主航行过程中通过智能感知系统实时探测预定航线及其周边的障碍物，并自动规避USV 1障碍物。本文设定的 避碰问题如图 所示，具体描述如下：t0 时刻艇A以速度V 沿预定航线USV OS航行， 智能感知系统感知到航线OS以及周边出现此前未探测到的障碍物区域 B1 ，B ， 2 A B ；以艇 总长为直径作圆，所覆盖的区域视作该3 USV Bi-RRT区域。基于上述情况，本文基于改进A算法设计一种自动避碰算法，使艇 尽量保留原定航线，在 B ，B 的限制下安全、高效地规避 B1 。2 3
2 速度障碍法和Bi-RRT算法2.1 速度障碍法的原理
Fiorini等［10］提出，该方法的避速度障碍法由 Featherstone碰原理为：首先，基于 的理论，将任意多边形表示为相应的一系列圆，同时为了便于建USV模与计算，将 与障碍物分别用不同的二维圆USV形代替；然后，将 尺寸附加到障碍物尺寸上计USV算两者之间的速度障碍关系，从而将 简化为一个质点，使得障碍物区域膨胀为半径更大的圆USV形区域；最后，定义锥形碰撞区为 与障碍物发USV生碰撞的相对速度的集合，几何上表示为以为顶点向障碍物圆周作出的两条切线所夹的二维区域，而在锥形碰撞区以外区域中的相对速度将USV使 安全避碰。USV速度障碍法为 避碰过程中如何改变和控Bi-RRT制航向提供了简单高效的依据，也是 算法USV在 避碰中运用及改进的理论依据。
2.2 Bi-RRT算法
Bi-RRT La Valle 等［11 ］受算法是 双向搜索技La Valle 1998术［12］的启发而提出。相比 于 年提出的经典快速扩展随机树（RRT）算法［12］，Bi-RRT算法分别以起点 X 与终点 X 为根节点，构造两init goal
棵快速扩展随机树，两树同时生长，直到相遇为止。Bi-RRT RRT算法由于保持了经典 算法在状态空间中规划路径的思想，所以也保持了其优越性。La Valle［13］对 RRT算法分别进行了完整性约束、非完整性约束、Kinodynamic、闭式运动链条件下的路径规划测试，结果表明，该算法在处理具有微分约束的系统及单查询非完整性约束系统的路Bi-RRT径规划问题上表现得非常高效。 算法作RRT为 算法的改进，算法终止条件是以路径规划起点为根节点生长的搜索树与以路径规划终止点为根节点生长的搜索树相遇，理论上这可以提高对位姿空间搜索的效率；同时在该算法中，两树扩展节点的操作有着向对方搜索树靠近的趋势，理论上可以增强搜索的目的性，缩短路径规划时间。设两棵搜索树分别为T 与T ，搜索树 T 以a b a USV当前位置 X 进行初始化，搜索树T 以原始init b航线上的航迹恢复点 X 进行初始化。迭代过goal
程中某搜索树T 生成随机点 P ( X r Y r) ，T 中离r该随机点 P 最近的节点为 P ( X p Y p) ，P 将作为r p p USV该迭代过程中的父节点以 探索步长 S 向 Pr进行延伸，延伸得到的子节点为 P ( X Y n) 。障n n碍物坐标为 P ( X o Y o) ，障碍物膨胀半径为 R 。o Bi-RRT 2程序流程如图 所示。Bi-RRT USV算法应用于 避碰时两棵搜索树A分别以艇 当前位置 X ( X 0 Y 0) 与预定航线OS init上的航迹恢复点 X ( X1 Y1) 为根节点，然后不断goal扩展节点直到两树满足相遇条件，最后通过两棵A搜索树相遇点分别进行回溯操作，得到艇 避碰3路径点及完成路径规划。图 所示为算法的构建过程。
3 Bi-RRT算法的改进
USV基于速度障碍法，综合考虑 避碰的实际情况与算法本身的运行效率及实时性，针对搜索树延伸方式不利于智能规避障碍物以及父节点选Bi-RRT取方式影响算法实时性的弊端，本文对 算法进行了改进。
3.1 障碍物排斥向量R的设计
Bi-RRT算法搜索树延伸方式为：先将一棵搜索树中距离位姿空间随机点最近的节点扩展至新节点，然后再扩展另一棵搜索树距离该新节点最近的节点。尽管该扩展方法理论上加强了两棵树延伸的目的性，但是若该随机点引导两棵搜索树
USV同时向趋近障碍物方向发展，其表现在 避碰USV模型上则是该随机点引导 航向调整至锥形碰撞区，所带来的影响是轻则加大后续节点延伸标准步长后落至障碍物区域概率增大，加大算法的无谓消耗而重则使算法无法收敛和规划出有效的避碰路径。鉴此，本文提出一种基于速度障碍法的障碍R，以 Bi-RRT物排斥向量 改进 算法的延伸步骤，改进后在迭代过程中能自动识别父节点延伸方向是否位于锥形碰撞区，并能根据避碰危险度系数R USV ω动态调整 的作用强度，使 避碰路径更加合理高效。Bi-RRT根据 算法的延伸步骤，延伸子节点坐标 P ( X n Y n) 为n
USV若该次延伸操作使 航向引导至锥形碰撞区，即几何上满足 α1 < θ1 ，其中：
Bi-RRT此时，将触发本文改进 算法中 R 与ω ，在 R与 ω作用下，延伸子节点 P 及其坐标不n再满足式（1）和式（2），由 V过渡向量 及向量表达式（8）得到 ′ P ： n
为安全避碰临界距离。式（6）的核心函式中，L s 4数为双曲正切函数 y = tanh(x) ，该函数图像如图所示。 7式（ ）表明，当父节点 P 延伸方向位于锥形p碰撞区以内时，USV航向偏危险，此时在原始延伸基础上，添加 P 到 P 连线方向根据 ω动态调节o p 5的 R ，让搜索树朝着远离障碍物的趋势延伸。图所示为R与 ω作用示意图。4由图 可知，函数 y = tanh(x) 位于 x 轴正半轴部分有如下特性：1）当输入 x Î(0 2) 时，y Î[ 0 1]且数值变化剧烈；2）当输入 x Î( 2 +¥) 时，y 对 x | |> | |的的变化不敏感。当 P P L 时，随着 P P p o s p o 2），此 1增大，根据特性 时 ω值保持在 附近，表现USV在 避障上则是当父节点 P 距离障碍物中心p满足一定安全限度时， R 的作用不明显；当| |< | | 1）， P P L 时，随着 P P 的减小，根据特性p o s p o USV此时 ω值将显著增大，表现在 避障上则是父节点 P 越靠近障碍物中心，R的作用越明显，使p USV航向向着远离障碍物的趋势延伸。
3.2 并行伸展与目标点吸引向量A的设计
Bi-RRT算法对两棵搜索树的父节点的定位与伸展规定了先后顺序，T 产生新节点 P 后，Tb a n才能实施延伸步骤。尽管两棵树延伸有相互接近的趋势，但是这种顺序结构的执行无谓地消耗了运行时间。为此，本文提出一种两棵搜索树并行伸展且在特定条件下具有相互接近趋势的改进方法。该改进方法通过两棵搜索树各自产生随机点，根据随机点各自选取父节点自行延伸的方式完成搜索树的扩展。R在特定条件下激发目标点吸引向量A，使 Bi-RRT搜索树并行拓展延伸的同时也不失算法两搜索树相互趋近的优点。在迭代过程中，设某棵搜索树基于式（1）得到Bi-RRT P 后，若 α1 > θ1 ，此时将触发本文改进 算n法中的 A ，在 A作用下，延伸子节点 P 及其坐标n 1 2 19不再满足式（ ）和式（ ），由过渡向量U 及式（ ）给出 Pn '' ：
式中，P 为目标点坐标。对于搜索树T 而言，Pg g a为原始航线上航迹恢复点 X ；对于搜索树 Tb goal USV而言，P 为 当前位置 X 。g init式（9）表明，当父节点延伸方向位于锥形碰撞USV区外时， 航向偏安全，此时在原始延伸基础上，添加 P 到 P 连线方向上的 A ，使搜索树有朝p g 6着目标点生长的趋势。图 所示为 A作用示意图。
3.3 改进Bi-RRT自动避碰算法流程
由于本文两搜索树为同时并行延伸，互不干扰，因此以 T 为例，给出利用本文算法实现自动a 7 1避碰的流程，如图 所示。 T 除在第 步初始化b步骤中是以原始航线上航迹恢复点 X 初始化goal
外，其他流程与T 完全相同。a
4 仿真结果与分析
Bi-RRT为了验证改进 算法的有效性，使用Microsoft Visual Studio 2017 C++程开发环境编写Bi-RRT序进行仿真实验，并将 算法与本文改进Bi-RRT算法进行了对比。本文选取文献［14］所USV 20 kn，设定述高速 进行仿真实验，其航速为=10。单步探索步长 S仿真实验中，USV预定航线OS设为二维坐标1象限角平分线上从点（0，0）至点（100，100）系第线段；USV 设为（40，40）；航当前位置 X 迹恢复init为（65，65）；OS点 X 上障碍物 B1考虑安全区半goal 10，OS径设为 周边障碍物 B ，B 半径分别设为2 3 10，15；为了测试算法对各种避碰环境的通用性， C++程序随B1 ，B ，B 圆心坐标O1 ，O ，O 均由2 3 2 3 OS机数函数生成，作为算法输入，且限定O1 在 上并位于 X 与 X 之间； O 和 O 距离 OS不超init goal 2 3 25。过 8 Bi-RRT Bi-RRT图 所示为 算法与改进 算法对随机障碍物避碰路径点生成情况对比。由图可见，USV Bi-RRT按照改进 算法规划出的避碰路径点航行至航迹恢复点 X 的路径转向点明显减goal
少，路径更加平滑。这是由于父节点延伸方向位于锥形碰撞区以外时触发了目标点吸引向量A，使两棵搜索树延伸更具有目的性，减少了因算法的随机性所带来的路径震荡问题。由于两种算法均为随机算法，为了降低结果的随机性，提高论证的可信度，以该避碰环境作为输入，多次运行此两种算法，以比较路径点数目、父节点延伸失败次数与算法完成避碰路径点规划1 9所消耗的时间，结果如表 所示。图 所示为两种算法的搜索树延伸失败节点对比。由于两种算法探索步长设置相同，因此路径1 8可点数目可以直接反映路径长度。由表 与图见，USV Bi-RRT按照改进 算法规划出的避碰路径
点航行至航迹恢复点 X 的路径长度明显短于goal Bi-RRT同等条件下 算法的结果。当航速一定时， Bi-RRT使用改进 算法规划出的路径不仅能使
USV USV能在更短时间内完成避碰动作，而且 所USV消耗的燃料也更少，对于提升 执行任务时的续航力有着重要意义。1 9 Bi-RRT如表 和图 所示，改进 算法两搜索Bi-RRT树T 与 T 父节点延伸失败次数明显少于a b Bi-RRT算法，改进 算法搜索树延伸失败点数量明Bi-RRT显少于改进前算法，这表明改进 算法迭代过程中子延伸节点有更大概率通过“碰撞检测”，这是由于障碍物排斥向量R与碰撞危险度系数ω动态调节父节点延伸方向，使延伸得到的新子节点能有更大概率得以保留，一定程度上加速了算1法的收敛。由表 对比，还可以发现，本文提出的Bi-RRT改进 算法在完成相同避碰路径规划任务时的耗时比改进前的算法有显著降低，这是由于两棵搜索树并行延伸扩展的方式以及当父节点延伸方向位于锥形碰撞区外时触发了目标吸引向量
A，提高了算法的实时性。
5结语
Bi-RRT本文将 算法与速度障碍法结合，针对父节点延伸方向位于锥形碰撞区内的情况，提出了障碍物排斥向量 R 与避碰危险度系数 ω 来动态调节父节点延伸方向，从而使搜索树向着远离障碍物的趋势延伸，并使延伸得到的新子节点能有更大的概率得以保留，从而加速算法的收敛。此外，提出了两棵搜索树并行延伸的方法，当父节点延伸方向位于锥形碰撞区外时触发目标吸引向量A，使两棵搜索树延伸更具有目的性，减少了由于算法的随机性带来的路径震荡问题。该改进算USV法未来可应用于对实时性要求更高的 动态避USV障问题，以及持续大风浪流扰动情况下 回归原预定航线的航迹滚动规划问题。
参考文献：
［ 1］ 吴博，文元桥，吴贝，等.水面无人艇避碰方法回顾与展望［ J］. 武汉理工大学学报（交通科学与工程版），2016，40（3）：456-461.
WuB ， Wen Y Q ， WuB ，et al. Review and expecta⁃ tion on collision avoidance method of unmanned sur⁃ face vessel［J］. Journal of Wuhan University of Tech⁃ nology （Transportation Science & Engineering ）， 2016，40（3）：456-461（in Chinese）. 2］ 王程博，张新宇，张加伟，等. ［ 未知环境中无人驾驶船舶智能避碰决策方法［J］.中国舰船研究，2018，13 （6）：72-77. Wang C B，Zhang X Y，Zhang J W，et al. Method for intelligent obstacle avoidance decision-making of un⁃ manned vessel in unknown waters［J］. Chinese Journal of Ship Research，2018，13（6）：72-77（in Chinese）. ［3］ Svec P，Schwartz M，Thakur A，et al. Trajectory plan⁃ ning with look-ahead for unmanned sea surface vehi⁃ cles to handle environmental disturbances［C］//2011 IEEE/RSJ International Conference on Intelligent Ro⁃ bots and Systems. San Francisco，CA：IEEE，2011： 1154-1159. ［4］ Kuwata Y，Wolf M T，Zarzhitsky D，et al. Safe mari⁃ time navigation with colregs using velocity obstacles ［C］//2011 IEEE/RSJ International Conference on Intel⁃ ligent Robots and Systems. San Francisco，CA：IEEE， 2011：4728-4734. ［5］ Benjamin M R，Curcio J A，Leonard J J，et al. Naviga⁃ tion of unmanned marine vehicles in accordance with the rules of the road［C］//Proceedings 2006 IEEE Inter⁃ national Conference on Robotics and Automation. Or⁃ lando，FL：IEEE，2006：3581-3587. 6］ 庄佳园，张国成，苏玉民，等. ［ 水面无人艇危险规避
方法［J］.东南大学学报（自然科学版），2013，43（增1）：126-130.
刊Zhuang J Y，Zhang G C， Su Y M ，et al. Obstacle avoidance method for USV［J］. Journal of Southeast University（Natural Science Edition），2013，43（Supp 1）：126-130（in Chinese）. 7］ 吴博，熊勇，闻元桥. ［ 基于速度障碍原理的无人艇自
动避碰算法［J］. 大连海事大学学报，2014，40（2）： 13-16. WuB ，Xiong Y，Wen Y Q. Automatic collision avoid⁃ ance algorithm for unmanned surface vessel based on velocity obstacles［J］. Journal of Dalian Maritime Uni⁃ versity，2014，40（2）：13-16（in Chinese）. 8］ 张洋洋，瞿栋，柯俊，等. ［ 基于速度障碍法和动态窗口法的无人水面艇动态避障［J］.
上海大学学报（自然科学版），2017，23（1）：1-16. Zhang Y Y， Qu D Ke J ，et al. Dynamic obstacle ， avoidance for USV based on velocity obstacle and dy⁃ namic window method［J］. Journal of Shanghai Univer⁃ sity（Natural Science Edition），2017，23（1）：1-16 （in Chinese）. ［9］ La Valle S M，Kuffner J J. Randomized kinodynamic planning［J］. The International Journal of Robotics Re⁃ search，2001，20（5）：378-400. ［10］ Fiorini P，Shiller Z. Motion planning in dynamic en⁃ vironments using velocity obstacles［J］. International Journal of Robotics Research， 1998， 17 （7 ）： 760-772. ［11］ La Valle S M，Kuffner J J. Rapidly-exploring random trees：Progress and prospects［J］. Algorithmic & Com⁃ putational Robotics New Directions，2001：293-308. ［12］ Pohl I. Bidirectional and heuristic search in path prob⁃ lems［D］. California：Stanford University，1969. ［13］ La Valle S M. Rapidly-exploring random trees：a new tool for path planning［D］. Iowa：Iowa State Universi⁃ ty，1998. 14］ 庄佳园，张磊，孙寒冰，等. ［ 应用改进随机树算法
J］.的无人艇局部路径规划［ 哈尔滨工业大学学报，2015，47（1）：112-117. Zhuang J Y，Zhang L， Sun H B ，et al. Improved rap⁃ idly-exploring random tree algorithm application in unmanned surface vehicle local path planning［J］. Journal of Harbin Institute of Technology，2015，47 （1）：112-117（in Chinese）.