（二）机器人工具箱三维空间描述

（二）机器人工具箱三维空间描述

 

 

1.旋转矩阵

 

Rx=rotx(pi/3); %绕x轴的旋转矩阵Rx(pi/3)，同理如下

Ry=roty(pi/4);

Rz=rotz(pi/2);

 

2.带变量的旋转矩阵

 

syms alpha beta gama

Rx1=rotx(alpha) %绕x轴的旋转矩阵，同理如下

Ry1=roty(beta)

Rz1=rotz(gama)

 

3.坐标系绘制

 

trplot(Rx,'frame','Rx_1','color','g');%Rx即要绘制的旋转矩阵，frame是当前坐标系的标识

 

显示效果

 

 3.坐标系绘制

 

trplot(Rx,'frame','Rx_1','color','g');%Rx即要绘制的旋转矩阵，frame是当前坐标系的标识

 

4.旋转动画显示

while(1)

tranimate(Rz*Rx*Ry);%显示从基坐标开始到Rz的变换动画

pause(5);%等待5s再循环动画

end

 

5.欧拉角（zyz时）

 

ou_zyz=eul2r(0.1,0.2,0.3);%zyz欧拉角，等价于下面的旋转变换的叠乘

R1=rotz(0.1)*roty(0.2)*rotz(0.3);

trplot(ou_zyz);%绘制欧拉角

tranimate(ou_zyz);%动画显示欧拉变换

rot_eul1=tr2eul(R1);%旋转矩阵转换为欧拉角

 

6.欧拉角（卡尔丹式 xyz，翻滚、俯仰、偏航）

 

ou_xyz=rpy2r(0.1,0.2,0.3);%xyz欧拉角，理论上等价于下面旋转变换的叠乘

R2=rotx(0.1)*roty(0.2)*rotz(0.3);

rot_eul2=tr2rpy(R2);%旋转矩阵转换为欧拉角

 

7.绕任意轴旋转

 

R3=eul2r(0.1,0.2,0.3);

[theta,v]=tr2angvec(R3);%theta 表示旋转角度的大小，v 表示转轴的方向向量

 

运行结果

theta =

 

   0.4466

 

 

v =

 

0.0450    0.4486   0.8926

 

上面的这些信息实际上是包含在 R3 的特征值和特征向量中，仔细对比lambdta对角线上的元素为R3的特征值，由于正交旋转矩阵必有有一个特征值为1其余的为lambdta值为 cos(theta)±isin(theta),而特征值为1所对应的列向量即为旋转轴的方向向量

 

[v,lambda]=eig(R3)

 

运行结果

v =

 

  0.7064 + 0.0000i   0.7064 +0.0000i   0.0450 + 0.0000i

 -0.0143 - 0.6318i  -0.0143 +0.6318i   0.4486 + 0.0000i

 -0.0284 + 0.3175i  -0.0284 -0.3175i   0.8926 + 0.0000i

 

 

lambda =

 

  0.9019 + 0.4319i   0.0000 +0.0000i   0.0000 + 0.0000i

  0.0000 + 0.0000i   0.9019 - 0.4319i   0.0000 + 0.0000i

  0.0000 + 0.0000i   0.0000 +0.0000i   1.0000 + 0.0000i

 

上面的红色字体为 cos(theta)= 0.9019 、sin(theta)= 0.4319 ，黄色背景表示特征值1对应的列向量为旋转轴

 

8.四元数

四元数的表示为 q=s+v=s+v1i+v2j+v3k,其中s为标量

Q=Quaternion(rpy2r(0.1,0.2,0.3));%运行结果中第一个是标量，其他的为i、j、k

 

运行结果

Q =

 

0.98186 < 0.064071, 0.091158, 0.15344>

 

若Q.norm=1 则表示的是单位四元数，即  s²+v1²+v2²+v3²=1

单位矢量非常特殊，他可以表示绕轴n转了theta角，相关关系为s=cos(theta/2) ,v=sin(theta/2)n

 

8.四元数

 

Q=Quaternion(rpy2r(0.1,0.2,0.3));%运行结果中第一个是标量，其他的为i、j、k

Q.inv();%表示四元数的逆或者说是共轭

Q*Q.inv();%乘积为标量

Q/Q;%等价于Q*Q.inv()

Q.R;%表示将四元数转换为旋转矩阵

Q.plot();%绘制四元数坐标系