基于改進A*＋RDP 算法的無人艇回塢路徑規劃方法*

張美燕，楊慶生，蔡文郁*

(1.浙江水利水電學院電氣工程學院，浙江 杭州 310018；2.杭州電子科技大學電子信息學院，浙江 杭州 310018)

無人艇(Unmanned Surface Vessel，USV)[1]具有廣泛的應用前景，在科研、環境監測、軍事用途、資源勘探和巡邏搜救等領域，無人艇已經成為國內外海洋裝備的研究熱點。無人艇一般隨母船執行任務，因此要求具備快速作業與安全回塢的能力。高效可靠的無人艇收放技術可以提高海上作業的效率[2]，圖1 所示為母船尾部外掛喇叭口狀桁架式回收塢方案，外掛式回收塢和母船的連接形式為柔性連接，即在母船兩側錨機和中間的拖帶絞車，共三根纜繩固定回收塢三個固定點，依靠無人艇自動回塢就可以實現無人艇的安全回收。

圖1 無人艇回收塢結構示意圖

目前關于無人艇自動回塢技術的研究不多[3-4]。2014 年，Joel 等[3]采用三維激光雷達實現對回塢碼頭建圖識別，應用于小型無人水面艦艇的自動回塢，但是具有三維激光雷達成本較高、點云數據建圖耗時過多等缺陷，因此不適用于動態回塢場景。2018 年，Yilmaz 等[4]研究了基于視距(Line of Sight，LOS)和純追蹤(Pure Pursuit，PP)的自動回塢導引策略，并提出優化模型預測控制器(Model Predictive Controller，MPC)和級聯PID 控制器的參數來優化無人艇的路徑跟隨性能。

除此之外，無人艇自主回塢的關鍵是要解決復雜海況下的路徑規劃問題[5-6]。無人艇的路徑規劃方法主要劃分為全局路徑規劃、局部路徑規劃和混合路徑規劃等幾種。基于搜索的全局路徑規劃方法將無人艇視為無動力學約束的質點，僅考慮空間約束，相關的研究比較成熟。具有代表性的路徑規劃方法——A*搜索算法[7]通過構建合適的啟發式函數，使得搜索過程往代價值更小的方向搜索，兼顧搜索效率和路徑質量。隨機采樣搜索算法RRT*(Rapidlyexploring Random Tree)[8]能夠快速找到可行的路徑，但是隨機采樣特性導致路徑質量不高。2006 年，Larson 等[9]將A*搜索算法用于無人艇的避障路徑規劃，在有限的時間內搜索最佳路徑；2014 年，Kim 等人基于改進的A*算法，即在A*算法的代價函數中增加曲率半徑代價，規劃更合理的無人艇路徑[10]。2019 年，Song 等[11]提出了一種改進的A*算法，結合路徑平滑算法規劃路徑，并于Springer 號無人艇驗證可行性。在路徑規劃的基礎上，考慮運動學和動力學等約束，如速度、方向、旋轉角速度、旋轉半徑(曲率)等，從而規劃出無人艇回塢的具體軌跡。

本文針對無人艇回塢路徑規劃問題，提出全局路徑搜索和軌跡生成相結合的路徑規劃方法，使用改進的A*啟發式搜索算法和拉默-道格拉斯-普克算法(Ramer-Douglas-Peucker，RDP)[12]計算全局路徑點，滿足無碰撞和艏向約束；考慮無人艇的運動學參數、軌跡連續性約束，采用貝塞爾曲線構建生成推力變化率(Snap)[13]最小的規劃路線，滿足安全回塢的需求。

1 基于改進A*＋RDP 算法的自動回塢路徑規劃

傳統的路徑規劃算法一般將路徑長度作為優化目標之一，即航跡的路徑總長度L(p)，本文也將航跡總長度作為優化目標之一[14]。無人艇在回塢階段需要與移動的回收塢進行對接，為了避免發生碰撞，保證船體和被救援人員的安全，無人艇控制系統必須保證推進力變化平滑，不存在推進力的突變。本文將總軌跡的Snap 值之和最小(即無人艇的推力變化率最小)也作為優化目標，因此本文研究的自動回塢路徑規劃優化目標如下所示:

式中，p表示路徑規劃算法得到的參考軌跡函數，di表示第i段航跡的長度，N段航跡組成的總軌跡長度為L(p)，Snapi表示第i段航跡的加速度變化率，N段航跡的Snap 總和為T(p)。

基于改進A*＋RDP 算法的自動回塢路徑規劃主要流程如圖2 所示，主要包括柵格地圖生成、全局路徑規劃、RDP 算法優化、軌跡曲線插值等四個部分。將自動回塢區域的電子海圖生成柵格地圖數據，輸入A*全局路徑規劃算法獲取避障安全路徑，然后采用RDP 算法簡化路徑點，降低計算復雜度，最后采用曲線插值方法輸出平滑路徑導引回塢任務。

圖2 自動回塢路徑規劃流程圖

1.1 柵格地圖生成

