李 楊，杜 度，于雙寧，毛柳偉，徐雪峰，徐 野

（1.海軍研究院，北京 100161；2.天津航海儀器研究所九江分部，江西 九江 332000）

0 引言

水下無人航行器（UUV）的局部自動(dòng)避碰是指UUV在航行過程中，遇到島嶼、船只、浮標(biāo)等動(dòng)態(tài)靜態(tài)障礙物時(shí)，能夠自動(dòng)識(shí)別避讓障礙物，完成指定任務(wù)?；谏窠?jīng)網(wǎng)絡(luò)法、遺傳算法、蟻群算法、粒子群算法等的智能避碰算法更加靈活，可應(yīng)用范圍廣[1-6]，但在實(shí)時(shí)性要求較高的工作環(huán)境中，過大的運(yùn)算量會(huì)使得運(yùn)算時(shí)間較長，無法保證實(shí)時(shí)性的規(guī)劃航路輸出。

人工勢(shì)場(chǎng)法、速度障礙法具有運(yùn)算量小，輸入信息簡(jiǎn)單，較易實(shí)現(xiàn)等優(yōu)點(diǎn)，因此，合理選擇并改進(jìn)傳統(tǒng)避碰算法能夠優(yōu)化UUV避碰航路規(guī)劃。文獻(xiàn)[7]引入速度斥力勢(shì)函數(shù)，構(gòu)建的動(dòng)態(tài)勢(shì)場(chǎng)解決了在動(dòng)態(tài)環(huán)境中避障時(shí)容易產(chǎn)生無法繞開障礙物和避讓行動(dòng)不合理等問題。但未能結(jié)合載體的操縱性能規(guī)劃路徑，可能會(huì)導(dǎo)致載體不能跟隨航路。文獻(xiàn)[8]改進(jìn)了人工勢(shì)場(chǎng)法，給機(jī)器人一個(gè)外力以避免其在局部最小點(diǎn)處停止或者徘徊，確保機(jī)器人能夠逃出最小值陷阱并順利到達(dá)目標(biāo)位置，解決了人工勢(shì)場(chǎng)法容易陷入最小值陷阱問題，但未能解決避讓動(dòng)態(tài)障礙物問題。

針對(duì)水下三維空間的避碰環(huán)境，本文提出了一種聯(lián)合使用人工勢(shì)場(chǎng)法和速度障礙法的避碰算法，能避讓動(dòng)態(tài)與靜態(tài)障礙物，生成避碰路徑，并根據(jù)UUV的操縱性能進(jìn)行路徑的柔順性處理。

1 算法執(zhí)行流程

將UUV 的起點(diǎn)記為p0，在p0處根據(jù)探測(cè)到的環(huán)境信息判斷障礙物是否在UUV的領(lǐng)域內(nèi)，再根據(jù)計(jì)算危險(xiǎn)度決定是否應(yīng)用人工勢(shì)場(chǎng)法和速度障礙法，之后根據(jù)重新分配的斥力計(jì)算合力，合力的方向?yàn)樵诋?dāng)前路徑節(jié)點(diǎn)處UUV的指令航向。局部避障路徑通過連接路徑節(jié)點(diǎn)產(chǎn)生，路徑節(jié)點(diǎn)由合力F和前進(jìn)步長獲得。在p0處按合力F方向取合適的步長后到達(dá)p1，在p1位置應(yīng)用上述方法計(jì)算合力，沿此合力方向前進(jìn)相同步長，以此類推直到到達(dá)下一子目標(biāo)點(diǎn)，順次連接pi，得到局部避障路徑。算法流程圖如圖1所示。

圖1 算法流程圖Fig.1 Algorithm flowchart

2 具體實(shí)施

2.1 改進(jìn)型人工勢(shì)場(chǎng)法

定義引力勢(shì)場(chǎng)為UUV與目標(biāo)位置相對(duì)位置的函數(shù)：

式中：ka為正常數(shù)是引力勢(shì)場(chǎng)系數(shù)；ρgoal為UUV當(dāng)前位置與目標(biāo)點(diǎn)間的距離。

假設(shè)UUV所處航行環(huán)境中有N個(gè)礙航物，第i個(gè)礙航物與UUV間的斥力勢(shì)函數(shù)為

式中：kr為斥力勢(shì)場(chǎng)強(qiáng)度系數(shù)，調(diào)節(jié)斥力大??；ks為范圍縮放系數(shù)，調(diào)節(jié)勢(shì)場(chǎng)的有效作用范圍；iρ為第i個(gè)礙航物與UUV間的距離；0ρ為一正常數(shù)，表示礙航物的尺度范圍。將引力勢(shì)場(chǎng)與斥力勢(shì)場(chǎng)疊加可得全局勢(shì)場(chǎng)為

引力勢(shì)場(chǎng)的負(fù)梯度為UUV所受引力，方向由UUV指向目標(biāo)點(diǎn)：

