MPU-6000（6050）為全球首例整合性6軸運(yùn)動(dòng)處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時(shí)間軸之差的問(wèn)題，減少了大量的封裝空間。運(yùn)動(dòng)感測(cè)游戲 現(xiàn)實(shí)增強(qiáng) 電子穩(wěn)像 （EIS： Electronic Image Stabilization） 光學(xué)穩(wěn)像（OIS： Optical Image Stabilization） 行人導(dǎo)航器 “零觸控”手勢(shì)用戶(hù)接口 姿勢(shì)快捷方式 認(rèn)證

　　MPU6050互補(bǔ)濾波法融合四元數(shù)姿態(tài)原理及代碼

　　小四軸的飛行，姿態(tài)檢測(cè)主要用到的傳感器是MPU6050。從MPU6050讀出來(lái)的加速度和角速度數(shù)據(jù)最后要轉(zhuǎn)成姿態(tài)，可以轉(zhuǎn)換成歐拉角（偏航角、俯仰角和滾轉(zhuǎn)角）或四元數(shù)表示，為了減少計(jì)算量（歐拉角涉及正弦運(yùn)算，運(yùn)算量相對(duì)較大），方便在STM32主控上實(shí)現(xiàn)，可以轉(zhuǎn)換成四元數(shù)表示。

　　那么問(wèn)題來(lái)了，四元數(shù)姿態(tài)融合算法哪家強(qiáng)？這里介紹圓點(diǎn)博士小四軸飛控開(kāi)源代碼關(guān)于四元數(shù)姿態(tài)融合的算法以及代碼實(shí)現(xiàn)，不能說(shuō)最強(qiáng)（國(guó)外有很多優(yōu)秀的飛控算法），只是實(shí)現(xiàn)效果可以滿(mǎn)足小四軸的成功飛行，個(gè)人水平渣渣，請(qǐng)大牛批評(píng)指正。博士的代碼中主要用到的是互補(bǔ)濾波法。下圖為該方法的原理圖（來(lái)自論文：禤永俊，《《四軸飛行器遙感平臺(tái)的實(shí)現(xiàn)方案》》）。

　　先介紹一下互補(bǔ)濾波的基本概念，這是阿莫論壇上一個(gè)會(huì)員的總結(jié)：對(duì)mpu6050來(lái)說(shuō)，加速度計(jì)對(duì)四軸或小車(chē)的加速度比較敏感，取瞬時(shí)值計(jì)算傾角誤差比較大；而陀螺儀積分得到的角度不受小車(chē)加速度的影響，但是隨著時(shí)間的增加積分漂移和溫度漂移帶來(lái)的誤差比較大。所以這兩個(gè)傳感器正好可以彌補(bǔ)相互的缺點(diǎn)。

　　不過(guò)要怎么彌補(bǔ)呢？經(jīng)過(guò)上面的介紹是否感覺(jué)到可以用濾波器做文章呢？這里講的互補(bǔ)濾波就是在短時(shí)間內(nèi)采用陀螺儀得到的角度做為最優(yōu)，定時(shí)對(duì)加速度采樣來(lái)的角度進(jìn)行取平均值來(lái)校正陀螺儀的得到的角度。就是，短時(shí)間內(nèi)用陀螺儀比較準(zhǔn)確，以它為主；長(zhǎng)時(shí)間用加速度計(jì)比較準(zhǔn)確，這時(shí)候加大它的比重，這就是互補(bǔ)了，不過(guò)濾波在哪里？加速度計(jì)要濾掉高頻信號(hào)，陀螺儀要濾掉低頻信號(hào)，互補(bǔ)濾波器就是根據(jù)傳感器特性不同。

