基于擴(kuò)展卡爾曼的SLAM問(wèn)題研究

王 望 許 凱 顏 瑾 西安石油大學(xué)電子工程學(xué)院 710065

?

基于擴(kuò)展卡爾曼的SLAM問(wèn)題研究

王 望 許 凱 顏 瑾 西安石油大學(xué)電子工程學(xué)院 710065

【文章摘要】

移動(dòng)機(jī)器人是目前的研究熱點(diǎn)，而同時(shí)定位與地圖創(chuàng)建是機(jī)器人研究領(lǐng)域難點(diǎn)，是實(shí)現(xiàn)機(jī)器人在未知環(huán)境下自主導(dǎo)航的前提。目前，對(duì)于機(jī)器人在已知地圖的自主定位已經(jīng)有了多種解決辦法，但是隨著工業(yè)的發(fā)展和人類(lèi)探索領(lǐng)域的擴(kuò)大，在一些對(duì)人有危害性的作業(yè)現(xiàn)場(chǎng)（如有毒氣體檢測(cè)、礦山開(kāi)采，災(zāi)難搶險(xiǎn)等）或者不適合人類(lèi)工作的地域（如海洋和海底探測(cè)、星際探險(xiǎn)等），人們希望機(jī)器人能夠代替人類(lèi)工作，實(shí)現(xiàn)難以獲得機(jī)器人工作的環(huán)境信息和定位信息。而移動(dòng)機(jī)器人的同時(shí)定位與地圖創(chuàng)建SLAM則能夠較好的解決這個(gè)問(wèn)題。

【關(guān)鍵詞】

擴(kuò)展卡爾曼；SLAM；移動(dòng)機(jī)器人

1　SLAM問(wèn)題

SLAM（Simultaneous Localization And Mapping）是移動(dòng)機(jī)器人構(gòu)建環(huán)境地圖同時(shí)進(jìn)行自身定位的過(guò)程。SLAM中，在沒(méi)有任何先驗(yàn)信息的情況下，移動(dòng)機(jī)器人結(jié)合里程儀等外部測(cè)量設(shè)備檢測(cè)外界的環(huán)境特性，在線估測(cè)平面內(nèi)的運(yùn)行軌跡和所有環(huán)境特征的定位。

2　擴(kuò)展卡爾曼濾波器

圖1　仿真結(jié)果

卡爾曼濾波器估計(jì)用線性隨機(jī)差分方程描述的離散時(shí)間過(guò)程的狀態(tài)變量，廣泛應(yīng)用于定位、創(chuàng)建地圖和導(dǎo)航等算法上。但如果被估計(jì)的過(guò)程和（或）觀測(cè)變量與過(guò)程的關(guān)系是非線性的，那么將期望和方差線性化的卡爾曼濾波器稱(chēng)作擴(kuò)展卡爾曼濾波器（Extended Kalman Filter），簡(jiǎn)稱(chēng)EKF。

3　EKF-SLAM環(huán)境特征

在EKF-SLAM中，地圖是一個(gè)巨大的傳感器向量集和路標(biāo)狀態(tài)。EKF通過(guò)預(yù)測(cè)和校正過(guò)程確定地圖。為了獲取真實(shí)的測(cè)量值，EKF過(guò)程中有一個(gè)路標(biāo)初始化的步驟，新觀測(cè)的路標(biāo)會(huì)被添加到地圖中。路標(biāo)初始化由逆觀測(cè)函數(shù)及其雅可比進(jìn)行計(jì)算。

在EKF-SLAM中，地圖由一個(gè)使用平均值和狀態(tài)向量協(xié)方差矩陣的高斯變量P表示：

4　EKF-SLAM過(guò)程

4.1地圖初始化

初始時(shí)刻地圖中沒(méi)有路標(biāo)，因此n=0。通常將當(dāng)前的機(jī)器人位姿作為初始位置：，

4.2機(jī)器人運(yùn)動(dòng)狀態(tài)的預(yù)測(cè)

在EKF中，設(shè)x為狀態(tài)向量，u為控制向量，n為擾動(dòng)向量，那么可以將時(shí)間更新函數(shù)表示為：。

在SLAM問(wèn)題中移動(dòng)機(jī)器人的狀態(tài)是隨時(shí)間改變的，因此狀態(tài)向量的每部分都有不同的表示：

4.3地圖中路標(biāo)的觀測(cè)值

在EKF中，設(shè)y是包括噪聲的測(cè)量值，h（）是觀測(cè)函數(shù)，v是測(cè)量噪聲，路標(biāo)觀測(cè)值由EKF一步步處理。觀測(cè)值僅僅依賴(lài)于機(jī)器人位置及狀態(tài)R，傳感器狀態(tài)S和路標(biāo)狀態(tài)。假設(shè)已經(jīng)觀測(cè)到路標(biāo)，定義觀測(cè)函數(shù)：

在這些等式中，式7和8為觀測(cè)更新均值和協(xié)方差矩陣，式9和10組成了濾波器的更新在測(cè)量路標(biāo)時(shí)，這些等式會(huì)迭代應(yīng)用。

5　測(cè)試結(jié)果與分析

運(yùn)用MATLAB進(jìn)行仿真，圖1中深色的橢圓形表示校正的誤差范圍，藍(lán)色實(shí)心點(diǎn)集表示每一個(gè)時(shí)刻k對(duì)路標(biāo)的預(yù)測(cè)值，綠色實(shí)心點(diǎn)表示機(jī)器人預(yù)定軌跡，綠色圓圈表示對(duì)路標(biāo)的后驗(yàn)觀測(cè)結(jié)果，紅色星號(hào)表示預(yù)置路標(biāo)。清晰的顯示了移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建過(guò)程。

由仿真結(jié)果可知應(yīng)用基于EKF進(jìn)行同時(shí)定位與地圖創(chuàng)建，EKF能夠準(zhǔn)確地跟蹤機(jī)器人的位置，SLAM可以完成機(jī)器人的同時(shí)定位與地圖構(gòu)建的，同時(shí)可以看出，如何在大規(guī)模環(huán)境下提高SLAM算法的準(zhǔn)確度和實(shí)時(shí)性是下一步研究工作的重點(diǎn)。