無人艇路徑規劃首先需要對環境進行建模，因為船載電子海圖包含了島嶼、礁石等海上障礙物信息，本文直接采用電子海圖生成路徑規劃所需的柵格地圖。電子海圖模型采用地理坐標系統，不便于直接進行路徑規劃，采用墨卡托投影方法[15]將地圖信息的經緯度轉換為米制單位，轉換公式如下所示:

上式表示經緯度坐標(λ，φ)對應的米制投影坐標為(x，y)，R表示地球半徑。經過墨卡托投影轉換，地圖信息的經緯度統一轉換成米制單位。除此之外，電子海圖中的靜態障礙物也需要映射到柵格地圖中。在實際工程中，柵格地圖以二維數組形式保存，每個柵格點處，障礙物區域對應數值為255，非障礙物區域數值為0。按上述方法進行柵格化處理，原始電子海圖如圖3(a)所示，處理后的柵格地圖如圖3(b)所示。

圖3 地圖處理示意圖

1.2 全局路徑規劃

全局路徑規劃的目標是在離散的柵格化地圖，規劃出一條滿足碰撞約束、艏向約束、安全距離約束的無人艇路徑。本文以A*算法作為全局路徑規劃算法的基礎，研究以無人艇為規劃對象的A*改進算法。傳統的A*算法節點搜索策略將規劃對象視為質點，不考慮規劃對象的運動學約束，節點搜索方向為鄰近的8 個柵格，即規劃對象不受轉向約束，節點搜索示意如圖4(a)所示。本文的研究對象為無人艇，搜索策略應該考慮無人艇的艏向，并根據其轉向角度限制范圍進行搜索，考慮約束的搜索示意如圖4(b)所示。除此之外，傳統的A*算法不考慮規劃對象的物理尺寸和安全距離約束，但是無人艇在海上行駛，應遵守安全距離約束，如圖4(c)所示，需考慮橫向安全距離和縱向安全距離。

圖4 改進A*算法工作原理

A*算法中每個節點都存儲了自身的鍵值f，鍵值決定了節點在訪問列表中的優先級，鍵值越低意味著代價越小，訪問優先級高。節點n的鍵值計算表達式f(n)如下式所示:

式中，g(n)為節點n到起始點的距離，h(n)代表搜索的啟發式函數。本文使用節點到目標點的歐氏距離作為啟發式函數，如下式所示:

式中，(x，y)代表當前點坐標，(xgoal，ygoal)代表目標點坐標。

本文研究的A*改進算法主要包括以下兩方面:

①啟發式函數h(n)改進

采用歐氏距離作為啟發式函數時，如果當前節點位置下無人艇的偏航角并不是朝著目標節點，此時的代價估計值應該更大，也就說歐氏距離并不能反映出無人艇艏向角度的影響。本文在啟發式函數基礎上額外考慮了艏向角偏差的加權值，如下式所示:

式中，θ為無人艇的艏向角，θgoal為無人艇指向目標點的艏向角，β為權重因子，取值0.2?；诟倪M后啟發式函數的A*算法原理示意如圖5 所示。

圖5 改進啟發式函數算法原理

②安全避障約束改進

如圖6(a)所示，在傳統的A*搜索策略中，只有當路徑點完全處于障礙物的坐標點時才判定為不可通行，這與實際情況并不相符，因此需要額外考慮安全約束。如圖6(b)所示，當前節點向鄰節點拓展搜索時，鄰節點向周圍的橫向安全距離D和縱向安全距離L范圍可到達的節點進行障礙物判斷:若有障礙物，則標記該鄰節點為不可通行，否則，標記為可通行。

圖6 考慮安全約束的搜索策略

1.3 RDP 算法優化

柵格地圖的精度越高，路徑中包含的離散路徑點數越多，但是過多的路徑點產生了矢量特征的冗余。拉默-道格拉斯-普克(RDP)矢量數據壓縮算法可以用較少的矢量數據保留原始矢量數據的特征。原始軌跡經過RDP 算法優化的路徑點保留了原始路徑的基本輪廓，但是減少了路徑點。因此，本文結合RDP 算法對全局路徑進行簡化，減少中間路徑點、增加直達點。

1.4 軌跡曲線插值

改進A*算法規劃的全局路徑是由直線段構成的路徑，不適合直接作為無人艇的參考航跡，因此需要進行平滑處理。本文利用Bezier 曲線[16]的凸包性質和導數性質，通過控制點約束各階Bezier 曲線，實現對各階軌跡施加不等式約束。伯恩斯坦形式的Bezier 曲線表達式如下:

以最小化總體軌跡的Snap 值之和為優化目標，將每段Bezier 曲線的時間取值歸一化到[0，1]區間，總路徑軌跡的Snap 值之和為:

式中，τ∈[0，1]，pi＝[pi0，pi1，pi2，…，pin]T為第i段Bezier曲線的多項式系數向量，Qi為對應第i段曲線的Q矩陣:

因此，基于全局路徑點的Bezier 曲線參數求解問題可轉化為如下約束優化模型:

