基于改進(jìn)RRT-Connect算法的室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃研究

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

0 引言（Introduction）

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

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

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

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

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

2 改進(jìn)RRT-Connect算法（Improve RRT-Connectalgorithm）

2.1 目標(biāo)偏置采樣方法

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

2.2 基于地圖復(fù)雜程度的可變步長(zhǎng)策略

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

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

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

2.3 目標(biāo)引力導(dǎo)向策略

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

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

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

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

2.5 路徑平滑

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

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

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

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

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

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

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

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

4 結(jié)論（Conclusion）

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

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