一種子目標(biāo)和水平集方法融合的AUV動態(tài)航路規(guī)劃新算法

盛亮， 蘇寧， 羅榮， 馮煒

(1.海軍航空大學(xué) 航空基礎(chǔ)學(xué)院，山東，煙臺 264001； 2.錦西工業(yè)學(xué)校，遼寧，葫蘆島 125001；3.海軍研究院，北京 102442)

自主水下潛航器(autonomous underwater vehicle，AUV)在復(fù)雜的海洋環(huán)境中航行時(shí)，一是事先不可能預(yù)知航路范圍內(nèi)的所有固定障礙物和威脅區(qū)；二是突發(fā)的移動障礙物，比如大型魚類、魚群、其他水下潛航器等也可能出現(xiàn)在預(yù)先規(guī)劃好的航路上；三是預(yù)先規(guī)劃的全局航路上也可能會遇到時(shí)空變化劇烈的海流. 以上這些情況都會危及AUV的航行安全，故其必須具備良好的自主實(shí)時(shí)避障、避險(xiǎn)航路規(guī)劃能力. 基于此，眾多學(xué)者開展了AUV實(shí)時(shí)避障航路規(guī)劃方法的研究.

文獻(xiàn)[1-10]中開展了水下潛航器的實(shí)時(shí)避障航路規(guī)劃研究，如進(jìn)化計(jì)算[1]、遺傳算法[2]、粒子群算法[3-4]、蟻群[5]、人工勢場法[6,7]、幾何方法[8]、生物啟發(fā)方法[9]以及啟發(fā)式搜索的D*Lite算法[10]等，仿真表明這些算法都能夠有效地規(guī)避未知的靜止障礙物和突發(fā)運(yùn)動障礙物，但這些算法都存在一個(gè)明顯的先天不足之處：未知障礙物建模為圓形或球形，且假定圓心或球心和半徑可由傳感器獲得的信息直接得出. 而在實(shí)際航行中，AUV很難獲得障礙物的具體尺寸和中心位置. 文獻(xiàn)[11-13]中基于虛擬子目標(biāo)的思想研究了機(jī)器人的避障路徑規(guī)劃，原理清晰、簡單，實(shí)現(xiàn)方便. 此外，從AUV實(shí)時(shí)避障航路規(guī)劃的眾多研究中可知，絕大多數(shù)的實(shí)時(shí)避障研究仍在二維平面內(nèi)進(jìn)行，少量的三維空間避障規(guī)劃也是將空間分成了水平面和垂直面進(jìn)行規(guī)劃，理論基礎(chǔ)仍是二維平面內(nèi)的實(shí)時(shí)避障航路規(guī)劃. 因此，當(dāng)前AUV自主實(shí)時(shí)避障航路規(guī)劃的研究重點(diǎn)仍在二維平面內(nèi)的航路規(guī)劃上.

本文將這種思想融入到水平集算法之中，基于AUV的前視聲吶探測的障礙物情形，預(yù)估障礙物尺寸和中心位置，據(jù)此得出安全可靠的子目標(biāo)點(diǎn)，提出了一種基于子目標(biāo)法和水平集方法[14]的二維自主實(shí)時(shí)航路規(guī)劃新算法. 新算法能確保建立更為符合實(shí)際的避障航路，且在完全避障的情況下，盡可能利用海流航行至終點(diǎn).

1 基于AUV前視聲吶探測的障礙物模型

1.1 前視聲吶探測模型

AUV用于避障探測的傳感器主要是前視聲吶，其獲取的實(shí)時(shí)信息為其自主實(shí)時(shí)避障航路規(guī)劃提供依據(jù). 本文以某型水下AUV搭載的前視聲吶為例進(jìn)行說明. 圖1是其探測區(qū)域示意圖，它是一種多波束的主動型聲吶，通常安裝在水下潛航器的前端，通過發(fā)射多束聲波對前方環(huán)境進(jìn)行水平和垂直掃描探測，圖中所示水平探測角度范圍為[-αso/2,αso/2]，垂直探測范圍[-βso/2,βso/2]. 若前方有障礙物且在聲吶探測范圍內(nèi)時(shí)，反射回的聲波被聲吶接收處理生成原始的聲吶圖像，再通過相應(yīng)的圖像處理和特征提取算法得到柵格化的輪廓邊緣. 通過反射回的聲波還可以得到AUV到障礙物邊緣的距離以及相應(yīng)的方位. 以上述獲取的信息為基礎(chǔ)，即可開展后續(xù)的自主實(shí)時(shí)避碰航路規(guī)劃[15]. 本文不涉及圖像處理和特征提取研究，故而假定通過聲吶探測已直接獲取了柵格化的障礙物輪廓.

