mpu6050姿态解算原理分析及程序设计

　　使用MPU6050硬件DMP解算姿态是非常简单的，下面介绍由三轴陀螺仪和加速度计的值来使用四元数软件解算姿态的方法。

　　我们先来看看如何用欧拉角描述一次平面旋转（坐标变换）：

　　

　　设坐标系绕旋转α角后得到坐标系，在空间中有一个矢量在坐标系中的投影为，在内的投影为由于旋转绕进行，所以Z坐标未变，即有。

　　

　　转换成矩阵形式表示为：

　　

　　整理一下：

　　

　　所以从旋转到可以写成上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋转要转三次：

　　

　　上面得到了一个表示旋转的方向余弦矩阵。

　　不过要想用欧拉角解算姿态，其实我们套用欧拉角微分方程就行了：

　　

　　上式中左侧，是本次更新后的欧拉角，对应row、pit、yaw。右侧，是上个周期测算出来的角度，三个角速度由直接安装在四轴飞行器的三轴陀螺仪在这个周期转动的角度，单位为弧度，计算间隔时T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。间因此求解这个微分方程就能解算出当前的欧拉角。

　　前面介绍了什么是欧拉角，而且欧拉角微分方程解算姿态关系简单明了，概念直观容易理解，那么我们为什么不用欧拉角来表示旋转而要引入四元数呢？

　　一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时解算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“GimbalLock”。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。

　　四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。