基于改進(jìn)A*＋RDP 算法的無人艇回塢路徑規(guī)劃方法*

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

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

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

圖1 無人艇回收塢結(jié)構(gòu)示意圖

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

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

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

1 基于改進(jìn)A*＋RDP 算法的自動(dòng)回塢路徑規(guī)劃

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

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

基于改進(jìn)A*＋RDP 算法的自動(dòng)回塢路徑規(guī)劃主要流程如圖2 所示，主要包括柵格地圖生成、全局路徑規(guī)劃、RDP 算法優(yōu)化、軌跡曲線插值等四個(gè)部分。將自動(dòng)回塢區(qū)域的電子海圖生成柵格地圖數(shù)據(jù)，輸入A*全局路徑規(guī)劃算法獲取避障安全路徑，然后采用RDP 算法簡化路徑點(diǎn)，降低計(jì)算復(fù)雜度，最后采用曲線插值方法輸出平滑路徑導(dǎo)引回塢任務(wù)。

圖2 自動(dòng)回塢路徑規(guī)劃流程圖

1.1 柵格地圖生成

無人艇路徑規(guī)劃首先需要對環(huán)境進(jìn)行建模，因?yàn)榇d電子海圖包含了島嶼、礁石等海上障礙物信息，本文直接采用電子海圖生成路徑規(guī)劃所需的柵格地圖。電子海圖模型采用地理坐標(biāo)系統(tǒng)，不便于直接進(jìn)行路徑規(guī)劃，采用墨卡托投影方法[15]將地圖信息的經(jīng)緯度轉(zhuǎn)換為米制單位，轉(zhuǎn)換公式如下所示:

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

圖3 地圖處理示意圖

1.2 全局路徑規(guī)劃

全局路徑規(guī)劃的目標(biāo)是在離散的柵格化地圖，規(guī)劃出一條滿足碰撞約束、艏向約束、安全距離約束的無人艇路徑。本文以A*算法作為全局路徑規(guī)劃算法的基礎(chǔ)，研究以無人艇為規(guī)劃對象的A*改進(jìn)算法。傳統(tǒng)的A*算法節(jié)點(diǎn)搜索策略將規(guī)劃對象視為質(zhì)點(diǎn)，不考慮規(guī)劃對象的運(yùn)動(dòng)學(xué)約束，節(jié)點(diǎn)搜索方向?yàn)猷徑? 個(gè)柵格，即規(guī)劃對象不受轉(zhuǎn)向約束，節(jié)點(diǎn)搜索示意如圖4(a)所示。本文的研究對象為無人艇，搜索策略應(yīng)該考慮無人艇的艏向，并根據(jù)其轉(zhuǎn)向角度限制范圍進(jìn)行搜索，考慮約束的搜索示意如圖4(b)所示。除此之外，傳統(tǒng)的A*算法不考慮規(guī)劃對象的物理尺寸和安全距離約束，但是無人艇在海上行駛，應(yīng)遵守安全距離約束，如圖4(c)所示，需考慮橫向安全距離和縱向安全距離。

圖4 改進(jìn)A*算法工作原理

A*算法中每個(gè)節(jié)點(diǎn)都存儲了自身的鍵值f，鍵值決定了節(jié)點(diǎn)在訪問列表中的優(yōu)先級，鍵值越低意味著代價(jià)越小，訪問優(yōu)先級高。節(jié)點(diǎn)n的鍵值計(jì)算表達(dá)式f(n)如下式所示:

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

式中，(x，y)代表當(dāng)前點(diǎn)坐標(biāo)，(xgoal，ygoal)代表目標(biāo)點(diǎn)坐標(biāo)。

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

①啟發(fā)式函數(shù)h(n)改進(jìn)

采用歐氏距離作為啟發(fā)式函數(shù)時(shí)，如果當(dāng)前節(jié)點(diǎn)位置下無人艇的偏航角并不是朝著目標(biāo)節(jié)點(diǎn)，此時(shí)的代價(jià)估計(jì)值應(yīng)該更大，也就說歐氏距離并不能反映出無人艇艏向角度的影響。本文在啟發(fā)式函數(shù)基礎(chǔ)上額外考慮了艏向角偏差的加權(quán)值，如下式所示:

式中，θ為無人艇的艏向角，θgoal為無人艇指向目標(biāo)點(diǎn)的艏向角，β為權(quán)重因子，取值0.2。基于改進(jìn)后啟發(fā)式函數(shù)的A*算法原理示意如圖5 所示。

圖5 改進(jìn)啟發(fā)式函數(shù)算法原理

②安全避障約束改進(jìn)

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

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

1.3 RDP 算法優(yōu)化

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

1.4 軌跡曲線插值

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

以最小化總體軌跡的Snap 值之和為優(yōu)化目標(biāo)，將每段Bezier 曲線的時(shí)間取值歸一化到[0，1]區(qū)間，總路徑軌跡的Snap 值之和為:

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

因此，基于全局路徑點(diǎn)的Bezier 曲線參數(shù)求解問題可轉(zhuǎn)化為如下約束優(yōu)化模型:

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

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

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

等式約束保證了航跡滿足連續(xù)性約束，不等式約束保證航跡滿足運(yùn)動(dòng)學(xué)參數(shù)的約束。本文的運(yùn)動(dòng)學(xué)約束主要考慮無人艇的航速和加速度約束，如下所示:

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

2 仿真結(jié)果

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

表1 無人艇路徑規(guī)劃仿真參數(shù)

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

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

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

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

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

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

表2 對比算法仿真結(jié)果

從表2 可見:①A*算法規(guī)劃的路徑長度比RRT*算法更短；改進(jìn)的A*＋RDP 算法相比原始A* 算法的路徑長度下降約4%，改進(jìn)的RRT*＋RDP 算法相比原始RRT*算法的路徑長度下降約9%；②A*算法的路徑點(diǎn)數(shù)量少于RRT*算法的路徑點(diǎn)數(shù)量，改進(jìn)后的A*＋RDP 算法和RRT*＋RDP算法，路徑點(diǎn)數(shù)量下降約84%。上述實(shí)驗(yàn)結(jié)果表明，RRT*算法的路徑隨機(jī)性大、質(zhì)量差，容易生成彎折的路徑，且有碰撞風(fēng)險(xiǎn)，不利于后端的軌跡生成。改進(jìn)后的A*＋RDP 算法相較于傳統(tǒng)A*算法，增加了艏向約束和安全距離約束，無碰撞風(fēng)險(xiǎn)，同時(shí)更接近路徑較短的優(yōu)化目標(biāo)。

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

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

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

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

表3 仿真結(jié)果數(shù)據(jù)

3 結(jié)語

本文通過分析無人艇回塢路徑規(guī)劃的優(yōu)化目標(biāo)和約束條件，提出了一種全局路徑規(guī)劃和軌跡生成算法相結(jié)合的無人艇回塢路徑規(guī)劃算法。以改進(jìn)A*搜索算法為基礎(chǔ)，結(jié)合RDP 算法優(yōu)化路徑點(diǎn)數(shù)量，以路徑長度和Snap 最小為優(yōu)化目標(biāo)，生成平滑Bezier 曲線軌跡。仿真結(jié)果驗(yàn)證了本文提出的改進(jìn)A*和RDP 算法對全局路徑的優(yōu)化效果，說明基于多約束優(yōu)化模型生成Bezier 曲線作為無人艇回塢路徑具有可行性。