六自由度機(jī)器人運(yùn)動(dòng)學(xué)仿真及軌跡規(guī)劃*

樂(lè)　英 ,岳艷波

(華北電力大學(xué) 能源動(dòng)力與機(jī)械工程學(xué)院，河北 保定　071000)

?

六自由度機(jī)器人運(yùn)動(dòng)學(xué)仿真及軌跡規(guī)劃*

樂(lè)英 ,岳艷波

(華北電力大學(xué) 能源動(dòng)力與機(jī)械工程學(xué)院，河北 保定071000)

為減小機(jī)器人運(yùn)動(dòng)過(guò)程中的振動(dòng)和沖擊，延長(zhǎng)機(jī)器人使用壽命，提高軌跡規(guī)劃的精度，以六自由度工業(yè)機(jī)器人為研究對(duì)象，針對(duì)采用五次多項(xiàng)式樣條規(guī)劃的軌跡因加加速度有突變而導(dǎo)致的軌跡精度不高的問(wèn)題，采用五次和七次非均勻B樣條對(duì)機(jī)器人軌跡進(jìn)行規(guī)劃。機(jī)器人的運(yùn)動(dòng)學(xué)建模和軌跡規(guī)劃的仿真通過(guò)MATLAB平臺(tái)實(shí)現(xiàn)。仿真結(jié)果表明,采用五次和七次非均勻B樣條規(guī)劃的運(yùn)動(dòng)軌跡速度、加速度光滑，加加速度連續(xù)沒(méi)有突變，機(jī)器人軌跡精度較好，運(yùn)動(dòng)性能優(yōu)于五次多項(xiàng)式樣條。

工業(yè)機(jī)器人；非均勻B樣條；軌跡規(guī)劃；MATLAB

0　引言

機(jī)器人軌跡規(guī)劃的一個(gè)目標(biāo)是研究一種可靠的方法提高機(jī)器人運(yùn)動(dòng)軌跡的精度和穩(wěn)定性，規(guī)劃的各關(guān)節(jié)位移、速度、加速度、加加速度曲線光滑連續(xù)無(wú)突變[1]。避免由于突變引發(fā)的的機(jī)械磨損和振動(dòng)沖擊[2]。

機(jī)器人軌跡規(guī)劃方法主要是利用多項(xiàng)式、拋物線、樣條曲線以及不同曲線相結(jié)合進(jìn)行規(guī)劃。在實(shí)際工程應(yīng)用中，常采用三次多項(xiàng)式對(duì)關(guān)節(jié)軌跡進(jìn)行規(guī)劃。但是應(yīng)用三次多項(xiàng)式進(jìn)行軌跡規(guī)劃得不到光滑的加速度曲線。增加多項(xiàng)式次數(shù)，可以提高運(yùn)動(dòng)軌跡和控制精度，但是隨之計(jì)算量也會(huì)加大，而且高次插值易發(fā)生“龍格”現(xiàn)象。文獻(xiàn)[3]應(yīng)用五次多項(xiàng)式樣條進(jìn)行軌跡規(guī)劃，用MATLAB仿真驗(yàn)證表明，五次多項(xiàng)式樣條軌跡規(guī)劃方法效果優(yōu)于三次多項(xiàng)式樣條。文獻(xiàn)[4]分析比較了三、五、七次多項(xiàng)式樣條軌跡規(guī)劃方法，文獻(xiàn)[5]采用三次非均勻B樣條對(duì)機(jī)器人軌跡進(jìn)行規(guī)劃，但都沒(méi)有考慮加加速度的影響。在機(jī)械系統(tǒng)中，進(jìn)給運(yùn)動(dòng)的加加速度對(duì)系統(tǒng)的穩(wěn)定性有重要影響。如果加加速度有突變，在階躍變化的上升沿和下降沿, 機(jī)械系統(tǒng)會(huì)產(chǎn)生振動(dòng)和沖擊, 影響運(yùn)行平穩(wěn)性, 進(jìn)而影響加工質(zhì)量[6]。

采用五次多項(xiàng)式樣條規(guī)劃的軌跡加加速度有突變，精度不夠高，而七次多項(xiàng)式樣條約束條件比較多。針對(duì)這些問(wèn)題，本文以REBOT-V-6R機(jī)器人為研究對(duì)象，采用五次和七次非均勻B樣條對(duì)機(jī)器人進(jìn)行軌跡規(guī)劃，并在MATLAB平臺(tái)上進(jìn)行仿真，驗(yàn)證軌跡規(guī)劃方法的有效性和可靠性。

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

