張嘯天，陳熙源

(東南大學(xué)儀器科學(xué)與工程學(xué)院，江蘇 南京 210096)

水面無人艇(unmanned surface vehicle，USV)，是一種無人操作的帶有多類型傳感器的水面艦艇，具有體積小、全天時(shí)、低成本的優(yōu)點(diǎn)。在配備先進(jìn)的控制系統(tǒng)、傳感器系統(tǒng)和通信系統(tǒng)的情況下，可用于勘探巡邏和測繪等任務(wù)，擁有著廣闊的應(yīng)用前景[1]。

路徑規(guī)劃是水面無人艇的最基本的關(guān)鍵技術(shù)之一，它的目的是通過接收傳感器所得到的信息，結(jié)合無人艇的運(yùn)動(dòng)狀態(tài)，確定USV 航行的最佳軌跡，并在USV 實(shí)際行駛過程中，根據(jù)實(shí)際位置與預(yù)設(shè)位置的誤差，不斷修正受到外界因素影響的行進(jìn)路線，以達(dá)到相關(guān)的任務(wù)要求。路徑規(guī)劃算法的優(yōu)劣，不僅決定了其自主性水平，而且也是圓滿實(shí)現(xiàn)任務(wù)的前提[1-2]。

無人艇的路徑規(guī)劃從廣義上屬于機(jī)器人路徑規(guī)劃的一種。目前主流的路徑規(guī)劃算法可以分為三類:基于圖搜索的路徑規(guī)劃方法，基于采樣的路徑規(guī)劃方法以及基于智能算法的路徑規(guī)劃算法。其中基于隨機(jī)采樣原理的快速搜索隨機(jī)樹(RRT)算法由于無需對(duì)環(huán)境進(jìn)行具體建模，以及對(duì)于具有微分約束和非線性動(dòng)力學(xué)的系統(tǒng)有效的優(yōu)點(diǎn)備受研究者的關(guān)注[3]。由于RRT 算法產(chǎn)生的路徑不具備最優(yōu)性，Karaman 等提出了RRT*算法，在原有算法的基礎(chǔ)上加入了重布線過程，使算法具備漸進(jìn)最優(yōu)性[4]。廖彬等[5]提出了一種F-RRT*算法，通過隨機(jī)創(chuàng)建父節(jié)點(diǎn)以及引入三角不等式對(duì)路徑成本進(jìn)行優(yōu)化。Naderi[6]提出了一種RT-RRT*，通過實(shí)時(shí)構(gòu)建地圖，對(duì)搜索樹進(jìn)行更新，實(shí)現(xiàn)RRT*的動(dòng)態(tài)規(guī)劃。以上改進(jìn)均針對(duì)RRT*算法的搜索效率進(jìn)行提升，進(jìn)而匹配應(yīng)用場景。

無人艇的應(yīng)用場景存在影響載體運(yùn)動(dòng)的風(fēng)浪流，但目前無人艇的路徑規(guī)劃研究主要局限在靜水環(huán)境，較少考慮水流影響。侯春曉等[7]提出了應(yīng)用于內(nèi)河無人船的一般曲線路徑的LOS 循跡控制算法，能夠較好地處理內(nèi)河中漲潮和落潮時(shí)的路徑規(guī)劃問題，當(dāng)處于不同水流條件時(shí)規(guī)劃的路徑存在較大差別。吳博等[7]提出了一種基于速度障礙的自動(dòng)避碰算法，該算法考慮了無人艇與風(fēng)浪流的相對(duì)速度，并分析其相對(duì)速度與位置的角度關(guān)系，從而進(jìn)行路徑規(guī)劃?？偟膩碚f，目前解決水流影響的基本手段是將無人艇運(yùn)動(dòng)與水流運(yùn)動(dòng)進(jìn)行合成，再引入具體路徑規(guī)劃算法進(jìn)行求解。

