【四足机器人】学习笔记 欧拉角与旋转矩阵之间的转化关系

在研究四足机器人站立姿态的时候，需要将通过IMU模块采集到的欧拉角数据信息转变为旋转矩阵R，关于如何将获取的欧拉角转换为旋转矩阵，博主在这里进行了说明。

欧拉角定义：以经典直角坐标系为例，经典直角坐标系包含x轴、y轴、z轴，而绕这三个轴旋转的的角度总称为欧拉角

我们通过IMU模块可以得到三个角度信息，分别是Roll(翻滚角)，Pitch(俯仰角)和Yaw(偏航角)，翻滚角、俯仰角和偏航角的定义如下：

定义物体运动的右、上、前三个方向构成右手系
绕上方向轴的角度是Yaw偏航角
绕右方向轴的角度是Pitch俯仰角
绕前方向轴的角度是Roll翻滚角

关于如何在ubuntu16.04下通过ROS快速有效地得到IMU数据信息，博主在之前的文章中已经说明过了，链接如下，需要的小伙伴可以去查看
链接: https://blog.csdn.net/weixin_45417246/article/details/115801106.

一、基础旋转矩阵

在了解欧拉角与旋转矩阵之间的转换关系之前，什么是右手坐标系，什么是左手坐标系，它们的定义如下图

基础旋转矩阵的定义是绕某单一坐标轴进行旋转时对应的矩阵，绕轴旋转时，以顺时针方向为正，但是进行旋转时，所朝向的方向一定为坐标原点，根据得到的欧拉角(p,h,b)信息，p,h,b对应于Roll(翻滚角)，Pitch(俯仰角)和Yaw(偏航角)，则可以计算得到在右手坐标系下的基础旋转矩阵

//在右手系中绕X轴旋转p° 对应的矩阵Rx
        | 1    0       0 |
  Rx=   | 0   cosp  -sinp|
        | 0   sinp   cosp|
//在右手系中绕Y轴旋转h° 对应的矩阵Ry
        | cosh   0   sinh|
  Ry=   |  0     1     0 |
        |-sinh   0   cosh|
//在右手系中绕Z轴旋转b° 对应的矩阵Rz
        |cosb  -sinb   0 |
  Rz=   |sinb   cosb   0 |
        |  0     0     1 |

同理，在左手坐标系下的基础旋转矩阵包括以下三个矩阵

//在左手系中绕X轴旋转p° 对应的矩阵Rx
        | 1     0        0 |
  Rx=   | 0    cosp    sinp|
        | 0   -sinp    cosp|
//在左手系中绕Y轴旋转h° 对应的矩阵Ry
        | cosh   0   -sinh|
  Ry=   |  0     1      0 |
        | sinh   0    cosh|
//在左手系中绕Z轴旋转b° 对应的矩阵Rz
        |cosb    sinb   0 |
  Rz=   |-sinb   cosb   0 |
        |  0      0     1 |

对于在经典直角坐标系中向量的单一旋转，一般只需要乘以对应的旋转矩阵就可以得到旋转后的向量

  AB=[0,0,1| //向量AB
以右手坐标系为例，进行变换，绕x轴顺时针旋转90°
        | 1   0   0 |
  Rx=   | 0   0  -1 |
        | 0   1   0 |
  AB’=AB*Rx=[0, 1, 0]
以右手坐标系为例，进行变换，绕y轴顺时针旋转90°
        | 0   0   1|
  Ry=   | 0   1   0|
        |-1   0   0|
  AB’‘=AB*Ry=[-1, 0, 0]
  以右手坐标系为例，进行变换，绕z轴顺时针旋转90°
        |0  -1   0 |
  Rz=   |1   0   0 |
        |0   0   1 |
  AB’’‘=AB*Ry=[0, 0, 1]

通过一个最简单的旋转式子，可以看到计算结果符合基础旋转矩阵的定义，左手坐标系同理。

二、组合旋转矩阵

组合旋转矩阵的定义是多次旋转组合对应的矩阵，参考旋转变换的性质，物体绕轴旋转对应的矩阵具有可累乘的性质，即多一次旋转就是多一次矩阵乘法。
以Z-X-Y顺规的欧拉角对应的组合矩阵就是R = RzRxRy，XYZ的顺序决定了旋转矩阵的定义式，当欧拉角的顺规改变时，旋转矩阵的计算公式也需相应改变。在坐标变换关系中，顺规的顺序组合一共有12种，分别是XYZ，XZY，YZX，YXZ，ZXY，ZYX，XYX，YXY，ZXZ，XZX，YZY，ZYZ，相对应的旋转矩阵也有12种。

//欧拉角(p,h,b)在右手系中对应的旋转矩阵（Z-X-Y顺规）
               |cosbcosh-sinbsinpsinh  -sinbcosp  cosbsinh+sinbsinpcosh|
R = Rz*Rx*Ry = |sinbcosh+cosbsinpsinh   cosbcosp  sinbsinh-cosbsinpcosh|
               |     -cospsinh            sinp           cospcosh      |

//欧拉角(p,h,b)在右手系中对应的旋转矩阵（Z-X-Y顺规）
                |cosbcosh+sinbsinpsinh   sinbcosp  -cosbsinh+sinbsinpcosh|
R1 = Rz*Rx*Ry = |-sinbcosh+cosbsinpsinh  cosbcosp   sinbsinh+cosbsinpcosh|
                |      cospsinh            -sinp           cospcosh      |

这样我们就得到了顺规对应的旋转矩阵，实现了把IMU模块得到的欧拉角信息转换为旋转矩阵的形式。