全國各地已經陸續開放低空管制，北京也將在2015年全面開放低空領域，這對低空飛行器將是一個十分重大的好消息!低空飛行器也將迎來一個新的發展春天。實際上，近年四軸飛行器發展相當迅速，國內的航拍水平越來越高，順豐及亞馬遜已在嘗試將無人機用于快遞行業。越來越多的人開始關注并研究四軸飛行器。

　　本文將分析一種常見的四軸飛行器姿態解算方法，Mahony的互補濾波法。此法簡單有效，希望能給學習四軸飛行器的朋友們帶來幫助。關于姿態解算和濾波的理論知識，推薦秦永元的兩本書，一是《慣性導航》，目前已出到第二版了;二是《卡爾曼濾波與組合導航原理》。程序中的理論基礎，可在書中尋找。

　　同時歡迎到論壇發帖交流：http://forum.eepw.com.cn/forum/368/1

　　下面開始進入正題：

　　先定義Kp，Ki，以及halfT 。

　　Kp，Ki，控制加速度計修正陀螺儀積分姿態的速度

　　halfT ，姿態解算時間的一半。此處解算姿態速度為500HZ，因此halfT 為0.001

　　#define Kp 2.0f

　　#define Ki 0.002f

　　#define halfT 0.001f

　　初始化四元數

　　float q0 = 1, q1 = 0, q2 = 0, q3 = 0;

　　定義姿態解算誤差的積分

　　float exInt = 0, eyInt = 0, ezInt = 0;

　　以下為姿態解算函數。

　　參數gx，gy，gz分別對應三個軸的角速度，單位是弧度/秒;

　　參數ax，ay，az分別對應三個軸的加速度原始數據

　　由于加速度的噪聲較大，此處應采用濾波后的數據

　　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;

　　把四元數換算成“方向余弦矩陣”中的第三列的三個元素。根據余弦矩陣和歐拉角的定義，地理坐標系的重力向量，轉到機體坐標系，正好是這三個元素。所以這里的vx、vy、vz，其實就是當前的機體坐標參照系上，換算出來的重力單位向量。(用表示機體姿態的四元數進行換算)

　　vx = 2*(q1*q3 - q0*q2);

　　vy = 2*(q0*q1 + q2*q3);

　　vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

　　這里說明一點，加速度計由于噪聲比較大，而且在飛行過程中，受機體振動影響比陀螺儀明顯，短時間內的可靠性不高。陀螺儀噪聲小，但是由于積分是離散的，長時間的積分會出現漂移的情況，因此需要將用加速度計求得的姿態來矯正陀螺儀積分姿態的漂移。

　　在機體坐標參照系上，加速度計測出來的重力向量是ax、ay、az;陀螺積分后的姿態來推算出的重力向量是vx、vy、vz;它們之間的誤差向量，就是陀螺積分后的姿態和加速度計測出來的姿態之間的誤差。

　　向量間的誤差，可以用向量積(也叫外積、叉乘)來表示，ex、ey、ez就是兩個重力向量的叉積。這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體坐標系的糾正。

　　叉乘是數學基礎，百度百科里有詳細解釋。

　　ex = (ay*vz - az*vy);

　　ey = (az*vx - ax*vz);

　　ez = (ax*vy - ay*vx);

　　將叉乘誤差進行積分

　　exInt = exInt + ex*Ki;

　　eyInt = eyInt + ey*Ki;

　　ezInt = ezInt + ez*Ki;

　　用叉乘誤差來做PI修正陀螺零偏，通過調節Kp，Ki兩個參數，可以控制加速度計修正陀螺儀積分姿態的速度

　　gx = gx + Kp*ex + exInt;

　　gy = gy + Kp*ey + eyInt;

　　gz = gz + Kp*ez + ezInt;

　　四元數微分方程，沒啥好說的了，看上面推薦的書吧，都是理論的東西，自個琢磨琢磨

　　實在琢磨不明白，那就把指定的參數傳進這個函數，再得到相應的四元數，最后轉化成歐拉角即可了。不過建議還是把理論弄清楚一點。

　　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;

　　四元數單位化

　　norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

　　q0 = q0 / norm;

　　q1 = q1 / norm;

　　q2 = q2 / norm;

　　q3 = q3 / norm;

　　}

　　姿態解算后，就得到了表示姿態的四元數。但四元數不夠直觀，一般將其轉化為歐拉角。轉化時根據旋轉軸的次序不同，公式也不同。以下給出其中一種公式：

　　讀完程序，深刻的意識到了理論基礎的重要性。Mahony的互補濾波函數，確實很巧妙，利用叉乘誤差來修正四軸的姿態，姿態解算速度越快，則解算的精度越高。在許多國內開源程序中，也是用到了這種方法。在解四元數微分方程時，該程序用到了一階畢卡解法。同樣可用于解四元數微分方程的還有龍格-庫塔法，由于篇幅有限，此處就不介紹龍格-庫塔法了，有興趣的網友請自行查閱相關資料。