基于熵規(guī)則的信息濾波SLAM算法*

郭 潤， 石守東

(寧波大學 信息科學與工程學院，浙江 寧波 315211)

?

基于熵規(guī)則的信息濾波SLAM算法*

郭 潤， 石守東

(寧波大學 信息科學與工程學院，浙江 寧波 315211)

信息濾波同時定位與構圖(IFSLAM)算法如何獲得稀疏化的信息矩陣是解決濾波性能的關鍵因素。通過對稀疏擴展信息濾波深入分析，發(fā)現(xiàn)其稀疏化結果難以保證關聯(lián)性最弱的環(huán)境主動特征被稀疏掉；為了改進這一缺陷，提出一種基于熵稀疏規(guī)則的改進SLAM算法，該算法利用熵性質、綜合當前以及下一觀測時刻來選擇與位姿關聯(lián)性最弱的環(huán)境特征作為稀疏特征點；有效提高了算法的稀疏性能。在Matlab上對改進算法進行仿真，驗證改進算法的有效性。

信息濾波(IF)； 同時定位與構圖(SLAM)； 稀疏規(guī)則； 熵性質

0 引 言

移動機器人在未知環(huán)境中如何實現(xiàn)真正的自主導航、路徑規(guī)劃[1,2]等問題，在機器人研究領域獲得了極為廣泛的關注。同時定位與地圖構建(simultaneous localization and map building，SLAM)是實現(xiàn)機器人真正自主導航和路徑規(guī)劃等問題的關鍵技術之一[3]。SLAM 是指機器人在未知環(huán)境中，依靠自身攜帶的傳感器遞增式地創(chuàng)建環(huán)境地圖，同時利用已創(chuàng)建的環(huán)境地圖來估計機器人在地圖中的位置。

最初SLAM算法是由Smith R等人[4]提出來的。他們利用擴展卡爾曼濾波(extended Kalman filtering,EKF)來解決SLAM問題。然而，標準EKF SLAM算法的時間復雜度與空間復雜度高[5]，使其難以應用到大環(huán)境中。隨著SLAM技術的不斷發(fā)展，產(chǎn)生了許多優(yōu)秀的SLAM算法，主要有：Guivant J E等人[6]的EKF改進算法—壓縮擴展卡爾曼濾波，Nasuriwong S等人[7]的Rao-Blackwellized Particle Filter SLAM算法，朱奇光等人[8]的球面單徑容積FastSLAM算法，以及Cheein F A等人[9]的最優(yōu)擴展信息濾波算法(EIF SLAM)。其中擴展信息濾波算法與擴展卡爾曼濾波算法是互為對偶形式，在數(shù)學推導上，二者均具有嚴密性以及準確性等特點，但在處理SLAM問題當中，信息濾波的表示形式具有一定的計算優(yōu)勢，因此引起了眾多學者的關注。

目前最為著名的信息濾波SLAM算法是稀疏擴展信息濾波(sparse extended information filtering，SEIF)SLAM算法[10]，該算法在稀疏化過程信息丟失隨機性較大，故算法難以保證其全局一致性。Walter M R[11]在標準SEIF SLAM的基礎之上，提出了精確稀疏擴展信息濾波(ESEIF SLAM)算法，雖然算法濾波性能有所改善，但增加了機器人位姿重新定位，且此時在觀測步驟中必須要有之前觀測到的環(huán)境特征點，故其適用上有一定缺陷。

本文將從信息濾波的核心—稀疏規(guī)則角度出發(fā)，在深入分析標準SEIF SLAM算法稀疏規(guī)則的基礎上，提出一種基于熵性質的稀疏規(guī)則改進SLAM算法，該算法使得稀疏化過程中關聯(lián)性更強的環(huán)境特征點得以保留下來，從而有效提高了SLAM算法的精度和一致性。

1 IFSLAM算法

1.1 SLAM問題描述

SLAM問題可以用一個后驗條件概率來表示，該后驗條件概率服從一個多維的高斯分布，可表示為

p(ξt|zt,ut)

(1)

1.2 EIF SLAM基本技術

1.2.1 運動更新

運動更新是通過機器人動力學模型預測機器人下一時刻狀態(tài)的后驗分布，該過程由兩步實現(xiàn)：將新位置的向量加入狀態(tài)后驗分布和邊緣化舊位置的狀態(tài)向量。

1)放大信息矩陣

加入新的位置變量后的信息矩陣和信息向量分別為

(2)

(3)

2)邊緣化

