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

黃文青　陳凌珊　李婷婷　尚大偉

摘 要： RRT算法是一種能夠處理障礙物和差分約束的問題的算法，被廣泛應(yīng)用于移動(dòng)機(jī)器人的路徑規(guī)劃。針對(duì)于基本RRT算法存在的隨機(jī)性較大和所求解路徑非最優(yōu)等問題，需要對(duì)其進(jìn)行改進(jìn)從而優(yōu)化性能與運(yùn)行效率。本文主要采用雙向RRT算法融合人工勢(shì)場(chǎng)法的方案進(jìn)行改進(jìn)后的路徑規(guī)劃，然后借助Dijkstra算法進(jìn)一步處理所求解的路徑，以尋求路徑的最優(yōu)解。仿真結(jié)果表明，本方案可以減少基本RRT算法隨機(jī)性的影響，提高移動(dòng)機(jī)器人路徑規(guī)劃的效率。

關(guān)鍵詞： 移動(dòng)機(jī)器人; 改進(jìn)RRT算法; 融合人工勢(shì)場(chǎng)法; 路徑規(guī)劃

文章編號(hào)： 2095-2163（2021）07-0032-05中圖分類號(hào)：TP242文獻(xiàn)標(biāo)志碼： A

Mobile robot path planning based on improved RRT algorithm

HUANG Wenqing， CHEN Lingshan，? LI Tingting， SHANG Dawei

（School of Mechanical and Automotive Engineering， Shanghai University of Engineering Science， Shanghai 201620， China）

【Abstract】RRT algorithm is an algorithm that can deal with obstacles and differential constraints， which is widely used in path planning of mobile robots. In view of the large randomness of the basic RRT algorithm and the non-optimal path of the solution， it needs to be improved to optimize performance and operating efficiency. This paper mainly adopts the two-way RRT algorithm fusing artificial potential field method to carry out the improved path planning， and then uses the Dijkstra algorithm to further process the solved path for finding the optimal solution of the path. The simulation results show that this scheme can reduce the influence of the randomness of the basic RRT algorithm and improve the efficiency of mobile robot path planning.

【Key words】mobile robot; improved RRT algorithm; fused artificial potential field method; path planning

0 引 言

隨著智能制造行業(yè)的快速發(fā)展，移動(dòng)機(jī)器人正在發(fā)揮著越來越重要作用，人們對(duì)其的需求量也在不斷增加。在使用過程中，移動(dòng)機(jī)器人需要自主、安全且快速地避開障礙物，并找到到達(dá)目標(biāo)位置的可行路徑，因此路徑規(guī)劃問題是移動(dòng)機(jī)器人研究領(lǐng)域中的一個(gè)熱點(diǎn)[1]。國內(nèi)外學(xué)者針對(duì)此問題提出了許多可行的方法，常用的有A*算法、人工勢(shì)場(chǎng)法、概率路線圖（Probability Roadmap Method，PRM）算法、蟻群算法、遺傳算法、粒子群優(yōu)化算法等[2]。

作為一種能解決高維環(huán)境下路徑規(guī)劃問題的有效算法，快速搜索隨機(jī)樹（Rapidly Exploring Random Tree，RRT）最早由Lavalle等人提出[3]。相比其它算法，RRT算法的收斂速度較快，能夠保證概率完備性[4]。當(dāng)然，由于隨機(jī)采樣的特點(diǎn)，RRT算法的求解效率比較低。此外，其生成的路徑結(jié)果比較粗糙，只是一個(gè)可行解而并非最優(yōu)路徑[5]。針對(duì)RRT算法存在的問題，國內(nèi)外學(xué)者嘗試了很多種改進(jìn)方法。其中，國外比較有代表性的有RRT-Connect算法（Kuffner等人于2000年提出）[6]、RRT*算法（漸進(jìn)最優(yōu)）[7]、B-RRT*算法（有選擇性地產(chǎn)生新節(jié)點(diǎn)）[8]、IB-RRT*算法（在B-RRT*算法的基礎(chǔ)上進(jìn)一步提高搜索速度）[9]和RRT*-Connect算法（使所求解的路徑向最優(yōu)解收斂）[10]等。國內(nèi)對(duì)于RRT算法的研究相對(duì)晚一些，王道威等人[11]在2016年提出了動(dòng)態(tài)步長(zhǎng)的RRT算法，潘思宇等人[12]在2017年提出了一種引入節(jié)點(diǎn)啟發(fā)式采樣函數(shù)的RRT*算法，陳波芝等人[13]在2018年提出了用于雙機(jī)械臂避障的改進(jìn)RRT算法。