采用五次多項(xiàng)式樣條、五次和七次非均勻B樣條在關(guān)節(jié)空間對(duì)機(jī)器人軌跡進(jìn)行規(guī)劃。根據(jù)已知的工作空間原始路徑點(diǎn)，反求出對(duì)應(yīng)的關(guān)節(jié)空間各關(guān)節(jié)位移。利用這些求出的關(guān)節(jié)位移值，加上設(shè)定的所需速度、加速度值，分別用五次多項(xiàng)式樣條、五次和七次非均勻B樣條插值求出相應(yīng)的關(guān)節(jié)軌跡曲線?；谝?guī)劃出的關(guān)節(jié)軌跡曲線，再正向求解出機(jī)器人末端規(guī)劃后的運(yùn)動(dòng)軌跡。

1.1五次多項(xiàng)式樣條

五次多項(xiàng)式樣條中，兩節(jié)點(diǎn)之間的曲線段由五次多項(xiàng)式擬合生成，整個(gè)曲線由一段五次多項(xiàng)式組成。五次多項(xiàng)式函數(shù)式中共有6個(gè)未知系數(shù)，對(duì)其求一階、二階導(dǎo)，可求出機(jī)器人末端的速度、加速度關(guān)于時(shí)間t的函數(shù)式[7]：

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

(2)

(3)

其中與起始和終止時(shí)間點(diǎn)t0、tf對(duì)應(yīng)的角位移θ0和θf(wàn)為已知，且滿足條件：

(4)

在每段多項(xiàng)式起始點(diǎn)和終止點(diǎn)處滿足二階連續(xù)，設(shè)置始末點(diǎn)的速度、加速度，且滿足條件：

(5)

每個(gè)點(diǎn)的角位移值為已知量，設(shè)定其角速度和角加速度為0(可設(shè)定為其它值)，兩點(diǎn)處共可得到6個(gè)已知條件。將已知條件代入公式(1)、(2)、(3)中并聯(lián)立求解，可解出6個(gè)未知的系數(shù)。

1.2五次非均勻B樣條

非均勻B樣條的公式如下，

(6)

式中,di為控制頂點(diǎn)，Ni,k(u)為k次規(guī)范B樣條基函數(shù),且:

(7)

式中,k表示非均勻B樣條次數(shù),i表示序號(hào)。

非均勻B樣條插值曲線由n+k個(gè)控制頂點(diǎn)di(i=0,1,2,…n+k-1)定義，節(jié)點(diǎn)矢量相應(yīng)為U=[u0,u1,…un+2k]。

設(shè)機(jī)器人的總運(yùn)動(dòng)時(shí)間為tall=tn-t0，采用累計(jì)弦長(zhǎng)參數(shù)化方法對(duì)時(shí)間節(jié)點(diǎn)ti歸一化，得到節(jié)點(diǎn)矢量U=[u0,u1,…un+2k]。且有：

i=k+1,k+2,…,n+k-1

(8)

用于插值n+1個(gè)數(shù)據(jù)點(diǎn)pi的k次非均勻B樣條曲線方程可寫(xiě)為：

(9)

將曲線定義域[uk,un+k]內(nèi)的節(jié)點(diǎn)值依次代入方程，得到滿足插值條件的n+1個(gè)方程：

ui+k∈[uk,un+k],i=0,1,…n

(10)

對(duì)于k次非均勻B樣條開(kāi)曲線還需增加k-1個(gè)通常由邊界條件給定的附加方程,附加方程通過(guò)切矢邊界條件得到，因取兩端節(jié)點(diǎn)重復(fù)度為k,其首末控制頂點(diǎn)就是首末數(shù)據(jù)點(diǎn),因此有:

(11)

同理可得：

(12)

將已知的始末點(diǎn)角位移和設(shè)定的角速度、角加速度值代入公式(10)、(11)、(12)中，并聯(lián)立求解出非均勻B樣條插值曲線的控制頂點(diǎn)[8],同理可求出七次非均勻B樣條的插值曲線。對(duì)求出的非均勻B樣條曲線依次求導(dǎo)，可分別得到機(jī)器人運(yùn)動(dòng)軌跡的速度、加速度、加加速度曲線。

2　機(jī)器人建模與仿真

以REBOT-V-6R機(jī)器人為研究對(duì)象，通過(guò)D-H法建立機(jī)器人的數(shù)學(xué)模型，該機(jī)器人各關(guān)節(jié)及連桿參數(shù)如表1所示。

表1　六自由度機(jī)器人的D-H參數(shù)表

