杜開君，茅云生，向祖權，周永清，宋利飛，劉 彬

(武漢理工大學a.高性能船舶技術教育部重點實驗室;b.交通學院，武漢430063)

基于海事規(guī)則的水面無人艇動態(tài)障礙規(guī)避方法

杜開君a，b，茅云生a，b，向祖權a，b，周永清a，b，宋利飛b，劉 彬b

(武漢理工大學a.高性能船舶技術教育部重點實驗室;b.交通學院，武漢430063)

針對水面無人艇對探測到的電子海圖上沒有標示的動態(tài)障礙物的規(guī)避問題,提出符合國際海上避碰規(guī)則公約的規(guī)避方法,將動態(tài)障礙物某一運動時刻轉換為相對無人艇瞬時靜止的狀態(tài),對避障模型進行實時計算;并通過粒子群優(yōu)化算法解算出無人艇進行避障所需的最優(yōu)航速和航向改變量。仿真實驗表明,該方法可得到合理的規(guī)避策略,有效地完成動態(tài)障礙物的規(guī)避任務。

水面無人艇;動態(tài)障礙物;粒子群優(yōu)化算法;路徑規(guī)劃;海事規(guī)則

水面無人艇(unmanned surface vehicle,USV)作為一種無人海上航行器,是惟一一種可以在水上、水面、水下同時發(fā)揮作用的無人運載平臺［1］。在航行過程中,水面無人艇處在不可預見的動態(tài)的海洋環(huán)境中,障礙物很可能出現(xiàn)在已經規(guī)劃好的全局路徑上,而AIS或航海雷達并沒有探測到,電子海圖上也沒有標示障礙物及其運動信息。為了保證無人艇能夠快速、安全地到達目標,需要對使其處在危險狀況下的動態(tài)障礙物進行規(guī)避,這正是決定無人艇智能化程度高低的關鍵技術之一［1-5］。在分析各種無人艇障礙規(guī)避方法的基礎上,提出基于海事規(guī)則的水面無人艇動態(tài)障礙規(guī)避方法。該方法根據國際海上航行規(guī)則公約將碰撞情形劃分為追越、正面相遇、交叉相遇3種局面;引入極坐標系下的粒子群優(yōu)化算法,通過改變無人艇航速和航向,實現(xiàn)規(guī)避動態(tài)障礙物的目的。

1 極坐標系下的建模

常用的兩種平面坐標系為笛卡爾坐標系和極坐標系。笛卡爾坐標系在建模時具有局限性,如只能向前走,容易掉入陷阱;而極坐標空間則對長度和角度敏感,可以將各維分量與路徑處的期望航速和航向關聯(lián),并且便于描述障礙物信息。所以,采取極坐標系進行建模。

極坐標系下的起點和終點分別為在全局規(guī)劃路徑中選取的子目標段的起點和終點。這兩點的連線為極坐標系的極軸,起始點為極點,逆時針方向為正。

真實環(huán)境下,障礙物的形狀各種各樣,通過雷達掃描出來的雷達圖像或通過聲吶、傳感器得到的障礙物形狀會很復雜,直接對這些未經處理的障礙物形狀進行避障,其計算過程往往會代價高昂,導致計算時間增加。為了縮短計算時間,在進行路徑規(guī)劃之前,需要對障礙物進行包圍體處理,使障礙物形狀得到合理的簡化。

國內大部分學者對障礙物包圍體的處理僅采取圓形包圍處理,未充分考慮到海面上運行的船舶均為長寬比較大的障礙,僅采取圓形包圍會損失大量可行區(qū)間,從而造成避障策略不合理。這里對障礙物的包圍體處理包括兩種方式。

1)對長寬比較小(取小于2∶1)的障礙物來說,采用圓形進行障礙包圍,不會損失太多的可行路徑;且極大地簡少了計算量,可有效縮短計算時長。外包圓具體取值方法為:其直徑取障礙物所有頂點連線的最大值,即

式中:P——障礙頂點且(i,j∈N,i≠j),圓心即為最大連線的中點。

2)對長寬比較大(大于2∶1)的障礙物來說,將障礙物進行更貼合實際形狀的模擬(即有向包圍盒,OBB［6］)。雖然增大了計算量,但采用此方法,可得到更多的有效路徑區(qū)間,產生更優(yōu)的避障路徑,使得避障能耗更小、續(xù)航力更久。

