基于改進RRT 算法的六自由度機械臂路徑規(guī)劃

朱永浩 ，李 丹 ，龔 旭 ，金仁才

(安徽工業(yè)大學 電氣與信息工程學院, 安徽 馬鞍山 243032)

隨著機器人技術的發(fā)展，機械臂具有工作空間大、操作簡便、靈活性和自由度高等優(yōu)點[1]，在焊縫焊接、制造加工、拆卸裝配、噴漆和碼垛等領域得到越來越廣泛的應用[2-4]。與此同時，各項工作對機械臂的工作狀態(tài)有更高的要求，路徑規(guī)劃作為機械臂工作的關鍵技術，受到越來越多的關注和研究。機械臂的路徑規(guī)劃和智能小車類似，均是以機器人當前位置為起點，規(guī)劃出1 條運動到終點、無碰撞的最優(yōu)路徑[5]。但是，機械臂的運動規(guī)劃主要用于多維空間，規(guī)劃難度更大。

近年，機械臂路徑規(guī)劃算法主要分為基于圖搜索、基于勢場、基于采樣等類別[6]。其中基于采樣的算法無需預處理，也無需提前建圖，在多維空間中得到更廣泛的應用?？焖贁U展隨機樹(rapidly-exploring random tress，RRT)算法是機械臂路徑規(guī)劃中最常用的算法[7]，但該算法在復雜環(huán)境下會出現(xiàn)效率降低且無法得到最優(yōu)路徑的問題?；赗RT 的改進算法，Kuffner 等[8]提出讓起點和終點同時生成隨機樹來進行雙向拓展的RRT-connect 算法；Ferguson等[9]提出動態(tài)快速搜索樹(dynamic-RRT，DRRT)算法，通過給定目標拓展方向，可較大程度地降低規(guī)劃時陷入局部最優(yōu)的情況；Karaman[10]提出改進的RRT*算法，通過添加圖優(yōu)化和剪枝理論，保留傳統(tǒng)RRT 算法的完備性，同時兼具漸進最優(yōu)性，但大大增加了搜索時間；Islam 等[11]提出RRT*-smart 算法，利用智能采樣使搜索路徑快速收斂；Klemm 等[12]結合RRT*的漸進最優(yōu)和RRT-connect 的雙向搜索，提出使搜索路徑朝理論最優(yōu)解收斂的RRT*-connect算法；朱宏輝等[13]通過加入規(guī)避步長延伸法改進RRT*算法，但改進算法用于復雜路徑的規(guī)劃時，難以獲得最優(yōu)解；陳肇星等[14]提出的DSSP-RRT 算法通過動態(tài)采樣減少迭代次數(shù)，但存在可能陷入局部最優(yōu)的情況。綜上可看出：改進的RRT 算法均在一定程度上優(yōu)化了路徑規(guī)劃性能，但依然存在一些不足，且在高維空間下的機械臂路徑規(guī)劃通常是不穩(wěn)定的，需在不同層面上做出改進。鑒于此，結合RRT*和RRT-connect 算法的優(yōu)點，提出1 種改進的RRT算法對六自由度機械臂進行路徑規(guī)劃，并分別利用Python 中的matplotlib 功能包以及ROS 平臺中的動態(tài)規(guī)劃庫(open motion planning library，OMPL)，對睿爾曼RM-65 機械臂進行避障路徑規(guī)劃仿真實驗，結果顯示該算法規(guī)劃出的路徑更優(yōu)且用時更少。

1 機械臂運動學分析

1.1 運動學模型

以六自由度機械臂為對象，對睿爾曼RM-65機械臂進行正運動學分析。機械臂可視為由關節(jié)和連桿組成，求解機械臂末端相對于基準坐標系的位姿，即為機械臂的正運動學建模[15]。機械臂結構尺寸如圖1，其中d1=240.5 mm，d2=102.0 mm，d3=256.0 mm，d4=210.0 mm，d5=144.0 mm，d6=850.5 mm，d7=81.5 mm。

圖1 RM-65 機械臂結構尺寸Fig.1 Structural dimensions of RM-65 robotic arm

