一種用于水下無人航行器的局部避碰路徑規(guī)劃方法研究

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

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

0 引言

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

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

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

1 算法執(zhí)行流程

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

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

2 具體實施

2.1 改進型人工勢場法

定義引力勢場為UUV與目標位置相對位置的函數(shù)：

式中：ka為正常數(shù)是引力勢場系數(shù)；ρgoal為UUV當前位置與目標點間的距離。

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

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

引力勢場的負梯度為UUV所受引力，方向由UUV指向目標點：

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

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

2.2 速度障礙法

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

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

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

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

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

合力F為來自目標引力、障礙物的斥力以及由速度障礙法計算的轉向力的疊加。

2.3 避碰危險度

將水平面的最近會遇距離（DCPA）和垂直面的到達最近會遇點的時間（HCPA）作為影響碰撞危險度最大的因素，各個因素的模糊模型設置如下：

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

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

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

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

2.5 人工勢場法與速度障礙法疊加

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

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

2.6 航路的柔順性修正

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

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

3 仿真結果

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

圖3 仿真路徑結果圖Fig.3 Simulation path result

4 結束語

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