本文采用雙向RRT算法融合人工勢(shì)場(chǎng)法進(jìn)行改進(jìn)后的路徑規(guī)劃，再利用Dijkstra算法對(duì)所求得的路徑再次優(yōu)化，使得移動(dòng)機(jī)器人能夠迅速有效地避開各類障礙物，以提高路徑規(guī)劃的效率。

1 基本RRT算法

1.1 RRT算法的基本思想

對(duì)于移動(dòng)機(jī)器人而言，路徑規(guī)劃是指其能夠在具有障礙物的較復(fù)雜環(huán)境中找到一條由起始點(diǎn)qinitial抵達(dá)目標(biāo)點(diǎn)qgoal的路徑，且在運(yùn)動(dòng)過程中不碰到周圍的障礙物[14]，如圖1所示。

作為一種常用的路徑規(guī)劃算法，RRT算法是適用于高維空間和復(fù)雜約束的問題，這里的約束主要是由障礙物造成的代數(shù)約束和由環(huán)境變化帶來的微分約束。其基本思想是移動(dòng)機(jī)器人在向目標(biāo)點(diǎn)運(yùn)動(dòng)的過程中借助產(chǎn)生的隨機(jī)點(diǎn)來決定步長(zhǎng)，從而避開障礙物。求解時(shí)不需要對(duì)移動(dòng)機(jī)器人進(jìn)行系統(tǒng)建模，也無需對(duì)搜索區(qū)域進(jìn)行幾何劃分，搜索的范圍較廣、覆蓋率較高。

1.2 RRT算法的具體過程

根據(jù)RRT算法的基本思想，若要構(gòu)建RRT算法，必須要先明確算法的輸入與輸出。RRT算法的輸入主要有環(huán)境信息、起終點(diǎn)位置、節(jié)點(diǎn)的生成次數(shù)以及隨機(jī)點(diǎn)與最近樹節(jié)點(diǎn)的距離;輸出主要有隨機(jī)樹的頂點(diǎn)與邊、起終點(diǎn)間的原始路徑、生成的連接起終點(diǎn)的隨機(jī)樹以及平滑處理后的優(yōu)化路徑，如圖2所示。

總地來說，RRT算法的過程可以分為3部分：插入新的節(jié)點(diǎn)、障礙物檢測(cè)和非完整約束檢測(cè)。當(dāng)且僅當(dāng)這兩類檢測(cè)均滿足要求時(shí)，才能加入新的節(jié)點(diǎn)。其偽代碼如下：

其中，rand（ ）的作用是在環(huán)境中產(chǎn)生隨機(jī)點(diǎn);neighbour（ ）的作用是找出隨機(jī)樹上距離隨機(jī)點(diǎn)最近的節(jié)點(diǎn);input（ ）的作用是依據(jù)隨機(jī)點(diǎn)和鄰近節(jié)點(diǎn)的特征擴(kuò)展隨機(jī)樹;state（ ）的作用是生成新的樹節(jié)點(diǎn);judge（ ）和if（ ）主要是進(jìn)行障礙物檢測(cè)和約束判斷。

2 RRT算法的改進(jìn)