無人艇在海面航行過程中,會存在許多不確定性因素,如障礙物運動的不確定性、無人艇本身運動的不確定性、執(zhí)行機構運行時的不確定性、無人艇自身定位和對障礙物進行探測的系統(tǒng)也不可避免地存在誤差等。為了簡化計算,將水面無人艇作為一個質心處理,所以還需要對障礙物進行膨脹建模。

2 動態(tài)障礙物避障模型

無人艇和圓形動態(tài)障礙物當前時刻的幾何模型見圖1。圖中:VUSV為無人艇的運動速度;VObs為障礙物的運動速度;α為極軸到VUSV的角度,可表示為α=∠(VUSV,ex),其中:ex為極軸;同理β=∠(VObs,ex)為極軸到VObs的角度;θ=∠(LRO,ex)為極軸到無人艇與障礙物圓心連線(LRO)的角度;φ=∠(VUSV,ΔV)為ΔV到VUSV的角度;γ=∠(ΔV,LRO),μ=∠(LRO,tan L),tan L為障礙圓切線。

圖1 避障模型示意

為使無人艇能在下一時刻避開障礙區(qū)間,γ應該取在(LRO-μ,LRO+μ)范圍之外的角度。下面將通過求解γ,找到無人艇航速和轉向需要滿足的條件,以實現(xiàn)無人艇避障。

如圖2所示,將ΔV分解為指向障礙物圓心的速度分量ΔV0,和其垂直速度分量ΔVγ。其中ΔV0促使無人艇朝向障礙物運動,ΔVγ促使無人艇逃離障礙物。

圖2 對速度ΔV分解示意圖

利用VUSV、VObs、ΔV構成的速度三角形求解ΔVr和γ。

式中:

式(5)中,假設圓形動態(tài)障礙物運動不發(fā)生突變,其運動速度VObs和運動方向β在很短的時間變化很小,可以忽略不計,d VObs=0,dβ=0,則有

所以式(3)可以轉化為用差分形式表示為

在VUSV、VObs、ΔV構成的速度三角形(如圖2)中,存在式(11)關系,其中φ為VUSV、VObs夾角,將其帶入式(10),可得到式(12)。

要使無人艇逃離危險區(qū)域(LRO-μ,LRO+μ)范圍,Δγ需要滿足下面的不等式。

式(12)說明Δγ同無人艇的航速改變量VUSV和航向改變量Δα存在關聯(lián),于是避障任務就轉化成調整無人艇的航速VUSV和航向Δα。

3 國際海上避碰規(guī)則公約的約束

水面無人艇在海面航行過程中,為了避免對海上其他船舶及無人艇自身的安全造成威脅,在進行避障的過程中必須滿足相關規(guī)則。國際海事組織制定了國際海上避碰規(guī)則公約(International Regulations for Preventing Collisions At Sea,1972/ COLREGS),但現(xiàn)在還沒有專門的法律法規(guī)對無人艇的航行行為做出明確的規(guī)定,較合理的解決方法就是使無人艇遵守國際海上避碰規(guī)則公約［7］。

國際海上避碰規(guī)則公約的13、14、15條分別對無人艇在航行過程中可能出現(xiàn)的碰撞局面:追越、正面相遇和交叉相遇,做出了規(guī)定。

圖3給出了4種情況的避障模型,障礙物A狀態(tài)為無人艇在起始點“Start”處探測到該障礙物的位置,箭頭為其運動方向;障礙物B狀態(tài)則為無人艇對該障礙物避障結束時,障礙物所處的位置。

以下無人艇和運動障礙物之間的航向角度差,均以無人艇航向方向為基準(即航向角為0°)進行描述。海事規(guī)則沖突情景定義見圖4。

1)追越。若無人艇高速航行與障礙物航向角度差在［0°,45°］與 ［315°,360°)之間時,接近危險距離,則無人艇左側航行。

圖3 避障模型示意

圖4 海事規(guī)則沖突情景定義

2)正面相遇。若夾角在［165°,195°］之間,接近危險距離,則無人艇應右側航行,見圖3b)。

3)交叉相遇。若夾角在(45°,165°)之間,接近危險距離,則障礙物在無人艇右側交叉相遇,無人艇應右側航行,見圖3c);若夾角在(195°, 315°)之間,接近危險距離,則障礙物在無人艇左側交叉相遇,此時無人艇應左側航行,見圖3d)。

