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

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

01慣性傳感器

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

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

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

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

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

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

02三種濾波算法

1.參考濾波方案

在2012年直立車模組別首次引入智能車競賽的，競賽秘書處給出了一個(gè) 參考設(shè)計(jì)方案^[1] 。它是在對(duì)陀螺儀信號(hào)進(jìn)行積分的基礎(chǔ)上，使用加速度計(jì)得到的角度信號(hào)對(duì)輸出進(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í)際上角度輸出信號(hào)是陀螺儀和加速度計(jì)信號(hào)經(jīng)過一級(jí)低通濾波后疊加在一起，所以陀螺儀和加速度計(jì)輸出信號(hào)中如果存在直流分量的漂移，都會(huì)引起輸出角度信號(hào)的零點(diǎn)漂移。

2. 互補(bǔ)濾波器

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

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

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

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

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

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

3. 卡爾曼濾波

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

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

令：

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

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

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

當(dāng)為常量的時(shí)候，對(duì)方程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ǔ)濾波器比較接近，它們是利用不同傳感器噪聲在頻率上的差別抑制噪聲的?？柭鼮V波器則根據(jù)傳感器對(duì)傾角影響的機(jī)制不同，通過構(gòu)造系統(tǒng)狀態(tài)變量將信息進(jìn)行融合。

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

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

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

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

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

參考資料