圖1 AUV前視聲吶探測范圍Fig.1 The detection range of AUV’s forward-looking sonar

海洋環(huán)境中，AUV在遇到障礙物時(shí)，不考慮轉(zhuǎn)彎角限制，可在AUV前方的三維空間眾多方向上實(shí)施機(jī)動避障，若進(jìn)行航路規(guī)劃時(shí)都考慮到，算法將極為復(fù)雜，且規(guī)劃效率低. 因而，本文將聲吶探測、障礙物建模及自主實(shí)時(shí)避障航路規(guī)劃均限定在水平面內(nèi). 故而，前視聲吶的探測范圍可以表示為圖2形式. 其中，Rso為其有效的最大探測半徑，[-αso/2,αso/2]仍為其角度探測范圍.

圖2 前視聲吶二維探測范圍模型Fig.2 The 2D model of detection range of AUV’s forward-looking sonar

1.2 未知固定障礙物模型

AUV沿著全局最優(yōu)航路航行過程中，前視聲吶在探測到未知的固定障礙物時(shí)，并不能完全獲知障礙物的尺寸和中心位置，僅能夠探測到障礙物的部分邊緣位置及其距離AUV的距離，如圖3所示. 將未知固定障礙物限定為凸集，非凸集的部分用補(bǔ)缺的方法轉(zhuǎn)換為凸集[16]，進(jìn)而采用凸集的外切圓對障礙物建模.

圖3 聲吶探測的障礙物邊緣Fig.3 The edges of the obstacle detected by sonar

設(shè)某時(shí)刻t時(shí)，AUV在其預(yù)定的航向上發(fā)現(xiàn)有障礙物，此時(shí)，可探測到的障礙物邊緣與AUV及其聲吶的探測范圍如圖4所示3種相對位置關(guān)系.

圖4 聲吶探測區(qū)與障礙物位置關(guān)系Fig.4 The relationship between sonar detection area and obstacle position

圖4(a)中，聲吶發(fā)出的聲波無法探測到障礙物的邊緣，而在未知障礙物邊緣的情況下，無法基于此時(shí)的局部障礙物邊緣信息進(jìn)行建模并航路規(guī)劃，因此，AUV應(yīng)減速，并判斷兩側(cè)以最大探測角發(fā)出的聲波線與障礙物相交的點(diǎn)Q1和Q2哪個(gè)離AUV距離更遠(yuǎn)，圖中所示為右側(cè)Q2點(diǎn). AUV為確保更高的安全性，減速航行的同時(shí)向更遠(yuǎn)點(diǎn)Q2方向逐步轉(zhuǎn)向，每一步的轉(zhuǎn)向角設(shè)為Δαso，轉(zhuǎn)向直至發(fā)出的最大探測角的聲波線與障礙物邊緣相切，記下此時(shí)的點(diǎn)Q′2. 構(gòu)建外切圓形障礙物模型時(shí)將以點(diǎn)Q1和點(diǎn)Q′2為基準(zhǔn). 圖4(b)中，聲吶發(fā)出的聲波探測到了障礙物的某一側(cè)(右側(cè))邊緣，與障礙物相交于點(diǎn)Q4. 此時(shí)構(gòu)建外切圓形障礙物模型時(shí)將以點(diǎn)Q3和點(diǎn)Q4為基準(zhǔn). 圖4(c)中，聲吶發(fā)出的聲波探測到障礙物的所有邊緣，分別與障礙物相交于點(diǎn)Q5和點(diǎn)Q6. 故此時(shí)構(gòu)建外切圓形障礙物模型時(shí)將以點(diǎn)Q3和點(diǎn)Q4為基準(zhǔn).

