無人艇航行規(guī)則及運動學約束下的改進RRT軌跡規(guī)劃方法

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

（1.青島海洋科學與技術試點國家實驗室，青島266237；2.中國航天科技集團有限公司，北京100048；3.北京航天控制儀器研究所，北京100039）

0 引言

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

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

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

1 基于改進RRT的路徑搜索算法

1.1 改進的探索點及步長選擇策略

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

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

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

然后，基于步長可以得到新的采樣點xnew

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

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

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

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

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

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

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

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

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

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

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

圖1 無人艇與目標船只相對位置Fig.1 Relative position of USV and target vessel

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

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

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

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

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

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

圖3 單搜索隨機樹Fig.3 Single search random tree

圖4 四向擴展隨機樹Fig.4 Four-way extended random tree

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

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

2 基于RRT的軌跡生成方法

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

2.1 速度運動模型

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

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

圖6 速度運動模型示意圖Fig.6 Schematic diagram of velocity motion model

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

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

2.2 非完整約束

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

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

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

2.3 基于速度運動模型的代價值計算方法

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

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

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

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

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

2.4 動態(tài)窗口

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

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

3 仿真實驗

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

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

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

（1）對遇局面

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

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

（2）追越局面

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

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

（3）交叉局面

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

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

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

表2～表4為上述3種會遇局面的實驗對比結果。

表2 對遇實驗結果對比Table 2 Comparison of HO experiment results

表3 追越實驗結果對比Table 3 Comparison of OT experiment results

表4 右交叉實驗結果對比Table 4 Comparison of CFR experiment results

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

4 實船驗證

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

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

（1）對遇局面

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

圖13 兩船對遇實驗控制界面示意圖Fig.13 Control interface diagram of two ship HO experiment

圖14 兩船對遇實驗Fig.14 Diagram of two ship HO experiment

（2）追越局面

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

圖15 兩船追越實驗控制界面示意圖Fig.15 Control interface diagram of two ship OT experiment

圖16 兩船追越實驗Fig.16 Diagram of two ship OT experiment

（3）交叉局面

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

圖17 兩船右交叉實驗控制界面示意圖Fig.17 Control interface diagram of two ship CFR experiment

圖18 兩船右交叉實驗Fig.18 Diagram of two ship CFR experiment

表5 實船實驗結果對比Table 5 Comparison of actual ship tests results

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

通過上述實船實驗可以看出，本文提出的算法是有效的。

5 結論

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