探究機器人是如何實現自主移動的——淺談機器人的定位導航技術（二）

04-30

探究機器人是如何實現自主移動的——淺談機器人的定位導航技術（二）

上一期小編大致介紹了基於概率模型的SLAM方法的基本原理，眾所周知，在概率模型SLAM方法中有一個比較重要的家族：卡爾曼濾波家族。本期小編將給大家詳細介紹下卡爾曼濾波。

什麼是卡爾曼濾波

卡爾曼濾波（Kalman filtering），又稱線性二次型估計(Linear quadratic estimation)，是Swerling(1958)和Kalman(1960)作為線性高斯系統中的預測和濾波技術而提出的。最早由Stanley Schmidt首次實現，後被Kalman發掘並應用於NASA的阿波羅計劃。提到阿波羅計劃，大家可能會有種小編是在幫卡爾曼濾波蹭「人類登月」熱度的感覺，其實不然，在工程領域裡，卡爾曼濾波一樣也很「高大上」。作為線性高斯系統預測方法中最優、最高效的工具，卡爾曼濾波器及其變種已經被廣泛應用在諸如機器人導航，感測器數據融合，導彈追蹤，人臉識別等多種領域近半個世紀。

卡爾曼濾波解決的問題是「線性高斯系統的預測和濾波」，那麼什麼是線性高斯系統呢？高斯分布想必大家應該都有所耳聞，或者換個說法，正態分布，這個高中經常被數學老師拿來讓我們估算人群身高分布的「大殺器」。它的長相是下圖這個樣子的：

正態分布很簡單，只需要兩個參數就可以表示了，即期望 $μ$ 和方差 $σ$ 。一個多元正態分布的表達式為：

$p(x)=det?(2πσ)^{-frac{1}{2}e^{-frac{1}{2} (x-μ)^{T} σ^{-1}(x-μ)}}$ (1)

呃，上面的公式看看就好，其實只需要知道正態分布的期望 $μ$ 就是它的對稱軸，方差 $σ$ 就是它的「胖瘦」情況，上面的圖中給出了幾個不同 $μ$ 和 $σ$ 的正態分布的樣子。一個所謂的「高斯線性系統」需要滿足三個條件：

1）狀態轉移概率 $p(x_{t}|u_{t},x_{t-1})$ 是帶有隨機高斯雜訊的線性函數；

$x_t=A_t x_(t-1)+B_t u_t+ε_t$ (2)

$x_{t}$ 是t時刻的狀態向量，它是上一時刻的狀態 $x_{t-1}$ 和當前時刻的控制量 $u_{t}$ 的線性組合， $ε_{t}$ 表示一個隨機的高斯雜訊，它是由狀態轉移引入的不確定性。需要注意的是，式2表達的不是一個確定的解，而是一個分布，它由期望 $A_tx_{t-1}+B_tu_t$ 和方差 $r_{t}$ 確定。

2）測量概率 $p(z_t|x_t)$ 也與帶有高斯雜訊的狀態量呈線性關係；

$z_t=C_t x_t+ω_t$ (3)

$z_t$ 是 $t$ 時刻的測量向量， $ω_t$ 是測量時的高斯雜訊。同樣，式3也是一個分布，期望是 $C_tx_t$ ，方差是 $q_t$ 。

3）初始置信度 $bel(x_0)$ 也是正態分布的，它期望是 $μ_0$ 方差是 $σ_0$ 。

如果一個系統能滿足上述三個條件，則它的後驗估計也滿足高斯分布，其期望為 $μ_t$ 方差為 $σ_t$ 。換句話說，一個最優估計的問題，它的先驗概率分布是高斯分布，同時它的測量概率分布也是高斯分布，那麼它的後驗概率也滿足高斯分布，並可求它的期望和方差。

