石英男， 胥光申*， 盛曉超

(1.西安工程大學(xué) 機(jī)電工程學(xué)院， 陜西 西安 710048；2.西安工程大學(xué) 西安市現(xiàn)代智能紡織裝備重點(diǎn)實(shí)驗(yàn)室， 陜西 西安 710048)

工業(yè)機(jī)械臂是機(jī)械化和自動(dòng)化生產(chǎn)加工過程中一種重要的機(jī)械設(shè)備，在自動(dòng)化生產(chǎn)過程中通過模擬人的動(dòng)作抓取和移動(dòng)物體來完成各項(xiàng)工作，提高勞動(dòng)生產(chǎn)力[1]。

機(jī)械臂避障軌跡規(guī)劃問題是機(jī)械臂研究的重要課題，主要研究當(dāng)工作空間中存在障礙物的情況下，如何尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。現(xiàn)有的機(jī)械臂避障軌跡規(guī)劃的研究有人工勢(shì)場(chǎng)法、蟻群算法、柵格法和快速拓展隨機(jī)樹算法RRT等。課題組基于快速拓展隨機(jī)樹RRT算法進(jìn)行研究，RRT算法是一種通過隨機(jī)構(gòu)建空間填充樹進(jìn)行有效搜索的非凸、高維空間算法。相較于基于圖的搜索算法，RRT算法最主要的優(yōu)點(diǎn)就是收斂速度較快，在多自由度機(jī)械臂的路徑規(guī)劃問題中發(fā)揮著重要作用。

對(duì)于關(guān)節(jié)空間的軌跡規(guī)劃問題，國(guó)內(nèi)外學(xué)者多采用多項(xiàng)式插值法。基于多項(xiàng)式插值法的運(yùn)行位移、速度和加速度曲線比基于線性插值法的運(yùn)行曲線更為平穩(wěn)，機(jī)械臂的軌跡精度更高。文獻(xiàn)[2]中分析了在工業(yè)機(jī)器人軌跡規(guī)劃中采用三、五次多項(xiàng)式的運(yùn)動(dòng)性能，結(jié)果表明五次多項(xiàng)式的軌跡規(guī)劃效果優(yōu)于三次多項(xiàng)式。在實(shí)際工程應(yīng)用中，常采用五次多項(xiàng)式插值法進(jìn)行關(guān)節(jié)軌跡規(guī)劃，該方法的計(jì)算量較小，但關(guān)節(jié)位移變化較大影響了機(jī)械臂工作的平穩(wěn)性[3]。

因此，課題組采用七次多項(xiàng)式對(duì)各關(guān)節(jié)進(jìn)行軌跡規(guī)劃。課題組使用MATLAB機(jī)器人工具箱對(duì)機(jī)械臂進(jìn)行建模以及RRT避障軌跡規(guī)劃的仿真。研究結(jié)果表明：機(jī)械臂在運(yùn)動(dòng)過程中，各關(guān)節(jié)角位移、角速度及角加速度隨時(shí)間變化曲線沒有發(fā)生突變，機(jī)械臂通過RRT算法能夠?qū)崿F(xiàn)平穩(wěn)的避障運(yùn)動(dòng)。

1 六軸機(jī)械臂的運(yùn)動(dòng)學(xué)分析

1.1 機(jī)械臂結(jié)構(gòu)及連桿參數(shù)

課題組選擇了經(jīng)典的6自由度串聯(lián)機(jī)械臂來進(jìn)行建模。6自由度串聯(lián)機(jī)械臂由6個(gè)關(guān)節(jié)及6個(gè)桿件組成，具有6個(gè)關(guān)節(jié)自由度。根據(jù)標(biāo)準(zhǔn)D-H表示法，建立6自由度串聯(lián)機(jī)械臂坐標(biāo)系如圖1所示。6自由度串聯(lián)機(jī)械臂D-H參數(shù)如表1所示[4-5]。

圖1 6自由度機(jī)械臂坐標(biāo)系

