基于改進(jìn)RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃

郭夢(mèng)詩(shī) 馮麗娟 代傳壘

摘要：針對(duì)傳統(tǒng)RRT算法在多障礙物、曲折狹窄道路等無(wú)規(guī)律環(huán)境下，隨機(jī)性大、收斂速度慢、效率低等問(wèn)題，提出一種改進(jìn)RRT路徑規(guī)劃算法，以提高在二維環(huán)境下移動(dòng)機(jī)器人的路徑規(guī)劃性能。改進(jìn)算法通過(guò)引入障礙物因子進(jìn)行區(qū)域節(jié)點(diǎn)采樣，減少采樣時(shí)間和次數(shù);同時(shí)對(duì)新產(chǎn)生的節(jié)點(diǎn)進(jìn)行約束，降低方向隨機(jī)性，減少目標(biāo)區(qū)域振蕩情況，加快搜索速度;此外，剔除冗余節(jié)點(diǎn)使路徑更加平滑，路徑長(zhǎng)度縮短且對(duì)內(nèi)存需求降低。通過(guò)實(shí)驗(yàn)仿真驗(yàn)證：改進(jìn)算法能滿(mǎn)足復(fù)雜環(huán)境下的避障路徑規(guī)劃，隨機(jī)性降低速度較快，具有較好的可行性和有效性。

關(guān)鍵詞：改進(jìn)RRT算法路徑規(guī)劃平滑避障

Path Planning of Mobile Robot Based on Improved RRT Algorithm

GUO Mengshi FENG LijuanDAI Chuanlei

（School of electronic and electrical engineering， Zhengzhou University of science and technology， Zhengzhou， Henan Province， 450064 China）

Abstract： Aiming at the problems of large randomness， slow convergence and low efficiency of traditional RRT algorithm in irregular environments such as multi obstacles and tortuous narrow roads， an improved RRT path planning algorithm is proposed to improve the path planning performance of mobile robot in two-dimensional environment. The improved algorithm reduces the sampling time and times by introducing the obstacle factor to sample the regional nodes; at the same time， the new nodes are constrained to reduce the direction randomness， reduce the oscillation in the target area and speed up the search speed; in addition， eliminating redundant nodes makes the path smoother， the path length shorter and the memory demand lower. The experimental simulation shows that the improved algorithm can meet the obstacle avoidance path planning in complex environment， reduce the randomness quickly， and has good feasibility and effectiveness.

Key Words： Improved RRT algorithm;Path planning;Smooth;Obstacle avoidance

隨著科技的發(fā)展，機(jī)器人機(jī)械臂在生產(chǎn)生活中的應(yīng)用愈加廣泛[1]。與此同時(shí)，路徑規(guī)劃問(wèn)題成為此領(lǐng)域的熱點(diǎn)及難點(diǎn)問(wèn)題。由于機(jī)器人應(yīng)用場(chǎng)景廣、靈活性強(qiáng)，若避障規(guī)劃效率低，則工作質(zhì)量易受到影響[2]。避障規(guī)劃是指在有障礙物等特定環(huán)境下，機(jī)器人機(jī)械臂在自身和約束條件下尋找到一條從起點(diǎn)到終點(diǎn)的無(wú)碰撞有效路徑[3]。多種路徑規(guī)劃算法被廣泛提出，并在實(shí)際生產(chǎn)生活中取得了較好的效果，比如基于搜索的粒子群算法[4]、A*算法[5]、人工勢(shì)場(chǎng)法[6]等。以上算法當(dāng)機(jī)器人環(huán)境維度、自由度、空間復(fù)雜度等增加時(shí)，其避障規(guī)劃復(fù)雜度倍增[7]?？焖偎阉麟S機(jī)樹(shù)（RRT）算法是一種典型的基于采樣的規(guī)劃方法，此算法不必對(duì)環(huán)境障礙物進(jìn)行精確描述，且在多維復(fù)雜環(huán)境中效果明顯[8]。

傳統(tǒng)RRT算法采樣均勻，復(fù)雜環(huán)境收斂慢，且其搜索路徑一般不是最優(yōu)。因此大量學(xué)者對(duì)此算法存在的缺陷提出了多種改進(jìn)和解決方案。Faris 等人通過(guò)引入多次樣條曲線(xiàn)將RRT算法路徑點(diǎn)進(jìn)行排序獲得平滑路徑[9]; Gammell 引入空間約束減少規(guī)劃時(shí)間[10];尹高揚(yáng)等人通過(guò)改進(jìn)鄰近點(diǎn)的采樣選取進(jìn)行全局采樣，但在搜索時(shí)間上有所延長(zhǎng)[11]。以上算法在路徑規(guī)劃長(zhǎng)度上明顯減少，但運(yùn)算時(shí)耗時(shí)較長(zhǎng)。故而，如何高效地獲取一條可行較優(yōu)的路徑仍是目前避障規(guī)劃研究的熱點(diǎn)和重點(diǎn)。

