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

趙文龍 Abdou Yahouza M.Sani

（上海工程技術(shù)大學機械與汽車工程學院 上海 201620）

1 引言

全局路徑規(guī)劃算法主要有基于圖搜索的Dijkstra、A*、D*算法以及基于采樣的PRM、RRT、RRT*等算法［5］。相比于常用的A*等算法，基于采樣的路徑規(guī)劃算法在高維及大尺度的環(huán)境下具有顯著的優(yōu)勢，此外其還考慮了機器人的非完整性約束，因此對該類算法的研究非常有必要。LaValle 等提出的快速隨機擴展樹法（RRT）［6～7］，是一種概率完備的算法，但規(guī)劃的路徑不是最優(yōu)的。為了解決RRT算法所規(guī)劃的路徑不最優(yōu)的問題，文獻［8］中，Karaman Sertac等提出了RRT*算法，其改進了RRT算法的父節(jié)點選擇機制，并增加了重布線機制。這使得RRT*算法能漸進收斂到最優(yōu)路徑，但改進的方法為了優(yōu)化路徑而增加了耗時。文獻［9］提出的informed RRT*算法引入了一個啟發(fā)式的橢圓采樣區(qū)域，通過在橢圓區(qū)域里不斷采樣使路徑逐漸趨于優(yōu)化，由于限制了采樣區(qū)域從而減少了無效采樣，提高了算法的效率。為了平滑路徑，文獻［10］中，Webb D J 等提出的Kinodynamic RRT*算法將機器人的動力學約束考慮在內(nèi)，通過改變轉(zhuǎn)向函數(shù)以適應機器人導航中的運動或其他約束。文獻［11］引入了一種Dubins 路徑用來做平滑優(yōu)化，但是由于是直線和圓弧拼接產(chǎn)生的路徑，因此路徑曲率不連續(xù)。文獻［12］中杜明博等結(jié)合環(huán)境約束和車輛約束，引入了合理的度量函數(shù)并提出一種最大曲率約束的后處理方法使規(guī)劃的路徑平滑可行。為了簡化隨機樹的結(jié)構(gòu)，文獻［13］中，譚建豪等提出一種改進的RRT*FN 算法，其采用啟發(fā)式采樣的方法，并對樹中的葉子結(jié)點采用加權(quán)刪除法以減少內(nèi)存消耗，同時提升算法性能。為了選擇合適的度量函數(shù)，文獻［14］中，F(xiàn)aust提出利用偏好評估的強化學習，提高機器人學習效率，在復雜的環(huán)境下使得移動機器人快速規(guī)劃出可行路徑。文獻［15］中，徐娜等將移動機器人的非完整約束條件與RRT 搜索算法相結(jié)合，以解決受動力學約束的移動機器人運動規(guī)劃問題。文獻［16］中，劉成菊等提出以一定概率選擇目標點、增加引力分量以及路徑平滑處理等方式改進的RRT 算法，將其應用于nao 機器人在動態(tài)移動障礙環(huán)境下的路徑規(guī)劃。

相較于以上改進方法，本文針對RRT 算法隨機采樣存在的大量無效采樣以及路徑不最優(yōu)等問題進行改進，以二維環(huán)境中的移動機器人為載體進行算法驗證。通過引入偏向目標的隨機點采樣方式改進算法采樣的無目標性，使算法能夠加速收斂。采用剪枝操作和多項式插值的方法對路徑進行優(yōu)化處理，使路徑平滑，更利于機器人執(zhí)行。

2 基本算法描述

快速隨機擴展樹法是一種基于采樣的全局路徑規(guī)劃算法，其節(jié)點擴展示意圖如圖1（a）所示。首先初始化隨機樹，將起始點和目標點插入運動空間中，把起始點xinit作為隨機樹的根節(jié)點，然后在運動空間中隨機選擇一個點xrand，再在隨機樹中搜索出距離xrand最近的節(jié)點xnear，將兩點連接起來，如果兩者間的距離小于或等于設定的步長則將xrand直接設為新節(jié)點xnew，否則在連線上取距離點xnear為步長的點作為xnew。之后再判斷新節(jié)點以及連線是否與障礙物發(fā)生碰撞，若碰撞則刪去該新節(jié)點再重新選擇隨機點繼續(xù)如上步驟，若沒有發(fā)生碰撞則將xnew節(jié)點和邊Ej插入隨機樹T 中。如此不斷擴展隨機樹，直至節(jié)點包含目標節(jié)點或進入目標區(qū)域。最后反向追溯到根節(jié)點便可以在隨機樹中找到一條由從初始點到目標點的路徑。

