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

尹斌，盧文濤，魏文卿

（洛陽(yáng)理工學(xué)院，河南洛陽(yáng)，471000）

0 引言

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

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

1 RRT*算法的引入

1.1 RRT 算法原理

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

算法1RRT 算法的偽代碼

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

圖1 RRT 算法生成結(jié)點(diǎn)

1.2 RRT*算法原理

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

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

2 在ROS 中驗(yàn)證RRT*算法

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

計(jì)算機(jī)的CPU 型號(hào)是：Intel Core i5-10210U，8G 內(nèi)存，運(yùn)行Ubuntu 18.04 操作系統(tǒng)，ROS 版本為Kinetic。

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

圖2 RRT 算法的擴(kuò)展過(guò)程

圖3 RRT*算法改進(jìn)示意圖

表1 UR5 機(jī)械臂的D-H 參數(shù)

搭建的仿真環(huán)境如圖4 所示，UR5 機(jī)械臂前面放置了一個(gè)工作臺(tái)，用于模擬機(jī)械臂的工作環(huán)境，同時(shí)也作為機(jī)械臂運(yùn)動(dòng)過(guò)程中的障礙物。

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

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

圖5 UR5 的起始位姿和目標(biāo)位姿

基于上述的RRT*算法，編寫(xiě)了UR5 機(jī)械臂的避障運(yùn)動(dòng)規(guī)劃算法。運(yùn)行該算法，成功規(guī)劃出了從起始位姿到目標(biāo)位姿的無(wú)碰撞路徑。UR5 機(jī)械臂的運(yùn)動(dòng)軌跡如圖6 所示，從圖中可以看出，機(jī)械臂成功躲避了工作臺(tái)，伸入到門(mén)字形擋板內(nèi)，到達(dá)目標(biāo)位姿。

圖6 機(jī)械臂的運(yùn)動(dòng)軌跡

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

圖7 路徑代價(jià)的變化趨勢(shì)

3 總結(jié)

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