無人艇航行規(guī)則及運(yùn)動(dòng)學(xué)約束下的改進(jìn)RRT軌跡規(guī)劃方法

文龍貽彬， 劉 友， 穆京京， 胡常青， 唐軍武

（1.青島海洋科學(xué)與技術(shù)試點(diǎn)國家實(shí)驗(yàn)室，青島266237；2.中國航天科技集團(tuán)有限公司，北京100048；3.北京航天控制儀器研究所，北京100039）

0 引言

目前，無人水面艇（以下簡稱無人艇）在國內(nèi)外軍事和民用領(lǐng)域的應(yīng)用日益增多，而無人艇能夠自主航行的關(guān)鍵則取決于路徑規(guī)劃技術(shù)［1］。其中，基于動(dòng)態(tài)環(huán)境中的實(shí)時(shí)避碰與軌跡規(guī)劃又是無人艇路徑規(guī)劃的關(guān)鍵所在。

在路徑規(guī)劃領(lǐng)域，RRT算法是一種應(yīng)用非常廣泛的方法。與其它方法相比，RRT算法有著無需對(duì)系統(tǒng)進(jìn)行建模、無需對(duì)搜索區(qū)域進(jìn)行幾何劃分、在搜索空間的覆蓋率高、搜索的范圍廣、可以盡可能的探索未知區(qū)域等優(yōu)點(diǎn)。但將其直接應(yīng)用于無人艇上也存在著諸多缺陷，如均勻隨機(jī)采樣降低算法收斂速度、生成的路徑不夠平滑、沒有考慮運(yùn)動(dòng)學(xué)約束等。因此，針對(duì)RRT算法在軌跡規(guī)劃上的應(yīng)用，國內(nèi)外學(xué)者提出了很多改進(jìn)方法。 Webb 等［2］提出了 Kinodynamic RRT*方法， 通過狀態(tài)方程進(jìn)行采樣，并構(gòu)建了一種控制器使規(guī)劃出來的路徑符合動(dòng)力學(xué)約束。Palmieri等［3］提出了Theta*-RRT方法，將Theta*與RRT方法相結(jié)合，提高了軌跡規(guī)劃的速度。Elbanhawi等［4］將車輛動(dòng)力學(xué)約束要求與B樣條曲線特性相結(jié)合，從而規(guī)劃出平滑軌跡。莊佳園等［5］提出了一種改進(jìn)的RRT算法用于無人艇的局部路徑規(guī)劃，通過引入抑制因子等手段，提高了算法速度。但上述方法均未考慮動(dòng)態(tài)障礙物。

針對(duì)以上問題，本文提出了雙層RRT算法。在第一層框架中考慮動(dòng)態(tài)障礙物對(duì)無人艇避碰決策的影響，因此首先結(jié)合海事規(guī)則、最短會(huì)遇時(shí)間與安全距離構(gòu)建最優(yōu)位置窗口，然后基于改進(jìn)的RRT算法實(shí)時(shí)規(guī)劃出一條聯(lián)通路徑。在第二層框架中則考慮到無人艇的運(yùn)動(dòng)學(xué)約束，將上一層的聯(lián)通路徑點(diǎn)作為分段啟發(fā)點(diǎn)，然后結(jié)合速度運(yùn)動(dòng)模型來限制無人艇的拐角與轉(zhuǎn)彎半徑，并基于速度運(yùn)動(dòng)模型得到的弧長計(jì)算出每一個(gè)節(jié)點(diǎn)的代價(jià)值，最終在動(dòng)態(tài)障礙物環(huán)境中得到一條可行平滑軌跡。

1 基于改進(jìn)RRT的路徑搜索算法

1.1 改進(jìn)的探索點(diǎn)及步長選擇策略