4 動態(tài)障礙規(guī)避

4.1 粒子群優(yōu)化算法及其適應度函數的確定

粒子群優(yōu)化算法是一種集群優(yōu)化算法。假設有一群隨機粒子以一定的速度飛行,每個粒子當前自己找到的最優(yōu)解為Pi,d,全局最優(yōu)解為Pg,d。其中:d=1,2,…,N。N為粒子群迭代次數［8］。粒子在找到這兩個最優(yōu)解后,按式(14)、(15)來分別更新速度和位置。

式中:r1和r2為0-1之間均勻分布的隨機數。w是慣性權值,文中w的取值選取廣泛采用的線性遞減慣性權重［9］;c1和c2為學習因子的取值,經多次試驗,均取值為2.0。

無人艇在航行過程中,較多使用的是不改變航速,僅改變航向;但在某些情況下還需要同時改變航速和航向的策略,進行避障。航速和航向改變量即為粒子群優(yōu)化算法的兩個維度值,無人艇路徑規(guī)劃問題就可以表達成一個多條件下的目標優(yōu)化問題。

式中:φ、μ、γ、ΔV已知,f(ΔVUSV,Δα)為要實現(xiàn)最優(yōu)的目標,即為粒子群優(yōu)化算法中的適應度函數, m1、m2分別為航速改變量和航向改變量的權值。計算得到最優(yōu)解后還需判斷是否在可行域內,即需要滿足國際海上避碰規(guī)則公約的約束,無人艇最小回轉直徑的約束,以及不與全局障礙物碰撞的約束。若該段路徑中存在多個動態(tài)障礙物,則只需對每個障礙物加入式(16)中第2、3個約束即可。

4.2 避障結束點的確定

在對某一子目標段內的動態(tài)障礙物實現(xiàn)成功規(guī)避后,無人艇需要沿著全局規(guī)劃路徑點繼續(xù)前行;采用上述的算法得到實現(xiàn)避障后的最優(yōu)航速和航向,所以還需確定無人艇成功避障時刻的避障結束點。

若障礙物是朝向無人艇運行,在未到達避障結束點之前,障礙物在全局中的極徑在不斷縮小,見圖5a)。圖中ˉVusv為經過優(yōu)化后的無人艇速度、ΔˉV為優(yōu)化后無人艇和障礙物的合速度。此時避障結束點的選取,可根據在避障模型中,將障礙物視為靜止障礙物,以合速度ΔˉV到達障礙物最遠端(圖中1/4圓弧段處)所耗費的時間t,作為避障結束所耗費的時間;進而轉換到全局坐標系下,以此時間段t計算避障結束點的極徑值和極角。

若障礙物正遠離起始點運行,在未到達避障結束點之前,障礙物在全局中的極徑在不斷增大,見圖5b)。此情況下若還按上述方法進行處理,無疑會造成避障結束點遠離本段子目標點。所以在此情況下,設置避障結束點即為本段起始點時障礙物所處位置的極徑處為避障結束點處的極徑。

綜合以上兩種求避障結束點的情形,在計算時選取兩者極徑較小的那一點作為避障結束點。

以上為針對動態(tài)圓形障礙物的避障結束點選取方法,動態(tài)矩形障礙物的避障結束點選取方法相同。

圖5 避障結束點示意

4.3 算法描述

結合文中對基于海事規(guī)則的水面無人艇動態(tài)障礙規(guī)避方法,給出該算法的流程,見圖6。

圖6 基于海事規(guī)則的水面無人艇動態(tài)障礙規(guī)避方法流程

在粒子群優(yōu)化算法中,可行域判斷是指要同時滿足海事規(guī)則、無人艇最小回轉直徑及不與全局障礙物碰撞的約束。為了避免算法陷入局部最優(yōu),降低解算時間,在可行域的判斷中設置海事規(guī)則判斷最大次數;若判斷次數大于設置的最大次數,則不對海事規(guī)則進行考慮。

5 仿真實驗結果及分析

為了驗證基于海事規(guī)則的水面無人艇動態(tài)障礙規(guī)避方法,采用在Visual C++平臺上開發(fā)的水面無人艇危險避障算法仿真實驗平臺進行仿真實驗分析。設立了如圖7所示的避障環(huán)境,該避障環(huán)境包括了海事規(guī)則中正面相遇、交叉相遇、追越的3種情形,以及多障礙路徑段的避障。

