段鎖林，談　剛，周玉勤，朱海勇

(1.常州大學(xué) 機(jī)器人研究所，江蘇 常州　213164；2.江蘇潤(rùn)儀儀表有限公司，江蘇 金湖　211600)

?

移動(dòng)機(jī)器人SLAM問題的研究

段鎖林1，談剛1，周玉勤2，朱海勇2

(1.常州大學(xué) 機(jī)器人研究所，江蘇 常州213164；2.江蘇潤(rùn)儀儀表有限公司，江蘇 金湖211600)

針對(duì)移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建(SLAM)的問題，就擴(kuò)展卡爾曼(EKF)算法所存在的缺陷即不適合大范圍環(huán)境及密集環(huán)境等特征數(shù)量較大的場(chǎng)合，提出了一種改進(jìn)的EKF-SLAM算法;它在擴(kuò)展卡爾曼(EKF)算法上采用Rao-Blackwellise的分解思想-分解估計(jì)構(gòu)架,將SLAM問題分解為路徑估計(jì)和地圖估計(jì)兩個(gè)問題從而進(jìn)行預(yù)測(cè)步驟，觀測(cè)步驟，更新步驟和向量增廣步驟4個(gè)步驟；仿真結(jié)果顯示改進(jìn)的EKF-SLAM算法比EKF-SLAM算法在特征數(shù)量較大的場(chǎng)合更具有優(yōu)異性;它大大降低了計(jì)算復(fù)雜度，提高了準(zhǔn)確性，為在比較復(fù)雜環(huán)境下實(shí)時(shí)解決移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建(SLAM)的問題提供了一種有效方法。

移動(dòng)機(jī)器人；同時(shí)定位與地圖創(chuàng)建；擴(kuò)展卡爾曼算法；路徑估計(jì)；地圖估計(jì)

0　引言

隨著社會(huì)往信息化和智能化的方向發(fā)展，移動(dòng)機(jī)器人在國(guó)內(nèi)外很多領(lǐng)域都有著巨大的應(yīng)用需求，如太空和海底探索、礦井環(huán)境探測(cè)、深海光纜修復(fù)、化工等高危車間自動(dòng)搬運(yùn)等。移動(dòng)機(jī)器人的發(fā)展如日中天，而它的同時(shí)定位與建圖(SLAM) 問題是一個(gè)急需解決的問題。

移動(dòng)機(jī)器人同時(shí)定位與建圖[1](SLAM)問題可以描述為:移動(dòng)機(jī)器人從一個(gè)未知的位置出發(fā),在不斷運(yùn)動(dòng)過程中根據(jù)自身位姿估計(jì)和傳感器對(duì)環(huán)境的感知構(gòu)建增量式地圖,同時(shí)利用該地圖更新自己的定位。

本文只研究機(jī)器人的自主導(dǎo)航問題就是兩個(gè)子問題：“Where am I?”、“What does the world looks like?”:第一個(gè)問題是定位，第二個(gè)問題是地圖創(chuàng)建。

1　定位與地圖創(chuàng)建(SLAM)

移動(dòng)機(jī)器人定位[2]是指機(jī)器人判斷自身在所處環(huán)境中位姿(位置和方位角)信息的過程,回答了“Where am I?”的問題,主要有全局定位(Global Localization)和位置跟蹤(Position Tracking)兩種類型。全局定位又稱絕對(duì)定位,是指移動(dòng)機(jī)器人在沒有任何先驗(yàn)信息的情況下,利用自身攜帶的外部傳感器(如激光測(cè)距儀、聲納、攝像頭和激光雷達(dá)等)所獲取的環(huán)境信息確定自身在已知環(huán)境地圖中的位置的過程。位置跟蹤又稱相對(duì)定位,是指移動(dòng)機(jī)器人在給定初始位姿(位置和方位角)信息的情況下,利用自身內(nèi)部傳感器(如里程計(jì)、加速度計(jì)、陀螺儀和電子羅盤等)提供的速度、加速度和方位等信息對(duì)機(jī)器人自身的位姿進(jìn)行跟蹤估計(jì)的過程。

