機器人運動估計系列（番外篇）——從貝葉斯濾波到卡爾曼（上）_機器人基礎技術

機器人運動估計系列（番外篇）——從貝葉斯濾波到卡爾曼（上）

濾波理論在機器人狀態估計裡的重要性不言而喻，因此對濾波理論進行系統的瞭解非常有必要。本篇文章目的是從貝葉斯濾波（BF）的角度來串聯現在流行的幾種濾波方法：卡爾曼（KF）、擴充卡爾曼（EKF）、無跡卡爾曼（UKF）、粒子濾波（PF）等。

本篇文章主要參考文獻為：
《貝葉斯濾波與平滑》，作者：希莫·薩日伽，譯者：程建華等。
英文原版：《Bayesian Filtering and Smoothing》

為了簡便起見，下文中用英文縮寫來代表各種濾波器。
貝葉斯濾波：Bayes Filter：BF
卡爾曼濾波：Kalman Filter：KF
擴充卡爾曼：Extend Kalman Filter：EKF
無跡卡爾曼：Unscent Kalman Filter：UKF 1 為什麼是BF。

首先，BF是什麼。BF是依據機器人感應器擷取的觀測資料，利用Bayes公式（機率論）去估計機器人的狀態的一種手段。這裡的關鍵點在於，BF假設了機器人當前的狀態服從某一個機率分布，將機器人狀態估計問題建模為一個機率分布的估計問題，從而利用機率論的數學工具來解決機器人的狀態估計。

那麼為什麼要用BF呢。這是因為，機器人的運動模型以及感應器的觀測模型常常受到雜訊幹擾，而這種雜訊都是隨機性的。這樣建模就可以有利於我們把雜訊的分布、統計特性給估計出來，從而去除雜訊，得到真實觀測以及真實狀態。

那麼，BF具體是怎麼對機器人狀態估計問題建模的呢。

舉一個例子，我做了一隻小狗機器人，它身上裝了GPS，可以時刻返回小狗機器人的位置。為了遙控它，我還給它裝了一個遙控接收裝置（根據小狗之前在的位置加上遙控量，也能算出小狗的位置）。但是，我遇到兩個問題。一是，我花的錢太少了，買到GPS不太准，本來小狗應該在(5,3)的位置，但是測量的結果卻是(5.1,2.8)。二是，小狗機器人接收到指令是前進1米，但是實際走的卻可能是0.98米。小狗機器人的觀測過程和運動過程中都存在雜訊的幹擾，這使得我想要知道我的小狗跑到哪裡變得很困難，需要藉助濾波手段來估計小狗位置。

基於這個例子，由於受到雜訊汙染，小狗機器人位置和GPS的觀測可以被建模為機率分布，以表徵其不確定性，即：
xk∼p(xk|x1:k−1,y1:k−1)
yk∼p(yk|x1:k,y1:k−1)
其中， x,y 分別代表小狗機器人的位置，以及GPS測量到的小狗機器人的位置。下標代表的是時間。這兩個式子分別可以稱為小狗機器人系統的動態模型（通常用狀態方程描述，即從一個狀態如何變化到另一個狀態）和觀測模型（通常用觀測方程描述，即目前狀態下能夠擷取到怎麼樣的觀測），表達的意思是：k時刻小狗的位置服從以1到k-1時刻的小狗位置以及1到k-1時刻的GPS測量值為條件的一個機率分布；另外，k時刻GPS的測量值服從以1到k時刻的小狗位置以及1到k-1時刻的GPS測量值為條件的一個機率分布。

當然，上面這種建模方式是比較一般化的情況，而有些情況下（例如本例），k時刻的狀態和觀測未必和很久以前的狀態與觀測有關係，這時可以對這種建模方式進行一定簡化。

簡化一：狀態量的馬爾科夫性
假設k時刻小狗的位置只與k-1時刻的小狗的位置有關，由此：
xk∼p(xk|xk−1)
簡化二：觀測量的條件獨立性
假設k時刻GPS的位置的值只與k時刻小狗的位置有關，由此：
yk∼p(yk|xk)
上述兩個簡化的含義非常直觀，通常的機器人狀態估計問題基本能進行這樣的簡化。

當然，機器人狀態估計不一定只能用求解機率分布的方法。最小二乘（觀測與狀態誤差最小）也是常用的方法，卡爾曼濾波最早實際上是從最小二乘法的基礎上推匯出來的。 2 BF的基本架構

由於將機器人的狀態估計問題考慮為了估計機器人狀態的機率分布問題，那麼就可以引入Bayes公式來進行計算了。這部分首先介紹Bayes公式，然後介紹利用Bayes公式估計狀態機率分布的方法。 2.1 Bayes公式

