基于卡爾曼濾波與粒子濾波的SLAM研究綜述

孫海波 童紫原 唐守鋒 童敏明 紀(jì)玉明

摘要：同時(shí)定位與地圖構(gòu)建（SLAM）問(wèn)題在移動(dòng)機(jī)器人研究領(lǐng)域受到了廣泛關(guān)注，其是機(jī)器人能否實(shí)現(xiàn)完全自主運(yùn)動(dòng)功能的關(guān)鍵。首先闡述了SLAM系統(tǒng)相關(guān)模型，并介紹了經(jīng)典卡爾曼濾波相關(guān)知識(shí);其次介紹基于擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波與粒子濾波的SLAM算法如何解決現(xiàn)實(shí)世界的非線性、非高斯問(wèn)題，并總結(jié)了各算法優(yōu)缺點(diǎn);最后，展望了基于卡爾曼濾波的SLAM算法發(fā)展趨勢(shì)。

關(guān)鍵詞關(guān)鍵詞：SLAM;卡爾曼濾波;擴(kuò)展卡爾曼濾波;無(wú)跡卡爾曼濾波;粒子濾波

SLAM Research Based on Kalman Filter and Particle Filter

SUN Hai?bo?TONG Zi?yuan?TANG Shou?feng?TONG Min?ming?JI Yu?ming

（1. School of Information and Control Engineering， China University of Mining and Technology，Xuzhou 221000，China;

2.School of Information and Electrical Engineering，University of New South Wales， Sydney2052，Australia;

3.Xuzhou Hanlin Technology Co. LTD，Xuzhou 221000，China）

Abstract：The problem of simultaneous localization and mapping （SLAM） in the field of?mobile robotics is getting more and more attention， which is regarded as the key technology to the robot's ability to achieve fully autonomous motion functions. first of all ，this article elaborates the relevant models of SLAM system， and introduces the related knowledge of classical?Kalman filter. Secondly， the SLAM algorithm based on extended Kalman filter， unscented?Kalman filter and particle filter is introduced to solve the nonlinear and non?Gaussian problems in the real world. The advantages and disadvantages of each algorithm are summarized. Finally， the development trend of SLAM algorithm based on Kalman filter is forecasted.

Key Words：SLAM; Kalman filter ; unscented Kalman filter; particle filter

0?引言

目前，基于SLAM技術(shù)的機(jī)器人定位技術(shù)有激光儀定位、紅外線定位、超聲波定位、機(jī)器視覺(jué)定位等[1]。SLAM問(wèn)題是由概率機(jī)器人學(xué)演變而來(lái)的，其原理是將模型方法與行為方法相結(jié)合，處理機(jī)器人在未知環(huán)境中的自主移動(dòng)問(wèn)題，是實(shí)現(xiàn)移動(dòng)機(jī)器人避障、路徑規(guī)劃以及目標(biāo)跟蹤等復(fù)雜任務(wù)的前提與基礎(chǔ)。同時(shí)，卡爾曼濾波從系統(tǒng)狀態(tài)切入，解決了非平穩(wěn)隨機(jī)過(guò)程等動(dòng)態(tài)系統(tǒng)的信號(hào)處理問(wèn)題。卡爾曼濾波只需要k時(shí)刻的測(cè)量量與k-1時(shí)刻的估計(jì)量，并通過(guò)不斷預(yù)估與修正，即可實(shí)時(shí)估計(jì)信號(hào)狀態(tài)，該方式節(jié)省了大量?jī)?chǔ)存與運(yùn)算空間?，F(xiàn)階段，卡爾曼濾波已演變出不同版本的算法形式，從而大大地提高了算法的精度與穩(wěn)定性，并成為定位與導(dǎo)航系統(tǒng)中的研究熱點(diǎn)。

1?SLAM系統(tǒng)數(shù)學(xué)模型

