基于改進(jìn)雙向RRT 的無人船路徑規(guī)劃

開放科學(xué)（資源服務(wù)）標(biāo)志碼（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

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

改進(jìn)的雙向RRT*算法全局路徑規(guī)劃

1.1 問題定義

遵循Karaman 和Frazzoli提出的運(yùn)動規(guī)劃問題的公式[6]，給定一個配置空間 m ，配置空間進(jìn)一步分為可用空間 m_free 和障礙空間 m_obstacle 。令 （m_free，Q_init，Q_goal） 為運(yùn)動規(guī)劃問題，其中 Q_init 是初始自由配置并且 Q_goal?m_free （204號是目標(biāo)區(qū)域。 σ：[0，1]?m_free 是一個路徑到自由空間的連續(xù)映射，若 σ（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 傳統(tǒng)雙向RRT*

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

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

1.3 改進(jìn)的雙向RRT*算法

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

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

1.3.1 改進(jìn)步長

步長的選擇對雙向RRT*的運(yùn)行效率至關(guān)重要。針對這一問題，我們提出了改進(jìn)方案：首先，為每棵樹設(shè)置獨(dú)立的步長，然后，將步長設(shè)計為自適應(yīng)步長，根據(jù)周圍環(huán)境動態(tài)調(diào)整。這樣，雙向樹能夠根據(jù)環(huán)境的不同，在運(yùn)動空間較小的區(qū)域找到路徑，同時在空間較大的區(qū)域快速擴(kuò)展，節(jié)省計算資源。由于直接表示樹周圍可用空間的大小較為困難，我們采用生成節(jié)點(diǎn)的概率來衡量當(dāng)前區(qū)域的可用空間大小，從而引導(dǎo)樹的擴(kuò)展。

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

圖中 n_a 表示生成節(jié)點(diǎn)次數(shù)， t_a 表示這些節(jié)點(diǎn)中變成樹節(jié)點(diǎn)的個數(shù)，則envir表示生成樹節(jié)點(diǎn)的有效率，代碼2到7行表示若節(jié)點(diǎn)生成率小于en1則表示當(dāng)前自由空間較小應(yīng)更換更小步長 step1，若節(jié)點(diǎn)生成率大于en2，則表示當(dāng)前自由空間較大應(yīng)更換更大的步長 step2。

1.3.2 關(guān)鍵節(jié)點(diǎn)的選擇

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

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

1.3.3 路徑平滑

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

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

具體做法如圖5所示。

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

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

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

2算法仿真和測試

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

2.1 實驗1

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

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

對比算法1的雙向RRT*有11個點(diǎn)，樹2有36個節(jié)點(diǎn)，總共有47個節(jié)點(diǎn)，路徑上總共有14個節(jié)點(diǎn)，路徑長度為 894.67m 。對比算法2的雙向RRT*的樹1有3個點(diǎn)，樹2有52個節(jié)點(diǎn)，并沒有找到路徑。可以明顯看出改進(jìn)的雙向RRT*算法對環(huán)境的適應(yīng)能力更強(qiáng)，生成的節(jié)點(diǎn)數(shù)更少，生成的路徑更短。

圖8中藍(lán)色的路徑為圖7（a）去除不必要節(jié)點(diǎn)后剩余的路徑，根據(jù)關(guān)鍵路徑選擇策略優(yōu)化后的路徑為圖8 中的紅色路徑。優(yōu)化前路徑上一共有9個節(jié)點(diǎn)優(yōu)化后路徑上共有6個節(jié)點(diǎn)，明顯的降低了路徑的冗余節(jié)點(diǎn)。 優(yōu)化前的長度 883.63m ，優(yōu)化后的長度 874.54m ，較優(yōu)化之前少 1.03%

在優(yōu)化關(guān)鍵節(jié)點(diǎn)后，本文使用公式（3）對路徑進(jìn)行優(yōu)化，優(yōu)化后的結(jié)果如圖9中藍(lán)色的路徑。可以明顯看出優(yōu)化后的路徑更為光滑更加符合無人船的運(yùn)動要求。

2.2 實驗2

此外本文設(shè)計了對比實驗將改進(jìn)的雙向RRT*算法與傳統(tǒng)的RRT*算法以及雙向RRT*算法進(jìn)行了比較，具體見圖10、圖11所示。

圖10（a）顯示的使RRT*算法進(jìn)行路徑規(guī)劃，其中節(jié)點(diǎn)總數(shù)154總路徑長為 3768m 。圖10（b）顯示的使用傳統(tǒng)的雙向RRT*進(jìn)行路徑規(guī)劃，其中樹一的節(jié)點(diǎn)45，樹二的節(jié)點(diǎn)93，總節(jié)點(diǎn)為138，總路徑長為 3438m 。

圖10（c）顯示改進(jìn)步長的雙向RRT*進(jìn)行路徑規(guī)劃其中樹一的節(jié)點(diǎn)82，樹二的節(jié)點(diǎn)26，節(jié)點(diǎn)總數(shù)為108，總路徑長為 3044m 。可以看出只改變步長的雙向RRT*算法在節(jié)點(diǎn)方面比RRT*算法少了 29.87% ，比雙向RRT*算法少了 21.74% 。在路徑長度方面改進(jìn)步長的雙向RRT*比RRT*少 19.22% ，比傳統(tǒng)的雙向RRT*少 11.46% 。

為了更直觀的觀察實驗結(jié)果，在圖10（c）的算法上繼續(xù)執(zhí)行關(guān)鍵節(jié)點(diǎn)選擇策略，使用二階貝塞爾函數(shù)優(yōu)化曲線。并將處理后的曲線與RRT*和雙向RRT*算法生成的路徑進(jìn)行比較，如圖8所示。

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

3結(jié)論

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

參考文獻(xiàn)：

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

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

[3]郭輝，劉祚時，黃鵬，等.基于改進(jìn) RRT-Conmect算法的機(jī)械臂路徑規(guī)劃[J].組合機(jī)床與自動化加工技術(shù)，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]蔣啟龍，許健.改進(jìn) PSO-PH-RRT*算法在智能車路徑規(guī)劃中的應(yīng)用[J/OL].東北大學(xué)學(xué)報（自然科學(xué)版），1-8[2025-04-11].DOI：10.12068/j. issn.1005-3026. 2025.20239047.

[6]KARAMANS，F(xiàn)RAZZOLIE.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ī)器人轉(zhuǎn)向特性的全局路徑規(guī)劃[J].湖北工業(yè)大學(xué)學(xué)報，2024，39（2）：57-62.DOI：10.3969/j.issn.1003-4684.2024.02.012.