基于互補(bǔ)濾波的飛行器姿態(tài)解算

1、姿態(tài)解算一、主線(xiàn)姿態(tài)表示方式：矩陣表示，軸角表示，歐拉角表示，四元數(shù)表示。慣性測(cè)量單元IMU（Inertial Measurement Unit）：MPU6050芯片，包含陀螺儀和加速度計(jì)，分別測(cè)量三軸加速度和三軸角速度。注意，傳感器所測(cè)數(shù)據(jù)是原始數(shù)據(jù)，包含了噪聲，無(wú)法直接用于飛行器的姿態(tài)解算，因此需要對(duì)數(shù)據(jù)進(jìn)行濾波。濾波算法：非線(xiàn)性互補(bǔ)濾波算法，卡爾曼濾波算法，Mahony互補(bǔ)濾波算法。二、知識(shí)點(diǎn)補(bǔ)充加速度計(jì)和陀螺儀加速度計(jì)：加速度計(jì),可以測(cè)量加速度,包括外力加速度和重力加速度,因此，當(dāng)被測(cè)物體在靜止或勻速運(yùn)動(dòng)（勻速直線(xiàn)運(yùn)動(dòng)）的時(shí)候,加速度計(jì)僅僅測(cè)量的是重力加速度,而重力加速度與坐標(biāo)系（絕對(duì)

2、坐標(biāo)系）是固連的,通過(guò)這種關(guān)系,可以得到加速度計(jì)所在平面與地面 的角度關(guān)系 也就是橫滾角和俯仰角。把加速度傳感器水平靜止放在桌子上，它的Z軸輸出的是1g的加速度。因?yàn)樗黌軸方向被重力向下拉出了一個(gè)形變。可惜的是，加速度傳感器不會(huì)區(qū)分重力加速度與外力加速度。所以，當(dāng)系統(tǒng)在三維空間做變速運(yùn)動(dòng)時(shí)，它的輸出就不正確了，或者說(shuō)它的輸出不能表明物體的姿態(tài)和運(yùn)動(dòng)狀態(tài)。陀螺儀：陀螺儀測(cè)量角速度。陀螺儀模型如圖1所示，陀螺儀的每個(gè)通道檢測(cè)一個(gè)軸的旋轉(zhuǎn)。圖1引自網(wǎng)絡(luò)上圖中，Rxz是R在XZ面上的投影，與Z軸的夾角為Axz。Ryz是R在ZY面上的投影，與Z軸的夾角為Ayz。陀螺儀就是測(cè)量上面定義角度的變化率，換句話(huà)

3、說(shuō)，它會(huì)輸出一個(gè)與上面這些角度變化率線(xiàn)性相關(guān)的值。加速度計(jì)工作原理介紹（摘自網(wǎng)絡(luò)）大多數(shù)加速度計(jì)可歸為兩類(lèi)：數(shù)字和模擬。數(shù)字加速度計(jì)可通過(guò)I2C，SPI或USART方式獲取信息，而模擬加速度計(jì)的輸出是一個(gè)在預(yù)定范圍內(nèi)的電壓值，你需要用ADC（模擬量轉(zhuǎn)數(shù)字量）模塊將其轉(zhuǎn)換為數(shù)字值。不管使用什么類(lèi)型的ADC模塊，都會(huì)得到一個(gè)在一定范圍內(nèi)的數(shù)值。例如一個(gè)10位ADC模塊的輸出值范圍在0-1023間。假設(shè)我們從10位ADC模塊得到了以下的三個(gè)軸的數(shù)據(jù)：每個(gè)ADC模塊都有一個(gè)參考電壓，假設(shè)在我們的例子中，它是3.3V。要將一個(gè)10位的ADC值轉(zhuǎn)成電壓值，我們使用下列公式：將3個(gè)軸的值代入上式，得到：每個(gè)

4、加速度計(jì)都有一個(gè)零加速度的電壓值，這個(gè)電壓值對(duì)應(yīng)于加速度為0g。通過(guò)計(jì)算相對(duì)0g電壓的偏移量我們可以得到一個(gè)有符號(hào)的電壓值。比方說(shuō)，0g電壓值，通過(guò)下面的方式可以得到相對(duì)0g電壓的偏移量:現(xiàn)在我們得到了加速度計(jì)的電壓值，但它的單位還不是，最后的轉(zhuǎn)換，我們還需要引入加速度計(jì)的靈敏度，單位通常是。比如，加速度計(jì)的靈敏度。靈敏度值可以在加速度計(jì)說(shuō)明書(shū)中找到。要獲得最后的單位為的加速度，我們使用下列公式計(jì)算：綜上，可以把以上步驟用以下公式表達(dá)現(xiàn)在我們得到了慣性力矢量的三個(gè)分量，如果設(shè)備除了重力外不受任何外力影響，那我們就可以認(rèn)為這個(gè)方向就是重力矢量的方向。（自此明白了文獻(xiàn)1中所說(shuō)只使用加速度計(jì)獲得的角