其中Li為各關(guān)節(jié)編號(hào)，theta為關(guān)節(jié)變量，d為連桿偏移量，a為連桿長(zhǎng)度，alpha連桿扭角。

圖1　 機(jī)器人示教界面

通過(guò)MATLAB中的robotics toolbox工具箱[9]建立機(jī)器人的模型，通過(guò)工具箱中的LINK函數(shù)建立機(jī)器人的各連桿模型，

L1=Link([0 10 10 pi/2 0 0]);

將機(jī)器人命名為“bot”，通過(guò)Teach函數(shù)，可在三維空間中通過(guò)拖動(dòng)滑塊，觀察機(jī)器人模型的運(yùn)動(dòng)狀況。如圖1所示。

teach(bot)

通過(guò)工具箱中的fkine函數(shù)，設(shè)定初始關(guān)節(jié)變量q0，可以求出機(jī)器人末端在工作空間的絕對(duì)坐標(biāo)。

q0=[0 0 0 0 0 0];

T0=bot.fkine(q0);

求出機(jī)器人工作初始姿態(tài)下，末端在笛卡爾空間中的坐標(biāo)點(diǎn)為p1=[41 -4 41.1]。

在機(jī)器人工作空間中，可以直觀的觀察到機(jī)器人末端的運(yùn)動(dòng)情況，但是規(guī)劃的軌跡容易產(chǎn)生奇異點(diǎn)[10]，通過(guò)工具箱中jocb函數(shù)，可以將機(jī)器人末端的速度映射為每個(gè)關(guān)節(jié)的角速度。

q=[0 0 0 0 0 0];

J0=bot.jacob0(q);

Qd=[qd1qd2qd3qd4qd5qd6]′;

V=J0*Qd;%V=[dxdydzwxwywz]′

通過(guò)工具箱中的bot.A函數(shù)，可以求出機(jī)器人每個(gè)運(yùn)動(dòng)桿件的運(yùn)動(dòng)情況，避免機(jī)器人各部件與周邊物件相碰撞。

A3=bot.A(1 2 3,q1)%經(jīng)q1角度變化后，3關(guān)節(jié)的空間坐標(biāo)。

A5.1=bot.A([2 3 4 5],q1)%經(jīng)q1變化后，5關(guān)節(jié)相對(duì)1關(guān)節(jié)的空間坐標(biāo)。

以建立的機(jī)器人模型為平臺(tái)，以機(jī)器人末端為作業(yè)點(diǎn)，使末端在三維空間中畫(huà)一個(gè)矩形，畫(huà)圖過(guò)程中機(jī)器人末端的姿態(tài)保持不變。為便于觀察，以p1為矩形的一個(gè)頂點(diǎn)，其它三個(gè)頂點(diǎn)的坐標(biāo)依次為：p2=[41 26 41.1];

p3=[11 26 41.1];p4=[11 -4 41.1];

在矩形的每條邊上，等距離的取10個(gè)中間點(diǎn)，四條邊共40個(gè)點(diǎn)。40個(gè)點(diǎn)的坐標(biāo)信息如下：

X=[41 41 41 41 41 41 41 41 41 41 41 38 35 32 29 26 23 20 17 14 11 11 11 11 11 11 11 11 11 11 11 14 17 20 23 26 29 32 35 38];

Y=[-4 -1 2 5 8 11 14 17 20 23 26 26 26 26 26 26 26 26 26 26 26 23 20 17 14 11 8 5 2 -1 -4 -4 -4 -4 -4 -4 -4 -4 -4 -4];

Z方向坐標(biāo)值均為41.1。

通過(guò)工具箱中的ikine函數(shù)，可以反求出與工作空間相對(duì)應(yīng)的關(guān)節(jié)空間中機(jī)器人各關(guān)節(jié)角度值。

Q=bot.ikine(T,q);

由40個(gè)工作空間坐標(biāo)點(diǎn)反求出40×6個(gè)關(guān)節(jié)位移值，以這40×6個(gè)位移值為已知條件，分別以五次樣條和五次、七次非均勻B樣條規(guī)劃?rùn)C(jī)器人末端運(yùn)動(dòng)軌跡，并設(shè)定末端經(jīng)過(guò)每個(gè)路徑點(diǎn)的速度和加速度均為0(可設(shè)定為其它值)。

通過(guò)MATLAB編程求出與機(jī)器人各關(guān)節(jié)相對(duì)應(yīng)的五次樣條和五次、七次非均勻B樣條各6條。三種樣條均為關(guān)于時(shí)間t的函數(shù)，總時(shí)間t取為12s，各樣條曲線之間相互獨(dú)立。