地圖構(gòu)建[3]是指移動(dòng)機(jī)器人利用自身傳感器獲取的環(huán)境地圖信息對(duì)身處的環(huán)境進(jìn)行建模,進(jìn)而自動(dòng)地構(gòu)建環(huán)境地圖,回答了“What does the world looks like?”的問題。它是移動(dòng)機(jī)器人自主導(dǎo)航中的定位問題的前提。常用的地圖模型有特征地圖、柵格地圖、拓?fù)涞貓D和外觀地圖等。

2　改進(jìn)的SLAM算法研究

2.1移動(dòng)機(jī)器人的系統(tǒng)模型

2.1.1坐標(biāo)系統(tǒng)模型：

建立如下機(jī)器人坐標(biāo)系統(tǒng)。

圖1　移動(dòng)機(jī)器人坐標(biāo)系統(tǒng)

主要有以下幾個(gè)坐標(biāo)系：XWOWYW,XRORYR,XSOSYS分別表示全局、機(jī)器人和傳感器坐標(biāo)系。圖1表示其相互之間的關(guān)系。

2.1.2機(jī)器人位置模型

機(jī)器人位置信息：

(1)

機(jī)器人方向：機(jī)器人運(yùn)動(dòng)方向偏離全局坐標(biāo)系YR軸的夾角θ，夾角范圍為-180～180°。

2.1.3機(jī)器人運(yùn)動(dòng)模型

本文研究中的機(jī)器人運(yùn)動(dòng)模型可以用一個(gè)離散時(shí)間差分[4]方程來表示：

(2)

其中：f(x(k-1),u(k))為系統(tǒng)的非線性狀態(tài)轉(zhuǎn)移函數(shù)，x(k)為k時(shí)刻機(jī)器人的位姿信息，u(k)為輸入的控制信息，w(k)為系統(tǒng)建模中引入的誤差，即過程噪聲。

2.1.4環(huán)境地圖模型

本文采用特征地圖來進(jìn)行SLAM的定位實(shí)驗(yàn)研究，在這種形式下環(huán)境地圖由多個(gè)點(diǎn)特征構(gòu)成，每個(gè)點(diǎn)用其在全局坐標(biāo)系中的坐標(biāo)xi=(xi,yi)表示。

2.1.5傳感器觀測(cè)模型

(3)

其中：xi=(xi,yi)為環(huán)境特征i的坐標(biāo)，zi(k)為k時(shí)刻傳感器提取的特征i的觀測(cè)值，v(k)為觀測(cè)噪聲測(cè)量函數(shù)，h(x(k),xi)為機(jī)器人位姿信息x(k)和觀測(cè)特征坐標(biāo)xi的函數(shù)[5]：

(4)

2.1.6環(huán)境特征動(dòng)態(tài)模型

由于本文中對(duì)SLAM問題的研究都是基于靜態(tài)環(huán)境的用特征在全局坐標(biāo)系中的位置Li=(xi,yi)來表示，其中1，．．．,n甩為環(huán)境特征的標(biāo)號(hào)。

動(dòng)態(tài)模型為：

(5)

2.2EKF(擴(kuò)展卡爾曼)算法

建立非線性離散時(shí)間系統(tǒng)模型如下：

(6)

其中：ωk和νk為O均值高斯白噪聲序列，其統(tǒng)計(jì)特性服從0均值高斯分布。

首先，對(duì)系統(tǒng)狀態(tài)方程(6)進(jìn)行泰勒級(jí)數(shù)展開，略去二次項(xiàng)后得到預(yù)測(cè)協(xié)方差矩陣最Pk|k-1和當(dāng)前時(shí)刻系統(tǒng)狀態(tài)的預(yù)測(cè)值Xk|k-1。

(7)

lnnk=zk-zk|k

(8)

最后，根據(jù)預(yù)測(cè)的狀態(tài)估計(jì)Xk|k-1和新息lnnk，對(duì)協(xié)方差陣Pk|k和 k時(shí)刻系統(tǒng)狀態(tài)估計(jì)值Xk|k進(jìn)行更新：

(9)