　　通過(guò)不同的濾波器（高通或低通，互補(bǔ)的），然后再相加得到整個(gè)頻帶的信號(hào)，例如，加速度計(jì)測(cè)傾角，其動(dòng)態(tài)響應(yīng)較慢，在高頻時(shí)信號(hào)不可用，所以可通過(guò)低通抑制高頻；陀螺響應(yīng)快，積分后可測(cè)傾角，不過(guò)由于零漂等，在低頻段信號(hào)不好。通過(guò)高通濾波可抑制低頻噪聲。將兩者結(jié)合，就將陀螺和加表的優(yōu)點(diǎn)融合起來(lái)，得到在高頻和低頻都較好的信號(hào)，互補(bǔ)濾波需要選擇切換的頻率點(diǎn)，即高通和低通的頻率。

　　我個(gè)人覺(jué)得互補(bǔ)濾波在博士代碼里的基本思想是以角速度積分得到角度為主進(jìn)行姿態(tài)解算，但是由于周?chē)h(huán)境或自身器件的原因，角速度計(jì)積分出來(lái)的角度可能會(huì)產(chǎn)生誤差的累積，長(zhǎng)時(shí)間可能引起嚴(yán)重的角度偏移，而加速度計(jì)雖然精度不高但沒(méi)有積累誤差，可以用來(lái)對(duì)角度進(jìn)行糾正。通過(guò)上位機(jī)采集互補(bǔ)濾波法融合的四元數(shù)姿態(tài)數(shù)據(jù)并顯示如下圖，實(shí)驗(yàn)證明，這種算法很好的消除了角度偏移。

　　下面來(lái)討論加速度計(jì)測(cè)出來(lái)的加速度向量如何糾正角度：

　?。?）首先是要用上一次融合的四元數(shù)估測(cè)出機(jī)坐標(biāo)系下的重力加速度向量，地理坐標(biāo)系下的重力加速度向量是r（E）=［0 0 1］，這里要將地理坐標(biāo)系下的重力加速度轉(zhuǎn)換成機(jī)坐標(biāo)系下的重力加速度，如下（具體參考：秦永元，《慣性導(dǎo)航-第二版》，P251）

　　r（E）=C（E-》A）r（A） //E（earth）表示r（E）是地理坐標(biāo)系下的重力加速度向量，C（E-》A）表示從地理坐標(biāo)系（E:earth）轉(zhuǎn)換成機(jī)（A:airplane）坐標(biāo)系的轉(zhuǎn)換四元數(shù)矩陣

　　r（A）=R（-1）（C（E-》A））r（E） //R（-1）（C（E-》A））表示對(duì)矩陣C（E-》A）取逆

　　r（A）為估測(cè)出的機(jī)坐標(biāo)系下的重力加速度向量，因?yàn)檫@是從上一次的姿態(tài)四元數(shù)中導(dǎo)出的向量，而姿態(tài)四元數(shù)主要是角速度計(jì)讀取的值的換算，所以這里只能說(shuō)估測(cè)。

　?。?）然后我們要得到加速度計(jì)的重力加速度值，因?yàn)榧铀俣扔?jì)是固聯(lián)在機(jī)體上的，所以讀出來(lái)的向量是機(jī)坐標(biāo)系下的加速度向量，又因?yàn)樾∷妮S運(yùn)動(dòng)速度較慢，除開(kāi)重力以外的加速度值一般較小，在誤差允許范圍內(nèi)我們可以假定加速度計(jì)測(cè)出來(lái)的就是重力加速度向量。

　?。?）將機(jī)坐標(biāo)系下的估測(cè)的重力加速度向量和加速度計(jì)測(cè)出的重力加速度向量分別規(guī)范化為a，b，然后叉積，叉積的結(jié)果為c=|a||b|sin（A）r（D），sin（A）角度偏移的正弦，r（D）為方向，小角情況下認(rèn)為sin（A）=A，A為偏移的角度，而且|a|=|b|=1，故叉積后角度偏移算出來(lái)了。

　?。?）將角度偏移乘以一定的系數(shù)疊加在角速度積分結(jié)果上，為什么要乘以一定的系數(shù)？這是因?yàn)檫@里算出的角度偏移是以加速度計(jì)為基準(zhǔn)的，而加速度計(jì)的誤差較大，直接疊加可能會(huì)引起比較大的震蕩，所以要乘以一定的系數(shù)FACTOR，但FACTOR不能太小，否則糾偏效果不好，具體取值可以試湊，然后在上位機(jī)觀測(cè)結(jié)果。