全局路徑規(guī)劃通常需要根據(jù)電子海圖中海域中的障礙、危險(xiǎn)區(qū)域信息，建立包含可以確保船只航行必要信息的環(huán)境模型[9-10]，接著從建立的模型中找出一條能滿足各種航行指標(biāo)的安全路徑[11-12]。與全局路徑規(guī)劃不同，水面無人艇局部路徑規(guī)劃算法需要根據(jù)艇上多類型傳感器實(shí)時(shí)感知周圍環(huán)境中的信息，根據(jù)距離、風(fēng)浪等因素動(dòng)態(tài)地修正局部路徑，確保無人艇航行路線的安全可靠[13-14]。

因此，要綜合考慮風(fēng)浪對(duì)于無人艇路徑規(guī)劃的影響，對(duì)于規(guī)劃算法進(jìn)行調(diào)整，同時(shí)結(jié)合預(yù)路徑，實(shí)時(shí)給出無人艇下一時(shí)刻的運(yùn)動(dòng)信息。本文采用基于IRRT*和DWA 的無人艇混合路徑規(guī)劃方法。全局路徑規(guī)劃方法選擇改進(jìn)的IRRT*算法。這種方法基于增量式搜索，特別適合大空間的路徑規(guī)劃問題[15]。局部路徑規(guī)劃方法選擇動(dòng)態(tài)窗口法(Dynamic Window Approach，DWA)，并在此基礎(chǔ)上結(jié)合海浪影響進(jìn)行改進(jìn)，能夠較好地符合無人艇的實(shí)際運(yùn)動(dòng)情況[16-17]。

1 全局路徑規(guī)劃

針對(duì)無人艇的路徑規(guī)劃，通常為了保證算法路徑規(guī)劃的實(shí)時(shí)性和有效性，分為全局路徑規(guī)劃和局部路徑規(guī)劃兩個(gè)部分進(jìn)行。全局路徑規(guī)劃通常首先通過電子海圖等環(huán)境信息，根據(jù)航行指令預(yù)規(guī)劃出一條全局路徑。在全局規(guī)劃的基礎(chǔ)上，無人艇通過自身傳感器探測裝置獲取周圍環(huán)境信息，結(jié)合無人艇具體的運(yùn)動(dòng)約束，再利用局部路徑規(guī)劃方法實(shí)現(xiàn)避障。這么做的好處是，通過預(yù)先花費(fèi)較長時(shí)間進(jìn)行全局路徑規(guī)劃得到全局路徑，避免了無人艇在局部避障過程中可能出現(xiàn)的盲目性，同時(shí)也能有效地減少局部路徑規(guī)劃算法所需要的時(shí)間，具體過程如圖1 所示。

圖1 無人艇路徑規(guī)劃流程

在進(jìn)行路徑規(guī)劃過程之前，通常需要對(duì)環(huán)境進(jìn)行建模，常用的方法包括柵格法、可視圖法和Voronoi 圖法，這個(gè)步驟對(duì)于其他路徑搜索算法往往是必要的。然而RRT*算法通過對(duì)狀態(tài)空間的采樣點(diǎn)進(jìn)行碰撞檢測，從而避免在狀態(tài)空間內(nèi)顯式地構(gòu)造障礙物，提供了大量的計(jì)算節(jié)省。

1.1 RRT*算法

為了解決機(jī)器人路徑規(guī)劃問題中存在非完整性約束或微分約束時(shí)，很難逐一對(duì)軌跡片段進(jìn)行求解的問題。LaValle 于1998 年提出了隨機(jī)采樣的RRT算法，他通過使用增量式的前向采樣，構(gòu)建一棵搜索樹，隨后再對(duì)樹結(jié)構(gòu)進(jìn)行搜索得到路徑。具體的流程為將起點(diǎn)位姿點(diǎn)加入隨機(jī)樹后，直接在空間中先隨機(jī)采樣一個(gè)位姿點(diǎn)，遍歷整個(gè)隨機(jī)樹中的節(jié)點(diǎn)并選出距離該隨機(jī)位姿點(diǎn)最近的節(jié)點(diǎn)，在滿足約束的前提下，以一定的步長向著隨機(jī)點(diǎn)方向生長，直至到達(dá)目標(biāo)區(qū)域。從目標(biāo)位姿點(diǎn)出發(fā)，依次尋找當(dāng)前位姿點(diǎn)的父節(jié)點(diǎn)直至回到起始位姿點(diǎn)，即可得到規(guī)劃路徑

