關(guān)鍵詞：移動機器人;路徑規(guī)劃;RRT-Connect算法;路徑優(yōu)化

0 引言（Introduction）

近年來，移動機器人在工業(yè)生產(chǎn)、物流運輸?shù)阮I(lǐng)域得到廣泛應(yīng)用，它們能夠代替人類執(zhí)行危險且繁重的工作任務(wù)。要確保移動機器人能夠在復雜的環(huán)境中安全、高效地執(zhí)行各項任務(wù)，關(guān)鍵在于為其規(guī)劃出一條能夠?qū)⑵瘘c、終點連接起來的無障礙路徑[1]。

當前，路徑規(guī)劃算法可分為全局路徑規(guī)劃算法與局部路徑規(guī)劃算法[2]。常見的全局路徑規(guī)劃算法主要包括A*算法[3]、快速擴展隨機樹（RRT）算法[4]等。其中，RRT算法因其強大的搜索能力和適用性而受到廣泛關(guān)注，但其采用的隨機采樣方法具有較高的盲目性，導致算法的效率較低，生成的路徑存在較多冗余節(jié)點[5]。RRT-Connect算法[6]在RRT算法的基礎(chǔ)上結(jié)合雙向搜索和節(jié)點貪婪擴展策略，以此提高了算法的搜索速度。Neural RRT*[7]利用A*算法生成的路徑訓練卷積神經(jīng)網(wǎng)絡(luò)（CNN）模型，并用該模型指導RRT*采樣。張瑞等[8]將動態(tài)窗口法與RRT-Connect算法融合，有效地解決了傳統(tǒng)RRT-Connect算法無法適應(yīng)環(huán)境動態(tài)變化的問題。

本文針對RRT-Connect算法的不足進行改進，首先通過目標偏置采樣方法、目標引力導向策略及自適應(yīng)步長提高算法的搜索效率，其次采用節(jié)點重組刪除RRT-Connect算法生成的初始路徑中的冗余節(jié)點，最后用3次B樣條曲線對優(yōu)化后的路徑進行曲線平滑。

1 傳統(tǒng)RRT-Connect算法（Traditional RRTConnectalgorithm）

RRT-Connect算法在規(guī)定區(qū)域范圍內(nèi)初始化兩棵隨機搜索樹，分別是起始樹Tree1和目標樹Tree2，將起始點qinit、目標點qgoal 分別添加到起始樹、目標樹。如圖1所示，在狀態(tài)空間內(nèi)隨機采樣一個節(jié)點qrand，遍歷Tree1中的所有節(jié)點，找到與qrand 最鄰近的節(jié)點qv1_near，由qv1_near 向qrand 擴展一定的步長λ 后，得到一個新的節(jié)點qv1_new，并對節(jié)點qv1_near 和節(jié)點qv1_new進行碰撞測試，若不經(jīng)過障礙物，則將節(jié)點qv1_new 加入Tree1。隨后，遍歷Tree2中的所有節(jié)點，找到與qv1_new 最鄰近的節(jié)點qv2tov1_near，以節(jié)點qv2tov1_near 為父節(jié)點向qv1_new 延伸步長λ 后，得到新節(jié)點qv2_new，并通過碰撞檢測判斷是否將qv2_new 加入Tree2。完成以上步驟即為一輪搜索。在每輪搜索結(jié)束后，都需判斷節(jié)點qv1_new 與節(jié)點qv2_new 是否相遇，若相遇，則從相遇的兩個節(jié)點開始進行回溯操作，即可得到一條連接起始點qinit和目標點qgoal 的無障礙路徑。若沒有相遇，則繼續(xù)進行新的一輪搜索。

2 改進RRT-Connect算法（Improve RRT-Connectalgorithm）

2.1 目標偏置采樣方法

RRT-Connect算法依靠隨機采樣生成節(jié)點qrand，在生成節(jié)點qrand 的采樣過程中以一定的概率選擇目標節(jié)點qgoal 為qrand，可以加快搜索效率。本文將Pbias 設(shè)置為目標偏置采樣的閾值，并隨機生成一個從0到1的數(shù)值P。當P 值小于Pbias時，則令qrand 等于qgoal;反之，則隨機采樣節(jié)點。目標偏置采樣公式為

2.2 基于地圖復雜程度的可變步長策略

傳統(tǒng)RRT-Connect算法采用固定步長生成新節(jié)點qnew，然而固定步長無法靈活地適應(yīng)不同復雜程度的地圖。針對這個問題，本文基于模糊邏輯思想，搭建模糊控制系統(tǒng)，將障礙物個數(shù)obstacles_num、障礙物總面積與地圖面積的比值obstacles_area 作為輸入，地圖復雜系數(shù)k 作為輸出，根據(jù)地圖復雜系數(shù)k 的具體值對步長進行動態(tài)調(diào)整，使算法能夠在障礙物稀疏的地圖中增大步幅，進而提升搜索速度，在障礙物密集的地圖中減小步幅，進而提高搜索精度。

