近海復雜環境下UUV動態路徑規劃方法研究

張宏瀚，王亞博，李娟，王元慧，嚴浙平

（哈爾濱工程大學 智能科學與工程學院, 黑龍江 哈爾濱 150001）

水下無人航行器（unmanned underwater vehicle，UUV）在水下搜救、地形勘探、探測資源等領域有著重要的地位[1]，而自主路徑規劃是UUV的核心技術之一，其決定著UUV執行任務的效率和安全性[2-3]。無人智能系統的路徑規劃分為全局路徑規劃和局部動態規劃兩部分，前者基于不完整地圖快速搜尋一條可達任務目標點的可行路徑[4-9]；后者則是利用智能系統的傳感器對周圍環境的感知及時避開全局路徑上的未知障礙物[10-11]。然而，近海環境下大量礁石、人工建筑物以及航行的漁船等因素致使UUV的航行情況變得異常復雜，對路徑規劃的要求也相應提高[12]。當UUV停泊港內需執行港外近海任務時，若其具備近海復雜環境下的動態路徑規劃能力，在抵達任務區域過程中則不需要借助大型漁船及起吊裝置，將極大的節省人力物力，對完善UUV功能有著重要的工程意義。

針對動態環境的路徑規劃問題，Chowdhury等[13]提出了一種PRM-A*算法，該算法將PRM算法和A*算法聯合規劃路徑，解決了路徑不平滑以及動態障礙物的避碰問題。趙旭等[14]提出了一種改進的A*算法，此算法將A*算法的搜索區域改為UUV可達的扇形區域，并且在路徑拓展階段以運動學為約束，實現了未知環境的路徑規劃問題。He等[15]提出了一種改進的蟻群算法，引入虛擬可視化概念克服了蟻群只能在相鄰網格行走的限制，使用非線性懲罰機制避免路徑出現大的轉角。李楊等[16]在遇到動態障礙物時將人工勢場法與速度障礙法相結合，根據障礙物的大小以及運動信息使UUV合理避障。

