利用欧拉角与旋转矩阵来对陀螺仪与加速度计的原始数据进行姿态求解

 

  1 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 角度

三次旋转的示意图如下：