擴展卡爾曼體系下的SLAM算法計算框架

摘 要：擴展卡爾曼體系下的SLAM算法是指SLAM算法中含有狀態預測過程和狀態更新過程。為了便于理解擴展卡爾曼體系下的SLAM算法計算過程，本文從濾波算法計算框架角度對它們分別進行了描述，對基于EKF的SLAM算法進行了說明和總結。

關鍵詞：擴展卡爾曼體系；SLAM算法；計算框架

1 前言

SLAM問題的本質是移動機器人整個路徑的概率估計問題。作為解決SLAM問題的主要方法之一，擴展卡爾曼濾波算法（EKF）在模型基本匹配時具有較高的估計精度，且具有模型簡單、易于實現等特點[1]。

2 EKF-SLAM算法

在應用EKF處理SLAM問題時，其變量被假設服從高斯分布，可用均值及方差表示， 分別為系統在k時刻的狀態估計值及狀態估計協方差[2]：

其中，▽fx，▽fu分別為f（·）相對于X和u的Jacobian矩陣，nv，nM，nu分別為機器人位姿 、地圖 、及控制命令uk的維數。▽fvx、▽fvu分別為fv（·）相對于Xv和u的Jacobian矩陣。f（·）是系統的狀態轉移函數，通常狀況下是非線性的[3]。在EKF-SLAM中，系統噪聲υk及觀測噪聲ωk認為是均值為零，方差分別為Qk及Rk的高斯白噪聲，且系統噪聲與觀測噪聲相互獨立[4]。

EKF-SLAM算法步驟：

⑴預測：利用系統的運動模型，結合k時刻系統狀態估計值 和控制命令uk，預測系統在k+1時刻的狀態（ ）、狀態估計的協方差矩陣（ ）和系統的觀測值（ ）：

⑵觀測：利用傳感器觀測模型得到機器人對于第i個路標的實際觀測 ，結合預測步驟得到的 ，此時可以得到新息（Innovation），以及該新息的協方差矩陣：

⑶更新：EKF-SLAM算法利用下式更新k+1時刻的系統狀態估計及其相應的協方差矩陣：

⑷向量增廣：傳感器在任意時刻觀測到的環境特征，可能即包括地圖中已有的特征也有新特征。其中已有特征可以用來更新狀態預測值，而新特征則要進行初始化并加入到狀態向量中[5]

其中，▽gx，▽gz分別為g（·）相對于Xv和Z的Jacobian矩陣。

3 小結

擴展卡爾曼濾波算法（EKF）的運用使得非線性問題線性化，這樣線性化后的預測值的方差依然可以按原來的方法進行線性計算。其原理是在上一時刻的預測點將系統狀態方程和觀測方程進行Taylor展開，并忽略所有非線性展開高階項，來實現對非線性方程的近似線性化。目前EKF已成為了最基本的SLAM問題解決方案。

[參考文獻]

[1]周武，趙春霞，沈亞強，等.基于全局觀測地圖模型的SLAM研究[J].機器人，2010，32（5）：647-654.

[2]張文玲，朱明清，陳宗海.基于強跟蹤UKF的自適應SLAM算法[J].機器人，2010，32（2）：190-195.

[3]郭劍輝.移動機器人同時定位與地圖構建方法研究[D].[博士學位論文].南京：南京理工大學計算機科學與技術學院，2008.

[4]段戰勝，韓崇昭，黨宏社.一種帶未知時變系統噪聲水平的目標跟蹤濾波器[J].系統仿真學報，2004，16（11）：2591-2593.

[5]范文兵，劉春風，張素貞.一種強跟蹤擴展卡爾曼濾波器的改進算法[J].控制與決策，2006，21（1）：73-76.