基于改進的MD-H 參數(shù)建模，建立的連桿坐標系如圖2。

圖2 連桿坐標系示意圖Fig.2 Schematic diagram of the connecting rod coordinate system

圖3 Matlab 仿真結果示意圖Fig.3 Schematic diagram of Matlab simulation results

x軸、y軸、z軸如圖2 中標注所示，機械臂最底部的坐標系base_link 為機械臂的基坐標系，Link1～Link6 分別對應關節(jié)1～6 的坐標系。根據(jù)機械臂的尺寸得到MD-H 參數(shù)，如表1。

表1 RM-65 機械臂MD-H 參數(shù)Tab.1 RM-65 robotic arm MD-H parameter

根據(jù)MD-H 參數(shù)與各關節(jié)角度，確定機械臂末端執(zhí)行器基于機械臂基座的空間位姿，矩陣ii-1T為連桿i坐標系到連桿i-1 坐標系的MD-H變換矩陣，該矩陣經(jīng)以下四次運動變換得到，具體變換公式如下：

由式(2)～(7)可得出機械臂末端坐標系相對于底端基坐標系的運動學位姿變換矩陣，即正運動學方程06T可表示為：

圖中：RPY為矩陣60T中的旋轉矩陣轉化得到的歐拉角，q1～q6為對應6 個關節(jié)角度；取3 組關節(jié)角度，分別通過Robotic Toolbox 工具箱讀取和建模得到正運動學方程計算，結果如表2。由表2 可看出：Robotic Toolbox 工具箱讀取所得位姿與運動學方程計算所得位姿結果基本一致，表明由MD-H 參數(shù)法建模計算得到的正運動學方程正確。

表2 位姿對比結果Tab.2 Pose comparison results

逆運動學建模是通過已知的機械臂末端位姿來確定滿足期望位姿的機械臂各關節(jié)的狀態(tài)[16]，常用的解法有數(shù)值求解法和解析求解法。其中數(shù)值求解法采用非線性優(yōu)化求解，適用范圍較廣但計算量較大、速度較慢；解析求解法速度快但往往會得出多組奇異解。因此，文中選擇數(shù)值求解法計算得到若干組逆解，根據(jù)關節(jié)角度范圍和碰撞關系的檢測在逆解中找出最優(yōu)解。

1.2 碰撞檢測

機械臂各連桿的形狀不規(guī)則，在進行碰撞檢測時常使用包圍盒法，利用簡單幾何體將機械臂包絡起來。常用的包圍盒法有軸對齊邊界框包圍盒(axis-aligned bounding box，AABB)、包圍球、方向包圍盒(oriented bounding box，OBB)等[17-18]。AABB 法采用平行于坐標軸的最小六面體包絡對象，包圍球法采用半徑最小的球體包絡對象，OBB 法采用最小長方體包絡對象。李智[19]提出了子彈形的包圍盒(bullet bounding box，BBB)，包圍盒模型為圓柱體搭配半球體的組合。使用該方法對機械臂進行包絡時，對于旋轉不會影響相鄰連桿模型的軸體，采用圓柱體來包絡；對于旋轉會影響相鄰連桿的軸體，采用半球來包絡，這樣可使包絡盒更緊湊，且不會大量增加后續(xù)計算量。

文中采用BBB 方法和包圍球方法對機械臂包圍盒進行包絡，將機械臂與空間障礙物的碰撞檢測簡化為求解圓柱中心線與球體半徑的關系問題以及圓柱中心線與AABB 包圍盒各平面的距離問題[20]。根據(jù)機械臂連桿旋轉是否會影響相鄰連桿，對第一軸、第四軸和第六軸采用圓柱包絡，對第二軸、第三軸和第五軸采用半球和球形包絡。具體為：將第一軸、第一連桿和第二軸轉換為子彈形包絡，第二連桿和第三軸轉換為球形包絡，第三連桿、第四軸、第四連桿和第五軸轉換形成子彈包絡，第五連桿、第六軸和第六連桿視作1 個圓柱體，末端執(zhí)行器視為1 個球體。使用包圍盒法可對機械臂包絡進行簡化，根據(jù)簡化的機械臂模型，對各包絡體間距離進行求解，各包絡體之間的最短距離可作為判斷是否發(fā)生碰撞的指標，距離大于0 則未發(fā)生碰撞，否則判斷為發(fā)生碰撞。

