Matlab坐标系变换---姿态的表示方法:旋转矩阵-欧拉角--四元数-齐次矩阵

1、前记：

老生常谈的坐标变换，主要将之前的系列稍稍梳理一下。因为在进行机器人逆解的时候，或者笛卡尔空间规划时需要知道机器人的末端位姿。借https://www.guyuehome.com/5478中的一个图，末端位姿由姿态矩阵和位置矢量组成。

1584359013(1)

而机器人的姿态可以由怎样表示？表示的方法怎么相互转换？3by3的旋转矩阵如何与3by1的位置矢量构成4by4齐次变换矩阵的呢？下面进行简单说明。

2、姿态的表示

1）、旋转矩阵

（1）旋转算子（以下分别是绕x轴，绕y轴，绕z轴）

而在matlab里按照Robotics Toolbox中分别为有rotx(),roty(),rotz()三个函数表示。

比如，现在只需要将世界坐标系沿其X轴旋转20度，代码和变化过程如下。

2）X-Y-Z固定角坐标系（按照参考坐标系的轴进行旋转变换）

那么以一个固定的坐标系（比如世界坐标系）作为参考系，分别依次绕其X轴旋转，Y旋转，Z旋转一定的角度后的旋转矩阵为;

R_xyz=rotz(Q3)*roty(Q2)*rotx(Q1) %固定角坐标系，旋转算子乘的顺序从右边开始

%这里是从右开始的。注意旋转的顺序和矩阵的乘的顺序在这里是相反的，乘的顺序不能整错。因为矩阵的乘法不满足交换律

即：导论书里面的下幅图

3）Z-Y-X欧拉角（按照旋转变换后坐标系的轴进行变换）

先将坐标系绕Z轴进行旋转，在绕新的坐标系的Y轴旋转，最后绕上一步新变换得到的坐标系X轴进行旋转，导论书上的图如下。

即经过Z-Y-X欧拉角变换后的坐标系，相对于没变换之前的姿态矩阵为;

R_ZYX=rotz(Q3)*roty(Q2)*rotx(Q1) %Z-Y-X欧拉角旋转算子乘的顺序从左边开始。这和DH变换的顺序一样。

现在你抛开上面两种变换的顺序，和按什么参考系进行变换，只看看两个变换的姿态矩阵乘的顺序和导论书两个图坐标系的最后变换结果。

得到的结果是一样的。

欧拉旋转定理要求绕3个轴依次旋转，但不能绕同一轴连续旋转两次，即不能出现ABB这种新式。一共有12种组合！

欧拉角Z-Y-X是按照卡尔丹式命名的，即绕三个不同轴旋转。有XYZ,XZY,YZX,YXZ,ZYX,ZXY

欧拉角式的命名方式式在旋转中绕一个特定的旋转轴不连续的重复旋转有:XYX,XZX,YXY,YZY,ZXZ,ZYZ

不同的旋转顺序最终得到的结果是不一样

任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。

如：一个旋转矩阵R=Rx(Q1)*Ry(Q2)*Rz(Q3)的旋转序列角度分别为绕X轴旋转Q1，再绕Y轴旋转Q2，最后绕Z轴旋转Q3得到的。这个旋转序列也称R-P-Y--->横滚-俯仰-偏航，是广泛使用的旋转角顺序。这种方式在航空即地面车辆而言，通常定义X轴为前进方向，Z轴垂直向下，Y轴指向右手方向。如下对应为：飞机前进方向为X轴，绕X旋转-横滚（Roll）；垂直向下为Z轴，绕Z旋转-偏航（Yaw）；右手方向为Y轴，绕Y旋转-俯仰（Pitch）

(1)欧拉角转旋转矩阵

到现在为止其实我们已经知道利用旋转算子将欧拉角转换乘旋转矩阵了。

Example：给定X-Y-Z的欧拉角为（20,30,40)。得到的旋转矩阵为:

（2）旋转矩阵转欧拉角z-y-x。

具体推导过程可以使用符号计算，然后解含变量少的方程组。

function t1=r2eul_z_y_x(R)

%R为3by3旋转矩阵

r11=R(1,1);r12=R(1,2);r13=R(1,3);
r21=R(2,1);r22=R(2,2);r23=R(2,3);
r31=R(3,1);r32=R(3,2);r33=R(3,3);

%解方程组后得以下
t1 = [-atan2(-r21, r11); -atan2(r31, sqrt(r32 ^ 2 + r33 ^ 2)); -atan2(-r32, r33);];

t1=t1*180/pi;
end

(3)四元数转旋转矩阵和齐次矩阵

function H=Q2T(q)
% Reshape the quaternions in the depth dimension
q = bsxfun(@times, q, 1./sqrt(sum(q.^2,2))).';
q2 = reshape(q,[4 1 size(q,2)]);
s = q2(1,1,:);
x = q2(2,1,:);
y = q2(3,1,:);
z = q2(4,1,:);
% Explicitly define concatenation dimension for codegen
tempR = cat(1, 1 - 2*(y.^2 + z.^2),   2*(x.*y - s.*z),   2*(x.*z + s.*y),...
2*(x.*y + s.*z), 1 - 2*(x.^2 + z.^2),   2*(y.*z - s.*x),...
2*(x.*z - s.*y),   2*(y.*z + s.*x), 1 - 2*(x.^2 + y.^2) );
R = reshape(tempR, [3, 3, length(s)]);
R = permute(R, [2 1 3]);
numMats = size(R,3);
% The rotational components of the homogeneous transformation matrix
% are located in elements H(1:3,1:3).
H = zeros(4,4,numMats,'like',R);
H(1:3,1:3,:) = R;
H(4,4,:) = ones(1,1,numMats,'like',R);
end

将齐次矩阵T变成3by3的旋转矩阵R，就是将齐次矩阵中的1到3行，1到3列提取出来就是。在matlab中使用：

R=T(1:3,1:3,:)

参考：https://www.codercto.com/a/68155.html

https://blog.csdn.net/lql0716/article/details/72597719