SLAM（Simultaneous Localization and Mapping）是由Smith、Self等[2]在1988年提出的，SLAM問(wèn)題可以描述為： 機(jī)器人在未知環(huán)境中從一個(gè)未知位置開(kāi)始移動(dòng)，在移動(dòng)過(guò)程中根據(jù)位置估計(jì)與地圖進(jìn)行自身定位，并在自身定位的基礎(chǔ)上建造增量式地圖，以實(shí)現(xiàn)機(jī)器人的自主定位及導(dǎo)航[3]。SLAM 問(wèn)題是在概率論理論基礎(chǔ)上建立相應(yīng)概率模型，如西安工程大學(xué)的王帆等[4]提出的運(yùn)動(dòng)概率模型、觀測(cè)概率模型與SLAM問(wèn)題整體概率模型是研究SLAM問(wèn)題最重要的3個(gè)基礎(chǔ)模型。

1.1?運(yùn)動(dòng)概率模型

運(yùn)動(dòng)概率模型是經(jīng)典的SLAM模型，可以?用條件概率pX?v?kX?v?k-1，U?k加以描述。其表示的具體含義為移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中輸入信息U?k，在其作用下推導(dǎo)出上一時(shí)刻位姿X?v?k-1運(yùn)動(dòng)到當(dāng)前時(shí)刻X?v?k的?條件概率。

1.2?觀測(cè)概率模型

用條件?概率pZ?kX?v?k，N描述觀測(cè)概率模型，其含義是在已知k時(shí)刻移動(dòng)機(jī)器人位姿X?v?k與N（環(huán)境特征）的條件下，求取K時(shí)刻測(cè)量Z?k的?條件概率。

1.3?SLAM問(wèn)題概率模型

用聯(lián)合后驗(yàn)?pX?kZ?1→k，U?1→k，X?v?0?描述整個(gè)SLAM問(wèn)題的概率模型，該模型描述為：在給定移動(dòng)機(jī)器人初始位?姿X?v?0、已知所有控制輸入信息U?1-k且獲得所有觀測(cè)信息Z?1-k的前提下，推導(dǎo)出k時(shí)刻?機(jī)器人位姿與環(huán)境特征組合的聯(lián)合狀態(tài)向量的條件概率。

綜上所述，SLAM問(wèn)題涉及兩個(gè)關(guān)鍵方面：①利用傳感器采集周?chē)h(huán)境信息，并對(duì)信息進(jìn)行數(shù)據(jù)處理，從而構(gòu)建與環(huán)境特征匹配的SLAM地圖;②根據(jù)環(huán)境特征信息進(jìn)行裝置的位姿估計(jì)[5]。SLAM算法既是對(duì)移動(dòng)機(jī)器人位姿及環(huán)境特征位置估計(jì)的貝葉斯?fàn)顟B(tài)評(píng)估問(wèn)題，又是在運(yùn)動(dòng)概率模型基礎(chǔ)上，通過(guò)輸入觀測(cè)數(shù)據(jù)，預(yù)測(cè)并更新無(wú)人機(jī)位姿及路標(biāo)位置的導(dǎo)航問(wèn)題[6]。SLAM系統(tǒng)數(shù)學(xué)模型如圖1所示。

2?基于卡爾曼濾波與粒子濾波的SLAM算法

由于SLAM算法在移動(dòng)機(jī)器人位姿估計(jì)中的廣泛應(yīng)用，已提出了半個(gè)世紀(jì)的卡爾曼濾波至今仍是學(xué)者們的研究熱點(diǎn)。本部分將介紹經(jīng)典卡爾曼濾波SLAM算法與幾種常用的SLAM算法。

2.1?經(jīng)典卡爾曼濾波SLAM算法

經(jīng)典卡爾曼濾波的核心思想是遞推求解，在忽略之前數(shù)據(jù)量的前提下，根據(jù)x?k-1遞推出x?k?。經(jīng)典卡爾曼濾波算法步驟與結(jié)構(gòu)如下：

步驟1：設(shè)置初始量?0、p?0。

步驟2：狀態(tài)預(yù)測(cè)并推算誤差協(xié)方差。

步驟3：計(jì)算卡爾曼增益。

步驟4：計(jì)算誤差。

步驟5：更新誤差協(xié)方差。

