玩過MPU6050的朋友大概都有過這種經(jīng)歷明明小車放在桌面上靜止不動(dòng)串口打印出來的角度數(shù)據(jù)卻上下跳個(gè)不停像是傳感器自己在“發(fā)抖”。用這樣的數(shù)據(jù)去做自平衡車或者云臺穩(wěn)定結(jié)果就是電機(jī)不停抖動(dòng)、重心根本穩(wěn)不住。我也在這上面折騰過很長時(shí)間試過各種網(wǎng)上流傳的代碼最后才搞明白濾波這件事核心不是套用某個(gè)公式而是想清楚你手里的數(shù)據(jù)到底臟在哪里、你要的是哪一種“干凈”。這篇文章就基于stm32_mpu6050這個(gè)經(jīng)典組合把從原始數(shù)據(jù)到可用姿態(tài)角的完整鏈路捋一遍。內(nèi)容包括傳感器數(shù)據(jù)為什么會有噪聲、幾種常見濾波算法的原理和代碼、實(shí)際調(diào)參時(shí)的判斷方法以及一個(gè)綜合互補(bǔ)濾波的實(shí)用方案。適合剛接觸STM32和MPU6050的初學(xué)者也適合已經(jīng)跑通了例程但發(fā)現(xiàn)效果不理想、想系統(tǒng)搞懂濾波的開發(fā)者。1. MPU6050原始數(shù)據(jù)為什么會“抖”噪聲的來源與特性分析先說結(jié)論MPU6050輸出的原始值本身不是不能用的但它直接拿來做姿態(tài)控制時(shí)效果一定會讓你懷疑人生。這不是傳感器壞了也不一定是I2C讀取時(shí)序有問題而是數(shù)據(jù)里混著幾類性質(zhì)完全不同的干擾信號。1.1 傳感器噪聲白噪聲、隨機(jī)游走與固定偏差MPU6050內(nèi)部是MEMS結(jié)構(gòu)測量加速度和角速度依靠微小的硅結(jié)構(gòu)在慣性力下發(fā)生的形變再通過電容變化轉(zhuǎn)換成數(shù)字信號。這種微機(jī)械架構(gòu)決定了它對溫度、振動(dòng)、電源紋波都非常敏感。具體表現(xiàn)可以拆成三類第一類是白噪聲也就是高頻隨機(jī)抖動(dòng)。你把MPU6050放在桌面上用串口以100Hz的頻率打印原始加速度值會看到數(shù)據(jù)在某個(gè)均值附近來回跳跳動(dòng)的幅度通常在±0.02g左右。這類噪聲在頻域上表現(xiàn)為寬帶均勻分布屬于典型的隨機(jī)過程也是濾波算法要處理的主要對象。第二類是隨機(jī)游走也叫零偏不穩(wěn)定性。陀螺儀的零偏零輸入時(shí)的角速度輸出值不是固定不變的它會隨著時(shí)間緩慢漂移。比如剛上電時(shí)靜止?fàn)顟B(tài)的陀螺儀Z軸輸出是-1.2°/s通電半小時(shí)后可能漂到-2.5°/s。這種漂移是低頻的、緩慢的用高頻濾波器根本濾不掉需要在算法層面做補(bǔ)償。第三類是固定偏置也就是bias。每顆芯片出廠時(shí)的零偏都不完全一樣第一顆MPU6050的X軸加速度零偏可能是0.03g第二顆可能就是-0.02g。這個(gè)偏差可以通過上電靜止幾十次取平均值的方式估算出來然后在每次讀取時(shí)減去。1.2 運(yùn)動(dòng)中的測量誤差加速度計(jì)怕振動(dòng)、陀螺儀怕積分漂移傳感器的噪聲不是靜止時(shí)才有的真正麻煩的是運(yùn)動(dòng)過程中引入的誤差。加速度計(jì)測量的是“比力”也就是物體受到的加速度總矢量和。當(dāng)你的設(shè)備靜止時(shí)它測量的只有重力加速度g此時(shí)可以通過三角函數(shù)算出傾角。但設(shè)備一旦運(yùn)動(dòng)起來加速度計(jì)測到的不再是單純的重力分量而是疊加了運(yùn)動(dòng)加速度的混合值。這時(shí)候如果你直接用atan2(acc_y, acc_z)算角度得到的姿態(tài)會瞬間被運(yùn)動(dòng)加速度“帶跑”表現(xiàn)得非?！百\”——小車一加速角度就亂跳。陀螺儀的情況正好相反。陀螺儀測量角速度短期內(nèi)積分出來的角度非常平滑不會像加速度計(jì)那樣被運(yùn)動(dòng)加速度干擾。但它有一個(gè)致命問題積分會把零偏誤差也累加起來導(dǎo)致角度隨時(shí)間緩慢漂移。你可能只是把設(shè)備靜止放了一分鐘積分出來的角度已經(jīng)從0°漂到了20°。加速度計(jì)姿態(tài)“噪聲大但無漂移”陀螺儀姿態(tài)“短期平滑但長期漂移”這兩者的互補(bǔ)性正是后面所有濾波算法的根本出發(fā)點(diǎn)。1.3 數(shù)字層面的干擾I2C讀取與量化誤差除了傳感器本身的物理特性數(shù)據(jù)鏈路也會引入“假噪聲”。STM32通過I2C或SPI讀取MPU6050時(shí)如果使用了模擬I2C又沒有加延時(shí)控制讀到的數(shù)據(jù)可能偶發(fā)跳變。MPU6050的加速度計(jì)是16位ADC輸出如果配置的量程是±2g那么每個(gè)LSB代表的物理量是2g/32768≈0.000061g。聽起來分辨率非常高但實(shí)際系統(tǒng)的底噪遠(yuǎn)大于這個(gè)值低四位往往就是隨機(jī)的不必過度解讀。判斷抖動(dòng)是傳感器問題還是讀取問題有個(gè)簡單辦法在MPU6050靜止?fàn)顟B(tài)下連續(xù)讀500個(gè)加速度原始值如果數(shù)據(jù)呈近似高斯分布且重復(fù)試驗(yàn)時(shí)均值穩(wěn)定那就說明讀取鏈路沒有問題抖動(dòng)就來自傳感器本身濾波算法要解決的就是這個(gè)噪聲。2. 動(dòng)手之前先算清賬根據(jù)應(yīng)用場景決定濾波目標(biāo)很多教程上來就貼濾波代碼卻從不說清楚這個(gè)濾波器適合什么場景、截止頻率為什么取這個(gè)值。在我看來這是最要命的一個(gè)坑——你把別人調(diào)試好的參數(shù)抄過來但你的設(shè)備運(yùn)動(dòng)特性和別人的完全不同效果自然千差萬別。2.1 運(yùn)動(dòng)頻率與濾波截止頻率的關(guān)系每個(gè)實(shí)際系統(tǒng)都有自己的特征運(yùn)動(dòng)頻率。手持云臺上的人的抖動(dòng)頻率通常在1~10Hz無人機(jī)飛行時(shí)姿態(tài)變化的典型頻率在0.5~5Hz而自平衡小車的傾角修正頻率可以低到0.1~2Hz。一個(gè)性能合格的濾波器應(yīng)該讓“你要的姿態(tài)信號”無損通過同時(shí)把“你不要的噪聲”壓下去。這就是截止頻率要解決的事。我常用一個(gè)生活化類比來理解這個(gè)事你想從一碗粥里挑出紅棗但粥里混著米粒、水、碎渣。如果你用一個(gè)孔特別大的漏勺結(jié)果是紅棗和碎渣一起撈上來如果用一個(gè)孔特別小的濾網(wǎng)紅棗也被卡住了。濾波器就是在找一個(gè)合適的“孔徑”——它要匹配你要撈的東西的“尺寸”。對MPU6050的姿態(tài)數(shù)據(jù)來說你想要的姿態(tài)變化頻率是信號傳感器上的高頻抖動(dòng)和振動(dòng)是噪聲。設(shè)計(jì)濾波器時(shí)先估算信號最高頻率比如兩輪自平衡車的傾角控制周期是5ms傾角變化率一般不會超過2Hz那低通濾波器的截止頻率設(shè)在2Hz左右就相對安全。如果設(shè)在20Hz姿態(tài)響應(yīng)快了但噪聲也大量通過控制效果一定抖。2.2 不同應(yīng)用場景的濾波需求分級我把常見場景按對濾波的要求分成幾檔。如果你給無人機(jī)做姿態(tài)解算需要實(shí)時(shí)響應(yīng)性極好一階低通濾波的相位延遲就會比較傷腦筋需要用互補(bǔ)濾波或卡爾曼這類能提供“加速度計(jì)修正趨勢、陀螺儀提供短期動(dòng)態(tài)”的方案。如果只是做一個(gè)可視化角度計(jì)顯示當(dāng)前傾斜角度延遲幾百毫秒完全無所謂滑動(dòng)平均濾波就足夠了。如果做的是計(jì)步器關(guān)注的不是精確角度而是周期性特征那么重點(diǎn)應(yīng)該是帶通或閾值檢測而不是單純低通。很多初學(xué)者糾結(jié)“到底哪個(gè)濾波算法最好”但現(xiàn)實(shí)中算法沒有絕對的優(yōu)劣只有和“你要解決的具體任務(wù)”是否匹配。你得先回答一個(gè)問題這個(gè)數(shù)據(jù)是用來給人看、還是給控制回路用人眼可以接受一定延遲但控制回路對延遲和噪聲都極其敏感。這兩者的濾波策略會完全不同。3. 常見的濾波算法代碼實(shí)現(xiàn)與調(diào)參要點(diǎn)在這一節(jié)我給出幾種在STM32上跑過的濾波實(shí)現(xiàn)全部用C語言寫可以直接移植。涉及的變量名盡量保持通用方便你改到自己的工程里。3.1 滑動(dòng)窗口濾波簡單直接但延遲代價(jià)要認(rèn)滑動(dòng)窗口濾波也叫移動(dòng)平均是思路最樸素的一種保存最近N次傳感器的值每次輸出這N個(gè)值的平均值。窗口越大平滑效果越強(qiáng)但響應(yīng)也越遲鈍。#define FILTER_WINDOW_SIZE 16 float filter_buffer[FILTER_WINDOW_SIZE]; uint8_t filter_index 0; float filter_sum 0.0f; float sliding_window_filter(float new_value) { filter_sum - filter_buffer[filter_index]; filter_buffer[filter_index] new_value; filter_sum new_value; filter_index; if (filter_index FILTER_WINDOW_SIZE) { filter_index 0; } return filter_sum / FILTER_WINDOW_SIZE; }窗口大小怎么選我做過一組對比實(shí)驗(yàn)IMU靜止在桌面對Z軸陀螺儀的原始值分別做8點(diǎn)、16點(diǎn)和32點(diǎn)的滑動(dòng)窗口濾波。窗口越大輸出越平滑但給一個(gè)階躍輸入突然把傳感器旋轉(zhuǎn)90°時(shí)8點(diǎn)約80ms就能跟上32點(diǎn)則需要300ms左右。如果控制回路的周期是5ms這300ms的延遲意味著30多個(gè)控制周期你的姿態(tài)都還是“舊數(shù)據(jù)”輕則影響手感重則直接振蕩發(fā)散。所以滑動(dòng)窗口濾波只適合對延遲不敏感的場景比如顯示參數(shù)、記錄日志、離線分析。你要用它做實(shí)時(shí)反饋控制就得掂量一下延遲能不能接受。3.2 一階低通濾波輕量級數(shù)字濾波器和截止頻率計(jì)算一階低通濾波也叫指數(shù)移動(dòng)平均是最經(jīng)典的輕量濾波方式計(jì)算量極小適合STM32這類資源有限的MCU。算法核心是當(dāng)前輸出 上次輸出 × 系數(shù) 當(dāng)前輸入 ×1 - 系數(shù)用一個(gè)系數(shù)決定新舊數(shù)據(jù)的權(quán)重。#define LOWPASS_ALPHA 0.2f float lowpass_output 0.0f; float lowpass_filter(float new_value) { lowpass_output (1.0f - LOWPASS_ALPHA) * lowpass_output LOWPASS_ALPHA * new_value; return lowpass_output; }這個(gè)α系數(shù)和截止頻率之間是有公式的α 1 - exp(-2π × fc × dt)其中fc是期望的截止頻率單位Hzdt是采樣周期單位s。如果采樣頻率是100Hzdt0.01s想要截止頻率是5Hz代入計(jì)算α≈0.269。注意α越大說明當(dāng)前新數(shù)據(jù)的權(quán)重越高濾波器響應(yīng)越快平滑效果越弱α越小歷史數(shù)據(jù)的權(quán)重越大平滑更強(qiáng)但延遲也更大。我建議選α?xí)r不要拍腦袋取0.1或0.2這樣“看著順眼”的數(shù)而要根據(jù)你的采樣率和期望截止頻率算出來算完再結(jié)合實(shí)測微調(diào)。這里有個(gè)我自己的經(jīng)驗(yàn)先用公式算出理論值然后把它放大1.5~2倍再試因?yàn)閷?shí)際系統(tǒng)中采樣間隔往往不完全均勻I2C讀取時(shí)間會導(dǎo)致dt抖動(dòng)理論值會偏保守。3.3 卡爾曼濾波輕量一維實(shí)現(xiàn)工程中最常用的姿態(tài)濾波方案很多教程把卡爾曼濾波講得很玄乎但工程上最常用的其實(shí)是簡化成一維的版本。完整卡爾曼要算矩陣協(xié)方差即便STM32F103跑起來壓力不大但代碼復(fù)雜度和理解成本對多數(shù)項(xiàng)目來說并不劃算。做單軸角度濾波時(shí)一維卡爾曼就夠用。狀態(tài)量定義如下角度angle、角速度偏置bias。陀螺儀測量值作為控制輸入驅(qū)動(dòng)狀態(tài)預(yù)測加速度計(jì)計(jì)算出的角度作為觀測值做修正。typedef struct { float angle; // 角度估計(jì)值 float bias; // 陀螺儀零偏 float P[2][2]; // 誤差協(xié)方差 float Q_angle; // 角度噪聲協(xié)方差 float Q_bias; // 零偏噪聲協(xié)方差 float R_measure; // 觀測噪聲協(xié)方差 } Kalman_t; void kalman_init(Kalman_t *kalman) { kalman-angle 0.0f; kalman-bias 0.0f; kalman-P[0][0] 0.0f; kalman-P[0][1] 0.0f; kalman-P[1][0] 0.0f; kalman-P[1][1] 0.0f; kalman-Q_angle 0.001f; kalman-Q_bias 0.003f; kalman-R_measure 0.03f; } float kalman_get_angle(Kalman_t *kalman, float new_angle, float new_rate, float dt) { float rate new_rate - kalman-bias; kalman-angle dt * rate; kalman-P[0][0] dt * (dt * kalman-P[1][1] - kalman-P[0][1] - kalman-P[1][0] kalman-Q_angle); kalman-P[0][1] - dt * kalman-P[1][1]; kalman-P[1][0] - dt * kalman-P[1][1]; kalman-P[1][1] kalman-Q_bias * dt; float S kalman-P[0][0] kalman-R_measure; float K0 kalman-P[0][0] / S; float K1 kalman-P[1][0] / S; float y new_angle - kalman-angle; kalman-angle K0 * y; kalman-bias K1 * y; float P00_temp kalman-P[0][0]; float P01_temp kalman-P[0][1]; kalman-P[0][0] - K0 * P00_temp; kalman-P[0][1] - K0 * P01_temp; kalman-P[1][0] - K1 * P00_temp; kalman-P[1][1] - K1 * P01_temp; return kalman-angle; }卡爾曼里的三個(gè)噪聲協(xié)方差參數(shù)Q_angle、Q_bias、R_measure決定了濾波器的“性格”。Q_angle越大表示你對“模型預(yù)測角度”越不信任濾波器會更傾向于相信加速度計(jì)觀測值響應(yīng)快但輸出更嘈雜R_measure越大表示你覺得加速度計(jì)觀測噪聲越厲害濾波器會更相信陀螺儀預(yù)測平滑但延遲大。我的調(diào)參經(jīng)驗(yàn)是先把R_measure固定到0.01~0.1之間再調(diào)Q_angle觀察靜態(tài)下角度輸出是否抖動(dòng)適中然后轉(zhuǎn)動(dòng)傳感器看動(dòng)態(tài)響應(yīng)能否跟上而不產(chǎn)生明顯滯后。3.4 互補(bǔ)濾波加速度計(jì)和陀螺儀各取所長最推薦入門首選如果讓我給初次做姿態(tài)濾波的人推薦一個(gè)方案我會說互補(bǔ)濾波。它對參數(shù)不敏感效果穩(wěn)定代碼也簡單非常適合作為首個(gè)能“真正用起來”的姿態(tài)濾波算法?；パa(bǔ)濾波的思想是陀螺儀積分得到的角度高頻特性好但低頻會漂移加速度計(jì)計(jì)算的角度低頻準(zhǔn)確但高頻噪聲大。把兩者加權(quán)融合各取所長。權(quán)重由參數(shù)alpha決定alpha越大越信任陀螺儀角度越平滑但對加速度計(jì)的修正越慢。#define COMP_ALPHA 0.98f float comp_angle 0.0f; float complementary_filter(float acc_angle, float gyro_rate, float dt) { // 陀螺儀積分 comp_angle gyro_rate * dt; // 和加速度計(jì)角度互補(bǔ)融合 float weight 0.98f; // 越大越信任陀螺儀 comp_angle weight * comp_angle (1.0f - weight) * acc_angle; return comp_angle; }這里的0.98是經(jīng)驗(yàn)值意味著每一拍里只“相信”加速度計(jì)2%。假設(shè)采樣率是100Hz這個(gè)權(quán)重相當(dāng)于加速度計(jì)的修正時(shí)間常數(shù)約0.5秒也就是當(dāng)你靜止放置設(shè)備時(shí)大約0.5秒后濾波角度會被加速度計(jì)“拉”到正確的絕對值上——這個(gè)響應(yīng)速度對大多數(shù)人來說足夠。想深入理解這個(gè)參數(shù)的自動(dòng)調(diào)節(jié)邏輯可以去搜Mahony互補(bǔ)濾波的實(shí)現(xiàn)思路但初學(xué)階段手動(dòng)設(shè)一個(gè)固定權(quán)重完全夠用。4. 實(shí)測案例基于STM32F103C8T6的濾波前后對比與調(diào)參觀察在實(shí)際板子上驗(yàn)證一下濾波效果。我用的是一塊經(jīng)典的STM32F103C8T6最小系統(tǒng)板通過軟件模擬I2C也就是常說的GPIO口模擬時(shí)序連接MPU6050模塊采樣率設(shè)為100Hz。硬件接線很簡單PB6接SCLPB7接SDAVCC接3.3VGND接GND。測試場景分三種靜止桌面、快速翻轉(zhuǎn)、持續(xù)晃動(dòng)。每種場景分別記錄原始加速度計(jì)算角、滑動(dòng)窗口濾波輸出、互補(bǔ)濾波輸出。4.1 靜止?fàn)顟B(tài)濾波前后數(shù)據(jù)波動(dòng)對比把板子平放在桌面上靜止不動(dòng)分別采集500個(gè)數(shù)據(jù)點(diǎn)。原始角度直接用atan2計(jì)算的波動(dòng)范圍大約是±1.2°標(biāo)準(zhǔn)差約0.6°。這種波動(dòng)對于顯示來說尚可接受但如果直接作為PID控制器的輸入控制量會很不穩(wěn)定電機(jī)就會有明顯的嗡嗡聲。改用16點(diǎn)滑動(dòng)平均后波動(dòng)范圍縮小到±0.4°標(biāo)準(zhǔn)差約0.2°。改用互補(bǔ)濾波alpha0.98后靜態(tài)角度波動(dòng)范圍降到±0.3°以內(nèi)而且響應(yīng)仍然非常靈敏用手輕碰一下板子讓角度突變1°濾波輸出大約在0.1秒內(nèi)就跟蹤到位。所以如果你做的是靜態(tài)角度測量滑動(dòng)窗口就夠用如果要做實(shí)時(shí)控制必須上互補(bǔ)濾波或卡爾曼。4.2 動(dòng)態(tài)翻轉(zhuǎn)濾波器對劇烈運(yùn)動(dòng)的跟隨性能把板子拿在手里快速翻轉(zhuǎn)90°對比不同濾波器對階躍輸入的響應(yīng)?；瑒?dòng)窗口濾波的延遲明顯翻轉(zhuǎn)過程中濾波角度會“拖尾”大約需要150ms才能跟上真實(shí)角度互補(bǔ)濾波在翻轉(zhuǎn)瞬間也能快速跟隨但會略微過沖約2°隨后在幾十毫秒內(nèi)穩(wěn)定到真實(shí)角度。這個(gè)過沖來自加速度計(jì)的“慣性”——翻轉(zhuǎn)瞬間運(yùn)動(dòng)加速度分量混入加速度計(jì)測量值導(dǎo)致加速度計(jì)計(jì)算角瞬間跳變。互補(bǔ)濾波雖然給了加速度計(jì)較小權(quán)重但這個(gè)瞬間錯(cuò)誤還是會影響輸出。這也是為什么在強(qiáng)加速度場景比如無人機(jī)急速拉升、小車急加速下純互補(bǔ)濾波表現(xiàn)并不理想需要更復(fù)雜的算法來“察覺”并抑制這種運(yùn)動(dòng)加速度干擾。對于大多數(shù)入門項(xiàng)目兩輪車、云臺、機(jī)械臂互補(bǔ)濾波的這點(diǎn)過沖完全在可接受范圍內(nèi)不必為它過度設(shè)計(jì)。4.3 實(shí)際操作中的兩個(gè)小坑初始化姿態(tài)估計(jì)和數(shù)據(jù)幀間隔這里分享兩個(gè)在實(shí)測中踩過的小坑。第一個(gè)坑是初始化姿態(tài)估計(jì)。很多教程直接給濾波器狀態(tài)量賦0但如果設(shè)備上電時(shí)并不是水平放置的濾波輸出就會從一個(gè)錯(cuò)誤的初始值開始收斂導(dǎo)致前幾秒的角度輸出非常離譜。解決辦法很簡單上電后先連續(xù)讀50次加速度計(jì)取平均然后算出初始角度再把它作為卡爾曼或互補(bǔ)濾波的角度初值寫進(jìn)去。這樣一開機(jī)姿態(tài)就是靠譜的。第二個(gè)坑是數(shù)據(jù)幀間隔不均勻。STM32通過I2C讀取MPU6050并做串口打印時(shí)整個(gè)循環(huán)耗時(shí)不是固定值。如果用定時(shí)器中斷做采樣中斷里執(zhí)行I2C讀取、濾波、串口發(fā)送那么數(shù)據(jù)間隔相對穩(wěn)定如果你用while循環(huán)加delay(10)實(shí)際間隔可能從8ms到15ms不等。這種時(shí)間間隔抖動(dòng)對互補(bǔ)濾波和卡爾曼影響很大因?yàn)樗鼈兊念A(yù)測方程都依賴dt。我的建議是用一個(gè)固定頻率的定時(shí)器中斷作為采樣節(jié)拍濾波計(jì)算放在中斷里或者中斷置標(biāo)志位、主循環(huán)里處理但無論哪種方式都要保證兩次濾波之間的dt是穩(wěn)定值。我在實(shí)際工程里把采樣率設(shè)為1kHz并在MPU6050的FIFO配合下批量讀取數(shù)據(jù)然后降采樣到100Hz做濾波。這樣既保證了I2C讀取的時(shí)序穩(wěn)定性又保留了數(shù)據(jù)完整性效果比“定時(shí)中斷里慢慢讀”好很多但初學(xué)者不必一開始就上這個(gè)方案先把基礎(chǔ)流程跑通、數(shù)據(jù)穩(wěn)定再考慮更精細(xì)的時(shí)序控制。5. 濾波不只是濾鏡從濾波到姿態(tài)解算的完整鏈路前面講的都是對單一軸的獨(dú)立濾波實(shí)際操作中MPU6050輸出三軸加速度和三軸角速度要得到完整的橫滾角、俯仰角、偏航角還需要做“姿態(tài)解算”——也就是把多個(gè)軸的濾波結(jié)果融合成三維姿態(tài)。5.1 加速度計(jì)滾轉(zhuǎn)/俯仰角計(jì)算atan2的正確姿勢靜止或緩慢運(yùn)動(dòng)時(shí)橫滾角Roll和俯仰角Pitch可以直接用加速度計(jì)的三軸分量計(jì)算。注意這里一定要用atan2而不是atan因?yàn)閍tan2能根據(jù)輸入符號自動(dòng)判斷象限輸出完整的-π到π范圍角度。float roll atan2f(acc_y, acc_z) * 180.0f / 3.14159f; float pitch atan2f(-acc_x, sqrtf(acc_y * acc_y acc_z * acc_z)) * 180.0f / 3.14159f;這里有個(gè)細(xì)節(jié)值得解釋計(jì)算Pitch時(shí)分母用了sqrt(acc_y2 acc_z2)而不是直接用acc_z。這樣做的好處是當(dāng)pitch接近±90°時(shí)acc_z趨近于0直接用acc_z當(dāng)分母會導(dǎo)致數(shù)值爆炸。加上acc_y的分量后分母不會真的落到0計(jì)算結(jié)果也就穩(wěn)定了。這個(gè)公式只對“加速度計(jì)沒有運(yùn)動(dòng)加速度干擾”的假設(shè)成立。只要你的設(shè)備不是持續(xù)高速機(jī)動(dòng)這個(gè)假設(shè)在多數(shù)入門項(xiàng)目中是成立的。5.2 偏航角Yaw為什么不能用加速度計(jì)磁力計(jì)的必要性很多剛接觸的人會疑惑“我也是三軸加速度計(jì)為什么偏航角算不出來”原因很簡單加速度計(jì)測的是“重力方向”而重力方向是完全豎直的它不攜帶任何關(guān)于“繞豎直軸旋轉(zhuǎn)了多少度”的信息。你可以試著把設(shè)備平放在桌面上水平旋轉(zhuǎn)360°加速度計(jì)的三軸讀數(shù)幾乎不變因?yàn)闆]有重力分量變化可以用來計(jì)算yaw。想得到y(tǒng)aw需要引入磁力計(jì)電子羅盤來測量地磁場方向但這又會引入磁干擾、傾角補(bǔ)償?shù)纫欢研聠栴}。這就是為什么很多入門項(xiàng)目都只做Roll和Pitch兩軸姿態(tài)——對兩輪自平衡車、四軸飛行器的部分模式下這兩軸就足夠用了。如果你必須測yaw建議購買集成磁力計(jì)的九軸模塊比如MPU9250或ICM20948并單獨(dú)學(xué)習(xí)磁力計(jì)校準(zhǔn)流程。5.3 一個(gè)你可以直接抄作業(yè)的完整融合例程這里給出一個(gè)適用于STM32F103C8T6的基礎(chǔ)級完整例程框架把前面的知識點(diǎn)串起來。假設(shè)MPU6050的讀取函數(shù)已經(jīng)寫好定時(shí)器以5ms周期調(diào)用main_imu_update也就是200Hz采樣率。// 全局變量 Kalman_t kalman_roll, kalman_pitch; float roll_angle 0.0f, pitch_angle 0.0f; void imu_main_init(void) { mpu6050_init(); // 靜態(tài)采50次算初始角度 float acc_x_sum 0, acc_y_sum 0, acc_z_sum 0; for (uint8_t i 0; i 50; i) { mpu6050_read_raw(acc_x_raw, acc_y_raw, acc_z_raw, gyro_x_raw, gyro_y_raw, gyro_z_raw); acc_x_sum acc_x_raw; acc_y_sum acc_y_raw; acc_z_sum acc_z_raw; delay_ms(2); } float acc_x acc_x_sum / 50.0f / 16384.0f; float acc_y acc_y_sum / 50.0f / 16384.0f; float acc_z acc_z_sum / 50.0f / 16384.0f; float init_roll atan2f(acc_y, acc_z) * 57.2958f; float init_pitch atan2f(-acc_x, sqrtf(acc_y * acc_y acc_z * acc_z)) * 57.2958f; kalman_init(kalman_roll); kalman_init(kalman_pitch); kalman_roll.angle init_roll; kalman_pitch.angle init_pitch; } void main_imu_update(void) { int16_t acc_x_raw, acc_y_raw, acc_z_raw; // 實(shí)際類型按你的讀取函數(shù)修改 int16_t gyro_x_raw, gyro_y_raw, gyro_z_raw; mpu6050_read_raw(acc_x_raw, acc_y_raw, acc_z_raw, gyro_x_raw, gyro_y_raw, gyro_z_raw); // 加速度單位換算假設(shè)量程±2g所以除以16384 float acc_x acc_x_raw / 16384.0f; float acc_y acc_y_raw / 16384.0f; float acc_z acc_z_raw / 16384.0f; // 角速度單位換算假設(shè)量程±250°/s所以除以131.0 float gyro_x gyro_x_raw / 131.0f; float gyro_y gyro_y_raw / 131.0f; float gyro_z gyro_z_raw / 131.0f; float acc_roll atan2f(acc_y, acc_z) * 57.2958f; float acc_pitch atan2f(-acc_x, sqrtf(acc_y * acc_y acc_z * acc_z)) * 57.2958f; // dt為0.005s即5ms roll_angle kalman_get_angle(kalman_roll, acc_roll, gyro_x, 0.005f); pitch_angle kalman_get_angle(kalman_pitch, acc_pitch, gyro_y, 0.005f); }注意這里陀螺儀量程、加速度量程與實(shí)際配置必須一致否則算出來的物理數(shù)值是錯(cuò)的后續(xù)濾波再漂亮也沒有意義。我在調(diào)試時(shí)會先用串口把換算后的acc_x、gyro_x打印出來靜止應(yīng)該接近0減去零偏后垂直放置對應(yīng)值應(yīng)該約等于±1g確認(rèn)量換算正確后再跑濾波這個(gè)習(xí)慣能省一堆排查時(shí)間。6. 濾波參數(shù)怎么調(diào)一套可復(fù)用的調(diào)參方法與評價(jià)標(biāo)準(zhǔn)濾波算法的代碼實(shí)現(xiàn)不難難的是參數(shù)調(diào)節(jié)。不少讀者跑通代碼后發(fā)現(xiàn)自己調(diào)出來的效果還不如原始數(shù)據(jù)——這不是算法問題而是缺少一套“怎么調(diào)、調(diào)到什么程度算好”的方法論。6.1 先測清“噪聲底”每個(gè)系統(tǒng)都有自己的噪聲水平調(diào)參之前先把系統(tǒng)的底噪水平測清楚。方法很簡單設(shè)備靜止以實(shí)際工作采樣率采集原始加速度和角速度數(shù)據(jù)各采集1000個(gè)點(diǎn)計(jì)算標(biāo)準(zhǔn)差。標(biāo)準(zhǔn)差就是你的系統(tǒng)“噪聲底”。這個(gè)數(shù)值決定了濾波器的強(qiáng)度需求——如果加速度X軸靜止標(biāo)準(zhǔn)差是0.03g那濾波后標(biāo)準(zhǔn)差能降到0.01g就已經(jīng)是很好的效果不要指望能濾到0.001g那會導(dǎo)致嚴(yán)重延遲。記錄這個(gè)噪聲底以后做任何相關(guān)項(xiàng)目的濾波參數(shù)初值都能從這套歷史數(shù)據(jù)里找到起點(diǎn)而不是每次從零開始碰運(yùn)氣。6.2 給傳感器一個(gè)階躍信號觀察響應(yīng)速度制作一個(gè)簡易測試架把MPU6050固定在一根硬桿的一端手持另一端?？焖傩D(zhuǎn)90°并保持靜止記錄濾波輸出角度隨時(shí)間的變化曲線。觀察兩個(gè)指標(biāo)從開始旋轉(zhuǎn)到濾波輸出首次接近目標(biāo)值的時(shí)間延遲/上升時(shí)間。穩(wěn)定后濾波輸出與真實(shí)角度的誤差穩(wěn)態(tài)精度。用MATLAB、Python的matplotlib或者直接用STM32的串口發(fā)送數(shù)據(jù)到匿名上位機(jī)都能畫出曲線。我的判斷標(biāo)準(zhǔn)是濾波后角度的延遲時(shí)間不應(yīng)超過控制周期的3~5倍穩(wěn)態(tài)誤差應(yīng)在±0.5°以內(nèi)。如果延遲過長說明濾波器對傳感器的信任過度偏向了平滑側(cè)需要調(diào)大新數(shù)據(jù)權(quán)重如果穩(wěn)態(tài)誤差偏大且波動(dòng)劇烈說明平滑不足需要調(diào)大平滑權(quán)重。6.3 一份快速排查指南現(xiàn)象與參數(shù)對應(yīng)關(guān)系我整理了一張表格記錄常見現(xiàn)象和對應(yīng)的調(diào)參方向供大家對照參考現(xiàn)象可能原因調(diào)參方向靜止時(shí)濾波輸出波動(dòng)大平滑權(quán)重不夠增大滑動(dòng)窗口/減小低通α/增大卡爾曼R_measure動(dòng)態(tài)響應(yīng)太慢跟不上動(dòng)作平滑過度減小滑動(dòng)窗口/增大低通α/減小卡爾曼R_measure短時(shí)間快速抖動(dòng)高頻振動(dòng)耦合先加低通濾波去掉高頻分量再進(jìn)姿態(tài)濾波角度緩慢漂移陀螺儀零偏未補(bǔ)償上電初始化時(shí)估算零偏并減去翻轉(zhuǎn)瞬間角度突變運(yùn)動(dòng)加速度干擾增加加速度計(jì)權(quán)重降低或者使用更復(fù)雜的運(yùn)動(dòng)加速度補(bǔ)償靜止穩(wěn)定但一通電就振蕩濾波延遲過大與PID耦合降低PID增益同時(shí)減小濾波延遲先讓控制穩(wěn)住再談濾波這張表解決的是“現(xiàn)象→參數(shù)”的映射問題排查效率比逐個(gè)參數(shù)盲調(diào)高很多。7. 避坑集合MPU6050濾波路上最常見的幾個(gè)錯(cuò)誤認(rèn)知最后聊聊我見過最多的幾個(gè)錯(cuò)誤認(rèn)知這些坑幾乎每個(gè)初做MPU6050濾波的人都會踩一遍。7.1 “濾波算法越復(fù)雜越好”的誤解卡爾曼不是萬靈丹Mahony也不是。算法越復(fù)雜計(jì)算量越大參數(shù)越多越難調(diào)到工作良好的狀態(tài)。對兩輪小車這種應(yīng)用一階互補(bǔ)濾波已經(jīng)夠用對四旋翼這種高速動(dòng)態(tài)系統(tǒng)Mahony互補(bǔ)濾波通常是更均衡的選擇只有對精度和響應(yīng)要求都比較高的系統(tǒng)才值得上全狀態(tài)卡爾曼。先跑通最簡單的看實(shí)測數(shù)據(jù)再?zèng)Q定要不要升級算法。7.2 “濾波代碼抄過來就能用”的誤解很多人從網(wǎng)上抄一段濾波代碼按原樣粘貼到自己工程里期望立刻得到平滑數(shù)據(jù)。結(jié)果往往不理想。原因包括你的采樣頻率和原代碼作者不同dt錯(cuò)位會導(dǎo)致算法行為完全不同你的運(yùn)動(dòng)場景不同原始信號帶寬不同你用的傳感器量程不同換算系數(shù)不同。抄代碼可以但必須自己核對采樣周期、量程、換算、初始值。這四件事沒問題代碼才有意義。7.3 “濾波可以解決所有噪聲問題”的誤解濾波處理的只是數(shù)據(jù)層面的噪聲硬件層面的問題濾波幫不了忙。電源紋波太大、地線接觸不良、I2C上拉電阻太小導(dǎo)致信號變形、傳感器安裝在劇烈振動(dòng)的電機(jī)旁邊且沒有減震——這些問題在源頭上就搞臟了數(shù)據(jù)再好的濾波器也只能把垃圾數(shù)據(jù)濾成“更平滑的垃圾數(shù)據(jù)”。處理噪聲問題的優(yōu)先級應(yīng)該是先改善硬件、再接好地、再做濾波而不是一上來就琢磨濾波。以我自己的經(jīng)驗(yàn)每次遇到“數(shù)據(jù)抖得離譜”的情況第一反應(yīng)永遠(yuǎn)是排查接線、電源、安裝方式而不是調(diào)濾波。等硬件問題解決了濾波這個(gè)環(huán)節(jié)會比你想象的輕松得多。