從圖1（b）可以看出，RRT 算法規(guī)劃的路徑存在較多折角，并不能保證機器人的速度連續(xù)性，不利于機器人執(zhí)行，而且此路徑也不是到達目標點的最近路徑。其原因在于，RRT算法是在地圖范圍內(nèi)隨機撒點，然后將無碰撞的一系列點連接起來構(gòu)成路徑的。其過大的隨機性不僅造成路徑不最優(yōu)，而且因為探索了大量無效的采樣點而導致其規(guī)劃效率降低。

圖1 RRT算法示意圖

3 改進的RRT算法

基本的RRT 算法隨機性過大，在復雜的環(huán)境下其收斂速度顯著下降，且規(guī)劃的路徑大多蜿蜒曲折，并不是一條最優(yōu)路徑。因此，本文首先對其采樣方式進行改進，并對其規(guī)劃的路徑進行剪枝處理以及多項式曲率擬合處理。

3.1 偏向目標的采樣

相對于RRT 算法的均勻隨機采樣，本文引入了一種偏向目標的采樣方法，具體如圖2 所示：首先繼承了RRT 算法的采樣方式，在地圖中隨機獲得一個采樣點xtrand，然后將此隨機點與目標點連接起來，再在兩者之間的連線上隨機采樣一個點作為最終的隨機點xrand。在完成隨機點采樣后，本文算法的碰撞檢測策略，隨機樹擴展方式以及路徑的回溯創(chuàng)建均采用與RRT 算法相同的方法。本文改進的采樣方式不僅能夠保證一定的隨機性還能使隨機樹向著目標點擴展，減少大量無效的采樣，加快算法的收斂速度。

圖2 偏向目標采樣示意圖

3.2 路徑剪枝處理

由RRT 算法規(guī)劃出的路徑存在許多角度突然變化的折線，有些節(jié)點之間并不存在障礙物，而軌跡也呈彎折狀，如圖3 所示，其導致中間存在許多沒有必要的節(jié)點，如圖中node1和node2?？梢詫⑵溟g冗余的中間節(jié)點從路徑中剔除以縮短路徑的長度，同時也可以在一定程度上平滑路徑。

綜上所述，本文基于多元交通方式運用改進的兩步移動搜索法相比于鄰近距離法對養(yǎng)老服務設施可達性進行了修正研究，同時對老年人選擇不同出行方式進行對比分析，并從公平性視角對武漢市養(yǎng)老服務設施布局進行評價.主要研究以下問題：1) 采用兩步移動搜索法與最小鄰近距離法測度可達性的相關程度如何.2) 受不同交通方式的影響，公共服務設施可達性有何差別.3) 武漢市養(yǎng)老服務設施可達性空間格局是怎樣的，哪些公平性較差區(qū)域需要合理布局養(yǎng)老服務設施資源.

圖3 剪枝處理示意圖

路徑的起始點和目標點附近都可能存在冗余節(jié)點，因此本文的剪枝處理從路徑的第二個節(jié)點開始。首先以路徑的起始點為根節(jié)點，從第二個節(jié)點開始依次連接起始點并檢測其連線是否與障礙物發(fā)生碰撞。若沒有碰撞則一直檢測后續(xù)節(jié)點，直至檢測到某個節(jié)點與起始節(jié)點的連線與障礙物發(fā)生碰撞。此時將上一個節(jié)點xpre與當前節(jié)點xcur依次插入一個新的路徑儲存變量path2，然后將起始點與節(jié)點xpre的連線作為新路徑。同時把當前節(jié)點置為新的根節(jié)點，再繼續(xù)對剩余節(jié)點做碰撞檢測，以此循環(huán)，直到目標節(jié)點加入檢測后跳出循環(huán)。此時path2中儲存的即是剪枝處理后的新路徑節(jié)點。

