最近在研究小四軸的飛行，姿態(tài)檢測主要用到的傳感器是MPU6050。從MPU6050讀出來的加速度和角速度數(shù)據(jù)最后要轉(zhuǎn)成姿態(tài)，可以轉(zhuǎn)換成歐拉角（偏航角、俯仰角和滾轉(zhuǎn)角）或四元數(shù)表示，為了減少計算量（歐拉角涉及正弦運算，運算量相對較大），方便在STM32主控上實現(xiàn)，可以轉(zhuǎn)換成四元數(shù)表示。

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

　　我們先來看看如何用歐拉角描述一次平面旋轉(zhuǎn)（坐標(biāo)變換）：

　　設(shè)坐標(biāo)系繞旋轉(zhuǎn)α角后得到坐標(biāo)系，在空間中有一個矢量在坐標(biāo)系中的投影為，在內(nèi)的投影為由于旋轉(zhuǎn)繞進行，所以Z坐標(biāo)未變，即有。

　　轉(zhuǎn)換成矩陣形式表示為：

　　整理一下：

　　所以從旋轉(zhuǎn)到可以寫成

　　上面僅僅是繞一根軸的旋轉(zhuǎn)，如果三維空間中的歐拉角旋轉(zhuǎn)要轉(zhuǎn)三次：

　　上面得到了一個表示旋轉(zhuǎn)的方向余弦矩陣。

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

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

　　前面介紹了什么是歐拉角，而且歐拉角微分方程解算姿態(tài)關(guān)系簡單明了，概念直觀容易理解，那么我們?yōu)槭裁床挥脷W拉角來表示旋轉(zhuǎn)而要引入四元數(shù)呢？

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

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

　　我們知道在平面（x，y）中的旋轉(zhuǎn)可以用復(fù)數(shù)來表示，同樣的三維中的旋轉(zhuǎn)可以用單位四元數(shù)來描述。我們來定義一個四元數(shù)：

　　我們可以把它寫成，其中，。那么是矢量，表示三維空間中的旋轉(zhuǎn)軸。w是標(biāo)量，表示旋轉(zhuǎn)角度。那么就是繞軸旋轉(zhuǎn)w度，所以一個四元數(shù)可以表示一個完整的旋轉(zhuǎn)。只有單位四元數(shù)才可以表示旋轉(zhuǎn)，至于為什么，因為這就是四元數(shù)表示旋轉(zhuǎn)的約束條件。

　　而剛才用歐拉角描述的方向余弦矩陣用四元數(shù)描述則為：

　　所以在軟件解算中，我們要首先把加速度計采集到的值（三維向量）轉(zhuǎn)化為單位向量，即向量除以模，傳入?yún)?shù)是陀螺儀x，y，z值和加速度計x，y，z值：

