|
作者：讓四軸飛 全國各地已經(jīng)陸續開(kāi)放低空管制，北京也將在2015年全面開(kāi)放低空領(lǐng)域，這對低空飛行器將是一個(gè)十分重大的好消息!低空飛行器也將迎來(lái)一個(gè)新的發(fā)展春天。實(shí)際上，近年四軸飛行器發(fā)展相當迅速，國內的航拍水平越來(lái)越高，順豐及亞馬遜已在嘗試將無(wú)人機用于快遞行業(yè)。越來(lái)越多的人開(kāi)始關(guān)注并研究四軸飛行器。 本文將分析一種常見(jiàn)的四軸飛行器姿態(tài)解算方法，Mahony的互補濾波法。此法簡(jiǎn)單有效，希望能給學(xué)習四軸飛行器的朋友們帶來(lái)幫助。關(guān)于姿態(tài)解算和濾波的理論知識，推薦秦永元的兩本書(shū)，一是《慣性導航》，目前已出到第二版了;二是《卡爾曼濾波與組合導航原理》。程序中的理論基礎，可在書(shū)中尋找。 下面開(kāi)始進(jìn)入正題： 先定義Kp，Ki，以及halfT 。 Kp，Ki，控制加速度計修正陀螺儀積分姿態(tài)的速度 halfT ，姿態(tài)解算時(shí)間的一半。此處解算姿態(tài)速度為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; 定義姿態(tài)解算誤差的積分 float exInt = 0, eyInt = 0, ezInt = 0; 以下為姿態(tài)解算函數。 參數gx，gy，gz分別對應三個(gè)軸的角速度，單位是弧度/秒; 參數ax，ay，az分別對應三個(gè)軸的加速度原始數據 由于加速度的噪聲較大，此處應采用濾波后的數據 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; 把四元數換算成“方向余弦矩陣”中的第三列的三個(gè)元素。根據余弦矩陣和歐拉角的定義，地理坐標系的重力向量，轉到機體坐標系，正好是這三個(gè)元素。所以這里的vx、vy、vz，其實(shí)就是當前的機體坐標參照系上，換算出來(lái)的重力單位向量。(用表示機體姿態(tài)的四元數進(jìn)行換算) vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; 這里說(shuō)明一點(diǎn)，加速度計由于噪聲比較大，而且在飛行過(guò)程中，受機體振動(dòng)影響比陀螺儀明顯，短時(shí)間內的可靠性不高。陀螺儀噪聲小，但是由于積分是離散的，長(cháng)時(shí)間的積分會(huì )出現漂移的情況，因此需要將用加速度計求得的姿態(tài)來(lái)矯正陀螺儀積分姿態(tài)的漂移。 在機體坐標參照系上，加速度計測出來(lái)的重力向量是ax、ay、az;陀螺積分后的姿態(tài)來(lái)推算出的重力向量是vx、vy、vz;它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加速度計測出來(lái)的姿態(tài)之間的誤差。 向量間的誤差，可以用向量積(也叫外積、叉乘)來(lái)表示，ex、ey、ez就是兩個(gè)重力向量的叉積。這個(gè)叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來(lái)糾正陀螺。由于陀螺是對機體直接積分，所以對陀螺的糾正量會(huì )直接體現在對機體坐標系的糾正。 叉乘是數學(xué)基礎，百度百科里有詳細解釋。 ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); 將叉乘誤差進(jìn)行積分 exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; 用叉乘誤差來(lái)做PI修正陀螺零偏，通過(guò)調節Kp，Ki兩個(gè)參數，可以控制加速度計修正陀螺儀積分姿態(tài)的速度 gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; 四元數微分方程，沒(méi)啥好說(shuō)的了，看上面推薦的書(shū)吧，都是理論的東西，自個(gè)琢磨琢磨 實(shí)在琢磨不明白，那就把指定的參數傳進(jìn)這個(gè)函數，再得到相應的四元數，最后轉化成歐拉角即可了。不過(guò)建議還是把理論弄清楚一點(diǎn)。 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; } 姿態(tài)解算后，就得到了表示姿態(tài)的四元數。但四元數不夠直觀(guān)，一般將其轉化為歐拉角。轉化時(shí)根據旋轉軸的次序不同，公式也不同。以下給出其中一種公式： 
讀完程序，深刻的意識到了理論基礎的重要性。Mahony的互補濾波函數，確實(shí)很巧妙，利用叉乘誤差來(lái)修正四軸的姿態(tài)，姿態(tài)解算速度越快，則解算的精度越高。在許多國內開(kāi)源程序中，也是用到了這種方法。在解四元數微分方程時(shí)，該程序用到了一階畢卡解法。同樣可用于解四元數微分方程的還有龍格-庫塔法，由于篇幅有限，此處就不介紹龍格-庫塔法了，有興趣的網(wǎng)友請自行查閱相關(guān)資料。 |