這樣，通過不斷迭代就能預(yù)測(cè)系統(tǒng)在不同時(shí)刻的狀態(tài)值。因?yàn)镋KF方法能夠有效地解決跟蹤問題和非線性系統(tǒng)的狀態(tài)估計(jì)，所以在移動(dòng)機(jī)器人的SLAM的研究中得到了廣泛的應(yīng)用。但在全狀態(tài)SLAM[7]算法中，機(jī)器人位姿和所有地圖特征的位置組成狀態(tài)向量，此刻SLAM算法的計(jì)算復(fù)雜度與狀態(tài)向量的維數(shù)成指數(shù)關(guān)系，由于地圖特征的數(shù)目急劇增大，算法的計(jì)算量很大。因而EKF算法的缺陷就顯現(xiàn)出來了，它不適合大范圍環(huán)境及密集環(huán)境等特征數(shù)量較大的場(chǎng)合。

2.3基于EKF改進(jìn)的SLAM算法

FastSLAM[8]算法是濾波算法的一種比較經(jīng)典的算法，它采用Rao-Blackwellise[9-10]的分解思想，首先將SLAM問題分解為路徑估計(jì)和地圖估計(jì)兩個(gè)問題，其次基于路徑估計(jì)與地圖估計(jì)是相互獨(dú)立的，將地圖特征的數(shù)目分解為M個(gè)獨(dú)立的特征估計(jì)，結(jié)果化繁為簡(jiǎn)將一個(gè)2M+3維的特征估計(jì)問題簡(jiǎn)化化為一個(gè)3維和M個(gè)2維估計(jì)問題，如式(10)：

(10)

本文改進(jìn)的算法是在FastSLAM的濾波算法下改進(jìn)EKF-SLAM算法的，F(xiàn)astSLAM濾波算法每個(gè)粒子有一份地圖，因此計(jì)算十分復(fù)雜，本文只選取其分解思想而EKF-SLAM方法也有缺陷所以作者選取兩者優(yōu)點(diǎn)進(jìn)行對(duì)算法的改進(jìn)。在公式(10)所示的分解估計(jì)構(gòu)架下，改進(jìn)的EKF-SLAM算法能將問題的計(jì)算復(fù)雜度大大降低。具體方法操作：首先在公式(10)所示的分解估計(jì)構(gòu)架下，選用性能優(yōu)異的卡爾曼濾波器來遞推估計(jì)移動(dòng)機(jī)器人的位姿，然后用較高感知精度的傳感器觀測(cè)環(huán)境特征。地圖中的每個(gè)特征都能滿足獨(dú)立的二維高斯分布，因此最后用EKF更新地圖中觀測(cè)到的特征。

2.4改進(jìn)算法的具體實(shí)現(xiàn)步驟

一般情況下移動(dòng)機(jī)器人在未知環(huán)境中不斷地循環(huán)執(zhí)行4個(gè)過程：預(yù)測(cè)步驟，觀測(cè)步驟，更新步驟和向量增廣步驟，從而完成移動(dòng)機(jī)器人的 SLAM。

2.4.1路徑估計(jì)

路徑估計(jì)由機(jī)器人各時(shí)刻的位姿估計(jì)組成，如果假設(shè)機(jī)器人的位姿向量服從高斯分布，那么就用性能優(yōu)異的卡爾曼濾波器來遞推估計(jì)移動(dòng)機(jī)器人位姿向量的均值與方差，要估計(jì)每一時(shí)刻的機(jī)器人位姿需要進(jìn)行兩步驟：預(yù)測(cè)和觀測(cè)。

預(yù)測(cè)：機(jī)器人位姿用性能優(yōu)異的卡爾曼濾波器估計(jì)，位姿向量Xk+1的預(yù)測(cè)均值和方差用UT變換來計(jì)算。

觀測(cè)：用UT變換近似計(jì)算卡爾曼系數(shù)，用公式(11)和公式(12)計(jì)算位姿向量Xk+1的濾波值和濾波方差。

濾波值：

(11)

濾波方差：

(12)

2.4.2地圖估計(jì)

在公式(10)的分解估計(jì)構(gòu)架下，地圖估計(jì)可以分解為M個(gè)特征估計(jì)，用EKF[11]估計(jì)特征的概率分布P(mi|x1:k+1,z1:k+1,u1:k+1)。另外地圖估計(jì)可表示為：