針對(duì)RRT算法在較復(fù)雜環(huán)境中隨機(jī)性較大、收斂速度較慢和運(yùn)行效率較低等問題，本文主要做出如下改進(jìn)。

2.1 雙向RRT算法

基本RRT算法的搜索過程是從起始點(diǎn)qinitial開始生成隨機(jī)樹，從而延伸至整個(gè)狀態(tài)空間。很明顯，當(dāng)狀態(tài)空間的環(huán)境較為復(fù)雜時(shí)，譬如障礙物較多或者運(yùn)行路徑較狹小時(shí)，算法的收斂速度會(huì)大幅下降，導(dǎo)致需要花費(fèi)很長(zhǎng)的時(shí)間來計(jì)算路徑，有時(shí)甚至得不到結(jié)果，如圖3所示。

針對(duì)上述問題，雙向RRT算法能夠從目標(biāo)點(diǎn)qgoal生成隨機(jī)樹，也是進(jìn)行隨機(jī)采樣并擴(kuò)展，這就使得對(duì)狀態(tài)空間的搜索效率得到提高。需要提出的是，從目標(biāo)點(diǎn)qgoal生成的隨機(jī)樹擴(kuò)展方式是不同的，其傾向于朝起始點(diǎn)qinitial的隨機(jī)樹新節(jié)點(diǎn)qnew擴(kuò)展。在整個(gè)路徑的搜尋過程中，這兩棵隨機(jī)樹能夠交替擴(kuò)展。當(dāng)qgoal的隨機(jī)樹無法擴(kuò)展，或者其新節(jié)點(diǎn)qnew與qnew重合時(shí)，退出算法。此時(shí)，由目標(biāo)點(diǎn)qgoal和起始點(diǎn)qinitial生成的2棵隨機(jī)樹互相連接，一個(gè)路徑的可行解就得到了，如圖4所示。很顯然，相對(duì)于基本RRT算法的搜索過程，這種雙向搜索過程步長(zhǎng)更大，隨機(jī)樹的生長(zhǎng)速度也更快。這使得雙向RRT算法的運(yùn)算效率更快。

由于目標(biāo)點(diǎn)qgoal隨機(jī)樹的擴(kuò)展方式有所不同，且也涉及到障礙物的判斷與碰撞檢測(cè)，這里給出部分偽代碼見如下。

2.2 人工勢(shì)場(chǎng)法的融合

為了進(jìn)一步提高RRT算法的搜索效率，降低隨機(jī)性，本文利用人工勢(shì)場(chǎng)法的目標(biāo)導(dǎo)向特征來引導(dǎo)隨機(jī)樹向著目標(biāo)點(diǎn)qgoal擴(kuò)展，使RRT算法能盡快完成路徑規(guī)劃。

人工勢(shì)場(chǎng)法最早是Khatib提出的[15]，主要定義了目標(biāo)點(diǎn)qgoal的引力勢(shì)場(chǎng)和斥力勢(shì)場(chǎng)，繼而引導(dǎo)移動(dòng)機(jī)器人朝著勢(shì)場(chǎng)函數(shù)負(fù)梯度方向運(yùn)動(dòng)，從而避免與障礙物碰撞。其中，引力勢(shì)場(chǎng)函數(shù)為：

斥力勢(shì)場(chǎng)函數(shù)為：

相應(yīng)地，其負(fù)梯度分別為：

其中，kp為引力系數(shù);kr為斥力系數(shù);xgoal為目標(biāo)點(diǎn)qgoal的位置;y為移動(dòng)機(jī)器人與障礙物的距離;y0為障礙物的斥力作用的最大范圍。

根據(jù)人工勢(shì)場(chǎng)法的特性，其與雙向RRT法融合可以進(jìn)一步修正RRT算法的隨機(jī)樹擴(kuò)展方向，提高算法的實(shí)時(shí)性，避免產(chǎn)生局部極小值，優(yōu)化移動(dòng)機(jī)器人路徑規(guī)劃的能力。

