1、機械手臂建模
要建立PUMA560的機器人對象，首先我們要了解PUMA560的D-H參數，之後我們可以利用Robotics Toolbox工具箱中的link和robot函數來建立PUMA560的機器人對象。
其中link函數的調用格式：
L = LINK([alpha A theta D])
L =LINK([alpha A theta D sigma])
L =LINK([alpha A theta D sigma offset])
L =LINK([alpha A theta D], CONVENTION)
L =LINK([alpha A theta D sigma], CONVENTION)
L =LINK([alpha A theta D sigma offset], CONVENTION)
參數CONVENTION可以取‘standard’和‘modified’，其中‘standard’代表採用標準的D-H參數，‘modified’代表採用改進的D-H參數。參數‘alpha’代表扭轉角 ，參數‘A’代表杆件長度，參數‘theta’代表關節角，參數‘D’代表橫距，參數‘sigma’代表關節類型：0代表旋轉關節，非0代表移動關節。另外LINK還有一些數據域：
LINK.alpha %返回扭轉角
LINK.A %返回杆件長度
LINK.theta %返回關節角
LINK.D %返回橫距
LINK.sigma %返回關節類型
LINK.RP %返回‘R’(旋轉)或‘P’(移動)
LINK.mdh %若爲標準D-H參數返回0，否則返回1
LINK.offset %返回關節變量偏移
LINK.qlim %返回關節變量的上下限 [min max]
LINK.islimit(q) %如果關節變量超限，返回 -1, 0, +1
LINK.I %返回一個3×3對稱慣性矩陣
LINK.m %返回關節質量
LINK.r %返回3×1的關節齒輪向量
LINK.G %返回齒輪的傳動比
LINK.Jm %返回電機慣性
LINK.B %返回粘性摩擦
LINK.Tc %返回庫侖摩擦
LINK.dh return legacy DH row
LINK.dyn return legacy DYN row
其中robot函數的調用格式：
ROBOT %創建一個空的機器人對象
ROBOT(robot) %創建robot的一個副本
ROBOT(robot, LINK) %用LINK來創建新機器人對象來代替robot
ROBOT(LINK, ...) %用LINK來創建一個機器人對象
ROBOT(DH, ...) %用D-H矩陣來創建一個機器人對象
ROBOT(DYN, ...) %用DYN矩陣來創建一個機器人對象
2、矩陣變換
利用MATLAB中Robotics Toolbox工具箱中的transl、rotx、roty和rotz可以實現用齊次變換矩陣表示平移變換和旋轉變換。下面舉例來說明：
A機器人在x軸方向平移了0.5米，那麼我們可以用下面的方法來求取平移變換後的齊次矩陣：
>> transl(0.5,0,0)
ans =
1.0000 0 0 0.5000
0 1.0000 0 0
0 0 1.0000 0
0 0 0 1.0000
B機器人繞x軸旋轉45度，那麼可以用rotx來求取旋轉後的齊次矩陣：
>> rotx(pi/4)
ans =
1.0000 0 0 0
0 0.7071 -0.7071 0
0 0.7071 0.7071 0
0 0 0 1.0000
C機器人繞y軸旋轉90度，那麼可以用roty來求取旋轉後的齊次矩陣：
>> roty(pi/2)
ans =
0.0000 0 1.0000 0
0 1.0000 0 0
-1.0000 0 0.0000 0
0 0 0 1.0000
D機器人繞z軸旋轉-90度，那麼可以用rotz來求取旋轉後的齊次矩陣：
>> rotz(-pi/2)
ans =
0.0000 1.0000 0 0
-1.0000 0.0000 0 0
0 0 1.0000 0
0 0 0 1.0000
當然，如果有多次旋轉和平移變換，我們只需要多次調用函數在組合就可以了。另外，可以和我們學習的平移矩陣和旋轉矩陣做個對比，相信是一致的。
3軌跡規劃
利用Robotics Toolbox提供的ctraj、jtraj和trinterp函數可以實現笛卡爾規劃、關節空間規劃和變換插值。
其中ctraj函數的調用格式：
TC = CTRAJ(T0, T1, N)
TC = CTRAJ(T0, T1, R)
參數TC爲從T0到T1的笛卡爾規劃軌跡，N爲點的數量，R爲給定路徑距離向量，R的每個值必須在0到1之間。
其中jtraj函數的調用格式：
[Q QD QDD] = JTRAJ(Q0, Q1, N)
[Q QD QDD] = JTRAJ(Q0, Q1, N, QD0, QD1)
[Q QD QDD] = JTRAJ(Q0, Q1, T)
[Q QD QDD] = JTRAJ(Q0, Q1, T, QD0, QD1)
參數Q爲從狀態Q0到Q1的關節空間規劃軌跡，N爲規劃的點數，T爲給定的時間向量的長度，速度非零邊界可以用QD0和QD1來指定。QD和QDD爲返回的規劃軌跡的速度和加速度。
其中trinterp函數的調用格式：
TR = TRINTERP(T0, T1, R)
參數TR爲在T0和T1之間的座標變化插值，R需在0和1之間。
要實現軌跡規劃，首先我們要創建一個時間向量，假設在兩秒內完成某個動作，採樣間隔是56ms，那麼可以用如下的命令來實現多項式軌跡規劃：t=0:0.056:2; [q,qd,qdd]=jtraj(qz,qr,t);
其中t爲時間向量，qz爲機器人的初始位姿，qr爲機器人的最終位姿，q爲經過的路徑點，qd爲運動的速度，qdd爲運動的加速度。其中q、qd、qdd都是六列的矩陣，每列代表每個關節的位置、速度和加速度。如q(:,3)代表關節3的位置，qd(:,3)代表關節3的速度，qdd(:,3)代表關節3的加速度。
4、運動學的正問題
利用Robotics Toolbox中的fkine函數可以實現機器人運動學正問題的求解。
其中fkine函數的調用格式：
TR = FKINE(ROBOT, Q)
參數ROBOT爲一個機器人對象，TR爲由Q定義的每個前向運動學的正解。
以PUMA560爲例，定義關節座標系的零點qz=[0 0 0 0 0 0]，那麼fkine(p560,qz)將返回最後一個關節的平移的齊次變換矩陣。如果有了關節的軌跡規劃之後，我們也可以用fkine來進行運動學的正解。比如：
t=0:0.056:2; q=jtraj(qz,qr,t); T=fkine(p560,q);
返回的矩陣T是一個三維的矩陣，前兩維是4×4的矩陣代表座標變化，第三維是時間。
5運動學的逆問題
利用Robotics Toolbox中的ikine函數可以實現機器人運動學逆問題的求解。
其中ikine函數的調用格式：
Q = IKINE(ROBOT, T)
Q = IKINE(ROBOT, T, Q)
Q = IKINE(ROBOT, T, Q, M)
參數ROBOT爲一個機器人對象，Q爲初始猜測點（默認爲0），T爲要反解的變換矩陣。當反解的機器人對象的自由度少於6時，要用M進行忽略某個關節自由度。
有了關節的軌跡規劃之後，我們也可以用ikine函數來進行運動學逆問題的求解。比如：
t=0:0.056:2; T1=transl(0.6,-0.5,0); T2=transl(0.4,0.5,0.2); T=ctraj(T1,T2,length(t)); q=ikine(p560,T);
我們也可以嘗試先進行正解，再進行逆解，看看能否還原。
Q=[0 –pi/4 –pi/4 0 pi/8 0]; T=fkine(p560,q); qi=ikine(p560,T);