對(duì)求出的三種樣條曲線依次求導(dǎo)，得到各關(guān)節(jié)運(yùn)動(dòng)軌跡的角速度、角加速度、角加加速度曲線。畫(huà)出機(jī)器人各關(guān)節(jié)的運(yùn)動(dòng)曲線，為便于比較，可由圖中任意截取一段(機(jī)器人末端在每個(gè)路徑點(diǎn)處的速度、加速度均為0)，這里取t=[2.5,3.5]時(shí)間段，只列出基于三種曲線規(guī)劃的1、2、3關(guān)節(jié)運(yùn)動(dòng)曲線。分別如圖2～圖4所示。

圖2　截取的五次多項(xiàng)式樣條規(guī)劃1-3關(guān)節(jié)運(yùn)動(dòng)曲線

圖3　截取的五次非均勻B樣條規(guī)劃1-3關(guān)節(jié)運(yùn)動(dòng)曲線

圖4　截取的七次非均勻B樣條規(guī)劃1-3關(guān)節(jié)運(yùn)動(dòng)曲線

由圖2～圖4中可以看出，由兩種樣條曲線規(guī)劃出的速度、加速度區(qū)別不是很大，都比較連續(xù)光滑。但五次多項(xiàng)式規(guī)劃的運(yùn)動(dòng)軌跡在中間路徑點(diǎn)處加加速度有突變。而五次非均勻B樣條規(guī)劃的運(yùn)動(dòng)軌跡在中間點(diǎn)處的加加速度是連續(xù)的，七次非均勻B樣條規(guī)劃的加加速度連續(xù)且光滑。在加加速度有突變的地方，會(huì)產(chǎn)生振動(dòng)和沖擊，因此，基于五次和七次非均勻B樣條的規(guī)劃方法可使機(jī)器人性能更穩(wěn)定，壽命更長(zhǎng)。

在規(guī)劃出的五次多項(xiàng)式樣條和非均勻B樣條上以等時(shí)間間隔0.05s采集各關(guān)節(jié)位移數(shù)據(jù)，可采集到240組關(guān)節(jié)位移數(shù)據(jù)。通過(guò)工具箱中的fkine函數(shù)，可求出與240組關(guān)節(jié)位移數(shù)據(jù)相對(duì)應(yīng)的240個(gè)工作空間坐標(biāo)點(diǎn)。T1=bot.fkine(q1);

這些坐標(biāo)點(diǎn)均為機(jī)器人末端規(guī)劃路徑點(diǎn)。將其在三維空間中展示出，如圖5所示。

圖5　基于五次多項(xiàng)式樣條的機(jī)器人末端軌跡

規(guī)劃的軌跡在X、Y、Z方向上均有誤差，這里以Z方向上的誤差為例進(jìn)行分析，非均勻B樣條規(guī)劃的軌跡也有類(lèi)似的結(jié)果。將三種曲線規(guī)劃的軌跡在Z方向的擺動(dòng)做成誤差曲線，如圖6～圖8所示。

圖6　五次多項(xiàng)式樣條軌跡誤差

圖7　 五次非均勻B樣條軌跡誤差

通過(guò)比較可以看出，基于五次非均勻B樣條規(guī)劃的軌跡誤差比五次多項(xiàng)式樣條要小，七次非均勻B樣條規(guī)劃的軌跡誤差最小。說(shuō)明五次非均勻B樣條規(guī)劃的軌跡要優(yōu)于五次樣條，七次非均勻B樣條規(guī)劃的軌跡要優(yōu)于五次非均勻B樣條。

進(jìn)一步的可以驗(yàn)證，如果單純?cè)黾右?guī)劃路徑點(diǎn)采集數(shù)量，雖可以使機(jī)器人運(yùn)動(dòng)路徑更加平穩(wěn)光滑，但并不能減小誤差，且運(yùn)算量也會(huì)隨之增大。要減小誤差，只能增加原始路徑點(diǎn)的數(shù)量(既增加矩形每條邊上的路徑點(diǎn))或采用其它規(guī)劃方法。

圖8　 七次非均勻B樣條軌跡誤差

3　結(jié)論

