六軸機器人建模方法、正逆解、軌跡規(guī)劃實例與Matalb Robotic Toolbox 的實現(xiàn)

rookie 2020-04-11

展開全文

摘要
本文主要是給大家一個系統(tǒng)的概念，如何用Matlab實現(xiàn)六軸機器人的建模和實現(xiàn)軌跡規(guī)劃。以后將會給大家講解如何手寫正逆解以及軌跡插補的程序。程序是基于Matlab2016a，工具箱版本為Robotic Toolbox 9.10。
1.D-H建模
三個兩兩相互垂直的XYZ軸構(gòu)成歐幾里得空間，存在六個自由度：沿XYZ平移的三個自由度，繞XYZ旋轉(zhuǎn)的三個自由度。在歐幾里得空間中任意線性變換都可以通過這六個自由度完成。
Denavit-Hartenberg提出的D-H參數(shù)模型能滿足機器人學(xué)中的最小線性表示約定，用4個參數(shù)就能描述坐標(biāo)變換：繞X軸平移距離a；繞X軸旋轉(zhuǎn)角度alpha；繞Z軸平移距離d；繞Z軸旋轉(zhuǎn)角度theta。
2.標(biāo)準(zhǔn)D-H模型和改進D-H模型
標(biāo)準(zhǔn)和改進對比圖
對比來看參數(shù)并沒有改變，標(biāo)準(zhǔn)的 D-H 模型是將連桿的坐標(biāo)系固定在該連桿的輸出端（下一關(guān)節(jié)），也即坐標(biāo)系i-1與關(guān)節(jié)i對齊；改進的 D-H模型則是將坐標(biāo)系固定在該連桿的輸入端（上一關(guān)節(jié)），也即坐標(biāo)系i-1與關(guān)節(jié)對齊i-1。（具體建?？梢姾竺娴牟┛停?
3.利用 Matlab Robotic Toolbox 建立機器人模型
alpha:連桿扭角；
a:連桿長度；
theta:關(guān)節(jié)轉(zhuǎn)角；
d:關(guān)節(jié)距離；
offset:偏移

clear;
clc;
%建立機器人模型
%       theta    d        a        alpha     offset
L1=Link([0       0.4      0.025    pi/2      0     ]); %定義連桿的D-H參數(shù)
L2=Link([pi/2    0        0.56     0         0     ]);
L3=Link([0       0        0.035    pi/2      0     ]);
L4=Link([0       0.515    0        pi/2      0     ]);
L5=Link([pi      0        0        pi/2      0     ]);
L6=Link([0       0.08     0        0         0     ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %連接連桿，機器人取名manman
robot.plot([0,pi/2,0,0,pi,0]);%輸出機器人模型，后面的六個角為輸出時的theta姿態(tài)

本段代碼取名為代碼段1，效果圖如下：

在代碼段1的后面加入display函數(shù)可以輸出模型的一些參數(shù)

robot.display();

其中表格為D-H參數(shù)，grav為重力加速度矢量，base為基坐標(biāo)系的齊次矩陣，tool為工具坐標(biāo)系和末端連桿的坐標(biāo)系之間的變換矩陣。
在代碼段1的后面加入teach指令，則可調(diào)整各個關(guān)節(jié)角度，能夠讓初學(xué)者更好的了解六軸機器人的結(jié)構(gòu)。

teach(robot);

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

clear;
clc;
%建立機器人模型
%       theta    d        a        alpha     offset
L1=Link([0       0.4      0.025    pi/2      0     ]); %定義連桿的D-H參數(shù)
L2=Link([pi/2    0        0.56     0         0     ]);
L3=Link([0       0        0.035    pi/2      0     ]);
L4=Link([0       0.515    0        pi/2      0     ]);
L5=Link([pi      0        0        pi/2      0     ]);
L6=Link([0       0.08     0        0         0     ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %連接連桿，機器人取名manman
theta=[0,0,0,0,0,0];%指定的關(guān)節(jié)角
p=robot.fkine(theta)%fkine正解函數(shù)，根據(jù)我們給定的關(guān)節(jié)角theta，求解出末端位姿p
q=robot.ikine(p)%ikine逆解函數(shù)，根據(jù)我們給定的末端位姿p，求解出關(guān)節(jié)角q

我們可以查看p和q，對比theta和q，發(fā)現(xiàn)是一致的（實際情況中并不是完全一致，會有一點偏差，我這里選的點特殊了）。

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

clear;
clc;
%建立機器人模型
%       theta    d        a        alpha     offset
L1=Link([0       0.4      0.025    pi/2      0     ]); %定義連桿的D-H參數(shù)
L2=Link([pi/2    0        0.56     0         0     ]);
L3=Link([0       0        0.035    pi/2      0     ]);
L4=Link([0       0.515    0        pi/2      0     ]);
L5=Link([pi      0        0        pi/2      0     ]);
L6=Link([0       0.08     0        0         0     ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %連接連桿，機器人取名manman
T1=transl(0.5,0,0);%根據(jù)給定起始點，得到起始點位姿
T2=transl(0,0.5,0);%根據(jù)給定終止點，得到終止點位姿
q1=robot.ikine(T1);%根據(jù)起始點位姿，得到起始點關(guān)節(jié)角
q2=robot.ikine(T2);%根據(jù)終止點位姿，得到終止點關(guān)節(jié)角
[q ,qd, qdd]=jtraj(q1,q2,50); %五次多項式軌跡，得到關(guān)節(jié)角度，角速度，角加速度，50為采樣點個數(shù)
grid on
T=robot.fkine(q);%根據(jù)插值，得到末端執(zhí)行器位姿
plot3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:)));%輸出末端軌跡
hold on
robot.plot(q);%動畫演示

這里寫圖片描述
藍(lán)色細(xì)線就是規(guī)劃的軌跡，六軸機器人manman將會動態(tài)演示從起始點到終止點的過程。
PS：本文的所有程序都是調(diào)用現(xiàn)成的函數(shù)，僅為大家建立一個概念，后面的博客將給大家講解各個函數(shù)寫法。