一．旋轉矩陣：
（1）基本
R = rotx(pi/2)
R = roty(pi/2)
R = rotz(pi/2)
分别對X Y Z軸生成3*3的旋轉矩陣
R = rotx(30, 'deg')
R = roty(30, 'deg')
R = rotz(30, 'deg')
可以改變輸入的方式
（2）姿态的叙述方法：
1.Y-Z-Y歐拉角
R = rotz(a)*roty(b)*rotz(c)
eul2r(a,b,c)
旋轉矩陣反解出y-z-y歐拉角度的函數為
tr2eul(R)
2.x-y-z歐拉角
R = rotx(r)*roty(p)*rotz(y)
rpy2r（r,p,y）
旋轉矩陣反解出x-y-z歐拉角度的函數為
tr2rpy(R)
3.等效軸角坐标表示法
把坐标系b看做原坐标a按向量V方向按右手方向旋轉theta度
旋轉矩陣反解出等效軸角坐标表示的函數為
[theta,V] = tr2angvec(R)
由等效軸角坐标表示轉換為旋轉矩陣的函數
R = angvec2r(theta, V)（記得把v做機關化的處理v = v / norm(v)）
4.歐拉參數法表示（4元數）
在等效軸角坐标表示法的基礎上更進一步
E1=Kx*sin（θ/2）;E2=Ky*sin（θ/2）;E3=Kz*sin（θ/2）;E4=cos(θ/2);
求解4元數的函數為
Quaternion(R)
二．齊次變換矩陣
4*4矩陣用于描述坐标系的位置和姿态
1.平移算子
transl(x, y, z)
2.旋轉算子
trotx(pi/2)；troty(pi/2)；trotz(pi/2)
3.各種表示下齊次矩陣的求解
rpy2tr(roll, pitch, yaw, options) 求解x-y-z歐拉角變化對應的齊次矩陣 options：’deg’或不填
angvec2tr(theta, V)求解等效軸角坐标表示的齊次矩陣
eul2tr(phi, theta, psi, options）反解出Y-Z-Y歐拉角 options：’deg’或不填
同樣可以用
tr2eul(t)反解出Y-Z-Y歐拉角
tr2rpy(t)反解出x-y-z歐拉角
[theta,vec] = tr2angvec(R);反解等效軸角坐标表示
Quaternion(R) 反解4元數表示
4.求出姿态的相關表示
trprint(T, OPTIONS)
可以表示出齊次矩陣的參數
Options為：
（1）'rpy' ； 'euler' ；'angvec' ；
改變轉換的方式為rpy,歐拉角，等效軸角坐标
（2）'radian' ；
改變顯示的方式，和‘deg’功能相對。
5.旋轉矩陣與齊次矩陣
r2t（t），rt2tr（t）可以把旋轉矩陣t增廣為齊次矩陣
tr2rt（t），t2r（t）可以把齊次矩陣t降維旋轉矩陣
6.
trplot（R）
trplot2（R）
可以在畫圖中顯示出齊次矩陣
7.
tranimate（(P1, P2, OPTIONS)
可以顯示從P1變到p2的動畫
P1，P2可以是旋轉矩陣，齊次矩陣和4元數這幾個形式任意一個
Options::
'fps', fps fps的值可以改變動畫的速度(預設 10)
'nsteps', n n是插入的中間點個數(預設 50)
'axis',A 坐标系限制A=[xmin, xmax, ymin, ymax, zmin, zmax]
'movie',M Save frames as files in the folder M
三.一維軌迹生成：tpoly和lspb
1.五次多項式軌迹規劃
典型用法
[p,pd,pdd] = tpoly(p0, p1, 50);
subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p');
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');
subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd');
也可以
[S,SD,SDD] = tpoly(S0, SF, N, SD0, SDF)
SD0為初始速度，SDF為最終速度
S0為起點位置，SF為終點位置，N為步數
2.抛物線軌迹規劃
典型用法
[p,pd,pdd] = lspb(p0, p1, 50);
subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p');
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');
subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd');
也可以
[S,SD,SDD] = LSPB(S0, SF, M, V)
V為限定的最大速度
四.機械臂的建構
1.Link
Link（）是建構連杆的基本函數
建立連杆的基本規則是
L = Link(DH, OPTIONS)
DH = [THETA D A ALPHA SIGMA OFFSET]
THETA D A ALPHA 為基本的DH參數
theta 關節角度
d 連杆偏移量
a 連杆長度
alpha 連杆扭角
Sigma可以設定連杆的類型 0為旋轉 1為伸長
offset 關節變量偏移量
qlim 關節變量範圍[min max]
Options：
'standard' for standard D&H parameters (default).
'modified' for modified D&H parameters.
'revolute' 旋轉關節(default)
'prismatic' 伸長關節
建立一個旋轉連杆
L = Link([0 1.2 0.3 pi/2]);或 L = Link([0 1.2 0.3 pi/2 0])
L = Link([0 1.2 0.3 pi/2],’revolute’)
L = Link( 'd', 1.2, 'a', 0.3, 'alpha', pi/2);
L = Link('revolute', 'd', 1.2, 'a', 0.3, 'alpha', pi/2);
L = Revolute('d', 1.2, 'a', 0.3, 'alpha', pi/2);
建立一個移動連杆
L = Link([0 1.2 0.3 pi/2 1])
L = Link([0 1.2 0.3 pi/2],’prismatic’)
L = Link( 'theta', 0, 'a', 0.3, 'alpha', pi/2);
L = Link('prismatic', ' theta', 0, 'a', 0.3, 'alpha', pi/2);
L = prismatic(' theta', 0, 'a', 0.3, 'alpha', pi/2);
通過操作
L.theta；L.d；L.a；L. alpha 。。。。
可以顯示，修改連杆的參數
操作函數：
%A 連杆變換矩陣
% RP 關節類型: 'R' 或 'P'
% friction 摩擦力
% nofriction 摩擦力忽略
% dyn 顯示動力學參數
% islimit 測試關節是否超出軟限制
% isrevolute 測試是否為旋轉關節
% isprismatic 測試是否為移動關節
% display 連杆參數以表格形式顯示
% char 轉為字元串
L.A（X）可以顯示連杆變換矩陣，但是需要補齊DH參數變量，X在旋轉連杆中就是theta 值
在伸長連杆中就是d的值。
L.RP 傳回關節類型 傳回R表示旋轉關節，P為伸長關節
其他的用法類似。。。
2.SerialLink
使用SerialLink可以把連杆聯系起來形成機械臂
（1）基本用法
R = SerialLink(R1, options) 複制機器人R1
R = SerialLink([L1 L2 ...], OPTIONS) 機器人連接配接, 将L2的基座連接配接到L1的末端.
R = SerialLink(DH, OPTIONS),矩陣DH的構成：每個關節一行，每一行為[theta d a alpha]（預設為旋轉關節），第五列（sigma）為可選列，sigma=0（預設）為旋轉關節，sigma=1為移動關節
OPTIONS可以是：
'name'、'comment'、'manufacturer'
'base'、'tool'、'gravity'、'plotopt'
'name'可以用于給機械臂命名
bot = SerialLink([L1 L2], 'name', 'my robot')
'name'、'comment'、'manufacturer'的用法都差不多。用于标定一些額外資訊
'base', 'tool',用于設定基坐标和工具坐标的原點
>>mdl_puma560
>>p560_2 = SerialLink(p560, 'base', transl(-0.5, 0.5, 0) )
（2）R.teach()可以進入一個自由調節的界面
>> p560.teach()
在界面中可以任意調節個關節變量
調節好需要的機械臂姿态後按×
然後p560.getpos()可以進一步得到剛調出來的各變量的值
（3）R.plot(Q, options)可以顯示出機械臂
當Q為1*N維向量，向量元素為各關節變量，顯示的是靜态的機械臂
Q為M*N矩陣，列向量元素為各關節變量，顯示一個機械臂的動圖
Options::
'workspace', W 為空間限制W = [xmn, xmx ymn ymx zmn zmx]
'floorlevel',L 底闆在Z軸的顯示位置
Eg
>>mdl_puma560
>> p560.plot(q, 'floorlevel',0);
五．運動學常用函數
正運動學求解
Q = R.fkine(T)
逆運動學求解
T = R.ikine(Q, OPTIONS)
Options:: 'deg'
Eg：
clear;
clc;
clear L
% th d a alpha
L(1) = Link([ 0 0 0 -pi/2 0]);%定義連杆
L(2) = Link([ 0 1 0 pi/2 0]);
L(3) = Link([ 0 0 0 0 1]);
L(4) = Link([ 0 0 0 -pi/2 0]);
L(5) = Link([ 0 0 0 pi/2 0]);
L(6) = Link([ 0 1 0 0 0]);
bot = SerialLink(L, 'name', 'Stanford arm');%連接配接連杆
T=transl(1,2,3)*trotz(60,'deg')*troty(30,'deg')*trotz(90,'deg')
inverse_kinematics=bot.ikine(T,'pinv');%逆向運動學
theta1=inverse_kinematics(1);
theta2=inverse_kinematics(2);
d3=inverse_kinematics(3);
theta4=inverse_kinematics(4);
theta5=inverse_kinematics(5);
theta6=inverse_kinematics(6);
forward_kinematics=bot.fkine([theta1 theta2 d3 theta4 theta5 theta6])%前向運動學，驗證結果的準确性.