斥力勢(shì)函數(shù)的負(fù)梯度為作用于UUV的斥力，方向由礙航物指向UUV：

此斥力的構(gòu)建不作為分段函數(shù)的形式設(shè)置，是為防止在進(jìn)入分段函數(shù)設(shè)置的臨界距離后突然增大的斥力使得航路急轉(zhuǎn)，以免UUV無法跟隨。

2.2 速度障礙法

人工勢(shì)場(chǎng)法適用于對(duì)靜態(tài)障礙物的避碰規(guī)劃，但當(dāng)UUV航行區(qū)域內(nèi)有動(dòng)態(tài)的障礙物時(shí)，人工勢(shì)場(chǎng)法僅能根據(jù)動(dòng)態(tài)障礙物的位置信息進(jìn)行規(guī)劃，無法根據(jù)動(dòng)態(tài)障礙物的速度信息進(jìn)行靈活處理，針對(duì)動(dòng)態(tài)障礙物，可以利用速度障礙法進(jìn)行避障規(guī)劃。

應(yīng)用速度障礙法時(shí)，首先進(jìn)行碰撞區(qū)域的判斷，將障礙物進(jìn)行膨化處理后，建立UUV–障礙物（U–O）環(huán)境模型，確定碰撞區(qū)域，如圖2所示，建立U–O坐標(biāo)系。在t時(shí)刻，UUV向障礙物兩側(cè)切線方向發(fā)射射線L1和L2，L1和L2與x軸的夾角分別為θ1和θ2，水下無人航行器A和障礙物B的速度分別為Va和Vb，兩者相對(duì)速度為VR=Va-Vb，VR與x軸的夾角為θa。如果θ1＜θa＜θ2，則認(rèn)為UUV將駛向碰撞區(qū)域，需要進(jìn)行規(guī)避，可根據(jù)UUV與障礙船的相對(duì)速度指向位于二者連線的左側(cè)或右側(cè)進(jìn)行左轉(zhuǎn)或右轉(zhuǎn)操作[9]。

圖2 速度障礙法碰撞區(qū)域示意圖Fig.2 Schematic diagram of collision area

在人工勢(shì)場(chǎng)法斥力與引力的基礎(chǔ)上，在垂直UUV當(dāng)前航向方向上疊加一個(gè)轉(zhuǎn)向力FTrepi，在數(shù)值上設(shè)置為越靠近OA連線則轉(zhuǎn)向力就越大：

設(shè)OA連線與x軸夾角為θ0，如果相對(duì)速度的指向與x軸夾角θ1＜θa＜θ0，F(xiàn)Turni應(yīng)指向航向的右側(cè)，如果相對(duì)速度的指向與x軸夾角θ0＜θa＜θ2，F(xiàn)Turni應(yīng)指向航向的右側(cè)。

合力F為來自目標(biāo)引力、障礙物的斥力以及由速度障礙法計(jì)算的轉(zhuǎn)向力的疊加。

2.3 避碰危險(xiǎn)度

將水平面的最近會(huì)遇距離（DCPA）和垂直面的到達(dá)最近會(huì)遇點(diǎn)的時(shí)間（HCPA）作為影響碰撞危險(xiǎn)度最大的因素，各個(gè)因素的模糊模型設(shè)置如下：

其中U(DCPA)稱為水平面上的空間碰撞危險(xiǎn)度（SCR），主要受最近會(huì)遇距離、相對(duì)方位、UUV操縱性、能見度、航行水域情況等因素影響；U(HCPA)為垂直面上的碰撞危險(xiǎn)度模型。d1為水平面上的安全會(huì)遇領(lǐng)域，K1?SDA為水平面上的UUV安全通過距離，h1為垂直面上的安全會(huì)遇領(lǐng)域，h2=2h1。UUV只有水平面和垂直面的碰撞危險(xiǎn)度同時(shí)不為0時(shí)，才存在一定的碰撞危險(xiǎn)，其余情況下不存在碰撞危險(xiǎn)[10]。所以UUV總的危險(xiǎn)度為

2.4 根據(jù)危險(xiǎn)度重新分配斥力

UUV水下航路規(guī)劃區(qū)別于水面船，水下的航路是三維的，受到的障礙物斥力也是三維空間內(nèi)的一個(gè)矢量。將UUV的水下機(jī)動(dòng)分解為水平面上的轉(zhuǎn)向機(jī)動(dòng)與豎直方向上的俯仰機(jī)動(dòng)，顯然UUV在水平面和豎直方向上的機(jī)動(dòng)性和UUV自身空間尺度差別很大，由這個(gè)三維空間的斥力指引UUV機(jī)動(dòng)不合適，需要對(duì)斥力進(jìn)行重新分配，本文提出了一種重新分配斥力的方法。由斥力建模公式?jīng)Q定的斥力分布隨距離衰減是非常明顯的，因此，距離UUV較遠(yuǎn)的障礙物對(duì)UUV的影響可以忽略，U(HCPA)與U(DCPA)都等于0時(shí)，斥力的分配可以沿勢(shì)場(chǎng)梯度方向。對(duì)于距離較近U(HCPA)與U(DCPA)都等于1時(shí)，按照比重相同進(jìn)行分配。U(HCPA)與U(DCPA)相差較大時(shí)，按較大危險(xiǎn)度分配斥力較小原則分配斥力：

