王坤 曾國(guó)輝 魯敦科 黃勃 李曉斌

摘 要：針對(duì)帶啟發(fā)式的快速擴(kuò)展隨機(jī)樹（RRTConnect）算法路徑生成的隨機(jī)性以及漸進(jìn)最優(yōu)的雙向快速擴(kuò)展隨機(jī)樹（BRRT*）算法收斂速度的緩慢性，提出了一種基于BRRT*改進(jìn)的高效路徑規(guī)劃算法（EBRRT*）。首先引入一種智能采樣函數(shù)，使隨機(jī)樹的擴(kuò)展更具方向性，從而減少尋路時(shí)間，并提高路徑的平滑性;其次在BRRT*算法的基礎(chǔ)上，在EBRRT*算法中加入了一種快速擴(kuò)展策略，使改進(jìn)后的算法在自由空間中使用RRTConnect算法的擴(kuò)展方式進(jìn)行快速擴(kuò)展，而在障礙物空間則使用改進(jìn)的漸進(jìn)最優(yōu)的快速擴(kuò)展隨機(jī)樹（RRT*）算法進(jìn)行擴(kuò)展，在提高擴(kuò)展效率的同時(shí)避免算法陷入局部最優(yōu)。將EBRRT*算法分別與快速擴(kuò)展隨機(jī)樹（RRT）、RRTConnect、RRT* 和BRRT*算法進(jìn)行仿真對(duì)比，仿真結(jié)果表明，改進(jìn)后的算法在路徑規(guī)劃效率及路徑平滑性方面均明顯優(yōu)于其他算法;且相對(duì)于BRRT*算法，其在路徑規(guī)劃時(shí)間上降低了68.3%，在迭代次數(shù)上減少了48.6%。

關(guān)鍵詞：移動(dòng)機(jī)器人;路徑規(guī)劃;快速擴(kuò)展隨機(jī)樹;帶啟發(fā)式的快速擴(kuò)展隨機(jī)樹算法;漸進(jìn)最優(yōu)的雙向快速擴(kuò)展隨機(jī)樹算法

中圖分類號(hào)：TP242.6

文獻(xiàn)標(biāo)志碼：A

Abstract： To overcome the randomness of RRTConnect and slow convergence of BRRT*（asymptoticallyoptimal Bidirectional Rapidlyexploring Random Tree） in path generation， an efficient path planning algorithm based on BRRT*， abbreviated as EBRRT*， was proposed. Firstly， an intelligent sampling function was intriduced to achieve more directional expansion of random tree， which could improve the smoothness of path and reduce the seek time. A rapidlyexploring strategy was also added in EBRRT* by which RRTConnect exploration mode was adopted to ensure rapidly expanding in the free space and improved asymptoticallyoptimal Rapidlyexploring Random Tree （RRT*） algorithm was adopted to prevent trapped in local optimum in the obstacle space. Finally， EBRRT* algorithm was compared with Rapidlyexploring Random Tree （RRT）， RRTConnect， RRT* and BRRT* algorithms. The simulation results show that the improved algorithm is superior to other algorithms in the efficiency and smoothness of path planning. It reduced the path planning time by 68.3% and the number of iterations by 48.6% compared with BRRT* algorithm.

英文關(guān)鍵詞Key words： mobile robot; path planning; Rapidlyexploring Random Tree （RRT）; RRTConnect algorithm; asymptoticallyoptimal Bidirectional Rapidlyexploring Random Tree （BRRT*） algorithm

0 引言

隨著人工智能等新興領(lǐng)域的不斷崛起，移動(dòng)機(jī)器人的發(fā)展速度也獲得飛速提升。目前，諸如家庭、工廠以及醫(yī)院等公共場(chǎng)所對(duì)移動(dòng)機(jī)器人的需求正在不斷增加，而移動(dòng)機(jī)器人作為生產(chǎn)工具，需要經(jīng)常從一個(gè)位置移動(dòng)至另一個(gè)位置，所以如何規(guī)劃出一條即無障礙物又可以最小化消耗的路徑成為了該領(lǐng)域的一項(xiàng)熱門研究。國(guó)內(nèi)外的學(xué)者紛紛對(duì)此進(jìn)行了相關(guān)的研究，并提出了許多可行的路徑規(guī)劃理論及方法， 其中包括蟻群算法[1]、粒子群優(yōu)化算法[2]、遺傳算法[3]、人工勢(shì)場(chǎng)法[4]和A*[5]算法等。這些算法在簡(jiǎn)單環(huán)境下可以實(shí)現(xiàn)快速收斂到最優(yōu)路徑，但在處理復(fù)雜環(huán)境及高維空間時(shí)一方面收斂速度會(huì)急劇下降，另一方面其所生成的路徑并未考慮移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)約束，導(dǎo)致規(guī)劃出的路徑并不能被機(jī)器人所執(zhí)行。

