六自由度串聯(lián)機(jī)械臂最優(yōu)軌跡規(guī)劃

劉豪進(jìn),趙炎康,周振興

(中南林業(yè)科技大學(xué)，湖南 長(zhǎng)沙 410007)

機(jī)械手的規(guī)劃軌跡精度取決于機(jī)械臂的運(yùn)動(dòng)學(xué)模型的建立，機(jī)械臂的各關(guān)節(jié)空間的路徑軌跡規(guī)劃以及機(jī)械臂的插值算法補(bǔ)償[1]。本文從ABB IRB1660工業(yè)型機(jī)械臂參數(shù)設(shè)計(jì)入手，建立機(jī)械臂仿真模型，在正逆運(yùn)動(dòng)學(xué)仿真計(jì)算的基礎(chǔ)上對(duì)機(jī)械臂進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃，模擬仿真機(jī)械臂末端執(zhí)行其在空間任意兩點(diǎn)間的運(yùn)動(dòng)過(guò)程，分析其運(yùn)動(dòng)學(xué)參數(shù)，使用 Robotics Toolbox[2]機(jī)器人工具箱，對(duì)連桿進(jìn)行軌跡規(guī)劃，針對(duì)機(jī)械手臂運(yùn)動(dòng)特性入手，完成仿真分析。

1 機(jī)械手臂模型建立

1.1 D-H坐標(biāo)系表述

為了具體分析機(jī)械臂運(yùn)動(dòng)詳情，建立運(yùn)動(dòng)學(xué)方程[3]是必要的，而現(xiàn)在大多數(shù)是多自由度的機(jī)械臂，具有多根連桿和關(guān)節(jié)，也就需要針對(duì)每一根連桿和關(guān)節(jié)來(lái)建立坐標(biāo)系，并建立起各個(gè)坐標(biāo)系的關(guān)聯(lián)。為了論述鄰近兩連桿之間空間位置關(guān)系，引入了D-H參數(shù)法。D-H參數(shù)法為關(guān)節(jié)鏈中的每一根連桿桿件建立了對(duì)應(yīng)坐標(biāo)系，如圖1所示。

圖1 連桿坐標(biāo)簡(jiǎn)化模型

相鄰兩連桿之間的參數(shù)：①兩根連桿之間(兩公垂線ai軸與ai-1軸之間)的距離di；②笛卡爾空間坐標(biāo)xi軸線、yi軸線和zi軸線；③連桿轉(zhuǎn)角θi為兩根公垂線 ai與ai-1之間的夾角，即xi軸與xi-1軸之間的夾角，繞zi-1軸從xi-1旋轉(zhuǎn)到xi軸，符合右手規(guī)則時(shí)為正；④連桿扭角ai為兩關(guān)節(jié)軸線之間的夾角，即zi軸與 zi-1軸之間的夾角，繞x軸從zi-1軸旋轉(zhuǎn)到zi軸；⑤第i根連桿就用L來(lái)表示[4]。

而對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)， d是常數(shù)；對(duì)于移動(dòng)關(guān)節(jié)， di是變量。對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)，θi為變量；對(duì)于移動(dòng)關(guān)節(jié)， θi為常數(shù)。當(dāng)兩關(guān)節(jié)軸線平行時(shí)， ai=0；當(dāng)兩關(guān)節(jié)軸線垂直時(shí)， ai=90°。

通過(guò)測(cè)量得到ABB IRB1660型機(jī)械臂D-H參數(shù)為下表1所示。

表1 ABB IRB1660連桿參數(shù)

注：ai表示第i根公垂線，αi表示第i根連桿扭角，di表示公垂線之間的軸線距離，θi表示第i根連桿轉(zhuǎn)角。

2 機(jī)械臂軌跡規(guī)劃仿真

機(jī)械臂軌跡仿真需要利用MATLAB自帶畫(huà)圖程序來(lái)實(shí)現(xiàn)，用到拆分窗口函數(shù)，以及畫(huà)圖函數(shù)。將預(yù)先規(guī)劃好的時(shí)間 、關(guān)節(jié)旋轉(zhuǎn)角度、角速度及其加速度等參數(shù)輸入進(jìn)函數(shù)，即可繪圖模擬[5]。用分析函數(shù)進(jìn)行曲線擬合分析。