下面拿擎朗智能第三代系列機器人——peanut舉個例子，一輛做勻速運動的peanut，它的初始位置為 $s_0$ ，速度為 $v$ ，求它在 $t$ 時刻的位置，大家一定立馬就能給出答案： $s_t=s_0+v_t$ 。沒錯，理論上 $t$ 時刻的位置就應該是這個值。但在實際工程應用中，位置 $s$ 和速度 $v$ 並不會是理想值，它會有一定的不確定性，我們假設這個不確定性是滿足高斯分布的。如果沒有後驗的話，這個不確定性會不斷地疊加，就如上圖（a）所示，隨著peanut的不斷前行，位置 $s$ 概率分布的方差會越來越大，peanut對自己位置的估計也就越來越不準確。為了確保位姿估計的準確性，加入了一個觀測值去修正計算得到的速度，例如用里程計去測量每次走過的距離 $s_t』$ ，如上圖（b）所示，藍色表示peanut當前時刻的先驗估計，它由上一時刻的估計值 $s_{t-1}$ 通過運動方程求得；綠色表示從里程計獲得的觀測值得概率分布，它與先驗估計有一定的偏差；紫色表示通過卡爾曼濾波後，修正的概率分布。從濾波的結果來看，後驗估計也是高斯分布，它的分布綜合了先驗和觀測的期望，且縮小了方差。

卡爾曼濾波的一般步驟

通過上面的介紹，相信大家對卡爾曼濾波能幹啥應該有個大致的了解了，那麼它是如何使用的呢？廢話不多說，先上公式，卡爾曼濾波的經典五步：

$ar{μ_t }=A_t μ_{t-1}+B_t u_t$ (4)

$ar{σ_t }=A_t σ_{t-1} A_t^T+r_t$ (5)

$K_t=ar{σ_t } C_t^T ({C_t ar{σ_t }C_t^T+q_t})^{-1}$ (6)

$μ_t=ar{μ_t }+K_t (z_t-C_t ar{μ_t })$ (7)

$σ_t=(I-K_t C_t)ar{σ_t }$ (8)

好吧，看到上面的公式大家可能會有點懵，下面小編盡量用比較通俗的語句解釋下這五步具體做了些什麼。

計算狀態轉移的期望， $μ_{t-1}$ 表示上一狀態的期望值，其實就是上一次後驗分布的期望， $u_t$ 是控制量，這樣就求出了先驗概率分布的期望值；
光有期望值還不夠，還需要計算方差才能得到一個完整的正態分布，公式5就是計算先驗概率方差的過程，其中 $σ_{t-1}$ 表示上一次後驗的方差， $r_t$ 是狀態轉移引入高斯雜訊的方差。
得到先驗概率分布後，就需要通過貝葉斯估計求解後驗概率分布。公式6是求解卡爾曼增益 $K$ ，它通過觀測概率分布的方差 $q_t$ 和先驗概率分布的方差求得。
卡爾曼增益 $K$ 主要用來衡量「預測值」和「觀測值」的比重，即是更相信預測的結果還是更相信觀測的結果；具體表現在公式7，後驗的期望是先驗的期望加上卡爾曼增益乘以估計和觀測期望的偏差。
後驗的期望得到後，再通過公式8更新後驗的方差，就得到了後驗估計得概率分布。

通過上述五步就精確地求解了一個「線性高斯系統」的後驗估計，演算法里運算量最大的矩陣求逆運算和矩陣乘法運算時間複雜的分別可近似為 $O(k^{2.4})$ 和 $O(k^2)$ ， $k$ 是矩陣維度。相對其他演算法，卡爾曼濾波演算法是相當高效的。

卡爾曼濾波的變種

卡爾曼濾波雖然高效且精確，但是他有個致命的缺陷，即要求系統是高斯線性的，而實際應用中，絕大多數系統不是高斯線性的。於是就有了所謂的擴展卡爾曼濾波EKF（Extended Kalman Filter）和無跡卡爾曼濾波UKF（Unscented Kalmen Filter ）的變種。

EKF和UKF都是將實際狀態轉移模型局部線性化，只不過前者採用的是泰勒展開進行線性化，而後者採用的是通過加權統計線性回歸過程實現隨機線性化。通俗的講，EKF是計算非線性函數的切線來進行線性化的，因此EKF需要知道狀態轉移方程的雅可比矩陣（Jacobian Matrix），UKF則是通過提取所謂的δ點，用這些δ點擬合的直線作為局部線性化的參考，δ的取值與分布的協方差有關。EKF和UKF的演算法複雜度近似，一般情況下EKF會稍微快一點，而估計的結果一般情況下UKF要優於EKF。

卡爾曼濾波作為經典的估計演算法，被廣泛應用在各個領域之中，在機器人領域裡，卡爾曼濾波主要應用在多感測器融合，SLAM的最優估計等問題中。以上就是本期關於卡爾曼濾波的內容，下期小編將給大家介紹粒子濾波的相關內容。