將機器人放大狀態(tài)的信息矩陣和信息向量進行邊緣化，則有

(4)

(5)

其中

Ω=(Λxtxt+FTQ-1F)-1{ηt-FTQ-1[f(μxt,ut+1)-

Fμxt]}

(6)

1.2.2 測量更新

在測量更新中，根據(jù)傳感器觀測來對信息矩陣和信息向量進行更新，可表示為

(7)

(8)

2 稀疏化過程

在SLAM過程中，隨著機器人的運動以及觀測更新，使得信息矩陣變得越來越密集，即信息矩陣中機器人與特征點、特征點與特征點之間的關聯(lián)數(shù)增多；可以發(fā)現(xiàn)隨著時間的推移機器人與特征點的關聯(lián)性越來越弱，但不會消失。為了降低算法的復雜度，需把關聯(lián)性弱的特征點從信息矩陣中去掉。設信息矩陣中機器人與特征點、特征點與特征點之間的關聯(lián)數(shù)有上界，分別為θx和θy。當機器人移動或者觀測到新特征點的時候，就可能會超出閾值θx或者θy，這時就需要執(zhí)行稀疏化操作。

2.1 SEIF的稀疏規(guī)則

在SEIF SLAM中，其稀疏化步驟是發(fā)生在運動預測過程中.下面將環(huán)境特征點分成互不相關的三類，即M=M+⊕M0⊕M-。其中，M+代表當前活動的特征點集，在下一時刻仍然活動；M-代表當前不活動的特征點集，在下一時刻仍然不活動；M0代表當前活動的特征點集，在下一時刻變得不活動。其狀態(tài)向量的后驗分布為

p(xt,M)=p(xt,M+,M0,M-)

=p(xt|M+,M0,M-)p(M+,M0,M-)

=p(xt|M+,M0,M-=0)p(M+,M0,M-)

(9)

在式(9)最后一步第一項中，如果主動特征M+和M0已知，那么位姿xt與被動特征M-無關，因此,可以設置M-為任意值，如上面將其設為0。

為了使信息矩陣變得稀疏，SEIF SLAM通過去除M0來控制信息矩陣的大小,見圖1所示。

圖1 SEIF的稀疏過程Fig 1 Sparse process of SEIF

雖然稀疏化操作使得整個算法的計算效率得到提高,但由于稀疏化步驟自身不可避免的近似性，故如何選出當前時刻在主動特征當中與機器人位姿關聯(lián)最弱的特征變?yōu)楸粍犹卣骶统闪岁P鍵因素。

2.2 改進的稀疏規(guī)則

2.2.1 信息熵特性

信息熵[12]的定義式為

(10)

式中X為離散隨機變量，P(X)為其概率分布。信息熵H(X)衡量的是隨機變量X的不確定性，其值隨變量X不確定性的增大也隨之增加。

利用條件概率的概念引出條件熵，其定義為

(11)

式中P(X|Y=y)為隨機變量X在條件Y=y下的條件概率。條件熵H(X|Y=y)衡量的是在Y=y的條件下，隨機變量X的不確定性。

在SLAM問題中，移動機器人位姿xt為三維隨機向量，其概率分布為

μt)}

(12)

在t時刻觀測到環(huán)境特征點mi后位姿xt的條件概率分布為

(13)

根據(jù)以上熵的性質，可以計算出機器人位姿xt的熵以及在觀測到活動特征點條件下的條件熵，由于觀測到特征點的條件熵可能使機器人位姿的熵增加，也可能減少。那么就可以從觀測到的活動特征點當中選出使位姿條件熵值最大的特征點，即關聯(lián)性最弱的特征點，進行稀疏化操作。

2.2.2 改進稀疏規(guī)則

通過對標準SEIF SLAM的稀疏過程分析得知，其在執(zhí)行稀疏化的時候，只考慮到了當前時刻與位姿xt相關聯(lián)的主動特征，很難保證每一次執(zhí)行稀疏化操作后，保留下來的環(huán)境特征與機器人位姿關聯(lián)性最強。如果稀疏化操作發(fā)生在位姿運動到xt+1時刻且同時執(zhí)行一次觀測操作之后，那么可以利用在t時刻位姿所觀測到的環(huán)境特征的條件熵和t+1時刻位姿所觀測到的環(huán)境特征的條件熵來綜合決定相關性最弱的環(huán)境特征，即

(14)

式中mi為t時刻位姿觀測到的環(huán)境主動特征,如果在t+1未觀測到特征mi,即在t+1時刻H(xt+1|m=mi)值取最大值。

