郭 潤(rùn)， 石守東

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

?

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

郭 潤(rùn)， 石守東

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

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

信息濾波(IF)； 同時(shí)定位與構(gòu)圖(SLAM)； 稀疏規(guī)則； 熵性質(zhì)

0 引 言

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

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

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

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

1 IFSLAM算法

1.1 SLAM問(wèn)題描述

SLAM問(wèn)題可以用一個(gè)后驗(yàn)條件概率來(lái)表示，該后驗(yàn)條件概率服從一個(gè)多維的高斯分布，可表示為

p(ξt|zt,ut)

(1)

1.2 EIF SLAM基本技術(shù)

1.2.1 運(yùn)動(dòng)更新

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

1)放大信息矩陣

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

(2)

(3)

2)邊緣化

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

(4)

(5)

其中

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

Fμxt]}

(6)

1.2.2 測(cè)量更新

在測(cè)量更新中，根據(jù)傳感器觀測(cè)來(lái)對(duì)信息矩陣和信息向量進(jìn)行更新，可表示為

(7)

(8)

2 稀疏化過(guò)程

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

2.1 SEIF的稀疏規(guī)則

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

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)最后一步第一項(xiàng)中，如果主動(dòng)特征M+和M0已知，那么位姿xt與被動(dòng)特征M-無(wú)關(guān)，因此,可以設(shè)置M-為任意值，如上面將其設(shè)為0。

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

圖1 SEIF的稀疏過(guò)程Fig 1 Sparse process of SEIF

雖然稀疏化操作使得整個(gè)算法的計(jì)算效率得到提高,但由于稀疏化步驟自身不可避免的近似性，故如何選出當(dāng)前時(shí)刻在主動(dòng)特征當(dāng)中與機(jī)器人位姿關(guān)聯(lián)最弱的特征變?yōu)楸粍?dòng)特征就成了關(guān)鍵因素。

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

2.2.1 信息熵特性

信息熵[12]的定義式為

(10)

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

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

(11)

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

在SLAM問(wèn)題中，移動(dòng)機(jī)器人位姿xt為三維隨機(jī)向量，其概率分布為

μt)}

(12)

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

(13)

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

2.2.2 改進(jìn)稀疏規(guī)則

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

(14)

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

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

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

mj)}

(15)

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

通過(guò)式(15)來(lái)判斷關(guān)聯(lián)性最弱的特征點(diǎn)，并對(duì)其進(jìn)行稀疏化操作，見圖2所示。

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)仿真與分析結(jié)果

假設(shè)在仿真環(huán)境中移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)數(shù)學(xué)表達(dá)式為

(16)

式中L為在t時(shí)間內(nèi)運(yùn)動(dòng)的距離;W為水平方向到頂點(diǎn)的垂直高度;gt為t時(shí)刻移動(dòng)機(jī)器人的方向角，其表達(dá)式為

(17)

機(jī)器人觀測(cè)數(shù)學(xué)表達(dá)式為

(18)

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

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

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

圖7 改進(jìn)算法與標(biāo)準(zhǔn)SEIF SLAM誤差比較Fig 7 Error comparison of improved algorithmand standard SEIF SLAM

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

圖8 改進(jìn)算法與標(biāo)準(zhǔn)SEIF SLAM均值誤差比較Fig 8 Error comparison of improved algorithmand standard SEIF SLAM

(19)

(20)

(21)

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

圖9 改進(jìn)算法與標(biāo)準(zhǔn)SEIF SLAM一致性比較Fig 9 Consistency comparison of improvedalgorithm and standard SEIF SLAM

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

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

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

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

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

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

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

[5] 趙亞鳳，任洪娥.基于無(wú)線傳感器網(wǎng)絡(luò)的同時(shí)定位與跟蹤[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] 朱奇光，袁 梅，王梓巍,等.機(jī)器人球面單徑容積FastSLAM算法[J].機(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] 曹 樂，王朝英，孔云波,等.基于信息熵的多無(wú)源傳感器數(shù)據(jù)關(guān)聯(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

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

TP 24

A

1000—9787(2016)12—0132—05

郭 潤(rùn)(1989-)，男，四川內(nèi)江人，碩士研究生，研究方向?yàn)橐苿?dòng)機(jī)器人導(dǎo)航與定位。