不失一般性，以圖4(b)中情形為例進(jìn)行建模，圖4(a)和圖4(c)中情形在建模時(shí)方法一樣. 在圖4(b)中，由于能探測到障礙物的右側(cè)邊緣，故可認(rèn)為右側(cè)信息較左側(cè)更完備，障礙物建模的可信度更高，因此航路規(guī)劃時(shí)選擇右側(cè)通過安全性更高，因而下一步子目標(biāo)點(diǎn)選擇時(shí)選擇從障礙物模型右側(cè)選點(diǎn). 基于點(diǎn)Q3和點(diǎn)Q4為基準(zhǔn)的圓形障礙物建模如圖5所示.

圖5 基于探測基準(zhǔn)點(diǎn)的圓形障礙物模型Fig.5 The circular obstacle model based on detection datum point

則中心點(diǎn)Q7坐標(biāo)可表示為

由三角函數(shù)公式可知點(diǎn)Q3和點(diǎn)Q4間距離為

(1)

另可得∠OlmsQ4Q3為

(2)

易知∠OlmsQ4Q3=∠O4QobQ7,故

(3)

故而，Q4Oob距離為

(4)

則障礙物模型的圓心在AUV隨體坐標(biāo)系下的坐標(biāo)為：(d1，d4). 圓的半徑為：r=d4. 航路規(guī)劃時(shí)，通常將AUV當(dāng)做一個(gè)質(zhì)點(diǎn)，因而，障礙物建模時(shí)通常按照AUV的尺寸進(jìn)行膨脹處理，亦即最后得到的障礙物模型為圓心在坐標(biāo)(d1，d4)處，半徑為r=d4+l0，式中l(wèi)0為AUV的長度.

對于圖4(a)中的障礙物情形，以點(diǎn)Q1和點(diǎn)Q′2為基準(zhǔn)，完全參照上述方法建模；對于圖4(c)中的障礙物情形，選擇從距離AUV較遠(yuǎn)的交點(diǎn)Q6側(cè)繞行，故以該點(diǎn)作為外切點(diǎn)對障礙物建模，方法不變.

1.3 突發(fā)移動障礙物模型

海洋環(huán)境下，突發(fā)的移動障礙物的尺寸和圓心建模，參照未知固定障礙物的建模思路進(jìn)行. 除此之外，由于移動障礙物具備一定的速度，且方向和大小往往是隨機(jī)的，在上述模型的基礎(chǔ)上需要加上動態(tài)不確定性.

海洋中的突發(fā)移動障礙物主要是指魚類、魚群、AUV平臺或潛艇等，它們在水下運(yùn)動的過程中有較大的隨機(jī)性，因而本文將突發(fā)的移動障礙物模型統(tǒng)一設(shè)定為一個(gè)隨機(jī)的離散時(shí)間系統(tǒng)[17]，即

(5)

式中Wi為障礙物的狀態(tài)表示，代表了障礙物位置、速度及隨機(jī)性. 可進(jìn)一步表示為

Wi=[WpWvWυ]T

式中：Wp=[WpxWpy]為障礙物圓心的期望坐標(biāo)；Wυ表示障礙物圓心坐標(biāo)的不確定性，其隨不確定性參數(shù)dυ的變化而變化；Wv=[WvxWvy]為障礙物的運(yùn)動速度，表示對地速度，由海流速度和自身速度合成；障礙物的隨機(jī)性運(yùn)動由Ui -1表示，代表一個(gè)疊加在障礙物速度上的隨機(jī)擾動，其服從正態(tài)分布：Ui -1～N(0,σ2). 模型化后，突發(fā)移動障礙物的狀態(tài)可表示為

(6)

方程組可變?yōu)榫仃囆问?/p>

Wi=G0Wi -1+U0Ui -1+I0dυ

(7)

式中：

突發(fā)移動障礙物模型的仿真見圖6所示.

圖6 海流環(huán)境中的突發(fā)移動障礙物模型Fig.6 The model of sudden moving obstacle in current environment

從圖6中可以看出，障礙物模型包含了原來的圓形模型和新加入的運(yùn)動因素和隨機(jī)因素，建成了一種用于自主實(shí)時(shí)避障航路規(guī)劃的近似話筒型的移動障礙物模型.

2 基于子目標(biāo)法和水平集方法的自主實(shí)時(shí)航路規(guī)劃算法

