前幾天，有參加智能車競賽的同學在公眾號上詢問一個關于直立車模控制中角度融合算法的問題。感覺這是所有初次準備智能車模競賽的同學都會碰到的問題。

卓大大，請問您可以針對卡爾曼濾波、互補濾波、清華濾波的原理與異同做一個推送嗎？最近在查閱相關的資料學習濾波算法，感覺好難理解呀/::<

01慣性傳感器

在車模直立控制中被廣泛使用的慣性傳感器通常是由加速度計和陀螺儀組成。價格比較便宜的慣性測量傳感器，輸出的信號往往包含有各種噪聲。陀螺儀輸出車模傾角 的變化率（角速度） 信號。它通常包括緩慢變化的偏移量 和高斯白噪聲 。

其中偏移量 的導數(shù)是隨機的，通常描述為一個白噪聲信號： 。

在重力場中，加速度傳感器正交的兩個輸出，通過求反正切可以傳感器相對于重力加速度的傾角。但傳感器的運動所產(chǎn)生的加速度也會疊加在輸出信號中，進而產(chǎn)生干擾信號。$$\theta _M  = \theta T  + n\theta$$

通過濾波器算法將陀螺儀和加速度計所得到的角度信息進行融合，最大限度抑制噪聲對測量角度的影響。

前面提問中所提到的三種濾波算法，分別利用傳感器噪聲不同特點來抑制噪聲的影響?？此七@些方法原理相差很大，借助于以下條件，能夠便于我們進行分析和比較：

• 這三種算法都屬于線性濾波算法；
• 在穩(wěn)態(tài)下，可以借助于輸入輸出之間的傳遞函數(shù)來比較算法的特性；
• 針對車模直立控制，所涉及到的角度只有一個；

02三種濾波算法

1.參考濾波方案

在2012年直立車模組別首次引入智能車競賽的，競賽秘書處給出了一個 參考設計方案^[1] 。它是在對陀螺儀信號進行積分的基礎上，使用加速度計得到的角度信號對輸出進行負反饋調(diào)節(jié)，從而抑制積分中存在的發(fā)散問題。下圖是參考設計方案中相應的原理框圖和程序?qū)崿F(xiàn)。

加速度傳感器的Z周輸出電壓，經(jīng)過偏移量，比率調(diào)整之后，形成算法的輸入:

陀螺儀的輸出電壓經(jīng)過偏移量，比率調(diào)整之后，形成算法輸入:

算法的輸出為角度： （g_fCarAngle）。

根據(jù)參考程序代碼，算法可以使用迭代方程描述為：

輸出角度

通過上面的化簡公式可以看出，實際上角度輸出信號是陀螺儀和加速度計信號經(jīng)過一級低通濾波后疊加在一起，所以陀螺儀和加速度計輸出信號中如果存在直流分量的漂移，都會引起輸出角度信號的零點漂移。

2. 互補濾波器

互補濾波器的基本結(jié)構(gòu)如下圖所示。假設兩個測量信號 都包含了實際信號 以及噪聲。在信號 中的噪聲主要分布在高頻，而在信號 中的噪聲主要分布在低頻。

代表著低通濾波器，那么對應的 就是互補的高通濾波器。它們分別提取 中的低頻和高頻成分，然后疊加在一起。

在上圖右邊給出了互補濾波器基本結(jié)構(gòu)的恒等變形。將 與 相減之后，就剩下了高頻噪聲 與低頻噪聲 相減。再經(jīng)過低通濾波器 ，就剩下 去抵消 中的低頻噪聲了。

只所以討論這個變形，就是因為前面參考方案就是變形的互補濾波器。將陀螺儀信號 的積分看做帶有低頻噪聲（積分漂移信號）的觀察信號 。它與加速度計信號 （帶有高頻運動噪聲）相減，后面有比例、積分組成的負反饋組成一個一階低通濾波器 ，濾波之后與 疊加，形成最后的輸出信號。

通過上面討論可以看出，如果在互補濾波器中的低通濾波器的階次為一階的話，那么就和前面的參考方案是一致了。