為解決高維環(huán)境下路徑規(guī)劃問題，基于采樣的路徑規(guī)劃算法被提出， 其中包括概率路圖法（Probability Roadmap Method， PRM）[6]和快速擴(kuò)展隨機(jī)樹（Rapidlyexploring Random Tree， RRT）算法[7]。PRM算法作為最早的基于采樣的路徑規(guī)劃算法，其在路徑搜索的實(shí)時(shí)性與最優(yōu)性上可以滿足要求，但仍未考慮到移動(dòng)機(jī)器人的非完整約束[8]，且當(dāng)環(huán)境中存在未知障礙物時(shí)會(huì)嚴(yán)重影響其規(guī)劃效率。由于PRM算法存在的不足，LaValle等提出了一種基于采樣的RRT算法，該算法具有概率完備且收斂速度快，可應(yīng)用在未知障礙物等優(yōu)點(diǎn)，但其也存在一些缺陷[9-10]：1）由于算法采用隨機(jī)采樣狀態(tài)空間進(jìn)行擴(kuò)展的方式，使得隨機(jī)樹的擴(kuò)展沒有方向性，導(dǎo)致最終生成的路徑并非最優(yōu)路徑;2）由于算法需要探索整個(gè)未知空間，并進(jìn)行反復(fù)迭代以找到一條可行路徑，使得算法在運(yùn)行中需要消耗大量的內(nèi)存;3）由于算法的隨機(jī)性，導(dǎo)致最終生成的路徑較為粗糙，不適合移動(dòng)機(jī)器人直接采用。

對(duì)于RRT算法存在的不足，國(guó)內(nèi)外學(xué)者對(duì)其作了一定程度的改進(jìn)，其中國(guó)外較為典型的改進(jìn)有帶啟發(fā)式的快速擴(kuò)展隨機(jī)樹（RRTConnect）算法[11]、漸進(jìn)最優(yōu)的快速擴(kuò)展隨機(jī)樹（asymptoticallyoptimal Rapidlyexploring Random Tree， RRT*）算法[12]、漸進(jìn)最優(yōu)的雙向快速擴(kuò)展隨機(jī)樹（asymptoticallyoptimal Bidirectional Rapidlyexploring Random Tree， BRRT*）算法[13]和IBRRT*（Intelligent Bidirectional RRT*）算法[14]。RRTConnect算法由Kuffner等[11]于2000年提出，該算法通過并行生成兩棵隨機(jī)樹的方式來提高尋找路徑的速度。RRT*算法由Karaman等于2010年提出，該算法的提出很好地解決了RRT算法生成路徑非最優(yōu)問題，但由于在探索中計(jì)算量的增加，使其路徑生成效率大幅降低[15]。Jordan等通過借鑒RRTConnect算法思想，于2013年提出了雙向擴(kuò)展的RRT*算法，即BRRT*，同時(shí)通過改進(jìn)RRTConnect算法的連接函數(shù)，從而保證算法所連接的兩棵樹可以生成一條最優(yōu)路徑。Qureshi等[14]于2015年提出了IBRRT*算法，在BRRT*算法中引入一種智能樣本插入函數(shù)，從而提高算法收斂到最優(yōu)路徑的速度。國(guó)內(nèi)對(duì)RRT算法的研究較少，目前較早的研究為康亮等[16]于2010年提出了一種將RRT算法與滾動(dòng)窗口相結(jié)合的改進(jìn)算法，該算法克服了RRT算法只能在環(huán)境已知的條件下進(jìn)行路徑規(guī)劃的限制;馮楠[17]于2014年提出一種改進(jìn)RRTConCon的算法，提高了RRT算法的穩(wěn)定性，使其更易朝著目標(biāo)點(diǎn)擴(kuò)展;潘思宇等[18]于2017年提出一種RRT*的改進(jìn)算法，引入一種節(jié)點(diǎn)啟發(fā)式采樣函數(shù)，提高路徑規(guī)劃的速度與質(zhì)量。