下面分四種情形來討論(設在t時刻會超過稀疏閾值；不失一般性，設經(jīng)過改進稀疏規(guī)則計算得，特征m1使得位姿xt熵值最大。)

情形1：在t+1時刻除了觀測到t時刻所對應的主動特征外，還觀測到了新的環(huán)境特征，見圖2(b)；此時新觀測到的環(huán)境特征m5有利于t+1位姿的精確定位，可改善狀態(tài)向量的后驗分布。因此,有必要作為選擇t時刻與位姿關聯(lián)最弱的主動特征的一個參考量。此時可類比式(14)將判斷條件作為

mj)}

(15)

式中mj為在t時刻未觀測到而在t+1時刻觀測到的環(huán)境特征。

通過式(15)來判斷關聯(lián)性最弱的特征點，并對其進行稀疏化操作，見圖2所示。

圖2 在t+1時刻觀測到新特征以及t時刻所有特征條件下的稀疏化過程Fig 2 Sparse process under condition of new feature andall features of time t are observed at time t+1

情形2：在位姿從xt移動到xt+1之后，在t+1時刻沒有觀測到新特征，即只觀測到了m1,m2,m3,m4,見圖3(b)，利用式(14)判斷出關聯(lián)性最弱的特征點，并將其稀疏化，見圖3所示。

圖3 在t+1時刻觀測到t時刻所有特征條件下的稀疏化過程Fig 3 Sparse process under the condition of all features of time t are observed at time t+1

情形3：在位姿從xt移動到xt+1之后，在t+1時刻只觀測到 時刻的部分特征且沒有觀測到新特征，見圖4(b)，假設只觀測到m3，m4,而m1,m2沒有被觀測到，在這里只需對m1,m2進行判斷，利用式(14)判斷關聯(lián)性最弱的環(huán)境特征，對其執(zhí)行稀疏化操作，見圖4所示。

圖4 在t+1時刻觀測到t時刻部分特征條件下的稀疏化過程Fig 4 Sparse process under the condition of some features of time t are observed at time t+1

情形4：在位姿從xt移動到xt+1之后，在t+1時刻觀測到t時刻的部分特征m3,m4的同時觀測到新特征m5，見圖5(b)，在這里只需對m1,m2進行判斷，利用式(15)判斷關聯(lián)性最弱的環(huán)境特征，對其執(zhí)行稀疏化操作，見圖5所示。

圖5 在t+1時刻觀測到新特征以及t時刻部分特征條件下的稀疏化過程Fig 5 Sparse process under the condition of new feature andsome features of time t are observed at time t+1

綜上分析可知，改進稀疏化方法，可以保留與位姿關聯(lián)性最強的環(huán)境特征，有利于SLAM算法精度以及一致性的提高。

3 實驗仿真與分析結果

假設在仿真環(huán)境中移動機器人運動學數(shù)學表達式為

(16)

式中L為在t時間內(nèi)運動的距離;W為水平方向到頂點的垂直高度;gt為t時刻移動機器人的方向角，其表達式為

(17)

機器人觀測數(shù)學表達式為

(18)

式(18)中觀測到的環(huán)境特征點笛卡爾坐標為(x(i),y(i))。

圖6 SLAM仿真環(huán)境Fig 6 SLAM simulation environment

本文將在相同機器人初始狀態(tài)和環(huán)境條件下，對改進算法與標準SEIFSLAM算法進行比較分析。圖7所示為改進算法和標準SEIFSLAM算法估計機器人位姿狀態(tài)的誤差比較，改進算法估計偏差相比標準SEIFSLAM算法有了明顯的減小。

圖7 改進算法與標準SEIF SLAM誤差比較Fig 7 Error comparison of improved algorithmand standard SEIF SLAM

為了統(tǒng)計SLAM處理過程的實際效果，利用Monte-Carlo仿真來分析改進算法與SEIF算法的處理性能。經(jīng)過30次Monte-Carlo仿真后，從圖8可以看出，改進算法的估計結果相比稀疏擴展信息濾波更好。

圖8 改進算法與標準SEIF SLAM均值誤差比較Fig 8 Error comparison of improved algorithmand standard SEIF SLAM

(19)

(20)

(21)

在本文實驗中只研究各狀態(tài)的估計誤差，故利用式(21)對各狀態(tài)的估計誤差以及協(xié)方差矩陣估計值進行比較，通過仿真實驗數(shù)據(jù)繪出比較結果曲線，便可直觀判斷其一致性性能,如圖9。

