摘要
本文主要是給大家一個(gè)系統(tǒng)的概念，如何用Matlab實(shí)現(xiàn)六軸機(jī)器人的建模和實(shí)現(xiàn)軌跡規(guī)劃。以后將會(huì)給大家講解如何手寫(xiě)正逆解以及軌跡插補(bǔ)的程序。程序是基于Matlab2016a，工具箱版本為Robotic Toolbox 9.10。
1.D-H建模
三個(gè)兩兩相互垂直的XYZ軸構(gòu)成歐幾里得空間，存在六個(gè)自由度：沿XYZ平移的三個(gè)自由度，繞XYZ旋轉(zhuǎn)的三個(gè)自由度。在歐幾里得空間中任意線性變換都可以通過(guò)這六個(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ì)比來(lái)看參數(shù)并沒(méi)有改變，標(biāo)準(zhǔn)的 D-H 模型是將連桿的坐標(biāo)系固定在該連桿的輸出端（下一關(guān)節(jié)），也即坐標(biāo)系i-1與關(guān)節(jié)i對(duì)齊；改進(jìn)的 D-H模型 則是將坐標(biāo)系固定在該連桿的輸入端（上一關(guān)節(jié)），也即坐標(biāo)系i-1與關(guān)節(jié)對(duì)齊i-1。（具體建?？梢?jiàn)后面的博客）
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
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'); %連接連桿，機(jī)器人取名manman
robot.plot([0,pi/2,0,0,pi,0]);%輸出機(jī)器人模型，后面的六個(gè)角為輸出時(shí)的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)整各個(gè)關(guān)節(jié)角度，能夠讓初學(xué)者更好的了解六軸機(jī)器人的結(jié)構(gòu)。

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
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'); %連接連桿，機(jī)器人取名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，對(duì)比theta和q，發(fā)現(xiàn)是一致的（實(shí)際情況中并不是完全一致，會(huì)有一點(diǎn)偏差，我這里選的點(diǎn)特殊了）。


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

clear;
clc;
%建立機(jī)器人模型
%       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'); %連接連桿，機(jī)器人取名manman
T1=transl(0.5,0,0);%根據(jù)給定起始點(diǎn)，得到起始點(diǎn)位姿
T2=transl(0,0.5,0);%根據(jù)給定終止點(diǎn)，得到終止點(diǎn)位姿
q1=robot.ikine(T1);%根據(jù)起始點(diǎn)位姿，得到起始點(diǎn)關(guān)節(jié)角
q2=robot.ikine(T2);%根據(jù)終止點(diǎn)位姿，得到終止點(diǎn)關(guān)節(jié)角
[q ,qd, qdd]=jtraj(q1,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,:)),squeeze(T(3,4,:)));%輸出末端軌跡
hold on
robot.plot(q);%動(dòng)畫(huà)演示

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