基于改進RRT算法的6-DOF機器人路徑規劃

李 季 史晨發 邵 磊 劉宏利

(天津理工大學 天津 300384)

0 引 言

工業機器人較高的穩定性和工作效率在自動化生產中逐漸替代人工操作。隨著多自由度機器人應用場景逐漸多樣化，對工業機器人的運動規劃和運動控制提出了更加智能化、高精度等要求。路徑規劃是工業機器人的研究重點，傳統的局部規劃算法并不能適應現階段多自由度機器人MIMO高冗余、高耦合性復雜系統。全局規劃算法是在自由空間內隨機分布采樣點，對采樣點進行篩選判斷后，將有效節點連接最終生成路徑，將其應用在多自由度機器人路徑規劃中，提高了路徑搜索成功率且降低6-DOF機器人資源損耗。

RRT算法是一種全局規劃算法，具有概率完備性，針對RRT算法在機器人路徑規劃中的缺點，國內外學者對其進行改進。文獻[1]利用雙向擴展樹，將啟發函數引入至隨機樣本中，提高了算法收斂速度。文獻[2]通過罰函數法建立以機械臂運動平穩性和機械臂末端最優性為代價適用度函數，使改進RRT算法應用于多自由度機器人在關節空間內路徑規劃。文獻[3]提出了一種基于改進勢場法的動態避障規劃算法，并在離線、在線這兩種狀態中仿真驗證。文獻[4]基于光流的動態障礙物檢測并結合人工勢場理論完成機器人避障的方法，能實現障礙物精準定位及避障，但其對圖像實時處理將耗費較多資源。本文利用RRT全局路徑規劃算法，結合局部路徑優化，從隨機采樣點引入權重系數、設置動態距離增量、路徑縮短、拐角平滑處理等多個方面提高路徑搜尋成功率，加快收斂速度，在最短時間內生成連續平滑的最優路徑[5]。

1 基本算法原理

1.1 械臂運動學模型建立

6-DOF工業機械臂是具有多自由度的開鏈式連桿機構，起始端與固定底座相連接，末端是機械手，中間由若干關節和連桿構成。機械臂的每個關節有自由旋轉度與獨立位置，需要借助數學模型建立機械臂末端在世界坐標系中坐標與機械臂關節角度的聯系[6]。

本文中6-DOF機器人的連桿的數學模型采用Denavit-Hartenberg參數。先在固定底座上建立一個基礎三維坐標系，再對機械臂的每個關節上的中心節點建立相對應坐標系，相鄰關節上的坐標系之間的平移矢量和旋轉矢量通過齊次變換方法求出。6-DOF機器人DH連桿坐標系如圖1所示。

圖1 6-DOF機器人DH連桿坐標系

4個DH參數的含義分別為：θi表示關節角，di表示連桿偏移量，ai表示連桿長度，αi表示連桿扭轉角。這4個參數確定了相鄰連桿間的狀態空間描述，根據六自由度機械臂的運動學正解和逆解，能將機械臂所在自由空間內的世界坐標與機械臂的各關節相對運動量、旋轉度用矩陣函數實現相互轉換[7]。

1.2 機械臂障礙物檢測

機械臂避障檢測要先對障礙物定位，由于障礙物形狀各異，若將每個障礙物進行精準建模，則將耗費大量計算時間，所以碰撞檢測的核心思想就是將障礙物抽象化處理。

根據6-DOF的機械臂固有屬性，將5個連桿簡化成不同半徑的圓柱體，文獻[8]采用的是長方體包絡檢測法，這種方法會造成一些可規劃的空間缺失，從而造成生成節點與最優節點偏差過大。同時選用成像法能精確描述障礙物信息，但其工作量大，實時性較差。所以本文將復雜障礙物經抽象處理后簡化成球體和長方體進行預處理。

首先對球體障礙物提出基于包圍球(Sphere)的碰撞檢測方法，將障礙物碰撞檢測問題轉換成機械臂連桿中心線段至障礙物球心距離判斷。球體障礙物在空間內做旋轉運動時，若球心位置保持不變，此碰撞檢測方法依然適用，這將節省一部分系統計算時間。