　　角度增量出來(lái)后接下來(lái)就好辦了，主要是將各種計(jì)算結(jié)果往四元數(shù)微分方程一代，結(jié)果就出來(lái)了，簡(jiǎn)單推導(dǎo)如下（具體見(jiàn)秦永元，《慣性導(dǎo)航-第二版》，p254）

　　d（Q）/d（t）=1/2 × Q叉乘w（A） Q為上一次的姿態(tài)四元數(shù)，w（A）表示MPU6050讀出的機(jī)體坐標(biāo)系下的角速度

　　delta（Q）=1/2 × Q叉乘（w（A） ×delta（t）+c） delta（t）為積分時(shí)間

　　Q（new）=Q+delta（Q） 新的四元數(shù)姿態(tài)求出

　　以下是代碼實(shí)現(xiàn)，attitude是上一次姿態(tài)融合的四元數(shù)（內(nèi)存地址），gyr［3］是MPU6050讀出來(lái)的角速度，acc［3］是MPU6050讀出來(lái)的加速度，interval為積分時(shí)間。

　　void mix_gyrAcc_crossMethod（quaternion * attitude，const float gyr［3］，const float acc［3］，float interval）

　　{

　　const static float FACTOR = 0.001;//取接近0的數(shù)

　　//

　　float w_q = attitude-》w;

　　float x_q = attitude-》x;

　　float y_q = attitude-》y;

　　float z_q = attitude-》z;

　　float x_q_2 = x_q * 2;

　　float y_q_2 = y_q * 2;

　　float z_q_2 = z_q * 2;

　　//

　　// 加速度計(jì)的讀數(shù)，單位化。

　　float a_rsqrt = math_rsqrt（acc［0］*acc［0］+acc［1］*acc［1］+acc［2］*acc［2］）;

　　float x_aa = acc［0］ * a_rsqrt;

　　float y_aa = acc［1］ * a_rsqrt;

　　float z_aa = acc［2］ * a_rsqrt; //加速度計(jì)測(cè)量出的加速度向量（載體坐標(biāo)系下）

　　//

　　// 載體坐標(biāo)下的重力加速度向量，單位化。

　　float x_ac = x_q*z_q_2 - w_q*y_q_2;

　　float y_ac = y_q*z_q_2 + w_q*x_q_2; //通過(guò)四元數(shù)旋轉(zhuǎn)矩陣與地理坐標(biāo)系下的重力加速度向量［0 0 0 1］叉乘得到載體坐標(biāo)系下的重力加速度向量

　　float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;//（主要）角速度計(jì)測(cè)出的四元數(shù)表示的載體坐標(biāo)系下的重力加速度向量（這里已轉(zhuǎn)換成載體坐標(biāo)系下）

　　//

　　// 測(cè)量值與常量的叉積。

　　float x_ca = y_aa * z_ac - z_aa * y_ac;

　　float y_ca = z_aa * x_ac - x_aa * z_ac;

　　float z_ca = x_aa * y_ac - y_aa * x_ac;//角速度計(jì)測(cè)出的角度誤差，疊加的FACTOR大小可以實(shí)驗(yàn)試湊

　　//

　　// 構(gòu)造增量旋轉(zhuǎn)。

　　float delta_x = gyr［0］ * interval / 2 + x_ca * FACTOR;

　　float delta_y = gyr［1］ * interval / 2 + y_ca * FACTOR;

　　float delta_z = gyr［2］ * interval / 2 + z_ca * FACTOR;

　　//

　　// 融合，四元數(shù)乘法。

　　attitude-》w = w_q - x_q*delta_x - y_q*delta_y - z_q*delta_z;

　　attitude-》x = w_q*delta_x + x_q + y_q*delta_z - z_q*delta_y;

　　attitude-》y = w_q*delta_y - x_q*delta_z + y_q + z_q*delta_x;

　　attitude-》z = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q;

　　quaternion_normalize（attitude）;//歸一化

　　}