Matlab机器人工具箱（0）——旋转与平移变换_matlab中机械臂先旋转再平移编程

前言

本文主要介绍如何使用matlab工具箱对旋转和平移进行操作。这里使用的RTB工具箱10.2版本，不同版本间代码风格可能会有差异，因此请注意工具箱的使用版本。

旋转变换

本部分的一些理论说明可参考旋转矩阵、欧拉角、四元数理论及其转换关系

旋转矩阵与可视化

close all;clc;clear;
%% 旋转
%% 以下为旋转矩阵表示法
% 绕x旋转90度
Rx = rotx(90);
% 绕y旋转90度
Ry = roty(90);
% 绕z旋转90度
Rz = rotz(90);

% 绘制旋转后的坐标系
trplot(Rx);
% 绘制世界坐标到Rx的动画过程
tranimate(Rx);

% 两个旋转的叠加, 即先绕世界坐标系的x轴转90度，
% 再沿当前坐标系的y轴转动90度
figure;
Rxy = Rx*Ry;
tranimate(Rxy);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

这部分比较直观，代码注释已经写得非常清楚了。
关于旋转矩阵，值得说明的一点是，有的人分不清什么时候左乘，什么时候右乘，其实很简单：以当前坐标系的轴旋转，就右乘；以世界坐标系的轴旋转，就左乘

欧拉角

%% zyz欧拉角————奇异点y=0,有多解
R_zyz = eul2r(15,30,60);  %这里输入是角度制
gamma = tr2eul(R_zyz); %这里输出是弧度制
% 等同于下式：
R_zyz = rotz(15)*roty(30)*rotz(60);

% 角度制与弧度制的相互转化
gamma2=rad2deg(gamma);
gamma3=deg2rad(gamma2);
1
2
3
4
5
6
7
8
9

这里有一个值得注意的问题：欧拉角转旋转矩阵的输入是角度制的，而旋转矩阵转欧拉角的输出却是弧度制 ，当然这跟工具箱的版本有关，用的时候要先确认一下输入输出的单位到底是弧度还是角度！
另外要指出的是，欧拉角的表示不唯一，并且当第二个旋转角（即y对应的旋转角度）为0时，欧拉角的表示发生奇异，这一点尤其要注意。

RPY角

%% rpy角——————attention    奇异点p=+-90,无多解
R_rpy = rpy2r(15,30,60);  %这里输入是角度制
rpy = tr2rpy(R_rpy); %这里输出是弧度制
rpy2=rad2deg(rpy);

% 等同于下式：
R_rpy2 = rotz(60)*roty(30)*rotx(15);
1
2
3
4
5
6
7

RPY角和zyz欧拉角同样有相似问题，但是解决了表示的不唯一问题，而且奇异点发生在p=±90°的位置。
另外一个值得注意的地方是，rpy2r的输入对应的顺序是pitch-yaw-roll，这与KDL等库中的顺序不同。

角轴

%% 角轴
[theta , v] = tr2angvec(rpy2r(15,30,60)); %theta为弧度制
R_angvec = angvec2r(theta, v );
1
2
3

单位四元数

%% 四元数
q = dcm2quat( rpy2r(15,30,60) ); %Aerospace Toolbox
R_quat = quat2dcm(q); %Aerospace Toolbox
% RTB 四元数类 
q = Quaternion( q ); %构造函数，把1x4的向量转成四元数类，方便运算

q.display;                   %显示四元数
q.double;                   %四元数转化为向量
q.matrix;                    %四元数转化为反对称矩阵
q.inv;                         %四元数的逆
q.norm;                     %四元数的模长
q.unit;                       %四元数单位化

% RTB 单位四元数类 
q = UnitQuaternion( R_quat ); 
R_UQuat = q.R;           %单位四元数转化为旋转矩阵
T_UQuat = q.T;           %单位四元数转化为齐次变换矩阵 
eular = q.toeul;            %单位四元数转化为欧拉角   
rpy =q.torpy;                %单位四元数转化为rpy
[ang,v] = q.toangvec;    %单位四元数转化为角轴
vec = q*[1 0 0 ]' ;          %用单位四元数直接对向量进行旋转操作
vec_2 = q* rotx(90);      %用单位四元数直接对旋转矩阵进行旋转操作
q.plot()                        %绘制四元数代表的坐标
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

值得注意的是，RTBv10.2版本的Quaternion和UnitQuaternion分离开了，这在示例代码中有所体现，和一些参考书中的介绍可能会有所不同。所以说，matlab机器人工具箱版本不一样，有些东西差距确实很大。
另外，在代码开头也展示了另一个工具箱（Aerospace Toolbox）四元数部分的介绍，用起来也很方便。

平移与旋转组合

%% 旋转与平移
% 纯平移
T_transl = transl(1,0,0);    %旋转不变，沿x轴平移1的齐次变换矩阵
% 纯旋转
T_rot = trotx(90);             %不移动，绕x轴转动90度
% 组合
T =  transl(1,0,0) * trotx(90);  % 先沿x方向移动1，再绕当前的x轴转动90度
%提取旋转部分
rot = t2r(T);
%提取平移部分
tans = transl(T);
% 绘图
figure;
trplot(T);
% 绘制动画
figure;
tranimate(T);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

总结

本文主要介绍了matlab机器人工具箱关于旋转与平函数的介绍，一些函数的扩展介绍仅包括高频使用的内容，如果需要用到一些没有列举到的方法，找matlab的help文档即可。下一篇博客介绍工具箱关于机器人建模和运动学的操作：Matlab机器人工具箱（1）——机器人的建立、绘制与正逆运动学

参考资料

机器人学、机器视觉与控制——MATLAB算法基础．电子工业出版社．2017-11-20