圖7 仿真分析示意

在該仿真實驗中,假定水面無人艇已經得到了全局最優(yōu)路徑節(jié)點Path1、Path2、Path3,“Start”為起始點,“Goal”為終止點,相距3 000 m;無人艇運行速度為40 kn,最大航速為50 kn。粒子群優(yōu)化算法中慣性權重w采用線型遞減策略,wmax= 0.95,wmin=0.2;c1=c2=2;m1=1,m2=70;設置粒子群迭代次數50,粒子數50。

該環(huán)境下包括一個全局靜態(tài)圓形障礙物SOC,和一個全局靜態(tài)矩形障礙物SOR。經過全局路徑規(guī)劃方法解算,得到全局規(guī)劃路徑節(jié)點見表1。包括1個動態(tài)圓形障礙物DOC1(見表2)和3個動態(tài)矩形障礙物DOR1、DOR2、DOR3(見表3)(分別選取上海外高橋制造的85 000 m3液化氣船,18 000 TEU集裝箱船和106 kDWT阿芙拉型原油船)。為了方便驗證,將動態(tài)障礙物的起始位置均定為無人艇起始點的位置。動態(tài)障礙物起始時刻的位置用Ai表示,避障結束點時刻障礙物的位置用Bi表示,箭頭方向為障礙物運動方向。

表1 全局規(guī)劃路徑節(jié)點

表2 圓形動態(tài)障礙物DOC參數

表3 矩形動態(tài)障礙物DOR參數

表4 動態(tài)障礙規(guī)避路徑節(jié)點

無人艇在起始時刻時沿子目標點Path1航行時,探測到已知動態(tài)矩形障礙物DOR1,并與無人艇呈185.17°相向運行,即夾角在165°和195°之間,為海事規(guī)則中的正面相遇情形。若無人艇繼續(xù)向前行進,會同障礙物相遇碰撞,所以需要無人艇做出相應的調整,從障礙物左舷即無人艇右轉向通行。通過該算法解算,耗時0.081 s;得到的避障策略為向右舷轉向11.34°,行進航速不變。避障結束點為P-AD0,其極坐標值見表4。圖7中較粗的實線即為無人艇改變航速、航向進行避障的航線,成功避障后逐漸恢復航速至執(zhí)行任務航速,朝向本段的全局子目標終點航行。

無人艇繼續(xù)航行到Path1位置時,探測到動態(tài)圓形障礙物DOC1,和動態(tài)矩形障礙物DOR2,并分別從無人艇兩側駛來,角度分別為75.17°和254.17°;為海事規(guī)則中的交叉相遇情形,需要分別從兩個障礙物的后方通過,即對DOC1而言為右側通行,對DOR2而言為左側通行。無人艇在Path1時刻進行避障,算法解算耗時0.119 s;得到的避障策略為航向不改變,航速從40 kn減少到27.1 kn行進。無人艇運行到避障結束點P-AD1后,就已成功完成了對障礙物DOC1和DOR2的規(guī)避,此時便逐漸加速到40 kn繼續(xù)朝向本段的子目標點Path2航行。

無人艇運行到Path3位置時,探測到已知動態(tài)矩形障礙物DOR3,并與無人艇呈32.06°運行,即角度差在(0°,45°)之間,且無人艇航速(40 kn)大于障礙物速度(15 kn),為海事規(guī)則中的追越情形。此時需要無人艇做出相應的調整,從障礙物左舷即無人艇左轉向通行。所以無人艇在Path3時刻通過該算法解算,耗時0.078 s;得到的避障策略為向左舷轉向7.62°,航速增加到42.0 kn行進。無人艇運行到避障結束點P-AD3后,就已成功完成了對障礙物DOR3的規(guī)避。此時便逐漸恢復至執(zhí)行任務航速40 kn,轉向本段的子目標點即終點航行。

6 結束語

對障礙物進行包圍體處理,使障礙物形狀得到合理簡化,減少了計算消耗,縮短了計算時間。且根據障礙物長寬比,采用圓形包圍體或有向包圍盒對障礙物進行簡化;較好地解決了國內學者對無人艇會遇障礙物簡化時,僅采用單一的圓形包圍體處理而導致的損失大量可行區(qū)間、產生不合理避障策略的問題。

