高速水面无人艇动态障碍物危险规避算法

吉大海，杨溢，戴捷，杨歌，3，廖煜雷

1．中国人民解放军91550部队，辽宁大连 116023

2．哈尔滨工程大学水下机器人技术重点实验室，黑龙江哈尔滨 150001

3．哈尔滨工程大学计算机科学与技术学院，黑龙江哈尔滨 150001

高速水面无人艇动态障碍物危险规避算法

吉大海1，杨溢2，戴捷2，杨歌2，3，廖煜雷2

1．中国人民解放军91550部队，辽宁大连 116023

2．哈尔滨工程大学水下机器人技术重点实验室，黑龙江哈尔滨 150001

3．哈尔滨工程大学计算机科学与技术学院，黑龙江哈尔滨 150001

水面无人艇（USV）是一种重要的海洋自主机器人，在障碍物环境中自主航行问题是当前USV的重要研究内容。针对高速USV在动态环境中的危险规避问题，提出一种基于行为的动态危险规避算法。算法首先对USV的运动特性进行分析获得基本运动空间，采用碰撞锥理论对USV与障碍物之间的情况进行判定，将海事规则约束和碰撞约束转换为USV基于行为的约束，通过求解基于偏航角度和速度的优化问题获得USV最优规避行为。仿真实验结果证明所提出的算法能够有效引导USV在高速（30 kn）情况下对动态障碍物实现有效危险规避。

水面无人艇；动态障碍物危险规避算法；碰撞锥理论；海事规则

水面无人艇（unmanned surface vehicle，USV）是一无人自主海洋机器人系统，具有体积小、隐蔽性好、机动性强以及造价低的特点。当前USV被广泛应用于水文调查、航道探测、环境监测、多AUV通信和协调以及一般机器人学等等相关研究［1-5］。USV还被成功应用到军事行动中，完成搜索、救援以及战场监控等任务［6］。

以有限的速度对动态障碍物的规避问题为一个NP难问题，将时间作为第三维向量加入到状态空间中，通过对该状态空间的搜索可以获得一个近似最优解，却很难保证实时性［7-8］。针对USV在海洋环境中动态障碍物的危险规避问题，Benjamin提出一种适合USV在海洋环境中安全航向的海事规则［9］。在文献［10］中分别采用了基于路径搜索的方法对USV航行过程中的动态障碍物进行危险规避，并将海事规则以偏好性的方式引导规避路径的选择。在文献［11］中以velocity obstacle（VO）［11］方法为基础的反应危险规避方法处理USV的动态障碍物的规避问题。针对多个动态障碍物的危险规避问题，Yo-shiaki Kuwata等［13］提出了一种基于VO的危险规避算法。虽然有的算法在实艇中进行了简单验证，然而目前所设计的算法主要针对低速（＜12 kn）航行的USV，因此高速USV的动态障碍物危险规避问题则需要进一步展开研究。

本文针对USV在高速情况下对动态障碍物的危险规避问题，提出了一种基于行为的动态障碍物危险规避算法（an algorithm of dynamic obstacle a-voidance based on behavior，ADOAB）。在仿真实验环境中通过设置不同冲突情况的障碍物环境，证明了算法ADOAB能够引导USV在动态障碍物环境中实现高速（30 kn）安全自主航行。

1 USV基本运动特性分析

当USV处于高速航行时，需要对USV的运动特性进行分析确定其运动状态空间，以确保USV在高速航行时所选择的规避行为的有效性｀。

2 USV海事规则图

为了确保船只的航行安全，在海洋环境中航行的船只都要遵守海上交通规则—International Regu-lations for Preventing Collisions at Sea（COL-REGS）［9］。当船只在航行的过程中主要会出现超越、相遇和交叉这3种冲突情况，针对这3种冲突情况定义USV与障碍物之间的冲突范围，并计算出相应的行为约束条件。

2．1 USV冲突范围定义

根据经验数据和实际的船舶航行情况，定义了USV与目标船只之间的超越、相遇和交叉3种冲突情况的范围，如图1所示。

