（学习笔记）matlab机器人工具箱攻略——矩阵变换，运动学

一．旋转矩阵：

（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])%前向运动学，验证结果的准确性.