其中：地圖中第i個(gè)特征的高斯均值和方差是ui和pi。在本文中，我們是采用環(huán)境中不動(dòng)的靜態(tài)物體作為地圖特征的，所以可以直接將這一時(shí)刻的估計(jì)均值和方差看作下一時(shí)刻的估計(jì)均值和方差。在SLAM問題中，路徑估計(jì)和地圖估計(jì)是數(shù)據(jù)關(guān)聯(lián)的，用于觀測(cè)環(huán)境特征的外部傳感器一般都有較高的感知精度，在公式(10)所示的分解估計(jì)架構(gòu)下，地圖估計(jì)的精度主要取決于路徑估計(jì)的精度。

更新步驟中，對(duì)于觀測(cè)到的特征我們用EKF進(jìn)行更新，對(duì)于沒有觀測(cè)到的特征保持上一時(shí)刻的值。地圖特征的均值和方差用公式(13)和(14)更新。

濾波值：

(13)

濾波方差：

(14)

在向量增廣步驟中，機(jī)器人自身攜帶的傳感器在每次觀測(cè)到的環(huán)境特征點(diǎn)中包括以前觀測(cè)到的特征點(diǎn)也包括新觀測(cè)到的新的特征點(diǎn)。以前觀測(cè)到的特征點(diǎn)在地圖中已經(jīng)存在，因此可以用來更新狀態(tài)的預(yù)測(cè)值，而新觀測(cè)到的特征點(diǎn)則要加入到系統(tǒng)的狀態(tài)向量中去。

實(shí)現(xiàn)改進(jìn)EKF-SLAM算法的具體步驟如圖2所示。

圖2　改進(jìn)算法的具體實(shí)現(xiàn)步驟

3　仿真研究

在 Matlab環(huán)境下進(jìn)行仿真設(shè)計(jì)，創(chuàng)建基于點(diǎn)特征的實(shí)驗(yàn)環(huán)境：

實(shí)驗(yàn)環(huán)境尺寸為：100 m×90 m

航標(biāo)：14個(gè)

特征：27個(gè)

移動(dòng)機(jī)器人移動(dòng)速度v：3 m/s

移動(dòng)機(jī)器人驅(qū)動(dòng)輪之間的距離L：1 m

移動(dòng)機(jī)器人轉(zhuǎn)動(dòng)角速度γ：0.2 πrad/s

實(shí)驗(yàn)系統(tǒng)采樣時(shí)間ΔT：0.03 s

圖3為實(shí)驗(yàn)環(huán)境，圖4為實(shí)驗(yàn)結(jié)果，圖3圖4的橫坐標(biāo)X，Y軸范圍在-50～50 m之間，由圖可見改進(jìn)算法的軌跡比EKF-SLAM算法的軌跡更貼近航線，由此得知改進(jìn)的算法的同時(shí)定位具有很好的跟蹤定位性能，圖5為實(shí)驗(yàn)所創(chuàng)建的地圖和環(huán)境地圖的比較，仿真結(jié)果表明在所能測(cè)量的范圍內(nèi)，移動(dòng)機(jī)器人在改進(jìn)的算法中對(duì)環(huán)境特征具有很好的定位能力，從而使得所創(chuàng)建的地圖具有很高的精度。

圖3　實(shí)驗(yàn)環(huán)境

圖4　實(shí)驗(yàn)結(jié)果

圖5　實(shí)驗(yàn)所創(chuàng)建的地圖和環(huán)境地圖的比較

4　結(jié)論

本文以移動(dòng)機(jī)器人為實(shí)驗(yàn)平臺(tái)，在研究其同時(shí)定位與地圖創(chuàng)建(SLAM)問題上提出了一種改進(jìn)的擴(kuò)展卡爾曼(EKF)算法。它的優(yōu)點(diǎn)是其“分解"思想，在公式(3)～(10)所示的分解估計(jì)構(gòu)架下，將SLAM問題分解為路徑估計(jì)和地圖估計(jì)兩個(gè)問題。選用性能優(yōu)異的卡爾曼濾波器來遞推估計(jì)機(jī)器人的位姿，用EKF更新地圖中觀測(cè)到的特征。SLAM問題的計(jì)算復(fù)雜度大大降低，實(shí)驗(yàn)表明了改進(jìn)的EKF—SLAM具有更好的性能。