AUV在自主航行過程中，因受到水下聲波探測距離的限制，能夠獲取的環(huán)境信息有限，為確保避障成功和航行安全，每一次自主實(shí)時(shí)航路規(guī)劃都是規(guī)劃空間較小的局部航路規(guī)劃. 因而，需要在每次探測到航向上存在障礙物或可能與障礙物交會的情況下，在局部規(guī)劃空間中設(shè)定一個(gè)合適的子目標(biāo)點(diǎn)以取代預(yù)先規(guī)劃的下一航路點(diǎn)，再利用水平集方法快速規(guī)劃出能夠到達(dá)子目標(biāo)點(diǎn)且確保能避開障礙物、充分利用海流的局部最優(yōu)航路.

2.1 子目標(biāo)點(diǎn)的選取

對于未知固定障礙物和突發(fā)移動障礙物，子目標(biāo)的選取方法有所不同，下面分開進(jìn)行說明.

2.1.1遭遇未知固定障礙物時(shí)子目標(biāo)點(diǎn)的選取

遭遇未知固定障礙物時(shí)，不失一般性，以圖5所示模型為基礎(chǔ)進(jìn)行選取. 此時(shí)，選取方式如圖7所示.

圖7 未知固定障礙物時(shí)的子目標(biāo)點(diǎn)Fig. 7 Sub-target for unknown fixed obstacle

子目標(biāo)點(diǎn)選取的過程如下：先將圖5中的障礙物圓形模型進(jìn)行膨脹處理，膨脹后圓心位置不變，半徑增加AUV的自身長度，即l0，形成膨脹后的障礙物圓形模型，即圖7中大網(wǎng)格狀圓表示區(qū)域. 在實(shí)施自主實(shí)時(shí)航路規(guī)劃過程中，大網(wǎng)格圓區(qū)域?qū)⒆鳛橐?guī)劃空間中的禁航區(qū)一直存在. 沿著半徑增加的方向OobQ4繼續(xù)延伸l0長度到達(dá)一點(diǎn)Q8，此點(diǎn)即為當(dāng)前選定的子目標(biāo)點(diǎn). 可求出子目標(biāo)點(diǎn)的坐標(biāo)為：(d1，2l0).

2.1.2遭遇突發(fā)移動障礙物時(shí)子目標(biāo)點(diǎn)的選取

突發(fā)移動障礙物出現(xiàn)在AUV預(yù)計(jì)的航路上或可能在預(yù)計(jì)航路上交會時(shí)，將會出現(xiàn)障礙物向AUV預(yù)計(jì)航路的左方移動、障礙物向AUV預(yù)計(jì)航路的右方移動、障礙物在AUV預(yù)計(jì)航路上與其相向運(yùn)動或在預(yù)計(jì)航路上與其同向運(yùn)動且速度小于AUV的速度等4種情形.

① 障礙物向AUV預(yù)計(jì)航路的左方移動.

此種情形下有可能會與AUV在其預(yù)定的航道上相遇. 為了確保完全避障，需要規(guī)劃出一條局部優(yōu)化避障航路. 可在初次探測到障礙物所在位置的右方選取子目標(biāo)點(diǎn)Q9，如圖8所示. 此時(shí)，AUV應(yīng)向預(yù)計(jì)航路的右側(cè)轉(zhuǎn)彎航行.

圖8 障礙物向AUV預(yù)計(jì)航路的左方移動Fig.8 The moving obstacle to the left of the AUV’s intended course

② 障礙物向AUV預(yù)計(jì)航路的右方移動.

此種情形子目標(biāo)點(diǎn)的選擇方法與圖8中類似，應(yīng)在初次探測到障礙物所在位置的左方選取子目標(biāo)點(diǎn).

③ 障礙物在AUV預(yù)計(jì)航路上且兩者相向運(yùn)動.

此種情形碰撞威脅的風(fēng)險(xiǎn)較高，需要以較大的轉(zhuǎn)彎角進(jìn)行改航回避，且此時(shí)應(yīng)盡可能先降低航行速度. 子目標(biāo)通常選擇第二次探測到障礙物時(shí)的最近點(diǎn)左方或右方垂直距離2l0處，如圖9中的Q10點(diǎn)所示.

圖9 障礙物在AUV預(yù)計(jì)航路上且兩者相向運(yùn)動Fig.9 The moving obstacle on the predicted route of AUV and in opposite direction

④ 障礙物在AUV預(yù)計(jì)航路上且兩者同向運(yùn)動.

