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

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

關(guān)鍵詞：擴展卡爾曼體系；SLAM算法；計算框架

1 前言

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

2 EKF-SLAM算法

在應(yīng)用EKF處理SLAM問題時，其變量被假設(shè)服從高斯分布，可用均值及方差表示， 分別為系統(tǒng)在k時刻的狀態(tài)估計值及狀態(tài)估計協(xié)方差[2]：

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

EKF-SLAM算法步驟：

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

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

⑶更新：EKF-SLAM算法利用下式更新k+1時刻的系統(tǒng)狀態(tài)估計及其相應(yīng)的協(xié)方差矩陣：

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

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

3 小結(jié)

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

[參考文獻]

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

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

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

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

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