該控制系統(tǒng)的輸入量obstacles_num、obstacles_area 的論域分別為[0，50]、[0，0.5]，模糊集均為{S，M ，L}。輸出量k的論域為[0，1]，模糊集為{XS，S，M ，L，XL}。各變量的隸屬度函數(shù)均采用Trimf隸屬度函數(shù)表示，如圖2所示。同時，根據(jù)“障礙物個數(shù)越多，障礙物總面積與地圖面積的比值越大，則地圖復雜程度越高”這一原則，制定合理的模糊規(guī)則表，如表1所示。

根據(jù)輸入量的模糊值和模糊規(guī)則表，經(jīng)過模糊推理得到模糊化的輸出量，并運用面積重心法得到輸出量的具體值kmap，該值反映了地圖的復雜程度，應(yīng)與步長成反比，所以步長Lmap的計算式可表示為

2.3 目標引力導向策略

傳統(tǒng)RRT-Connect算法生成節(jié)點qrand 后，自節(jié)點qnear 向節(jié)點qrand 延伸固定步長λ，并通過碰撞測試后生成新的節(jié)點qnew。本文通過在每次生成新節(jié)點時添加目標引力，使其生成的新節(jié)點更加偏向目標節(jié)點，采用目標引力導向策略拓展生成新節(jié)點的過程如圖3所示，圖3中Lmap 為2.2節(jié)生成的可變步長。

2.4 基于節(jié)點重組的冗余節(jié)點刪除

由于RRT-Connect算法生成的原始路徑存在較多冗余節(jié)點，冗余節(jié)點的存在會增加路徑的長度和拐點的個數(shù)，在實際使用過程中會導致機器人的能耗過大，因此本文采用節(jié)點重組的方法對RRT-Connect算法生成的原始路徑進行優(yōu)化，去除其中的冗余節(jié)點。

獲取RRT-Connect算法生成的初始路徑節(jié)點集Q {Qi，1≤i≤n}，同時創(chuàng)建節(jié)點集V，用于存放優(yōu)化后的路徑節(jié)點，并將起點Q1 和終點Qn 加入節(jié)點集V 中作為初始值。將起點Q1 作為根節(jié)點，依次連接Q3、Q4、…、Qm ，并判斷直線Q1Q3、Q1Q4、…、Q1Qm 是否經(jīng)過障礙物，若Q1Q3、Q1Q4、…、Q1Qm均沒有經(jīng)過障礙物，則繼續(xù)連接Qm +1;若Q1Q3、Q1Q4、…、Q1Qm -1 均沒有經(jīng)過障礙物，Q1Qm 經(jīng)過了障礙物，則將Qm -1記為路徑的關(guān)鍵節(jié)點，并將其加入節(jié)點集V 中。將Qm -1 作為根節(jié)點開始新的一輪節(jié)點重連，直到連接終點Qn，此時節(jié)點集V 中包含了優(yōu)化后的路徑節(jié)點，將其依次連接，便可得到優(yōu)化后的路徑。如圖4所示，虛線表示RRT-Connect算法生成的初始路徑，實線則表示優(yōu)化后的路徑。

2.5 路徑平滑

當前，常用的路徑平滑方法主要有B樣條曲線和貝塞爾曲線，貝塞爾曲線在控制點之間的距離較大時，會出現(xiàn)曲線光滑性下降的問題，并且貝塞爾曲線的形狀調(diào)整通常需要同時改變多個控制點而無法進行局部調(diào)整[9]。

相比之下，B樣條曲線的控制點可以自由調(diào)整，并且高次數(shù)的插值可以逼近復雜的曲線形狀。因此，本文采用3次B樣條曲線方法對節(jié)點優(yōu)化后的路徑進行平滑處理，以提高路徑的連續(xù)性和曲線的光滑度。B樣條曲線的數(shù)學表達式為

3 仿真結(jié)果與分析（Simulation results and analysis

為驗證改進RRT-Connect算法在路徑規(guī)劃中的有效性，本文在Intel（R） Core（TM） i5-12500U CPU@3.10 GHz仿真環(huán)境中，采用Python3.10實現(xiàn)各算法的功能，并對路徑規(guī)劃結(jié)果進行比較和分析。

