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

1．中國人民解放軍91550部隊，遼寧大連 116023

2．哈爾濱工程大學(xué)水下機器人技術(shù)重點實驗室，黑龍江哈爾濱 150001

3．哈爾濱工程大學(xué)計算機科學(xué)與技術(shù)學(xué)院，黑龍江哈爾濱 150001

高速水面無人艇動態(tài)障礙物危險規(guī)避算法

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

1．中國人民解放軍91550部隊，遼寧大連 116023

2．哈爾濱工程大學(xué)水下機器人技術(shù)重點實驗室，黑龍江哈爾濱 150001

3．哈爾濱工程大學(xué)計算機科學(xué)與技術(shù)學(xué)院，黑龍江哈爾濱 150001

水面無人艇（USV）是一種重要的海洋自主機器人，在障礙物環(huán)境中自主航行問題是當前USV的重要研究內(nèi)容。針對高速USV在動態(tài)環(huán)境中的危險規(guī)避問題，提出一種基于行為的動態(tài)危險規(guī)避算法。算法首先對USV的運動特性進行分析獲得基本運動空間，采用碰撞錐理論對USV與障礙物之間的情況進行判定，將海事規(guī)則約束和碰撞約束轉(zhuǎn)換為USV基于行為的約束，通過求解基于偏航角度和速度的優(yōu)化問題獲得USV最優(yōu)規(guī)避行為。仿真實驗結(jié)果證明所提出的算法能夠有效引導(dǎo)USV在高速（30 kn）情況下對動態(tài)障礙物實現(xiàn)有效危險規(guī)避。

水面無人艇；動態(tài)障礙物危險規(guī)避算法；碰撞錐理論；海事規(guī)則

水面無人艇（unmanned surface vehicle，USV）是一無人自主海洋機器人系統(tǒng)，具有體積小、隱蔽性好、機動性強以及造價低的特點。當前USV被廣泛應(yīng)用于水文調(diào)查、航道探測、環(huán)境監(jiān)測、多AUV通信和協(xié)調(diào)以及一般機器人學(xué)等等相關(guān)研究［1-5］。USV還被成功應(yīng)用到軍事行動中，完成搜索、救援以及戰(zhàn)場監(jiān)控等任務(wù)［6］。

以有限的速度對動態(tài)障礙物的規(guī)避問題為一個NP難問題，將時間作為第三維向量加入到狀態(tài)空間中，通過對該狀態(tài)空間的搜索可以獲得一個近似最優(yōu)解，卻很難保證實時性［7-8］。針對USV在海洋環(huán)境中動態(tài)障礙物的危險規(guī)避問題，Benjamin提出一種適合USV在海洋環(huán)境中安全航向的海事規(guī)則［9］。在文獻［10］中分別采用了基于路徑搜索的方法對USV航行過程中的動態(tài)障礙物進行危險規(guī)避，并將海事規(guī)則以偏好性的方式引導(dǎo)規(guī)避路徑的選擇。在文獻［11］中以velocity obstacle（VO）［11］方法為基礎(chǔ)的反應(yīng)危險規(guī)避方法處理USV的動態(tài)障礙物的規(guī)避問題。針對多個動態(tài)障礙物的危險規(guī)避問題，Yo-shiaki Kuwata等［13］提出了一種基于VO的危險規(guī)避算法。雖然有的算法在實艇中進行了簡單驗證，然而目前所設(shè)計的算法主要針對低速（＜12 kn）航行的USV，因此高速USV的動態(tài)障礙物危險規(guī)避問題則需要進一步展開研究。

本文針對USV在高速情況下對動態(tài)障礙物的危險規(guī)避問題，提出了一種基于行為的動態(tài)障礙物危險規(guī)避算法（an algorithm of dynamic obstacle a-voidance based on behavior，ADOAB）。在仿真實驗環(huán)境中通過設(shè)置不同沖突情況的障礙物環(huán)境，證明了算法ADOAB能夠引導(dǎo)USV在動態(tài)障礙物環(huán)境中實現(xiàn)高速（30 kn）安全自主航行。

1 USV基本運動特性分析

當USV處于高速航行時，需要對USV的運動特性進行分析確定其運動狀態(tài)空間，以確保USV在高速航行時所選擇的規(guī)避行為的有效性｀。

2 USV海事規(guī)則圖

為了確保船只的航行安全，在海洋環(huán)境中航行的船只都要遵守海上交通規(guī)則—International Regu-lations for Preventing Collisions at Sea（COL-REGS）［9］。當船只在航行的過程中主要會出現(xiàn)超越、相遇和交叉這3種沖突情況，針對這3種沖突情況定義USV與障礙物之間的沖突范圍，并計算出相應(yīng)的行為約束條件。

2．1 USV沖突范圍定義