此種情形下若障礙物的速度高于AUV的速度，則不用考慮避障規(guī)劃問題，AUV可以完全按照預(yù)計(jì)航路航行；若障礙物的速度低于AUV的速度，AUV有可能在預(yù)計(jì)的這段航路上追上障礙物，因而需要開展避障規(guī)劃，此時(shí)子目標(biāo)點(diǎn)選擇方法如圖10所示.

圖10 障礙物在AUV預(yù)計(jì)航路上且兩者同向運(yùn)動Fig.10 The moving obstacle on the predicted route of AUV and in same direction

在圖9中點(diǎn)Q10選取的是在預(yù)計(jì)航路的左側(cè)，與第二次探測到障礙物時(shí)的最近點(diǎn)相距l(xiāng)0，且兩者的連線與預(yù)計(jì)航向垂直. AUV到達(dá)點(diǎn)Q10后再次進(jìn)行判斷前方航路上與障礙物交會的可能性，此時(shí)的情形類同于本小節(jié)①中圖8的情形，繼續(xù)進(jìn)行子目標(biāo)點(diǎn)的選取和航路規(guī)劃；若無交會的可能，則按照自主實(shí)時(shí)規(guī)劃的局部航路航行至終點(diǎn).

2.2 水平集方法

考慮海流影響，將水平集方法用于AUV的航路規(guī)劃時(shí)[10]，設(shè)海流速度為v(X,t)，AUV的運(yùn)動由0水平集的運(yùn)動決定，水平集演化方程中的演化速度則由海流速度和AUV自身速度合成，演化方程為

由式(8)生成模擬海流

(8)

式中：vx(r)，vy(r)分別為水平和縱向方向的渦流速度分量；κ、ξ分別用來描述渦流強(qiáng)度、渦流半徑坐標(biāo)；ri為第i個(gè)渦流中心的位置，仿真中，渦流中心位置隨機(jī)生成，n為疊加的渦流個(gè)數(shù)；vcx，vcy分別為定常流vc在水平、縱向方向的速度分量.

初始條件為：φ(X,0)=|X-Ys|，Ys為AUV的初始位置. 其中，F(xiàn)AUV(X,t)為AUV自身速度，亦即AUV相對海流的速度. 為保證所求路徑為時(shí)間最優(yōu)，要求FAUV(X,t)的方向?yàn)樗郊瘮?shù)的梯度方向，則式(8)可進(jìn)一步化為如下Hamilton-Jacobi方程，亦即基于水平集方法的AUV航路規(guī)劃演化方程

(9)

當(dāng)AUV航行至終點(diǎn)附近時(shí)，終點(diǎn)落在了0水平集內(nèi). 通過式(10)進(jìn)行回溯求解，即得最優(yōu)航路. 回溯方程為

(10)

2.3 算法步驟

基于子目標(biāo)法和水平集方法的自主實(shí)時(shí)航路規(guī)劃算法包含針對未知固定障礙物的算法和針對突發(fā)移動障礙物的算法兩部分，現(xiàn)給出遭遇未知固定障礙物時(shí)算法步驟.

① 初始化規(guī)劃空間為二維柵格笛卡爾空間，初始化AUV速度、起始點(diǎn)和終點(diǎn)坐標(biāo)，初始化已知固定障礙物和威脅區(qū). 設(shè)定演化參數(shù)和AUV速度，按式(8)初始化模擬海流場.

② 依照水平集方法規(guī)劃出當(dāng)前海洋環(huán)境下的最優(yōu)航路，并按照該航路開始航行并啟動前視聲吶探測.

③ 聲吶探測發(fā)現(xiàn)當(dāng)前航向上存在未知固定障礙物，按照1.2中的方法對障礙物進(jìn)行建模并處理成禁航區(qū)；按照2.1.1中的方法選取子目標(biāo)點(diǎn)Qi，記錄下當(dāng)前點(diǎn)位置和子目標(biāo)點(diǎn)位置.

