如何使用matlab进行二维和三维空间的位姿描述及坐标系的变换
本篇文章主要与大家分享一下如何使用matlab进行二维和三维空间的位姿描述及坐标系的变换，文章内容处于更新和补充中，（我同时安装了机器人工具箱9.10版本和10.4版本，因此文章也介绍了两种版本工具箱下的用法）
一、
1、二维空间的位姿描述
（1）在二维空间下对进行坐标系的平移和旋转可以使用se2函数（在9.10版本工具箱中用se2函数，10.4版本为SE2函数），我们在matlab的命令行窗口输入help se2可以查看matlab的帮助文档中对se2函数使用的解释，其参数X，Y分别是在X、Y方向平移的距离，参数THETA是旋转的角度，具体的例子如下：
①在X方向平移1，在Y方向平移2 ，旋转90度
%9.10版本机器人工具箱
T=se2(1,2,90,'deg') %参数THETA为角度制
T=se2(1,2,pi/2) %参数THETA为弧度制
%10.4版本机器人工具箱
T=SE2(1,2,pi/2) %参数THETA为弧度制
T=SE2(1,2,90,'deg') %参数THETA为角度制
运行结果如下：
T =
0.0000 -1.0000 1.0000
1.0000 0.0000 2.0000
0 0 1.0000
（2）在二维空间下对进行坐标系的旋转可以使用rot2函数（10.4版本工具箱），其参数为旋转的角度，具体的例子如下：
R=rot2(pi)
运行结果如下：
R =
-1.0000 -0.0000
0.0000 -1.0000
（3）求二维平面内，平移、旋转的齐次变换矩阵，可以分别用transl2()函数和trot2（）函数，具体的例子如下：
%原点平移至（1,2）
transl2(1,2)
%逆时针旋转30度
trot2(30,'deg')
运行结果如下：
ans =
1 0 1
0 1 2
0 0 1
ans =
0.8660 -0.5000 0
0.5000 0.8660 0
0 0 1.0000
（4）我们可以使用trplot2函数（9.10和10.4版本均可使用）绘制出上面介绍的坐标变换图像，我们在matlab的命令行窗口输入help trplot2可以查看matlab的帮助文档中对trplot2函数使用的解释（本文后面介绍的函数也会适当的截取帮助文档内的内容，后面就不做此说明了 在命令行窗口输入help 后面跟想要查询的名字就可以了）通过帮助文档中列出的Options，可以知道我们可以对很多参数进行设定，具体的例子如下：
①例子1：最简单的例子，按照默认值画出上面介绍的T的图像
trplot2(T)
运行结果如下：
②例子2：画出上面介绍的T的图像，并将新的坐标系命名为A，颜色线条颜色设定为红色
trplot2(T, 'frame', 'A', 'color', 'r')
运行结果如下：
2、三维空间的位姿描述
（1）在三维空间下对进行坐标系的平移可以使用transl函数，其参数X，Y，Z分别是在X、Y、Z方向上平移的距离，具体的例子如下：
①在X方向平移1，在Y方向平移2 ，在Z方向上平移3
T = transl(1, 2, 3)
运行结果如下：
T =
1 0 0 1
0 1 0 2
0 0 1 3
0 0 0 1
（2）在三维空间下对进行坐标系的旋转可以使用rotx、roty、rotz函数，或者trotx、troty、trotz函数，这两类函数的不同之处是rotx、roty、rotz函数是3X3的矩阵，而trotx、troty、trotz函数是4X4的矩阵
①例子1：沿X轴旋转60度（π/3）
T1=rotx(60,'deg') %角度制 3x3
T2=rotx(pi/3) %弧度制 3x3
T3=trotx(pi/3) %弧度制 4x4
运行结果如下：
T1 =
1.0000 0 0
0 0.5000 -0.8660
0 0.8660 0.5000
T2 =
1.0000 0 0
0 0.5000 -0.8660
0 0.8660 0.5000
T3 =
1.0000 0 0 0
0 0.5000 -0.8660 0
0 0.8660 0.5000 0
0 0 0 1.0000
（3）我们可以利用trplot函数绘制出绘制出上面介绍的坐标变换图像，同样我们可以对Options中所列出的参数根据需要进行设定，一些具体的例子如下：
①例子1：绘制出绕X轴旋转60度的图像
T1=rotx(60,'deg') %也可以使用T1=trotx(60,'deg')
trplot(T1)
运行结果如下：
②例子2：绘制出绕X轴旋转60度的图像，并分别以红色、绿色、蓝色显示X、Y、Z轴
T1=rotx(60,'deg') %也可以使用T1=trotx(60,'deg')
trplot(T1,'rgb')
运行结果如下：
（4）我们可以利用tranimate函数查看动态的变换效果，一些具体的例子如下：
①例子1：绘制出绕X轴旋转60度的动态图像
T1=rotx(60,'deg') %也可以使用T1=trotx(60,'deg')
tranimate(T1)
运行结果如下：
②例子2：绘制出在X方向平移1，在Y方向平移2 ，在Z方向上平移3的动态图像
T4=transl(1,2,3)
tranimate(T4)
运行结果如下：
③例子3：绘制出绕X轴旋转60度的动态图像，并设定每秒要显示的帧数为5（默认为10）
T1=rotx(60,'deg') %也可以使用T1=trotx(60,'deg')
tranimate(T1,'fps','5')
④例子4：绘制出绕X轴旋转60度的动态图像，并将每一帧的图像以图片的形式保存在名为text_001的文件夹中
T1=rotx(60,'deg') %也可以使用T1=trotx(60,'deg')
tranimate(T1,'movie','text_001')
3、将二维空间的位姿矩阵转换成三维空间位姿矩阵
（1）我们可以利用se3函数将二维空间的位姿矩阵转换成三维空间位姿矩阵，具体的例子如下：
T1=se2(1,2,pi/3)
T2=se3(T1)
运行结果如下：
T1 =
0.5000 -0.8660 1.0000
0.8660 0.5000 2.0000
0 0 1.0000
T2 =
0.5000 -0.8660 0 1.0000
0.8660 0.5000 0 2.0000
0 0 1.0000 0
0 0 0 1.0000
（2）我们可以利用tranimate函数将其动态变换的过程显示出来
T1=se2(1,2,pi/3)
T2=se3(T1)
tranimate(T2)
4、坐标变换的综合实例
（1）绕动坐标系进行旋转，比如先绕X轴旋转π/2，再绕新的Y轴旋转π/2 。(此种旋转方式等效于先绕定坐标系Y轴旋转π/2，再绕定坐标系的X轴旋转π/2)
T1=trotx(pi/2);
T2=troty(pi/2);
T5=T1*T2;
tranimate(T5)
（2）先在在X方向平移1，在Y方向平移2，在Z方向上平移3，再绕动坐标系进行旋转，比如先绕X轴旋转π/2，再绕新的Y轴旋转π/3
T1=trotx(pi/2);
T2=troty(pi/3);
T3=transl(1,2,3);
T4=T3*T1*T2;
tranimate(T4)
5、各种姿态表示方法之间的转化
（1）从欧拉角直接转换为旋转矩阵 用eul2r()函数，如：
R=eul2r(0.1,0.2,0.3)
（2）从旋转矩阵求欧拉角，用tr2eul()函数，如：
gamma = tr2eul(R)
（3）从旋转矩阵求RPY角，用tr2rpy()函数，如：
R1 = rpy2r(0.1, 0.2, 0.3)
gamma = tr2rpy(R1)