孫海波 童紫原 唐守鋒 童敏明 紀玉明

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

關鍵詞關鍵詞：SLAM;卡爾曼濾波;擴展卡爾曼濾波;無跡卡爾曼濾波;粒子濾波

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

1?SLAM系統(tǒng)數學模型

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

1.1?運動概率模型

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

1.2?觀測概率模型

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

1.3?SLAM問題概率模型

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

綜上所述，SLAM問題涉及兩個關鍵方面：①利用傳感器采集周圍環(huán)境信息，并對信息進行數據處理，從而構建與環(huán)境特征匹配的SLAM地圖;②根據環(huán)境特征信息進行裝置的位姿估計[5]。SLAM算法既是對移動機器人位姿及環(huán)境特征位置估計的貝葉斯狀態(tài)評估問題，又是在運動概率模型基礎上，通過輸入觀測數據，預測并更新無人機位姿及路標位置的導航問題[6]。SLAM系統(tǒng)數學模型如圖1所示。

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

由于SLAM算法在移動機器人位姿估計中的廣泛應用，已提出了半個世紀的卡爾曼濾波至今仍是學者們的研究熱點。本部分將介紹經典卡爾曼濾波SLAM算法與幾種常用的SLAM算法。

2.1?經典卡爾曼濾波SLAM算法

經典卡爾曼濾波的核心思想是遞推求解，在忽略之前數據量的前提下，根據x?k-1遞推出x?k?。經典卡爾曼濾波算法步驟與結構如下：

步驟1：設置初始量?0、p?0。

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

步驟3：計算卡爾曼增益。

步驟4：計算誤差。

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

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

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

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

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

基于EKF的SLAM算法可分為3個步驟：預測過程、更新過程與向量增廣，算法結構如圖2所示。

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

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

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

（1）初始化。

（2）Sigma采樣。使用對稱采樣策略，得到當前時刻狀態(tài)估計的點集。

（3）預測方程。

（4）更新方程。

機器人運動中不斷地進行步驟（2）、（4），即可獲得機器人各時刻的準確位置。

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

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

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

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

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

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

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

4?結語

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

參考文獻參考文獻：

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

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

[3]?白志軍.四旋翼無人機室內自主導航系統(tǒng)的研究與實現[D].廈門：廈門大學，2014.

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

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

[6]?楊曉曉.室內機器人單目視覺同時定位與地圖構建技術研究與實現[D].成都：成都信息工程學院，2014.

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

[8]?李志雄，王姮.基于無跡卡爾曼濾波的運動機器人定位研究[J].輕工科技，2015（11）：73?74，76.

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

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

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

[12]?駱云祥.非線性濾波在移動機器人SLAM中的應用[D].南京：南京理工大學，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，FREITAS 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.