RRT*算法是在RRT 算法基礎(chǔ)上提出的一種路徑優(yōu)化算法。在快速地找到初始路徑之后繼續(xù)進(jìn)行采樣，不斷地進(jìn)行優(yōu)化直到找到目標(biāo)點(diǎn)或者達(dá)到設(shè)定的最大循環(huán)次數(shù)。RRT*算法過程以偽代碼形式描述如下:

算法1 RRT*算法

雖然RRT*算法相較于RRT 算法能夠給出一條漸進(jìn)最優(yōu)的路線，但是在實(shí)際環(huán)境運(yùn)用時(shí)也存在一些缺點(diǎn)，例如最優(yōu)解收斂較慢，迭代次數(shù)以及設(shè)置的運(yùn)行時(shí)間會(huì)影響最后的路線等。

采樣算法的質(zhì)量與規(guī)劃算法的運(yùn)行效率有著密不可分的關(guān)系，有效的采樣策略能夠節(jié)約大量的搜索時(shí)間。初始RRT*算法采取的策略是在地圖中直接進(jìn)行隨機(jī)采樣，會(huì)產(chǎn)生大量與最優(yōu)路徑明顯無關(guān)的采樣點(diǎn)，導(dǎo)致算法的實(shí)時(shí)性不佳。后續(xù)的研究者針對(duì)采樣算法進(jìn)行了改進(jìn)，例如Informed-RRT*(IRRT*)，在得到可行路徑后對(duì)采樣范圍進(jìn)行縮小，初始點(diǎn)xinit和目標(biāo)點(diǎn)xgoal設(shè)置為橢圓的焦點(diǎn)，焦距為Cmin，當(dāng)前的最優(yōu)路徑長度Cbest為橢圓長軸，建立橢圓作為新的搜索范圍，如圖2 所示。由于橢圓的性質(zhì)，橢圓內(nèi)的任意一點(diǎn)到兩個(gè)焦點(diǎn)的距離之和小于長軸長度，所以只對(duì)橢圓內(nèi)部進(jìn)行采樣，隨著采樣過程的進(jìn)行，路徑長度Cbest不斷縮短，橢圓采樣面積減小，能夠有效地有效提升收斂速度。采樣空間可以表示為:

圖2 IRRT*原理示意圖

1.2 改進(jìn)隨機(jī)采樣

在IRRT*的基礎(chǔ)上，為了進(jìn)一步提高搜索效率，降低算法的隨機(jī)性，在生成新節(jié)點(diǎn)xrand的過程中，引入啟發(fā)式函數(shù)，引導(dǎo)新節(jié)點(diǎn)朝向xgoal生長，減小了規(guī)劃時(shí)間。

這里的Pr是位于(0，1)的隨機(jī)數(shù)，α是給定的常數(shù)。LineTo(xgoal) 指在當(dāng)前節(jié)點(diǎn)與xgoal的連線上進(jìn)行采樣；Random(χ)指在地圖上隨機(jī)采樣；當(dāng)目標(biāo)點(diǎn)xgoal已經(jīng)包含在隨機(jī)樹中時(shí)，xrand會(huì)使用Ellipse(xinit，xgoal)在上文提到的橢圓內(nèi)部進(jìn)行采樣。因此，該采樣算法能夠更有效地生成新節(jié)點(diǎn)，這也意味著采樣時(shí)間得到了減少，算法的實(shí)時(shí)性得到提高。

為了評(píng)估啟發(fā)式IRRT*算法相較于傳統(tǒng)RRT*算法的改進(jìn)，進(jìn)行仿真對(duì)比，如圖3 所示，同時(shí)將尋找路徑所需節(jié)點(diǎn)數(shù)和路徑長度列入表1。

圖3 3 種RRT*路徑規(guī)劃結(jié)果

表1 不同RRT*路徑長度和時(shí)間對(duì)比