對長方體障礙物碰撞檢測采用AABB包圍盒碰撞檢測算法[9]，該碰撞檢測算法的核心是根據障礙物的6個平面與6-DOF工業機械臂連桿空間位置所對應關系進行計算，具體分為以下三類：機械臂連桿線段與障礙物平面相交、平行、異面。計算分析連桿線段和障礙物6個面的線面關系是否在所設定的安全距離區間內來判斷機械臂是否發生碰撞。

2 傳統RRT算法

2.1 原理簡介

快速探索隨機樹(RRT)是一種通過隨機構建空間填充樹來有效搜索非凸高維空間的算法。從搜索空間中的生成隨機樣本來逐步構建樹，并且趨向于大多數未檢測區域生長。障礙物和差異約束的問題可以應用數學模型進行量化處理，廣泛用于機器人運動規劃中[10]，傳統RRT算法擴展圖如圖2所示。

圖2 傳統RRT算法擴展圖

傳統RRT算法流程如下：

(1) 先對自由空間邊界值限定，在拓撲空間X內會根據均勻隨機分布函數生成隨機節點Xrand。

(2) 遍歷已生成的隨機樹上的節點尋找到與該隨機點Xrand距離最近的節點Xnearest，沿著Xnearest至Xrand方向以固定距離增量步長eta生成新的節點Xnew。

(3) 利用碰撞檢測函數篩選有效節點，在Xnew至Xnearest線段上插入若干點，依次檢測每個點與障礙物的距離是否大于預設值。

(4) 循環以上新節點構建過程直至新節點到達目標節點Xgoal，最后依次搜尋并連接父節點構建出Xint到Xgoal路徑。

2.2 優缺點分析

RRT算法是全局規劃算法，具有概率完備性，理論上迭代次數趨于無窮時肯定能搜尋到路徑。在避障過程中只需確定障礙物具體坐標信息和抽象化處理后的位姿形態，隨機生成的節點根據碰撞檢測函數指導隨機樹的擴展，和局部路徑規劃相比，系統不易陷入局部最小解。然而，傳統RRT算法在6-DOF機器人路徑規劃中存在以下缺點：

(1) 隨機性大。新節點是根據均勻隨機分布函數生成的，缺少目標方向的引導，具有較大盲目性，迭代次數相差較大，在本文設定的迭代次數1 000內找尋路徑成功率較低。

(2) 冗余節點多。根據搜尋父節點構建的路徑中會有大量冗余點，傳統RRT算法生成的節點并沒有考慮路徑成本，路徑距離較長。

(3) 路徑抖動大。在一些轉折點處，兩段相鄰的線段轉折角較大，對于機械臂關節會造成損傷同時也會造成抖動現象，影響系統精度。

3 改進RRT算法

3.1 算法流程

本文改進RRT算法，優先保證系統在設定迭代次數內的成功率，在新節點生成之前先判斷與目標點的距離并配合不同距離增量，使路徑能減小振蕩，有效逃離局部最小值。同時在生成初始路徑上再對節點進行二次處理，篩選距離最優節點，舍棄冗余節點，最后再對篩選過的節點利用平滑處理函數在與障礙物保持安全域量的前提下調整路徑平滑程度。改進RRT算法流程圖如圖3所示。

圖3 改進RRT算法流程圖

3.2 引入權重系數

傳統RRT算法隨機采樣點沒有加入目標節點的引導信息，生成擴展樹過程中大量節點都是無效節點，所以在搜尋路徑時隨機性較大。本文對隨機分布節點引入權重系數，使之向目標節點逐漸趨近，使整體擴展樹生長方向由傳統隨機發散式轉變為目標偏置式擴展樹[11]。引入權重系數節點擴展示意圖如圖4所示。

圖4 引入權重系數節點擴展示意圖

在隨機函數的基礎上引入權重系數，沿著Xnearest至Xrand和Xnearest至Xgoal兩個方向分別取P1、P2權重系數，共同作用下生成全新節點。

(1)

當P2大于P1時，全新節點向Xgoal方向偏置；當P1大于P2時，全新節點更趨向于隨機方向。王兆光[15]利用啟發式的思想，在隨機節點的擴展過程中引入兩個參數，一個是目標偏置值bias，一個是隨機系數p。同時為了避免目標偏置值大小設置不合理會造成陷入局部最小解，引入Extend函數，先將隨機生成的節點Xrand與Xgoal連接，判斷是否與障礙物發生碰撞，當存在障礙物時將目標偏置值bias設置為0，這樣能有效地逃離局部最小，但此時新節點隨機性較大，且步長不確定性會造成目標節點周圍的振蕩效果。謝龍[3]將改進人工勢場法應用于RRT中的全新節點，啟發思想和上述方法相似，引力和斥力這兩個參數需在每個新節點生成時不斷配置，這一部分將耗費大量時間。