2 RRT 算法的改進

RRT 算法是1 種基于隨機采樣的快速路徑規(guī)劃算法，通過采樣在工作空間全局搜索，以給定的起點為根節(jié)點，利用隨機取點生成新的子節(jié)點使擴展樹不斷生長，具有出色的空間搜索能力，對高維空間的路徑規(guī)劃也具有出色的性能[21]。RRT 算法的主要步驟如圖4，包括采樣、尋找鄰近節(jié)點、拓展根節(jié)點、生成候選節(jié)點、碰撞檢測以及生成鄰近節(jié)點。具體過程為：確定起點(S)與終點(F)，并明確起點與終點之間的障礙物信息；以起點為根節(jié)點，隨機擴展采樣，遍歷樹中所有節(jié)點，生成候選節(jié)點Tn，并以固定步長生成1 個鄰近節(jié)點Tn+1，若Tn與Tn+1之間不存在障礙物，則Tn及Tn+1加入擴展樹，否則放棄此次采樣點并重新采樣；循環(huán)上一步驟，直到采樣得到1 個節(jié)點Tn+1為終點F或該節(jié)點距離終點F的距離小于設定的閾值，則規(guī)劃路徑成功并退出采樣。

圖4 RRT 算法示意圖Fig.4 Schematic diagram of RRT algorithm

RRT 算法在多維空間中適用性很強，但算法本身在搜索路徑的過程中具有隨機性，導致每次規(guī)劃出的路徑各不相同，規(guī)劃出的是可行路徑，并不是最優(yōu)路徑，且在復雜狹小環(huán)境中難以搜索到路徑。為此，文中結合RRT*算法和RRT-connect算法的優(yōu)點，在搜索過程中，應用目標采樣思想對路徑進行調整與重新布線，使每次采樣趨近給定的擴展方向，改進RRT 算法。

2.1 目標采樣

RRT 算法在隨機采樣的過程中，搜索樹時常會擴展到離目標很遠的地方，即“無用區(qū)域”。為提高算法的效率，應用目標采樣的思想加強算法向目標點搜索的趨向性，減少無用搜索，即人為引導采樣過程中隨機點的生成，在生成1 個隨機節(jié)點Tn時，以一定概率將節(jié)點生成在終點處，即Tn=TF。在提高算法效率的前提下，為保持搜索樹對于未知空間的搜索能力，一般將概率p設定在0.05～0.10 之間。具體步驟為：生成1 個(0,1)的隨機數(shù)rand，若rand≤p，則節(jié)點生成在終點處，即Tn=TF；若rand＞p，則新節(jié)點保持隨機生成，圖5 為加入目標采樣前后的路徑規(guī)劃效果。

圖5 加入目標采樣前后的路徑規(guī)劃效果Fig.5 Path planning effect before and after adding target sampling

從圖5 可看出：加入目標采樣后的算法既能保證搜索樹擴展時的隨機性，又能一定程度上減免對“無用區(qū)域”搜索耗費的時間，在障礙物較少的空間效果較好。

2.2 貪婪思想

當隨機采樣到目標點附近區(qū)域時，受固定步長的限制，即使采樣點與目標點之間無障礙物，搜索樹也無法直接到達目標點，轉而會繼續(xù)進行無效搜索，使路徑復雜、規(guī)劃時長增加。針對這些采樣點，引用貪婪思想，當搜索樹生成的新節(jié)點與目標點間的距離在設定的閾值內時，判斷新節(jié)點Tn1～Tn5與目標點F直接相連的可行性，如圖6。當新節(jié)點與目標點之間存在障礙物時(如Tn1，Tn3，Tn4，Tn5)，該節(jié)點繼續(xù)隨機采樣拓展；若之間不存在障礙物時(如Tn2)，則該節(jié)點停止隨機采樣，且不受固定步長的限制，將該節(jié)點Tn2與目標點F直接相連，并將兩點間的路徑放入最終規(guī)劃出的路徑中。

