matlab求逆矩阵_MPU6050姿态解算2-欧拉角&旋转矩阵

1 IMU姿态解算

IMU，即惯性测量单元，一般包含三轴陀螺仪与三轴加速度计。之前的文章

码农爱学习：MPU6050姿态解算方式1-DMP​zhuanlan.zhihu.com

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

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

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

三次旋转的示意图如下：

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

2 旋转矩阵

旋转矩阵的知识请先参阅

码农爱学习：3维旋转矩阵推导与助记​zhuanlan.zhihu.com

这里只列出本篇需要用到的3个旋转矩阵，注意这3个旋转矩阵是坐标变换的旋转矩阵。

3 欧拉角旋转

欧拉角旋转的知识请先参阅

码农爱学习：欧拉角旋转​zhuanlan.zhihu.com