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