在智能服務(wù)機(jī)器人逐漸成為行業(yè)風(fēng)口浪尖的今天，移動(dòng)機(jī)器人的身影越來(lái)越多地出現(xiàn)在人們身邊。相信隨著傳感技術(shù)、智能技術(shù)和計(jì)算技術(shù)等的不斷提高，智能移動(dòng)機(jī)器人一定能夠在生產(chǎn)和生活中融入人類生活中。上一期小編大致介紹了基于概率模型的SLAM方法的基本原理，本期，小編給大家詳細(xì)介紹下概率模型SLAM方法中一個(gè)比較重要的家族：卡爾曼濾波。

什么是卡爾曼濾波

卡爾曼濾波（Kalman filtering），又稱線性二次型估計(jì)(Linear quadratic estimation)，是Swerling(1958)和Kalman(1960)作為線性高斯系統(tǒng)中的預(yù)測(cè)和濾波技術(shù)而提出的。

最早由Stanley Schmidt首次實(shí)現(xiàn)，后被Kalman發(fā)掘并應(yīng)用于NASA的阿波羅計(jì)劃。提到阿波羅計(jì)劃，大家可能會(huì)有種小編是在幫卡爾曼濾波蹭“人類登月”熱度的感覺(jué)，其實(shí)不然，在工程領(lǐng)域里，卡爾曼濾波一樣也很“高大上”。

作為線性高斯系統(tǒng)預(yù)測(cè)方法中最優(yōu)、最高效的工具，卡爾曼濾波器及其變種已經(jīng)被廣泛應(yīng)用在諸如機(jī)器人導(dǎo)航，傳感器數(shù)據(jù)融合，導(dǎo)彈追蹤，人臉識(shí)別等多種領(lǐng)域近半個(gè)世紀(jì)。

卡爾曼濾波解決的問(wèn)題是“線性高斯系統(tǒng)的預(yù)測(cè)和濾波”，那么什么是線性高斯系統(tǒng)呢？高斯分布想必大家應(yīng)該都有所耳聞，或者換個(gè)說(shuō)法，正態(tài)分布，這個(gè)高中經(jīng)常被數(shù)學(xué)老師拿來(lái)讓我們估算人群身高分布的“大殺器”。它的長(zhǎng)相是下圖這個(gè)樣子的：

正態(tài)分布很簡(jiǎn)單，只需要兩個(gè)參數(shù)就可以表示了，即期望μ和方差σ。一個(gè)多元正態(tài)分布的表達(dá)式為：

呃，上面的公式看看就好，其實(shí)只需要知道正態(tài)分布的期望μ就是它的對(duì)稱軸，方差σ就是它的“胖瘦”情況，上面的圖中給出了幾個(gè)不同μ和σ的正態(tài)分布的樣子。一個(gè)所謂的“高斯線性系統(tǒng)”需要滿足三個(gè)條件：

1）狀態(tài)轉(zhuǎn)移概率p(xt|ut,xt-1)是帶有隨機(jī)高斯噪聲的線性函數(shù)；

xt是t時(shí)刻的狀態(tài)向量，它是上一時(shí)刻的狀態(tài)xt-1和當(dāng)前時(shí)刻的控制量ut的線性組合，εt表示一個(gè)隨機(jī)的高斯噪聲，它是由狀態(tài)轉(zhuǎn)移引入的不確定性。需要注意的是，式2表達(dá)的不是一個(gè)確定的解，而是一個(gè)分布，它由期望Atxt-1+Btut和方差rt確定。

2）測(cè)量概率p(zt|xt)也與帶有高斯噪聲的狀態(tài)量呈線性關(guān)系；

zt是t時(shí)刻的測(cè)量向量，ωt是測(cè)量時(shí)的高斯噪聲。同樣，式3也是一個(gè)分布，期望是Ctxt，方差是qt。

3） 初始置信度bel(x0)也是正態(tài)分布的，它期望是μ0方差是σ0。

如果一個(gè)系統(tǒng)能滿足上述三個(gè)條件，則它的后驗(yàn)估計(jì)也滿足高斯分布，其期望為μt方差為σt。換句話說(shuō)，一個(gè)最優(yōu)估計(jì)的問(wèn)題，它的先驗(yàn)概率分布是高斯分布，同時(shí)它的測(cè)量概率分布也是高斯分布，那么它的后驗(yàn)概率也滿足高斯分布，并可求它的期望和方差。

下面拿擎朗智能第三代系列機(jī)器人——peanut舉個(gè)例子，一輛做勻速運(yùn)動(dòng)的peanut，它的初始位置為s0，速度為v，求它在t時(shí)刻的位置，大家一定立馬就能給出答案：st=s0+vt。

沒(méi)錯(cuò)，理論上t時(shí)刻的位置就應(yīng)該是這個(gè)值。但在實(shí)際工程應(yīng)用中，位置s和速度v并不會(huì)是理想值，它會(huì)有一定的不確定性，我們假設(shè)這個(gè)不確定性是滿足高斯分布的。

如果沒(méi)有后驗(yàn)的話，這個(gè)不確定性會(huì)不斷地疊加，就如上圖（a）所示，隨著peanut的不斷前行，位置s概率分布的方差會(huì)越來(lái)越大，peanut對(duì)自己位置的估計(jì)也就越來(lái)越不準(zhǔn)確。