表1中，關(guān)節(jié)角θi為從xi-1到xi繞zi的轉(zhuǎn)動(dòng)角度；距離di為從xi-1到xi沿zi的距離；長(zhǎng)度ai為從zi到zi+1沿xi的距離；角度αi為從從zi到zi+1繞xi轉(zhuǎn)動(dòng)的角度。

表1 6自由度機(jī)械臂D-H參數(shù)

連桿變換的一般表達(dá)式：

(1)

(2)

變換矩陣表示由3個(gè)方向向量和1個(gè)位置向量組成。向量n，o和a是3個(gè)方向向量，表示機(jī)械臂末端相對(duì)于參考坐標(biāo)系的方向，如n軸相對(duì)于參考坐標(biāo)系x軸方向表示為nx。第4個(gè)向量p是位置向量，向量p表示機(jī)械臂末端坐標(biāo)系的原點(diǎn)相對(duì)于參考坐標(biāo)系的位置，如px表示機(jī)械臂末端坐標(biāo)系的原點(diǎn)在參考坐標(biāo)系x軸的px處。

1.2 機(jī)械臂MATLAB建模

根據(jù)機(jī)械臂的D-H參數(shù)表，使用MATLAB的機(jī)器人工具箱Robotics Toolbox對(duì)6自由度機(jī)械臂進(jìn)行建模，在Link函數(shù)輸入連桿參數(shù)，再利用SearlLink函數(shù)連接各關(guān)節(jié)建立機(jī)械臂[6]，最后得到圖2所示的6自由度機(jī)械臂的三維模型。

圖2 6自由度機(jī)械臂仿真模型

2 基于RRT算法的路徑規(guī)劃

2.1 機(jī)械臂避障規(guī)劃

課題組在獲取起始點(diǎn)和目標(biāo)點(diǎn)以及障礙物等環(huán)境信息后，通過RRT算法求得路徑，利用機(jī)器人工具箱將起始點(diǎn)和目標(biāo)點(diǎn)之間所經(jīng)過的路徑點(diǎn)與時(shí)間相對(duì)應(yīng)，將關(guān)節(jié)變量變換成與時(shí)間關(guān)系的函數(shù)，然后對(duì)角速度和角加速度進(jìn)行約束進(jìn)而得到機(jī)械臂在避障運(yùn)動(dòng)過程中的軌跡規(guī)劃。

2.2 快速拓展隨機(jī)樹RRT算法

RRT算法是一種與樹的生長(zhǎng)相似的構(gòu)造方法。在路徑規(guī)劃時(shí)，會(huì)在狀態(tài)空間中生成一個(gè)隨機(jī)點(diǎn)，從樹中找到距離該隨機(jī)點(diǎn)最近的位姿點(diǎn)，沿著位姿點(diǎn)到隨機(jī)點(diǎn)的生長(zhǎng)方向，判斷隨機(jī)點(diǎn)是否和障礙物發(fā)生碰撞，如不碰撞，將該隨機(jī)點(diǎn)添加到樹中，不斷重復(fù)，直到最終點(diǎn)附近區(qū)域被搜索到為止。RRT算法的流程如圖3所示[7]。

圖3 RRT算法流程圖

RRT算法路徑規(guī)劃的描述：

1) 初始化搜索樹。令狀態(tài)空間中的起始點(diǎn)為搜索樹的起點(diǎn)，此時(shí)搜索樹中只存在根節(jié)點(diǎn)Xstart一個(gè)節(jié)點(diǎn)；

2) 在狀態(tài)空間中產(chǎn)生一個(gè)隨機(jī)點(diǎn)Xrandom，若該點(diǎn)不在障礙物范圍內(nèi)則計(jì)算搜索樹中所有節(jié)點(diǎn)到點(diǎn)Xrandom的歐式距離，并找到距離最近的節(jié)點(diǎn)Xnear，若該點(diǎn)在障礙物范圍內(nèi)則刪除點(diǎn)Xrandom并重新生成隨機(jī)點(diǎn)，重復(fù)該過程直至找到最近的點(diǎn);

