基于Matlab的林地作業(yè)車臂架軌跡規(guī)劃

莊徐 鄭哲文 李科軍

(中南林業(yè)科技大學(xué)材料科學(xué)與工程學(xué)院，湖南 長沙 410000)

引言

在我國林區(qū)伐木作業(yè)領(lǐng)域普遍實現(xiàn)了半機(jī)械化[1]，但森林樹木的采伐仍然以人工勞動為主，工作強(qiáng)度較大，效率較低。近年來，隨著人力成本增加森林作業(yè)成本也在不斷地增加，如何提高森林作業(yè)質(zhì)量和效率成為人們亟待解決的問題[1]。林地作業(yè)車具備快速、準(zhǔn)確、多功能等優(yōu)點[3]，與一般工業(yè)機(jī)器人相比，其臂架結(jié)合機(jī)械結(jié)構(gòu)與液壓驅(qū)動的方式來控制機(jī)械手的運(yùn)動軌跡，能夠提升林區(qū)作業(yè)效率，減輕工人的勞動強(qiáng)度，是林業(yè)現(xiàn)代化的重要標(biāo)志之一[13]。

目前，國內(nèi)外學(xué)者對于運(yùn)動學(xué)分析與軌跡規(guī)劃進(jìn)行了深入研究。沈雅瓊等[5]運(yùn)用齊次變換矩陣，建立平動和轉(zhuǎn)動軌跡方程，進(jìn)行位置和姿態(tài)的插補(bǔ)。滕儒民等[6]采用內(nèi)點法對凸優(yōu)化模型求解，計算出大高度舉高消防車進(jìn)行軌跡規(guī)劃的最優(yōu)解。秦超等[7]利用五次多項式法和擺線運(yùn)動法對比研究末端執(zhí)行器的位移和速度，得出五次多項式法較擺線運(yùn)動法安全性較高。張文典等[9]運(yùn)用關(guān)節(jié)空間對六關(guān)節(jié)臂型機(jī)器人進(jìn)行軌跡規(guī)劃仿真，并驗證該模型能夠準(zhǔn)確實現(xiàn)機(jī)器人關(guān)節(jié)控制。王智興等[10]利用MATLAB下的Robotics Toolbox模塊建立了運(yùn)動學(xué)模型并驗證了運(yùn)動學(xué)方程的正確性。李慶等[11]利用MATLAB與Solidworks對機(jī)器人的軌跡進(jìn)行聯(lián)合仿真求解，證明正運(yùn)動學(xué)與逆運(yùn)動學(xué)求解的準(zhǔn)確性。萬正海等[12]利用MATLAB對自建機(jī)器人手臂進(jìn)行軌跡規(guī)劃仿真，驗證了自建機(jī)器人手臂設(shè)計參數(shù)的合理性。Xi[14]應(yīng)用Bi-RRT算法對冗余機(jī)器人進(jìn)行避障軌跡規(guī)劃，通過仿真實驗驗證了能夠?qū)崿F(xiàn)自動避障需求。Zhang等[15]采用SQP算法用于液壓機(jī)器人挖掘機(jī)軌跡規(guī)劃，驗證了相同權(quán)重下的SQP方法求解最優(yōu)解的效率更高，挖掘軌跡更平滑，自主運(yùn)行變得更加穩(wěn)定和高效。王寧[16]以六自由度工業(yè)焊接機(jī)器人為研究對象，運(yùn)用五次多項式對各個關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到了各個關(guān)節(jié)連續(xù)平滑的軌跡曲線，有效解決了加速度不連續(xù)的問題。雖然相關(guān)研究人員在工業(yè)機(jī)器人領(lǐng)域取得了一定的研究成果，但關(guān)于林區(qū)工程機(jī)械的運(yùn)動學(xué)分析以及軌跡規(guī)劃仿真仍有較大的開展空間。

本文以曲臂式林地作業(yè)車的臂架末端的運(yùn)動軌跡為研究對象，通過建立D-H坐標(biāo)系[8]，分析臂架末端在關(guān)節(jié)空間下的軌跡，在MATLAB環(huán)境中實現(xiàn)臂架軌跡的仿真，得到臂架末端相對于基座標(biāo)系的運(yùn)動曲線，以及運(yùn)動中各關(guān)節(jié)的位移、速度、加速度隨時間變化的曲線。為了提高臂架在工作狀態(tài)下軌跡規(guī)劃的精確性，本文不考慮升降機(jī)構(gòu)的變化，僅研究林地作業(yè)車在升降機(jī)構(gòu)全伸狀態(tài)下的軌跡規(guī)劃。

