基于改進雙向RRT 的無人船路徑規劃

開放科學（資源服務）標志碼（OSID）

Path planning forunmanned surface vehicles based

on an improved bidirectional RRT algorithm

WANG Xingmin'，LIU Ruixue'，LI Qian'， ZHANG Weizhong1*，DONG Wei2（1.InstituteofAutomation，QiluUniversityofTechnology（ShandongAcademyofSciences），Jinan250l4，China;2.IstituteofOceanographic Instrumentation，Qilu Universityof Technology（Shandong AcademyofSciences），Qingdao 266061，China）

Abstract ： Oceans are not only super-ecosystems but also strategic resource reservoirs，and thus，ocean monitoring is crucial.Unmannedsurface vehicles（USVs）arenewtypes of multfunctional unmanned platforms foroceanmonitoring，and pathplaning plays acrucial roleasacore technologyin theiroperation.Withthecontinuous increase in maritime traffic densityandupgrading of navigation safety standards，traditional path planning methods are facing growing chalenges in adapting to complexenvironments.In this study，a multidimensional improvement strategyisproposed toaddress the limitationsofthebidirectionalrapidly-exploringrandomtreestar（Bi-RRT*）algoritminUSVpathplanning.First，anadaptive step-sizeadjustment mechanism，basedonenvironmentalfeatureperception，isestablished；second，akeynodeselection strategy isdesigned;andfinalyBeziercurvesareusedtosmooththegeneratedpath，producingasmoothertrajectorythat beter meets the kinematicrequirementsof USVs.Simulation results showthattheimproved bidirectional RRT*algorithm" outperformsits traditional counterpart in termsof node-generation effciency，overallperformance，andpath smoothness. Key words ∵ unmanned surface vehicles;ocean monitoring;path planning；bidirectional RRT*;adaptive step-size;Bezier Curve;key node selection