3) 生成新節(jié)點(diǎn)。在點(diǎn)Xnear和點(diǎn)Xrandom連線方向，由點(diǎn)Xnear指向點(diǎn)Xrandom固定生長(zhǎng)距離Growdistance生成一個(gè)新的節(jié)點(diǎn)Xnew，并判斷該節(jié)點(diǎn)是否在障礙物范圍內(nèi)，若不在障礙物范圍內(nèi)則將點(diǎn)Xnew添加到搜索樹中，否則的話返回步驟2)重新對(duì)狀態(tài)空間進(jìn)行隨機(jī)采樣；

4) 停止搜索。當(dāng)點(diǎn)Xnear和目標(biāo)點(diǎn)Xgoal之間的距離小于某個(gè)標(biāo)準(zhǔn)時(shí)，將點(diǎn)Xgoal作為搜索樹的最后一個(gè)節(jié)點(diǎn)，退出搜索，算法結(jié)束并得到所規(guī)劃的路徑。

2.3 機(jī)械臂RRT路徑規(guī)劃

使用MATLAB平臺(tái)，在三維空間中設(shè)定起始點(diǎn)Source=[100,100,10]，設(shè)定目標(biāo)點(diǎn)Goal=[100,-100,10]，在起始點(diǎn)與目標(biāo)點(diǎn)之間添加障礙物，得到如圖4所示的三維空間環(huán)境。

圖4 三維空間環(huán)境

在三維空間中，通過RRT算法建立搜索樹如圖5所示。

圖5 RRT搜索樹

根據(jù)RRT搜索樹得到機(jī)械臂的運(yùn)動(dòng)路徑軌跡如圖6所示。

圖6 有障礙物的路徑軌跡

根據(jù)RRT算法得到的路徑，使用MATLAB機(jī)器人工具箱，通過五次多項(xiàng)式插值法對(duì)機(jī)械臂進(jìn)行軌跡規(guī)劃，得到機(jī)械臂的運(yùn)動(dòng)過程和各個(gè)關(guān)節(jié)運(yùn)行曲線如圖7～10所示。

圖7 6自由度機(jī)械臂避障運(yùn)動(dòng)軌跡

圖8 各關(guān)節(jié)角位移曲線

圖9 各關(guān)節(jié)角速度曲線

圖10 關(guān)節(jié)角加速度曲線

從圖7中可以看到機(jī)械臂從起始點(diǎn)Source開始，沿著軌跡線繞過障礙物，最終達(dá)到指定目標(biāo)點(diǎn)Goal。說明了RRT算法在三維空間進(jìn)行路徑規(guī)劃的可行性。

仿真結(jié)果表明：使用RRT算法在存在障礙物的情況下進(jìn)行路徑規(guī)劃可以有效的避開障礙物，是一種有效的軌跡規(guī)劃算法。但是圖8中可以明顯的看出關(guān)節(jié)1和關(guān)節(jié)6的關(guān)節(jié)位移變化較為明顯，各關(guān)節(jié)的角速度曲線也不夠平滑，說明采用五次多項(xiàng)式進(jìn)行軌跡規(guī)劃時(shí)機(jī)械臂的運(yùn)動(dòng)平穩(wěn)性有所欠缺，不滿足對(duì)作業(yè)精度要求較高的場(chǎng)合。為了提高機(jī)械臂在避障過程中的作業(yè)精度，課題組提出在避障時(shí)采用七次多項(xiàng)式插值法來對(duì)機(jī)械臂進(jìn)行軌跡規(guī)劃[8-9]。

3 七次多項(xiàng)式插值的軌跡規(guī)劃

3.1 七次多項(xiàng)式算法

一般多項(xiàng)式插值法的次數(shù)越高其曲線越平滑，但是次數(shù)過高會(huì)導(dǎo)致插值結(jié)果偏離原函數(shù)。為了保證計(jì)算精度一般最高選取七次多項(xiàng)式插值法進(jìn)行插值計(jì)算。七次多項(xiàng)式插值法規(guī)劃出的曲線比五次多項(xiàng)式插值法得到的曲線更加平穩(wěn)連續(xù)，機(jī)械臂運(yùn)動(dòng)性能更高。