傳統(tǒng)的RRT算法有著很大的隨機(jī)性，這有利于對(duì)未知環(huán)境的探索，但同時(shí)會(huì)讓算法收斂速度變慢，降低算法的實(shí)時(shí)性。此外，傳統(tǒng)的RRT算法的搜索步長是固定的。當(dāng)選取較大步長時(shí)，可以降低擴(kuò)展次數(shù)，增加算法收斂效率，但如果周邊環(huán)境復(fù)雜，大步長容易直接碰到障礙物區(qū)域，從而生成無效解，反過來又降低了算法效率；當(dāng)步長較小時(shí)，可以增加算法的避碰能力，但由于增加了擴(kuò)展次數(shù)，會(huì)降低算法的實(shí)時(shí)性。

為了解決上述問題，往往會(huì)采用基于概率的目標(biāo)偏向策略、引入動(dòng)態(tài)步長、增加引力分量［6］等方法。但上述方法中的概率參數(shù)、引力系數(shù)等因子的取值與周邊環(huán)境的復(fù)雜程度、障礙物分布情況相關(guān)，因而難以定量建立取值公式。而動(dòng)態(tài)步長策略達(dá)到閾值后，也會(huì)失效。針對(duì)上述方法的缺陷，本文改進(jìn)了探索點(diǎn)的概率選擇方法并引入一步步長的策略，更好地兼顧了算法的隨機(jī)性與收斂性。針對(duì)基于概率的目標(biāo)偏向策略來引導(dǎo)隨機(jī)樹的生長方向的方法，可以得到如下探索點(diǎn)的取值公式

式（1）中，xe為探索點(diǎn)，xrandom為隨機(jī)采樣點(diǎn)，xgoal為終點(diǎn)，pn為啟發(fā)式采樣概率，pr為隨機(jī)數(shù)，且pr∈ ［0， 1］。

然后，基于步長可以得到新的采樣點(diǎn)xnew

式（2）中，xnear為隨機(jī)樹中離xe最近的節(jié)點(diǎn)，ρ為步長。

可以認(rèn)為，隨著隨機(jī)采樣點(diǎn)探索次數(shù)的增多，算法收斂的概率會(huì)逐步增大。計(jì)f為隨機(jī)采樣點(diǎn)探索成功的次數(shù)，即xe＝xrandom，且能生成xnew。每成功一次，f＝f＋1；當(dāng)目標(biāo)采樣點(diǎn)探索失敗，即xe＝xgoal，但無法生成xnew，則定f＝0。

因此，可定義如下的采樣概率公式

式（3）中，λ為比例因子，0＜λ＜1。λ越小，算法的隨機(jī)性越大，通常取λ＝0.1以兼顧算法隨機(jī)性與收斂性。

由于傳統(tǒng)動(dòng)態(tài)步長策略在達(dá)到閾值后，動(dòng)態(tài)步長將不再發(fā)生變化。結(jié)合傳統(tǒng)動(dòng)態(tài)步長方法［7］以及一步步長策略，建立如下動(dòng)態(tài)步長公式

式（4）中，ρ為動(dòng)態(tài)步長；d為當(dāng)前節(jié)點(diǎn)到周邊障礙物的最近距離；dt為距離閾值；α為衰減系數(shù)；ρmin＝ρmaxe-α，ρmax為 最 大 步 長；d（xnear，xgoal）為xnear與xgoal間 的 距 離；pos＝pr為0到1間的隨機(jī)數(shù)；ξ＝

即當(dāng)xe＝xgoal時(shí)， 隨著d（xnear，xgoal）減小，d（xnear，xstart）增大， 一步步長成功的可能性會(huì)更大。因此，采取一步步長策略的概率會(huì)逐步增大。而當(dāng)xe＝xrandom時(shí)，則依然使用傳統(tǒng)動(dòng)態(tài)步長的策略：xnear離障礙物較遠(yuǎn)時(shí)，采用大步長ρmax，加快算法收斂速度；xnear在障礙物附近時(shí)，基于指數(shù)衰減函數(shù)得到的小步長來確保航跡平滑度。這樣，就兼顧了算法的實(shí)時(shí)性與路徑的平滑度。