3.3 多項式路徑平滑

對于RRT 算法規(guī)劃的路徑，其存在不光滑的轉(zhuǎn)角，由于動力學約束，機器人跟隨此路徑走到轉(zhuǎn)角處必須先停下來，再一次性轉(zhuǎn)至對應角度才能繼續(xù)行走。然而這并不是想要的結(jié)果，因此本文針對這一問題，在3.2 小節(jié)剪枝處理的基礎上采用多項式對路徑的轉(zhuǎn)角處進行平滑處理。本文采用五次多項式進行路徑平滑處理，具體如下：

1）五次多項式插值：

2）將上式分別求一階，二階導得

3）邊界條件：

多項式邊界條件如表1所示。

表1 多項式邊界條件

代入邊界條件并寫成矩陣形式得：

此步基于剪枝處理得到的路徑進行處理，首先遍歷路徑中的所有節(jié)點與邊，找到包含轉(zhuǎn)角處的節(jié)點以及緊鄰此節(jié)點的兩節(jié)點作為五次多項式的邊界條件。然后對此轉(zhuǎn)角進行曲率插值擬合，從而得到一條光滑的曲線用以代替轉(zhuǎn)角。

3.4 算法總體實現(xiàn)

基于上述改進，本文算法的整體實現(xiàn)如下表偽代碼所示，主要包括初始化隨機樹，在無碰撞的空間中擴展隨機樹，反向搜索隨機樹，找到一條路徑，然后再對路徑進行剪枝處理以及多項式平滑處理。

4 實驗與分析

為了驗證算法有效性，本文將以上三種改進措施依次在Matlab平臺上實現(xiàn)，并將實驗結(jié)果與未改進前的算法進行對比，從而分析改進的效果。此外還將改進的整體算法與RRT、RRT*、informed RRT*算法進行對比分析。本實驗運行環(huán)境：64bit Windows10 操作系統(tǒng)；Matlab 2016a；處理器AMD RYZEN5 2500U；主頻2.0GHz；內(nèi)存8GB。

4.1 改進采樣方式的實驗

此部分驗證改進采樣方式的效果并與RRT 算法作對比，本實驗中算法的迭代次數(shù)均設為3000，隨機樹的擴展步長設置為0.35，終止的條件為找到目標節(jié)點或達到最大迭代次數(shù)。實驗結(jié)果如圖4所示：其中深色和淺色的點分別表示起始點和目標點，細實線表示隨機樹，粗實線表示規(guī)劃的路徑。

圖4 改進采樣方式的算法

表2 改進算法偽代碼

從圖中可以看出，未改進的算法生成的隨機樹均勻的遍布整個地圖，而改進算法的隨機樹朝著目標點生長，在目標點區(qū)域附近其密度明顯高于地圖上其他區(qū)域。對兩種算法花費的時間以及規(guī)劃的路徑長度進行了統(tǒng)計。由于快速隨機擴展樹算法的隨機性，每次規(guī)劃的路徑和所花時間基本都不同。因此，實驗中兩個算法都運行了五次然后取平均值，其結(jié)果如表3所示。

表3 改進的RRT算法與未改進的算法對比

由以上圖表可以看出，改進后的算法在搜索路徑時具有目標偏向性，避免了大量無效的采樣，因此其規(guī)劃效率顯著提高。對于所規(guī)劃路徑的長度，改進算法也縮短了不少。在平滑度優(yōu)化方面兩者差異并不大但也有所優(yōu)化。

4.2 路徑剪枝處理實驗

此部分實驗單獨考察路徑剪枝處理給算法帶來的變化。環(huán)境地圖采用與上小節(jié)相同的地圖，大小為800*800，最大迭代次數(shù)、擴展步長、算法終止條件均與4.1小節(jié)相同。其實驗結(jié)果如圖5所示。

圖5 剪枝處理實驗圖