采用七次多項(xiàng)式插值法進(jìn)行關(guān)節(jié)空間的軌跡規(guī)劃，設(shè)七次多項(xiàng)式：

θt=a0+a1t+a2t2+a3t3+a4t4+a5t5+a6t6+a7t7。

(3)

當(dāng)時(shí)間t=0時(shí)：

(4)

當(dāng)時(shí)間t=tm時(shí):

(5)

式(3)～(5)可寫成如下的矩陣形式：

(6)

公式(6)可簡(jiǎn)化為θ=Ma。通過計(jì)算M-1求出各關(guān)節(jié)位置，進(jìn)而求得目標(biāo)運(yùn)動(dòng)軌跡的軌跡方程[10-11]。

3.2 七次多項(xiàng)式插值法避障仿真

本研究中機(jī)械臂主要做避障運(yùn)動(dòng)，過程較為簡(jiǎn)單，所以采用點(diǎn)到點(diǎn)運(yùn)動(dòng)(point-to-point，PTP)的軌跡規(guī)劃。之前已經(jīng)通過RRT算法求得了機(jī)械臂的路徑，采用七次多項(xiàng)式插值法進(jìn)行插值計(jì)算，并輸出其關(guān)節(jié)角位移、關(guān)節(jié)角速度和關(guān)節(jié)角加速度隨時(shí)間的變化曲線如圖11～13所示。

對(duì)比圖11～13結(jié)果和圖8～10結(jié)果可知，機(jī)械臂的各個(gè)關(guān)節(jié)角位移、關(guān)節(jié)角速度和角加速度的變化更加平滑，這說明采用七次多項(xiàng)式插值法比采用五次多項(xiàng)式插值法的軌跡規(guī)劃精度更高。

圖11 各關(guān)節(jié)角位移曲線

圖12 各關(guān)節(jié)角速度曲線

圖13 各關(guān)節(jié)角加速度曲線

較五次多項(xiàng)式插值法來說，七次多項(xiàng)式插值法需要保證角加速度曲線連續(xù)，無(wú)凸變，其機(jī)械臂關(guān)節(jié)的角加速度軌跡更加平滑，機(jī)械臂各關(guān)節(jié)電機(jī)的負(fù)載更加穩(wěn)定，提升了機(jī)械臂的作業(yè)精度。

五次多項(xiàng)式插值法和七次多項(xiàng)式插值法的優(yōu)缺點(diǎn)對(duì)比如表2所示[12]。

表2 2種多項(xiàng)式插值法的優(yōu)缺點(diǎn)對(duì)比

4 結(jié)語(yǔ)

為了提高機(jī)械臂在避障過程中作業(yè)精度，課題組提出采用七次多項(xiàng)式插值法進(jìn)行軌跡規(guī)劃，并在MATLAB平臺(tái)上進(jìn)行仿真實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，對(duì)比使用五次多項(xiàng)式插值法的軌跡規(guī)劃曲線來看，采用七次多項(xiàng)式插值法對(duì)機(jī)械臂運(yùn)動(dòng)進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃時(shí)，機(jī)器人各個(gè)關(guān)節(jié)運(yùn)動(dòng)軌跡更加平滑，關(guān)節(jié)的角速度不存在突變的問題，關(guān)節(jié)的角加速度也更加連續(xù)，實(shí)現(xiàn)了機(jī)械臂的穩(wěn)定運(yùn)行，提高了機(jī)械臂的作業(yè)精度，是一種有效的軌跡規(guī)劃方法。課題組的研究為之后的機(jī)械臂避障研究提供了參考，進(jìn)一步的研究將引入工業(yè)以太網(wǎng)EtherCAT作為通信方式進(jìn)行機(jī)械臂控制平臺(tái)開發(fā)，實(shí)現(xiàn)對(duì)機(jī)械臂的避障控制。