模擬環(huán)境地圖尺寸為500 m×500 m，設(shè)置無人艇路徑規(guī)劃起點(diǎn)位于左上角，終點(diǎn)位于右下角，坐標(biāo)分別為(25，25)和(475，475)，白色和黑色分別代表可通行區(qū)域和不規(guī)則障礙物。通過傳統(tǒng)RRT*算法所獲得的路徑如圖中實(shí)線所示，所需的擴(kuò)展節(jié)點(diǎn)為1 450 個(gè)，路徑長度為733.32 m。使用IRRT*算法獲得路徑如圖中點(diǎn)劃線所示，所需的擴(kuò)展節(jié)點(diǎn)為613 個(gè)，路徑長度為722.09 m，而加入了啟發(fā)式函數(shù)的IRRT*算法所得路徑如圖中虛線所示，所需的擴(kuò)展節(jié)點(diǎn)為526 個(gè)，路徑長度為701.31 m。

可以看出，使用啟發(fā)式IRRT*算法能夠減少所需擴(kuò)展節(jié)點(diǎn)數(shù)量，由于RRT*算法主要耗時(shí)在碰撞檢測過程，而每個(gè)新生成的節(jié)點(diǎn)都需要調(diào)用該過程，因此可減少碰撞檢測部分，以降低算法所需時(shí)間。根據(jù)式(2)，控制采樣算法的一個(gè)關(guān)鍵參數(shù)α，調(diào)整該參數(shù)能夠改變采樣算法對(duì)于目標(biāo)點(diǎn)的趨向，這里的α＝0.3。與IRRT*算法相比，引入啟發(fā)式函數(shù)后，所需節(jié)點(diǎn)減少了14%。因此，當(dāng)無人艇在寬闊水面進(jìn)行預(yù)規(guī)劃時(shí)，啟發(fā)式IRRT*算法能夠有效地給出一條全局路徑。

2 動(dòng)態(tài)窗口法

由于無人艇的航行環(huán)境往往較為開闊且復(fù)雜，不但需要對(duì)地圖上已標(biāo)記的障礙物進(jìn)行規(guī)劃，也需要通過無人艇所搭載的傳感器檢測到的動(dòng)態(tài)障礙物信息進(jìn)行實(shí)時(shí)躲避，而局部路徑規(guī)劃能夠較好地解決該問題。結(jié)合上文通過啟發(fā)式IRRT*算法給出的全局路徑，同時(shí)考慮到海流的影響對(duì)于局部路徑規(guī)劃方法進(jìn)行改進(jìn)，使得無人艇在運(yùn)行過程中能夠更好地對(duì)水面的情況做出及時(shí)的響應(yīng)。

動(dòng)態(tài)窗口法(Dynamic Window Approach，DWA)是一種基于速度采樣的局部規(guī)劃方法，在速度空間采樣多組速度，并模擬一定時(shí)間內(nèi)無人艇的軌跡，根據(jù)預(yù)設(shè)的評(píng)價(jià)函數(shù)來比較這些軌跡的優(yōu)劣，從中選取最優(yōu)軌跡對(duì)應(yīng)的速度執(zhí)行。動(dòng)態(tài)窗口的具體含義是根據(jù)載體的運(yùn)動(dòng)特性，將其速度采樣空間限制在一個(gè)可行的動(dòng)態(tài)范圍內(nèi)。

2.1 速度采樣

根據(jù)無人艇本身的限制和環(huán)境約束，可以將速度的采樣空間限定在一定范圍內(nèi)，如式(3)～式(5)所示。

式中，vmin、vmax、ωmin、ωmax分別表示速度和角速度的最小值和最大值，vc、ωc表示當(dāng)前的速度、角速度，表示加速度和角加速度的最大值。

同時(shí)基于無人艇運(yùn)動(dòng)安全的考慮，需要在碰到障礙物前停下來，因此速度和角速度必須滿足:

式中，dist(v，ω)表示在速度(v，ω)所對(duì)應(yīng)軌跡上與障礙物最近的距離。

2.2 考慮流場評(píng)價(jià)函數(shù)