1 林地作業(yè)車組成結(jié)構(gòu)

林地作業(yè)車需要各臂架的協(xié)調(diào)運(yùn)作來實現(xiàn)末端的工作裝置具備在林區(qū)復(fù)雜的工作環(huán)境下以指定姿態(tài)進(jìn)行動作的能力，主要由底盤、升降機(jī)構(gòu)、俯仰機(jī)構(gòu)、調(diào)平機(jī)構(gòu)和機(jī)械手組成，結(jié)構(gòu)簡圖如圖1所示。

圖1 林地作業(yè)車

2 林地作業(yè)車臂架運(yùn)動學(xué)分析

2.1 正運(yùn)動學(xué)模型的建立

林地作業(yè)車的臂架由連接在一起的連桿系統(tǒng)組成，可以將臂架各組成機(jī)構(gòu)簡化為4個幾何參數(shù)，即2個連桿間的夾角θ，2個連桿間的距離d，連桿的長度a以及連桿的轉(zhuǎn)角α。根據(jù)標(biāo)準(zhǔn)型D-H參數(shù)表示法，建立臂架連桿坐標(biāo)系，如圖2所示，2個連桿之間可通過一個4×4的齊次變換矩陣表示連桿坐標(biāo)系的變化關(guān)系，最終得到末端機(jī)械手坐標(biāo)系相對于基座標(biāo)系的總變換矩陣。臂架D-H參數(shù)如表1所示。

圖2 臂架D-H坐標(biāo)系的建立

(1)

式中，c表示cos，s表示sin，i=1、2、3、4、5、6。

表1 臂架D-H建模參數(shù)

將表(1)中的數(shù)據(jù)帶入式(1)中可得林地作業(yè)車各連桿間的齊次變換矩陣：

(2)

由各連桿間的變換矩陣可推導(dǎo)出末端機(jī)械手坐標(biāo)相對于基坐標(biāo)系的位姿矩陣：

(3)

nx=(c1c2-s1s2)[(c3c4-s3s4)(c5c6-s5s6)-(c3s4+s3c4)(s5c6+c5s6)]

ny=(c1c2+s1s2)[(c3c4-s3s4)(c5c6-s5s6)-(c3s4+s3c4)(s5c6+c5s6)]

nz=(c3s4+s3c4)(s5s6-c5c6)-(c3c4-s3s4)(s5c6+c5s6)

ox=-c1s2-s1c2

oy=c1c2-s1s2

oz=0

ax=(c1c2-s1s2)[(c3s4+s3c4)(c5c6-s5s6)+(c3c4-s3s4)(c5s6+s5c6)]

ay=(c1s2+s1c2)[(c3s4+s3c4)(c5c6-s5s6)+(c3c4-s3s4)(c5s6+s5c6)]

az=(c3c4-s3s4)(c5c6-s5s6)-(c3s4+s3c4)(c5s6+s5c6)

px=a1c1-d3(c1s2+s1c2)+(c1c2-s1s2)[(a4+c5a5)(c3c4-s3s4)-s5a5(c3s4+s3c4)]

py=a1s1+d3(c1c2-s1s2)+(c1s2+s1c2)[(a4+c5a5)(c3c4-s3s4)-s5a5(c3s4+s3c4)]

pz=d2-a5(c3s4c5+s3c4c5+c3c4s5-s3s4s5)-a4(c3s4+s3c4)

假設(shè)臂架初始狀態(tài)的關(guān)節(jié)變量為p=[0,0,0,0,pi/2,0]，調(diào)用Toolbox中的fkine()函數(shù)可以得到臂架末端坐標(biāo)系相對于基座坐標(biāo)系的位姿矩陣，即為臂架的正運(yùn)動學(xué)解。此時由MATLAB計算得到臂架的正運(yùn)動學(xué)解：

(4)

