基于七次多項式插值的RRT避障規(guī)劃

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

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

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

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

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

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

1 六軸機械臂的運動學(xué)分析

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

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

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

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

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

連桿變換的一般表達式：

(1)

(2)

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

1.2 機械臂MATLAB建模

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

圖2 6自由度機械臂仿真模型

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

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

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

2.2 快速拓展隨機樹RRT算法

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

圖3 RRT算法流程圖

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

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

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

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

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

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

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

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

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

圖5 RRT搜索樹

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

圖6 有障礙物的路徑軌跡

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

圖7 6自由度機械臂避障運動軌跡

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

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

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

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

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

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

3.1 七次多項式算法

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

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

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

(3)

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

(4)

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

(5)

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

(6)

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

3.2 七次多項式插值法避障仿真

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

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

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

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

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

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

五次多項式插值法和七次多項式插值法的優(yōu)缺點對比如表2所示[12]。

表2 2種多項式插值法的優(yōu)缺點對比

4 結(jié)語

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