240
前幾天，有參加智能車(chē)競(jìng)賽的同學(xué)在公眾號(hào)上詢問(wèn)一個(gè)關(guān)于直立車(chē)?？刂浦薪嵌热诤纤惴ǖ膯?wèn)題。感覺(jué)這是所有初次準(zhǔn)備智能車(chē)模競(jìng)賽的同學(xué)都會(huì)碰到的問(wèn)題。
卓大大，請(qǐng)問(wèn)您可以針對(duì)卡爾曼濾波、互補(bǔ)濾波、清華濾波的原理與異同做一個(gè)推送嗎？最近在查閱相關(guān)的資料學(xué)習(xí)濾波算法，感覺(jué)好難理解呀 /::<
▲ 平衡雙輪自行車(chē)
01 慣性傳感器
在車(chē)模直立控制中被廣泛使用的慣性傳感器通常是由加速度計(jì)和陀螺儀組成。價(jià)格比較便宜的慣性測(cè)量傳感器，輸出的信號(hào)往往包含有各種噪聲。陀螺儀輸出車(chē)模傾角的變化率（角速度）信號(hào)。它通常包括緩慢變化的偏移量和高斯白噪聲。
其中偏移量的導(dǎo)數(shù)是隨機(jī)的，通常描述為一個(gè)白噪聲信號(hào)：。
在重力場(chǎng)中，加速度傳感器正交的兩個(gè)輸出，通過(guò)求反正切可以傳感器相對(duì)于重力加速度的傾角。但傳感器的運(yùn)動(dòng)所產(chǎn)生的加速度也會(huì)疊加在輸出信號(hào)中，進(jìn)而產(chǎn)生干擾信號(hào)。$$theta _M ?= theta T ?+ ntheta$$
▲ 采集數(shù)據(jù)及其顯示
?
通過(guò)濾波器算法將陀螺儀和加速度計(jì)所得到的角度信息進(jìn)行融合，最大限度抑制噪聲對(duì)測(cè)量角度的影響。
前面提問(wèn)中所提到的三種濾波算法，分別利用傳感器噪聲不同特點(diǎn)來(lái)抑制噪聲的影響?？此七@些方法原理相差很大，借助于以下條件，能夠便于我們進(jìn)行分析和比較：
- 這三種算法都屬于線性濾波算法；在穩(wěn)態(tài)下，可以借助于輸入輸出之間的傳遞函數(shù)來(lái)比較算法的特性；針對(duì)車(chē)模直立控制，所涉及到的角度只有一個(gè)；
02 三種濾波算法
1. 參考濾波方案
在 2012 年直立車(chē)模組別首次引入智能車(chē)競(jìng)賽的，競(jìng)賽秘書(shū)處給出了一個(gè) 參考設(shè)計(jì)方案[1] 。它是在對(duì)陀螺儀信號(hào)進(jìn)行積分的基礎(chǔ)上，使用加速度計(jì)得到的角度信號(hào)對(duì)輸出進(jìn)行負(fù)反饋調(diào)節(jié)，從而抑制積分中存在的發(fā)散問(wèn)題。下圖是參考設(shè)計(jì)方案中相應(yīng)的原理框圖和程序?qū)崿F(xiàn)。
▲ 參考濾波方案軟件實(shí)現(xiàn)
加速度傳感器的 Z 周輸出電壓，經(jīng)過(guò)偏移量，比率調(diào)整之后，形成算法的輸入:
陀螺儀的輸出電壓經(jīng)過(guò)偏移量，比率調(diào)整之后，形成算法輸入:
算法的輸出為角度：（g_fCarAngle）。
▲ 參考濾波方案的核心算法
?
根據(jù)參考程序代碼，算法可以使用迭代方程描述為：
輸出角度
其中：
?
通過(guò)上面的化簡(jiǎn)公式可以看出，實(shí)際上角度輸出信號(hào)是陀螺儀和加速度計(jì)信號(hào)經(jīng)過(guò)一級(jí)低通濾波后疊加在一起，所以陀螺儀和加速度計(jì)輸出信號(hào)中如果存在直流分量的漂移，都會(huì)引起輸出角度信號(hào)的零點(diǎn)漂移。
2. 互補(bǔ)濾波器
互補(bǔ)濾波器的基本結(jié)構(gòu)如下圖所示。假設(shè)兩個(gè)測(cè)量信號(hào)都包含了實(shí)際信號(hào)以及噪聲。在信號(hào)中的噪聲主要分布在高頻，而在信號(hào)中的噪聲主要分布在低頻。
代表著低通濾波器，那么對(duì)應(yīng)的就是互補(bǔ)的高通濾波器。它們分別提取中的低頻和高頻成分，然后疊加在一起。
▲ 左：基本互補(bǔ)濾波器的結(jié)構(gòu)。右：基本結(jié)構(gòu)的變形，只對(duì)噪聲進(jìn)行低通濾波
?
在上圖右邊給出了互補(bǔ)濾波器基本結(jié)構(gòu)的恒等變形。將與相減之后，就剩下了高頻噪聲與低頻噪聲相減。再經(jīng)過(guò)低通濾波器，就剩下去抵消中的低頻噪聲了。
只所以討論這個(gè)變形，就是因?yàn)榍懊鎱⒖挤桨妇褪亲冃蔚幕パa(bǔ)濾波器。將陀螺儀信號(hào)的積分看做帶有低頻噪聲（積分漂移信號(hào)）的觀察信號(hào)。它與加速度計(jì)信號(hào)（帶有高頻運(yùn)動(dòng)噪聲）相減，后面有比例、積分組成的負(fù)反饋組成一個(gè)一階低通濾波器，濾波之后與疊加，形成最后的輸出信號(hào)。
▲ 參考濾波方案本質(zhì)上是變形后的互補(bǔ)濾波器
?
通過(guò)上面討論可以看出，如果在互補(bǔ)濾波器中的低通濾波器的階次為一階的話，那么就和前面的參考方案是一致了。
在實(shí)際應(yīng)用中，可以增加濾波器的階次，來(lái)提高角度計(jì)算的精度。比如在陀螺儀輸入信號(hào)中，再增加一級(jí)高通濾波，可以進(jìn)一步抑制陀螺儀信號(hào)中的直流分量的漂移。
3. 卡爾曼濾波
使用卡爾曼濾波的方法來(lái)融合來(lái)自陀螺儀和加速度計(jì)的信號(hào)，不在頻率范圍內(nèi)考慮它們各自噪聲的特點(diǎn)，而是將這兩個(gè)信號(hào)從功能上分為兩類。陀螺儀信號(hào)是過(guò)程驅(qū)動(dòng)變量，它激動(dòng)系統(tǒng)狀態(tài)（車(chē)模傾角，角速度）發(fā)生變化；加速度計(jì)得到的角度則是觀察變量，是來(lái)修正系統(tǒng)狀態(tài)。
選擇車(chē)模傾角和陀螺儀偏變化量作為系統(tǒng)狀態(tài)變量，得到離散系統(tǒng)狀態(tài)方程為：
令：
則對(duì)應(yīng)卡爾曼濾波方程為：
?
在這個(gè)過(guò)程中，如果系統(tǒng)噪聲協(xié)方差矩陣以及觀測(cè)噪聲方差始終為常量。那么由上面方程 2、3、5 迭代之后，誤差協(xié)方差矩陣和卡爾曼增益矩陣就會(huì)逐步收斂到一個(gè)常量。
在下面參數(shù)下，經(jīng)過(guò)迭代 1000 次之后，卡爾曼增益矩陣參數(shù)的變化曲線?？梢钥慈ニ饾u曲線與常數(shù)。
▲ K 參數(shù)迭代數(shù)值
?
當(dāng)為常量的時(shí)候，對(duì)方程 1、4 再聯(lián)立起來(lái)，就形成關(guān)于狀態(tài)變量的差分方程?？紤]到，都比較小，將化簡(jiǎn)過(guò)程中涉及到這些小量乘積（二階小量）都省略掉，那么最終可以將輸出角度與輸入之間的差分方程化簡(jiǎn)為：
其中分別是的 z 變換。對(duì)照上面簡(jiǎn)化的輸入輸出之間的關(guān)系與前面參考方案中對(duì)應(yīng)的方程，可以看到卡爾曼濾波器本質(zhì)上和互補(bǔ)濾波器是相同的。如果不省略那些參數(shù)小的項(xiàng)數(shù)，濾波算法就變成二階的互補(bǔ)濾波器的形式了。
觀察噪聲來(lái)自于加速度計(jì)傳感器，系統(tǒng)內(nèi)部噪聲來(lái)自于陀螺儀。當(dāng)遠(yuǎn)遠(yuǎn)大于的時(shí)候，所計(jì)算出的卡爾曼濾波增益的數(shù)值比較小，這樣才能夠進(jìn)行上述的算法簡(jiǎn)化。
03 算法關(guān)系
可以看出。當(dāng)所使用的慣性傳感器，陀螺儀的噪聲遠(yuǎn)遠(yuǎn)低于加速度傳感器的噪聲時(shí)，低通濾波器的階次取一階濾波器的情況下，三種濾波器的算法效果在平穩(wěn)狀態(tài)下是相同的。
從原理上，參考方案與互補(bǔ)濾波器比較接近，它們是利用不同傳感器噪聲在頻率上的差別抑制噪聲的?？柭鼮V波器則根據(jù)傳感器對(duì)傾角影響的機(jī)制不同，通過(guò)構(gòu)造系統(tǒng)狀態(tài)變量將信息進(jìn)行融合。
在車(chē)模直立控制應(yīng)用中，互補(bǔ)濾波器和參考方案所需要的計(jì)算量小，參數(shù)只有濾波時(shí)間常數(shù)，調(diào)試相對(duì)比較簡(jiǎn)單。但該算法對(duì)于陀螺儀工作點(diǎn)的漂移無(wú)法抑制，使用時(shí)，可以對(duì)陀螺儀信號(hào)再通過(guò)一個(gè)高通濾波器，消除陀螺儀信號(hào)工作點(diǎn)變化對(duì)輸出角度的影響。
卡爾曼濾波器計(jì)算量相對(duì)較大，需要 MCU 的算力強(qiáng)才行。由于卡爾曼濾波器內(nèi)部變量包括有對(duì)陀螺儀工作點(diǎn)的估計(jì)，所以陀螺儀的工作點(diǎn)的緩慢變化不會(huì)影響計(jì)算角度的變化。
▲ RECORD1 數(shù)據(jù)
?
在使用濾波器算法的時(shí)候，都會(huì)涉及到算法的初始狀態(tài)的設(shè)置問(wèn)題。對(duì)于互補(bǔ)濾波器相對(duì)比較簡(jiǎn)單，將內(nèi)部積分累加變量初始化為 0 便可以適應(yīng)大部分情況。對(duì)于卡爾曼濾波器，它的參數(shù)矩陣則需要選擇恰當(dāng)?shù)某跏贾?，以使得算法盡快達(dá)到收斂的目的。比如可以將算法收斂后的矩陣的值作為程序下一次運(yùn)行的初始值，這樣就可以更快使得算法進(jìn)入穩(wěn)定狀態(tài)。
此外，需要根據(jù)傳感器輸出信號(hào)的實(shí)際情況，合理的確定卡爾曼濾波器中的參數(shù)，這樣才能夠在算法的收斂以及濾波效果方面取得良好的平衡。
?
關(guān)于這三種算法在理論上的分析就到這里，它們?cè)趯?shí)際控制中的效果以后在給出。
參考資料
[1]參考設(shè)計(jì)方案: https://wenku.baidu.com/view/74481d2ff5335a8102d220b5.html