本文與上述研究不同之處是將每一個隨機節點都引入一個目標偏置的合力方向，且P1和P2在設定數值時需要根據尋找路徑成功率、迭代次數、耗費時間等多方面綜合考慮。當權重系數配置完成后配合動態距離增量，每次全新節點的生成計算量小，可以實現路徑規劃時間最短的指標。

3.3 動態距離增量

在新節點Xnew生成時會先判斷是否到達目標節點Xgoal所設定的領域范圍內。當Xnearest不在Xgoal鄰域時，固定距離增量步長設置為eta1，當Xnearest進入Xgoal鄰域，設定領域半徑為R，若步長還是eta1則會造成Xnew在目標點附近發生振蕩，容易使路徑陷入局部死循環，所以需要將步長設置成eta2，且保證eta2

圖5 動態距離增量示意圖

3.4 路徑縮短處理

在對隨機節點引入權重系數后大大降低了隨機性，生成的初始路徑是由多個離散點組成的連續線段，其中包含較多的冗余點造成與最短路徑相差較大，所以采用路徑縮短處理將這些冗余點剔除[13]。路徑縮短處理示意圖如圖6所示。

圖6 路徑縮短處理示意圖

具體操作步驟如下：

(1) 經過權重函數處理過的隨機節點到達目標點Xgoal時，從Xgoal點向前依次尋找所對應的父節點。

(2) 將其每個坐標數據信息順序編號，從起始點Xint依次向下一個節點連線并再次進行障礙物檢測。

(3) 當所選節點Xm和起始節點連線和障礙物發生碰撞，選取這個節點前一個節點Xm-1作為一個全新擴展樹節點，并將Xint至Xm-1中所有節點剔除。

(4) 從Xm-1開始依次向后對每個節點重連判斷直至到達目標點。

路徑縮短處理示意圖中，通過重新連線再進行碰撞檢測后，只選取1、5、8、11，路徑整體距離有效縮短。

3.5 路徑平滑處理

RRT算法在經過以上路徑縮短操作后的擴展樹上節點之間會包含較多轉折點，相鄰節點拐角過大會造成路徑成鋸齒狀，系統狀態轉換過快會降低機械臂運行的平穩性，還會引起電機的沖擊和振動，加速機械零件的磨損，甚至對機械臂造成損壞。目前國內學者選用七次非均勻B樣條插值法，雖然精度較高，急動度連續且光滑，規劃的速度、加速度值較小[14]，但是計算量較大。本文算法基于時間最短的指標，在兩段路徑銜接點不連續處根據機械臂曲率約束進行路徑平滑處理[14]，通過迭代操作使拐角處節點偏離原始位置，并不需要通過插值法添加新的節點，下面描述具體操作步驟。

假設經過權重系數引導新節點和路徑縮短處理后擴展樹中節點序列為[M1,M2,…,Mn]，對其路徑平滑處理后的節點序列為[N1,N2,…,Nn]，路徑平滑處理本質上是對擴展樹上節點處理后滿足以下這兩種距離趨向最小解：

(1) 保證平滑處理后的Ni與原節點Mi偏離程度最小，即求解N(i)使‖M(i)-N(i)‖的平方值最??；(2) 保證平滑處理后的節點N(i)與N(i+1)之間距離取值最小，即N(i)滿足‖N(i)-N(i+1)‖最小。

在曲線平滑函數中，采用梯度下降法，即通過設定迭代次數求解出最優解，不斷調整N(i)，使目標函數取值最小化。先將擴展樹節點初始化操作N(i)=M(i)，設定迭代次數為10次。

N(i)=N(i)+α×[M(i)-N(i)]+

β×[N(i-1)-2×N(i)+N(i+1)]

(2)

通過調整公式中α和β值大小控制路徑的平滑程度，最后進行迭代操作。通過以上平滑處理后，滿足約束條件，序列由[M1,M2,…,Mn]變為[N1,N2,…,Nn]，路徑平滑處理示意圖如圖7所示。