式中，C為Bezier 曲線系數組成的向量，Aeq為等式約束矩陣，Aie為不等式約束矩陣，beq和bie分別是等式約束值和不等式約束值。Bezier 曲線的參數求解問題，轉化為二次規劃(Quadratic Programming，QP)問題[17]。為滿足該模型的參數變量求解，本文取n＝7 階Bezier 曲線。由于航跡必須通過全局路徑規劃的各路徑點，還需滿足起始航速、起始加速度等約束，構建等式約束矩陣如下所示:

式中，j表示第j段軌跡，l表示階次，μ表示分量，為歸一化系數表示起始點組成的向量表示l階的第i個控制點，定義如下:

除此之外，在各段Bezier 曲線的連接處必須滿足各階連續性約束，即兩段曲線的連接處的位置、航速和航速加速度保持連續，如下所示:

等式約束保證了航跡滿足連續性約束，不等式約束保證航跡滿足運動學參數的約束。本文的運動學約束主要考慮無人艇的航速和加速度約束，如下所示:

式中，v為無人艇航速，a為無人艇加速度。根據式(10)、式(12)和式(13)，將多段軌跡的約束寫成矩陣形式，代入等式約束矩陣Aeq和不等式約束矩陣Aie，式(9)所示的QP 問題可通過拉格朗日等經典方法[17]求解，求解每段軌跡的Bezier 曲線參數，最終得到平滑的回塢軌跡。

2 仿真結果

為了驗證本文算法的性能，選取了廣東省陽江市陽江港口的實際電子海圖場景，經度111.809 046 294 04，緯度21.712 751 661 48，利用MATLAB 軟件編程實現算法仿真。無人艇回塢路徑規劃主要參數設置如表1 所示。

表1 無人艇路徑規劃仿真參數

為了對比A*、RRT*、改進A*＋RDP、改進RRT*＋RDP 等算法性能，包括路徑質量(路徑長度、路徑點數量)和算法效率(算法執行時間)，不同起點和終點條件下的仿真結果如圖7～圖10所示。

圖7 A*和RRT*算法仿真對比(場景1)

圖8 A*算法仿真對比(場景2)

圖9 RRT*算法仿真對比(場景3)

圖10 改進A*和RRT*算法仿真對比(場景4)

通過多次仿真取均值，A*算法、改進的A*與RDP 結合算法、RRT*算法、改進RRT*和RDP 結合算法四種算法的數值結果對比如表2 所示。

表2 對比算法仿真結果

從表2 可見:①A*算法規劃的路徑長度比RRT*算法更短；改進的A*＋RDP 算法相比原始A* 算法的路徑長度下降約4%，改進的RRT*＋RDP 算法相比原始RRT*算法的路徑長度下降約9%；②A*算法的路徑點數量少于RRT*算法的路徑點數量，改進后的A*＋RDP 算法和RRT*＋RDP算法，路徑點數量下降約84%。上述實驗結果表明，RRT*算法的路徑隨機性大、質量差，容易生成彎折的路徑，且有碰撞風險，不利于后端的軌跡生成。改進后的A*＋RDP 算法相較于傳統A*算法，增加了艏向約束和安全距離約束，無碰撞風險，同時更接近路徑較短的優化目標。

軌跡曲線插值的仿真結果如圖11 和圖12 所示，藍色柵格點為A*搜索算法搜索過的區域，圖11(a)和圖12(a)為A*算法的路徑，可見A*算法在全局搜索階段得到一條無碰撞路徑；圖11(b)和圖12(b)為經過RDP 算法優化的路徑，RDP 算法減少了A*路徑的彎折，減少了路徑點數量；圖11(c)和圖12(c)為基于Snap 值最優的多項式軌跡，生成的軌跡經過RDP 算法的所有路徑點，而且軌跡間連接平滑。

圖11 軌跡曲線插值仿真結果(場景1)

圖12 軌跡曲線插值仿真結果(場景2)

圖11 和圖12 的仿真結果數據如表3 所示。從表3 可以看到，使用A*算法生成路徑點，然后生成貝塞爾軌跡，計算耗時最多，主要是因為A*算法生成的冗余路徑點較多，軌跡生成算法求解多段曲線的參數，計算效率較低。改進的A*＋RDP 算法生成路徑點，再生成貝塞爾軌跡，在兩種場景下的計算效率提高了約9%～13%。因此，驗證了本文提出的改進A*＋RDP 算法可以減少冗余的路徑點，提高軌跡生成算法的計算效率。

表3 仿真結果數據

3 結語

本文通過分析無人艇回塢路徑規劃的優化目標和約束條件，提出了一種全局路徑規劃和軌跡生成算法相結合的無人艇回塢路徑規劃算法。以改進A*搜索算法為基礎，結合RDP 算法優化路徑點數量，以路徑長度和Snap 最小為優化目標，生成平滑Bezier 曲線軌跡。仿真結果驗證了本文提出的改進A*和RDP 算法對全局路徑的優化效果，說明基于多約束優化模型生成Bezier 曲線作為無人艇回塢路徑具有可行性。