MPU6050姿态解算2-欧拉角&旋转矩阵

只看该作者 · 2022-9-27 16:29

IMU姿态解算

IMU，即惯性测量单元，一般包含三轴陀螺仪与三轴加速度计。之前的文章MPU6050姿态解算1-DMP方式已将对MPU6050这款IMU作了简单的介绍，并通过其内部的DMP处理单元直接得到姿态解算的四元数结果。本篇将通过软件解算的方式，利用欧拉角与旋转矩阵来对陀螺仪与加速度计的原始数据进行姿态求解，并将两种姿态进行互补融合，最终得到IMU的实时姿态。

本篇的姿态解算选用的旋转顺序为ZYX，即IMU坐标系初始时刻与大地坐标系重合，然后依次绕自己的Z、Y、X轴进行旋转，这里先自定义一下每次的旋转名称和符号：

绕IMU的Z轴旋转：航向角yaw，转动 y 角度
绕IMU的Y轴旋转：俯仰角pitch，转动 p 角度
绕IMU的X轴旋转：横滚角row，转动 r 角度

只看该作者 · 2022-9-27 16:31

三次旋转的示意图如下：

只看该作者 · 2022-9-27 16:34

另外，横滚roll，俯仰pitch，偏航yaw的实际含义如下图：

只看该作者 · 2022-9-27 22:47

只看该作者 · 2022-9-27 22:48

只看该作者 · 2022-9-27 23:06

旋转矩阵

旋转矩阵的知识请先参阅3维旋转矩阵推导与助记这里只列出本篇需要用到的3个旋转矩阵，注意这3个旋转矩阵是坐标变换的旋转矩阵。

只看该作者 · 2022-9-27 23:07

欧拉角旋转

欧拉角旋转的知识请先参阅欧拉角旋转这里需要说明的是，本篇需要用到的绕着自己运动的轴，以ZYX顺序旋转。

只看该作者 · 2022-9-27 23:07

加速度计解算姿态角

加速度计测量的是其感受到的加速度，在静止的时候，其本身是没有加速运动的，但因为重力加速度的作用，根据相对运动理论，其感受的加速度与重力加速度正好相反，即读到的数据是竖直向上的。加速度计的英文简写为acc，下面用首字母a代表加速度计数据。

只看该作者 · 2022-9-27 23:08

加速度利用静止时刻感受到重力加速度，计算姿态：

当加速度计水平放置，即Z轴竖直向上时，Z轴可以读到1g的数值（g为重力加速度），X轴和Y轴两个方向读到0，可以记作（0，0，g）。

当加速度计旋转一定的姿态时，重力加速度会在加速度的3个轴上产生相应的分量，其本质是大地坐标系下的（0，0，g）在新的加速度计自身坐标系下的坐标，加速度计读到的3个值就是（0，0，g）向量的新坐标。

只看该作者 · 2022-9-27 23:09

姿态的旋转选用ZYX顺序的3次旋转方式，则上述描述可表示为：

只看该作者 · 2022-9-27 23:10

解这个方程，可以得到roll和pitch角（由于绕Z旋转时，感受到的重力加速度是不变的，因此加速度计无法计算yaw角）

只看该作者 · 2022-9-27 23:11

3次旋转过程的分解过程如下图：

只看该作者 · 2022-9-27 23:12

陀螺仪解算姿态角

陀螺仪测量的绕3个轴转动的角速度，因此，对角速度积分，可以得到角度。陀螺仪的英文简写为gyro，下面用首字母g代表陀螺仪数据。

如下图，IMU在第n个时刻的姿态角度为r、p、y，其含义为IMU坐标系从初始位置，经过绕Z旋转y角度，绕Y旋转p角度，绕X旋转r角度，得到了最终的姿态，此时需要计算下一个时刻(n+1)的姿态。设n+1时刻的姿态角为r+Δr、p+Δp、y+Δy，该姿态也是经历了3次旋转。要想计算n+1时刻的姿态，只要在n时刻姿态的基础上，加上对应的姿态角度变化量即可。姿态角度的变化量可以通过角速度与采用时间周期积分即可。

只看该作者 · 2022-9-27 23:13

只看该作者 · 2022-9-27 23:23

这里红框中dr/dt等角速度实际是假想的角速度，用于姿态更新，姿态更新是以大地坐标系为参考，而陀螺仪在第n个状态读出的角速度是以它自己所在的坐标系为参考，需要将读到的gyro陀螺数据经过变换，才能用于计算更新第n+1次的姿态。

只看该作者 · 2022-9-27 23:24

那dr/dt等角速度该怎样理解呢？看下面这个图，还是将其分解为3次旋转：

只看该作者 · 2022-9-27 23:25

首先来看dy/dt，它是3次旋转过程中绕Z轴的yaw角的角速度，3次旋转首先就是绕着Z轴旋转，Z轴方向的单位向量可表示为[0 0 1]T，T表示向量转置，因此[0 0 dy/dt]T表示在图中状态①的坐标中绕Z的角速度。由于之后该坐标系还要经历绕Y和绕X的两次旋转，因此这里[0 0 dy/dt]T角速度在经历两次旋转后，在最终的坐标系(状态③)中的坐标也要经历两次变换。图中的[gx_Z gy_Z gz_Z]T表示3次旋转过程中绕Z轴的yaw角的角速度在最终姿态中的等效转动角速度，实际就是状态①坐标系中绕Z轴的角速度在状态③坐标系中的新的坐标。

只看该作者 · 2022-9-27 23:26

同理，dp/dt还需要经历1次旋转变换，而dr/dt不需要经历旋转。

将dy/dt，dp/dt，dr/dt三者都变换到状态③坐标系中的新的坐标相加，实际就是状态③时刻陀螺仪自己读到的gyro数据。

只看该作者 · 2022-9-27 23:27

所以，从dr/dt等角速度到陀螺仪读到的角速度gx等的转换关系推导过程如下：

只看该作者 · 2022-9-27 23:28

进一步，再把这里的状态③理解为状态n，则根据状态n时刻读到的陀螺仪数据，反解dy/dt等角速度数据，即可更新得到状态n+1的姿态。反解就是求逆矩阵，即：