在實際應用中，可以增加濾波器的階次，來提高角度計算的精度。比如在陀螺儀輸入信號中，再增加一級高通濾波，可以進一步抑制陀螺儀信號中的直流分量的漂移。

3. 卡爾曼濾波

使用卡爾曼濾波的方法來融合來自陀螺儀和加速度計的信號，不在頻率范圍內(nèi)考慮它們各自噪聲的特點，而是將這兩個信號從功能上分為兩類。陀螺儀信號 是過程驅(qū)動變量，它激動系統(tǒng)狀態(tài)（車模傾角，角速度）發(fā)生變化；加速度計得到的角度 則是觀察變量，是來修正系統(tǒng)狀態(tài)。。

選擇車模傾角 和陀螺儀偏變化量 作為系統(tǒng)狀態(tài)變量，得到離散系統(tǒng)狀態(tài)方程為：

令：

則對應卡爾曼濾波方程為：

在這個過程中，如果系統(tǒng)噪聲協(xié)方差矩陣 以及觀測噪聲方差 始終為常量。那么由上面方程2、3、5迭代之后，誤差協(xié)方差矩陣 和卡爾曼增益矩陣 就會逐步收斂到一個常量。

在下面參數(shù)下，經(jīng)過迭代1000次之后，卡爾曼增益矩陣參數(shù) 的變化曲線?？梢钥慈ニ饾u曲線與常數(shù) 。

當為常量的時候，對方程1、4再聯(lián)立起來，就形成關于狀態(tài)變量 的差分方程。考慮到 ， 都比較小，將化簡過程中涉及到這些小量乘積（二階小量）都省略掉，那么最終可以將輸出角度 與輸入 之間的差分方程化簡為：

觀察噪聲 來自于加速度計傳感器，系統(tǒng)內(nèi)部噪聲 來自于陀螺儀。當 遠遠大于 的時候，所計算出的卡爾曼濾波增益 的數(shù)值比較小，這樣才能夠進行上述的算法簡化。

03算法關系

可以看出。當所使用的慣性傳感器，陀螺儀的噪聲遠遠低于加速度傳感器的噪聲時，低通濾波器的階次取一階濾波器的情況下，三種濾波器的算法效果在平穩(wěn)狀態(tài)下是相同的。

從原理上，參考方案與互補濾波器比較接近，它們是利用不同傳感器噪聲在頻率上的差別抑制噪聲的?？柭鼮V波器則根據(jù)傳感器對傾角影響的機制不同，通過構(gòu)造系統(tǒng)狀態(tài)變量將信息進行融合。

在車模直立控制應用中，互補濾波器和參考方案所需要的計算量小，參數(shù)只有濾波時間常數(shù) ，調(diào)試相對比較簡單。但該算法對于陀螺儀工作點的漂移無法抑制，使用時，可以對陀螺儀信號再通過一個高通濾波器，消除陀螺儀信號工作點變化對輸出角度的影響。

卡爾曼濾波器計算量相對較大，需要MCU的算力強才行。由于卡爾曼濾波器內(nèi)部變量包括有對陀螺儀工作點的估計，所以陀螺儀的工作點的緩慢變化不會影響計算角度的變化。

在使用濾波器算法的時候，都會涉及到算法的初始狀態(tài)的設置問題。對于互補濾波器相對比較簡單，將內(nèi)部積分累加變量初始化為0便可以適應大部分情況。對于卡爾曼濾波器，它的參數(shù)矩陣 則需要選擇恰當?shù)某跏贾担允沟盟惴ūM快達到收斂的目的。比如可以將算法收斂后的 矩陣的值作為程序下一次運行的初始值，這樣就可以更快使得算法進入穩(wěn)定狀態(tài)。

此外，需要根據(jù)傳感器輸出信號的實際情況，合理的確定卡爾曼濾波器中的 參數(shù)，這樣才能夠在算法的收斂以及濾波效果方面取得良好的平衡。

關于這三種算法在理論上的分析就到這里，它們在實際控制中的效果以后在給出。

參考資料