六軸機(jī)器人建模方法、正逆解、軌跡規(guī)劃與實(shí)現(xiàn)

張興華　肖仕馳

摘要：本文用Matlab實(shí)現(xiàn)六軸機(jī)器人的建模和實(shí)現(xiàn)軌跡規(guī)劃。講解如何寫正逆解以及軌跡插補(bǔ)的程序。程序是基于Matlab2 01 6a，工具箱版本為Robotic Toolbox9.10。

關(guān)鍵詞：六軸機(jī)器人建模Matlab

1 D-H建模

三個(gè)兩兩相互垂直的XYZ軸構(gòu)成歐幾里得空間，存在六個(gè)自由度：沿XYZ平移的三個(gè)自由度，繞XYZ旋轉(zhuǎn)的三個(gè)自由度。在歐幾里得空間中任意線性變換都可以通過這六個(gè)自由度完成。

Denavit-Hartenberg提出的D-H參數(shù)模型能滿足機(jī)器人學(xué)中的最小線性表示約定，用4個(gè)參數(shù)就能描述坐標(biāo)變換：繞X軸平移距離a;繞X軸旋轉(zhuǎn)角度alpha;繞Z軸平移距離d;繞Z軸旋轉(zhuǎn)角度theta。

2標(biāo)準(zhǔn)D-H模型和改進(jìn)D-H模型

對(duì)比來看參數(shù)并沒有改變，標(biāo)準(zhǔn)的D-H模型是將連桿的坐標(biāo)系固定在該連桿的輸出端（下一關(guān)節(jié)），也即坐標(biāo)系i 1與關(guān)節(jié)l對(duì)齊;改進(jìn)的D-H模型則是將坐標(biāo)系固定在該連桿的輸入端（上一關(guān)節(jié)），也即坐標(biāo)系i-l與關(guān)節(jié)對(duì)齊i-l。

3利用Matlab Robotic Toolbox建立機(jī)器人模型

alpha：連桿扭角;a：連桿長(zhǎng)度;theta：關(guān)節(jié)轉(zhuǎn)角;d：關(guān)節(jié)距離offset：偏移

clear;clc;%建立機(jī)器人模型

%theta d

a

alpha

offset

Ll=Link（[0

0.

25 p1/2

0

】）;%定義連桿的D-H參數(shù)

L2=Link（[p1/2

0

0.56

0

0

】）;

L3=Link（[0

0

0.035

p1/2

0

]）;

L4=Link（[0

0.515

0 p1/2

0

]）;

L5=Link（[pi

0

0

p1/2

0

]）;

L6=Link（[0

0.08

0

0

0

]）;

robot=SeriaILink（[Ll L2 L3 L4 L5 L61，1namel，manman）;%連接連桿，機(jī)器人取名manman

robot.plot（[0，p1/2，0，O，pi.O]）;%輸出機(jī)器人模型，后面的六個(gè)角為輸出時(shí)的theta姿態(tài)

robot.display（）;

其中表格為D-H參數(shù)，grav為重力加速度矢量base為基坐標(biāo)系的齊次矩陣，tool為工具坐標(biāo)系和末端連桿的坐標(biāo)系之間的變換矩陣。在代碼段l的后面加入teach指令，則可調(diào)整各個(gè)關(guān)節(jié)角度。

teach（robot）;

4運(yùn)動(dòng)學(xué)正逆解

運(yùn)動(dòng)學(xué)正解：根據(jù)6個(gè)關(guān)節(jié)角結(jié)算出末端位姿。

運(yùn)動(dòng)學(xué)逆解：根據(jù)末端位姿結(jié)算出關(guān)節(jié)角，這里會(huì)存在8組逆解，本文中用的反解函數(shù)會(huì)智能輸出最優(yōu)的一組解。

正解程序：

clear;clc;%建立機(jī)器人模型

%theta d

a

alpha

offset

Ll=Link（[0

0.

25 p1/2

0

]）;%定義連桿的D-H參數(shù)

L2=Link（[p1/2

0

0.56

0

0

】）;

L3=Link（[0

0

0.035 p1/2

0

]）;

L4=Link（[0

0.515

0

p1/2

0

】）;

L5=Link（[pi

O

pl/2

0

]）;

L6=Link（[0

0.08

0

0

]）;

robot=SerialLink（[Ll L2 L3 L4 L5 L6]，namel，manman）;%連接連桿，機(jī)器人取名manman

theta=LO，O，O，0，O，0];%指定的關(guān)節(jié)角

p=robot.fkine（theta）%fkine正解函數(shù)，根據(jù)給定的關(guān)節(jié)角theta，求解出末端位姿p

q=roloot.ikine（p）%ikine逆解函數(shù)，根據(jù)給定的末端位姿p，求解出關(guān)節(jié)角q

可以查看p和q，對(duì)比theta和q，發(fā)現(xiàn)是一致的

5軌跡規(guī)劃

在實(shí)際應(yīng)用中，我們一般都是知道末端的軌跡，然后使機(jī)器人動(dòng)作。本文的例子是根據(jù)給定兩個(gè)點(diǎn)的值，得到末端位姿，根據(jù)末端位姿再來規(guī)劃軌跡。

clear;clc;%建立機(jī)器人模型

%theta

d

a

alpha

offset

Ll=Link（[0

0.4

0.025 p1/2

0

]）;%定義連桿的D-H參數(shù)

L2=Link（[p1/2

0

0.56

0

0

]）;

L3=Link（[0

0

0.035 p1/2

0

]）;

L4=Link（[0

0.515

0

p1/2

0

]）;

L5=Link（[pi

0

0

p1/2

0

]）;

L6=Link（[0

0.08

0

0

]）;

robot=SeriaILink（[Ll L2 L3 L4 L5 L61，1namel，manman）;%連接連桿，機(jī)器人取名manman

Tl=transl（0.5，O，0）;%根據(jù)給定起始點(diǎn)，得到起始點(diǎn)位姿

T2=transl（0，0.5，0）;%根據(jù)給定終止點(diǎn)，得到終止點(diǎn)位姿

ql=roloot.ikine（Tl）;%根據(jù)起始點(diǎn)位姿，得到起始點(diǎn)關(guān)節(jié)角

q2=roloot.ikine（T2）;%根據(jù)終止點(diǎn)位姿，得到終止點(diǎn)關(guān)節(jié)角

【q，qd，qddl=jtraj（ql，q2，50）;%五次多項(xiàng)式軌跡，得到關(guān)節(jié)角度，角速度，角加速度，50為采樣點(diǎn)個(gè)數(shù)

grid on

T=robot.fkine（q）;%根據(jù)插值，得到末端執(zhí)行器位姿

plot3（squeeze（T（1，4，：》，squeeze（T（2，4，i》，squeeze（T（3.4，：）））;%輸出末端軌跡

hold on

robot.plot（q）;%動(dòng)畫演示

參考文獻(xiàn)

[1]王永志.基于LinuxCNC的六軸機(jī)器人軟件控制系統(tǒng)及運(yùn)動(dòng)算法研究[D].華南理工大學(xué)，2018.

[2]郭小羽，六軸機(jī)器人控制器底層軟件設(shè)計(jì)及其算法仿真研究[D].深圳大學(xué)，2017.