摘要 
本文主要是給大家一個系統的概念，如何用Matlab實現六軸機器人的建模和實現軌跡規劃。以後將會給大家講解如何手寫正逆解以及軌跡插補的程式。程式是基於Matlab2016a，工具箱版本為Robotic Toolbox 9.10，需要安裝包的可以在下方留言。 
1.D-H建模 
目前國內看到的機器人教材都是來自蔡自興老師的對的書，因此常用的建模方法都D-H建模。 
三個兩兩相互垂直的XYZ軸構成歐幾里得空間，存在六個自由度：沿XYZ平移的三個自由度，繞XYZ旋轉的三個自由度。在歐幾里得空間中任意線性變換都可以通過這六個自由度完成。 
Denavit-Hartenberg提出的D-H引數模型能滿足機器人學中的最小線性表示約定，用4個引數就能描述座標變換：繞X軸平移距離a；繞X軸旋轉角度alpha；繞Z軸平移距離d；繞Z軸旋轉角度theta。 
2.標準D-H模型和改進D-H模型

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
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12

本段程式碼取名為程式碼段1，效果圖如下： 
 
在程式碼段1的後面加入display函式可以輸出模型的一些引數

robot.display();

• 1

 
其中表格為D-H引數，grav為重力加速度向量，base為基座標系的齊次矩陣，tool為工具座標系和末端連桿的座標系之間的變換矩陣。 
在程式碼段1的後面加入teach指令，則可調整各個關節角度，能夠讓初學者更好的瞭解六軸機器人的結構。

teach(robot);

• 1

 
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

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14

我們可以檢視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);%動畫演示

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21

 
藍色細線就是規劃的軌跡，六軸機器人manman將會動態演示從起始點到終止點的過程。 
PS：本文的所有程式都是呼叫現成的函式，僅為大家建立一個概念，後面的部落格將給大家講解各個函式寫法。