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

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

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

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

文章編號： 2095-2163（2021）07-0032-05中圖分類號：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ā)展，移動機器人正在發(fā)揮著越來越重要作用，人們對其的需求量也在不斷增加。在使用過程中，移動機器人需要自主、安全且快速地避開障礙物，并找到到達(dá)目標(biāo)位置的可行路徑，因此路徑規(guī)劃問題是移動機器人研究領(lǐng)域中的一個熱點[1]。國內(nèi)外學(xué)者針對此問題提出了許多可行的方法，常用的有A*算法、人工勢場法、概率路線圖（Probability Roadmap Method，PRM）算法、蟻群算法、遺傳算法、粒子群優(yōu)化算法等[2]。

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

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

1 基本RRT算法

1.1 RRT算法的基本思想

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

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

1.2 RRT算法的具體過程

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

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

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

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

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

2.1 雙向RRT算法

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

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

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

2.2 人工勢場法的融合

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

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

斥力勢場函數(shù)為：

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

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

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

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

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

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

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

2.3 路徑處理

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

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

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

（2）循環(huán)迭代，依次求解起始點到各頂點之間的最短路徑。因為每次求解都涉及一個頂點，所以循環(huán)次數(shù)比頂點總數(shù)少1。

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

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

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

3 仿真試驗

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

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

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

4 結(jié)束語

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

參考文獻(xiàn)

[1]王坤， 曾國輝， 魯敦科， 等. 基于改進(jìn)漸進(jìn)最優(yōu)的雙向快速擴展隨機樹的移動機器人路徑規(guī)劃算法[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]. 科學(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]王道威， 朱明富， 劉慧. 動態(tài)步長的RRT路徑規(guī)劃算法[J]. 計算機技術(shù)與發(fā)展， 2016（3）： 105-107，112.

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

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

[14]金丹. 基于改進(jìn)RRT算法的移動機器人路徑規(guī)劃[J]. 現(xiàn)代計算機， 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

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

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

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

收稿日期： 2020-12-24