④ 依照水平集方法從當(dāng)前位置點(diǎn)和剛剛選取的子目標(biāo)點(diǎn)Qi間以及該子目標(biāo)點(diǎn)Qi和終點(diǎn)間規(guī)劃出兩條優(yōu)化航路i和′i，然后沿航路i航行至點(diǎn)Qi，在點(diǎn)Qi處開啟聲吶探測，當(dāng)探測到航路′i的當(dāng)前航向上仍然存在障礙物時(shí)，i++，轉(zhuǎn)③；若無障礙物，則沿航路′i向終點(diǎn)航行，將此時(shí)探測到的未在航路上的障礙物邊緣按1.2中的方法進(jìn)行建模并處理成禁航區(qū)后與先前的禁航區(qū)合并.

其算法流程圖如圖11所示.

圖11 遭遇未知固定障礙物時(shí)自主實(shí)時(shí)航路規(guī)劃混合算法Fig.11 Hybrid algorithm for autonomous real-time route planning when encountering unknown fixed obstacles

遭遇突發(fā)移動障礙物時(shí)的算法步驟與遭遇未知固定障礙物時(shí)的算法步驟類似，其算法流程圖如下圖12所示.

圖12 遭遇突發(fā)移動障礙物時(shí)自主實(shí)時(shí)航路規(guī)劃混合算法Fig.12 Hybrid algorithm for autonomous real-time route planning when encountering sudden moving obstacles

3 案例仿真與結(jié)果分析

本節(jié)對新算法開展仿真研究，驗(yàn)證模型和算法的可靠性和有效性.

3.1 案例一：未知固定障礙物情形

未知固定障礙物環(huán)境下，設(shè)定規(guī)劃空間為1 000 m×1 000 m的二維笛卡爾坐標(biāo)空間. 柵格間距為1 m. 起始點(diǎn)坐標(biāo)為(10，35)，終點(diǎn)坐標(biāo)為(90，70). 真實(shí)海流的模擬由5個(gè)隨機(jī)渦流和斜向右上方的定常流疊加而成. 已知障礙物與威脅區(qū)的設(shè)置：2個(gè)敵對探測威脅區(qū)域分別為：威脅1圓心坐標(biāo)(350,500)，威脅半徑為5.5 m. 威脅2圓心坐標(biāo)(650,400)，威脅半徑7 m. 2個(gè)障礙物建模為5邊形模型，障礙物1的5個(gè)頂點(diǎn)坐標(biāo)為：(510,760)，(460,742)，(471,620)，(563,620)，(572,729)；障礙物2的5個(gè)頂點(diǎn)坐標(biāo)為：(399,351)，(370,340)，(361,272)，(430,258)，(430,340). 算法首先基于已知的障礙物和威脅區(qū)通過水平集航路規(guī)劃方法生成一條全局最優(yōu)航路，即圖13中所示虛線所示航路.

圖13 遭遇未知固定障礙物時(shí)的航路規(guī)劃仿真Fig.13 Simulation of route planning when encountering unknown fixed obstacles

AUV先沿著規(guī)劃的虛線所示航路潛行，至6角星點(diǎn)處，前視聲吶探測到有未知的固定障礙物，則啟動障礙物建模和子目標(biāo)點(diǎn)選取，依據(jù)探測到的障礙物邊緣信息建立的模型即圖中的“+”號邊緣圓，子目標(biāo)點(diǎn)即為米字點(diǎn)所在位置，坐標(biāo)為(698.972,791.523). 在AUV當(dāng)前點(diǎn)(6角星點(diǎn))和子目標(biāo)點(diǎn)(米字點(diǎn))、子目標(biāo)點(diǎn)和終點(diǎn)之間分別進(jìn)行最優(yōu)航路規(guī)劃，所得即為圖中實(shí)線表示的拼接后的航路. 從圖14中可知，AUV能夠在充分利用海流的情況下，安全避開障礙物航行至終點(diǎn). 此外，整個(gè)自主實(shí)時(shí)航路規(guī)劃僅取了一個(gè)子目標(biāo)點(diǎn)就實(shí)現(xiàn)了至終點(diǎn)的避碰航路規(guī)劃.

圖14 障礙物向預(yù)計(jì)航路的左方移動時(shí)航路規(guī)劃仿真Fig.14 Simulation of route planning when obstacles move to the left of the predicted route

預(yù)先規(guī)劃的全局最優(yōu)航路和自主實(shí)時(shí)規(guī)劃后的實(shí)際航路數(shù)據(jù)如表1所示.

