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

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

1 前言

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

2 EKF-SLAM算法

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

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

EKF-SLAM算法步驟：

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

⑵觀測(cè)：利用傳感器觀測(cè)模型得到機(jī)器人對(duì)于第i個(gè)路標(biāo)的實(shí)際觀測(cè) ，結(jié)合預(yù)測(cè)步驟得到的 ，此時(shí)可以得到新息（Innovation），以及該新息的協(xié)方差矩陣：

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

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

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

3 小結(jié)

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

[參考文獻(xiàn)]

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

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

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

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

[5]范文兵，劉春風(fēng)，張素貞.一種強(qiáng)跟蹤擴(kuò)展卡爾曼濾波器的改進(jìn)算法[J].控制與決策，2006，21（1）：73-76.