為了確保位姿估計(jì)的準(zhǔn)確性，加入了一個(gè)觀測(cè)值去修正計(jì)算得到的速度，例如用里程計(jì)去測(cè)量每次走過(guò)的距離st’，如上圖（b）所示，

藍(lán)色表示peanut當(dāng)前時(shí)刻的先驗(yàn)估計(jì)，它由上一時(shí)刻的估計(jì)值st-1通過(guò)運(yùn)動(dòng)方程求得；

綠色表示從里程計(jì)獲得的觀測(cè)值得概率分布，它與先驗(yàn)估計(jì)有一定的偏差；

紫色表示通過(guò)卡爾曼濾波后，修正的概率分布。

從濾波的結(jié)果來(lái)看，后驗(yàn)估計(jì)也是高斯分布，它的分布綜合了先驗(yàn)和觀測(cè)的期望，且縮小了方差。

卡爾曼濾波的一般步驟

通過(guò)上面的介紹，相信大家對(duì)卡爾曼濾波能干啥應(yīng)該有個(gè)大致的了解了，那么它是如何使用的呢？廢話不多說(shuō)，先上公式，

卡爾曼濾波的經(jīng)典五步：

好吧，看到上面的公式大家可能會(huì)有點(diǎn)懵，下面小編盡量用比較通俗的語(yǔ)句解釋下，

這五步具體做了什么：

1)計(jì)算狀態(tài)轉(zhuǎn)移的期望，μt-1表示上一狀態(tài)的期望值，其實(shí)就是上一次后驗(yàn)分布的期望，ut是控制量，這樣就求出了先驗(yàn)概率分布的期望值；

2)光有期望值還不夠，還需要計(jì)算方差才能得到一個(gè)完整的正態(tài)分布，公式5就是計(jì)算先驗(yàn)概率方差的過(guò)程，其中σt-1表示上一次后驗(yàn)的方差，rt是狀態(tài)轉(zhuǎn)移引入高斯噪聲的方差。

3)得到先驗(yàn)概率分布后，就需要通過(guò)貝葉斯估計(jì)求解后驗(yàn)概率分布。公式6是求解卡爾曼增益K，它通過(guò)觀測(cè)概率分布的方差qt和先驗(yàn)概率分布的方差求得。

4)卡爾曼增益K主要用來(lái)衡量“預(yù)測(cè)值”和“觀測(cè)值”的比重，即是更相信預(yù)測(cè)的結(jié)果還是更相信觀測(cè)的結(jié)果；具體表現(xiàn)在公式7，后驗(yàn)的期望是先驗(yàn)的期望加上卡爾曼增益乘以估計(jì)和觀測(cè)期望的偏差。

5)后驗(yàn)的期望得到后，再通過(guò)公式8更新后驗(yàn)的方差，就得到了后驗(yàn)估計(jì)得概率分布。

通過(guò)上述五步就精確地求解了一個(gè)“線性高斯系統(tǒng)”的后驗(yàn)估計(jì)，算法里運(yùn)算量最大的矩陣求逆運(yùn)算和矩陣乘法運(yùn)算時(shí)間復(fù)雜的分別可近似為O(k2.4)和O(k2)，k是矩陣維度。相對(duì)其他算法，卡爾曼濾波算法是相當(dāng)高效的。

卡爾曼濾波的變種

卡爾曼濾波雖然高效且精確，但是他有個(gè)致命的缺陷，即要求系統(tǒng)是高斯線性的，而實(shí)際應(yīng)用中，絕大多數(shù)系統(tǒng)不是高斯線性的。

于是就有了所謂的擴(kuò)展卡爾曼濾波EKF（Extended Kalman Filter）和無(wú)跡卡爾曼濾波UKF（Unscented Kalmen Filter ）的變種。

EKF和UKF都是將實(shí)際狀態(tài)轉(zhuǎn)移模型局部線性化，只不過(guò)前者采用的是泰勒展開(kāi)進(jìn)行線性化，而后者采用的是通過(guò)加權(quán)統(tǒng)計(jì)線性回歸過(guò)程實(shí)現(xiàn)隨機(jī)線性化。

通俗的講，EKF是計(jì)算非線性函數(shù)的切線來(lái)進(jìn)行線性化的，因此EKF需要知道狀態(tài)轉(zhuǎn)移方程的雅可比矩陣（Jacobian Matrix），UKF則是通過(guò)提取所謂的δ點(diǎn)，用這些δ點(diǎn)擬合的直線作為局部線性化的參考，δ的取值與分布的協(xié)方差有關(guān)。

EKF和UKF的算法復(fù)雜度近似，一般情況下EKF會(huì)稍微快一點(diǎn)，而估計(jì)的結(jié)果一般情況下UKF要優(yōu)于EKF。

卡爾曼濾波作為經(jīng)典的估計(jì)算法，被廣泛應(yīng)用在各個(gè)領(lǐng)域之中，在機(jī)器人領(lǐng)域里，卡爾曼濾波主要應(yīng)用在多傳感器融合，SLAM的最優(yōu)估計(jì)等問(wèn)題中。

以上就是本期關(guān)于卡爾曼濾波的內(nèi)容，下期小編將給大家介紹粒子濾波的相關(guān)內(nèi)容。