從表1可知，避障規(guī)劃后，實(shí)際航行長度較預(yù)先規(guī)劃的航路要長2.32%，航行時(shí)間要長2.06%. 增加值很小，因此，2.3.1節(jié)中提出的針對未知固定障礙物的避障混合算法不僅能夠完全避障，而且避障后規(guī)劃的航路仍然是最優(yōu)的. 算法的可靠性和有效性得到了驗(yàn)證.

表1 兩種航路性能對比Tab.1 Performance comparison of two routes

3.2 案例二：突發(fā)移動障礙物情形

突發(fā)障礙物情形是在案例一規(guī)劃空間基礎(chǔ)上加入未知的移動障礙物，這些障礙物的位置具有不確定性. 按照1.3節(jié)的方式對移動障礙物建模. 障礙物速度分布在-3 m/s和3 m/s之間，方向由速度分量的不同所決定，位置的不確定性隨時(shí)間的增長而線性增加，為簡化分析，假定從聲吶探測到障礙物到AUV安全完成避障的整個(gè)過程中障礙物的移動方向大致上不變. 圖14～圖17分別為4種情形下的避障航路規(guī)劃仿真示意圖.

圖15 障礙物向預(yù)計(jì)航路的右方移動時(shí)航路規(guī)劃仿真Fig.15 Simulation of route planning when obstacles move to the right of the predicted route

圖16 障礙物在預(yù)計(jì)航路上且相向運(yùn)動時(shí)航路規(guī)劃仿真Fig.16 Simulation of route planning when obstacles move in opposite direction on the predicted route

圖17 障礙物在預(yù)計(jì)航路上且相向運(yùn)動時(shí)航路規(guī)劃仿真Fig.17 Simulation of route planning when obstacles move in same direction on the predicted route

顯然，從圖14～圖17中易知，4種情形下采用圖12中的算法進(jìn)行自主實(shí)時(shí)避障航路規(guī)劃是完全成功的. 預(yù)先規(guī)劃的全局最優(yōu)航路和4種情形下自主實(shí)時(shí)規(guī)劃后的AUV實(shí)際航路取平均后得到數(shù)據(jù)如表2所示.

表2 兩種航路性能對比Tab.2 Performance comparison of two routes

從表2易知，避障規(guī)劃后，實(shí)際平均航行長度較預(yù)先規(guī)劃的航路要長1.87%，航行時(shí)間要長4.77%. 航路長度增加值很小，但航行時(shí)間增加值稍有些大，主要原因在于為了成功避開移動的障礙物，自主實(shí)時(shí)規(guī)劃的航路上海流所起的正作用不如全局預(yù)先規(guī)劃航路上的海流正作用大. 總體而言，在保證100%規(guī)避突發(fā)移動障礙物的情況下，本文算法規(guī)劃的自主實(shí)時(shí)航路的性能較原來的全局最優(yōu)航路的性能下降量很小，說明本文算法規(guī)劃的航路是可靠和有效的，算法是符合實(shí)際要求的.

4 結(jié) 論

由于水下環(huán)境的不可完全預(yù)知，故本文針對海洋環(huán)境中的未知固定障礙物和突發(fā)移動障礙物開展了AUV的自主實(shí)時(shí)避障航路規(guī)劃技術(shù)研究. 在AUV前視聲吶探測模型的基礎(chǔ)上，分別對未知固定障礙物和突發(fā)移動障礙物進(jìn)行了建模，提出了2類較為符合實(shí)際的二維障礙物模型；在2類模型的基礎(chǔ)上，進(jìn)一步研究了航路規(guī)劃中子目標(biāo)點(diǎn)的選取方法. 針對未知固定障礙物，提出了一種基于外切膨脹障礙物圓的子目標(biāo)點(diǎn)選取方法；針對突發(fā)的移動障礙物，基于其運(yùn)動方向與AUV預(yù)計(jì)的航向的關(guān)系，給出了4種不同的選取子目標(biāo)點(diǎn)的方法. 最后結(jié)合子目標(biāo)點(diǎn)法和水平集方法，對未知固定障礙物和移動障礙物中的4種運(yùn)動情形進(jìn)行了自主實(shí)時(shí)避障航路規(guī)劃，仿真結(jié)果表明，本文提出的算法均能做到完全的避障，且避障后的航路性能質(zhì)量較原來全局最優(yōu)航路的性能質(zhì)量下降得很小.