2.1 建立初始化機(jī)械臂連桿模型

本文主要使用的是Robotics Toolbox 為機(jī)械臂的仿真，創(chuàng)建相應(yīng)的機(jī)械臂對(duì)象模型。

通過(guò)語(yǔ)句來(lái)使機(jī)械臂狀態(tài)初始化,代入表1數(shù)據(jù)：>>L1=link([0,0,pi/2,0,0],'standard')

……

用SerialLink函數(shù)建立機(jī)械臂模型，并用畫(huà)圖teach函數(shù)畫(huà)出，如圖2。

>>robot=SerialLink([L1,L2,L3,L4,L5,L6]);

>>robot.name='Robotic6R';

>>q0=[0 0 0 0 0 0];>>robot.plot(q0);

圖2 機(jī)械手臂仿真圖

然后調(diào)節(jié)teach函數(shù)來(lái)驅(qū)使機(jī)械手臂進(jìn)行動(dòng)作，通過(guò)調(diào)節(jié)相應(yīng)滑條來(lái)驅(qū)使連桿關(guān)節(jié)發(fā)生旋轉(zhuǎn)，模擬實(shí)際操作機(jī)械手臂的全過(guò)程。

針對(duì)機(jī)械臂的運(yùn)動(dòng)學(xué)特性，進(jìn)行分析仿真。工具箱中fkine、ikine函數(shù)可以求運(yùn)動(dòng)學(xué)正、逆問(wèn)題[6-8]。首先設(shè)置六個(gè)關(guān)節(jié)扭角分別為：-pi/8、-pi/8、-pi/4、0、pi/8、0，用q向量來(lái)代替，計(jì)算末端位置T。編程如下:

>> q=[-pi/8 -pi/8 -pi/4 0 pi/8 0];

q =

-0.3927 -0.3927 -0.7854 0 0.3927 0

>> T=fkine(Robotic6R,q);

T =

0.1913 -0.8446 0.5000 1.2597

-0.7325 -0.4619 -0.5000 0.0202

0.6533 -0.2706 -0.7071 0.5904

0 0 0 1.0000

T就是機(jī)械臂在扭角為q時(shí)的末端位置。

借助ikine函數(shù)用已知的空間位置T來(lái)反求各個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度，編程如下:

>> qi=ikine(Robotic6R,T);

qi =

-0.3927 -0.3927 -0.7854 0.0000 0.3927 -0.0000

qi為已知ABB機(jī)械臂末端執(zhí)行器位置為T(mén)時(shí)各關(guān)節(jié)的轉(zhuǎn)角。由上述仿真計(jì)算結(jié)果得qi=q。綜上所述，通過(guò)求解運(yùn)動(dòng)學(xué)正問(wèn)題，反求運(yùn)動(dòng)學(xué)逆問(wèn)題得到初始角度值，證明Matlab中的機(jī)械臂物理建模完全正確。

2.2 仿真軌跡規(guī)劃

關(guān)節(jié)空間中機(jī)械臂的每一條軌跡運(yùn)動(dòng)通過(guò)拆解為每個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)一定角度所形成，反映各關(guān)節(jié)轉(zhuǎn)角與機(jī)器人末端執(zhí)行器的位置或姿態(tài)一一對(duì)應(yīng)。對(duì)機(jī)械臂仿真，進(jìn)行軌跡規(guī)劃時(shí)，用到工具箱中的jtraj函數(shù)和ctraj函數(shù)。Jtraj函數(shù)是已知初始和終止的關(guān)節(jié)角度，利用五次多項(xiàng)式公式來(lái)進(jìn)行關(guān)節(jié)空間軌跡分析； ctraj函數(shù)是已知初始和終止的末端關(guān)節(jié)姿態(tài)，利用勻速加速度和勻速減速度來(lái)規(guī)劃笛卡爾空間坐標(biāo)軌跡規(guī)劃[9]。

2.2.1 關(guān)節(jié)空間軌跡規(guī)劃