對(duì)于動(dòng)態(tài)窗口法采樣得到多組速度后，根據(jù)預(yù)設(shè)的時(shí)間窗口和前向模擬時(shí)間，模擬得到對(duì)應(yīng)的多組軌跡，從中剔除不安全的軌跡，計(jì)算其他軌跡的評(píng)價(jià)函數(shù)。

式中，G(v，ω)表示處于當(dāng)前速度對(duì)應(yīng)軌跡的總評(píng)價(jià)函數(shù)值，hea(v，ω)用于評(píng)價(jià)當(dāng)前航向與目標(biāo)點(diǎn)方向之間的偏差，dis(v，ω)用于評(píng)價(jià)當(dāng)前軌跡上各點(diǎn)與障礙物之間的最小距離，vel(v，ω)用于評(píng)價(jià)當(dāng)前速度的大小，wav(v，ω)用于評(píng)價(jià)當(dāng)前軌跡與全局規(guī)劃得到軌跡的偏離程度，α、β、γ、η分別為各項(xiàng)參數(shù)。

區(qū)別于移動(dòng)機(jī)器人，無人艇路徑規(guī)劃問題中不能忽略流場的影響?？紤]流場的無人艇路徑規(guī)劃方法的優(yōu)勢(shì)主要在于，基于無人艇體積小續(xù)航能力不強(qiáng)的前提，利用流場有效地減少行駛耗能。流場類型主要分為兩種:一種流速和流向相對(duì)固定的內(nèi)河流場，另一種則是時(shí)變的海洋流場。為了反映洋流擾動(dòng)的實(shí)際情況，其數(shù)學(xué)模型如下:

在式(9)中，采用與時(shí)間相關(guān)的無維度函數(shù)B(t)來表征曲流振幅的振蕩，且B(t)＝B0＋εcos(ξt＋ψ)，其中B0、ε、ξ、ψ、k、c是用來表征時(shí)變流場的參數(shù)。因此，在某時(shí)刻t，無人艇當(dāng)前位置所在流場的水流流速vs在直角坐標(biāo)系下的投影vsx和vsy為:

在無人艇路徑規(guī)劃中，水流的影響應(yīng)當(dāng)從流向和流速兩方面考慮，流場對(duì)于無人艇的影響體現(xiàn)在其航行速度上。因此為了綜合考慮海流對(duì)于無人艇運(yùn)動(dòng)的影響，本文在傳統(tǒng)動(dòng)態(tài)窗口法基礎(chǔ)上引入了海流評(píng)價(jià)函數(shù)wav(v，ω)

式中，〈v，vs〉代表無人艇當(dāng)前速度v與流場的夾角，范圍為[0，π]。這里假定無人艇能夠克服流場帶來的不良影響，滿足:

同時(shí)，為了避免流場評(píng)價(jià)函數(shù)wav(v，ω)的某項(xiàng)在評(píng)價(jià)函數(shù)太占優(yōu)勢(shì)，也需要對(duì)其進(jìn)行歸一化處理:

2.3 改進(jìn)DWA 算法仿真

為了驗(yàn)證改進(jìn)動(dòng)態(tài)窗口法的效果，進(jìn)行對(duì)比仿真實(shí)驗(yàn)，分別對(duì)原始DWA 算法和考慮流場的DWA算法進(jìn)行驗(yàn)證，仿真結(jié)果如圖4 和圖5 所示。

圖4 不考慮流場的DWA 仿真

圖5 考慮流場的DWA 仿真

兩種方法驗(yàn)證實(shí)驗(yàn)初始設(shè)置均相同，航行環(huán)境設(shè)為50 m×50 m，圖中的黑色圓點(diǎn)代表障礙物位置，障礙物半徑設(shè)為2 m，箭頭代表流場，其中箭頭長短代表流速大小，箭頭方向代表流速方向。起始點(diǎn)設(shè)為左下角，坐標(biāo)為(5，5)；目標(biāo)點(diǎn)為右上角，坐標(biāo)為(45，45)，曲線代表無人艇實(shí)時(shí)航行軌跡。