　　《code》void IMUupdate（float gx， float gy， float gz， float ax， float ay， float az） { float norm; float vx， vy， vz; float ex， ey， ez; norm = sqrt（ax*ax + ay*ay + az*az）; ax = ax / norm; ay = ay / norm; az = az / norm;

　　下面把四元數(shù)換算成方向余弦中的第三行的三個元素。剛好vx，vy，vz 其實就是上一次的歐拉角（四元數(shù)）的機體坐標(biāo)參考系換算出來的重力的單位向量。

　　《code》// estimated direction of gravity vx = 2*（q1*q3 - q0*q2）; vy = 2*（q0*q1 + q2*q3）; vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

　　axyz是機體坐標(biāo)參照系上，加速度計測出來的重力向量，也就是實際測出來的重力向量。

　　axyz是測量得到的重力向量，vxyz是陀螺積分后的姿態(tài)來推算出的重力向量，它們都是機體坐標(biāo)參照系上的重力向量。

　　那它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加計測出來的姿態(tài)之間的誤差。

　　向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來表示，exyz就是兩個重力向量的叉積。

　　這個叉積向量仍舊是位于機體坐標(biāo)系上的，而陀螺積分誤差也是在機體坐標(biāo)系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現(xiàn)在對機體坐標(biāo)系的糾正。

　　《code》// integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki;

　　用叉積誤差來做PI修正陀螺零偏

　　《code》// integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt;

　　四元數(shù)微分方程，其中T為測量周期，為陀螺儀角速度，以下都是已知量，這里使用了一階龍哥庫塔求解四元數(shù)微分方程：

　　《code》// integrate quaternion rate and normalise q0 = q0 + （-q1*gx - q2*gy - q3*gz）*halfT; q1 = q1 + （q0*gx + q2*gz - q3*gy）*halfT; q2 = q2 + （q0*gy - q1*gz + q3*gx）*halfT; q3 = q3 + （q0*gz + q1*gy - q2*gx）*halfT;

　　最后根據(jù)四元數(shù)方向余弦陣和歐拉角的轉(zhuǎn)換關(guān)系，把四元數(shù)轉(zhuǎn)換成歐拉角：

　　所以有：

　　Q_ANGLE.Yaw = atan2（2 * q1 * q2 + 2 * q0 * q3， -2 * q2*q2 - 2 * q3* q3 + 1）* 57.3; // yaw

　　Q_ANGLE.Y = asin（-2 * q1 * q3 + 2 * q0* q2）* 57.3; // pitch

　　Q_ANGLE.X = atan2（2 * q2 * q3 + 2 * q0 * q1， -2 * q1 * q1 - 2 * q2* q2 + 1）* 57.3; // roll

　　以下是代碼實現(xiàn)，attitude是上一次姿態(tài)融合的四元數(shù)（內(nèi)存地址），gyr［3］是MPU6050讀出來的角速度，acc［3］是MPU6050讀出來的加速度，interval為積分時間。

　　void mix_gyrAcc_crossMethod（quaternion * attitude，const float gyr［3］，const float acc［3］，float interval）

　　{

　　const static float FACTOR = 0.001;//取接近0的數(shù)

　　//

　　float w_q = attitude-》w;

　　float x_q = attitude-》x;

　　float y_q = attitude-》y;

　　float z_q = attitude-》z;

　　float x_q_2 = x_q * 2;

　　float y_q_2 = y_q * 2;

　　float z_q_2 = z_q * 2;

　　//

　　// 加速度計的讀數(shù)，單位化。

　　float a_rsqrt = math_rsqrt（acc［0］*acc［0］+acc［1］*acc［1］+acc［2］*acc［2］）;

　　float x_aa = acc［0］ * a_rsqrt;

　　float y_aa = acc［1］ * a_rsqrt;

　　float z_aa = acc［2］ * a_rsqrt; //加速度計測量出的加速度向量（載體坐標(biāo)系下）

　　//

　　// 載體坐標(biāo)下的重力加速度向量，單位化。

　　float x_ac = x_q*z_q_2 - w_q*y_q_2;

　　float y_ac = y_q*z_q_2 + w_q*x_q_2; //通過四元數(shù)旋轉(zhuǎn)矩陣與地理坐標(biāo)系下的重力加速度向量［0 0 0 1］叉乘得到載體坐標(biāo)系下的重力加速度向量

　　float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;//（主要）角速度計測出的四元數(shù)表示的載體坐標(biāo)系下的重力加速度向量（這里已轉(zhuǎn)換成載體坐標(biāo)系下）

　　//

　　// 測量值與常量的叉積。

　　float x_ca = y_aa * z_ac - z_aa * y_ac;

　　float y_ca = z_aa * x_ac - x_aa * z_ac;

　　float z_ca = x_aa * y_ac - y_aa * x_ac;//角速度計測出的角度誤差，疊加的FACTOR大小可以實驗試湊

　　//

　　// 構(gòu)造增量旋轉(zhuǎn)。

　　float delta_x = gyr［0］ * interval / 2 + x_ca * FACTOR;

　　float delta_y = gyr［1］ * interval / 2 + y_ca * FACTOR;

　　float delta_z = gyr［2］ * interval / 2 + z_ca * FACTOR;

　　//

　　// 融合，四元數(shù)乘法。

　　attitude-》w = w_q - x_q*delta_x - y_q*delta_y - z_q*delta_z;

　　attitude-》x = w_q*delta_x + x_q + y_q*delta_z - z_q*delta_y;

　　attitude-》y = w_q*delta_y - x_q*delta_z + y_q + z_q*delta_x;

　　attitude-》z = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q;

　　quaternion_normalize（attitude）;//歸一化

　　}