5、度是基于飛行器在勻速飛行或靜止的條件下得到的）圖2引自網(wǎng)絡(luò)我們感興趣的角度是向量R和X，Y，Z軸之間的夾角，那就令這些角度為Axr，Ayr，Azr。觀(guān)察由R和Rx組成的直角三角形圖2中，那么，角度即為三、互補(bǔ)濾波算法加速度計(jì)是極易受外部干擾的傳感器（如機(jī)械振動(dòng)），但是測(cè)量值的誤差不隨時(shí)間的變化。陀螺儀輸出的角速度可以積分得到角度，動(dòng)態(tài)性能好，受外部干擾小，但積分會(huì)造成誤差累積?？梢钥闯觯鼈儍?yōu)缺點(diǎn)互補(bǔ)，結(jié)合起來(lái)才能有好的效果。經(jīng)典互補(bǔ)濾波算法（Classical Complementary Filter）經(jīng)典互補(bǔ)濾波算法基本原理是充分利用加速度計(jì)提供的低頻角度信號(hào)和陀螺儀提供的高頻角速度信號(hào)，

6、對(duì)加速度計(jì)進(jìn)行低通濾波，對(duì)陀螺儀進(jìn)行高通濾波，分別濾出相應(yīng)的干擾信號(hào)，為兩者的有效融合提供了很好的解決方案2。圖3 經(jīng)典互補(bǔ)濾波算法頻域形式原理圖2融合后姿態(tài)角估計(jì)值為其中，為陀螺儀測(cè)量的角速度，為加速度計(jì)測(cè)量的角度值，為比例系數(shù)，為高通濾波器，為低通濾波器。圖4 經(jīng)典互補(bǔ)濾波算法時(shí)域形式原理圖2對(duì)進(jìn)行反拉氏變換，可得時(shí)域微分形式為 改進(jìn)后的互補(bǔ)濾波算法（Explicit Complementary Filter）經(jīng)典互補(bǔ)濾波算法實(shí)現(xiàn)簡(jiǎn)單，但是估算精度角度較低，文獻(xiàn)3提出了一種改進(jìn)算法（ECF），在經(jīng)典互補(bǔ)濾波算法的補(bǔ)償環(huán)節(jié)加入積分器，以消除陀螺儀漂移常值誤差，原理框圖如下圖。圖5 改進(jìn)后的互

7、補(bǔ)濾波算法2時(shí)域微分形式為四、四旋翼飛行器姿態(tài)解算流程四旋翼姿態(tài)解算整體流程框圖如下圖5 四元數(shù)表示的姿態(tài)解算整體流程框圖4其中，Gyroscope為陀螺儀，是陀螺儀輸出的測(cè)量數(shù)據(jù)，Accelerometer是加速度計(jì)，在A(yíng)irspeed模塊以及作用下，加速度輸出測(cè)量數(shù)據(jù)。第一，對(duì)加速度計(jì)測(cè)出來(lái)的數(shù)據(jù)進(jìn)行歸一化處理：；第二，將重力加速度旋轉(zhuǎn)到機(jī)體坐標(biāo)，得到重力加速度在三個(gè)軸上的加速度分量，作為標(biāo)準(zhǔn)加速度。其中為世界坐標(biāo)系到機(jī)體坐標(biāo)系的變換矩陣。（文獻(xiàn)4圖中此處原文中有錯(cuò)誤，已經(jīng)在截圖中進(jìn)行了修改）第三，歸一化后的加速度計(jì)值與標(biāo)準(zhǔn)加速度做叉積運(yùn)算，求得誤差向量；第四，對(duì)誤差進(jìn)行濾波，改進(jìn)后的互補(bǔ)

8、濾波算法（ECF）可以表示為以下四元數(shù)形式： ， 第四，求解四元數(shù)微分方程，求得最新的；第五，代入以下公式求得姿態(tài)角。參考文獻(xiàn)1 郭曉鴻,楊忠,陳喆,等. EKF和互補(bǔ)濾波器在飛行姿態(tài)確定中的應(yīng)用J. 傳感器與微系統(tǒng),2011,30(11):149-152.2 傅忠云,朱海霞,孫金秋,等. 基于慣性傳感器 MPU6050 的濾波算法研究J. 壓電與聲光, 2015 (2015 年 05): 821-825,829.3 Mahony R, Hamel T, Pflimlin J M. Nonlinear complementary filters on the special orthogona

9、l groupJ. IEEE Transactions on automatic control, 2008, 53(5): 1203-1218.4 Euston M, Coote P, Mahony R, et al. A complementary filter for attitude estimation of a fixed-wing UAVC/Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on. IEEE, 2008: 340-345.附程序：void IMUup

10、date(float gx, float gy, float gz, float ax,float ay, float az) float norm; float vx, vy, vz; float ex, ey, ez; float q0q0 = q0*q0; float q0q1 = q0*q1; float q0q2 = q0*q2; float q1q1 = q1*q1; float q1q3 = q1*q3; float q2q2 = q2*q2; float q2q3 = q2*q3; float q3q3 = q3*q3; if(ax*ay*az=0) return; / 第一步

11、：對(duì)加速度數(shù)據(jù)進(jìn)行歸一化 norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; / 第二步：DCM矩陣旋轉(zhuǎn) vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3 ; / 第三步：在機(jī)體坐標(biāo)系下做向量叉積得到補(bǔ)償數(shù)據(jù) ex = ay*vz - az*vy ; ey = az*vx - ax*vz ; ez = ax*vy - ay*vx ; / 第四步：對(duì)誤差進(jìn)行PI計(jì)算，補(bǔ)償角速度 exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; / 第五步：