RRT(rapid-exploration random tree）算法由Lavall等[17]提出，被廣泛用于解決路徑規劃問題，但是RRT算法存在拓展盲目性、路徑冗余度高、路徑不平滑等問題，主要的改進方向有增加算法的導向性和提高的路徑可行性[18-20]。劉成菊等[21]提出的RRT改進算法在拓展階段以一定概率選擇目標點，采樣的其他點則增加引力分量使其偏向目標點，同時使用路徑緩存策略以及動態擴展策略解決動態避碰問題。尹高揚等[22]在節點拓展階段添加動力學約束、航跡距離約以達到增加路徑的可行性以及縮短距離的作用。上述提出的算法在靜態環境下效果較好，但是存在計算開銷大、不適用于近海環境存在動態障礙物的路徑規劃問題。

為了解決近海復雜環境下的動態路徑規劃問題，本文提出一種改進RRT和動態窗口法融合的動態避碰算法AGT-RRT(adaptive guidance target RRT)。首先對RRT算法進行改進以加快全局路徑的規劃速度，然后在得到的全局路徑基礎之上，使用自適應子節點選取策略獲取DWA（dynamic window approach）算法的子目標點，將難以實現的全局動態任務規劃分解成多個簡單的動態規劃任務，實現動態避碰功能的同時增加DWA算法的導向性。最后，通過仿真驗證了算法的有效性。

1 建立UUV模型

近海環境下由于水位較淺，為了簡化系統模型，本文只討論UUV的水平方向運動，即UUV的縱蕩、橫蕩、艏搖3個自由度。

1.1 建立UUV運動學模型

式中：ψ為UUV的艏向角，r、u、v分別為UUV的角速度、橫向線速度、縱向線速度，x、y分別為UUV在水平位置坐標。

1.2 建立UUV動力學模型

式中：

其中：m為UUV的自身質量，Xu˙、Yv˙、Nr˙為UUV的附加質量，τ1、τ3為UUV推進器提供的推力以及提供的轉向力矩，Jz為轉動慣量。

1.3 建立UUV聲吶模型

引入文獻[23]中提到的Solstice聲吶模型，該聲吶為多孔徑測掃聲吶，當障礙物處于聲吶的200 m范圍內可以被其捕捉到，被捕捉到的障礙物和聲吶之間的距離關系如下：

式中：x0、y0分別為UUV在水平位置坐標，x、y分別為障礙物在水平位置坐標，R為聲吶有效工作范圍。

1.4 建立UUV自身物理約束模型

UUV在運動過程中由于自身性能影響，存在最大速度、轉向角的限制：

式中：Vs為速度對(v,r)的約束范圍，vmin、vmax為UUV速度v的最大最小值，rmin、rmax為r的最大最小值。

2 全局路徑規劃算改進

2.1 自適應目標引導策略

本小節介紹為增加隨機樹生長的方向性設計的自適應目標引導策略。在任務空間V中隨機獲得一個采樣點，首先計算采樣點與終點之間的距離d，然后將距離d乘以比例因子p得到一個距離d′，之后將采樣點xrand朝著xrand與xgoal的矢量方向移動d′，得到一個新的采樣點，其計算公式如下：

這樣此算法不僅使拓展的方向偏向xgoal，而且樹空間T中更加靠近xgoal的節點被優先被選為拓展點，加快了算法收斂的速度。

比例因子如果設置的過大，在復雜任務環境容易陷入局部極小值（見圖1（a）），此時比例因子為0.3，即使多次迭代也不能擺脫。因此本文采用自適應比例因子。在算法開始迭代之前賦予比例因子一個初始值，然后根據每次迭代成功或是失敗修改比例因子，以加快收斂速度，防止陷入局部極小值，其計算公式如下：

圖1 算法改進前后對比Fig.1 Comparison chart before and after improvement

迭代成功：

迭代失?。?/p>

式中：nsuccess、nfail分別為成功迭代次數以及失敗迭代次數，α、β、χ、δ分別為權重，p0為比例因子p的初始值。改進后拓展圖如圖1(b)所示。

2.2 避碰策略

觀察算法規劃過程發現采樣點遠大于隨機樹節點數量，這是由于在拓展階段發生多次碰撞，為了進一步加快收斂速度，本文采用避碰策略。

當算法發生碰撞時不會直接放棄此條路徑而是以朝向的矢量方向為軸線向周圍輻射出一個扇形區域，在此扇形區域以一定間隔生成多條路徑，如果其中有無碰路徑，則按照此方向相應的進行拓展。

在采用轉向避碰策略之后如果仍然不能成功拓展則表示此節點大概率陷入局部極小值，為了逃離局部極小值本文采用重選節點避碰策略。在拓展的階段放棄作為拓展的隨機節點，選用之前生成的xrand作為拓展的節點，則使拓展的方向偏離目標點，可以快速逃離局部極小值。

圖2中給出了避碰策略的有效性，灰色、洋紅色、藍色曲線分別表示正常拓展、轉向避碰策略后拓展、重選節點避碰策略后的路徑。從圖中可以發現，隨機樹在發生碰撞之后可以根據節點所處的不同情況選取不同的策略，在隨機樹朝向目標點生長時如果陷入局部極小值，可以使用設計的策略快速逃離，達到了設計的目的。

圖2 避障策略仿真Fig.2 Simulation diagram of obstacle avoidance strategy

3 局部避碰算法設計

本文采用DWA算法實現局部路徑規劃，但是DWA算法僅能在窗口大小的范圍內規劃路徑，極易陷入局部最小值。為了解決這個問題，本文采用自適應子節點選取策略和重規劃策略。

3.1 DWA模型設計

1) DWA規劃UUV下一時刻狀態方程如下：

式中：x=[xyψvr]T，V為速度對[vr] 。

2)制動距離計算，當制動距離Ddis大于UUV與障礙物的距離則舍棄相應的路徑，公式如下：

其中amax為UUV的最大加速度。

3)評價函數計算，評價函數主要由艏向角得分、安全距離得分以及速度得分3部分構成。為了防止不同評價體系單差異帶來的異常，對3種評價體系歸一化處理，具體公式如下：

其中Ei為每條軌跡的參數。最后可得評價函數如下：

其中：e為評價函數得分，a、b、c分別為艏向角的分系數、安全距離的分系數、速度得分系數。

3.2 自適應子節點選取策略

在未知路徑彎曲度時，固定距離子節點易導致DWA算法陷入局部最小值，因此本文提出自適應子節點選取策略。首先，初始長度dinit，在全局路徑上選取距離當前位置一個初始長度的子節點xchild，之后計算當前位置和子節點在全局路徑上的距離d1與當前位置到子節點的直線距離d2的比值k，如果比值大于設置的閾值κ表示全局規劃的路徑較為曲折，則縮短子節點的選取距離，直至小于設置的閾值；如果比值小于設置的閾值κ則表示路徑較為平滑，則增加子節點的選取距離，直至大于設置的閾值。

3.3 重規劃策略

全局路徑規劃是基于不完整地圖信息規劃的路徑，在實際環境中可能會出現新的障礙物，當障礙物較小使用DWA即可避過障礙物，但是當障礙物過大，DWA算法可能會陷入局部極小值導致UUV無法通行。因此本文針對這種情況采用重規劃策略，當DWA算法陷入局部極小值時，則返回主程序中，把雷達掃描到的障礙物加入到地圖之中，然后以當前位置為起點重新規劃全局路徑，之后再按照之前設計的方案進行規劃路徑。

