|
作者：讓四軸飛 全國各地已經陸續開放低空管制，北京也將在2015年全面開放低空領域，這對低空飛行器將是一個十分重大的好消息!低空飛行器也將迎來一個新的發展春天。實際上，近年四軸飛行器發展相當迅速，國內的航拍水平越來越高，順豐及亞馬遜已在嘗試將無人機用于快遞行業。越來越多的人開始關注并研究四軸飛行器。 本文將分析一種常見的四軸飛行器姿態解算方法，Mahony的互補濾波法。此法簡單有效，希望能給學習四軸飛行器的朋友們帶來幫助。關于姿態解算和濾波的理論知識，推薦秦永元的兩本書，一是《慣性導航》，目前已出到第二版了;二是《卡爾曼濾波與組合導航原理》。程序中的理論基礎，可在書中尋找。 下面開始進入正題： 先定義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的互補濾波函數，確實很巧妙，利用叉乘誤差來修正四軸的姿態，姿態解算速度越快，則解算的精度越高。在許多國內開源程序中，也是用到了這種方法。在解四元數微分方程時，該程序用到了一階畢卡解法。同樣可用于解四元數微分方程的還有龍格-庫塔法，由于篇幅有限，此處就不介紹龍格-庫塔法了，有興趣的網友請自行查閱相關資料。 |
