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