盛亮， 蘇寧， 羅榮， 馮煒

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

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

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

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

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

1.1 前視聲吶探測模型

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

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

海洋環(huán)境中，AUV在遇到障礙物時，不考慮轉(zhuǎn)彎角限制，可在AUV前方的三維空間眾多方向上實施機動避障，若進行航路規(guī)劃時都考慮到，算法將極為復(fù)雜，且規(guī)劃效率低. 因而，本文將聲吶探測、障礙物建模及自主實時避障航路規(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)航路航行過程中，前視聲吶在探測到未知的固定障礙物時，并不能完全獲知障礙物的尺寸和中心位置，僅能夠探測到障礙物的部分邊緣位置及其距離AUV的距離，如圖3所示. 將未知固定障礙物限定為凸集，非凸集的部分用補缺的方法轉(zhuǎn)換為凸集[16]，進而采用凸集的外切圓對障礙物建模.

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

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

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

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

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

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

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

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

(1)

另可得∠OlmsQ4Q3為

(2)

易知∠OlmsQ4Q3=∠O4QobQ7,故

(3)

故而，Q4Oob距離為

(4)

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

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

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

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

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

(5)

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

Wi=[WpWvWυ]T

式中：Wp=[WpxWpy]為障礙物圓心的期望坐標(biāo)；Wυ表示障礙物圓心坐標(biāo)的不確定性，其隨不確定性參數(shù)dυ的變化而變化；Wv=[WvxWvy]為障礙物的運動速度，表示對地速度，由海流速度和自身速度合成；障礙物的隨機性運動由Ui -1表示，代表一個疊加在障礙物速度上的隨機擾動，其服從正態(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中可以看出，障礙物模型包含了原來的圓形模型和新加入的運動因素和隨機因素，建成了一種用于自主實時避障航路規(guī)劃的近似話筒型的移動障礙物模型.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 水平集方法

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

由式(8)生成模擬海流

(8)

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

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

(9)

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

(10)

2.3 算法步驟

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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