對(duì)比分析圖4、圖5 軌跡，相較于傳統(tǒng)動(dòng)態(tài)窗口法僅考慮路徑最短，可以明顯看出考慮流場的動(dòng)態(tài)窗口法能夠使無人艇在行駛過程中盡可能避開逆流場方向，在條件允許的情況下選擇沿流場方向。雖然此方法規(guī)劃的路徑長度相比傳統(tǒng)DWA 算法更長，但是能夠在成功避障的基礎(chǔ)上，考慮到能量優(yōu)化目標(biāo)，計(jì)算出一條高質(zhì)量的路徑。

3 混合路徑規(guī)劃

雖然動(dòng)態(tài)窗口法在局部避障時(shí)具有良好性能，但在實(shí)際運(yùn)行中規(guī)劃的路徑極易出現(xiàn)缺乏目的性的情況，基于此本文在原有動(dòng)態(tài)窗口法的基礎(chǔ)上將改進(jìn)IRRT*生成的全局路線作為參數(shù)輸入到DWA中，作為局部目標(biāo)點(diǎn)，以此保證規(guī)劃器得到的路線既有良好的避障性能，也不會(huì)出現(xiàn)抵達(dá)不了目標(biāo)點(diǎn)的情況。

為了驗(yàn)證混合路徑規(guī)劃算法的可行性，利用實(shí)際場景進(jìn)行仿真實(shí)驗(yàn)，圖6 是海域的衛(wèi)星地圖，對(duì)其進(jìn)行二值化處理得到地圖如圖7 所示，白色代表可航行區(qū)域。

圖6 無人艇航行區(qū)域衛(wèi)星地圖

圖7 二值化地圖

仿真航行區(qū)域?yàn)?12 m×400 m，起始點(diǎn)坐標(biāo)為(360，300)，目標(biāo)點(diǎn)為(45，80)。無人艇的最大速度設(shè)為10 m/s，最大加速度為2.5 m/s，最大角速度為20°/s，水流流向?yàn)橛移?5°。

如僅采用DWA 算法進(jìn)行路徑規(guī)劃，結(jié)果如圖8所示，由于行駛路徑較為復(fù)雜，算法易陷入局部最小“陷阱”，導(dǎo)致任務(wù)失敗。

圖8 僅使用DWA 路徑規(guī)劃失敗

采用改進(jìn)IRRT*給出一條預(yù)規(guī)劃路徑，如圖9折線所示。路徑長度為675.018 4 m。在全局路徑的基礎(chǔ)上，使用DWA 算法進(jìn)行動(dòng)態(tài)驗(yàn)證。規(guī)劃航行結(jié)果如另一曲線所示，長度為726.167 5 m。

圖9 基于全局路徑的混合路徑規(guī)劃結(jié)果

仿真結(jié)果能夠很好體現(xiàn)混合路徑規(guī)劃的優(yōu)勢(shì)，將改進(jìn)的IRRT*算法和DWA 算法相結(jié)合，能克服局部規(guī)劃缺乏目的性，易陷入局部最優(yōu)的問題，也能實(shí)現(xiàn)根據(jù)無人艇具體的運(yùn)動(dòng)學(xué)約束和實(shí)時(shí)的障礙物信息進(jìn)行避障的要求，更有利于USV 在復(fù)雜水面條件下實(shí)現(xiàn)路徑規(guī)劃。

4 結(jié)論

分別對(duì)路徑規(guī)劃算法中的全局路徑規(guī)劃算法和局部路徑規(guī)劃算法進(jìn)行改進(jìn)，提出了改進(jìn)IRRT*的全局路徑規(guī)劃算法和考慮流場的DWA 算法。本文采用的改進(jìn)IRRT*算法與傳統(tǒng)IRRT*算法相比，搜索節(jié)點(diǎn)減少了14%，有效提升了搜索效率。考慮流場的DWA 算法在有流場情況下能夠較好地利用流場影響，規(guī)劃出相對(duì)節(jié)能的局部路徑。將兩種方法聯(lián)合使用，通過混合路徑規(guī)劃仿真驗(yàn)證，能夠?qū)崿F(xiàn)無人艇在行進(jìn)時(shí)對(duì)路徑進(jìn)行修正，安全完成任務(wù)。