图1 USV冲突范围定义

2．2 USV海上交通规则

USV为非人为操纵船只，在海洋环境中航行时，能主动对其他目标船只进行规避，而不享有被人为操纵船只避让的权利。根据USV的特性和海洋环境船舶航行交通规则，设计出满足USV在海洋环境中航行的海事规则，如图2所示。

图2 USV在不同冲突情况下的海事规则

1）超越规则：当USV与动态障碍物之间的冲突情况为超越时，USV沿左舷规避目标障碍物，如图2（a）所示。

2）相遇规则：当USV与动态障碍物之间的冲突情况为相遇时，USV沿右舷规避目标障碍物，如图2（b）所示。

3）右交叉规则：当USV与动态障碍物之间的冲突情况为右交叉时，USV沿着目标障碍物的尾部进行规避，如图2（c）所示。

4）左交叉规则：当USV与动态障碍物之间的冲突情况为左交叉时，USV沿着目标障碍物的尾部进行规避，如图2（d）所示。

2．3 不同冲突情况对应海事规则约束分析

为了简化USV在高速航行过程中对动态目标船只的规避问题，将USV在超越、相遇、右交叉和左交叉这几种冲突情况下需要满足的海事规则转换为基于行为的约束条件：

3 动态障碍物约束分析

3．1 碰撞判定定理

引理1（碰撞判定定理）［13］当一个质点O与一个半径为R的圆F以恒定的速度VO和VF运动（如图3所示），假定它们在有限时间内其运动状态保持不变，则有：

1）当Vθ＝0且Vr＜0时，质点O与圆F将会直线相碰撞；

2）当Vθ≠0且Vr＜ 0且r2V2θ≤R2（V2r＋V2θ）时，质点O和圆F将会发生碰撞。

Vθ为质点O和圆F沿着垂直于直线OP方向的相对速度，Vr为质点O和圆F沿着直线OP方向的相对速度。

式中：α为速度VO与水平轴之间的夹角，β为速度VF与水平轴之间的夹角，θ为直线OP与水平轴之间的夹角。

图3 碰撞几何关系

3．2 USV碰撞约束分析

当USV在动态障碍物环境中自主航行时，根据引理1对USV当前状态和动态目标障碍物之间碰撞情况进行分析，即USV视为图3中对应的质点O，而障碍物则为图3中对应的圆F，USV与障碍物之间的冲突约束为

式中：α和v为USV的候选航向和线速度，ViF为USV对应第i个障碍物的速度，β为速度VF与水平轴之间的夹角，θ为直线OP与水平轴之间的夹角，如图3所示。

USV在进行碰撞规避候选角度约束则为VAvoid

式中N为目标障碍物的个数。

4 动态障碍物危险规避算法

4．1 规避行为向量优化模型

以USV航向偏航程度和USV的线速度为优化目标，采用加权平均的方法将其转换为单目标优化问题，通过求解此单目标约束优化问题获得一组最优航向角度（θA，Best，vA，Best）。 优化目标：

式中：Jd（θ，v）为航向偏航程度和线速度的加权优化目标函数；θk为USV当前位置与全局目标关键点在绝对坐标系下所构成的角度；VHeading为USV在大地坐标系下的艏向角窗口；AAvoid为碰撞约束对应的可行航向角度集合；ΘRule为USV冲突对应海事规则的约束可行角度集合；ξ1和ξ2为加权系数。

4．2 算法流程

算法ADOAB的流程如下所示：

1）计算USV的线速度窗口VLineVelociy和艏向窗口 VHeading；

2）对USV周围的障碍物进行碰撞检测，若碰撞，则转步骤3），若不碰撞，则转步骤7）；

3）计算USV规避范围内的障碍物的规避可行向量VAvoid；

4）根据USV之间的最短碰撞时间，确定与USV发生冲突的障碍物Oiobs；

5）根据障碍物Oiobs与USV之间形成的冲突情况确定相应的海事规则约束ΘRule；

