這次我們來(lái)看一個(gè)在嵌入式、機(jī)器人、傳感器數(shù)據(jù)處理領(lǐng)域繞不開的話題低通濾波與互補(bǔ)濾波。這兩個(gè)概念聽起來(lái)可能有些理論化但它們的核心價(jià)值非常直接如何從充滿噪聲的傳感器數(shù)據(jù)中提取出我們真正關(guān)心的、平滑穩(wěn)定的信號(hào)。無(wú)論是四軸飛行器的姿態(tài)解算、平衡小車的角度估計(jì)還是任何需要融合陀螺儀和加速度計(jì)數(shù)據(jù)的場(chǎng)景你幾乎都會(huì)遇到它們。這篇文章的重點(diǎn)不是推導(dǎo)復(fù)雜的數(shù)學(xué)公式而是從工程師的視角出發(fā)講清楚這兩種濾波方法到底是什么、各自解決什么問(wèn)題、在代碼里怎么實(shí)現(xiàn)、以及在實(shí)際項(xiàng)目中如何選擇和調(diào)參。我們會(huì)用最直白的語(yǔ)言和可運(yùn)行的代碼示例讓你能快速理解原理并立刻應(yīng)用到自己的項(xiàng)目中。如果你正在為傳感器數(shù)據(jù)抖動(dòng)、融合結(jié)果漂移而頭疼這篇文章可以直接收藏備用。1. 核心能力速覽在深入細(xì)節(jié)之前我們先通過(guò)一個(gè)表格快速把握低通濾波和互補(bǔ)濾波的核心定位與差異這有助于你快速判斷在什么場(chǎng)景下該用誰(shuí)。能力項(xiàng)低通濾波 (Low-Pass Filter, LPF)互補(bǔ)濾波 (Complementary Filter)核心目標(biāo)濾除信號(hào)中的高頻噪聲保留低頻趨勢(shì)。融合兩個(gè)或多個(gè)傳感器的數(shù)據(jù)取長(zhǎng)補(bǔ)短得到一個(gè)更優(yōu)的估計(jì)。處理對(duì)象單一信號(hào)源如一個(gè)傳感器的原始數(shù)據(jù)。多個(gè)信號(hào)源經(jīng)典應(yīng)用陀螺儀 加速度計(jì)。關(guān)鍵參數(shù)截止頻率、時(shí)間常數(shù)、濾波系數(shù)α。融合系數(shù)通常也是一個(gè)類似α的權(quán)重。輸出特點(diǎn)響應(yīng)滯后但非常平滑。動(dòng)態(tài)響應(yīng)好來(lái)自陀螺儀長(zhǎng)期穩(wěn)定來(lái)自加速度計(jì)。典型應(yīng)用平滑ADC采樣值、去除按鍵抖動(dòng)、平滑控制信號(hào)。IMU姿態(tài)估計(jì)如MPU6050數(shù)據(jù)處理、數(shù)據(jù)融合。實(shí)現(xiàn)復(fù)雜度極低一階IIR濾波只需一行代碼。較低核心算法也就幾行代碼。資源消耗幾乎可忽略幾個(gè)變量一次乘加運(yùn)算。低需要讀取多個(gè)傳感器并進(jìn)行簡(jiǎn)單運(yùn)算。簡(jiǎn)單來(lái)說(shuō)低通濾波是“凈化”一個(gè)信號(hào)而互補(bǔ)濾波是“組合”多個(gè)信號(hào)。接下來(lái)我們將從原理到實(shí)踐一步步拆解。2. 適用場(chǎng)景與使用邊界理解適用場(chǎng)景是正確使用工具的第一步。盲目套用公式往往得不到好結(jié)果。2.1 低通濾波何時(shí)該用何時(shí)不該用適合使用低通濾波的場(chǎng)景傳感器數(shù)據(jù)平滑例如超聲波測(cè)距傳感器的單次讀數(shù)跳動(dòng)很大但實(shí)際距離變化是連續(xù)的。使用LPF可以平滑讀數(shù)用于顯示或低速控制。去除信號(hào)毛刺在數(shù)字采樣中偶爾會(huì)因干擾出現(xiàn)尖峰脈沖。一個(gè)合適的LPF可以有效抑制這種毛刺?？刂菩盘?hào)濾波PID控制器的輸出可能因測(cè)量噪聲而產(chǎn)生高頻抖動(dòng)直接作用于執(zhí)行機(jī)構(gòu)如電機(jī)可能導(dǎo)致震動(dòng)或噪音。對(duì)控制量進(jìn)行低通濾波可以使輸出更柔和。按鍵消抖可以看作一種特殊的數(shù)字低通濾波通過(guò)多次采樣判斷穩(wěn)定狀態(tài)。低通濾波的局限性使用邊界相位滯后這是LPF最根本的缺點(diǎn)。濾波后的信號(hào)在時(shí)間上會(huì)落后于原始信號(hào)。截止頻率越低濾波越狠滯后越嚴(yán)重。在需要快速響應(yīng)的實(shí)時(shí)控制系統(tǒng)中這種滯后可能導(dǎo)致系統(tǒng)不穩(wěn)定。無(wú)法區(qū)分噪聲與有用高頻信號(hào)LPF會(huì)無(wú)差別地衰減所有高頻分量。如果你的有用信號(hào)本身包含高頻成分例如振動(dòng)分析LPF會(huì)將其一并濾除造成信號(hào)失真。對(duì)階躍響應(yīng)差當(dāng)輸入信號(hào)發(fā)生突變時(shí)LPF的輸出會(huì)緩慢地“爬”向新值而不是立刻跟隨。結(jié)論低通濾波適用于對(duì)實(shí)時(shí)性要求不高但要求數(shù)據(jù)平滑穩(wěn)定的場(chǎng)景。如果系統(tǒng)對(duì)響應(yīng)速度要求苛刻需要慎用或選用截止頻率很高的LPF。2.2 互補(bǔ)濾波融合的藝術(shù)互補(bǔ)濾波的經(jīng)典與最佳應(yīng)用場(chǎng)景IMU姿態(tài)估計(jì)。這是互補(bǔ)濾波的“殺手級(jí)”應(yīng)用。它完美解決了單一傳感器的缺陷陀螺儀通過(guò)積分角速度得到角度。短期精度高動(dòng)態(tài)響應(yīng)好無(wú)滯后。但缺點(diǎn)是由于積分誤差漂移會(huì)隨時(shí)間累積長(zhǎng)期穩(wěn)定性極差角度會(huì)慢慢“飄走”。加速度計(jì)通過(guò)測(cè)量重力加速度分量可以計(jì)算出姿態(tài)角滾轉(zhuǎn)、俯仰。長(zhǎng)期絕對(duì)穩(wěn)定沒有累積誤差。但對(duì)運(yùn)動(dòng)加速度非常敏感短期動(dòng)態(tài)響應(yīng)極差在物體運(yùn)動(dòng)時(shí)計(jì)算的角度噪聲很大?；パa(bǔ)濾波的思想非常巧妙用高通濾波器處理陀螺儀積分角度濾除其低頻漂移用低通濾波器處理加速度計(jì)計(jì)算的角度濾除其高頻噪聲然后將兩者相加。因?yàn)椤案咄ā薄暗屯ā薄叭ā崩碚撋纤苑Q之為“互補(bǔ)”。適合使用互補(bǔ)濾波的場(chǎng)景任何需要融合高頻特性好和低頻特性好的傳感器的場(chǎng)景。IMU是最典型的例子。低成本姿態(tài)估計(jì)在不需要絕對(duì)高精度如工業(yè)級(jí)AHRS的場(chǎng)合如玩具無(wú)人機(jī)、平衡車、自穩(wěn)云臺(tái)互補(bǔ)濾波是性價(jià)比最高的選擇。需要兼顧響應(yīng)速度和穩(wěn)定性的數(shù)據(jù)融合?；パa(bǔ)濾波的局限性系數(shù)需要精心調(diào)節(jié)融合系數(shù)α的選擇至關(guān)重要需要在實(shí)際系統(tǒng)中調(diào)試。對(duì)加速度干擾敏感雖然加速度計(jì)的角度被低通濾波但如果存在持續(xù)的非重力加速度如車輛勻加速仍然會(huì)導(dǎo)致估計(jì)角度出現(xiàn)偏差。這不是互補(bǔ)濾波獨(dú)有的問(wèn)題是所有基于加速度計(jì)的姿態(tài)估計(jì)的共性問(wèn)題。無(wú)法替代更復(fù)雜的算法對(duì)于高動(dòng)態(tài)、高精度的應(yīng)用如專業(yè)無(wú)人機(jī)、慣性導(dǎo)航需要卡爾曼濾波、Mahony或Madgwick等更復(fù)雜的非線性濾波算法。結(jié)論互補(bǔ)濾波是入門多傳感器數(shù)據(jù)融合最簡(jiǎn)單、最有效的方法。它在響應(yīng)速度和長(zhǎng)期穩(wěn)定性之間取得了優(yōu)秀的平衡且計(jì)算量極小非常適合資源受限的嵌入式系統(tǒng)。3. 環(huán)境準(zhǔn)備與前置條件本文的講解和代碼示例不依賴于特定的硬件或復(fù)雜的庫(kù)重點(diǎn)在于算法原理和代碼實(shí)現(xiàn)。你只需要一個(gè)能運(yùn)行C/C或Python的環(huán)境即可?；A(chǔ)軟件環(huán)境C/C環(huán)境用于嵌入式開發(fā)理解。例如Keil、IAR、Arduino IDE、PlatformIO或者簡(jiǎn)單的GCC編譯器。我們將展示適用于單片機(jī)的代碼。Python環(huán)境可選但推薦用于算法仿真和快速驗(yàn)證。建議安裝Python 3.6以及NumPy和Matplotlib庫(kù)。# 安裝Python數(shù)據(jù)分析與繪圖庫(kù) pip install numpy matplotlib數(shù)據(jù)來(lái)源你可以使用真實(shí)傳感器如MPU6050通過(guò)串口打印的數(shù)據(jù)。自己模擬生成的帶噪聲數(shù)據(jù)。我們下文提供的仿真代碼。硬件準(zhǔn)備如果想連接真實(shí)傳感器一塊開發(fā)板如STM32、ESP32、Arduino。一個(gè)IMU模塊如MPU6050它同時(shí)包含陀螺儀和加速度計(jì)。連接線如I2C或SPI線。核心知識(shí)準(zhǔn)備基本的編程知識(shí)。了解傳感器如陀螺儀、加速度計(jì)數(shù)據(jù)的基本含義原始值、量程、單位轉(zhuǎn)換。對(duì)“噪聲”、“漂移”、“頻率”有直觀概念。4. 一階低通濾波器的原理與實(shí)現(xiàn)我們從最簡(jiǎn)單的、也是工程中最常用的一階低通濾波器開始。它在數(shù)字域的實(shí)現(xiàn)被稱為一階無(wú)限脈沖響應(yīng)濾波器。4.1 公式推導(dǎo)與直觀理解一階低通濾波在連續(xù)域的傳遞函數(shù)是H(s) 1 / (τs 1)其中τ是時(shí)間常數(shù)。 在數(shù)字域我們通常使用迭代的形式這避免了復(fù)雜的數(shù)學(xué)運(yùn)算Y(n) α * X(n) (1 - α) * Y(n-1)參數(shù)解釋Y(n)本次濾波后的輸出值。Y(n-1)上一次濾波后的輸出值需要保存的狀態(tài)。X(n)本次采集到的原始輸入值。α濾波系數(shù)取值范圍(0, 1]。α 越大濾波效果越弱響應(yīng)快但不平滑α 越小濾波效果越強(qiáng)非常平滑但滯后嚴(yán)重。這個(gè)公式在做什么它實(shí)際上是在做加權(quán)平均。本次的輸出是“本次輸入”和“上次輸出”的加權(quán)平均。α決定了你更相信新的測(cè)量值X(n)還是更相信歷史濾波值Y(n-1)。α 與截止頻率的關(guān)系假設(shè)采樣周期為T期望的截止頻率為fc則α ≈ 2πfcT / (2πfcT 1)或者更常用的近似公式α T / (T τ)其中τ 1 / (2πfc)。簡(jiǎn)單記憶α越小截止頻率fc越低濾波越“狠”。4.2 C語(yǔ)言實(shí)現(xiàn)嵌入式友好以下代碼可以直接嵌入到你的單片機(jī)項(xiàng)目中。// 一階低通濾波器結(jié)構(gòu)體 typedef struct { float alpha; // 濾波系數(shù) α float last_out; // 上一次的輸出值 Y(n-1) } LPF_1st; // 初始化濾波器 void LPF_1st_Init(LPF_1st *filter, float alpha, float init_value) { filter-alpha alpha; filter-last_out init_value; // 通常用第一次的采樣值初始化 } // 執(zhí)行一次濾波計(jì)算 float LPF_1st_Update(LPF_1st *filter, float input) { float output; output filter-alpha * input (1.0f - filter-alpha) * filter-last_out; filter-last_out output; // 更新狀態(tài) return output; } // 使用示例 int main() { LPF_1st my_filter; float raw_data, filtered_data; // 初始化假設(shè)采樣周期T0.01s(100Hz)期望截止頻率fc5Hz // 計(jì)算 alpha T / (T 1/(2*pi*fc)) 0.01 / (0.01 1/(2*3.14*5)) ≈ 0.24 LPF_1st_Init(my_filter, 0.24, 0.0f); // 初始輸出設(shè)為0 while(1) { raw_data read_sensor(); // 假設(shè)的讀取傳感器函數(shù) filtered_data LPF_1st_Update(my_filter, raw_data); // 使用 filtered_data 進(jìn)行后續(xù)處理或控制 delay(10); // 延遲10ms保持100Hz采樣率 } return 0; }4.3 Python仿真與效果觀察讓我們用Python生成一個(gè)帶噪聲的信號(hào)然后用一階低通濾波處理它直觀感受α的影響。import numpy as np import matplotlib.pyplot as plt # 生成仿真信號(hào)一個(gè)1Hz的正弦波 高頻噪聲 fs 100 # 采樣頻率 100 Hz t np.arange(0, 3, 1/fs) # 3秒時(shí)間軸 signal_clean np.sin(2 * np.pi * 1 * t) # 1Hz 干凈正弦波 noise 0.5 * np.sin(2 * np.pi * 20 * t) 0.2 * np.random.randn(len(t)) # 20Hz噪聲隨機(jī)噪聲 signal_noisy signal_clean noise # 一階低通濾波函數(shù) def low_pass_filter_1st(data, alpha): filtered np.zeros_like(data) filtered[0] data[0] # 用第一個(gè)值初始化 for i in range(1, len(data)): filtered[i] alpha * data[i] (1 - alpha) * filtered[i-1] return filtered # 測(cè)試不同的alpha值 alphas [0.9, 0.3, 0.1, 0.05] # α越大濾波越弱 filtered_signals [] for alpha in alphas: filtered_signals.append(low_pass_filter_1st(signal_noisy, alpha)) # 繪圖 plt.figure(figsize(12, 8)) plt.subplot(2, 1, 1) plt.plot(t, signal_clean, g-, linewidth2, labelClean Signal (1Hz)) plt.plot(t, signal_noisy, r-, alpha0.6, labelNoisy Signal) plt.title(Original Signals) plt.xlabel(Time [s]) plt.ylabel(Amplitude) plt.legend() plt.grid(True) plt.subplot(2, 1, 2) plt.plot(t, signal_clean, g-, linewidth2, labelClean Signal) for i, alpha in enumerate(alphas): plt.plot(t, filtered_signals[i], labelfFiltered (α{alpha})) plt.title(Low-Pass Filtering Effect with Different α) plt.xlabel(Time [s]) plt.ylabel(Amplitude) plt.legend() plt.grid(True) plt.tight_layout() plt.show()運(yùn)行這段代碼你會(huì)清晰地看到α0.9濾波效果微弱輸出幾乎緊跟噪聲信號(hào)滯后很小。α0.3噪聲被明顯抑制正弦波輪廓顯現(xiàn)有輕微滯后。α0.1信號(hào)非常平滑但滯后相位偏移已經(jīng)很明顯波峰波谷明顯落后于原始信號(hào)。α0.05極度平滑滯后非常嚴(yán)重信號(hào)幅度也有所衰減。調(diào)試建議在實(shí)際項(xiàng)目中α沒有固定最優(yōu)值。你需要根據(jù)信號(hào)的最高有用頻率和你希望濾除的噪聲頻率來(lái)權(quán)衡。通常從0.1~0.3開始嘗試觀察效果后再微調(diào)。5. 互補(bǔ)濾波器的原理與實(shí)現(xiàn)以IMU為例理解了低通濾波互補(bǔ)濾波就水到渠成了。我們以最經(jīng)典的融合陀螺儀和加速度計(jì)計(jì)算角度為例。5.1 算法流程拆解互補(bǔ)濾波的核心公式如下以俯仰角angle為例// 偽代碼流程 1. 讀取陀螺儀Y軸角速度 gyro_y乘以時(shí)間 dt 得到角度增量。 gyro_angle_delta gyro_y * dt; 2. 用加速度計(jì)數(shù)據(jù)計(jì)算當(dāng)前俯仰角 acc_angle。 // 假設(shè)加速度計(jì)數(shù)據(jù)已歸一化且傳感器坐標(biāo)系正確 acc_angle atan2(acc_x, sqrt(acc_y*acc_y acc_z*acc_z)) * 180 / PI; 3. 執(zhí)行互補(bǔ)濾波融合。 // 公式angle α * (angle gyro_angle_delta) (1 - α) * acc_angle // 但更常見的寫法是 angle angle gyro_angle_delta; // 1. 用陀螺儀積分進(jìn)行預(yù)測(cè) angle angle α * (acc_angle - angle); // 2. 用加速度計(jì)測(cè)量值進(jìn)行修正 4. 輸出融合后的角度 angle。關(guān)鍵點(diǎn)解釋第一步陀螺儀積分利用陀螺儀動(dòng)態(tài)響應(yīng)好的特性對(duì)上一時(shí)刻的角度進(jìn)行“預(yù)測(cè)”。angle gyro_angle_delta可以看作一個(gè)對(duì)角度的高通濾波過(guò)程因?yàn)樗蕾囉谧兓课⒎謺?huì)逐漸遺忘舊值即抑制陀螺儀的低頻漂移。第二步加速度計(jì)修正利用加速度計(jì)長(zhǎng)期穩(wěn)定的特性對(duì)預(yù)測(cè)值進(jìn)行“修正”。(acc_angle - angle)是加速度計(jì)測(cè)量值與當(dāng)前估計(jì)值的偏差。用系數(shù)α通常很小如0.01將這個(gè)偏差緩慢地“拉”回來(lái)。這可以看作一個(gè)對(duì)加速度計(jì)角度的低通濾波過(guò)程濾除了其高頻噪聲。系數(shù) α 的角色這里的α與低通濾波中的α意義類似但更直接地代表了你更信任加速度計(jì)還是陀螺儀。α越小如0.001修正越緩慢系統(tǒng)更信任陀螺儀的動(dòng)態(tài)性能但加速度計(jì)糾正漂移的速度慢。α越大如0.1修正越快系統(tǒng)更信任加速度計(jì)的絕對(duì)基準(zhǔn)但更容易受加速度計(jì)高頻噪聲影響。5.2 C語(yǔ)言實(shí)現(xiàn)嵌入式實(shí)戰(zhàn)代碼以下是適用于MPU6050等IMU的互補(bǔ)濾波C語(yǔ)言實(shí)現(xiàn)。#include math.h // 用于 atan2, sqrt 函數(shù) // 互補(bǔ)濾波器結(jié)構(gòu)體 typedef struct { float angle; // 融合后的角度估計(jì)值 float bias; // 陀螺儀零偏估計(jì)可選用于動(dòng)態(tài)校準(zhǔn) float alpha; // 互補(bǔ)濾波系數(shù) (通常很小如 0.01) float dt; // 采樣周期單位秒 (例如 0.01 對(duì)應(yīng) 100Hz) } ComplementaryFilter; // 初始化濾波器 void CF_Init(ComplementaryFilter *cf, float alpha, float dt, float init_angle) { cf-angle init_angle; cf-bias 0.0f; // 初始零偏設(shè)為0 cf-alpha alpha; cf-dt dt; } // 更新濾波器輸入為陀螺儀角速度(度/秒)和加速度計(jì)計(jì)算的角度(度) float CF_Update(ComplementaryFilter *cf, float gyro_rate, float acc_angle) { float angle_delta; // 可選簡(jiǎn)單的陀螺儀零偏動(dòng)態(tài)估計(jì)在靜止時(shí)效果較好 // cf-bias cf-bias cf-alpha * 0.1f * (acc_angle - cf-angle); // 緩慢估計(jì)零偏 // gyro_rate - cf-bias; // 補(bǔ)償零偏 // 1. 陀螺儀積分預(yù)測(cè) angle_delta gyro_rate * cf-dt; cf-angle angle_delta; // 2. 加速度計(jì)修正更新 // 這是互補(bǔ)濾波的核心用加速度計(jì)的角度緩慢糾正陀螺儀積分的漂移 cf-angle cf-angle cf-alpha * (acc_angle - cf-angle); return cf-angle; } // 從加速度計(jì)原始數(shù)據(jù)計(jì)算俯仰角 (單位度) // 假設(shè)傳感器放置X軸向前Y軸向左Z軸向上 // acc_x, acc_y, acc_z: 加速度計(jì)讀數(shù)已轉(zhuǎn)換為重力加速度g單位 float calculate_pitch_from_accel(float acc_x, float acc_y, float acc_z) { // 使用 atan2 函數(shù)避免除零錯(cuò)誤計(jì)算范圍在 -90° 到 90° 之間 // 注意這是簡(jiǎn)化計(jì)算實(shí)際需要考慮傳感器安裝方向 return atan2(-acc_x, sqrt(acc_y * acc_y acc_z * acc_z)) * 180.0f / M_PI; } // 主循環(huán)示例 int main() { ComplementaryFilter pitch_filter; float gyro_y, acc_x, acc_y, acc_z; float acc_pitch, fused_pitch; // 初始化系數(shù)α0.01采樣周期dt0.01s(100Hz)初始角度0 CF_Init(pitch_filter, 0.01f, 0.01f, 0.0f); while(1) { // 1. 讀取傳感器數(shù)據(jù)假設(shè)函數(shù)已實(shí)現(xiàn) read_mpu6050(gyro_y, acc_x, acc_y, acc_z); // gyro_y: 俯仰角速度 // 2. 從加速度計(jì)計(jì)算俯仰角 acc_pitch calculate_pitch_from_accel(acc_x, acc_y, acc_z); // 3. 執(zhí)行互補(bǔ)濾波融合 fused_pitch CF_Update(pitch_filter, gyro_y, acc_pitch); // 4. 使用 fused_pitch 進(jìn)行控制或輸出 // printf(Pitch: %.2f deg\n, fused_pitch); delay(10); // 保持100Hz循環(huán) } return 0; }5.3 Python仿真對(duì)比純積分、純加速度計(jì)與互補(bǔ)濾波讓我們用Python模擬一個(gè)IMU的運(yùn)動(dòng)過(guò)程對(duì)比三種角度估計(jì)方法的優(yōu)劣。import numpy as np import matplotlib.pyplot as plt # 仿真參數(shù) fs 100.0 # 采樣頻率 100 Hz dt 1.0 / fs # 采樣周期 t np.arange(0, 10, dt) # 10秒仿真時(shí)間 # 1. 生成真實(shí)的俯仰角變化模擬物體先抬起后放下 true_pitch 30 * np.sin(2 * np.pi * 0.2 * t) # 0.2Hz的正弦變化幅度30度 # 2. 生成真實(shí)的角速度真實(shí)角速度是真實(shí)角度的微分 true_gyro np.gradient(true_pitch, dt) # 單位度/秒 # 3. 模擬傳感器數(shù)據(jù)添加噪聲和誤差 # 陀螺儀數(shù)據(jù)真實(shí)角速度 固定零偏 隨機(jī)噪聲 gyro_bias 0.5 # 度/秒的零偏模擬溫漂 gyro_noise 0.1 # 度/秒的隨機(jī)噪聲 gyro_measured true_gyro gyro_bias gyro_noise * np.random.randn(len(t)) # 加速度計(jì)數(shù)據(jù)當(dāng)姿態(tài)角為pitch時(shí)重力在各軸的分量 運(yùn)動(dòng)加速度噪聲 # 假設(shè)只有重力且傳感器X軸向前Z軸向上。則 acc_x -g * sin(pitch), acc_z g * cos(pitch) g 9.8 acc_x_ideal -g * np.sin(np.deg2rad(true_pitch)) acc_z_ideal g * np.cos(np.deg2rad(true_pitch)) # 添加高頻振動(dòng)噪聲模擬機(jī)體振動(dòng) vibration_noise 2.0 * np.sin(2 * np.pi * 15 * t) 0.5 * np.random.randn(len(t)) acc_x_measured acc_x_ideal vibration_noise acc_z_measured acc_z_ideal 0.1 * np.random.randn(len(t)) # Z軸噪聲小一些 # 從加速度計(jì)測(cè)量值反算角度這就是加速度計(jì)直接計(jì)算的角度噪聲很大 acc_pitch_measured np.arctan2(-acc_x_measured, acc_z_measured) * 180 / np.pi # 4. 三種估計(jì)方法 # 方法A純陀螺儀積分會(huì)漂移 pitch_gyro_only np.zeros_like(t) pitch_gyro_only[0] true_pitch[0] # 假設(shè)初始角度已知 for i in range(1, len(t)): pitch_gyro_only[i] pitch_gyro_only[i-1] gyro_measured[i] * dt # 方法B純加速度計(jì)角度噪聲大 pitch_acc_only acc_pitch_measured # 方法C互補(bǔ)濾波融合 alpha 0.02 # 融合系數(shù) pitch_complementary np.zeros_like(t) pitch_complementary[0] true_pitch[0] for i in range(1, len(t)): # 陀螺儀積分預(yù)測(cè) pitch_complementary[i] pitch_complementary[i-1] gyro_measured[i] * dt # 加速度計(jì)修正 pitch_complementary[i] pitch_complementary[i] alpha * (acc_pitch_measured[i] - pitch_complementary[i]) # 5. 繪圖對(duì)比 plt.figure(figsize(14, 10)) plt.subplot(3, 1, 1) plt.plot(t, true_pitch, k-, linewidth3, labelTrue Pitch) plt.plot(t, pitch_gyro_only, r-, labelGyro Only (Integral)) plt.title(Gyro-Only Integration: Drift Over Time) plt.ylabel(Angle [deg]) plt.legend() plt.grid(True) plt.subplot(3, 1, 2) plt.plot(t, true_pitch, k-, linewidth3, labelTrue Pitch) plt.plot(t, pitch_acc_only, b-, alpha0.7, labelAccelerometer Only) plt.title(Accelerometer-Only: Noisy but No Drift) plt.ylabel(Angle [deg]) plt.legend() plt.grid(True) plt.subplot(3, 1, 3) plt.plot(t, true_pitch, k-, linewidth3, labelTrue Pitch) plt.plot(t, pitch_complementary, g-, linewidth2, labelfComplementary Filter (α{alpha})) plt.title(Complementary Filter: Best of Both Worlds) plt.xlabel(Time [s]) plt.ylabel(Angle [deg]) plt.legend() plt.grid(True) plt.tight_layout() plt.show() # 計(jì)算并打印誤差的均方根(RMS) def rms_error(estimate, true): return np.sqrt(np.mean((estimate - true)**2)) print( 角度估計(jì)誤差對(duì)比 (RMS, 單位度) ) print(f純陀螺儀積分誤差: {rms_error(pitch_gyro_only, true_pitch):.2f} deg) print(f純加速度計(jì)誤差: {rms_error(pitch_acc_only, true_pitch):.2f} deg) print(f互補(bǔ)濾波誤差: {rms_error(pitch_complementary, true_pitch):.2f} deg)運(yùn)行仿真你會(huì)得到三張對(duì)比圖純陀螺儀積分初期跟隨很好但隨著時(shí)間的推移由于零偏的存在角度估計(jì)值會(huì)線性漂移最終完全偏離真實(shí)值。純加速度計(jì)沒有漂移問(wèn)題平均值圍繞真實(shí)值上下波動(dòng)但噪聲非常大高頻抖動(dòng)無(wú)法用于直接控制?；パa(bǔ)濾波它完美地結(jié)合了前兩者的優(yōu)點(diǎn)。曲線平滑抑制了加速度計(jì)的高頻噪聲同時(shí)又沒有明顯的長(zhǎng)期漂移用加速度計(jì)修正了陀螺儀的積分誤差。RMS誤差也會(huì)顯示互補(bǔ)濾波是最低的。這個(gè)仿真清晰地展示了為什么互補(bǔ)濾波在低成本IMU應(yīng)用中如此受歡迎。6. 進(jìn)階話題與參數(shù)整定掌握了基本實(shí)現(xiàn)后我們討論幾個(gè)工程實(shí)踐中的關(guān)鍵問(wèn)題。6.1 如何選擇融合系數(shù) αα是互補(bǔ)濾波器的“靈魂”選擇不當(dāng)效果大打折扣。經(jīng)驗(yàn)法則α通常在0.001到0.1之間。調(diào)試方法靜態(tài)測(cè)試將傳感器靜止放置觀察融合角度是否漂移。如果角度緩慢漂移說(shuō)明α太小加速度計(jì)的修正力太弱無(wú)法拉住陀螺儀的漂移。應(yīng)適當(dāng)增大α。動(dòng)態(tài)測(cè)試快速晃動(dòng)傳感器觀察融合角度是否跟得上真實(shí)運(yùn)動(dòng)。如果響應(yīng)遲鈍感覺“粘滯”說(shuō)明α太大系統(tǒng)過(guò)于信任緩慢的加速度計(jì)抑制了陀螺儀的快速響應(yīng)。應(yīng)適當(dāng)減小α。折中原則在靜態(tài)不漂移和動(dòng)態(tài)跟得上的前提下選擇一個(gè)盡可能小的α。因?yàn)楦〉摩烈馕吨鴮?duì)加速度計(jì)的高頻噪聲抑制得更好。自適應(yīng) α更高級(jí)的實(shí)現(xiàn)可以根據(jù)運(yùn)動(dòng)狀態(tài)動(dòng)態(tài)調(diào)整α。例如通過(guò)檢測(cè)加速度計(jì)數(shù)據(jù)的總矢量大小sqrt(ax^2ay^2az^2)是否接近重力加速度g來(lái)判斷當(dāng)前是否處于線性運(yùn)動(dòng)狀態(tài)。如果處于劇烈運(yùn)動(dòng)狀態(tài)總加速度遠(yuǎn)大于g則減小α更信任陀螺儀如果處于靜止或勻速狀態(tài)則增大α更信任加速度計(jì)。6.2 采樣周期 dt 的重要性dt必須是實(shí)際兩次調(diào)用濾波函數(shù)的時(shí)間間隔而不是你期望的周期。不準(zhǔn)確的dt會(huì)導(dǎo)致積分錯(cuò)誤。在嵌入式系統(tǒng)中最好使用定時(shí)器中斷來(lái)固定采樣周期。如果是在主循環(huán)中需要用系統(tǒng)時(shí)鐘精確計(jì)算從上一次更新到現(xiàn)在的實(shí)際耗時(shí)。在PC或樹莓派上可以使用高精度時(shí)鐘函數(shù)如time.perf_counter()in Python來(lái)測(cè)量實(shí)際耗時(shí)。dt與α的關(guān)系在公式angle gyro * dt中dt直接影響積分量。如果dt不準(zhǔn)確即使α調(diào)得再好效果也會(huì)變差。確保你的dt穩(wěn)定且準(zhǔn)確。6.3 初始化與初始角度濾波器啟動(dòng)時(shí)angle的初始值很重要。一個(gè)好的初始值可以加快收斂。簡(jiǎn)單方法上電后保持傳感器靜止1-2秒用這幾秒的加速度計(jì)數(shù)據(jù)的平均值來(lái)計(jì)算初始角度。因?yàn)殪o止時(shí)加速度計(jì)數(shù)據(jù)最可靠。直接賦值angle calculate_pitch_from_accel(ax, ay, az);6.4 擴(kuò)展到三維姿態(tài)滾轉(zhuǎn)、俯仰、偏航上面的例子只計(jì)算了俯仰角。對(duì)于完整的四軸飛行器需要滾轉(zhuǎn)Roll、俯仰Pitch和偏航Y(jié)aw。滾轉(zhuǎn)和俯仰都可以用上述互補(bǔ)濾波方法分別使用對(duì)應(yīng)的陀螺儀角速度gyro_x,gyro_y和從加速度計(jì)計(jì)算出的滾轉(zhuǎn)/俯仰角。偏航角Yaw加速度計(jì)無(wú)法提供偏航角的絕對(duì)參考因?yàn)橹亓Ψ较驘o(wú)法感知繞垂直軸的旋轉(zhuǎn)。因此互補(bǔ)濾波不能直接用于偏航角。通常做法只用陀螺儀積分計(jì)算偏航角并接受其隨時(shí)間漂移對(duì)于短時(shí)間飛行或需要磁力計(jì)校正。引入磁力計(jì)用磁力計(jì)提供偏航角的絕對(duì)參考然后對(duì)陀螺儀積分和磁力計(jì)角度再做一次互補(bǔ)濾波。這就是“六軸IMU加速度計(jì)陀螺儀”升級(jí)為“九軸IMU磁力計(jì)”的主要原因。7. 常見問(wèn)題與排查方法在實(shí)際部署中你可能會(huì)遇到以下問(wèn)題。這里提供一個(gè)排查清單。問(wèn)題現(xiàn)象可能原因排查方式解決方案角度估計(jì)持續(xù)單向漂移1. 陀螺儀存在零偏。2. 互補(bǔ)濾波系數(shù)α太小。1. 傳感器靜止觀察陀螺儀原始輸出是否接近0。2. 靜態(tài)測(cè)試看融合角度是否緩慢漂移。1. 增加軟件零偏校準(zhǔn)程序上電靜止時(shí)采樣平均。2. 適當(dāng)增大α。角度響應(yīng)遲鈍感覺“滯后”1. 互補(bǔ)濾波系數(shù)α太大。2. 采樣周期dt不準(zhǔn)確或太長(zhǎng)。3. 傳感器數(shù)據(jù)輸出速率低。1. 快速晃動(dòng)傳感器觀察角度曲線是否“跟不上”。2. 打印或測(cè)量實(shí)際的dt值。1. 適當(dāng)減小α。2. 確保dt是精確的時(shí)間差。3. 提高傳感器采樣率。角度噪聲大不停抖動(dòng)1. 加速度計(jì)噪聲大如機(jī)體振動(dòng)。2. 互補(bǔ)濾波系數(shù)α太大過(guò)多引入了加速度計(jì)的高頻噪聲。1. 觀察靜止時(shí)加速度計(jì)計(jì)算出的角度是否跳動(dòng)。2. 觀察融合角度波形。1. 對(duì)加速度計(jì)原始數(shù)據(jù)先做低通濾波。2. 減小α。3. 改善傳感器安裝減震?？焖龠\(yùn)動(dòng)時(shí)角度估計(jì)錯(cuò)誤物體存在線加速度破壞了加速度計(jì)測(cè)量重力的前提。檢測(cè)總加速度sqrt(ax^2ay^2az^2)是否遠(yuǎn)大于或小于g。1. 實(shí)現(xiàn)自適應(yīng)α運(yùn)動(dòng)時(shí)減小對(duì)加速度計(jì)的信任。2. 對(duì)于高動(dòng)態(tài)應(yīng)用考慮更復(fù)雜的算法如卡爾曼濾波。代碼運(yùn)行一段時(shí)間后角度“飛了”1. 變量溢出如角度超過(guò)360度未歸一化。2. 數(shù)值計(jì)算錯(cuò)誤如使用了單精度浮點(diǎn)導(dǎo)致累積誤差。3. 傳感器數(shù)據(jù)讀取錯(cuò)誤。1. 檢查角度變量范圍。2. 打印關(guān)鍵變量的中間值。3. 檢查傳感器通信是否正常。1. 對(duì)角度進(jìn)行周期歸一化如限制在 -180°~180°。2. 使用雙精度浮點(diǎn)或Q格式定點(diǎn)數(shù)。3. 增加傳感器數(shù)據(jù)校驗(yàn)。初始化時(shí)角度跳變初始角度設(shè)置不正確。觀察系統(tǒng)啟動(dòng)后第一秒的角度值。上電后延遲一段時(shí)間用穩(wěn)定的加速度計(jì)數(shù)據(jù)初始化角度。8. 最佳實(shí)踐與使用建議根據(jù)項(xiàng)目經(jīng)驗(yàn)遵循以下建議可以少走很多彎路。從仿真開始在把算法燒錄進(jìn)硬件前先用Python或Matlab仿真。用模擬數(shù)據(jù)驗(yàn)證你的算法邏輯和參數(shù)效果。本文提供的Python代碼就是一個(gè)很好的起點(diǎn)。分步調(diào)試第一步單獨(dú)測(cè)試傳感器數(shù)據(jù)讀取確保原始值正確、單位轉(zhuǎn)換無(wú)誤。第二步單獨(dú)測(cè)試加速度計(jì)計(jì)算角度的函數(shù)靜止時(shí)是否接近0度翻轉(zhuǎn)90度是否接近90度。第三步單獨(dú)測(cè)試陀螺儀積分快速旋轉(zhuǎn)后回到原點(diǎn)看積分角度是否也回到0附近短期內(nèi)。第四步將兩者用互補(bǔ)濾波結(jié)合進(jìn)行靜態(tài)和動(dòng)態(tài)測(cè)試。參數(shù)記錄與版本管理每次調(diào)整參數(shù)α,dt記錄下數(shù)值和測(cè)試效果。這能幫你快速回溯到穩(wěn)定可用的配置。資源管理在資源緊張的單片機(jī)上可以考慮使用定點(diǎn)數(shù)運(yùn)算來(lái)替代浮點(diǎn)數(shù)以提升速度。但要注意精度和溢出問(wèn)題。安全邊界對(duì)于控制應(yīng)用如無(wú)人機(jī)角度估計(jì)的可靠性至關(guān)重要。增加故障檢測(cè)機(jī)制例如如果加速度計(jì)數(shù)據(jù)異?？偧铀俣冗h(yuǎn)偏離g則暫時(shí)忽略其修正僅使用陀螺儀積分并給出警告。理解算法局限互補(bǔ)濾波是優(yōu)秀且簡(jiǎn)單的算法但它不是萬(wàn)能的。對(duì)于存在持續(xù)線性加速度如汽車加速、劇烈振動(dòng)或需要極高精度的場(chǎng)合需要研究擴(kuò)展卡爾曼濾波EKF、Mahony或Madgwick梯度下降等更魯棒的姿態(tài)解算算法。9. 總結(jié)與下一步低通濾波和互補(bǔ)濾波是嵌入式智能系統(tǒng)傳感器數(shù)據(jù)處理的兩塊基石。它們的價(jià)值不在于理論的高深而在于實(shí)現(xiàn)的簡(jiǎn)潔與效果的實(shí)用。低通濾波是你數(shù)據(jù)處理的“瑞士軍刀”用于平滑單個(gè)信號(hào)記住公式Y(jié)(n) α * X(n) (1-α) * Y(n-1)和α的調(diào)節(jié)意義就能解決一大半的噪聲問(wèn)題?；パa(bǔ)濾波是多傳感器融合的“優(yōu)雅入門”它用直觀的方式解決了陀螺儀漂移和加速度計(jì)噪聲的矛盾。掌握其預(yù)測(cè)-修正的兩步框架你就能讓MPU6050這類低成本IMU發(fā)揮出遠(yuǎn)超其價(jià)格的效果。最先應(yīng)該驗(yàn)證的用你的實(shí)際硬件運(yùn)行本文提供的C代碼框架先調(diào)通傳感器讀數(shù)然后觀察純積分和純加速度計(jì)的角度問(wèn)題最后加上互補(bǔ)濾波感受其“化腐朽為神奇”的效果。最容易踩的坑采樣周期dt用錯(cuò)用了固定值而非實(shí)際耗時(shí)。融合系數(shù)α取值不當(dāng)太大或太小。傳感器坐標(biāo)系和角度計(jì)算公式不匹配導(dǎo)致角度正負(fù)不對(duì)。沒有處理角度超過(guò)360度的情況。后續(xù)可以探索的方向自適應(yīng)互補(bǔ)濾波根據(jù)運(yùn)動(dòng)狀態(tài)動(dòng)態(tài)調(diào)整α。二階互補(bǔ)濾波進(jìn)一步優(yōu)化動(dòng)態(tài)性能和噪聲抑制。從互補(bǔ)濾波升級(jí)到卡爾曼濾波理解狀態(tài)預(yù)測(cè)和觀測(cè)更新的更一般形式。探索開源姿態(tài)解算庫(kù)如Madgwick AHRS、Mahony AHRS算法它們提供了更優(yōu)的性能和更完整的四元數(shù)表示。希望這篇從原理到代碼、從仿真到實(shí)戰(zhàn)的詳解能幫你速通這兩個(gè)關(guān)鍵算法并順利應(yīng)用到你的下一個(gè)項(xiàng)目中。