設(shè)置空間中任意兩點(diǎn)的轉(zhuǎn)角向量qz=[0 0 0 0 0 0]和q=[-pi/8 -pi/8 -pi/4 0 pi/8 0],采樣總時(shí)長(zhǎng)為5秒，采樣間隔為0.02秒。設(shè)置部分程序代碼如下：

>>qz=[0 0 0 0 0 0];

>>q=[-pi/8 -pi/8 -pi/4 0 pi/8 0];

>>t=[0:.02:5];

>>[qf,qd,qdd]=jtraj(q,qz,t);

>>subplot(3,2,1);

>>plot(t,qf(:,1));

>>xlabel('時(shí)間/s');

>>ylabel('角1旋轉(zhuǎn)角度/rad');

……其中qf是關(guān)節(jié)旋轉(zhuǎn)角度，qd是關(guān)節(jié)角速度，qdd是在關(guān)節(jié)角加速度。將程序中qf換成qd或qdd得到六個(gè)關(guān)節(jié)角的角速度和角加速度關(guān)于時(shí)間的函數(shù)圖像。得到關(guān)節(jié)旋轉(zhuǎn)角度與時(shí)間的關(guān)系函數(shù)如圖3所示，關(guān)節(jié)角速度與時(shí)間的關(guān)系函數(shù)如圖4所示，關(guān)節(jié)角加速度與時(shí)間的關(guān)系函數(shù)如圖5所示。

圖3 各關(guān)節(jié)角旋轉(zhuǎn)角度仿真曲線

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

機(jī)械臂從初始位置qz到達(dá)目標(biāo)位置q，以關(guān)節(jié)1為參考對(duì)象：圖3中關(guān)節(jié)1從-0.4(/rad)角度經(jīng)過(guò)5秒到達(dá)0(/rad)角度。圖4中，關(guān)節(jié)1的角速度從第0秒開(kāi)始上升，在第2.5秒左右達(dá)到峰值，在第5秒完全回落為0(/rad)。關(guān)節(jié)1角速度有一個(gè)先快后慢的過(guò)程，在第2.5秒速度最大。圖5中，關(guān)節(jié)1的角加速度在第1秒上升到頂峰，第2.5秒下降為0(/rad)，在第4秒下降為最低值，在5秒上升為0(/rad)。關(guān)節(jié)1角加速度有一個(gè)先上升后下降再上升的過(guò)程，在第1秒加速最快，第4秒加速最慢，在第2.5秒和第5秒不加速。

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

2.2.2 軌跡曲線擬合

在現(xiàn)實(shí)生活中，機(jī)械臂接收信號(hào)頻率過(guò)疏，則會(huì)造成機(jī)械臂的反應(yīng)遲鈍，不能很好地?cái)M合理想化的規(guī)劃軌跡。但接收信號(hào)過(guò)密，則會(huì)造成機(jī)械臂為了更好地?cái)M合理想化的規(guī)劃軌跡產(chǎn)生過(guò)平滑，而不停抖動(dòng)或偏離規(guī)劃軌跡。因此在軌跡規(guī)劃時(shí)要選擇更高效實(shí)惠的軌跡規(guī)劃，兼顧軌跡平滑與機(jī)械臂動(dòng)作平滑。

為了驗(yàn)證仿真中更符合實(shí)際需求的軌跡規(guī)劃，先設(shè)置橫向比較，再進(jìn)行縱向排除，得到最優(yōu)軌跡規(guī)劃。

選擇有效的關(guān)節(jié)角函數(shù)圖像，挑選出第一關(guān)節(jié)角的旋轉(zhuǎn)角度、角速度和角加速度作為參考基準(zhǔn)圖像。

首先對(duì)第一關(guān)節(jié)角的旋轉(zhuǎn)角度分別用三次方多項(xiàng)式(如圖6)、三次多項(xiàng)式三次樣條插值(如圖6)、四次多項(xiàng)式三次樣條插值(如圖7)與五次多項(xiàng)式三次樣條插值(圖7)進(jìn)行函數(shù)曲線擬合，得到殘差值和函數(shù)表達(dá)式。

圖6 旋轉(zhuǎn)角度三次多項(xiàng)式與其三次樣條插值擬合與殘差模對(duì)比