1.2 基于海事規(guī)則的四向擴(kuò)展隨機(jī)樹

在實(shí)際航行過程中，無人艇應(yīng)遵循 《國際海上避碰規(guī)則公約》（COLREGS）。根據(jù) COLREGS、船舶避碰的經(jīng)驗(yàn)數(shù)據(jù)以及實(shí)際船只在復(fù)雜海洋環(huán)境中的航行范圍，為無人艇定義了追越（Overtaking， OT）、 對(duì)遇（Head-on， HO）、 右交叉（Crossing from right， CFR）、 左交叉（Crossing from left，CFL）四種沖突局面，并得到?jīng)_突局面的關(guān)系函數(shù)和航行規(guī)則表。

式（5）中，P為目標(biāo)船只所在范圍，如圖1所示；Δθ為無人艇與目標(biāo)船只航向角角度差，并定義無人艇艏向?yàn)?°，順時(shí)針為正，則可知：fc∈｛HO，OT，CFR，CFL｝， 具體如表1所示。

根據(jù)COLREGS，可以得到無人艇在遇到目標(biāo)船只的避碰轉(zhuǎn)向USVturn

圖1 無人艇與目標(biāo)船只相對(duì)位置Fig.1 Relative position of USV and target vessel

表1 航行規(guī)則表Table 1 Navigation rules table

此外，最短會(huì)遇時(shí)間TCPA（Time to Closest Point of Approaching）即指無人艇與目標(biāo)船只在保持現(xiàn)有的航行狀態(tài)下會(huì)遇時(shí)兩船距離最近時(shí)所需要的時(shí)間，可以對(duì)目標(biāo)船只位置進(jìn)行預(yù)測(cè)。TCPA的計(jì)算公式［7］如下

式（7）中，RT為兩船相對(duì)距離，αT為相對(duì)方向，φR為相對(duì)速度航向，vR為相對(duì)速度。

根據(jù)USVturn、TCPA及安全距離范圍可以得到無人艇的最優(yōu)位置窗口，如圖2所示。根據(jù)表1及式（6），可知圖2中的無人艇與目標(biāo)船只處于對(duì)遇局面，無人艇應(yīng)向右拐彎，應(yīng)在無人艇右方構(gòu)建位置窗口。其中，Lvessel為目標(biāo)船只的船長，vvessel為目標(biāo)船只的速度，dmin、dmax分別為避碰距離的上限與下限。根據(jù)實(shí)際航行經(jīng)驗(yàn)，dmin應(yīng)略大于安全距離，dmax約為安全距離的3倍。根據(jù)過往航行數(shù)據(jù)，由此形成的最優(yōu)位置窗口就是無人艇在避碰過程中的必經(jīng)區(qū)域。

圖2 最優(yōu)位置窗口示意圖Fig.2 Diagram of optimal position window

相對(duì)于傳統(tǒng)RRT算法的單隨機(jī)樹，只能從起點(diǎn)生成，并向終點(diǎn)生長，如圖3所示。在實(shí)際航行中，可以根據(jù)態(tài)勢(shì)程度的不同在走廊中選取一點(diǎn)作為無人艇的必經(jīng)點(diǎn)，然后在起點(diǎn)與必經(jīng)點(diǎn)、必經(jīng)點(diǎn)與終點(diǎn)間分別構(gòu)造兩棵擴(kuò)展隨機(jī)樹，從而構(gòu)建出了四向擴(kuò)展隨機(jī)樹。四向擴(kuò)展隨機(jī)樹能夠并行搜索，從而提高了算法的效率，構(gòu)建的必經(jīng)點(diǎn)也增強(qiáng)了算法的啟發(fā)性，如圖4所示。

圖3 單搜索隨機(jī)樹Fig.3 Single search random tree

圖4 四向擴(kuò)展隨機(jī)樹Fig.4 Four-way extended random tree

