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