圖7 旋轉(zhuǎn)角度四次多項(xiàng)式與五次多項(xiàng)式三次樣條插值擬合與殘差模對(duì)比

經(jīng)過(guò)對(duì)比，在一定時(shí)間內(nèi)第一關(guān)節(jié)旋轉(zhuǎn)角度環(huán)境下，三次多項(xiàng)式三次樣條插值擬合得到的曲線與四次多項(xiàng)式三次樣條插值擬合得到的曲線基本一致。而利用五次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合得到的殘差模最小。并且由于三次多項(xiàng)式和三次多項(xiàng)式三次樣條插值的方程和曲線一致，因?yàn)樵谙铝袑?duì)第一關(guān)節(jié)的角速度和角加速度的函數(shù)擬合中，將二者合并為一個(gè)比較項(xiàng)。

對(duì)第一關(guān)節(jié)角速度進(jìn)行三次多項(xiàng)式三次樣條插值(如圖8)、四次多項(xiàng)式三次樣條插值(如圖8)與五次多項(xiàng)式三次樣條插值(圖9)進(jìn)行函數(shù)曲線擬合，得到殘差值和函數(shù)表達(dá)式。

圖8 角速度三次多項(xiàng)式與四次多項(xiàng)式三次樣條插值擬合與殘差模對(duì)比

圖9 角速度五次多項(xiàng)式三次樣條插值擬合與殘差模

經(jīng)過(guò)對(duì)比，在角速度環(huán)境下，利用五次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合得到的殘差模雖然最小，但是利用四次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合得到的殘差模與前者非常接近，而且從曲線表現(xiàn)形式上來(lái)看，四次多項(xiàng)式三次樣條插值的擬合曲線更平滑，而不會(huì)為了擬合曲線而頻率過(guò)高地小幅度劇烈抖動(dòng)，所以在角速度的環(huán)境下，利用四次多項(xiàng)式三次樣條插值來(lái)擬合曲線最合適。

對(duì)第一關(guān)節(jié)角加速度進(jìn)行三次多項(xiàng)式三次樣條插值(如圖10)、四次多項(xiàng)式三次樣條插值(如圖10)與五次多項(xiàng)式三次樣條插值(圖11)進(jìn)行函數(shù)曲線擬合，得到殘差值和函數(shù)表達(dá)式。

圖10 角加速度三次多項(xiàng)式與四次多項(xiàng)式三次樣條插值擬合與殘差模對(duì)比

經(jīng)過(guò)對(duì)比，在角加速度環(huán)境下，利用四次和五次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合得到的殘差模雖然最小，但是利用三次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合得到的殘差模與前二者非常接近，而且從曲線表現(xiàn)形式上來(lái)看，三次多項(xiàng)式三次樣條插值的擬合曲線更平滑，而不會(huì)為了擬合曲線而頻率過(guò)高地小幅度劇烈抖動(dòng)，所以在角加速度的環(huán)境下，利用三次多項(xiàng)式三次樣條插值來(lái)擬合曲線最合適。

綜上所述，在一定時(shí)間內(nèi)旋轉(zhuǎn)角度的環(huán)境下，選擇五次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合軌跡效率最佳。在第一關(guān)節(jié)角速度環(huán)境下，選擇四次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合軌跡效率最佳。在第一關(guān)節(jié)角加速度環(huán)境下，選擇三次多項(xiàng)式三次樣條插值來(lái)進(jìn)行擬合軌跡效率最佳。

3 結(jié)論

本文利用D-H參數(shù)法對(duì)ABB IRB1660型號(hào)機(jī)械臂進(jìn)行了參數(shù)采集和物理建模；基于Matlab對(duì)機(jī)械臂進(jìn)行初始化，通過(guò)正解反求逆解驗(yàn)證建模的合理性；將軌跡拆解為每一個(gè)關(guān)節(jié)角的旋轉(zhuǎn)動(dòng)作并分析最優(yōu)解擬合軌跡。本文重在探索機(jī)械臂的仿真模擬特性，有極強(qiáng)的經(jīng)濟(jì)性，并有助于減少實(shí)際操作中出現(xiàn)的問(wèn)題。