基于改進(jìn)RRT的路徑搜索算法的流程圖如圖5所示。由于RRT算法搜索出來的路徑是由各枝葉節(jié)點(diǎn)連接而成，因此在搜索出聯(lián)通路徑后，還需要使用最短路徑算法來優(yōu)化路徑。

圖5 基于改進(jìn)RRT的路徑搜索算法流程圖Fig.5 Flowchart of path search algorithm based on improved RRT

2 基于RRT的軌跡生成方法

基于改進(jìn)RRT的路徑搜索算法只是找到了一條路徑，卻沒有考慮無人艇的運(yùn)動(dòng)學(xué)模型。實(shí)際上，控制器不一定能跟隨此路徑。為了解決上述問題，本文提出了基于RRT的軌跡生成方法。

2.1 速度運(yùn)動(dòng)模型

在動(dòng)力學(xué)上，無人艇是具有非完整約束的。因此，無人艇的動(dòng)力學(xué)可以用速度運(yùn)動(dòng)模型g（x，y，θ，v，ω）進(jìn)行描述。其中，x、y為位置，θ為方向，v為速度，ω為角速度。則可以得到下一時(shí)刻狀態(tài)點(diǎn)的方程

對(duì)速度運(yùn)動(dòng)模型進(jìn)行閉式求解［8］，可以計(jì)算出從當(dāng)前點(diǎn)到下一狀態(tài)點(diǎn)的速度、角速度、轉(zhuǎn)彎半徑以及圓心點(diǎn)坐標(biāo)，如圖6所示。

圖6 速度運(yùn)動(dòng)模型示意圖Fig.6 Schematic diagram of velocity motion model

圖6中，A為初始狀態(tài)點(diǎn)；B為下一時(shí)刻狀態(tài)點(diǎn)；θ為A到B的航向角度差；θUSV為無人艇的拐角大小，且θ＝2θUSV；ξ為兩點(diǎn)間的Euclidean距離；R為無人艇的轉(zhuǎn)彎半徑，其取值大小為

因此，在已知A、B點(diǎn)的前提下，可以求得軌跡；在已知A、θUSV、ξ的前提下，也能求出B點(diǎn)。

2.2 非完整約束

考慮到無人艇的非完整約束，將無人艇的最大轉(zhuǎn)角作為約束條件，從而限制隨機(jī)采樣點(diǎn)的生成范圍。為了盡快找到可行軌跡且盡可能地利用第一層算法所搜索到的路徑信息，令式（1）中的pn＝1， 即一直使用一步步長策略，并基于拐角約束構(gòu)建如下采樣公式

由于θ∈ ［-θmax，θmax］， 結(jié)合式（9）可知

此外，根據(jù)船舶的相關(guān)設(shè)計(jì)規(guī)范，轉(zhuǎn)彎半徑與船長的比值一般在3～7.5之間。在考慮無人艇的最小轉(zhuǎn)彎半徑與拐角約束的情況下，可以計(jì)算出ξ值。

2.3 基于速度運(yùn)動(dòng)模型的代價(jià)值計(jì)算方法

傳統(tǒng)RRT算法的代價(jià)值是路徑節(jié)點(diǎn)A（x1，y1）與路徑節(jié)點(diǎn)B（x2，y2）的Euclidean距離。但當(dāng)考慮到無人艇的動(dòng)力學(xué)模型時(shí)， 狀態(tài)點(diǎn)A′（x1，y1，θ1）與狀態(tài)點(diǎn)B′（x2，y2，θ2）間的代價(jià)值就不能僅僅只考慮Euclidean距離，還需要考慮艏向角對(duì)代價(jià)值的影響。因此，令

式（12）中，θ為A′到B′的航向角度差，R為A′到B′的轉(zhuǎn)彎半徑，fcost即為A′到B′的實(shí)際軌跡長度，以此作為節(jié)點(diǎn)A′的控制能耗代價(jià)。