4 仿真與分析

4.1 AGT-RRT算法仿真驗證

AGT-RRT算法在運行時首先初始化隨機樹空間，初始化自適應比例因子p0為0.32，初始化計數器nsuccess、nfail為0，轉向避碰策略角度α為30°。在任務空間獲得采樣點xrand，執行目標引導策略獲得采樣點。若拓展成功則將其加入隨機樹空間；失敗則執行轉向避碰策略獲得采樣點xnew，若其成功拓展則將其加入隨機樹空間；失敗則執行重選節點策略，采用xrand作為拓展的節點，若其成功拓展則加入隨機樹空間，失敗則返回第一步在任務空間重新獲得采樣點。

本文設計了map1和map2驗證AGT-RRT算法的可行性。將經典RRT算法、文獻[11]中提出的AWA_RRT重算法、RRT-Connect算法與本文提出的算法作對比?？刂泼糠N算法的參數一致，步長均為30，AWA-RRT算法權重的初始值為0.5，AGT-RRT算法比例因子初始值為0.32，map1和map2的大小均為1 000×1 000，起止點為（50，50），目標點為（950，950）。由于每次規劃所需時間過少，為避免偶然性和便于對比分析，表1、2為1 000次規劃的總時間成本，路徑成本以及節點數量取1 000次規劃的平均值。

表1 map1 4種算法仿真數據Table 1 Map1 simulation data of four algorithms

圖3給出了4種算法在兩種環境下的仿真情況。map1環境較為簡單不存在局部極小值。本文算法可以使用轉向避障策略順利躲避障礙物。每次成功的拓展都會提高自適應目標引導策略比例因子的值，使得下一個采樣點更加接近目標點，加快了算法的收斂速度。對比觀察map1中路徑生成過程發現AGT-RRT算法拓展方向具有較好的引導性，基本沒有冗余節點，達到了設計的目的。

圖3 4種算法在不同環境的仿真圖Fig.3 Simulation diagrams of four algorithms in different environments

在存在局部極小值的map2中，對比算法增加許多無效拓展，而本文的算法可使用重選節點避碰策略繞過局部極小值。使用重選節點避碰策略表示環境較為復雜，則會降低自適應目標引導策略比例因子的值，以增強探索能力。map2中的AGT-RRT算法在應對局部極小值，產生的無效拓展較少，證明了設計的有效性。

對比表1和表2中數據發現，AGT-RRT算法在map1中時間成本相較于RRT、AWA-RRT、RRT-Connect算法分別減少94.1%、91.3%、72%，路徑成本也分別優化了18.1%、4.9%、15%。在環境更為復雜的map2中，時間成本分別減少79%、66.4%、40.8%，路徑成本相較于RRT算法和RRTConnect算法分別減少4.2%、5.4%，但是相較于AWA-RRT算法路徑成本增加了14.3%，節點數量減少78%。結合map2仿真環境和圖3中路徑觀察可知，這是由于本文算法在遇到障礙物優先執行轉向避障策略，加快算法的收斂速度所致。時間成本的減少，增加的路徑成本在可接受的范圍。觀察圖4和圖5箱線圖中的數據可以發現，所提出的算法有較少的離散點，這表明其魯棒性優于其他的3種算法，在不同的環境中有更強的適應能力，達到了設計的目的。

表2 map2 4種算法實驗結果Table 2 Map1 simulation data of four algorithms

圖4 map1 4種算法時間成本箱線圖Fig.4 Map1 four algorithms time cost boxplot

圖5 map2 4種算法時間成本箱線圖Fig.5 Map2 four algorithms time cost boxplot

使用AGT-RRT算法生成的路徑距離障礙物太近，轉向次數較多，不滿足實際路徑規劃的需求。本文將障礙物膨脹化保證路徑遠離障礙物，同時去除冗余節點減少路徑的轉向次數，使生成的路徑更為平滑，處理后的路徑如圖6所示。

圖6 去除冗余節點路徑（藍色）Fig.6 Remove redundant node path (blue)

4.2 動態路徑規劃仿真分析

為證明本文所設計算法的有效性，使用文獻[24]中所提出算法作對比仿真實驗。動態路徑規劃仿真結果如圖7所示，大小為500 m×500 m。固定變量，兩種算法均采用圖7中藍色折線作為全局路徑。二者算法參數見表3，其中d為對比算法特有的偏離函數權重，UUV性能參數見表4。路徑的初始位置為（50，50），終點為（470，470），初始方向角為0。