6）求解式（14）～（16）约束优化函数，获得USV的最优规避行为向量（θA，Best，vA，Best）；

7）根据全局关键点输出 USV的规避向量（θkey，vkey） ；

8）将USV的规避行为向量输入到USV基础运动控制模块中，引导USV进行自主航行；

9）判断USV是否到底目标点，是，则停止；否则转步骤1）。

5 实验分析

仿真实验中硬件平台包括上位 PC机、目标PC／104嵌入式计算机和图形工作站3台计算机．目标PC／104嵌入式计算机中使用的是VxWorks嵌入式操作系统，USV自适应危险规避程序在上位PC机进行开发并编译生成VxWorks镜像，通过FTP下载到目标机PC／104上运行．图形工作站上运行Ve-ga视景仿真程序，对实验场景、USV内部和外部传感器数据以及USV的运动状态数据进行仿真，仿真系统体系结构如图4所示。

图4 仿真系统体系结构

为了验证算法ADOAB能否引导高速USV对动态障碍物进行危险规避，以及能够规避的过程是否满足海事规则，以单个运动目标船只在仿真环境中进行实验，在实验过程中USV的航行速度为30 kn。USV在超越、相遇、右交叉和左交叉冲突情况下对动态目标船只的规避情况如图5～8所示。USV能够安全对动态目标船只进行规避，同时还能验证遵守海事规则，即说明本文所提出的算法ADOAB是可以有效引导高速航行的USV对动态目标障碍物进行有效危险规避。

图5 超越时的规避情况

图6 相遇时的规避情况

图7 右交叉时的规避情况

图8 左交叉时的规避情况

［13］，设计出处于超越冲突状态和左右交叉冲突情况的多目标冲突实验场景。USV的航行速度仍然保持30 kn，USV在算法的引导下对多个动态目标的规避情况如图9～12所示。USV能够有效对多个障碍物进行安全规避，同时还能保证在规避的过程中遵照海事规则。图9对应的实验中，USV和运动目标的速度分布情况如图10所示，USV和运动目标之间的距离分布如图11所示。图10对应的实验中，USV和运动目标的速度分布情况如图13所示，USV和运动目标之间的距离分布如图14所示。

图9 多个障碍物处于超越冲突时的规避

图10 规避多个障碍物USV和运动目标的速度分布

图11 规避多个障碍物USV到运动目标的距离分布

图12 多个障碍物处于左右交叉时的规避

图13 规避左右交叉冲突障碍物速度分布

图14 规避左右交叉冲突障碍物USV到运动目标的距离分布

6 结束语

针对高速航行的水面无人艇在海洋环境中对动态障碍物的危险规避问题，本文提出一种基于行为的动态障碍物危险规避算法。通过对USV运动状态碰撞的相关约束状态分析，构建了危险规避行为的约束优化模型；通过求解该约束优化模型，获得了USV的规避角度和规避线速度。仿真试验结果表明，所提ADOAB算法能够有效引导高速航行USV进行动态障碍物的危险规避，以保障USV安全航行。

参考文献：

［1］STEIMLE E T，HALL M L．Unmanned surface vehicles as environmental monitoring and assessment tools［C］／／Pro-ceedings of OCEANS 2006．Boston：USA，2006：1-5．

［2］BIBULI M，BRUZZONE G，CACCIA M，et al．Line follow-ing guidance control：application to the Charlie unmanned surface vehicle［C］／／International Conference on Intelligent Robots and Systems．Nice，France，2008：3641-3646．

［3］NAEEM W，SUTTON R．Modeling and control of an un-manned surface vehicle for environmental monitoring［C］／／UKACC International Control Conference．Glasgow，UK，2006：1-6．

［4］ALVES J，OLIVEIRA P，OLIVEIRA R，et al．Vehicle and mission control of the DELFIM autonomous surface craft［C］／／Proceedings of the 14th Mediterranean Conference on Control and Automation．Ancona，Italy，2006：1-6．