由以上步驟得出，經(jīng)典卡爾曼濾波適用于線性高斯系統(tǒng)。在解決移動(dòng)機(jī)器人與無(wú)人機(jī)的各種實(shí)際導(dǎo)航問(wèn)題時(shí)，系統(tǒng)大多是非線性和非高斯的，因此，拓展卡爾曼濾波適用范圍。國(guó)內(nèi)外學(xué)者相繼提出基于EKF[7]（擴(kuò)展卡爾曼濾波）的SLAM算法、基于UKF[8]（無(wú)跡卡爾曼濾波）的SLAM算法與基于PF[9]（粒子濾波）的SLAM算法。

2.2?基于擴(kuò)展卡爾曼濾波（EKF）的SLAM算法

擴(kuò)展卡爾曼濾波算法以卡爾曼濾波為基礎(chǔ)，與經(jīng)典卡爾曼濾波算法的不同之處在于，其僅將卡爾曼濾波里的矩陣A和H用公式函數(shù)f與h代替，如公式（1）、（2）所示。

然后采用泰勒展開(kāi)式的一階逼近非線性系統(tǒng)，如公式（3）、（4）所示，對(duì)其狀態(tài)進(jìn)行線性估計(jì)，并通過(guò)更新系統(tǒng)協(xié)方差矩陣對(duì)系統(tǒng)狀態(tài)進(jìn)行遞歸更新[10]。

基于EKF的SLAM算法可分為3個(gè)步驟：預(yù)測(cè)過(guò)程、更新過(guò)程與向量增廣，算法結(jié)構(gòu)如圖2所示。

EKF忽略了泰勒展開(kāi)式的高階項(xiàng)，從而給估計(jì)帶來(lái)誤差，所以要求系統(tǒng)的狀態(tài)方程與廣策方程都非常接近線性并且連續(xù)，才能得到較好的濾波結(jié)果。同時(shí)因?yàn)闋顟B(tài)噪聲與觀測(cè)噪聲的協(xié)方差矩陣保持不變，所以對(duì)其的估計(jì)可能不夠準(zhǔn)確，容易造成誤差積累，導(dǎo)致濾波器發(fā)散。因此，EKF?SLAMY適用于環(huán)境較為簡(jiǎn)單、環(huán)境特征較少、定位要求不高的場(chǎng)合。

2.3?基于無(wú)跡卡爾曼濾波（UKF）的SLAM算法

為解決EKF中的問(wèn)題，研究者提出一種基于無(wú)跡卡爾曼濾波的SLAM算法。無(wú)跡卡爾曼濾波采用無(wú)跡變換UT處理非線性均值與協(xié)方差，用一組確定的樣本點(diǎn)方式預(yù)測(cè)新時(shí)刻的狀態(tài)，是一種遞歸式貝葉斯估計(jì)方法[11?12]。該組確定性樣本點(diǎn)被稱(chēng)為Sigma點(diǎn)，其在非線性函數(shù)映射下，得到各點(diǎn)權(quán)重。無(wú)跡卡爾曼濾波算法步驟如下：

（1）初始化。

（2）Sigma采樣。使用對(duì)稱(chēng)采樣策略，得到當(dāng)前時(shí)刻狀態(tài)估計(jì)的點(diǎn)集。

（3）預(yù)測(cè)方程。

（4）更新方程。

機(jī)器人運(yùn)動(dòng)中不斷地進(jìn)行步驟（2）、（4），即可獲得機(jī)器人各時(shí)刻的準(zhǔn)確位置。

UKF通過(guò)確定后的采樣，可近似狀態(tài)的后驗(yàn)PDF，解決了系統(tǒng)非線性加劇后的濾波發(fā)散問(wèn)題，但是UKF算法在系統(tǒng)后驗(yàn)概率密度為非高斯的情況下，濾波結(jié)果依舊不理想。而且由于噪聲是隨時(shí)間變化的，會(huì)造成統(tǒng)計(jì)特性的不準(zhǔn)確，從而使濾波發(fā)散與定位誤差變大。

2.4?基于粒子濾波（PF）的SLAM算法

