如果你剛拿到一塊9軸IMU加速度計、陀螺儀、磁力計第一件事大概率是翻例程、找?guī)旌瘮?shù)先把數(shù)據(jù)流讀出來。讀數(shù)據(jù)其實不難真正卡住人的是姿態(tài)解算陀螺儀積分出來的角度幾分鐘就開始飄加速度計稍微一動就全是毛刺磁力計在室內(nèi)也被環(huán)境磁場帶得六親不認(rèn)。要把這三路數(shù)據(jù)融合成一個穩(wěn)定可用的姿態(tài)卡爾曼濾波器至今是最經(jīng)典、最能解釋清楚的方案。這篇文章我會從三個傳感器的誤差特性講起把狀態(tài)建模、Matlab代碼實現(xiàn)和調(diào)參經(jīng)驗一次說透適合正在做機(jī)器人姿態(tài)估計、慣性導(dǎo)航、平衡控制或者只是想把傳感器數(shù)據(jù)真正用起來的朋友參考。1. 三個傳感器各自的脾氣為什么單獨用任何一個都不夠1.1 加速度計測的是比力不是傾斜角度很多新手拿到加速度計的第一反應(yīng)是直接用反正切函數(shù)算俯仰角和橫滾角。這個思路對了一半但前提是傳感器嚴(yán)格靜止。加速度計輸出的本質(zhì)是比力也就是重力與運動加速度的矢量和在傳感器坐標(biāo)系下的投影。靜止時運動加速度為零輸出恰好是重力矢量所以可以通過三分量反推姿態(tài)。但一旦平臺在運動哪怕是勻速直線運動后再加一個輕微振動輸出的方向就不再等于重力方向直接反推角度自然就錯了。我見過不少調(diào)試場景把傳感器放在桌子上靜止讀數(shù)俯仰角很穩(wěn)一到手上輕微晃動角度輸出就跳得不成樣子。這其實是物理特性決定的不是傳感器壞了。加速度計對角速度變化不敏感對線加速度和振動極其敏感這是它最大的短板。另一個容易被忽略的點是加速度計的帶寬和噪聲。消費級MEMS加速度計的輸出噪聲通常在mg級別配合低通濾波還能接受但如果你把采樣率拉高到1kHz以上又不做濾波角度估計就會明顯抖動。所以加速度計適合做長期趨勢參考不適合做瞬時姿態(tài)。1.2 陀螺儀微分量的好處是響應(yīng)快壞處是積分必漂陀螺儀輸出的是角速度需要做積分才能得到角度變化。它的優(yōu)勢非常明顯動態(tài)響應(yīng)快不受線加速度干擾短時間內(nèi)的角度增量非常準(zhǔn)確。但問題也出在積分上。陀螺儀的誤差模型通常包含三部分固定零偏、隨時間緩慢波動的零偏漂移、以及白噪聲。零偏哪怕只有0.5度/秒積分一分鐘就是30度這還不算隨機(jī)游走帶來的額外誤差。我之前帶過一個同學(xué)A他一開始沒做陀螺儀零偏估計直接拿原始數(shù)據(jù)積分靜止?fàn)顟B(tài)下角度從0漂到十幾度他還以為是傳感器壞了。后來把靜止?fàn)顟B(tài)的一萬組數(shù)據(jù)取平均把零偏減掉漂移立刻小了一個數(shù)量級。固定零偏是最好處理的誤差真正麻煩的是隨溫度變化的那部分這也是為什么需要濾波器在線估計零偏而不是只在初始化時扣一次。陀螺儀在姿態(tài)解算里的角色是短期預(yù)測器高頻姿態(tài)變化靠它因為它在動態(tài)下最可信。1.3 磁力計唯一的航向參考也是最脆弱的傳感器磁力計輸出的是環(huán)境磁場矢量在傳感器坐標(biāo)系下的分量。在地球表面地磁場可以近似為一個方向基本固定的矢量水平分量指向磁北所以通過磁力計可以確定航向角。但磁力計的脆弱程度遠(yuǎn)超大多數(shù)人的預(yù)期。室內(nèi)鋼筋、電機(jī)、揚(yáng)聲器、大電流導(dǎo)線、甚至桌子上的金屬筆記本都會疊加一個額外的磁場。我實測過在一個普通實驗室角落磁力計讀數(shù)比開闊室外偏了將近15度。這說明磁力計的數(shù)據(jù)如果不做校準(zhǔn)和限幅在卡爾曼濾波里反而會幫倒忙。另外要明確一點磁力計要算出航向角必須先知道傳感器當(dāng)前的俯仰和橫滾角也就是要做傾斜補(bǔ)償。而傾斜補(bǔ)償又要依賴加速度計的姿態(tài)參考。所以磁力計和加速度計是深度耦合的任何一個出問題都會污染航向。1.4 從頻域看三個傳感器的互補(bǔ)邏輯把三個傳感器放在頻域里看就非常清晰陀螺儀在高頻段可信因為它的輸出是微分不受運動加速度影響但低頻段積分漂移嚴(yán)重。加速度計和磁力計在低頻段可信因為它們在長時間尺度上的趨勢是穩(wěn)定的但高頻段容易混入振動和運動干擾。所以姿態(tài)估計的問題本質(zhì)上是如何讓高頻可信的陀螺儀數(shù)據(jù)主導(dǎo)短時變化同時讓低頻可信的加速度計和磁力計持續(xù)修正陀螺儀的漂移?？柭鼮V波器做的事情正是這個用統(tǒng)計學(xué)上的協(xié)方差去決定當(dāng)前該更相信誰。2. 姿態(tài)數(shù)學(xué)歐拉角、旋轉(zhuǎn)矩陣與四元數(shù)怎么選2.1 歐拉角直觀但工程上是個坑歐拉角用三個角度表示姿態(tài)直觀易懂。但工程上用它做卡爾曼濾波非常難受。首先是萬向鎖問題當(dāng)俯仰角達(dá)到正負(fù)90度時橫滾和航向退化到同一個自由度姿態(tài)解算會突然失穩(wěn)。其次是三角函數(shù)帶來的非線性在狀態(tài)方程里做預(yù)測時需要反復(fù)算三角函數(shù)既慢又容易在極端角度下出錯。有人可能覺得我這輩子做的東西不會跑到90度俯仰。但現(xiàn)實中機(jī)器人翻身、無人機(jī)大動態(tài)機(jī)動、手持設(shè)備亂甩都會觸發(fā)這個問題。姿態(tài)濾波的通用性要求我們必須選一種全局無奇異點的表示方法。2.2 四元數(shù)的核心公式與物理意義四元數(shù)可以理解為一個旋轉(zhuǎn)軸加一個旋轉(zhuǎn)角的編碼形式是一個四維單位向量 q [q0, q1, q2, q3]其中模長恒為1。用四元數(shù)表示姿態(tài)沒有萬向鎖問題運算也只需要乘法和加法非常適合嵌入式和Matlab原型驗證。四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣的公式是姿態(tài)解算里的基礎(chǔ)。以下代碼約定旋轉(zhuǎn)矩陣 R 表示導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的旋轉(zhuǎn)即載體系向量 R * 導(dǎo)航系向量。function R quat2rot(q) q0 q(1); q1 q(2); q2 q(3); q3 q(4); R [q0^2q1^2-q2^2-q3^2, 2*(q1*q2-q0*q3), 2*(q1*q3q0*q2); 2*(q1*q2q0*q3), q0^2-q1^2q2^2-q3^2, 2*(q2*q3-q0*q1); 2*(q1*q3-q0*q2), 2*(q2*q3q0*q1), q0^2-q1^2-q2^2q3^2]; end四元數(shù)微分方程是卡爾曼濾波預(yù)測步的核心q_dot 0.5 * q ? omega其中omega是角速度構(gòu)造的四元數(shù)[0, wx, wy, wz]。離散化后就是狀態(tài)轉(zhuǎn)移公式具體會在下一章給出。2.3 坐標(biāo)系約定東北天坐標(biāo)系與載體系坐標(biāo)系的約定直接決定公式里的符號是新手最容易翻車的地方。我統(tǒng)一使用右手直角坐標(biāo)系導(dǎo)航坐標(biāo)系N取東-北-天也就是X東、Y北、Z上載體坐標(biāo)系B取右-前-上即X右、Y前、Z上。在這個約定下重力矢量在導(dǎo)航系中表示為[0; 0; -1]如果加速度計輸出歸一化到g單位。磁力計在導(dǎo)航系中的參考矢量是地磁場方向水平分量指向磁北垂直分量指向地面方向具體數(shù)值因緯度而異。這個約定和很多開源項目不完全一致所以看別人代碼時一定要先搞清楚坐標(biāo)系否則會出現(xiàn)靜止時角度正確一旋轉(zhuǎn)就發(fā)散的詭異現(xiàn)象。三種姿態(tài)表示方式對比如下表方便你直接做選型判斷表示方式維度奇異性計算復(fù)雜度適合濾波歐拉角3萬向鎖低不適合方向余弦矩陣9無高冗余多四元數(shù)4無低非常適合3. 卡爾曼濾波器的狀態(tài)建模融合的核心在設(shè)計狀態(tài)方程3.1 為什么狀態(tài)向量是七維而不是三維常見的卡爾曼濾波器狀態(tài)向量我選擇七維四元數(shù)4維加陀螺儀零偏3維。加零偏的原因很實際陀螺儀零偏不是固定值會隨溫度和時間緩慢變化。如果不在線估計它角速度預(yù)測就會一直帶一個未知偏差導(dǎo)致四元數(shù)預(yù)測持續(xù)漂移。把零偏納入狀態(tài)后濾波器會在運行過程中自動估計并修正它相當(dāng)于免費獲得了一個自適應(yīng)零偏補(bǔ)償。初始的零偏可以用靜止數(shù)據(jù)的均值來估計但溫度變化后會再次偏掉所以在線估計是必要的。3.2 狀態(tài)方程四元數(shù)微分方程與零偏的慢變假設(shè)狀態(tài)向量定義為x [q0, q1, q2, q3, bgx, bgy, bgz]^T其中q是姿態(tài)四元數(shù)bg是陀螺儀零偏。系統(tǒng)的連續(xù)時間狀態(tài)方程為q_dot 0.5 * q ? (omega_meas - bg) bg_dot 0零偏的導(dǎo)數(shù)設(shè)為零表示它在一個采樣周期內(nèi)基本不變變化由系統(tǒng)噪聲驅(qū)動。這樣建模后預(yù)測步中先用修正后的角速度更新四元數(shù)然后做歸一化。離散化時把四元數(shù)微分方程近似為q_{k1} (I 0.5 * Omega * dt) * q_k其中Omega是由角速度構(gòu)造的4x4反對稱矩陣function Omega buildOmega(omega) wx omega(1); wy omega(2); wz omega(3); Omega [0, -wx, -wy, -wz; wx, 0, wz, -wy; wy, -wz, 0, wx; wz, wy, -wx, 0]; end預(yù)測步的Matlab實現(xiàn)如下function [q_pred, bg_pred, P_pred] predict(q, bg, omega_meas, dt, Q) omega_corr omega_meas - bg; Omega 0.5 * buildOmega(omega_corr); F_q eye(4) Omega * dt; q_pred F_q * q; q_pred q_pred / norm(q_pred); bg_pred bg; F blkdiag(F_q, eye(3)); P_pred F * P * F Q; end這里F是7x7的狀態(tài)轉(zhuǎn)移矩陣Q是系統(tǒng)噪聲協(xié)方差矩陣后面調(diào)參章節(jié)會詳細(xì)講它的設(shè)置。3.3 觀測方程為什么把加速度計和磁力計當(dāng)作參考矢量卡爾曼濾波最關(guān)鍵的部分在于觀測方程。我們不用加速度計輸出反推的歐拉角作為觀測而是直接把測量矢量與預(yù)測矢量做差。這樣做的原因有兩個一是避免三角函數(shù)和角度的非線性包裝二是矢量觀測在數(shù)學(xué)上天然無縫。加速度計的觀測方程是傳感器坐標(biāo)系下的加速度計測量值 R(q)^T * g_N 噪聲其中g(shù)_N是導(dǎo)航系重力矢量[0; 0; -1]。這里R(q)^T把導(dǎo)航系矢量旋轉(zhuǎn)到載體系。磁力計的觀測方程類似傳感器坐標(biāo)系下的磁場測量值 R(q)^T * m_N 噪聲其中m_N是導(dǎo)航系下的地磁場參考矢量由校準(zhǔn)階段測得。如果采用完整三維磁力計觀測殘差的相位偏差會同時污染橫滾和俯仰。所以我更推薦一個簡化做法先利用加速度計修正后的姿態(tài)把磁力計數(shù)據(jù)旋轉(zhuǎn)到水平面再只取水平分量計算航向殘差用這個殘差去修正狀態(tài)向量中的航向相關(guān)部分。這樣做可以把磁力計的干擾限制在航向維度不會把橫滾俯仰帶歪。3.4 標(biāo)準(zhǔn)卡爾曼濾波的五個公式在本項目中的映射卡爾曼濾波的五個核心公式在項目里的具體維度如下預(yù)測x_pred F * x 過程噪聲P_pred F * P * F Q更新K P_pred * H * (H * P_pred * H R)^-1x x_pred K * (z - h)P (I - K * H) * P_pred其中 z 是傳感器實測的加速度矢量或磁場矢量h 是用當(dāng)前四元數(shù)預(yù)測出的對應(yīng)矢量H 是觀測方程的雅可比矩陣。H 矩陣在實際代碼中可以先用解析推導(dǎo)也可以借助Matlab符號工具箱生成。解析過程的核心是對四元數(shù)的每個分量求偏導(dǎo)雖然推導(dǎo)繁瑣但好處是計算速度快適合實時性要求高的場景。4. Matlab實現(xiàn)核心代碼與逐步驗證4.1 數(shù)據(jù)準(zhǔn)備單位統(tǒng)一與時間戳處理從傳感器讀出的原始數(shù)據(jù)通常不是標(biāo)準(zhǔn)單位。陀螺儀可能是度/秒加速度計可能是原始ADC計數(shù)磁力計可能是任意量程的磁場強(qiáng)度。Matlab里調(diào)試的第一步就是把所有數(shù)據(jù)統(tǒng)一到國際單位角速度轉(zhuǎn)成弧度/秒加速度計轉(zhuǎn)成g或m/s^2磁力計歸一化到單位向量。時間戳是另一個容易踩坑的地方。如果數(shù)據(jù)是等間隔采樣的直接用固定dt即可。但如果數(shù)據(jù)來自異步讀取每一幀的時間戳都不一樣就必須逐幀計算真實dt否則預(yù)測步的積分長度就會和實際時間不匹配濾波器必然震蕩。4.2 初始化四元數(shù)初值、協(xié)方差矩陣、Q和R初始四元數(shù)可以由初始靜止階段的加速度計和磁力計數(shù)據(jù)反推出來。最簡單的方式先利用加速度計求俯仰和橫滾角再利用磁力計求航向角然后把這三個歐拉角轉(zhuǎn)成四元數(shù)。Matlab里有現(xiàn)成的angle2quat函數(shù)可以直接用。協(xié)方差矩陣P的初始值不用太糾結(jié)給一個中等數(shù)量級的對角矩陣即可比如0.01乘以單位陣。濾波器會在幾步之內(nèi)自動收斂P給得太小反而會讓初期的觀測修正被壓制。Q和R的初始值我在下一章詳細(xì)講這里先給出一個能跑通的配法Q diag([0.001, 0.001, 0.001, 0.001, 0.005, 0.005, 0.005]); R_acc eye(3) * 0.05; R_mag eye(3) * 0.5;4.3 濾波器主循環(huán)的代碼形態(tài)在Matlab里主循環(huán)的核心框架大致如下。這個框架省略了部分中間變量的邊界處理但勝在邏輯清晰便于理解后再優(yōu)化。N length(t); q init_quat; bg zeros(3,1); P eye(7) * 0.01; for k 1:N dt t(k) - t(k-1); % 預(yù)測步 [q, bg, P] predict(q, bg, gyro(:,k), dt, Q); % 加速度計更新 g_N [0; 0; -1]; z_acc acc_norm(:,k); R_NB quat2rot(q); h_acc R_NB * g_N; H_acc computeJac(q, acc); K P * H_acc / (H_acc * P * H_acc R_acc); q q K * (z_acc - h_acc); q q / norm(q); P (eye(7) - K * H_acc) * P; % 磁力計更新航向殘差方式 z_mag mag_norm(:,k); h_mag R_NB * m_N; H_mag computeJac(q, mag); K_mag P * H_mag / (H_mag * P * H_mag R_mag); q q K_mag * (z_mag - h_mag); q q / norm(q); P (eye(7) - K_mag * H_mag) * P; % 提取歐拉角用于顯示和記錄 euler(:,k) quat2eul(q, ZYX); end這里computeJac是數(shù)值雅可比或者解析雅可比。調(diào)試階段可以用有限差分?jǐn)?shù)值雅可比來驗證解析推導(dǎo)是否正確實測中解析法性能更好。有一點必須強(qiáng)調(diào)四元數(shù)在每次更新后都要歸一化。很多人跑著跑著姿態(tài)突然發(fā)散八成是四元數(shù)模長悄悄偏離了1誤差協(xié)方差P被帶入了一個不合理的狀態(tài)最后整個矩陣崩掉。4.4 驗證流程靜態(tài)穩(wěn)定、動態(tài)響應(yīng)、航向精度調(diào)試卡爾曼濾波器我建議按下面三個順序來每一步都做記錄再進(jìn)入下一步。第一步是靜態(tài)測試。把傳感器固定在桌面上靜止3分鐘記錄輸出的歐拉角波動范圍。正常情況下橫滾和俯仰的波動應(yīng)該小于1度航向的漂移小于1度。如果航向漂移明顯優(yōu)先排查磁力計校準(zhǔn)。第二步是動態(tài)響應(yīng)測試。把傳感器繞某個軸快速旋轉(zhuǎn)90度再回到原位觀察濾波器是否跟得上有沒有明顯滯后或超調(diào)。滯后一般說明Q給得太小系統(tǒng)噪聲被低估預(yù)測過于自信。第三步是長時間漂移測試。放置在桌面運行半小時以上觀察航向和水平姿態(tài)是否有緩慢漂移。這一步能暴露陀螺儀零偏估計是否收斂、磁力計參考矢量是否正確。我在實際調(diào)試時還常用一個土辦法拿手機(jī)上的水平儀功能做對照。雖然手機(jī)有內(nèi)置的算法但躺著不動的情況下作為參考已經(jīng)足夠精確。5. 調(diào)參與避坑Q矩陣、R矩陣、采樣率和校準(zhǔn)那些事5.1 Q和R矩陣的物理含義數(shù)字背后是傳感器的噪聲水平調(diào)參是卡爾曼濾波器最容易被玄學(xué)化的部分。其實Q和R的物理意義非常明確Q是系統(tǒng)模型的協(xié)方差表示你對狀態(tài)方程的信任程度R是觀測噪聲的協(xié)方差表示你對傳感器的信任程度。Q越大濾波器越激進(jìn)響應(yīng)越快但噪聲越大R越大濾波器越平滑但滯后越明顯。初始值設(shè)置有個實操套路先采集傳感器靜止時的數(shù)據(jù)計算加速度計和磁力計各軸的方差作為R對角元的參考值。Q中的角速度白噪聲項可以參考傳感器數(shù)據(jù)手冊中的噪聲密度換算成噪聲方差。零偏隨機(jī)游走項沒有現(xiàn)成公式從0.0001數(shù)量級開始試觀察航向漂移的表現(xiàn)逐漸調(diào)整。調(diào)參順序也很重要。先把R固定住只調(diào)Q再把Q固定住小幅調(diào)R。兩者同時調(diào)會導(dǎo)致無法定位問題。5.2 磁力計校準(zhǔn)為何是航向精度的前提不校準(zhǔn)的磁力計數(shù)據(jù)在卡爾曼濾波里不僅無益反而有害。硬磁干擾來自傳感器附近的固定磁場源表現(xiàn)為各個方向測量值的中心偏移。軟磁干擾來自鐵磁性材料對磁力線的扭曲表現(xiàn)為橢圓畸變。完整的校準(zhǔn)流程是采集空間多個方向的磁場數(shù)據(jù)擬合出一個橢球然后做中心化和縮放。簡化版的校準(zhǔn)做法把傳感器在空間里轉(zhuǎn)幾圈記錄所有方向上的磁場模長。理想情況下模長應(yīng)該恒定。如果模長在300到500之間波動說明有顯著干擾。校準(zhǔn)后應(yīng)該把磁力計數(shù)據(jù)歸一化讓參考矢量的模長等于1。在實際項目中我在不同房間測試過校準(zhǔn)效果。校準(zhǔn)后在開闊走廊航向精度能達(dá)到2度以內(nèi)同一套參數(shù)拿到布滿金屬桌的實驗室誤差直接放大到8度以上。這說明磁力計更新在某些環(huán)境下還不如不加調(diào)濾波器時要有這個心理預(yù)期。5.3 采樣率、時間戳與dt的坑采樣率對濾波器性能的影響非常直接。陀螺儀在高頻下積分更準(zhǔn)確所以預(yù)測步頻率越高越好。但觀測更新步受限于加速度計和磁力計的噪聲水平頻率太高反而不穩(wěn)定。常見的做法是預(yù)測步跑到1kHz觀測步降到100Hz也就是所謂的多速率卡爾曼濾波。時間戳的坑我只說一個真實經(jīng)歷。有一次我把Matlab仿真里的dt寫成了固定值0.01但實際數(shù)據(jù)采集的間隔是0.009到0.011波動的結(jié)果濾波器在靜態(tài)下也出現(xiàn)了周期性波動。問題就出在固定dt和真實時間不匹配。后來改成逐幀計算真實dt波動立刻消失。這個細(xì)節(jié)特別隱蔽建議大家一上來就用真實時間戳。5.4 三個高頻踩坑點與排查思路第一個坑是觀測殘差符號反了。加速度計參考矢量的方向定義不同或者旋轉(zhuǎn)矩陣轉(zhuǎn)置寫反都會導(dǎo)致濾波器把殘差往錯誤方向修正表現(xiàn)為姿態(tài)迅速發(fā)散。排查方法是靜止時打印出預(yù)測值h和實測值z看兩者的方向是否一致。如果不一致優(yōu)先檢查R矩陣的方向和g_N的符號。第二個坑是協(xié)方差矩陣P失去對稱性。P理論上永遠(yuǎn)是對稱正定矩陣但在浮點運算下反復(fù)的矩陣乘法會讓對稱性慢慢丟失最終導(dǎo)致濾波發(fā)散。解決辦法是在每次更新后強(qiáng)制對稱化P (P P) / 2;第三個坑是四元數(shù)更新后忘記歸一化或者顯示歐拉角時遇到90度附近的跳躍。前者是真正的算法錯誤后者只是顯示層的問題。歸一化要放在殘差修正之后、下一次預(yù)測之前。而歐拉角顯示跳躍是萬向鎖的正常表現(xiàn)不代表濾波器壞了不要誤判成算法bug。我自己在后來的項目中逐漸把卡爾曼濾波的實現(xiàn)固定成一套標(biāo)準(zhǔn)流程靜止估零偏和R矩陣、實時時間戳、預(yù)測觀測分頻率、每次更新后強(qiáng)制歸一化和對稱化。這套流程幫我省掉了大量排查時間也基本覆蓋了九軸IMU姿態(tài)解算里的絕大部分坑。回到最開始的問題九軸IMU融合并沒有太多神秘感。它就是搞清楚三個傳感器的誤差特性然后用協(xié)方差矩陣去權(quán)衡每個時刻該相信誰。把陀螺儀當(dāng)作高頻預(yù)測器把加速度計和磁力計當(dāng)作低頻修正器卡爾曼濾波的全部邏輯就通了。如果你正在做姿態(tài)解算我建議先不要急著把代碼跑起來去看角度曲線而是先做靜態(tài)測試把零偏和R矩陣的初值確認(rèn)好再逐層加入動態(tài)和磁力計更新。最后留一個小技巧測試時在桌面上繞固定軸轉(zhuǎn)幾圈然后回到原始位置看航向是否能回零這一步能幫你快速暴露絕大多數(shù)方向符號錯誤。