坐标变换基础-欧拉角&固定角与位姿矩阵的相互转换

概念定义

空间中一个坐标系相对于另一个坐标系的变换关系用新坐标系的三个坐标轴相对于原坐标系的方向矢量来确定，可用 矩阵来描述。用齐次矩阵（4×4）来统一描述刚体的位置和姿态：

其中，R便是描述姿态的旋转矩阵。
和沿着三个坐标轴的平移运动不一样，旋转矩阵显得很不直观，也繁琐。因此往往需要使用更简洁的方式来描述姿态变换。固定角与欧拉角便是最常规的两种。

欧拉角是用来唯一地确定定点转动刚体位置的三个一组独立角参量，由章动角θ、进动角ψ和自转角φ组成，为L.欧拉首先提出，故得名。

固定角与欧拉角的区别在于，在旋转变换的过程中，欧拉角指的是旋转是绕物体自身的坐标轴旋转。固定角指的是旋转绕世界坐标系的轴旋转。以下介绍两种常见的表达形式来具体分析

X-Y-Z固定角

如图所示，首先将目标坐标系{B}与参考坐标系{A}重合，将坐标系{B}先绕X_A轴旋转γ度，再将坐标系{B}绕Y_A轴旋转β度，最后将坐标系{B}绕Z_A轴旋转α度。最终得到新的坐标系{B}。

可以看到每次旋转都是绕着固定坐标系{A}的轴，所以才称为X-Y-Z固定角。

• 欧拉角转换成旋转矩阵
  基于坐标系绕轴向量的旋转公式，由旋转顺序可直接推导：
  
  得：

• 旋转矩阵转换成欧拉角
  旋转矩阵已知，求未知量为γ，β，α，由式
  

相当用9个方程对3个未知量求解。经分析，通过对r11和r21对应两个等式求平方和再开根，可以求得cosβ，再由r31可以求得sinβ，即可解出β。
在不考虑β=±90°的情况下，联立r11和r21对应两个等式可以求得α，联立r32和r33两个等式可求得γ，具体公式如下：

考虑β=±90的特殊情况，α任意解都满足，一般取0。
若β=90°，则：

若β=-90°，则：

matlab代码如下

function coord_result = Tran_ZYX(pmatrix)
% 矩阵pmatrix转换成Z-Y-X型的位姿值想x,y,z,w,p,r
p_base = atan2(-pmatrix(3,1),sqrt(pmatrix(1,1)^2 + pmatrix(2,1)^2));
if (abs(pmatrix - pi/2) <= 0.000001)
	w_base = 0;
	r_base = atan2(pmatrix(1,2),pmatrix(2,2));
elseif (abs(pmatrix + pi/2) <= 0.000001)
    w_base = 0;
	r_base = atan2(-pmatrix(1,2),pmatrix(2,2)); 
else
	w_base = atan2(pmatrix(2,1)/cos(p_base),pmatrix(1,1)/cos(p_base));
    r_base = atan2(pmatrix(3,2)/cos(p_base),pmatrix(3,3)/cos(p_base));
end
%欧拉角形式的位姿值
coord_result(1) = pmatrix(1,4);
coord_result(2) = pmatrix(2,4);
coord_result(3) = pmatrix(3,4);
coord_result(4) = w_base*180/pi;
coord_result(5) = p_base*180/pi;
coord_result(6) = r_base*180/pi;

Z-Y-X欧拉角

如图所示，首先将目标坐标系{B}与参考坐标系{A}重合，将坐标系{B}先绕Z_B轴旋转α度，再绕旋转后的坐标系{B}的Y_B轴旋转β度，最后再绕旋转后的坐标系{B}的X_B轴旋转γ度。最终得到新的坐标系{B}。

可以看到每次旋转都是绕着运动坐标系{B}的轴旋转，因此称为Z-Y-X欧拉角。

• 欧拉角转换成旋转矩阵
  基于坐标系绕轴向量的旋转公式，由旋转顺序可直接推导：
  
  得：
  

这里可以发现一个问题，Z-Y-X欧拉角和X-Y-Z固定角的旋转矩阵完全想同，这里印证了一个结论

三次绕固定轴旋转的最终姿态和以相反顺序三次绕运动坐标轴旋转最终姿态相同

• 旋转矩阵转换成欧拉角
  由于和和X-Y-Z固定角的旋转矩阵一样，所以旋转矩阵换算成Z-Y-X欧拉角的解法公式也是一摸一样的。
  在不考虑β=±90°的情况下，具体公式如下：
  
  若β=90°，则：
  
  若β=-90°，则：
  
  matlab代码参考的X-Y-Z固定角的代码

Z-Y-Z欧拉角

和Z-Y-X欧拉角一样，每次旋转都是绕着运动坐标系{B}的轴旋转，但是最后不是绕X轴旋转而是再次绕Z轴旋转，因此称为Z-Y-X欧拉角。

• 欧拉角转换成旋转矩阵
  基于坐标系绕轴向量的旋转公式，直接得：
  
  在不考虑sinβ=0的情况下，联立r23和r13对应两个等式可以求得α，联立r31和r32两个等式可求得γ，具体公式如下：
  
  考虑β=0°|180°的特殊情况，α任意解都满足，一般取0。
  若β=0°，则：
  

若β=180°，则：

matlab代码如下

function coord_result = Tran_ZYZ(pmatrix)
% 矩阵pmatrix转换成Z-Y-Z型的位姿值想x,y,z,w,p,r
p_base = atan2(sqrt(pmatrix(3,1)^2 + pmatrix(3,2)^2),pmatrix(3,3));
if (abs(pmatrix) <= 0.000001)% 0°
	w_base = 0;
	r_base = atan2(-pmatrix(1,2),pmatrix(1,1));
elseif (abs(abs(pmatrix) - pi) <= 0.000001)% 180°
    w_base = 0;
	r_base = atan2(-pmatrix(1,2),-pmatrix(1,1)); 
else
	w_base = atan2(pmatrix(2,3)/sin(p_base),pmatrix(1,3)/sin(p_base));
    r_base = atan2(pmatrix(3,2)/sin(p_base),-pmatrix(3,1)/sin(p_base));
end
%欧拉角形式的位姿值
coord_result(1) = pmatrix(1,4);
coord_result(2) = pmatrix(2,4);
coord_result(3) = pmatrix(3,4);
coord_result(4) = w_base*180/pi;
coord_result(5) = p_base*180/pi;
coord_result(6) = r_base*180/pi;