粒子濾波是指利用狀態(tài)空間的一組加權(quán)隨機(jī)樣本逼近狀態(tài)量的后驗(yàn)概率分布。為了有效避免EKF算法因系統(tǒng)線性化近似造成的誤差，基于粒子濾波的SLAM算法采用一組加權(quán)樣本逼近最優(yōu)估計(jì)，從而不受模型線性程度與高斯假設(shè)的約束。將粒子濾波算法引入機(jī)器人的同時(shí)定位與地圖構(gòu)建功能是由Murphy & Doucet[13?14]提出的，并由Montemerla等[15?16]研究者完成粒子濾波算法框架的推廣。粒子濾波算法步驟可分為初始化、預(yù)測(cè)、觀測(cè)更新、再采樣與結(jié)果輸出幾個(gè)步驟，如圖3所示。

PF?SLAM算法的核心思想是將聯(lián)合狀態(tài)向量后驗(yàn)概率依據(jù)概率公式分解為機(jī)器人軌跡部分與以軌跡為條件的特征部分[15]。樣本重要性采樣是粒子濾波中常用的采樣方法，但該方法需要考慮過(guò)去時(shí)刻的觀測(cè)向量，從而增加了存儲(chǔ)與計(jì)算壓力，所以研究人員提出了序貫重要性采樣方法。其核心思想是利用一系列隨機(jī)樣本的加權(quán)與表示所需狀態(tài)向量的后驗(yàn)概率分布，得到狀態(tài)向量的估計(jì)值。當(dāng)粒子數(shù)目趨于無(wú)窮大時(shí)，可以用概率理論（大數(shù)定理與中心極限定理）加以證明，蒙特卡羅特性與后驗(yàn)概率密度函數(shù)表示等價(jià)，序貫重要性采樣濾波器接近于最優(yōu)貝葉斯濾波[17]，而該算法在使用過(guò)程中還需考慮粒子退化現(xiàn)象[18]。當(dāng)濾波一段時(shí)間后，用預(yù)測(cè)結(jié)果帶來(lái)的概率分布更新各粒子權(quán)重，匹配觀測(cè)結(jié)果的粒子會(huì)賦予更高權(quán)重，最后少數(shù)具有很高權(quán)重的粒子將起主導(dǎo)作用，而大多數(shù)權(quán)重較低的粒子，對(duì)結(jié)果影響較小，但還要花費(fèi)大量時(shí)間對(duì)權(quán)重較低的粒子進(jìn)行更新，所以無(wú)法避免粒子的退化現(xiàn)象。因此，要引入一個(gè)量度，即有效粒子的采樣尺度，當(dāng)采樣尺度小于測(cè)量值時(shí)需要重新采樣，也即再采樣。

由于PF?SLAM獨(dú)立于系統(tǒng)模型，所以在解決了EKF?SLAM與UKF?SLAM中由于線性化和噪聲非高斯造成的誤差后，其適用范圍更廣，但PF?SLAM在決定其狀態(tài)估計(jì)優(yōu)劣的最優(yōu)參數(shù)分布時(shí)很難進(jìn)行采樣。

3?基于卡爾曼濾波的SLAM算法研究趨勢(shì)

未來(lái)定位導(dǎo)航的發(fā)展趨勢(shì)是將多種算法進(jìn)行融合，單一算法已難以滿足需求，因此需要根據(jù)不同要求選擇不同算法。多傳感器的信息融合技術(shù)也逐漸成熟，從而提高了算法的實(shí)時(shí)性與定位精度。隨著機(jī)器學(xué)習(xí)的興起，與神經(jīng)網(wǎng)絡(luò)融合的卡爾曼濾波也被提出。

4?結(jié)語(yǔ)

針對(duì)運(yùn)動(dòng)機(jī)器人等移動(dòng)裝置的同時(shí)定位導(dǎo)航問(wèn)題，本文在SLAM系統(tǒng)的數(shù)學(xué)模型基礎(chǔ)上，對(duì)各種SLAM算法進(jìn)行闡述與總結(jié)，對(duì)理論及應(yīng)用中存在的問(wèn)題作出詳細(xì)介紹，并總結(jié)了各算法優(yōu)缺點(diǎn)，最后展望了SLAM算法未來(lái)發(fā)展趨勢(shì)?；诳柭鼮V波與粒子濾波的SLAM算法的應(yīng)用還有較大提升空間，未來(lái)仍需作進(jìn)一步深入研究。