實驗中將改進前后的路徑畫在同一張圖里進行比較，其中藍色粗實線是未優(yōu)化的路徑，紅色粗實線是經(jīng)過剪枝處理后的路徑。從圖中可以明顯看出，藍色的路徑存在較多彎折的冗余線段，改進后的算法將不必要的彎折部分濾掉而直接用直線連接起來，如此可以有效的縮短路徑的長度，并且使路徑的平滑度得到一定的優(yōu)化。

4.3 多項式路徑平滑實驗

本節(jié)實驗參數(shù)與4.1 和4.2 小節(jié)的實驗設定相同，地圖與前兩節(jié)保持一致。多項式插值是在經(jīng)過剪枝處理后的路徑上進行的，此處保留未優(yōu)化與部分優(yōu)化的路徑，將三條路徑進行對比。障礙物假設已經(jīng)做過膨化處理，而機器人視為一個質(zhì)點。實驗結(jié)果如圖6所示。

圖6 多項式路徑平滑實驗圖

圖中綠色粗實線即為經(jīng)過多項式平滑處理后的路徑，紅色是經(jīng)過4.2小節(jié)剪枝處理后的路徑，藍色為未改進算法的路徑。從圖中可以看出，改進后的算法在存在折角的路徑處進行了曲率擬合，與原算法的路徑相比，本文改進算法對路徑進行了有效的光滑，路徑長度也同時得到優(yōu)化。

4.4 總體改進算法的實驗分析

以上三小節(jié)采用控制變量的方法分別驗證了三個改進措施的效果，本節(jié)將三種措施綜合起來，驗證改進算法的整體效果。首先依舊采用800*800 的地圖，運行結(jié)果如圖7 所示。其中綠色粗實線為優(yōu)化后的路徑。從圖中可以看出該路徑雖不是最短路徑，但其比較光滑。此外隨機樹朝著目標生長，非目標區(qū)域的無效采樣樹非常稀疏，搜索效率明顯提高。

圖7 改進算法整體效果圖

其次，本文將RRT 算法、RRT Connect 算法、RRT*算法與改進算法進行對比。實驗中采用了一張100*100 的地圖，地圖在目標周圍設計了一處方形障礙物模擬凹形通道。迭代次數(shù)與步長等參數(shù)四種算法均保持一致，結(jié)果如圖8所示。

圖8 四種算法對比圖

從圖8 和表4 可以看出，路徑光滑度本文改進算法最好，RRT*算法也不錯。從平均路徑長度來看，本文改進算法最短但與RRT*算法相差不大，且都不是最短路徑。RRT Connect 算法和RRT 算法由于都是隨機性的，所以兩者路徑長度也相差不大。對于規(guī)劃效率來講，由于RRT Connect 算法是從起點和目標點同時搜索的，所以其耗時遠遠低于其他算法。綜上，本文改進的算法不但優(yōu)化了路徑而且還在一定程度上提高了算法的規(guī)劃效率。

表4 四種算法對比表

4.5 ROS平臺仿真實驗

本節(jié)將改進的算法在ROS上實現(xiàn)，為移植到真實機器人上做準備。導航的整體框架采用ROS 中的navigation stack，將其中的gloable_planner替換成本文的改進算法。此處采用stage 仿真工具搭建仿真環(huán)境，并且將RRT 算法和本文算法進行對比，具體效果如圖9 所示，從圖中可以看出本文算法明顯優(yōu)于RRT算法。

圖9 ROS仿真效果圖

5 結(jié)語

路徑規(guī)劃問題是移動機器人研究的關鍵問題，本文提出一種改進的RRT 算法，引入偏向目標的采樣策略，既保證采樣點的隨機性又提升其目標偏向性；對冗余路徑節(jié)點進行剪枝處理，濾除無效的彎折路徑；采用多項式插值的方法，對路徑進行曲率平滑處理。最后通過仿真實驗驗證了本文改進算法的效率不僅顯著提高，而且所得路徑的光滑度也大大提升。

本文研究主要針對于優(yōu)化RRT 算法規(guī)劃的路徑以及提高算法的規(guī)劃效率，實際機器人中有很多需要考慮的問題，后續(xù)研究可以擴展到動態(tài)環(huán)境下的路徑規(guī)劃，將更多約束考慮在內(nèi)，以提升機器人在更復雜的環(huán)境下的路徑規(guī)劃能力。