然后，將計(jì)算出來的控制能耗代價(jià)當(dāng)作一個(gè)啟發(fā)值，每一次都選取代價(jià)值最小的節(jié)點(diǎn)開始進(jìn)行軌跡搜索。

此外，考慮無人艇的最小轉(zhuǎn)彎半徑與拐角約束的情況下計(jì)算出來的ξ值可能會(huì)比較大，這就會(huì)導(dǎo)致相距較遠(yuǎn)。若只將這兩個(gè)節(jié)點(diǎn)加入隨機(jī)樹，雖然生成的軌跡依然是平滑的，但很難生成最優(yōu)軌跡。如圖7（a）所示，軌跡AGD是明顯優(yōu)于軌跡ABCD的。因此，需要在之間進(jìn)行子節(jié)點(diǎn)的擴(kuò)充，如圖7（b）所示。在A、B節(jié)點(diǎn)間進(jìn)行子節(jié)點(diǎn)擴(kuò)充，并分別計(jì)算cost值，再將這些子節(jié)點(diǎn)加入隨機(jī)樹，且擴(kuò)充的子節(jié)點(diǎn)的父節(jié)點(diǎn)都為即A節(jié)點(diǎn)。

圖7 中間節(jié)點(diǎn)擴(kuò)展示意圖Fig.7 Diagram of intermediate node expansion

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

上述的軌跡生成方法是在搜索到的路徑信息基礎(chǔ)上得到的軌跡，是假設(shè)軌跡不會(huì)碰到障礙物。實(shí)際上，路徑無障礙并不代表軌跡無障礙。

圖8 動(dòng)態(tài)窗口示意圖Fig.8 Diagram of dynamic window

3 仿真實(shí)驗(yàn)

無人艇與目標(biāo)船只的航向角皆以北向?yàn)?°且順時(shí)針為正，dmin＝50m、dmax＝100m，無人艇最小轉(zhuǎn)彎半徑為R＝30m，最大拐角θmax＝30°。 為了更好地描述目標(biāo)船只運(yùn)動(dòng)趨勢(shì)，結(jié)合藤井模型［9］，以半橢圓表示目標(biāo)船只。其中，橢圓的朝向即為目標(biāo)船只的運(yùn)動(dòng)方向，橢圓短軸為船長的3倍，長軸為船長的7倍，并設(shè)定目標(biāo)船只船長為2m。

圖9 基于RRT的軌跡生成方法流程圖Fig.9 Trajectory generation method based on RRT

本文基于3種不同的會(huì)遇局面進(jìn)行仿真實(shí)驗(yàn)，并通過轉(zhuǎn)折數(shù)目（轉(zhuǎn)折角度大于θmax）、最大轉(zhuǎn)折角度、路徑長度、與障礙物最近距離、規(guī)劃時(shí)間來評(píng)價(jià)路徑的優(yōu)劣。

（1）對(duì)遇局面

設(shè)初始運(yùn)動(dòng)參數(shù)如下：無人艇航速v＝5m／s、航向θ＝270°， 目標(biāo)船只航速v＝5m／s、 航向θ＝90°、距離無人艇100m。由圖10可知，無人艇與目標(biāo)船只將構(gòu)成對(duì)遇局面。仿真圖中的白色線條為無人艇的路徑，s為無人艇當(dāng)前位置，g為無人艇終點(diǎn)（后文相同）。

圖10 對(duì)遇局面下無人艇動(dòng)態(tài)航跡Fig.10 USV dynamic track under HO situation

（2）追越局面

設(shè)初始運(yùn)動(dòng)參數(shù)如下：無人艇航速v＝20m／s、航向θ＝270°， 目標(biāo)船只航速v＝3m／s、 航向θ＝270°、距離無人艇50m。由圖11可知，無人艇與目標(biāo)船只將構(gòu)成追越局面。

圖11 追越局面下無人艇動(dòng)態(tài)航跡Fig.11 USV dynamic track under OT situation

