mpu6050姿態解算原理分析及程序設計

　　使用MPU6050硬件DMP解算姿態是非常簡單的，下面介紹由三軸陀螺儀和加速度計的值來使用四元數軟件解算姿態的方法。

　　我們先來看看如何用歐拉角描述一次平面旋轉（坐標變換）：

　　設坐標系繞旋轉α角后得到坐標系，在空間中有一個矢量在坐標系中的投影為，在內的投影為由于旋轉繞進行，所以Z坐標未變，即有。

　　轉換成矩陣形式表示為：

　　整理一下：

　　所以從旋轉到可以寫成上面僅僅是繞一根軸的旋轉，如果三維空間中的歐拉角旋轉要轉三次：

　　上面得到了一個表示旋轉的方向余弦矩陣。

　　不過要想用歐拉角解算姿態，其實我們套用歐拉角微分方程就行了：

　　上式中左側，是本次更新后的歐拉角，對應row、pit、yaw。右側，是上個周期測算出來的角度，三個角速度由直接安裝在四軸飛行器的三軸陀螺儀在這個周期轉動的角度，單位為弧度，計算間隔時T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。間因此求解這個微分方程就能解算出當前的歐拉角。

　　前面介紹了什么是歐拉角，而且歐拉角微分方程解算姿態關系簡單明了，概念直觀容易理解，那么我們為什么不用歐拉角來表示旋轉而要引入四元數呢？

　　一方面是因為歐拉角微分方程中包含了大量的三角運算，這給實時解算帶來了一定的困難。而且當俯仰角為90度時方程式會出現神奇的“GimbalLock”。所以歐拉角方法只適用于水平姿態變化不大的情況，而不適用于全姿態飛行器的姿態確定。

　　四元數法只求解四個未知量的線性微分方程組，計算量小，易于操作，是比較實用的工程方法。