圖7 路徑平滑處理示意圖

4 仿真實驗

在MATLAB仿真中，對機械臂在二維空間中進行路徑軌跡，障礙物分別選取圓、長方形模擬障礙物在平面上的投影，起始點和目標點參數設置為(5,5)和(95,95)，擴展樹有效節點用黑點表示。權重系數P1、P2都設置為0時，全新隨機節點并沒有引入啟發信息，傳統RRT算法仿真圖如圖8所示。

圖8 傳統RRT算法仿真圖

可以看出，未經過優化處理的路徑多次規劃出來的路徑完全不一樣，迭代次數相差較大，冗余節點較多耗費較多時間計算且拐點多，路徑不平滑、方向變化大。

在改進RRT算法中，先對隨機生成節點引入權重系數，P1、P2這兩系數的確定對路徑規劃的效率有較大影響，在仿真實驗中將迭代次數設置為1 000次，規定次數內未到達目標點鄰域則算是路徑規劃失敗。分別選取不同權重系數P1、P2和變化步長eta1，每組數據樣本經過100次仿真實驗進行統計，測試結果如表1所示。

表1 測試結果

續表1

可以看出，P2大于P1合力方向整體偏向于目標節點，當P2大于P130%時，所設定迭代次數內成功率為100%，平均時間為0.9 s左右。當P2大于P150%時，所設定迭代次數內成功率為98%，平均時間為0.8 s左右。當P2大于P160%時，所設定迭代次數內成功率為95%，平均時間為1.2 s左右。

在權衡成功率和耗費時間后，Xnearest至Xgoal方向的權重系數P2取3，Xnearest至Xrand方向的權重系數P1取2，固定距離增量eta1=1，eta2=0.3，整體路徑規劃成功率和快速性達到最優狀態，將參數配置后的仿真圖如圖9所示。

圖9 引入權重系數仿真圖

在路徑縮短處理中，將引入權重系數后的路徑中所有節點進行重連、判斷、篩選后，去除冗余節點，能將路徑距離有效縮短，同時也讓機械臂關節轉動次數降低，保證末端機械臂的平穩性。路徑縮短過程是在已經生成的路徑上進行二次優化[15]，所以這一部分將耗費一些計算時間，但綜合考慮路徑整體距離有效縮短后，這些計算資源的損耗對整個系統是有意義的。

在最短路徑的基礎上，兩段路徑銜接點不連續處根據機械臂曲率約束進行路徑平滑處理。在轉折點處將原有節點利用圓滑處理函數進行重新標定，前提條件是調整α和β值的大小，保證拐角處平滑處理后的曲線和障礙物之間存在著安全域量。經路徑縮短和平滑處理后的最終優化路徑仿真如圖10所示。

圖10 最終優化路徑仿真圖

在標定參數后，對本文算法在二維空間狀態內的路徑規劃進行100次仿真實驗，與傳統RRT算法相比，在規定迭代次數1 000次以內，本文算法成功率為100%。將本文算法與RRT-star算法比較，RRT-star的核心也是在優化最優節點上，在此引入了貪婪思想，采用代價函數來選取拓展節點領域內最小代價的節點為父節點，同時，每次迭代后都會重新連接現有樹上的節點，從而保證計算的復雜度和漸進最優解。RRT-star最終路徑長度和本文算法相近，但是迭代次數較多，耗費時間過長。仿真結果表明，本文算法極大降低了傳統RRT算法的隨機性，整體路徑里程縮短，且經處理后的路徑斜率連續，拐角處平滑過渡使機械臂能更穩定地運行。算法比較結果如表2所示。

表2 算法比較結果

5 結 語

本文針對傳統RRT算法的缺點提出了一些改進方法，首先建立6-DOF機械臂運動型模型，對機械臂連桿、障礙物抽象化處理，有效減少計算量。在RRT隨機節點生成時引入權重系數，并配合不同距離增量減小趨近于目標節點的振蕩，同時剔除初始路徑中的冗余節點后將路徑距離有效縮短，拐角平滑處理后減少了機械臂抖動。仿真表明，改進RRT算法降低了隨機性，平均規劃時間縮短75%，路徑距離縮短25%，該算法在6-DOF機械臂全局路徑規劃中具有較好的應用前景。