基于方向決策RRT算法的移動機器人路徑規(guī)劃

劉想德，何翔鵬，胡 勇，胡小林

(1. 重慶郵電大學信息無障礙工程與機器人技術研發(fā)中心，重慶 400065；2. 重慶工業(yè)大數(shù)據(jù)創(chuàng)新中心有限公司，重慶 400000)

1 引言

移動機器人路徑規(guī)劃[1]是實現(xiàn)機器人自主導航的關鍵技術之一，其目的是使機器人在障礙物約束環(huán)境中，通過路徑長度、搜索時間，平滑度以及避障能力等性能指標，求解出一條從起始點到目標點的最優(yōu)或近似最優(yōu)路徑。

傳統(tǒng)的移動機器人路徑規(guī)劃算法包括dijkstra算法[2]、遺傳算法[3]、A*算法[4]、粒子群算法[5]等，然而面對復雜約束的機器人應用場景，這些算法往往需要大量的計算力。而Lavalle提出的快速擴展隨機樹(Rapidly-Exploring Random Tree， RRT)算法，在狀態(tài)空間中進行隨機碰撞檢測，將搜索空間逐漸擴展至可通行區(qū)域，進而搜索出一條從起始點到目標點的可行路徑[6-8]，避免了對規(guī)劃空間建模，運行速度快，被廣泛應用于移動機器人路徑規(guī)劃中。

傳統(tǒng)RRT算法應用于機器人路徑規(guī)劃時，由于擴展節(jié)點的隨機性大，導致求解過程中存在冗余搜索，且生成的路徑僅為可通行路徑，路徑長度以及平滑度并非最優(yōu)解。針對這些問題，Jian-Quan L等人[9]采用雙向RRT算法，從起始點和目標點同時進行擴展，進而提高求解效率。然而雙向RRT算法的求解路徑平滑度低，不符合機器人運動學規(guī)律。朱宏輝等人[10]通過選取最小成本的擴展節(jié)點，優(yōu)化了RRT算法的路徑長度，但犧牲了算法的收斂速度，導致搜索時間增加。賀伊琳等人[11]將目標偏向策略引入RRT算法，以固定概率將擴展節(jié)點向目標點延伸，減小了隨機性，提高了算法的運行效率。值得注意的是，面對復雜場景，基于偏向策略的RRT算法極易陷入局部最小狀態(tài)。此外，許多學者將RRT與其它智能算法混合，從而提高算法的求解效率。康搖等人[12]結合RRT和滾動窗口算法增強了算法搜索未知空間的能力。Zhou F等人[13]結合RRT和粒子群算法求解最優(yōu)路徑。劉恩海等人[14]提高了RRT算法運用在移動機器人時的避障能力，但是優(yōu)化過程需要大量運算，導致算法運行效率降低。

本文提出一種基于方向決策的RRT算法，通過引入方向信息決策機制，引導新節(jié)點趨向目標點擴展，減小無效迭代次數(shù)，進而縮短路徑搜索時間。同時，將一種啟發(fā)式采樣策略引入所提算法中，使搜索樹能夠快速跳出局部最小點，利用三次b樣條曲線對規(guī)劃路徑進行平滑處理。最后，通過仿真，驗證了本文算法的優(yōu)越性。

2 雙輪差速移動機器人運動模型

在輪式移動機器人中，雙輪差動機器人由于結構緊湊、轉彎靈活、易于控制等特點，被廣泛應用于工業(yè)、搜救領域，其運動方式由旋轉運動和平移運動組成[15]。

圖1 移動機器人運動模型

圖1中(a)表示雙輪差動機器人旋轉運動模型。其中，機器人左輪速度為vl，右輪速度為vr，旋轉角度為θ，則機器人線速度和角速度可分別描述為

(1)

(2)

機器人運動半徑為

(3)

機器人平移運動模型如圖1中(b)所示。設t-1時刻機器人位姿Pt-1=(xt-1，yt-1，θt-1)，當Δt時間內機器人以線速度v和角速度w移動，則t時刻移動機器人的位姿可表示為

(4)

移動機器人所有時刻的位姿構成了機器人的實際運行路徑。

3 傳統(tǒng)RRT算法