將關(guān)節(jié)變量p=[0,0,0,0,pi/2,0]代入式(2)中，所得結(jié)果與fkine()函數(shù)計算求解一致，由此可以驗證由MATLAB建立的林地作業(yè)車臂架模型的準(zhǔn)確性。

2.2 逆運(yùn)動學(xué)分析

2.2.1 關(guān)節(jié)轉(zhuǎn)角的求解

臂架末端在沿著期望軌跡運(yùn)行的過程中，需要各連桿的轉(zhuǎn)動協(xié)調(diào)配合來實現(xiàn)。根據(jù)林地作業(yè)車臂架的結(jié)構(gòu)特性，關(guān)節(jié)2和關(guān)節(jié)3在靜止時能夠確保臂架末端機(jī)械手在工作時與底盤處于垂直狀態(tài)。因此在工作狀態(tài)下θ2、θ3總是為0。

根據(jù)建立的D-H坐標(biāo)可知，關(guān)節(jié)6的坐標(biāo)由關(guān)節(jié)5坐標(biāo)繞x軸旋轉(zhuǎn)而來，關(guān)節(jié)6的運(yùn)動相對于關(guān)節(jié)5的位置不構(gòu)成影響。因此關(guān)節(jié)5與關(guān)節(jié)6相對于基座標(biāo)系的位姿矩陣相同，即：

(5)

(6)

圖3 林地作業(yè)車臂架關(guān)節(jié)簡圖

由圖3所示的臂架關(guān)節(jié)簡圖可知，可由余弦定理公式計算出θ5,將公式(3)與公式(5)帶入可得：

(7)

由于關(guān)節(jié)6的坐標(biāo)與關(guān)節(jié)5的坐標(biāo)相交于同一點，且關(guān)節(jié)6的運(yùn)動不影響關(guān)節(jié)5的位置，所以關(guān)節(jié)4的運(yùn)動不影響關(guān)節(jié)5與關(guān)節(jié)6的位置，則：

(8)

(9)

圖4 林地作業(yè)車臂架關(guān)節(jié)三維簡圖

由圖4林地作業(yè)車臂架關(guān)節(jié)三維簡圖中連桿1、連桿2、連桿4的幾何關(guān)系可知，關(guān)節(jié)4的位置矩陣：

(10)

將式(9)中各解帶入式(10)得：

(11)

化簡得：

(12)

(13)

由式(2)可知s6和c6的大小，則可計算出θ6的值：

(14)

綜上所述，可通過末端機(jī)械手已知的位姿矩陣計算出θ1、θ2、θ3、θ4、θ5、θ6的值，即林地作業(yè)車臂架的逆運(yùn)動學(xué)解。

2.2.2 逆運(yùn)動學(xué)的驗證

在MATLAB中，調(diào)用Robotics Toolbox中的link()函數(shù)與Seriallink()函數(shù)搭建林地作業(yè)車臂架模型，在臂架處于初始狀態(tài)和工作狀態(tài)下各關(guān)節(jié)的角度作為驗證數(shù)據(jù)，利用正運(yùn)動學(xué)公式求解出末端機(jī)械手相應(yīng)的齊次變換矩陣，并將所得的位姿矩陣帶入到ikine()函數(shù)中，證明逆運(yùn)動學(xué)求解的準(zhǔn)確性。臂架在初始狀態(tài)下和工作狀態(tài)下各關(guān)節(jié)轉(zhuǎn)角分別為p1=[0,0,0,0,pi/2,0]，p2=[pi/3,0,0,-pi/6,pi/6,pi/4],得到末端機(jī)械手的齊次變換矩陣為T1、T2。

(15)

通過MATLAB搭建的臂架在初始狀態(tài)和工作狀態(tài)下的運(yùn)動學(xué)模型如圖5所示，根據(jù)所得的齊次變換矩陣T1、T2，ikine()函數(shù)計算所得的值為Q1=[0,0,0,0,1.57.0],Q2=[-1.05,0,0,-0.52,0.52,0.79]，與臂架各關(guān)節(jié)轉(zhuǎn)角一致，驗證了逆運(yùn)動學(xué)求解的準(zhǔn)確性。

