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

尹斌，盧文濤，魏文卿

（洛陽理工學院，河南洛陽，471000）

0 引言

六自由度機械臂因其工作空間大、靈活性強，在工業(yè)自動化領域應用很廣泛，如在汽車領域用于搬運、焊接、噴涂等。工業(yè)現場中，機械臂周圍經常會有其他設備、工件，甚至還需要與人協(xié)同工作，這些都需要機械臂具備自動規(guī)劃路徑、躲避障礙物的能力。機械臂的避障路徑規(guī)劃，就是機械臂自動規(guī)劃一條從起始點到目標點的無碰撞路徑。傳統(tǒng)的避障路徑規(guī)劃算法：可視圖法[1]、人工勢場法[2]、自由空間法[3]、柵格法[4]等，這些算法都需要對障礙物進行準確的建模和描述，隨著機器人自由度的增加和機器人所處環(huán)境的復雜性增大，這些算法實現的難度增大，執(zhí)行效率也低。因此，這些算法不適合六自由度機械臂的避障路徑規(guī)劃。

為避免對障礙物的精確建模，有學者提出了基于隨機采樣的避障路徑規(guī)劃，這類方法不需要對機器人周邊環(huán)境建模，只須知道機器人與障礙物是否碰撞的信息，依據此信息，連接無碰撞的采樣點。隨著采樣點的增多，從而構建一張無碰撞的圖，最終從圖中搜索起始點到目標點的路徑。其中，快速擴展隨機樹法（Rapidly-exploring Random Trees, RRT）[5]是這類算法的典型代表，它由LaValle 提出，是一種基于隨機采樣的步進式的算法。學者們從不同方面對基本的 RRT 算法進行了改善[6-8]，Sertac 和Emilio 提出一種RRT 的改進方法—Rapidly-exploring Random Tree Star（RRT*）[9]，這種方法具有路徑優(yōu)化能力，而且具有概率完備性。本文將分析RRT*的原理，并在機器人操作系統(tǒng)（RobotOperating System， ROS）中搭建仿真平臺，對比RRT 和RRT*兩種算法，驗證RRT*算法的優(yōu)勢，解決六自由度機械臂的避障路徑規(guī)劃。

1 RRT*算法的引入

1.1 RRT 算法原理

基本的RRT 算法的偽代碼如算法1 所示。Extend 函數有三個返回值，“Advanced”表示擴展成功，但未到達目標構形；“Trapped”表示發(fā)生碰撞，沒有擴展成功；“Reached”表示擴展成功，到達目標構形。首先用初始構形 initx初始化圖G的節(jié)點集V，邊的集E為空集。RRT Planner 每次循環(huán)中，在自由空間中隨機采樣一個點randx，然后執(zhí)行Extend函數：尋找xrand的最近節(jié)點xnearest，并連接xnearest和xrand得到點xnew，如圖1 所示。如果xnearest和xnew連線的任意點與障礙物沒有碰撞，則將新產生的點xnew和邊(xnearest,xnew)添加到圖G(V,E)中，并判斷點xnew是否落在目標區(qū)域中，如果xnew落在目標區(qū)域中，則說明到達目標構形xgoal，停止循環(huán)，返回圖G(V,E)；如果沒有落在目標區(qū)域，則繼續(xù)循環(huán)，直到xnew落在目標區(qū)域或達到循環(huán)次數的上限。

算法1RRT 算法的偽代碼

圖2 是RRT 算法在的平面無障礙區(qū)域中的擴展過程，分別給出了迭代次數為100、200、700 的狀態(tài)。從圖中可以看出，擴展過程就像是以初始點為根的一棵樹，不斷有新的分支長出來，探索未知的空間[10]。

圖1 RRT 算法生成結點

1.2 RRT*算法原理

RRT 算法存在一個缺點，規(guī)劃的路徑不是最優(yōu)的。為了改進規(guī)劃路徑的質量，Sertac 和Emilio 提出一種RRT 的改進方法—RRT*，它具有漸進優(yōu)化能力，即隨著規(guī)劃時間的增加，規(guī)劃路徑逐漸趨于最優(yōu)值，并在理論上得到了論證。

RRT*算法沒有把新產生的點newx直接與它的最近節(jié)點xnearest相連，而是經過兩個階段的處理。第一階段：從xnew的附近節(jié)點Xnear中，尋找使距離xnew代價最小的節(jié)點作為xnew的父節(jié)點，保證了新添加的節(jié)點xnew對應的路徑代價有局部最小值，圖3（a）所示。第二階段：如果經過節(jié)點xnew到附近節(jié)點xnear的路徑代價小于節(jié)點xnear之前的路徑代價，則將xnear的父節(jié)點改為節(jié)點xnew，如圖3（b）所示。RRT*算法在這兩點上改進了RRT 算法，從而保證了RRT*算法的漸進優(yōu)化能力。

2 在ROS 中驗證RRT*算法

為了驗證RRT*算法的漸進優(yōu)化能力，在機器人操作系統(tǒng)（ROS）中進行算法的驗證。

計算機的CPU 型號是：Intel Core i5-10210U，8G 內存，運行Ubuntu 18.04 操作系統(tǒng)，ROS 版本為Kinetic。

六自由度機械臂采用的是優(yōu)傲協(xié)作機器人UR5，臂展850mm，本體質量33.5kg，額定負載5kg，其D-H 參數如表1 所示。

圖2 RRT 算法的擴展過程

圖3 RRT*算法改進示意圖

表1 UR5 機械臂的D-H 參數

搭建的仿真環(huán)境如圖4 所示，UR5 機械臂前面放置了一個工作臺，用于模擬機械臂的工作環(huán)境，同時也作為機械臂運動過程中的障礙物。

圖4 UR5 的仿真環(huán)境

設定UR5 機械臂初始位姿為零位，目標位置各個關節(jié)為[11.5°、 -7°、 80.5°、 90°、 90°、 0]，機械臂的狀態(tài)如圖5 所示。

圖5 UR5 的起始位姿和目標位姿

基于上述的RRT*算法，編寫了UR5 機械臂的避障運動規(guī)劃算法。運行該算法，成功規(guī)劃出了從起始位姿到目標位姿的無碰撞路徑。UR5 機械臂的運動軌跡如圖6 所示，從圖中可以看出，機械臂成功躲避了工作臺，伸入到門字形擋板內，到達目標位姿。

圖6 機械臂的運動軌跡

選取UR5 機械臂末端的笛卡爾路徑作為路徑代價，仿真中發(fā)現，隨著迭代次數的增加，路徑代價逐漸趨于最優(yōu)值，如圖7 所示，這證明了RRT*算法具有漸進優(yōu)化能力。

圖7 路徑代價的變化趨勢

3 總結

本文采用了RRT*算法解決六自由度機械臂的避障路徑規(guī)劃問題，論述了RRT*算法的原理，并在ROS 中搭建了UR5機械臂的仿真環(huán)境，基于RRT*算法UR5 機械臂成功規(guī)劃了避障路徑，并驗證了RRT*算法的漸進優(yōu)化能力。但本文也存在有待完善的地方，比如RRT*算法的規(guī)劃速度慢、成功率低，這是需要繼續(xù)改進的地方。