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

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

下面開始進入正題：

先定義Kp，Ki，以及halfT 。

Kp，Ki，控制加速度計修正陀螺儀積分姿態(tài)的速度

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

#define Kp 2.0f

#define Ki 0.002f

#define halfT 0.001f

初始化四元數(shù)

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

定義姿態(tài)解算誤差的積分

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

以下為姿態(tài)解算函數(shù)。

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

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

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

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

float norm;

float vx, vy, vz;

float ex, ey, ez;

將加速度的原始數(shù)據(jù)，歸一化，得到單位加速度

norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;

ay = ay / norm;

az = az / norm;

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

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

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

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

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

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

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

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

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修正陀螺零偏，通過調節(jié)Kp，Ki兩個參數(shù)，可以控制加速度計修正陀螺儀積分姿態(tài)的速度

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

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

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

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;

四元數(shù)單位化

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

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

}

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

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