圖6 引入貪婪思想的示意圖Fig.6 Schematic diagram of introducing greedy thinking

2.3 路徑平滑處理

在機械臂避障路徑規(guī)劃過程中，易在路徑拐點出現(xiàn)急加速的情況，使規(guī)劃過程的平穩(wěn)性降低，故需對路徑進行平滑處理。對于改進的RRT 算法，選取路徑中的若干拐點，使用B 樣條函數(shù)對拐點間的路徑進行平滑處理。考慮到B 樣條次數(shù)越高，平滑處理后的路徑越平滑，為保證最終規(guī)劃出的路徑效果，文中選擇五次B 樣條函數(shù)進行平滑優(yōu)化。根據(jù)需要平滑優(yōu)化的拐點坐標，構造出1 條參數(shù)化的B 樣條曲線，使用取值范圍為[0，1]的參數(shù)t表示曲線上的點；將參數(shù)t映射到實際的自變量范圍，根據(jù)拐點個數(shù)將自變量范圍劃分成若干區(qū)間，并對每個區(qū)間使用五次B 樣條插值；根據(jù)插值結果可得到各區(qū)間的插值函數(shù)。位置表達式為

其中，將式(13)代入式(10)～(12)，解得：

2.4 改進RRT 算法的實現(xiàn)

改進的RRT 算法流程圖如圖7。

圖7 改進的RRT 算法流程圖Fig.7 Flowchart of improved RRT algorithm

由圖7 可看出：采用改進的RRT 算法進行路徑規(guī)劃，其主要步驟如下：

1) 確定起點與終點，將起點和終點分別作為根節(jié)點向空間中擴展生成搜索樹，2 棵樹分別為TS，TF。

2) 加入目標采樣，使搜索樹TS按照給定方向擴展，采樣得到1 個新節(jié)點Tr1，檢查搜索樹TS與Tr1最近節(jié)點Tn1之間是否存在障礙物。若不存在障礙物，則將節(jié)點Tn1儲存進搜索樹；若存在障礙物則重復步驟2)，直到找到1 個無碰撞的節(jié)點Tn1。

3) 以Tn1附近規(guī)定的半徑范圍內尋找路徑代價更小的鄰近節(jié)點作為父節(jié)點，并重新布線，調整經(jīng)過新節(jié)點Tn1的路徑。

4) 與步驟2)類似，使搜索樹TF按照給定方向擴展，通過碰撞檢測，采樣得到1 個無碰撞的節(jié)點Tn2。

5) 以Tn2附近規(guī)定的半徑范圍內尋找路徑代價更小的鄰近節(jié)點作為父節(jié)點，并重新布線，調整經(jīng)過節(jié)點Tn2的路徑。

6) 循環(huán)步驟2)到5)，直到采樣得到的節(jié)點Tn1與Tn2重合或2 個節(jié)點間的距離小于設定的閾值，則規(guī)劃路徑成功并退出采樣，得到的TS至Tn1的路徑、TF至Tn2的路徑加上Tn1至Tn2的路徑即為最終規(guī)劃出的路徑。

3 仿真實驗與結果分析

為驗證改進RRT 算法的路徑規(guī)劃效果，利用Python 中的matplotlib 功能包以及ROS 平臺的動態(tài)規(guī)劃庫(open motion planning，OMPL)，對改進RRT算法與傳統(tǒng)RRT 算法進行路徑規(guī)劃仿真實驗。

3.1 三維空間仿真實驗與結果分析

三維空間地圖范圍單位大小為100×100×100，設置路徑規(guī)劃的起點坐標為(10,10,10)，終點坐標為(95,95,95)，如圖8。圖中冗余線段為拓展樹的樹結構，從起點連接到終點的線段為傳統(tǒng)RRT 算法和改進RRT 算法最終規(guī)劃出的路線。地圖中的障礙物為8 塊正方體，初始設定采樣長度為5、步長為5，當采樣次數(shù)超過1 000 次時認定此次路徑規(guī)劃失敗，放棄采樣。

