摘要
本文主要是給大家一個系統的概念，如何用Matlab實現六軸機器人的建模和實現軌跡規劃。以後將會給大家講解如何手寫正逆解以及軌跡插補的程序。程序是基於Matlab2016a，工具箱版本爲Robotic Toolbox 9.10。
1.D-H建模
三個兩兩相互垂直的XYZ軸構成歐幾里得空間，存在六個自由度：沿XYZ平移的三個自由度，繞XYZ旋轉的三個自由度。在歐幾里得空間中任意線性變換都可以通過這六個自由度完成。
Denavit-Hartenberg提出的D-H參數模型能滿足機器人學中的最小線性表示約定，用4個參數就能描述座標變換：繞X軸平移距離a；繞X軸旋轉角度alpha；繞Z軸平移距離d；繞Z軸旋轉角度theta。
2.標準D-H模型和改進D-H模型
對比來看參數並沒有改變，標準的 D-H 模型是將連桿的座標系固定在該連桿的輸出端（下一關節），也即座標系i-1與關節i對齊；改進的 D-H模型 則是將座標系固定在該連桿的輸入端（上一關節），也即座標系i-1與關節對齊i-1。（具體建模可見後面的博客）
3.利用 Matlab Robotic Toolbox 建立機器人模型
alpha:連桿扭角；
a:連桿長度；
theta:關節轉角；
d:關節距離；
offset:偏移
clear;
clc;
%建立機器人模型
% theta d a alpha offset
L1=Link([0 0.4 0.025 pi/2 0 ]); %定義連桿的D-H參數
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姿態本段代碼取名爲代碼段1，效果圖如下：
在代碼段1的後面加入display函數可以輸出模型的一些參數
robot.display();
其中表格爲D-H參數，grav爲重力加速度矢量，base爲基座標系的齊次矩陣，tool爲工具座標系和末端連桿的座標系之間的變換矩陣。
在代碼段1的後面加入teach指令，則可調整各個關節角度，能夠讓初學者更好的瞭解六軸機器人的結構。
teach(robot);
4.運動學正逆解
運動學正解：根據6個關節角結算出末端位姿。
運動學逆解：根據末端位姿結算出關節角，這裏會存在8組逆解，本文中用的反解函數會智能輸出最優的一組解。
正解程序：
clear;
clc;
%建立機器人模型
% theta d a alpha offset
L1=Link([0 0.4 0.025 pi/2 0 ]); %定義連桿的D-H參數
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];%指定的關節角
p=robot.fkine(theta)%fkine正解函數，根據我們給定的關節角theta，求解出末端位姿p
q=robot.ikine(p)%ikine逆解函數，根據我們給定的末端位姿p，求解出關節角q我們可以查看p和q，對比theta和q，發現是一致的（實際情況中並不是完全一致，會有一點偏差，我這裏選的點特殊了）。
5.軌跡規劃
在實際應用中，我們一般都是知道末端的軌跡，然後使機器人動作。本文的例子是根據給定兩個點的值，得到末端位姿，根據末端位姿再來規劃軌跡。
clear;
clc;
%建立機器人模型
% theta d a alpha offset
L1=Link([0 0.4 0.025 pi/2 0 ]); %定義連桿的D-H參數
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);%根據給定起始點，得到起始點位姿
T2=transl(0,0.5,0);%根據給定終止點，得到終止點位姿
q1=robot.ikine(T1);%根據起始點位姿，得到起始點關節角
q2=robot.ikine(T2);%根據終止點位姿，得到終止點關節角
[q ,qd, qdd]=jtraj(q1,q2,50); %五次多項式軌跡，得到關節角度，角速度，角加速度，50爲採樣點個數
grid on
T=robot.fkine(q);%根據插值，得到末端執行器位姿
plot3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:)));%輸出末端軌跡
hold on
robot.plot(q);%動畫演示
藍色細線就是規劃的軌跡，六軸機器人manman將會動態演示從起始點到終止點的過程。
PS：本文的所有程序都是調用現成的函數，僅爲大家建立一個概念，後面的博客將給大家講解各個函數寫法。