在仿真實驗中，仿真模擬地圖的大小為300×300，起點設(shè)置為（15，15），終點設(shè)置為（250，250），最大迭代次數(shù)均設(shè)置10 000，傳統(tǒng)RRT算法、RRT-Connect算法的固定步長設(shè)置為15，改進RRT-Connect算法的步長由地圖復雜系數(shù)k 決定，目標偏置率設(shè)為0.2。分別在3種環(huán)境下測試傳統(tǒng)RRT算法、RRT-Connect算法和改進RRT-Connect算法的采樣次數(shù)、路徑規(guī)劃時間、路徑長度及路徑節(jié)點數(shù)量。

環(huán)境一、環(huán)境二、環(huán)境三分別為簡單環(huán)境、中等環(huán)境、復雜環(huán)境，對應(yīng)的障礙物總面積與地圖面積的比值分別為14.83%、25.00%、39.40%，障礙物個數(shù)分別為13個、24個、37個。根據(jù)公式（2）可得，改進RRT-Connect算法在環(huán)境一、環(huán)境二、環(huán)境三中的步長分別為19.27、15.42、10.08。圖5至圖7分別表示傳統(tǒng)RRT算法、RRT-Connect算法和改進RRTConnect算法在環(huán)境一、環(huán)境二、環(huán)境三中的路徑規(guī)劃效果。由于RRT算法采樣具有隨機性，所以分別在3種環(huán)境下對3種算法進行了40次仿真，對仿真結(jié)果取平均值，結(jié)果如表2至表4所示。

由圖5（a）、圖6（a）、圖7（a）可以看出，傳統(tǒng)RRT算法通過隨機采樣生成節(jié)點，路徑搜索效率低下且生成的原始路徑中冗余節(jié)點數(shù)量偏多，導致路徑拐角較多，路徑長度也隨之增加。RRT-Connect算法通過雙向生長隨機樹可以更快地找到可行路徑，但仍存在采樣具有盲目性和路徑質(zhì)量不高的問題。

由圖5（c）、圖7（c）、表2和表4可以看出，本文提出的改進后的RRT-Connect算法根據(jù)地圖復雜程度選擇合適的步長拓展隨機樹，在簡單環(huán)境中采用大步長提升了算法的搜索效率，在復雜環(huán)境中采用小步長提高了算法在狹窄通道中找到可行路徑的可能性，同時采用目標偏置與目標引力導向策略，使生成的新節(jié)點偏向目標點的方向，從而降低搜索的盲目性，縮短路徑規(guī)劃的時間。基于節(jié)點重組的冗余節(jié)點刪除可有效優(yōu)化原始路徑，路徑節(jié)點數(shù)量與路徑長度均有所下降。相較于傳統(tǒng)RRT-Connect算法，本文改進后的RRT-Connect算法在環(huán)境一和環(huán)境三中采樣點個數(shù)分別減少了37.68%、27.43%，路徑規(guī)劃時間分別縮短了35.29%、32.04%，生成的路徑長度分別縮短了10.42%、24.88%，路徑節(jié)點數(shù)量分別減少了85.71%、70.91%。在環(huán)境二中，改進后的RRT-Connect算法采用與傳統(tǒng)RRT算法、傳統(tǒng)RRT-Connect算法大致相同的步長，由表3可得，相較于傳統(tǒng)RRT-Connect算法，本文提出的改進后的RRT-Connect算法在環(huán)境二中的采樣點個數(shù)減少了25.16%，路徑規(guī)劃時間縮短了29.17%，改進后的算法在環(huán)境二中在采樣次數(shù)和路徑規(guī)劃時間方面的提升效果不明顯。如圖5（d）、圖6（d）、圖7（d）所示，經(jīng)過B樣條曲線處理后的路徑更加流暢，沒有明顯的轉(zhuǎn)折，生成的路徑更適合用于實際環(huán)境中機器人的運行。

4 結(jié)論（Conclusion）

為解決傳統(tǒng)RRT-Connect算法生成的路徑質(zhì)量較低、隨機搜索盲目性較大的問題，本文提出一種改進RRT-Connect算法。該算法主要在3個方面對傳統(tǒng)RRT-Connect算法進行改進。①通過引入目標偏置采樣方法與目標引力導向策略，有效地降低了采樣的隨機性，縮短了路徑規(guī)劃時間。②基于地圖復雜程度的可變步長策略，根據(jù)地圖中障礙物的面積占比與障礙物總數(shù)得出相應(yīng)的步長，該策略在簡單和復雜環(huán)境中都能顯著提升算法的搜索效率。③基于節(jié)點重組的冗余節(jié)點刪除，能夠?qū)υ悸窂竭M行路徑優(yōu)化，優(yōu)化后的路徑與傳統(tǒng)RRTConnect算法生成的路徑相比，路徑長度縮短了24.88%，路徑節(jié)點數(shù)量減少了85.71%。

最終，通過一系列仿真實驗，充分驗證了所提出的改進RRT-Connect算法的有效性和優(yōu)越性。