傳統(tǒng)RRT算法的移動機器人路徑規(guī)劃，采用狀態(tài)空間內隨機采樣的方式擴展節(jié)點，將搜索樹從初始點不斷擴展，最終延伸至目標點。將機器人起始位置坐標作為根節(jié)點vinit，并在狀態(tài)空間內隨機生成一個隨機節(jié)點vrand，基于與vrand距離最短原則，搜索樹生成新節(jié)點vnear。為了形成碰撞判斷機制，vnear與vrand的相連形成線段lnear，在lnear上擴展一個測試節(jié)點vnew，其擴展步長為給定常數(shù)，判斷vnew以及l(fā)near是否與障礙物碰撞，若存在碰撞，則放棄當前節(jié)點vnear，重新選擇，并循環(huán)障礙物碰撞判斷過程。通過不斷構造新的子結點vnew，直到節(jié)點vnew與目標節(jié)點vgoal之間的距離小于一個擴展步長，且沒有發(fā)生障礙物碰撞，則擴展隨機樹構建成功。搜索樹的擴展過程如圖2所示。

圖2 傳統(tǒng)RRT算法擴展過程

4 改進RRT算法

4.1 基于方向決策機制

本文改進的RRT算法，為搜索樹上的節(jié)點引入方向變量orinode，首先確定根節(jié)點的orinode參數(shù)值為根節(jié)點vinit與目標節(jié)點vgoal的相對角度偏差值，可表示為

(5)

其中vinit和vgoal的直角坐標分別為(xinit，yinit)，(xgoal，ygoal)，Δy=(ygoal-yinit)，Δx=(xgoal-xinit)。

在搜索樹中引入方向變量后，基于啟發(fā)式采樣策略求得隨機節(jié)點vrand，在搜索樹上檢索與vrand距離最短的節(jié)點vnear。于此同時，獲取方向變量orinear，以orinear為基準，在節(jié)點vnear添加三個探索式節(jié)點vins1、vins2、vins3，添加探索式結點示意圖如圖3所示。

圖3 探索式結點示意圖

三個探索結點坐標可分別表示為

(6)

(7)

然后，計算探索式節(jié)點與vgoal的距離d，表示為

(8)

其中，探索節(jié)點的直角坐標為(xins，yins)，三個探索式節(jié)點vins1、vins2、vins3與vgoal的距離分別表示為d1、d2、d3(d1

(9)

使用三個探索節(jié)點與搜索樹中Vnear節(jié)點進行碰撞檢測，則反向擴展節(jié)點生成vnew，此時vnew的狀態(tài)如下

(10)

將擴展后的vnew節(jié)點加入搜索樹。重復執(zhí)行以上過程，直至vnew與vgoal之間的距離小于一個擴展步長，且無碰撞發(fā)生。即可以得到一條從起始點到目標點之間的較優(yōu)路徑規(guī)劃結果。

圖4所示為引入方向變量的RRT算法擴展過程，虛線表示該探索式節(jié)點與障礙物發(fā)生碰撞，紅色結點表示節(jié)點vnew，黃色線段為最終求解路徑。

圖4 改進RRT算法擴展過程

4.2 啟發(fā)式采樣策略

傳統(tǒng)RRT算法在移動機器人的路徑規(guī)劃中，存在以下缺點：

1)由于在狀態(tài)空間中隨機選取采樣點，求解過程中存在較多冗余擴展，導致算法求解效率低下。

2)面對多障礙物約束環(huán)境，特別是狹窄通道時，傳統(tǒng)RRT算法的搜索樹擴展需要進行大量運算，才能實現(xiàn)路徑規(guī)劃。

本節(jié)介紹一種啟發(fā)式采樣策略，保證搜索樹向目標節(jié)點擴展的同時，也增強了算法對復雜環(huán)境的適應性。具體步驟如下：

定義變量vnear′和vnear″分別記錄隨機樹搜索過程中最后兩次的擴展節(jié)點，定義一個概率閾值變量p。隨機生成一個概率值q。如果vnear′等于vnear″，則說明隨機樹可能陷入局部最小點，則比較p與q的值，然后執(zhí)行隨機采樣過程，若vnear′≠vnear″，則判斷當前最近點vnear′與vgoal，vnear″與vgoal之間是否發(fā)生碰撞，若未發(fā)生碰撞，則直接將vrand設定為vgoal，反之則執(zhí)行隨機采樣。

融入啟發(fā)式采樣策略后系統(tǒng)整體流程圖如圖5所示。

圖5 啟發(fā)式采樣策略流程圖

4.3 三次B樣條曲線擬合

利用RRT算法求解移動機器人路徑規(guī)劃，雖然可以得到一條無碰撞發(fā)生的可通行路徑，但生成的路徑存在多個折點，導致機器人在移動過程中出現(xiàn)卡頓感。

