Matlab坐标变换篇

一、安装Robotics System Toolbox

 

二、弄懂左乘和右乘

介绍的第一个函数为欧拉-齐次矩阵

%左手坐标系下，中指对着自己为x轴，z轴为拇指
T0=eul2tr(0,0,0) %基坐标系
Tx=transl(5,0,0) %xyz
Ty=transl(0,5,0) %xyz
Tz=transl(0,0,5) %xyz
trplot(T0) %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
pause(2);
hold on
trplot(Tx,'color','r')
trplot(Ty,'color','g')
trplot(Tz,'color','b')
xlim([-10 10]);
ylim([-10 10]);
zlim([-10 10]);

具体效果如下： 

 介绍的第二个函数为旋转矩阵

T0=eul2tr(0,0,0) %基坐标系
figure
subplot(221)
trplot(T0) %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
Rx=rpy2tr(90,0,0)%90度
Ry=rpy2tr(0,90,00)%90度
Rz=rpy2tr(0,0,90)%90度
subplot(222)
trplot(Rx,'color','r')
title('Rx=rpy2tr(90,0,0)%90度')
subplot(223)
trplot(Ry,'color','g')
title('Ry=rpy2tr(0,90,00)%90度')
subplot(224)
trplot(Rz,'color','b')
title('Rz=rpy2tr(0,0,90)%90度')

具体效果如下：  

 下面分析左乘和右乘的区别：

T0=eul2tr(0,0,0) %基坐标系
Tx=transl(5,0,0) %xyz
Ty=transl(0,5,0) %xyz
Tz=transl(0,0,5) %xyz

Rx=rpy2tr(90,0,0)%90度
Ry=rpy2tr(0,90,00)%90度
Rz=rpy2tr(0,0,90)%90度

subplot(221)
trplot(T0,'color','g') %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
title('基坐标系')
xlim([-10 10]);
ylim([-10 10]);
zlim([-10 10]);
grid on
subplot(222)
hold on
trplot(T0,'color','g')
trplot(Rz*Tx,'color','r')
title('Rz*Tx')
xlim([-10 10]);
ylim([-10 10]);
zlim([-10 10]);
grid on
hold off

subplot(223)
hold on
trplot(T0,'color','g')
trplot(Tx*Rx,'color','r')
title('Tx*Rx')
xlim([-10 10]);
ylim([-10 10]);
zlim([-10 10]);
grid on
hold off


subplot(224)
hold on
trplot(T0,'color','g')
trplot(Rz*Rx,'color','r')
title('Rz*Rx')
xlim([-2 2]);
ylim([-2 2]);
zlim([-2 2]);
grid on
hold off

         图2是先旋转，后平移，图3是先平移，后旋转，图4是先绕Z，后绕X，都是基于自身坐标系做的变换。

        下面会得出一个结论：依次左乘是相对于基坐标系的变换，右乘是相互对于自身坐标系的变换。因此，我们想要得到一个，先绕基坐标系Z旋转30°，再延基坐标系X轴平移50，再沿着自身X平移50，就可以表示为：

T0=eul2tr(0,0,0) %基坐标系
trplot(T0,'color','g') %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
hold on
trplot(transl(5,0,0)*rpy2tr(0,0,30),'color','r') %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
trplot(transl(5,0,0)*rpy2tr(0,0,30)*transl(3,0,0),'color','r') %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
xlim([-10 10]);
ylim([-10 10]);
zlim([-10 10]);

title("transl(5,0,0)*rpy2tr(0,0,30)*transl(3,0,0)");

三、坐标变换方式

首先介绍X-Y-Z固定角坐标系，为什么叫固定角坐标系？“固定”指的是旋转是绕着固定的参考系的各个主轴，比如：

      我们对{B}进行旋转，每次都是绕着固定坐标系{A}的轴进行旋转的，我们常将绕着X、Y、Z的旋转称为pitch、yaw、roll，也即俯仰、偏航、翻滚。将三次旋转利用旋转矩阵写出并相乘：

      乘积为：

      接着我们介绍Z-Y-X欧拉角坐标系，在这种表示法中，我们每次旋转都是绕着{B}的主轴进行旋转，也就是说，并不是绕着固定坐标系{A}的轴进行旋转了：

      在这种表示下，有：

      关于这个式子应该怎么理解呢？我们最终的目标是求出{B}相对于{A}的旋转，对于固定角坐标系，每次旋转都是基于{A}的，因而是按照旋转次序，依次左乘；对于欧拉角则是利用中间坐标系变换，所以依次右乘。比如上面的Z-Y-X欧拉角，记{A}绕{A}的Z旋转得到{B'}，然后{B'}绕{B'}的Y旋转得到{B''}，最后{B''}绕{B''}的X旋转得到{B}，所以，我们可以写出：

      式中的3个旋转角分别是绕着{A}的Z轴，{B'}的Y轴，{B‘’}的X轴旋转（所以相对于我们的旋转，是右乘，先乘以绕X的旋转，再乘以绕Y的旋转，最后才是绕Z的旋转），因此我们可以得出结论：X-Y-Z固定角与Z-Y-X欧拉角在同样的角度大小下，旋转所得到的最终结果是一样的，也就是说在这两种表示下，{B}相对于{A}的姿态一致。注意，这并不是巧合，是因为固定角表示下是基本旋转矩阵左乘，而欧拉角表示下是基本旋转矩阵的右乘，而恰好X-Y-Z与Z-Y-X是相反的旋转顺序，所以最终的效果就一样了。

      然后介绍一下等效轴角坐标表示法，也即用一个单位矢量加上一个旋转角表示旋转：

      因为我们的单位矢量长度恒为1，所以实际上确定它只需要两个参数，加上旋转角，也即三个参数，正好确定旋转所需要的3个自由度。当旋转轴K为一般轴时，等效旋转矩阵为：

      除了上面的固定角坐标系表示法、欧拉角表示法和等效轴角坐标系表示法之外，我们再介绍另一种姿态表示法，这种表示法通过四个数值来表示，称为欧拉参数。

      由等效旋转轴和等效旋转角定义的欧拉参数为：

      我们将这四个变量平方相加，得到：

      也即，这四个参数不是独立的，所以，仍然符合我们对于姿态的3自由度的认知。同时，由上面的式子可知，我们可以将一个姿态看作是四维空间中单位超球面上的一点，或者说，它是一个单位四元数。用这组参数表示的旋转矩阵为：

      已知旋转矩阵求欧拉参数为：