根據(jù)經(jīng)驗數(shù)據(jù)和實際的船舶航行情況，定義了USV與目標船只之間的超越、相遇和交叉3種沖突情況的范圍，如圖1所示。

圖1 USV沖突范圍定義

2．2 USV海上交通規(guī)則

USV為非人為操縱船只，在海洋環(huán)境中航行時，能主動對其他目標船只進行規(guī)避，而不享有被人為操縱船只避讓的權(quán)利。根據(jù)USV的特性和海洋環(huán)境船舶航行交通規(guī)則，設(shè)計出滿足USV在海洋環(huán)境中航行的海事規(guī)則，如圖2所示。

圖2 USV在不同沖突情況下的海事規(guī)則

1）超越規(guī)則：當USV與動態(tài)障礙物之間的沖突情況為超越時，USV沿左舷規(guī)避目標障礙物，如圖2（a）所示。

2）相遇規(guī)則：當USV與動態(tài)障礙物之間的沖突情況為相遇時，USV沿右舷規(guī)避目標障礙物，如圖2（b）所示。

3）右交叉規(guī)則：當USV與動態(tài)障礙物之間的沖突情況為右交叉時，USV沿著目標障礙物的尾部進行規(guī)避，如圖2（c）所示。

4）左交叉規(guī)則：當USV與動態(tài)障礙物之間的沖突情況為左交叉時，USV沿著目標障礙物的尾部進行規(guī)避，如圖2（d）所示。

2．3 不同沖突情況對應(yīng)海事規(guī)則約束分析

為了簡化USV在高速航行過程中對動態(tài)目標船只的規(guī)避問題，將USV在超越、相遇、右交叉和左交叉這幾種沖突情況下需要滿足的海事規(guī)則轉(zhuǎn)換為基于行為的約束條件：

3 動態(tài)障礙物約束分析

3．1 碰撞判定定理

引理1（碰撞判定定理）［13］當一個質(zhì)點O與一個半徑為R的圓F以恒定的速度VO和VF運動（如圖3所示），假定它們在有限時間內(nèi)其運動狀態(tài)保持不變，則有：

1）當Vθ＝0且Vr＜0時，質(zhì)點O與圓F將會直線相碰撞；

2）當Vθ≠0且Vr＜ 0且r2V2θ≤R2（V2r＋V2θ）時，質(zhì)點O和圓F將會發(fā)生碰撞。

Vθ為質(zhì)點O和圓F沿著垂直于直線OP方向的相對速度，Vr為質(zhì)點O和圓F沿著直線OP方向的相對速度。

式中：α為速度VO與水平軸之間的夾角，β為速度VF與水平軸之間的夾角，θ為直線OP與水平軸之間的夾角。

圖3 碰撞幾何關(guān)系

3．2 USV碰撞約束分析

當USV在動態(tài)障礙物環(huán)境中自主航行時，根據(jù)引理1對USV當前狀態(tài)和動態(tài)目標障礙物之間碰撞情況進行分析，即USV視為圖3中對應(yīng)的質(zhì)點O，而障礙物則為圖3中對應(yīng)的圓F，USV與障礙物之間的沖突約束為

式中：α和v為USV的候選航向和線速度，ViF為USV對應(yīng)第i個障礙物的速度，β為速度VF與水平軸之間的夾角，θ為直線OP與水平軸之間的夾角，如圖3所示。

USV在進行碰撞規(guī)避候選角度約束則為VAvoid

式中N為目標障礙物的個數(shù)。

4 動態(tài)障礙物危險規(guī)避算法

4．1 規(guī)避行為向量優(yōu)化模型

以USV航向偏航程度和USV的線速度為優(yōu)化目標，采用加權(quán)平均的方法將其轉(zhuǎn)換為單目標優(yōu)化問題，通過求解此單目標約束優(yōu)化問題獲得一組最優(yōu)航向角度（θA，Best，vA，Best）。 優(yōu)化目標：

式中：Jd（θ，v）為航向偏航程度和線速度的加權(quán)優(yōu)化目標函數(shù)；θk為USV當前位置與全局目標關(guān)鍵點在絕對坐標系下所構(gòu)成的角度；VHeading為USV在大地坐標系下的艏向角窗口；AAvoid為碰撞約束對應(yīng)的可行航向角度集合；ΘRule為USV沖突對應(yīng)海事規(guī)則的約束可行角度集合；ξ1和ξ2為加權(quán)系數(shù)。

4．2 算法流程

算法ADOAB的流程如下所示：

1）計算USV的線速度窗口VLineVelociy和艏向窗口 VHeading；

2）對USV周圍的障礙物進行碰撞檢測，若碰撞，則轉(zhuǎn)步驟3），若不碰撞，則轉(zhuǎn)步驟7）；

3）計算USV規(guī)避范圍內(nèi)的障礙物的規(guī)避可行向量VAvoid；