[1] 李仁厚，自主移動(dòng)機(jī)器人導(dǎo)論[M]．西安：西安交通大學(xué)出版社，2006.

[2] 陳衛(wèi)東，張飛．移動(dòng)機(jī)器人的同步白定位與地圖創(chuàng)建研究進(jìn)展[J].控制理論與應(yīng)用，2005，22(3)：455-460.

[3] 西格沃特R,諾巴克什IR.自主移動(dòng)機(jī)器人導(dǎo)論[M].李人厚. 西安: 西安交通大學(xué)出版社,2006,241-245.

[4]MontemerloM，ThnmS，KolIerD，etal．FastSLAM2．0：animprovedparticlefilteringalgorithmforsimultaneouslocalizationandmappingthatprovablyconverges[A]．ProceedingsoftheInternationalConferenceonArtificialIntelligence[C].California,CA，USA：UCAI，2003，1151-1156.

[5]MontemerloM，ThnmS，KollerD，etal．FastSLAM：afactoredsolutiontothesimultaneouslocalizationandmappingproblem[A].ProceedingsoftheAAAINationalConferenceonArtificialIntelligence[C].Edmonton，Canada,2002:593-598.

[6] 郭劍輝，趙春霞，陸劍峰，等．Rao-Blackwellised粒子濾波SLAM的一致性研究[J]．系統(tǒng)仿真學(xué)報(bào)，2008，20(23)：6401-6405.

[7] 李季，孫秀霞．基于改進(jìn)A-Star算法的無人機(jī)航跡規(guī)劃算法研究[J]．兵工學(xué)報(bào)，2008(7):787-792.

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

[9]MurphyK．Bayesianmaplearningindynamicenvironments[J]．AdvancesinNeuralInformationProcessingSystems(NIPS)，1999：1015-1021.

[10]OucetA，deFreitasN，MurphyKetal．Rao-blackwelliscdparticlefilteringfordynamicBayesiannetworks[A].ProceedingsoftheSixteenthConferenceonUncertaintyinArtificialIntelligence[C].2000：176-183.

[11]MullaneJ,VoBN,AdamsM,etal.ArandomfinitesetapproachtoBayesianslam[J].IEEETrans.Robotics,2011,27:268-282.

Research on Problem in SLAM for Mobile Robots

Duan Suolin1，Tan Gang1,Zhou Yuqin2,Zhu Haiyong2

(1.Robotics Institute， Changzhou University，Changzhou213164,China；2.Jiangsu Runyi Instrument Co.Ltd.,Jinhu211600,China)

Aiming at the problem of simultaneous localization and mapping(SLAM) for mobile robots,an improved EKF-SLAM algorithm was proposed while considering the existing defects of extended Kalman (EKF) algorithm-not suitable for a large number of characters,occasions such as a wide range of environments ,an intensive environment and so on.It put the decomposition thought of Rao-Blackwellise on the extended Kalman (EKF) algorithm-Decomposition Estimate framework ,which broke down the SLAM problem into the two issues of the path estimation and map estimation so that predicting,observating,updating and increasing vectors .The simulation results show that improved EKF-SLAM algorithm is more superior than EKF-SLAM algorithm in a number of features,occasions. The improved algorithm greatly reduces the computational complexity and improves the accuracy so it provides an effective method for the problem of simultaneous localization and mapping (SLAM) for mobile robots in the more complex environment.

mobile robots;simultaneous localization and mapping (SLAM); extended Kalman (EKF) algorithm; path estimation; map estimation

1671-4598(2016)04-0234-03DOI：10.16526/j.cnki.11-4762/tp.2016.04.067

TP24

A

2015-11-04；

2015-11-30。

江蘇省科技支撐計(jì)劃項(xiàng)目(社會(huì)發(fā)展)(BEK2013671)。

段鎖林(1956-),男，陜西岐山人，博士，教授，主要從事機(jī)器視覺與智能移動(dòng)機(jī)器人控制方向的研究。

談剛(1991-)，男，江蘇鹽城人，碩士，碩士研究生，主要從事智能機(jī)器人技術(shù)方向的研究。