ikine()函數(shù)計算出的結(jié)果與設(shè)定的初始關(guān)節(jié)變量相同，但由于臂架在運(yùn)動時可通過多組關(guān)節(jié)變化實現(xiàn)末端機(jī)械手到指定位置的路徑，其函數(shù)求解只是其中一組路徑，在進(jìn)行軌跡規(guī)劃時則需根據(jù)工作環(huán)境的情況去求解逆運(yùn)動學(xué)的最優(yōu)解。

3 林地作業(yè)車臂架軌跡規(guī)劃仿真

將臂架末端機(jī)械手作為目標(biāo)，機(jī)械手在工作時經(jīng)過的若干點即臂架的軌跡。臂架運(yùn)動時，通過運(yùn)動學(xué)求解計算出各關(guān)節(jié)的變化規(guī)律，結(jié)合工作對象選擇合適的關(guān)節(jié)規(guī)劃出一條光滑軌跡。對臂架進(jìn)行軌跡規(guī)劃主要有2種方法，關(guān)節(jié)空間下的軌跡規(guī)劃和笛卡爾空間下的軌跡規(guī)劃。根據(jù)林地作業(yè)車的結(jié)構(gòu)特征，主要考慮機(jī)械手夾取樹木時從初始位置至目標(biāo)位置的軌跡，因此選用關(guān)節(jié)空間下的軌跡規(guī)劃較為適用。

圖5 林地作業(yè)車臂架Robot模型

在關(guān)節(jié)空間下進(jìn)行軌跡規(guī)劃時，首先確定末端機(jī)械手的初始位置和目標(biāo)位置，對初始位置和目標(biāo)位置進(jìn)行逆運(yùn)動學(xué)求解計算出各關(guān)節(jié)所需變化的角度。本文在MATLAB平臺中采用五次多項式插值進(jìn)行軌跡規(guī)劃。

在RoboticsToolbox中調(diào)用jtraj()函數(shù)運(yùn)用到林地作業(yè)車模型上，設(shè)置初始位置關(guān)節(jié)轉(zhuǎn)角[-pi/6,0,0,0,pi/2,0],目標(biāo)位置關(guān)節(jié)轉(zhuǎn)角[pi/6,0,0,-pi/6,pi/6,pi/6]，步長設(shè)置為100，軌跡規(guī)劃如圖6所示。

圖6 臂架關(guān)節(jié)空間下的軌跡規(guī)劃

各關(guān)節(jié)的位移、速度、加速度如圖7～9所示。

圖7 關(guān)節(jié)位移

圖8 關(guān)節(jié)速度

圖9 關(guān)節(jié)加速度

分析圖7～9可知，臂架在初始位置和目標(biāo)位置的速度與加速度都為0。在50s時，除關(guān)節(jié)2和關(guān)節(jié)3外，各關(guān)節(jié)加速度為0且速度達(dá)到最大值，且關(guān)節(jié)1與關(guān)節(jié)5大小相同方向相反，關(guān)節(jié)4與關(guān)節(jié)6大小相同方向相反。在運(yùn)動狀態(tài)下，各關(guān)節(jié)的位移曲線光滑、均勻，且各關(guān)節(jié)的速度沒有出現(xiàn)劇變，說明臂架各關(guān)節(jié)在工作時較為穩(wěn)定，沒有出現(xiàn)明顯振動。

4 結(jié)論

本文根據(jù)林地作業(yè)車六自由度臂架的結(jié)構(gòu)進(jìn)行了運(yùn)動學(xué)分析，采用標(biāo)準(zhǔn)型D-H法建立臂架連桿坐標(biāo)系，完成了臂架各關(guān)節(jié)正逆運(yùn)動學(xué)求解，通過MATLAB軟件驗證了正逆運(yùn)動學(xué)模型的準(zhǔn)確性。

通過對臂架各關(guān)節(jié)正逆運(yùn)動學(xué)求解分析，得到了各關(guān)節(jié)和末端機(jī)械手的位姿之間的齊次變換矩陣。

運(yùn)用MATLAB軟件對林地作業(yè)車的臂架進(jìn)行了運(yùn)動學(xué)仿真和軌跡規(guī)劃，各關(guān)節(jié)的位移、速度、加速度隨時間變化的曲線光滑、連續(xù)，驗證了臂架各關(guān)節(jié)在運(yùn)行過程中的穩(wěn)定性。