（3）交叉局面

設(shè)初始運(yùn)動(dòng)參數(shù)如下：無人艇航速v＝10m／s、航向θ＝0°，目標(biāo)船只航速v＝10m／s、航向θ＝270°、距離無人艇112m。由圖12可知，無人艇與目標(biāo)船只將構(gòu)成右交叉局面。

圖12 右交叉局面下無人艇動(dòng)態(tài)航跡Fig.12 USV dynamic track under CFR situation

由圖10～圖12可知，改進(jìn)的雙層RRT算法得到的無人艇避碰軌跡都符合COLREGS。相較傳統(tǒng)RRT算法與第一層算法，該改進(jìn)算法能得到更加平滑的軌跡，在保持一定安全距離的同時(shí)，滿足了無人艇的運(yùn)動(dòng)特性。

表2～表4為上述3種會(huì)遇局面的實(shí)驗(yàn)對(duì)比結(jié)果。

表2 對(duì)遇實(shí)驗(yàn)結(jié)果對(duì)比Table 2 Comparison of HO experiment results

表3 追越實(shí)驗(yàn)結(jié)果對(duì)比Table 3 Comparison of OT experiment results

表4 右交叉實(shí)驗(yàn)結(jié)果對(duì)比Table 4 Comparison of CFR experiment results

由表2～表4可知，傳統(tǒng)RRT算法、第一層算法、第二層算法都能迅速地規(guī)劃出路徑。但第一層算法的規(guī)劃時(shí)間小于傳統(tǒng)RRT算法，可見基于改進(jìn)RRT的路徑搜索算法是有效的，能夠在解決動(dòng)態(tài)避碰問題的同時(shí)，加快算法的運(yùn)行效率。第二層算法的規(guī)劃時(shí)間雖然略大于第一層算法，但依然能夠迅速地找到路徑，且總體來說依然比傳統(tǒng)RRT算法要快。可見，本文的雙層RRT算法是可以滿足無人艇避碰實(shí)時(shí)性要求的。第二層算法的轉(zhuǎn)折數(shù)目始終為0，最大轉(zhuǎn)折角度為19.1°，這兩個(gè)指標(biāo)好于傳統(tǒng)RRT算法與第一層算法?？梢?，基于RRT的軌跡生成方法是有效的，可以得到平滑軌跡并且滿足了無人艇的運(yùn)動(dòng)特性。此外，第二層算法與障礙物的最近距離是傳統(tǒng)RRT算法的兩倍以上，可見本文算法得到的軌跡更加安全。

4 實(shí)船驗(yàn)證

針對(duì)算法開展實(shí)船實(shí)驗(yàn)。其中，無人艇長7.5m、寬2.5m，量化安全距離為30m，無人艇的最小轉(zhuǎn)彎半徑R為船長的3倍，無人艇的最大拐角θmax＝30°、dmin＝40m、dmax＝100m。

實(shí)驗(yàn)過程中，無人艇在每組場(chǎng)景下保持6kn以上的航速航行，并依靠導(dǎo)航雷達(dá)與激光雷達(dá)獲取障礙物信息。有人駕駛艇則充當(dāng)會(huì)遇船只，為了更好地驗(yàn)證算法的有效性，有人駕駛艇保持恒向恒速，不輕易采取大的機(jī)動(dòng)動(dòng)作。當(dāng)兩船會(huì)遇時(shí)，采取雙層RRT算法實(shí)現(xiàn)動(dòng)態(tài)避碰。

（1）對(duì)遇局面

目標(biāo)船只速度約為10kn，方向由東向西。無人艇最高航速為10kn，原始航線為圖13中的綠色虛線，1為起點(diǎn)，2為終點(diǎn)。無人艇軌跡如圖13、圖14（a）中的紅色虛線所示，目標(biāo)船只軌跡如圖14（a）中的藍(lán)色實(shí)線所示。無人艇與障礙物的最近距離為41.43m，如圖14（b）所示。