對(duì)于傳統(tǒng)RRT算法搜索慢、路徑冗余節(jié)點(diǎn)多和終點(diǎn)附近振蕩等問(wèn)題，本文進(jìn)行了算法改進(jìn)，使得機(jī)器人機(jī)械臂在避障路徑規(guī)劃上更加平穩(wěn)。通過(guò)Matlab仿真驗(yàn)證了本文算法在路徑長(zhǎng)度和節(jié)點(diǎn)數(shù)量等方面的有效性。

1 基本RRT算法

傳統(tǒng)RRT算法在1998年由Lavalle提出[12]，無(wú)需對(duì)環(huán)境進(jìn)行精確建模即可找到一條可行路徑。此算法的中心思想是從已知的確定的節(jié)點(diǎn)作為起始點(diǎn)，通過(guò)隨機(jī)采樣的方法在狀態(tài)空間中無(wú)規(guī)律地產(chǎn)生大量葉子節(jié)點(diǎn)，通過(guò)葉子節(jié)點(diǎn)逐漸向目標(biāo)點(diǎn)靠近的樹(shù)狀結(jié)構(gòu)，當(dāng)增加的節(jié)點(diǎn)中包括終點(diǎn)時(shí)，即找到一條起點(diǎn)到終點(diǎn)的有效路徑。此算法在全局路徑規(guī)劃中應(yīng)用廣泛。傳統(tǒng)RRT算法搜索仿真如圖1所示。

算法的擴(kuò)展示意圖如圖2所示。

RRT算法的偽代碼為：

其中，qstart為搜索起點(diǎn)，qrand指狀態(tài)環(huán)境中隨機(jī)采樣點(diǎn)，qnear為搜索過(guò)程的最近鄰點(diǎn)，qnew是擴(kuò)展后生成的葉子節(jié)點(diǎn)。

首先對(duì)算法進(jìn)行初始化，qstart既是擴(kuò)展樹(shù)起始點(diǎn)同時(shí)作為第一個(gè)鄰近點(diǎn)qnear，隨后在空間中隨機(jī)找尋一點(diǎn)qrand，之后距離此點(diǎn)最近距離的點(diǎn)作為下一個(gè)鄰近點(diǎn)qnear，同時(shí)以一定的擴(kuò)展步長(zhǎng)獲得新的節(jié)點(diǎn)qnew，且qnew和qnear的連線(xiàn)間不能存在障礙物，當(dāng)qnew找到終點(diǎn)或到達(dá)最大迭代次數(shù)時(shí)，算法終止。

此算法的有效性已得到充分驗(yàn)證，且在全局搜索和多維空間規(guī)劃上應(yīng)用廣泛，但算法本身也存在一定的不足。由于傳統(tǒng)算法采樣均勻、信息引導(dǎo)不充分、缺乏路徑導(dǎo)向性，因此在狹窄或多障礙物等復(fù)雜環(huán)境下易出現(xiàn)搜索時(shí)間長(zhǎng)、占用內(nèi)存大，且冗余節(jié)點(diǎn)多易導(dǎo)致路徑轉(zhuǎn)折多、距離長(zhǎng)等缺陷。

2 改進(jìn)RRT算法

針對(duì)傳統(tǒng)RRT的上述問(wèn)題進(jìn)行了改進(jìn)。首先對(duì)算法添加導(dǎo)向因子，增加路徑規(guī)劃引導(dǎo)域，同時(shí)對(duì)環(huán)境進(jìn)行柵格化，可以在低分辨率環(huán)境中搜索出有效路徑。其次去除多余采樣節(jié)點(diǎn)，可以提高收斂速度。之后改變節(jié)點(diǎn)生成方式，在引導(dǎo)域中不斷進(jìn)行擴(kuò)展搜索，找到一條可行無(wú)碰撞漸進(jìn)最優(yōu)路徑。最后進(jìn)行路徑簡(jiǎn)化，同時(shí)路徑多余節(jié)點(diǎn)剔除后進(jìn)行平滑，縮短規(guī)劃路徑的長(zhǎng)度，滿(mǎn)足機(jī)器人機(jī)械臂平穩(wěn)工作的要求。

對(duì)于相同的起止點(diǎn)，進(jìn)行改進(jìn)前后算法對(duì)比，如圖3和圖4所示。

圖3 ?兩種RRT算法路徑搜索圖

其中多岔的細(xì)線(xiàn)搜索路線(xiàn)是傳統(tǒng)RRT算法規(guī)劃路徑，較粗的路徑是改進(jìn)RRT算法規(guī)劃路徑。左下角為起始點(diǎn)，右上角為目標(biāo)點(diǎn)。