為使機(jī)器人運(yùn)動(dòng)平穩(wěn)，延長(zhǎng)機(jī)器人使用壽命，提高軌跡規(guī)劃精度，以REBOT-V-6R機(jī)器人為研究對(duì)象，通過(guò)MATLAB建立機(jī)器人三維模型。分別采用五次多項(xiàng)式樣條、五次和七次非均勻B樣條對(duì)機(jī)器人軌跡進(jìn)行規(guī)劃，通過(guò)MATLAB仿真比較三種曲線規(guī)劃的軌跡運(yùn)動(dòng)曲線和誤差曲線，結(jié)果表明五次、七次非均勻B樣條曲線規(guī)劃的軌跡優(yōu)于五次多項(xiàng)式樣條。

采用五次非均勻B樣條規(guī)劃的軌跡加加速度連續(xù)，可使機(jī)器人運(yùn)動(dòng)平穩(wěn)，軌跡光滑，提高機(jī)器人運(yùn)動(dòng)性能。在不考慮計(jì)算量的狀況下，七次非均勻B樣條優(yōu)于非均勻五次B樣條，加加速度連續(xù)且光滑，規(guī)劃的速度、加速度值較小，軌跡規(guī)劃精度更高，是一種穩(wěn)定可靠的軌跡規(guī)劃方法。

[1] 勾治踐，王成，付威.機(jī)器人軌跡規(guī)劃的有效方法[J].煤礦機(jī)械，2013,34(8):66-69.

[2] 楊錦濤，姜文剛，林永才.工業(yè)機(jī)器人沖擊最優(yōu)的軌跡規(guī)劃算法[J].科學(xué)技術(shù)與工程，2014,14(28)：64-69.

[3] 王寧，張新敏.基于MATLAB的六自由度機(jī)器人軌跡規(guī)劃與仿真[J].制造業(yè)自動(dòng)化，2014,36(8):95-97.

[4] 王學(xué)林,邢仁鵬,肖永飛,等.機(jī)械臂途經(jīng)N路徑點(diǎn)的連續(xù)軌跡插補(bǔ)算法研究[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2014(11):92-96.

[5] 賴永林，林茂松，梁艷陽(yáng).基于三次非均勻B樣條曲線的機(jī)器人軌跡規(guī)劃算法研究[J].科學(xué)技術(shù)與工程，2013,13(35):10511-10517.

[6] 趙國(guó)勇，趙玉剛，趙慶志.數(shù)控運(yùn)動(dòng)中加加速度連續(xù)的加減速方法[J].計(jì)算機(jī)集成制造系統(tǒng)，2011,17(2):16-21.

[7] 林仕高，劉曉麟，歐元賢.機(jī)械手笛卡爾空間軌跡規(guī)劃研究[J].機(jī)械設(shè)計(jì)與制造，2013(3):49-52.

[8] 李小霞，汪木蘭，劉坤,等.基于五次B樣條的機(jī)械手關(guān)節(jié)空間平滑軌跡規(guī)劃[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2012(8):39-42.

[9] 王智興，樊文欣，張保成.基于MATLAB的工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)分析與仿真[J].機(jī)電工程，2012,29(1):33-37.

[10] 計(jì)時(shí)鳴，黃希歡.工業(yè)機(jī)器人技術(shù)的發(fā)展與應(yīng)用綜述[J].機(jī)電工程，2015,32(1):1-13.

(編輯趙蓉)

The Kinematics Simulation and Trajectory Planning of Six-DOF Robot

YUE Ying ,YUE Yan-bo

(School of Energy,Power and Mechanical Engineering,North China Electric Power University,Baoding Hebei 071000 ,China )

In order to reduce the vibration and shock in the movement of robot , extend the service life , and improve the accuracy of trajectory planning, to solve the problem of trajectory inaccurate based on quintic polynomial caused by the curve of jerk occurs some breaking points,this thesis is concerning with trajectory planning study based on six-DOF industrial robot by means of quintic and septuple non-uniform B-spline.The modeling and simulation of the procedure for trajectory planning is realized by taking advantage of software MATLAB.The simulation results show that the proposed method could achieve smoother velocity and precision,the continuity of jerk is well,kinematics performance is better than quintic polynomial.

industrial robot; non-uniform B-spline; trajectory planning; MATLAB

1001-2265(2016)04-0089-04DOI:10.13462/j.cnki.mmtamt.2016.04.024

2015-06-05

河北省自然科學(xué)基金項(xiàng)目(E2014502042);中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金項(xiàng)目(11QJ61)

樂(lè)英(1971—)，女，河南信陽(yáng)人，華北電力大學(xué)(保定)副教授，博士，研究方向?yàn)闄C(jī)器人運(yùn)動(dòng)控制技術(shù)，(E-mail)yueying71@163.com。

TH166;TG659

A