表3 DWA算法評價函數權重Table 3 Weights of DWA algorithms evaluation function

表4 UUV性能參數表Table 4 UUV performance parameters

圖7 動態路徑規劃仿真結果Fig.7 Simulation result of dynamic path planning

圖7中洋紅色曲線為動態規劃的路徑，其中圖7(a)和圖7(b)為map3和map4的仿真結果，左圖為對比算法，右圖為本文所設計算法。本文算法首先執行自適應子節點選取策略，根據路徑的曲折程度選取DWA算法的子目標點。自適應子節點選取策略將完整的動態規劃任務分解為多個子任務分別進行規劃，從而降低全局路徑的曲折度對動態規劃的影響。

為增加仿真結果的可靠性，分別對二者進行10次仿真實驗，取平均值作為結果，具體數值見表5。從圖7（a）中可以看出二者均可以較好的參考全局路徑執行動態路徑規劃任務，通過計算發現，二者平局路徑長度相差只有1%，但是本文所提出算法規劃的路徑速度提升了10.4%，具有較大的速度提升，加快了任務的執行速度。

表5 地圖3仿真數據Table 5 Map 3 simulation data

在map4中，本文所設計的算法借助自適應子節點選取策略可以較好地利用全局路徑進行動態路徑規劃，而對比算法丟失了全局路徑信息，超出任務邊界無法抵達目標點。map4 比map3中全局路徑轉折角大，任務目標點與航行器當前艏向角相差過大，而UUV為大慣性體轉向速度較慢，導致對比算法脫離全局路徑，造成最優評價路徑更加偏向任務目標點，使得規劃路徑超出任務范圍，無法完成近海復雜環境中的UUV出港任務，證明了本文算法的優越性。

4.3 出港任務仿真分析

仿真實驗任務為航行器自主動態規劃出港路徑，以抵達港外任務區域。地圖實際大小為400 m×400 m，像素大小為1 200×1 200，比例為1∶3。

在仿真實驗中，本文采用Bluefin-9型UUV參數[25]，見表4。在使用AGT-RRT規劃全局路徑時以UUV最大航速（3 m/s）為一個步長，DWA規劃路徑時取最大航行速度為3 m/s。首先，AGTRRT算法規劃出一條全局路徑，雖然在狹小空間存在一些無效拓展，但是使用避障策略可輕易擺脫局部極小值，達到設計目的，之后去除初步規劃全局路徑的冗余節點。獲得全局路徑之后執行動態路徑規劃任務，使用自適應子節點選取策略在全局路徑上不斷選取子節點作為DWA算法的目標點，將復雜的路徑規劃任務分解成多個簡單的動態路徑規劃任務。之后DWA算法按照順序依次以生成的子目標點作為每階段的目標點執行動態路徑規劃任務，最后獲得出一條符合運動學約束的路徑。

任務執行過程中UUV速度每秒變化曲線以及角速度每秒變化曲線如圖8和圖9所示，可以發現，速度出現的幾次較大變化是由于每當DWA算法抵達目標點就會逐漸降低自身速度，保證UUV可以停止在目標點，最大速度變化率為0.6 m/s2，沒有超過最大加速度；角速度出現較大變化是由于路徑出現較大轉角，最大角速度變化率為0.5 rad/s2，沒有超過最大角加速度的范圍?？偮窂介L度為421 m，平均速度為1.5 m/s，可以較快完成出港任務。速度以及角速度的變化在切換子目標點以及在較為急促轉彎時變化較為劇烈，其余規劃時間變化均較為平緩，符合UUV的運動特性，達到設計的要求。

圖8 速度變化率曲線Fig.8 Velocity change rate curve

圖9 角速度變化率曲線Fig.9 Angular velocity change rate curve

5 結束語

為了提高近海環境特別是港口環境中UUV自主路徑規劃的安全性、快速性和動態避障能力，本文結合全局和局部路徑規劃設計了一種算法。首先，分析RRT算法存在的不足提出一種AGT-RRT算法。針對RRT的拓展盲目問題提出了自適應目標引導策略，同時引入轉向機制提高RRT算法的采樣成功率，進一步提高全局路徑規劃的效率，最后對比仿真實驗驗證了算法的有效性。針對DWA算法難以在較為復雜的環境實現動態規劃任務，采用自適應子節點選取策略獲取DWA算法的子目標點，將難以實現的全局動態任務規劃分解為多個簡單的動態規劃任務，從而實現復雜環境下的動態路徑規劃，仿真實驗驗證了算法的有效性和實用性。然而本文算法只能利用當前位置及下一個子目標點的信息，無法做到全局信息的利用，導致有些地方路徑較長，速度變化不平滑，對這些問題的處理將是今后的研究工作。