圖13 兩船對(duì)遇實(shí)驗(yàn)控制界面示意圖Fig.13 Control interface diagram of two ship HO experiment

圖14 兩船對(duì)遇實(shí)驗(yàn)Fig.14 Diagram of two ship HO experiment

（2）追越局面

目標(biāo)船只速度約為2kn，方向由西向東。無人艇最高航速為10kn，原始航線為圖15中的綠色虛線，1為起點(diǎn)，2為終點(diǎn)。無人艇軌跡如圖 15、圖16（a）中的紅色虛線所示，目標(biāo)船只軌跡如圖16（a）中的藍(lán)色實(shí)線所示。無人艇與障礙物的最近距離為37.31m，如圖16（b）所示。

圖15 兩船追越實(shí)驗(yàn)控制界面示意圖Fig.15 Control interface diagram of two ship OT experiment

圖16 兩船追越實(shí)驗(yàn)Fig.16 Diagram of two ship OT experiment

（3）交叉局面

目標(biāo)船只速度約為10kn，方向由南向北。無人艇最高航速為10kn，原始航線為圖17中的綠色虛線，1為起點(diǎn)，2為終點(diǎn)。無人艇軌跡如圖17、圖18（a）中的紅色虛線所示，目標(biāo)船只軌跡如圖18（a）中的藍(lán)色實(shí)線所示。無人艇與障礙物的最近距離為67.65m，如圖18（b）所示。

圖17 兩船右交叉實(shí)驗(yàn)控制界面示意圖Fig.17 Control interface diagram of two ship CFR experiment

圖18 兩船右交叉實(shí)驗(yàn)Fig.18 Diagram of two ship CFR experiment

表5 實(shí)船實(shí)驗(yàn)結(jié)果對(duì)比Table 5 Comparison of actual ship tests results

算法的平均規(guī)劃時(shí)間為0.015s，表明本文提出的算法符合無人艇的實(shí)時(shí)性需求。由圖14、圖16、圖18及表5可知，無人艇的軌跡平滑，滿足無人艇的運(yùn)動(dòng)約束和 《國家海上避碰規(guī)則公約》。從相對(duì)距離示意圖也可以看出，避碰路徑是滿足安全距離的。

通過上述實(shí)船實(shí)驗(yàn)可以看出，本文提出的算法是有效的。

5 結(jié)論

本文針對(duì)無人艇的動(dòng)態(tài)避碰進(jìn)行了研究，所提出的改進(jìn)雙層RRT算法考慮了無人艇的運(yùn)動(dòng)特性和 《國際海上避碰規(guī)則公約》所帶來的影響。在第一層算法中，基于 《國際海上避碰規(guī)則公約》、最短會(huì)遇時(shí)間建立了最優(yōu)位置窗口，從而構(gòu)造出了四向擴(kuò)展隨機(jī)樹，讓無人艇在避碰過程中能夠根據(jù)目標(biāo)船只的信息產(chǎn)生正確的決策，并搜索出對(duì)應(yīng)路徑。在第二層算法中，則通過結(jié)合速度運(yùn)動(dòng)模型實(shí)現(xiàn)了無人艇的軌跡生成。仿真分析和實(shí)船實(shí)驗(yàn)均驗(yàn)證了本文所提方法的有效性。其中，轉(zhuǎn)折數(shù)目始終為0，最大轉(zhuǎn)折角度指標(biāo)遠(yuǎn)好于傳統(tǒng)RRT算法，表明本文算法是可以得到平滑軌跡的。此外，算法運(yùn)行時(shí)間亦滿足無人艇實(shí)際航行與避碰的實(shí)時(shí)性要求。未來，可進(jìn)一步結(jié)合無人艇的動(dòng)力學(xué)特征，基于最低能耗、最小加加速度等優(yōu)化目標(biāo)對(duì)軌跡進(jìn)行優(yōu)化，生成更優(yōu)的軌跡。