本文主要針對(duì)BRRT*算法采樣的隨機(jī)性和擴(kuò)展方式的緩慢性，提出一種基于BRRT*的改進(jìn)算法EBRRT*（Efficient Bidirectional RRT*），該算法引入智能采樣函數(shù)代替原始算法中的隨機(jī)采樣函數(shù)，通過在起始點(diǎn)與目標(biāo)點(diǎn)之間采樣最優(yōu)固定點(diǎn)作為兩棵樹的目標(biāo)點(diǎn)，使隨機(jī)樹在擴(kuò)展中具有方向性，同時(shí)使用快速擴(kuò)展策略，將隨機(jī)樹的擴(kuò)展分成兩個(gè)階段：當(dāng)探索無障礙物空間時(shí)，算法采用RRTConnect算法的策略進(jìn)行快速擴(kuò)展;當(dāng)檢測(cè)到擴(kuò)展方向有障礙物時(shí)，算法啟動(dòng)改進(jìn)后的RRT*進(jìn)行擴(kuò)展，從而保證算法不會(huì)陷入局部最優(yōu)。通過仿真實(shí)驗(yàn)驗(yàn)證了改進(jìn)后的算法在有效性、實(shí)時(shí)性和優(yōu)越性上具有明顯優(yōu)勢(shì)。

4 結(jié)語

本文以BRRT*算法為基礎(chǔ)，通過將原始算法中的隨機(jī)采樣函數(shù)替換為智能采樣函數(shù)，并考慮RRTConnect算法的擴(kuò)展方式從而引入快速擴(kuò)展策略，使算法的擴(kuò)展更高效，生成路徑更平滑。通過對(duì)改進(jìn)后的算法在不同地圖中進(jìn)行仿真，其結(jié)果表明改進(jìn)后的算法在路徑生成速度、迭代次數(shù)及平滑性上相較于其他算法具有明顯優(yōu)勢(shì)。不過改進(jìn)后的算法仍存在一些不足之處，比如在擴(kuò)展方式切換時(shí)路徑會(huì)出現(xiàn)較大彎曲，下一步將嘗試在擴(kuò)展中加入平滑處理函數(shù)，使擴(kuò)展方式改變時(shí)路徑銜接更平滑。

參考文獻(xiàn) （References）

[1] DORIGO M， GAMBARELLA L M. Ant colony system： a cooperative learning approach to the traveling salesman problem[J]. IEEE Transactions on Evolutionary Computation， 1997， 1（1）： 53-66.

[2] 韓明，劉教民，吳朔媚，等. 粒子群優(yōu)化的移動(dòng)機(jī)器人路徑規(guī)劃算法[J]. 計(jì)算機(jī)應(yīng)用， 2017， 37（8）： 2258-2263.（HAN M， LIU J M， WU S M， et al. Path planning algorithm of mobile robot based on particle swarm optimization[J]. Journal of Computer Applications， 2017， 37（8）： 2258-2263.）

[3] OZDIKIA O. Genetic algorithm with random coordinates for route planning on a 3D terrain[C]// Proceedings of the 2011 5th International Conference on Genetic and Evolutionary Computing. Washington， DC： IEEE Computer Society， 2011： 146-149.

[4] CHEN T B， ZHANG Q. Robot motion planning based on improved artificial potential field[C]// Proceedings of the 2013 3rd International Conference on Computer Science and Network Technology. Piscataway， NJ： IEEE， 2013： 1208-1211.

[5] LIN M， YUAN K， SHI C， et al. Path planning of mobile robot based on improved A* algorithm[C]// Proceedings of the 2017 29th Chinese Control and Decision Conference. Piscataway， NJ： IEEE， 2017： 3570-3576.

[6] KAVRAKI L E， SVESTKA P， LATOMBE J C， et al. Probabilistic roadmaps for path planning in highdimensional configuration spaces [J]. IEEE Transactions on Robotics and Automation， 1996， 12（4）： 566-580.