式中：Flevi為第i個(gè)障礙物提供的水平方向斥力；Fveri為第i個(gè)障礙物提供的豎直方向斥力。對(duì)于較大的危險(xiǎn)度，說明距離較近，避讓所需的機(jī)動(dòng)越大，可能出現(xiàn)避讓不及的情況，因此在另外一個(gè)方向上采取機(jī)動(dòng)進(jìn)行規(guī)避是較好的選擇。

2.5 人工勢(shì)場(chǎng)法與速度障礙法疊加

應(yīng)用人工勢(shì)場(chǎng)法疊加速度障礙法解決了UUV針對(duì)靜態(tài)與動(dòng)態(tài)障礙物的避碰規(guī)劃，人工勢(shì)場(chǎng)法中由目標(biāo)點(diǎn)提供引力，障礙物提供斥力；速度障礙法中如果相對(duì)速度指向速度障礙區(qū)則會(huì)添加一個(gè)垂直當(dāng)前航向的偏轉(zhuǎn)力，將偏轉(zhuǎn)力疊加進(jìn)人工勢(shì)場(chǎng)法中，求得合力方向即為 UUV的前進(jìn)方向。

在避讓靜態(tài)障礙物時(shí)，應(yīng)用速度障礙法可以有效避免陷入人工勢(shì)場(chǎng)法的最小值陷阱問題，并能使路徑更加光順。在人工勢(shì)場(chǎng)法中，由障礙物產(chǎn)生的斥力場(chǎng)隨距離衰減很快，在控制 UUV進(jìn)行避讓時(shí)，會(huì)出現(xiàn)轉(zhuǎn)向過急現(xiàn)象，如果調(diào)節(jié)參數(shù)減緩隨距離衰減的程度，則會(huì)使斥力作用范圍過大，減少可通行范圍，造成空間浪費(fèi)。因此在避讓靜態(tài)障礙物時(shí)，將障礙物看做靜止的運(yùn)動(dòng)目標(biāo)，相對(duì)速度變?yōu)?UUV自身的速度，應(yīng)用速度障礙法，控制UUV提前轉(zhuǎn)向。2種算法疊加使用，相互配合，應(yīng)用速度障礙法防止轉(zhuǎn)向過急，應(yīng)用人工勢(shì)場(chǎng)法控制最近接近距離，可同時(shí)保證安全性與可操控性。

2.6 航路的柔順性修正

在航路的柔順性方面，除了疊加人工勢(shì)場(chǎng)法與速度障礙法防止轉(zhuǎn)彎過急外，結(jié)合UUV的最小回轉(zhuǎn)半徑、最大潛浮速度等機(jī)動(dòng)性能，得到單步最大轉(zhuǎn)向角度γ和單步最大豎直方向變動(dòng)深度h后進(jìn)行約束：

式中：iα為pi點(diǎn)處規(guī)劃前進(jìn)方向在水平面上投影后與x軸的夾角；Hi為pi點(diǎn)處規(guī)劃指令深度。

3 仿真結(jié)果

對(duì)本文提出的聯(lián)合應(yīng)用人工勢(shì)場(chǎng)法和速度障礙法的避碰算法進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證，UUV出發(fā)點(diǎn)位置為（0，0，–100），終點(diǎn)位置為（10 000，10 000，–100），航速為 6 kn；動(dòng)態(tài)障礙物出發(fā)點(diǎn)位置為（9 000，0，–100），終點(diǎn)為（0，9 000，–100），航速為6.35 kn；靜態(tài)障礙物坐標(biāo)為（3 000，2 500，–50），（6 000，2 500，0），（6 000，6 000，–100），仿真結(jié)果如圖3所示。

圖3 仿真路徑結(jié)果圖Fig.3 Simulation path result

4 結(jié)束語

仿真結(jié)果表明，疊加應(yīng)用人工勢(shì)場(chǎng)法和速度障礙法可以應(yīng)用于同時(shí)存在動(dòng)態(tài)與靜態(tài)障礙物的環(huán)境中，有效解決人工勢(shì)場(chǎng)法中部分極小值問題，并且能使路徑更為柔順，保證UUV能跟隨，算法運(yùn)算量小、實(shí)時(shí)性高，適合應(yīng)用于UUV局部避碰路徑規(guī)劃中。