4）根據(jù)USV之間的最短碰撞時間，確定與USV發(fā)生沖突的障礙物Oiobs；

5）根據(jù)障礙物Oiobs與USV之間形成的沖突情況確定相應(yīng)的海事規(guī)則約束ΘRule；

6）求解式（14）～（16）約束優(yōu)化函數(shù)，獲得USV的最優(yōu)規(guī)避行為向量（θA，Best，vA，Best）；

7）根據(jù)全局關(guān)鍵點輸出 USV的規(guī)避向量（θkey，vkey） ；

8）將USV的規(guī)避行為向量輸入到USV基礎(chǔ)運動控制模塊中，引導(dǎo)USV進行自主航行；

9）判斷USV是否到底目標點，是，則停止；否則轉(zhuǎn)步驟1）。

5 實驗分析

仿真實驗中硬件平臺包括上位 PC機、目標PC／104嵌入式計算機和圖形工作站3臺計算機．目標PC／104嵌入式計算機中使用的是VxWorks嵌入式操作系統(tǒng)，USV自適應(yīng)危險規(guī)避程序在上位PC機進行開發(fā)并編譯生成VxWorks鏡像，通過FTP下載到目標機PC／104上運行．圖形工作站上運行Ve-ga視景仿真程序，對實驗場景、USV內(nèi)部和外部傳感器數(shù)據(jù)以及USV的運動狀態(tài)數(shù)據(jù)進行仿真，仿真系統(tǒng)體系結(jié)構(gòu)如圖4所示。

圖4 仿真系統(tǒng)體系結(jié)構(gòu)

為了驗證算法ADOAB能否引導(dǎo)高速USV對動態(tài)障礙物進行危險規(guī)避，以及能夠規(guī)避的過程是否滿足海事規(guī)則，以單個運動目標船只在仿真環(huán)境中進行實驗，在實驗過程中USV的航行速度為30 kn。USV在超越、相遇、右交叉和左交叉沖突情況下對動態(tài)目標船只的規(guī)避情況如圖5～8所示。USV能夠安全對動態(tài)目標船只進行規(guī)避，同時還能驗證遵守海事規(guī)則，即說明本文所提出的算法ADOAB是可以有效引導(dǎo)高速航行的USV對動態(tài)目標障礙物進行有效危險規(guī)避。

圖5 超越時的規(guī)避情況

圖6 相遇時的規(guī)避情況

圖7 右交叉時的規(guī)避情況

圖8 左交叉時的規(guī)避情況

［13］，設(shè)計出處于超越?jīng)_突狀態(tài)和左右交叉沖突情況的多目標沖突實驗場景。USV的航行速度仍然保持30 kn，USV在算法的引導(dǎo)下對多個動態(tài)目標的規(guī)避情況如圖9～12所示。USV能夠有效對多個障礙物進行安全規(guī)避，同時還能保證在規(guī)避的過程中遵照海事規(guī)則。圖9對應(yīng)的實驗中，USV和運動目標的速度分布情況如圖10所示，USV和運動目標之間的距離分布如圖11所示。圖10對應(yīng)的實驗中，USV和運動目標的速度分布情況如圖13所示，USV和運動目標之間的距離分布如圖14所示。

圖9 多個障礙物處于超越?jīng)_突時的規(guī)避

圖10 規(guī)避多個障礙物USV和運動目標的速度分布

圖11 規(guī)避多個障礙物USV到運動目標的距離分布

圖12 多個障礙物處于左右交叉時的規(guī)避

圖13 規(guī)避左右交叉沖突障礙物速度分布

圖14 規(guī)避左右交叉沖突障礙物USV到運動目標的距離分布

6 結(jié)束語

針對高速航行的水面無人艇在海洋環(huán)境中對動態(tài)障礙物的危險規(guī)避問題，本文提出一種基于行為的動態(tài)障礙物危險規(guī)避算法。通過對USV運動狀態(tài)碰撞的相關(guān)約束狀態(tài)分析，構(gòu)建了危險規(guī)避行為的約束優(yōu)化模型；通過求解該約束優(yōu)化模型，獲得了USV的規(guī)避角度和規(guī)避線速度。仿真試驗結(jié)果表明，所提ADOAB算法能夠有效引導(dǎo)高速航行USV進行動態(tài)障礙物的危險規(guī)避，以保障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，F(xiàn)rance，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，F(xiàn)UJIMURA 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］唐平鵬，張汝波，史長亭，等．水面無人艇分層策略近程危險規(guī)避［J］．應(yīng)用科學(xué)學(xué)報，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．

國家自然科學(xué)基金資助項目（61100005）；中國博士后科學(xué)基金資助項目（2013M540271）；黑龍江省博士后基金資助項目（LBH-Z13055）；中央高?；究蒲袠I(yè)務(wù)費專項基金資助項目（HEUCFD1403）

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

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