隨著無人平臺的推廣以及相關定位、導航與控制技術的快速發展，越來越多的無人海洋自動平臺應用于海洋監測領域，如無人機、無人水下自主航行器等[1]。而工作于海洋表面的無人船也逐漸成為海洋監測平臺研究和應用的一個熱點[2]。為了保障無人船在航行過程中的安全，并幫助其在復雜環境中高效、安全地導航至檢測地區，路徑規劃顯得尤為關鍵。通過精心設計的路徑，可以有效縮短無人船到達監管區域的時間，降低能源消耗，并顯著提高整體運營效率。RRT*方法是RRT（rapidly-exploring random trees，快速隨機探索樹）的一種優化算法，是基于采樣的路徑規劃領域的重要里程碑，在實際的無人船應用中表現出了更好的性能。不過，RRT*算法依賴于整個配置空間的均勻采樣，其收斂速度較慢，這是一個顯著的缺點[3」。為此一些學者提出雙向 RRT\"算法，該算法通過多加入一棵樹顯著的縮短了收斂速度，提高了路徑的質量[4-5]雙向 RRT*算法雖然生成快速，簡單直觀。但是當目標點與終點的環境差距極大時，雙向RRT*算法往往表現不佳。此外雙向RRT*路徑搜索過程中經常遍歷大量冗余節點，有多個轉折點，導致搜索效率低下，不適用于無人船。為了解決這些問題，本文為雙向RRT*增加了步長變換功能和路徑二次優化功能。這些改進旨在提高雙向RRT*對復雜環境的搜索能力，尤其是起點和終點環境相差巨大的情況，減少路徑中的轉彎次數，并創建更使用于無人船的平滑軌跡，從而提高尋路任務的整體算法性能。

改進的雙向RRT*算法全局路徑規劃

1.1 問題定義

遵循Karaman 和Frazzoli提出的運動規劃問題的公式[6]，給定一個配置空間 m ，配置空間進一步分為可用空間 m_free 和障礙空間 m_obstacle 。令 （m_free，Q_init，Q_goal） 為運動規劃問題，其中 Q_init 是初始自由配置并且 Q_goal?m_free （204號是目標區域。 σ：[0，1]?m_free 是一個路徑到自由空間的連續映射，若 σ（0）=Q_init 且 σ（1）∈Q_goal ，則這個路徑是有效的。若路徑 ?τ∈[0，1] 如果 ?τ∈[0，1] ’ σ（τ）∈x_free 是最佳路徑，則 ，其中σ（0）=Q_init，σ（1）∈Q_goal，σ（τ）∈Q_free，?τ∈[0，1]_o （2

1.2 傳統雙向RRT*

雙向RRT*為雙向搜索策略，分別從起點和目標點出發，構建兩棵擴展樹：一棵從起點向目標點擴展，另一棵從目標點向起點擴展。最終，兩個樹相遇并連接，形成完整路徑。其偽代碼如圖1所示[7]：

該算法描述了雙向RRT*算法的主要步驟。首先將地圖信息，無人船的初始位置與目標位置輸入進去，然后初始化這兩棵樹并開始迭代 n 次，每次迭代后得到 Q_new ，通過回溯方法計算較早的父節點是否可以產生較小的路徑成本且不會撞上障礙物，從而確定最佳父節點，一直重復上述過程直到找到解決方案。第3行到第5行是產生新節點的過程。第6行對新節點和父節點連接線段進行碰撞檢測。第7行到第8行是樹節點通過回溯思想找到最佳父節點的過程，第9行是重寫樹找到最優路徑，第14行指令為若是新節點和父節點的連線之間有障礙物則進行下一次迭代，并且下一次迭代交換兩棵樹。

1.3 改進的雙向RRT*算法

本文改進的雙向RRT*算法的偽代碼如圖2所示：

從上述偽代碼中可以看出相較于傳統的雙向RRT*算法，本算法主要改進點有3個：（1）為每個樹分別設置步長，并且根據環境的不同選擇步長;（2）為解決冗余節點過多的問題，引入了關鍵節點選擇策略，有效減少了冗余節點的數量;（3）采用二階賽貝爾曲線對目標路徑中的拐點進行平滑處理，生成符合無人船運行要求的平滑路徑。

1.3.1 改進步長

步長的選擇對雙向RRT*的運行效率至關重要。針對這一問題，我們提出了改進方案：首先，為每棵樹設置獨立的步長，然后，將步長設計為自適應步長，根據周圍環境動態調整。這樣，雙向樹能夠根據環境的不同，在運動空間較小的區域找到路徑，同時在空間較大的區域快速擴展，節省計算資源。由于直接表示樹周圍可用空間的大小較為困難，我們采用生成節點的概率來衡量當前區域的可用空間大小，從而引導樹的擴展。

圖3為決定步長的偽代碼：

圖中 n_a 表示生成節點次數， t_a 表示這些節點中變成樹節點的個數，則envir表示生成樹節點的有效率，代碼2到7行表示若節點生成率小于en1則表示當前自由空間較小應更換更小步長 step1，若節點生成率大于en2，則表示當前自由空間較大應更換更大的步長 step2。

1.3.2 關鍵節點的選擇

在實際導航中，單一指令下，無人船的移動距離越長，機動流暢性越好，誤差越小，從而更有利于準確完成作業。為了減少傳統雙向RRT*產生路徑的冗余節點本文提出了一種關鍵點選擇策略，在保留關鍵轉折點的同時，去除冗余節點和不必要的轉折點，減少轉彎次數，從而提高路徑的平滑度。該策略的原理如圖4所示。

具體節點選擇步驟如下：令 s 節點依次往后連接找尋距離最遠且與 s 連接的直線上無障礙物的節點，在這中間的節點視為冗余節點并被刪除。遵循此規律不斷循環刪除每條路徑中間的冗余節點后可得到最終路徑。在圖4（a）中去除中間的冗余節點后剩下3個節點 S，3，E 組成的路徑見圖4（b）。

1.3.3 路徑平滑

由于處理后的雙向RRT*算法可能出現拐點出曲率過大，在這種情況下可能會使無人船的速度忽然改變從而脫離規劃的軌跡，最終導致無人船導航任務失敗。為了使無人船在路徑拐點處運動平穩，減少不必要的能量消耗，需要在路徑規劃完成后對路徑軌跡進行平滑處理。貝塞爾曲線是一種在三維空間中顯示光滑曲線的方法。當有 n+1 個控制點時，對應的 n 階貝塞爾曲線表示為[8]：

其中 Ψ_t 是歸一化時間變量， P（t） 是 n 階貝塞爾曲線的表達式， B_i，n（t） 是 n 階伯恩斯坦多項式的基函數；由于高階貝塞爾曲線數值穩定性差，為保證無人船在拐點處能夠順利轉向，利用二階貝塞爾曲線依次平滑雙向RRT*算法的path中的節點，輸出平滑路徑。

具體做法如圖5所示。

在圖5中，為了更好的表達將點3記為 P₀ ，其中 S_?P₀ 和 E 表示3個連續路徑點。圖5中的 P₀ 處存在一個非常大的拐點然后我們根據拐角的大小決定長度 L ，根據 L 計算出 P₁，P₂ 的位置，點 A 在線段 P₀P₁ 上，點 B 在線段 P₀P₂ 上，點 c 在線段 ? 上，且 ，則二階貝塞爾曲線的表達式為：

B（t）=（1-t）²P₀+2t（1-t）P₁+t²P₂，t∈[0，1]

將 P₀，P₁，P₂，t 帶入二階貝塞爾公式中得到圖中 P₁，P₂ 之間的黑色曲線

2算法仿真和測試

為了使仿真結果更具有說服力本文根據中國某水域來建立仿真環境，為了便于后續進行仿真實驗，對地圖使用進行處理。將地圖轉換為由0和1組成的矩陣，其中0表示可通航水域設置為藍色，1表示不可通航區域設置為白色。見圖6。其中圖6（b）和圖6（c）為我們從圖6（a）中提取出來的兩個仿真環境。

2.1 實驗1

首先我們選擇圖6（c）為我們的仿真環境，設定無人船的起點為（800，500），終點為（120，30），我們先使用算法2.改進的雙向RRT*進行實驗，其中 step1=40 ， step2=50 en1=0.3 ， en1=0.7 ，比較算法選傳統的雙向RRT*步長分別設置為 40m 和 50m 。圖7是對比仿真的結果。

仿真實驗數據見表1。從表1中可以看出改進的雙向RRT*的樹1有13個點，樹2有19個節點，總共有32個節點，路徑上有15個節點，相較于雙向RRT*對比算法1減少了 31.91% ，路徑長度為883.6m 相較于雙向RRT*對比算法1減少了 1.24% 。

對比算法1的雙向RRT*有11個點，樹2有36個節點，總共有47個節點，路徑上總共有14個節點，路徑長度為 894.67m 。對比算法2的雙向RRT*的樹1有3個點，樹2有52個節點，并沒有找到路徑。可以明顯看出改進的雙向RRT*算法對環境的適應能力更強，生成的節點數更少，生成的路徑更短。

圖8中藍色的路徑為圖7（a）去除不必要節點后剩余的路徑，根據關鍵路徑選擇策略優化后的路徑為圖8 中的紅色路徑。優化前路徑上一共有9個節點優化后路徑上共有6個節點，明顯的降低了路徑的冗余節點。 優化前的長度 883.63m ，優化后的長度 874.54m ，較優化之前少 1.03%

在優化關鍵節點后，本文使用公式（3）對路徑進行優化，優化后的結果如圖9中藍色的路徑。可以明顯看出優化后的路徑更為光滑更加符合無人船的運動要求。

2.2 實驗2

此外本文設計了對比實驗將改進的雙向RRT*算法與傳統的RRT*算法以及雙向RRT*算法進行了比較，具體見圖10、圖11所示。

圖10（a）顯示的使RRT*算法進行路徑規劃，其中節點總數154總路徑長為 3768m 。圖10（b）顯示的使用傳統的雙向RRT*進行路徑規劃，其中樹一的節點45，樹二的節點93，總節點為138，總路徑長為 3438m 。

圖10（c）顯示改進步長的雙向RRT*進行路徑規劃其中樹一的節點82，樹二的節點26，節點總數為108，總路徑長為 3044m 。可以看出只改變步長的雙向RRT*算法在節點方面比RRT*算法少了 29.87% ，比雙向RRT*算法少了 21.74% 。在路徑長度方面改進步長的雙向RRT*比RRT*少 19.22% ，比傳統的雙向RRT*少 11.46% 。

為了更直觀的觀察實驗結果，在圖10（c）的算法上繼續執行關鍵節點選擇策略，使用二階貝塞爾函數優化曲線。并將處理后的曲線與RRT*和雙向RRT*算法生成的路徑進行比較，如圖8所示。

在圖11中，改進后的雙向RRT*的路徑長度為 2574m ，相較于RRT*算法↓25.13% ，相較于雙向RRT*算法少15.43% ，并且從圖中可以看出改進的雙向RRT*的路徑不存在拐點，更適合無人船的真實運動情況

3結論

在本研究中，提出了一種改進的雙向RRT*算法。該算法通過設置一個與環境有關的間接變量來改變步長，使得該算法具有更強適應能力。在找到路徑后，對路徑進行優化：首先對路徑上大量的冗余節點進行篩選，刪除掉冗余節點，然后對清除冗余節點后的路徑使用貝賽爾曲線進行優化。為了驗證本文提出算法的優越性，根據現實中的某個海域來設計兩組仿真，其中一組用來描述本文改進算法較改進之前的變化，另一組將本文提的改進雙向 RRT*與RRT*以及雙向 RRT*算法進行對比。結果顯示改后的算法要優于改進前的算法，并且相較于RRT*和雙向RRT*本文所提算法的樹節點更少，總路徑更短，路徑拐點更少，更為平滑，更加符合無人船行駛的路徑要求。在未來，應考慮如何更好的反映表示環境對步長的影響，或者應用一些深度學習的知識去改變步長。

參考文獻：

[1]蔡立鵬，蔣海陽，謝卓冉，等.海洋監測平臺研究綜述[J]．電腦知識與技術，2023，19（36）：114-116.DOI：10.14004/j.cnki.ckt.2023.1935.

[2]金久才，張杰，邵峰，等.一種海洋環境監測無人船系統及其海洋應用[J]．海岸工程，2015，34（3）：87-92.DOI：10.3969/j.issn.1002-3682.2015.03.009.

[3]郭輝，劉祚時，黃鵬，等.基于改進 RRT-Conmect算法的機械臂路徑規劃[J].組合機床與自動化加工技術，2025（3）：41-45.DOI：10.13462/j.cnki.mmtamt.2025.03.009.

[4]FANJM，CHEN X，LIANG X. UAVtrajectory planing basedon bi-directional APF-RRT*algorithm with goal-biased[J].Expert Systems with Applications，2023，213：119137. DOI： 10.1016/j.eswa.2022.119137.

[5]蔣啟龍，許健.改進 PSO-PH-RRT*算法在智能車路徑規劃中的應用[J/OL].東北大學學報（自然科學版），1-8[2025-04-11].DOI：10.12068/j. issn.1005-3026. 2025.20239047.

[6]KARAMANS，FRAZZOLIE.Sampling-basedalgorithsforoptimalmotionplaningJ].The International Joualof RoboticsResearch，2011， 30（7） ： 846-894. D01： 10.1177/0278364911406761.

[7]MAGJ，DUANYL，LIMZ，etal.AprobabilitysmothingBi-RRTpath planningalgorithmforindorrobotJ].FutureGeneration Computer Systems，2023，143：349-360.DOI： 10.1016/j.future.2023.02.004.

[8]魏瓊，郭川，張道德，等.考慮履帶機器人轉向特性的全局路徑規劃[J].湖北工業大學學報，2024，39（2）：57-62.DOI：10.3969/j.issn.1003-4684.2024.02.012.