圖9 改進算法與標準SEIF SLAM一致性比較Fig 9 Consistency comparison of improvedalgorithm and standard SEIF SLAM

通過30次Monte-Carlo仿真后，由圖9可以看出，改進稀疏規(guī)則后的SLAM算法的一致性相比標準SEIF SLAM算法有了明顯的提高。

4 結束語

本文通過對標準SEIF SLAM的稀疏規(guī)則深入分析之后，提出了基于熵規(guī)則的改進算法。該算法綜合了熵性質以及不同觀測時刻特征點對狀態(tài)向量估計精度的影響，來得出稀疏化規(guī)則，通過與標準SEIF SLAM算法進行實驗對比分析，實驗結果表明改進算法在狀態(tài)估計精度以及算法一致性方面均更優(yōu)，從而證明了改進算法的有效性。

[1] 白金柯，陳立家，金 何,等.動態(tài)未知環(huán)境下一種新的機器人路徑規(guī)劃方法[J].傳感器與微系統(tǒng)，2011，30(10):33-36.

[2] 辛江慧，李舜酩，廖慶斌.基于傳感器信息的智能移動機器人導航評述[J].傳感器與微系統(tǒng)，2008，27(4):4-7.

[3] 丁 偉，孫 華，曾建輝.基于多傳感器信息融合的移動機器人導航綜述[J].傳感器與微系統(tǒng)，2006,25(7):1-3.

[4]SmithR,SelfM,CheesemanP.Autonomousrobotvehicles[M].BerlinHeidelberg:Springer-Verlag,1990:167-193.

[5] 趙亞鳳，任洪娥.基于無線傳感器網(wǎng)絡的同時定位與跟蹤[J].傳感器與微系統(tǒng)，2014,33(5):55-58.

[6]GuivantJE,NebotEM.Solvingcomputationalandmemoryrequirementsoffeature-basedsimultaneouslocalizationandmappingalgorithms[J].IEEETransactionsonRoboticsandAutomation,2003,19(4):749-755.

[7]NasuriwongS,YuwapoositanonP.Gaussiankernelposterioreliminationforfastlook-aheadrao-blackwellisedparticlefilteringforSLAM[J].AppliedMechanics&Materials,2015,781:555-558.

[8] 朱奇光，袁 梅，王梓巍,等.機器人球面單徑容積FastSLAM算法[J].機器人，2015,37 (6):708-717.

[9]CheeinFA,SteinerG,PainaGP,etal.OptimizedEIF-SLAMalgorithmforprecisionagriculturemappingbasedonstemsdetec-tion[J].Computers&ElectronicsinAgriculture,2011,78(2):195-207.

[10]ThrunS,LiuY,KollerD,etal.Simultaneouslocalizationandmappingwithsparseextendedinformationfilters[J].Interna-tionalJournalofRoboticsResearch,2004,23(7-8):693-716.

[11]WalterMR,EusticeRM,LeonardJ.Exactlysparseextendedinformationfiltersforfeature-basedSLAM[J].InternatioalJounalofRoboticsResearch,2007,26(4):335-359.

[12] 曹 樂，王朝英，孔云波,等.基于信息熵的多無源傳感器數(shù)據(jù)關聯(lián)[J].傳感器與微系統(tǒng)，2015，34(11):33-37.

Information filtering SLAM algorithm based on entropy rule*

GUO Run, SHI Shou-dong

(Faculty of Information Science and Engineering,Ningbo University,Ningbo 315211,China )

How to obtain sparse information matrix is the key factor for information filtering(IF)simultaneous localization and mapping(IFSLAM)algorithm to solve filtering performance.By deep analysis on sparse extended information filtering(SEIF),finding that it’s difficult for sparse result to guarantee the weakest relevance environment active characteristics of being thinning after sparse process; in order to improve this defect,an improved SLAM algorithm based on entropy sparse rules is proposed,it uses the entropy property and considering the current and next observing moment to select posture correlating the weakest environment characteristic as the sparse feature points;effectively improve the sparse performance of the algorithm.The improved algorithm is simulated on Matlab,and validity is verified.

information filtering(IF); simultaneous localization and mapping(SLAM); sparsity rules; entropy property

10.13873/J.1000—9787(2016)12—0132—05

2016—10—19

浙江省重中之重學科開放基金資助項目(XKXL1514); 浙江省教育廳科研項目(Y201121251)

TP 24

A

1000—9787(2016)12—0132—05

郭 潤(1989-)，男，四川內(nèi)江人，碩士研究生，研究方向為移動機器人導航與定位。