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

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

01慣性傳感器

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

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

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

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

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

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

02三種濾波算法

1.參考濾波方案

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

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

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

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

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

輸出角度

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

2. 互補(bǔ)濾波器

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

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

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

只所以討論這個(gè)變形，就是因?yàn)榍懊鎱⒖挤桨妇褪亲冃蔚幕パa(bǔ)濾波器。將陀螺儀信號 的積分看做帶有低頻噪聲（積分漂移信號）的觀察信號 。它與加速度計(jì)信號 （帶有高頻運(yùn)動(dòng)噪聲）相減，后面有比例、積分組成的負(fù)反饋組成一個(gè)一階低通濾波器 ，濾波之后與 疊加，形成最后的輸出信號。

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

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

3. 卡爾曼濾波

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

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

令：

則對應(yīng)卡爾曼濾波方程為：

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

在下面參數(shù)下，經(jīng)過迭代1000次之后，卡爾曼增益矩陣參數(shù) 的變化曲線。可以看去它逐漸曲線與常數(shù) 。

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

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

03算法關(guān)系

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

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

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

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

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

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

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

參考資料