為此，需要給隨機(jī)樹的節(jié)點(diǎn)qnew構(gòu)造目標(biāo)引力函數(shù)P（qnew）。目標(biāo)引力函數(shù)和隨機(jī)樹的擴(kuò)展機(jī)制共同作用，繼而引導(dǎo)節(jié)點(diǎn)qnew朝著目標(biāo)點(diǎn)延伸。因此，qnew的擴(kuò)展函數(shù)可表示為：

其中，T（qnew）為RRT算法的隨機(jī)擴(kuò)展函數(shù)。

由此構(gòu)造出的目標(biāo)引力函數(shù)為：

其中，ρ為引力系數(shù)。當(dāng)ρ取不同的值時(shí)，隨機(jī)樹向目標(biāo)點(diǎn)qgoal的導(dǎo)向性是不同的。經(jīng)過與人工勢(shì)場(chǎng)法的融合，RRT算法隨機(jī)點(diǎn)qnew的生成可以通過如下公式進(jìn)行計(jì)算：

2.3 路徑處理

由于雙向RRT算法仍具有一定的隨機(jī)性，其與人工勢(shì)場(chǎng)法融合后還是會(huì)有多余的隨機(jī)點(diǎn)。由于不具有目標(biāo)點(diǎn)的導(dǎo)向性，這些隨機(jī)節(jié)點(diǎn)并沒有意義，需要對(duì)其進(jìn)行刪減。為此，本文選擇Dijkstra算法處理所求路徑結(jié)果。

Dijkstra算法是以起始點(diǎn)作中心逐步向外求解到圖中（一般為有向圖）其余頂點(diǎn)的最短路徑，直到抵達(dá)目標(biāo)點(diǎn)。一般來說，Dijkstra算法主要分為3部分，對(duì)此擬做闡釋分述如下。

（1）初始化起始點(diǎn)、目標(biāo)點(diǎn)、步長(zhǎng)等信息，定義相應(yīng)數(shù)組來所求結(jié)果。

（2）循環(huán)迭代，依次求解起始點(diǎn)到各頂點(diǎn)之間的最短路徑。因?yàn)槊看吻蠼舛忌婕耙粋€(gè)頂點(diǎn)，所以循環(huán)次數(shù)比頂點(diǎn)總數(shù)少1。

（3）根據(jù)結(jié)果將相關(guān)頂點(diǎn)和距離信息存入對(duì)應(yīng)數(shù)組中。

具體來說，連接隨機(jī)樹中的各個(gè)節(jié)點(diǎn)qinitial、q1、q2、……、qgoal，碰到障礙物就停止，并存儲(chǔ)當(dāng)前節(jié)點(diǎn)qt，然后從下一個(gè)節(jié)點(diǎn)qt+1起再次連接至qgoal，過程中如有障礙物做類似處理。最后，數(shù)組{qt}中的每個(gè)元素就是處理后的路徑上的各節(jié)點(diǎn)。

綜上所述，整個(gè)改進(jìn)算法的流程如圖5所示。

3 仿真試驗(yàn)

本次改進(jìn)RRT算法試驗(yàn)的仿真平臺(tái)為Matlab R2017b，環(huán)境的空間范圍為550 mm*550 mm，各類黑色的幾何形狀為障礙物。初始化移動(dòng)機(jī)器人設(shè)置，起始點(diǎn)qinitial的坐標(biāo)設(shè)為（10，10），目標(biāo)點(diǎn)qgoal的坐標(biāo)設(shè)為（500，500）。RRT算法的步長(zhǎng)設(shè)為5，最大循環(huán)次數(shù)為10 000。

基本RRT算法的搜索過程和生成路徑如圖6所示，改進(jìn)算法的搜索過程和生成路徑如圖7所示。表1記錄的是這2種情形下的路徑長(zhǎng)度和規(guī)劃所用時(shí)間。