圖4 ?改進(jìn)RRT算法路徑搜索圖

從圖4可以看出，改進(jìn)的算法在路徑長(zhǎng)度上有明顯的改善，且在運(yùn)行和搜索時(shí)間上有明顯優(yōu)勢(shì)，通過(guò)對(duì)比可看出改進(jìn)算法的高效性和優(yōu)越性。

此外，經(jīng)過(guò)驗(yàn)證，在非柵格化狹窄空間中，改進(jìn)算法路徑規(guī)劃也有明顯效果，如圖5所示。

圖5 ?狹窄環(huán)境下兩種算法路徑對(duì)比

其中較細(xì)的折線(xiàn)為傳統(tǒng)RRT算法，加粗曲線(xiàn)為改進(jìn)后算法。左上角圓點(diǎn)為起始點(diǎn)，右下角圓點(diǎn)為目標(biāo)點(diǎn)。通過(guò)仿真可明顯看出，改進(jìn)后的算法剔除不必要的冗余節(jié)點(diǎn)，使規(guī)劃路徑更簡(jiǎn)，總體搜索代價(jià)更低。

3 結(jié)語(yǔ)

本文針對(duì)基本RRT算法在避障規(guī)劃中的一些不足，對(duì)其進(jìn)行了改進(jìn)。改進(jìn)算法剔除了大量冗余節(jié)點(diǎn)，并加入引導(dǎo)域指引新節(jié)點(diǎn)朝向目標(biāo)方向搜索。通過(guò)兩種算法的仿真對(duì)比，可驗(yàn)證改進(jìn)后的算法減少了隨機(jī)性，能夠高效快速地規(guī)劃出可行路徑，同時(shí)在路徑長(zhǎng)度和平滑方面更優(yōu)，更符合機(jī)器人機(jī)械臂在運(yùn)作過(guò)程中的平穩(wěn)性，整體效果更佳。

參考文獻(xiàn)

[1] 胡嘉陽(yáng)，韋巍.基于五次 NURBS 曲線(xiàn)的六軸機(jī)器人多目標(biāo)軌跡優(yōu)化[J].電子測(cè)量與儀器學(xué)報(bào)，2020，34（6）：198-203.

[2] 王洪斌，尹鵬衡，鄭維，等.基于改進(jìn)的 A～*算法與動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人路徑規(guī)劃[J].機(jī)器人，2020，42（3）：346-353

[3] 張玉偉， 左云波， 吳國(guó)新，等.基于改進(jìn)Informed-RRT*算法的路徑規(guī)劃研究[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2020（7）：5.

[4] WU X， YAN M， WANG J.An improved path planning approach based on Particle Swarm Optimization[C]//International Conference on Hybrid Intelligent Systems.IEEE，2012：157-161.

[5] BB MENG， GAO X.UAV Path Planning Based on Bidirectional Sparse A* Search Algorithm[J].IEEE，2010：1106-1109.

[6] CHEN Y B， LUO G C， MEI Y S， et al.UAV path planning using artificial potential field method updated by optimal control theory[J].International Journal of Systems Science，2016，47（6）：1407-1420.

[7] 朱佑滔，何志琴，施文燁.多種群蟻群算法在機(jī)械手臂路徑規(guī)劃中的設(shè)計(jì)與應(yīng)用[J].機(jī)械傳動(dòng)，2021，45（4）：160-165.

[8] 張衛(wèi)波，肖繼亮.改進(jìn) RRT 算法在復(fù)雜環(huán)境下智能車(chē)路徑規(guī)劃中的應(yīng)用[J].中國(guó)公路學(xué)報(bào)，2021，34（3）：225-234.

[9] Faris Janjo?， Ron Reichart， Philipp Niermeyer.Smooth Path-Generation Around Obstacles Using Quartic Splines and RRTs[J]. IFAC-PapersOnLine，2017，50（1）：9108-9113.

[10] J D GAMMELL， S S SRINIVASA， BARFOT.Informed-RRT：Optimal sampling-base path focused via direct sampling of an admissible ellipsoidal heuristic[C].International Conference on Intelligent Robots and Systems IEEE，shanghai，2014：2997-3004.

[11] 尹高揚(yáng)，周紹磊，吳青坡.基于改進(jìn) RRT 算法的無(wú)人機(jī)航跡規(guī)劃[J].電子學(xué)報(bào)，2017，45（7）：1764-1769.

[12] LAVALLE S M，KUFFNER J J. Randomized kinodynamic planning[C]∥Proc. of IEEE International Conference on Robotics and Automation. Piscataway，NJ：IEEE Press，1999：473-479.