李 季 史晨發(fā) 邵 磊 劉宏利

(天津理工大學 天津 300384)

0 引 言

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

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

1 基本算法原理

1.1 械臂運動學模型建立

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

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

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

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

1.2 機械臂障礙物檢測

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

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

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

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

2 傳統(tǒng)RRT算法

2.1 原理簡介

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

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

傳統(tǒng)RRT算法流程如下：

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

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

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

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

2.2 優(yōu)缺點分析

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

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

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

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

3 改進RRT算法

3.1 算法流程

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

圖3 改進RRT算法流程圖

3.2 引入權重系數(shù)

傳統(tǒng)RRT算法隨機采樣點沒有加入目標節(jié)點的引導信息，生成擴展樹過程中大量節(jié)點都是無效節(jié)點，所以在搜尋路徑時隨機性較大。本文對隨機分布節(jié)點引入權重系數(shù)，使之向目標節(jié)點逐漸趨近，使整體擴展樹生長方向由傳統(tǒng)隨機發(fā)散式轉變?yōu)槟繕似檬綌U展樹[11]。引入權重系數(shù)節(jié)點擴展示意圖如圖4所示。

圖4 引入權重系數(shù)節(jié)點擴展示意圖

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

(1)

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

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

3.3 動態(tài)距離增量

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

圖5 動態(tài)距離增量示意圖

3.4 路徑縮短處理

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

圖6 路徑縮短處理示意圖

具體操作步驟如下：

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

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

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

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

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

3.5 路徑平滑處理

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

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

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

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

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

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

(2)

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

圖7 路徑平滑處理示意圖

4 仿真實驗

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

圖8 傳統(tǒng)RRT算法仿真圖

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

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

表1 測試結果

續(xù)表1

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

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

圖9 引入權重系數(shù)仿真圖

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

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

圖10 最終優(yōu)化路徑仿真圖

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

表2 算法比較結果

5 結 語

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