由此可見，改進(jìn)后的算法能夠規(guī)劃出更優(yōu)化的路徑，而且所用時(shí)間更短。

4 結(jié)束語

本文分析了基本RRT算法的特征，并對(duì)其存在的相關(guān)問題進(jìn)行改進(jìn)。通過雙向RRT算法與人工勢(shì)場(chǎng)法的融合，路徑搜索過程的目標(biāo)導(dǎo)向性更強(qiáng)，路徑得到了優(yōu)化，時(shí)間也進(jìn)一步縮短，具有良好的應(yīng)用前景。

參考文獻(xiàn)

[1]王坤， 曾國輝， 魯敦科， 等. 基于改進(jìn)漸進(jìn)最優(yōu)的雙向快速擴(kuò)展隨機(jī)樹的移動(dòng)機(jī)器人路徑規(guī)劃算法[J]. 計(jì)算機(jī)應(yīng)用， 2019， 39（5）： 1312-1317.

[2]SFEIR J， SAAD M， Saliah-Hassane H： An improved artificial potential field approach to real-time mobile robot path planning in an nnknown environment [C]// IEEE International Symposium on Robotic and Sensors Environments. QC， Canada ：IEEE，2011：208-213.

[3]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.

[4]NGUYEN M K， LEONARD J， STEPHANE R. ART-RRT： As-rigid-as-possible exploration of ligand unbinding pathways [J]. Journal of Computational Chemistry， 2018， 39（11）：665-678.

[5]徐秉超， 嚴(yán)華. 一種改進(jìn)的雙向快速搜索隨機(jī)樹算法[J]. 科學(xué)技術(shù)與工程， 2020， 20（19）： 7765-7771.

[6]KUFFNER J J， LAVALLE S M. RRT-connect： An efficient approach to single-query path planning[C]//Proceedings of the 2000 IEEE International Conference on Robotics and Automation. Piscataway，NJ： IEEE，2000：995-1001.

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

[8]Jordan M， Perez A. Optimal bidirectional rapidly-exploring random trees [R]. Cambridge， MA ：Computer Science and Artificial Intelligence Laboratory， Massachusetts Institute of Technology， 2013.

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

[10]KLEMM S， OBERLANDER J， HERMANN A， et al. RRT*-Connect： Faster， asymptotically optimal motion planning[C]//2015 IEEE International Conference on Robotics and Biomumetics. Seattle， WA，USA：IEEE， 2015：1670-1677.

[11]王道威， 朱明富， 劉慧. 動(dòng)態(tài)步長(zhǎng)的RRT路徑規(guī)劃算法[J]. 計(jì)算機(jī)技術(shù)與發(fā)展， 2016（3）： 105-107，112.

[12]潘思宇， 徐向榮. 基于改進(jìn)RRT*的移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃算法[J]. 山西大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2017， 40（2）： 244- 254.

[13]陳波芝， 陸亮， 雷新宇， 等. 基于改進(jìn)快速擴(kuò)展隨機(jī)數(shù)算法的雙機(jī)械臂協(xié)同避障規(guī)劃方法[J]. 中國機(jī)械工程， 2018， 29（10）： 1220- 1226.

[14]金丹. 基于改進(jìn)RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃[J]. 現(xiàn)代計(jì)算機(jī)， 2018（6）： 41-44.

[15]KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[M]//COX I J， WILFONG G T. Autonomous robot behicles. New York， NY ：Springer，1986：396-404

基金項(xiàng)目： 上海工程技術(shù)大學(xué)研究生科研創(chuàng)新項(xiàng)目（0231-E3-0903-19-01213）。

作者簡(jiǎn)介： 黃文青（1996-），男，碩士研究生，主要研究方向：汽車智能化控制和測(cè)試。

通訊作者： 黃文青Email：394737977@qq.com

收稿日期： 2020-12-24