圖8 三維空間仿真實驗示意圖Fig.8 Schematic diagram of three-dimensional space simulation experiment

在相同仿真環(huán)境下，分別使用傳統(tǒng)和改進的RRT 算法進行50 次路徑規(guī)劃實驗，對比2 種算法規(guī)劃的平均路徑長度、計算時長和采樣點數(shù)，結果如表3。

表3 2 種算法的路徑規(guī)劃實驗結果Tab.3 Experimental results of path planning for two algorithms

由表3 可看出：與傳統(tǒng)RRT 算法相比，改進RRT 算法平均采樣點數(shù)略高，但規(guī)劃出的單位平均路徑長度從292.6 減少至206.2，即規(guī)劃出的路徑代價大幅減少，且平均計算時長從1.264 s 減至1.156 s，改進RRT 算法規(guī)劃出的路徑更優(yōu)且用時更少。

3.2 ROS 仿真實驗與結果分析

為驗證改進的RRT 算法在加入真實機械臂后的路徑規(guī)劃效果，使用六自由度睿爾曼RM-65 機械臂在ROS 平臺下進行仿真實驗，操作系統(tǒng)為Ubuntu18.04。使用Moveit! Setup Assistant 對機械臂進行配置，具體流程如圖9。

圖9 Moveit! Setup Assistant 配置流程圖Fig.9 Flowchart of Moveit! Setup Assistant configuration

在ROS 平臺下的動態(tài)規(guī)劃庫(open motion planning library，OMPL)中進行算法匹配，配置完成利用Moveit!及其插件接口實現(xiàn)機械臂在仿真環(huán)境下的避障運動規(guī)劃。如圖10 所示綠色機械臂為機械臂起始位姿，橙色機械臂為目標位姿，綠色長方體為障礙物，機械臂從起始位姿開始經(jīng)過路徑規(guī)劃并避開障礙物最終到達目標位姿視為實驗成功。

圖10 避障運動規(guī)劃示意圖Fig.10 Schematic diagram of obstacle avoidance movement planning

使用傳統(tǒng)RRT 算法和改進的RRT 算法分別進行30 次實驗，2 種算法的運動規(guī)劃過程如圖11。在選用軌跡可視化功能后，圖中連續(xù)出現(xiàn)的機械臂即為運動規(guī)劃過程中的路徑軌跡，機械臂軌跡越多說明規(guī)劃時間越長、路徑越復雜。

圖11 ROS 平臺下避障運動規(guī)劃示意圖Fig.11 Schematic diagram of obstacle avoidance motion planning on ROS platform

從圖11 可看出：在相同實驗環(huán)境和運動參數(shù)下，相比于傳統(tǒng)RRT 算法，改進的RRT 算法規(guī)劃時間和更短、規(guī)劃的路徑更優(yōu)，且路徑規(guī)劃成功率提高了10 個百分點，2 種算法平均規(guī)劃時間和規(guī)劃成功率統(tǒng)計如表4。

表4 避障運動規(guī)劃實驗結果Tab.4 Experimental results of obstacle avoidance movement planning

4 結論

提出1 種改進的RRT 算法對六自由度機械臂進行路徑規(guī)劃，結合RRT*和RRT-connect 算法的優(yōu)點，應用目標采樣的思想加強算法向目標點搜索的趨向性，引入貪婪思想提高算法效率，結合五次B樣條插值對路徑平滑優(yōu)化，使機械臂可在較短時間規(guī)劃出更優(yōu)路徑。利用Python 中的matplotlib 功能包統(tǒng)計傳統(tǒng)與改進RRT 算法規(guī)劃路徑所需的時間、采樣點數(shù)量與路徑長度，且在ROS 平臺中利用動態(tài)規(guī)劃庫對所提改進與傳統(tǒng)RRT 算法進行仿真實驗。結果表明：采用所提改進算法對六自由度機械臂進行路徑規(guī)劃，可規(guī)劃出路徑代價更小的路徑且用時更少，最終路徑也更為平滑，驗證了該算法在實際機械臂避障路徑規(guī)劃中的可行性。