參考文獻(xiàn)參考文獻(xiàn)：

[1]?黃玉強(qiáng).移動(dòng)機(jī)器人在未知環(huán)境下的基于視覺(jué)系統(tǒng)的地圖創(chuàng)建[D].南京：南京大學(xué)，2012.

[2]?曹美會(huì)，鮮斌，張旭，等.基于視覺(jué)的四旋翼無(wú)人機(jī)自主定位與控制系統(tǒng)[J].信息與控制，2015，44（2）：190?196.

[3]?白志軍.四旋翼無(wú)人機(jī)室內(nèi)自主導(dǎo)航系統(tǒng)的研究與實(shí)現(xiàn)[D].廈門(mén)：廈門(mén)大學(xué)，2014.

[4]?王帆.基于卡爾曼濾波和粒子濾波的移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建研究[D].西安：西安工程大學(xué)，2012.

[5]?劉書(shū)池.面向工業(yè)互聯(lián)網(wǎng)的井下無(wú)人機(jī)單目視覺(jué)SLAM定位方法[D].北京：北京交通大學(xué)，2017.

[6]?楊曉曉.室內(nèi)機(jī)器人單目視覺(jué)同時(shí)定位與地圖構(gòu)建技術(shù)研究與實(shí)現(xiàn)[D].成都：成都信息工程學(xué)院，2014.

[7]?馮少江，徐澤宇，石明全，等.基于改進(jìn)擴(kuò)展卡爾曼濾波的姿態(tài)解算算法研究[J].計(jì)算機(jī)科學(xué)，2017，44（9）：227?229.

[8]?李志雄，王姮.基于無(wú)跡卡爾曼濾波的運(yùn)動(dòng)機(jī)器人定位研究[J].輕工科技，2015（11）：73?74，76.

[9]?韓同輝，沈超，沈靜，等.基于PF算法的移動(dòng)機(jī)器人定位研究[J].機(jī)電一體化，2012，18（3）：13?16.

[10]?王聰偉.基于擴(kuò)展卡爾曼濾波的足式機(jī)器人運(yùn)動(dòng)速度估計(jì)研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2014.

[11]?金峰，蔡鶴皋.機(jī)器人IMU與激光掃描測(cè)距傳感器數(shù)據(jù)融合[J].機(jī)器人，2000（6）：470?473.

[12]?駱云祥.非線性濾波在移動(dòng)機(jī)器人SLAM中的應(yīng)用[D].南京：南京理工大學(xué)，2009.

[13]?MURPHY K. Bayesian map learning in dynamic environments[C].Proc?of?the?Conf?on Neural Information Processing Systems，NIPS，Denver，USA ，1999：1015?1021.

[14]?DOUCET A，F(xiàn)REITAS J DE，MURPHY K，et al.Rao?Blackwellized particle filtering for dynamic Bayesion networks[C].Proc of the Conf on Uncertainty in Artifical Intelligence，UAL，Stanford，USA，2000：176?185.

[15]?MONTEMERLO M， THRUN S， ROLLER D， et al. FastSLAM 2.0： an improved particle filtering algorithm for simultaneous localization and mapping that provably converges[J]. Proc.int.conf.on Artificial Intelligence， 2003， 133（1）：1151?1156.

[16]?MONTEMERLO M， THUN S， KOLLER D， et al. FastSLAM： a factored solution to simultaneous mapping and localization[C].Proceedings of the National Conference on Artificial Intelligence，2002：590?605.

[17]?KIM C， SAKTHIVEL R， WAN K C. Unscented FastSLAM： a robust algorithm for the simultaneous localization and mapping problem[C]. IEEE International Conference on Robotics and Automation. IEEE， 2007：2439?2445.

[18]?SHOJAIE K，AHMADI K，SHAHRI A M.Effects of iteration in Kalman filters family?for improvement of estimation accuracy in SLAM [C] .IEEE International Conference?on Advanced Intelligent Mechatronics，2007：1?6.