將國際海上避碰行為規(guī)則作為避障約束條件。通過仿真實驗可得出,該算法不僅對單一障礙物避障適用,同時針對多障礙物規(guī)避問題,也可得到合理的避障策略。

將粒子群優(yōu)化算法引入無人艇動態(tài)障礙規(guī)避中,很好地發(fā)揮了該算法在解決多目標優(yōu)化問題中操作流程簡單、參數簡潔、收斂效果好的特點。通過仿真實驗得出,避障解算時間在0.15 s之內;且根據避障情景可解算出僅改變航速、僅改變航向、同時改變航速和航向的3種可行策略。

仿真實驗結果表明,該算法能夠很好地完成對動態(tài)障礙物的規(guī)避,具有較強的環(huán)境適應能力、較快的解算速度、較好的避障效果,滿足無人艇對障礙物規(guī)避的要求。然而由于部分障礙物存在運動不確定性,預測到的障礙物運動信息難免出現(xiàn)不準確的情況。針對這些障礙物若僅采用文中提出的算法,不能保障無人艇成功進行避障。所以下一步需要解決的問題是:無人艇根據視覺傳感器或避障聲吶等障礙識別系統(tǒng)實時探測并反饋障礙物當前位置信息,在文中提出算法的基礎上,對阻礙無人艇航行的障礙物做出敏捷的應激反應式避障。

［1］YAN R,PANG S,S-UN H,et al.Development and missions of unmanned surface vehicle［J］.Journal ofMarine Science and Application,2010,9(4):451-457.

［2］BENJAMIN M R,CURCIO J A,LEONARD J J,et al. Navigation of unmanned marine vehicles in accordance with the rules of the road［C］∥Proceedings of the 2006 IEEE International Conference on Robotics and Automation.Orlando,Florida,USA,2006:3581-3587.

［3］莊佳園,蘇玉民,廖煜雷,等.基于航海雷達的水面無人艇局部路徑規(guī)劃［J］.上海交通大學學報,2012,46 (9):1371-1375.

［4］盧艷爽.水面無人艇路徑規(guī)劃算法研究［D］.哈爾濱:哈爾濱工程大學,2010.

［5］馬 闖,殷 波,馬文帥.水上機器人三維實時避障算法研究［J］.微計算機信息,2009,25(3-2):235-237.

［6］CASALINO G,TURETTA A,SIMETTI E.A three-layered architecture for real time path planning and obstacle avoidance for surveillance USVs operating in harbour fields［C］∥OCEANS 2009-EUROPE.Bremen,German,2009:1-8.

［7］LEE S,KWON K,JOH J.A fuzzy logic for autonomous navigation ofmarine vehicles satisfying COLREG guidelines［J］.International Journal of Control Automation and Systems,2004,2(2):171-181.

［8］蘇紹娟,王麗錚,王呈方.不確定性航線配船數學模型建模方法［J］.船海工程,2007,36(4):100-103.

［9］張麗平.粒子群優(yōu)化算法的理論及實踐［D］.杭州:浙江大學,2005.

A Dynamic Obstacle Avoidance Method of USV Based on COLREGS

DU Kai-juna,b,MAO Yun-shenga,b,XIANG Zu-quana,b,ZHOU Yong-qinga,b,SONG Li-feib,LIU Binb
(a.Key Laboratory of High Performance Ship Technology; b.School of Transportation,Wuhan University of Technology,Wuhan 430063,China)

To solve the problem for the unmanned surface vehicle(USV)avoiding the dynamic obstaclewhich is notmarked on an electronic chart,a dynamic obstacle avoidancemethod complies the international regulations for preventing collisions atsea (COLREGS)are presented.The dynamic obstacles are converted into an instantaneous static state related to USV in a motion moment,so as to compute the optimal speed and heading in real-time for the USV to avoid the obstacles by particle swarm optimization algorithm.Simulation experiments demonstrate that the dynamic obstacle avoidancemethod can calculate a reasonable avoidance strategy and accomplish the dynamic obstacle avoidance task effectively.

USV;dynamic obstacles;particle swarm optimization;path planning;maritime rule

10.3963/j.issn.1671-7953.2015.03.028

U675.85

A

1671-7953(2015)03-0119-06

2014-12-16

修回日期:2015-01-19

杜開君(1987-),男,碩士生

研究方向:船舶先進制造技術

E-m ail:dkjkls@126.com