[7] LAVALLE S M， KUFFNER J J. Randomized Kinodynamic planning [C]// Proceedings of the 1999 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 1999： 473-479.

[8] 宋金澤，戴斌，單恩忠，等. 一種改進(jìn)的RRT路徑規(guī)劃算法[J]. 電子學(xué)報(bào)， 2010， 38（Z1）： 225-228.（SONG J Z， DAI B， SHAN E Z， et al. An improved RRT path planning algorithm[J]. Acta Electronica Sinica， 2010， 38（Z1）： 225-228.）

[9] GAMMELL J D， SRINIVASA S S， BARFOOT T D. Informed RRT*： optimal samplingbased path planning focused via direct sampling of an admissible ellipsoidal heuristic[C]// Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE， 2014： 2997-3004.

[10] DOSHI A A， POSTULA A J， FLETCHER A， et al. Development of microUAV with integrated motion planning for opencut mining surveillance [J]. Microprocessors and Microsystems， 2015， 39（8）： 829-835.

[11]KUFFNER J J， LAVALLE S M. RRTconnect： an efficient approach to singlequery path planning[C]// Proceedings of the 2000 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2000： 995-1001.

莫棟成，劉國(guó)棟. 改進(jìn)的RRTConnect雙足機(jī)器人路徑規(guī)劃算法[J].計(jì)算機(jī)應(yīng)用， 2013， 33（8）： 2289-2292.（MO D C， LIU G D. Improved RRTConnect path planning algorithm for biped robot [J]. Journal of Computer Applications， 2013， 33（8）： 2289-2292.）

[12] KARAMAN S， FRAZZOLI E. Samplingbased algorithms for optimal motion planning[J]. The International Journal of Robotics Research， 2011， 30（7）： 846-894.

[13] JORDAN M， PEREZ A. Optimal bidirectional rapidlyexploring random trees [EB/OL].[2018-03-20]. https：//dspace.mit.edu/bitstream/handle/1721.1/79884/MITCSAILTR-2013021.pdf.

[14] QURESHI A H， AYAZ Y. Intelligent bidirectional rapidlyexploring random trees for optimal motion planning in complex cluttered environments*[J]. Robotics and Autonomous Systems， 2015， 68： 1-11.

[15] OLZHAS A， HUSEYIN A V. A novel RRT*based algorithm for motion planning in dynamic environments[C]// Proceedings of the 2017 IEEE International Conference on Mechatronics and Automation. Piscataway， NJ： IEEE， 2017： 1416-1421.

[16] 康亮，趙春霞，郭劍輝. 基于模糊滾動(dòng)RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃[J]. 南京理工大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2010， 34（5）： 642-648. （KANG L， ZHAO C X， GUO J H. Path planning based on fuzzy rolling rapidlyexploring random tree for mobile robot[J]. Journal of Nanjing University of Science and Technology （Natural Science）， 2010， 34（5）： 642-648.

[17] 馮楠. 自主移動(dòng)機(jī)器人路徑規(guī)劃的RRT算法研究[D]. 大連： 大連理工大學(xué)， 2014. （FENG N. Research on RRT algorithm of path planning for autonomous mobile robot[D]. Dalian： Dalian University of Technology， 2014.

[18] 潘思宇，徐向榮. 基于改進(jìn)RRT*的移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃算法[J]. 山西大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2017， 40（2）： 244-254.（PAN S Y， XU X R. Improved RRT*based motion planning algorithm for mobile robot [J]. Journal of Shanxi University （Natural Science Edition）， 2017， 40（2）： 224-254.）

[19] ZAID T， AHMED H Q， YASAR A， et al. Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments[J]. Robotics and Autonomous Systems， 2018， 108： 13-27.

[20] NOREEN I， KHAN A， HABIB Z. Optimal path planning using RRT* based approaches： a survey and future directions[J]. International Journal of Advanced Computer Science & Application， 2016， 7（11）： 97-107.

[21] IRAM N， AMNA K， HYEJEONG R， et al. Optimal path planning in cluttered environment using RRT*AB[J]. Intelligent Service Robotics， 2017， 11（1）： 41-52.