這個公式是接下去一切討論的前題，羅列如下，不多做解釋，其中a、b、c是不同的隨機變數。
Bayes公式：
p(a|b)=p(b|a)p(a)p(b)
此外，由此公式延伸出的公式還有：
p(a,b)=p(a|b)p(b)=p(b|a)p(a)
p(a|b)=p(b|a)p(a)∫p(b|a)p(a) da
p(a|b)p(b|c)=p(a,b|c)
以上公式非常重要，在下文中會陸續用到。讀者要是不理解可以先將其接受，之後再去查閱資料。

2.2 BF基本方程

首先，我們需要明確BF的目標是什麼。
估計機器人狀態的機率分布。在小狗的例子中，就是通過手頭資訊估計出k時刻小狗位置的機率分布 p(xk|y1:k) 。而我們手頭的資訊就包括，1到k-1時刻小狗的位置 x1:k−1 以及1到k時刻 GPS的測量值 y1:k 。 2.2.1 批處理方法

因此，我們首先想到的方法大概是：
p(xk|y1:k)=p(y1:k|xk)p(xk)p(y1:k)
這種方法也就是所謂的批處理貝葉斯方法。但是這種方法每次都要拿所有的測量值來重新計算機率分布，對於電腦是個沉重的負擔。因此，有了改進的基於遞迴的貝葉斯濾波方法。

2.2.2 遞迴方法

事實上，在k時刻，我們除了GPS的測量值 y1:k 以外，還有k-1時刻對小狗位置的估計 p(xk−1|y1:k−1) 。（需要注意，當k=1時，由於k-1=0時刻沒有觀測值，小狗位置的機率分布 p(x0) 比較特殊，與觀測無關。這個分布由人為給定，被當做小狗位置的先驗分布。）

因此，我們根據k-1時刻的小狗位置分布和k時刻的GPS觀測可以利用遞迴的方法得到k時刻的位置分布，這也就是BF的核心。估計k時刻小狗位置的計算過程包括兩步，如下：

1-Step：預測
這一步是用 p(xk−1|y1:k−1) 去得到 p(xk|y1:k−1) ，注意這裡並沒有用到k時刻的觀測值。
p(xk|y1:k−1)=∫p(xk|xk−1)p(xk−1|y1:k−1) dxk−1
其中， p(xk|xk−1) 代表了狀態由k-1時刻到k時刻的轉移機率，也就是上文中介紹了簡化了的小狗動態模型，這裡的不確定性是由小狗運動過程中的雜訊引入的。

這一步之所以稱為預測步，可以理解為，在已知k-1時刻機器人位置的情況下，根據機器人本身運動學模型（通常是狀態方程），去預測k時刻機器人的位置。 p(xk−1|y1:k−1) 在這裡被作為先驗分布， p(xk|y1:k−1) 是由機器人運動學模型預測得到的預測分布。

2-Step：更新
這一步是用觀測值 yk 去更新預測分布 p(xk|y1:k−1) ，從而得到k時刻機器人位置的後驗分布 p(xk|y1:k) 。
p(xk|y1:k)=p(yk|xk)p(xk|y1:k−1)∫p(yk|xk)p(xk|y1:k−1) dxk
其中， p(yk|xk) 代表了依據k時刻狀態 xk 觀測到 yk 的機率，也就是小狗機器人GPS的觀測模型，這裡的不確定性是由GPS觀測過程中的雜訊引入的。

這一步之所以稱為更新是因為這步中用到了k時刻的觀測量，從而將原本的預測分布 p(xk|y1:k−1) 更新為了後驗分布 p(xk|y1:k) 。而這個後驗分布，將作為估計k+1時刻小狗機器人位置的先驗分布，從而開始下一輪的遞迴解算。

總結
BF的整個過程可以描述為，首先選定一個最初狀態的先驗分布 p(x0) ，進行預測步，得到k=1的預測分布，再用k=1的觀測去更新預測分布得到k=1的後驗分布，然後將這個後驗分布作為先驗分布，去預測k=2的預測分布，然後以此類推。。。

雖然BF的過程看著很簡單，但是實際應用起來卻並沒有這麼容易。首先，初始的先驗分布不好猜測，動態模型以及觀測模型的機率分布也不容易確定，只能去假設其形式，常見的比如高斯分布。另外，公式裡的積分同樣是個非常頭疼的問題，只有再狀態模型、觀測模型很特殊的情況下才比較好求閉型解。因此，想要實際應用BF還需要做更多的假設，使得問題得到簡化。而不同的假設方案，就由此引出了不同的濾波器。接下來，我就將進一步介紹KF、EKF、UKF是如何在BF的架構下被推匯出來的。。。。。。請馬上走開，不要回來。。。。。。