為了優(yōu)化生成路徑，利用三次B樣條插值擬合法對RRT生成的路徑進行平滑處理，提升路徑的連續(xù)性和平滑性。

B樣條曲線定義為給定m+n+1個平面內控制點Pi(i=0，1，…，m+n)，稱n次參數(shù)曲線段，可表示為：

(11)

這些曲線段的總和稱為n次B樣條曲線。其中Gi，n(t)為基函數(shù)，定義為：

(12)

當式(12)中n取3時，即為3次B樣條，3次B樣條算法的基函數(shù)為

(13)

3次B樣條算法曲線方程為：

(14)

利用3次B樣條算法對改進RRT進行路徑平滑，圖5中(a)， (b)分別為3次B樣條處理前后的得到的路徑。

可見經平滑處理后的路徑連續(xù)性和平滑性得到了優(yōu)化。

圖6 規(guī)劃路徑平滑前后對比圖

5 實驗結果與分析

利用Matlab2016b在兩種環(huán)境下對比傳統(tǒng)RRT算法、基于目標偏向RRT算法和改進RRT算法的性能。

環(huán)境1下實驗結果如圖7(a)，(b)，(c)所示，環(huán)境2下實驗結果如圖7(d)，(e)，(f)所示。環(huán)境地圖尺寸均為800m×800m，黑色區(qū)域表示障礙物，擴展步長S為25m，概率閾值p為0.3。環(huán)境1為簡單場景，機器人起始點坐標為(50m，50m)點，目標點坐標為(750m，750m)點。環(huán)境2為較復雜場景，機器人起始點坐標為(50m，750m)點，目標點坐標為(750m，750m)點。藍色粗線條為不同算法下機器人的求解規(guī)劃路徑。

圖7 算法路徑規(guī)劃結果對比圖

從圖7中可以看到，在環(huán)境1中，傳統(tǒng)的RRT算法由于缺乏導向性，僅通過隨機采樣的方式拓展節(jié)點，導致出現(xiàn)大量無效拓展，且求解路徑非最優(yōu)解。而基于目標偏向的RRT算法雖然可以得到一條較優(yōu)路徑，但是當目標點和當前位置的連接線上存在障礙物時，會出現(xiàn)較多無效擴展，從圖8中可以看到，在較復雜場景中，傳統(tǒng)RRT算法和目標偏向性RRT算法除上述缺點之外，若環(huán)境中障礙物較為密集時，會產生更多的無效擴展，而改進RRT算法，能夠在進行較少迭代后跳出了這片區(qū)域。

圖8 算法迭代次數(shù)對比圖

圖8(a)、(b)是以上三種算法的迭代次數(shù)對比圖，圖中可以看出，在兩種環(huán)境下，本文改進RRT算法均能在較少迭代次數(shù)內收斂。

考慮到RRT算法是一種隨機性算法，為了驗證實驗結果具有普遍適應性，分別在兩種環(huán)境下進行30次試驗，得到的平均數(shù)據(jù)記錄于表1表示。

表1 不同環(huán)境地圖下算法實驗結果對比

在場景1中，相比于傳統(tǒng)RRT算法，改進RRT算法的迭代次數(shù)減少62.6%，求解時間縮短86.9%，路徑長度縮短23.1%；相比目標偏向性RRT算法，改進RRT算法的迭代次數(shù)減少24.1%，求解時間縮短48.2%，路徑長度縮短18.1%。場景2中，相比傳統(tǒng)RRT算法，改進RRT算法的迭代次數(shù)減少39.5%，求解時間縮短85.4%，路徑長度縮短8.4%；相比基于目標偏向性的RRT算法，迭代次數(shù)減少50.6%，求解時間縮短79.9%，路徑長度縮短9.4%。

在不同復雜程度的場景中，分析三種RRT算法的運行結果，可以看出OIS-RRT算法能以更少的迭代次數(shù)，更快的收斂速度求解得到一條更優(yōu)路徑。

6 結論

為了解決傳統(tǒng)RRT算法應用于移動機器人路徑規(guī)劃時擴展隨機性強，算法運行效率低，生成路徑質量非最優(yōu)的問題，本文提出一種基于方向決策RRT算法，通過方向變量，引導節(jié)點快速向目標點擴展；采用啟發(fā)式采樣策略提升算法在復雜環(huán)境下的適應性和避障能力；利用三次B樣條曲線優(yōu)化求解路徑，提高了路徑質量。通過仿真，驗證了本文改進RRT算法能夠有效縮短移動機器人求解路徑規(guī)劃的時間，并提升求解路徑質量。