［5］WANG J，CHEN C，HUANG P，et al．Modeling，simula-ting and experiment of an autonomous surface vehicle［J］．Energy Procedia，2011，11（1）：314-318．

［6］MOTWANI A．Survey of unmanned surface vehicles，Tech-nical Report MIDAS［R］．Plymouth：Plymouth University，2012．

［7］CANNY J，REIF J．New lower bound techniques for robot motion planning problem［C］／／Proceedings of 28th Annual IEEE Sumps on Foundation of Computer Science．Los An-geles：USA，1987：49-60．

［8］AGGARWAL N，FUJIMURA K．Motion planning amidst planar moving obstacles［C］／／Proceedings of IEEE Inter-national Conference on Robotics and Automation．San Die-go：USA，1994：2153-2158．

［9］BENJAMIN M R，CURCIO J A，NEWMAN P M．Naviga-tion of unmanned marine vehicles in accordance with the rules of the road［C］／／Proceedings of the 2006 IEEE Inter-national Conference on Robotics and Automation．Orlando：USA，2006：49-60．

［10］NAEEM W，IRWIN G W．COLREGS-based collision a-voidance strategies for unmanned surface vehicles［J］．Mechatronics，2012，22（6）：669-678．

［11］BANDYOPHADYAY T，SARCIONE L．A simple reactive obstacle avoidance algorithm and its application in Singa-pore harbor［J］．Field and Service Robots，2010，62（2）：455-465．

［12］FIORINI P，SHILLER Z．Motion planning in dynamic en-vironments using velocity obstacles［J］．International Jour-nal of Robotics Research，1998，17（7）：760-772．

［13］YOSHIAKI K，MICHAEL T W，DIMITRI Z，et al．Safe maritime navigation with COLREGS using velocity obsta-cles［C］／／2011 IEEE／RSJ International Conference on Intelligent Robots and Systems．San Francisco： USA，2011：4728-4734．

［14］唐平鹏，张汝波，史长亭，等．水面无人艇分层策略近程危险规避［J］．应用科学学报，2013，31（4）：418-426．

Dynamic obstacle avoidance algorithm for high-speed unmanned surface vehicles

JI Dahai1，YANG Yi2，DAI Jie2，YANG Ge2，3，LIAO Yulei2
1．People’s Liberation Army Forces 91550，Dalian116023，China
2．National Key Laboratory of Science and Technology on Autonomous Underwater Vehicle，Harbin Engineering University，Harbin 150001，China
3．College of Computer Science and Technology，Harbin Engineering University，Harbin 150001，China

An unmanned surface vehicle（USV）is a kind of important marine robot，and the problem of the autono-mous navigation in the obstacle environments is the significant research issues of USV．In this paper，an algorithm of dynamic obstacle avoidance based on behavior is proposed for the navigation of high-speed USV in the dynamic obstacle environments．Firstly，the basic motion space is calculated by analyzing the motion characteristics of USV，and according to the theory of collision cone the collision states of USV can be determined．The constraints of Inter-national Regulations for Preventing Collisions at Sea（COLREGS）and collision of USV are transferred into the be-havior based constraints，and then the optimal avoidance behavior can be obtained by solving the yaw-angle and ve-locity based constraints optimization problem．In the end，the validity of the algorithm is demonstrated by high-speed（30 kn）USV in the dynamic obstacles environments．

unmanned surface vehicle；algorithm of dynamic obstacle avoidance；collision cone；COLREG

TP24

A

1009-671X（2014）03-0040-006

10．3969／j．issn．1009-671X．201401008

http：／／www．cnki．net／kcms／doi／10．3969／j．issn．1009-671X．201401008．html

2014-01-14．

日期：2014-06-05．

国家自然科学基金资助项目（61100005）；中国博士后科学基金资助项目（2013M540271）；黑龙江省博士后基金资助项目（LBH-Z13055）；中央高校基本科研业务费专项基金资助项目（HEUCFD1403）

吉大海（1969-），男，工程师．

廖煜雷，E-mail：liaoyulei＠hrbeu．edu．cn．