基于空間域劃分的分布式SLAM算法

裴福俊,程雨航,李昊洋,居鶴華

(1.北京工業(yè)大學(xué)電子信息與控制工程學(xué)院,北京100124; 2.計(jì)算智能與智能系統(tǒng)北京市重點(diǎn)實(shí)驗(yàn)室,北京100124)

基于空間域劃分的分布式SLAM算法

裴?？?,2,程雨航1,2,李昊洋1,2,居鶴華1,2

(1.北京工業(yè)大學(xué)電子信息與控制工程學(xué)院,北京100124; 2.計(jì)算智能與智能系統(tǒng)北京市重點(diǎn)實(shí)驗(yàn)室,北京100124)

針對(duì)同步定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)中狀態(tài)量高維時(shí)變的問(wèn)題,本文通過(guò)綜合集中式和分布式實(shí)現(xiàn)結(jié)構(gòu)的各自優(yōu)勢(shì),提出了一種基于空間域劃分的分布式SLAM算法。該算法依據(jù)兩個(gè)路標(biāo)點(diǎn)與機(jī)器人連線之間的夾角,將整個(gè)空間域中的路標(biāo)點(diǎn)進(jìn)行區(qū)域劃分,保證每個(gè)子空間域內(nèi)含有兩個(gè)不共線的路標(biāo)點(diǎn),并將每個(gè)空間域內(nèi)的路標(biāo)點(diǎn)組合構(gòu)建觀測(cè)模型,采用分布式無(wú)味粒子濾波器進(jìn)行機(jī)器人位姿的估計(jì),而采用聯(lián)邦Kalman濾波完成對(duì)路標(biāo)點(diǎn)的估計(jì),并通過(guò)設(shè)計(jì)各子濾波器中粒子分布的調(diào)整方式改善了系統(tǒng)在動(dòng)態(tài)重構(gòu)過(guò)程的精度和穩(wěn)定性。最后,通過(guò)實(shí)際數(shù)據(jù)的仿真試驗(yàn)證明所提算法具有更好的實(shí)時(shí)性和濾波精度。

同步定位與地圖構(gòu)建;空間域劃分;分布式結(jié)構(gòu);動(dòng)態(tài)重構(gòu)

0 引 言

同步定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)已經(jīng)被世界上很多學(xué)者認(rèn)為是實(shí)現(xiàn)移動(dòng)機(jī)器人自主移動(dòng)的關(guān)鍵技術(shù)[13],也成為了移動(dòng)機(jī)器人相關(guān)研究的核心和難點(diǎn)。SLAM實(shí)際是一個(gè)非線性非高斯的狀態(tài)估計(jì)問(wèn)題,國(guó)內(nèi)外學(xué)者采用了多種非線性最優(yōu)估計(jì)方法來(lái)解決SLAM問(wèn)題,例如擴(kuò)展卡爾曼濾波(extended Kalman filer,EKF)[4]、無(wú)跡卡爾曼濾波(unscented Kalman filer,UKF)[5]、粒子濾波(particle filter,PF)[6]等。由于粒子濾波在解決非線性非高斯?fàn)顟B(tài)估計(jì)問(wèn)題方面的優(yōu)越性,成為了近年來(lái)國(guó)內(nèi)外學(xué)者研究的熱點(diǎn)方向,但其繁重的計(jì)算量制約了PF-SLAM的實(shí)時(shí)應(yīng)用。針對(duì)PF-SLAM存在的問(wèn)題,文獻(xiàn)[7]提出了RBPF-SLAM系列算法,隨后又對(duì)其進(jìn)行了擴(kuò)展提出了Fast-SLAM1.0算法,將SLAM問(wèn)題的過(guò)程模型直接作用于采樣粒子的重要性函數(shù),之后文獻(xiàn)[8]提出了改進(jìn)的Fast-SLAM2.0算法,提高了Fast-SLAM算法的精度。但是,這些算法均采用了集中式濾波器結(jié)構(gòu),即用一個(gè)濾波器完成對(duì)位姿與環(huán)境地圖的同時(shí)估計(jì),這種結(jié)構(gòu)中的狀態(tài)向量均是包括機(jī)器人和路標(biāo)信息的高維向量,并且狀態(tài)向量維數(shù)將隨環(huán)境動(dòng)態(tài)時(shí)變,必然造成集中式SLAM算法存在計(jì)算量大、穩(wěn)定性和容錯(cuò)性差等問(wèn)題。

與集中式SLAM算法相對(duì)應(yīng)的是基于分布式結(jié)構(gòu)的SLAM算法,文獻(xiàn)[9]首先提出了一種基于分布式結(jié)構(gòu)的SLAM算法,將整個(gè)狀態(tài)向量分為機(jī)器人位姿估計(jì)和路標(biāo)估計(jì)兩個(gè)過(guò)程,依據(jù)每個(gè)有效路標(biāo)點(diǎn)單獨(dú)建立子濾波器,分別實(shí)現(xiàn)對(duì)機(jī)器人位姿和路標(biāo)的估計(jì)。在分布式濾波結(jié)構(gòu)中,處理多維向量的集中濾波器被拆分成處理單個(gè)向量組成的子濾波器形式,從而有效降低了SLAM算法的計(jì)算復(fù)雜度,并提高了容錯(cuò)能力,但也造成了諸如各個(gè)子濾波器之間的信息融合精度差、子濾波器重構(gòu)致使系統(tǒng)穩(wěn)定性無(wú)法得到保證以及單個(gè)子濾波器中的粒子退化等問(wèn)題。文獻(xiàn)[10-11]在文獻(xiàn)[9]的基礎(chǔ)上,針對(duì)分布式粒子濾波SLAM算法進(jìn)行了改進(jìn),并對(duì)分布式粒子濾波的收斂性進(jìn)行了證明。

結(jié)合集中式和分布式兩種結(jié)構(gòu)模型的優(yōu)點(diǎn),本文提出了一種基于空間域劃分的分布式SLAM算法。該算法針對(duì)機(jī)器人自帶傳感器所探測(cè)的整個(gè)環(huán)境空間,依據(jù)兩個(gè)路標(biāo)點(diǎn)與機(jī)器人連線之間的夾角為大值的條件,將整個(gè)空間域中的路標(biāo)點(diǎn)進(jìn)行區(qū)域劃分,保證每個(gè)子空間域內(nèi)含有兩個(gè)不共線的路標(biāo)點(diǎn),并將每個(gè)空間域內(nèi)的有效路標(biāo)點(diǎn)組合構(gòu)建子濾波器的觀測(cè)模型,該算法保持了分布式結(jié)構(gòu)的計(jì)算量小、容錯(cuò)性好的優(yōu)點(diǎn),同時(shí)提高了各個(gè)子濾波器的估計(jì)精度。針對(duì)所建模型的非線性特點(diǎn),本文采用了分布式無(wú)跡粒子濾波(unscented particle filter,UPF)進(jìn)行機(jī)器人位姿的估計(jì),并采用聯(lián)邦卡爾曼濾波(federated Kalman filter,FKF)完成對(duì)路標(biāo)點(diǎn)的估計(jì)。同時(shí),充分利用UPF對(duì)于粒子分布的調(diào)整作用,利用空間域內(nèi)沒(méi)有經(jīng)歷動(dòng)態(tài)重構(gòu)的路標(biāo)點(diǎn)進(jìn)行量測(cè)更新有效地改善粒子分布,提高了系統(tǒng)的穩(wěn)定性和魯棒性。

1 SLAM模型

1.1 過(guò)程模型

SLAM系統(tǒng)狀態(tài)模型可看作一個(gè)馬爾可夫過(guò)程,即t時(shí)刻狀態(tài)與t-1時(shí)刻前的狀態(tài)無(wú)關(guān)。系統(tǒng)中機(jī)器人的運(yùn)動(dòng)控制信息由里程計(jì)提供,如圖1所示,為機(jī)器人的狀態(tài)轉(zhuǎn)移過(guò)程及實(shí)際機(jī)器人坐標(biāo)系。

圖1 機(jī)器人狀態(tài)轉(zhuǎn)移過(guò)程及坐標(biāo)系定義

運(yùn)動(dòng)過(guò)程中機(jī)器人的狀態(tài)[10]可以描述如下:st=(x,y, θ)T,其中,x,y表示坐標(biāo)值,θ表示偏航角(θ=0表示指向x軸正向,θ=π/2表示指向y軸正向)。機(jī)器人的狀態(tài)轉(zhuǎn)移模型如下:

式中,XL(t)=(xL(t)yL(t)?L(t))T為機(jī)器人在t時(shí)刻的位姿;ΔT為時(shí)間變化量;vc為機(jī)器人移動(dòng)速度;α為機(jī)器人變化的角度;L為兩輪軸間距;γ為高斯白噪聲。

1.2 量測(cè)模型

本文是依據(jù)激光傳感器給出機(jī)器人的觀測(cè)模型,測(cè)距激光傳感器的觀測(cè)量z是某個(gè)環(huán)境特征相對(duì)于傳感器的距離以及角度,在掃描目標(biāo)時(shí)容易出現(xiàn)誤差,實(shí)際狀況中,誤差密度函數(shù)由各種分布組合而成,一般情況包括:指數(shù)分布、均勻分布、高斯分布等,本文采用高斯分布描述誤差的分布情況。

將機(jī)器人狀態(tài)信息與觀測(cè)信息相關(guān)聯(lián),可以獲得路標(biāo)點(diǎn)的觀測(cè)模型[10]:

而在分布式模型中,則模型改寫為如下形式:

2 集中式與分布式SLAM算法對(duì)比分析

從式(2)和式(3)可以看出,SLAM問(wèn)題是非線性非高斯的狀態(tài)估計(jì)問(wèn)題。集中式SLAM算法是采用了單一濾波器實(shí)現(xiàn)對(duì)機(jī)器人位姿和路標(biāo)點(diǎn)的估計(jì)[12],不可避免地存在計(jì)算量大、復(fù)雜度高、容錯(cuò)性差、實(shí)時(shí)性差的問(wèn)題。而分布式SLAM算法是將本應(yīng)以矩陣形式集中計(jì)算的描述機(jī)器人位姿的粒子分布概率公式進(jìn)行分布化處理,之后在子濾波器中處理概率計(jì)算,最后根據(jù)相應(yīng)的融合算法進(jìn)行主濾波器融合[13]。這樣大大減少了計(jì)算復(fù)雜度,提高系統(tǒng)容錯(cuò)能力,針對(duì)集中式和分布式中的核心算法的對(duì)比分析如下:

首先,從粒子表征的角度來(lái)說(shuō),對(duì)于集中式SLAM算法,濾波器輸出結(jié)果即是用以表征機(jī)器人位置信息的粒子分布,如式(5)中左式所示;而分布式SLAM算法中,則每個(gè)子濾波器中均有對(duì)機(jī)器人位置的估計(jì)如式(5)中右式所示。

其次,集中式SLAM濾波器中的狀態(tài)維數(shù)隨著路標(biāo)點(diǎn)個(gè)數(shù)M時(shí)變,根據(jù)控制信息ut得到粒子預(yù)測(cè)分布后,將量測(cè)信息融入構(gòu)成N個(gè)粒子的權(quán)重值,M個(gè)路標(biāo)點(diǎn)的權(quán)值計(jì)算過(guò)程如式(6)所示,其中粒子標(biāo)號(hào)i=1,2,…,N,路標(biāo)點(diǎn)標(biāo)號(hào)j=1,2,…,M。

根據(jù)權(quán)重值更新,可輸出位姿估計(jì)的計(jì)算結(jié)果如式(7)。

而分布式SLAM中各濾波器所處理的數(shù)據(jù)維數(shù)固定且為總維數(shù)的1/M,根據(jù)各子濾波器中粒子群的預(yù)測(cè)分布,然后對(duì)每個(gè)子濾波器中的粒子進(jìn)行重要性權(quán)值估計(jì)如式(8)所示。

根據(jù)子濾波器位姿結(jié)果,進(jìn)行主濾波器的融合。如式(9)及式(10)所示。最后,集中式SLAM算法中,最終位姿的概率分布可表示為式(11),并將帶有wit權(quán)值的樣本映射為等權(quán)樣本。設(shè)置閾值Nthreshold,當(dāng)有效粒子數(shù)時(shí),進(jìn)入重采樣過(guò)程。

而分布式SLAM中,最終位姿是主濾波器進(jìn)行數(shù)據(jù)融合的結(jié)果,其概率分布可表示為式(12)的形式,各子濾波器獨(dú)立進(jìn)行重采樣:,為保持粒子集一致性,需要定期全局重采樣。

通過(guò)對(duì)兩者的比較,對(duì)于兩者的算法流程對(duì)估計(jì)結(jié)果的影響,分析如下:

式(11)和式(12)反映了集中式與分布式兩種方式的概率分布求解的不同之處:集中式粒子濾波器在每個(gè)單步的估計(jì)精度更高,可以更好地降低估計(jì)的不確定性,但是不可避免地會(huì)受到多維向量不斷增加和重構(gòu)造成的影響,造成算法的穩(wěn)定性、容錯(cuò)性和計(jì)算量問(wèn)題。而分布式結(jié)構(gòu)可以有效降低計(jì)算的復(fù)雜度和計(jì)算量,但由于每個(gè)子濾波器只考慮相對(duì)應(yīng)的路標(biāo)點(diǎn),需要按照式(10)進(jìn)行子濾波器的融合,導(dǎo)致融合算法中的ηjt直接決定了融合輸出的結(jié)果,而ηjt與有效粒子數(shù)有關(guān),也就與式(9)中每個(gè)子濾波器中權(quán)值的估計(jì)結(jié)果相關(guān),從而由于單個(gè)子濾波器中的估計(jì)精度有限必然會(huì)造成融合誤差的出現(xiàn)。

3 基于空間域組合觀測(cè)的SLAM算法

3.1 基于空間域劃分的SLAM模型

依據(jù)上文對(duì)集中式和分布式兩種SLAM算法流程的分析,本文借鑒了集中式和分布式兩種SLAM結(jié)構(gòu)的優(yōu)勢(shì),綜合利用分布式結(jié)構(gòu)降低計(jì)算量和計(jì)算復(fù)雜度以及提升容錯(cuò)性的優(yōu)點(diǎn),同時(shí)結(jié)合集中式結(jié)構(gòu)在單步估計(jì)中精度較高的優(yōu)勢(shì),設(shè)計(jì)了基于空間域劃分的分布式SLAM算法。

依據(jù)SLAM系統(tǒng)模型的可觀測(cè)性可知,當(dāng)同時(shí)觀測(cè)到非共線的兩個(gè)特征點(diǎn)時(shí),才能保證SLAM模型具有較好的可觀測(cè)性[14]。本文將分布式的結(jié)構(gòu)進(jìn)行相對(duì)集中化的改進(jìn),對(duì)于每個(gè)子濾波器不是僅僅引入單個(gè)路標(biāo)點(diǎn)作為觀測(cè)量,而是在每步計(jì)算過(guò)程中,選取兩個(gè)不共線的特征點(diǎn)組合成觀測(cè)量,這樣既能發(fā)揮在處理少觀測(cè)點(diǎn)時(shí)集中式粒子濾波的精度優(yōu)勢(shì),又能發(fā)揮分布式結(jié)構(gòu)處理多觀測(cè)點(diǎn)時(shí)降低計(jì)算復(fù)雜度的作用。

具體實(shí)現(xiàn)原理是:針對(duì)傳感器所觀測(cè)的整個(gè)空間,依據(jù)兩個(gè)路標(biāo)點(diǎn)與機(jī)器人連線之間的夾角θ,將整個(gè)觀測(cè)空間中的路標(biāo)點(diǎn)進(jìn)行區(qū)域劃分,保證其共線的可能性最低。假設(shè)機(jī)器人傳感器測(cè)量的路標(biāo)點(diǎn)情況如圖2所示,則將傳感器觀測(cè)到的整個(gè)空間劃分為幾個(gè)子空間域A-A',B-B', C-C',如果路標(biāo)點(diǎn)個(gè)數(shù)為偶數(shù),每個(gè)空間域內(nèi)包含兩個(gè)不共線的路標(biāo)點(diǎn)。而路標(biāo)點(diǎn)個(gè)數(shù)為奇數(shù)時(shí),將出現(xiàn)一個(gè)包含單個(gè)路標(biāo)點(diǎn)的子空間域。

空間域劃分實(shí)現(xiàn)流程如下:

步驟1 將激光傳感器輸入的原始數(shù)據(jù)轉(zhuǎn)化為路標(biāo)點(diǎn)信息。機(jī)器人通過(guò)激光傳感器掃描前方180°范圍內(nèi)的障礙物,在返回值中篩選t時(shí)刻范圍內(nèi)的路標(biāo)點(diǎn)z(t)={m1,m2,…,mn}。

步驟2 匹配t時(shí)刻與t-1時(shí)刻路標(biāo)點(diǎn)并判斷是否為新加入路標(biāo)點(diǎn)。對(duì)比t-1時(shí)刻路標(biāo)點(diǎn)集,設(shè)定鄰域Δ=,根據(jù)最小鄰域法進(jìn)行兩個(gè)時(shí)刻的路標(biāo)點(diǎn)匹配,若在鄰域內(nèi)則認(rèn)為t時(shí)刻與t-1時(shí)刻所測(cè)量的信息來(lái)源于同一路標(biāo)點(diǎn),否則視為新觀測(cè)到的路標(biāo)點(diǎn)。

步驟3 按照角度劃分空間域。機(jī)器人與路標(biāo)點(diǎn)的相對(duì)位置如圖2所示,定義路標(biāo)點(diǎn)方向角度θi為:在水平面內(nèi),由0°開始沿逆時(shí)針?lè)较蛐D(zhuǎn)到第i個(gè)路標(biāo)點(diǎn)所在位置所形成的角度。

圖2 空間域劃分處理示意圖

假設(shè)對(duì)應(yīng)的角度分布范圍為[θmin,θmax],分步將路標(biāo)點(diǎn)劃入不同子空間域中,將第n次劃分后未被劃分的路標(biāo)點(diǎn)按照對(duì)應(yīng)角度從小到大排列,構(gòu)成有序集,同時(shí),定義每次劃分過(guò)程中,將劃分為一個(gè)子空間域,剩余路標(biāo)點(diǎn)再次重復(fù)上述過(guò)程。例如:經(jīng)過(guò)第一次劃分后,剩余的路標(biāo)點(diǎn)有序集合變?yōu)棣?={θ1,θ2,…,θi-2,i∈Ν*},將其中的{θ1

min,θ1

mid}劃分入一個(gè)子空間域中。依照上述算法流程,設(shè)經(jīng)過(guò)n步劃分之后則將剩余路標(biāo)點(diǎn)劃歸同一個(gè)子空間域,至此劃分過(guò)程結(jié)束。

步驟4 空間域模型的建立。將每個(gè)子空間域中的所有路標(biāo)點(diǎn)組合成一個(gè)觀測(cè)向量建立觀測(cè)方程,從而基于各子空間域構(gòu)建各子濾波器的組合觀測(cè)模型,最終構(gòu)建基于空間域劃分的分布式SLAM系統(tǒng)方程如式(13)所示。

其中,k=1,2,…,n表示第k個(gè)子空間域。

基于上述空間域劃分方法建立的各子空間域及對(duì)應(yīng)的系統(tǒng)方程,對(duì)各子系統(tǒng)的濾波器進(jìn)行設(shè)計(jì),各子濾波器的設(shè)計(jì)均采用集中式濾波結(jié)構(gòu)如式(14):

對(duì)于子濾波器的融合,采用分布式濾波結(jié)構(gòu),對(duì)量測(cè)信息進(jìn)行融合如式(15):

式中,Mk為所劃分的子空間總數(shù)。

從式(13)可以看出,本文提出的分布式SLAM結(jié)構(gòu)中的機(jī)器人位姿估計(jì)部分仍然是非線性模型,因此將采用分布式UPF完成對(duì)機(jī)器人位姿的估計(jì)[15]。由于估計(jì)的過(guò)程中位姿估計(jì)和路標(biāo)點(diǎn)估計(jì)是分別進(jìn)行的,高概率情況下完成估計(jì)的位姿可近似認(rèn)為是機(jī)器人的準(zhǔn)確位置,結(jié)合傳感器的測(cè)量信息可以計(jì)算出路標(biāo)點(diǎn)近似精確值,此時(shí)路標(biāo)點(diǎn)則可以被認(rèn)為是靜態(tài)的,路標(biāo)點(diǎn)的估計(jì)模型可以簡(jiǎn)化為

式中,xL(t)為t時(shí)刻的路標(biāo)點(diǎn)位置;v為觀測(cè)噪聲。

從式(16)可以看出,路標(biāo)點(diǎn)的估計(jì)過(guò)程轉(zhuǎn)換為了一個(gè)線性過(guò)程,完全可以采用FKF完成對(duì)路標(biāo)點(diǎn)的估計(jì)和更新?；诳臻g域劃分的SLAM系統(tǒng)框圖如圖3所示。

圖3 基于空間域劃分的分布式SLAM系統(tǒng)

3.2 動(dòng)態(tài)重構(gòu)過(guò)程分析

在分布式SLAM算法的計(jì)算過(guò)程中,隨著機(jī)器人的運(yùn)動(dòng),某些路標(biāo)點(diǎn)可能不再出現(xiàn)在觀測(cè)范圍內(nèi)或是觀測(cè)到新的路標(biāo)點(diǎn),如圖4所示。此時(shí),所對(duì)應(yīng)的子空間域?qū)⒅貥?gòu),對(duì)應(yīng)的子濾波器的狀態(tài)量和觀測(cè)信息需要進(jìn)行舍棄或更新,從而會(huì)引起整個(gè)分布式系統(tǒng)的動(dòng)態(tài)重構(gòu),該動(dòng)態(tài)重構(gòu)過(guò)程必然會(huì)帶來(lái)系統(tǒng)重構(gòu)誤差,導(dǎo)致分布式SLAM系統(tǒng)的穩(wěn)定性和魯棒性問(wèn)題。本文所采用的模型結(jié)構(gòu)能夠充分利用UPF對(duì)于粒子分布的調(diào)整作用,利用空間域內(nèi)沒(méi)有經(jīng)歷動(dòng)態(tài)重構(gòu)的路標(biāo)點(diǎn)進(jìn)行量測(cè)更新,有效地改善粒子分布,提高了系統(tǒng)的穩(wěn)定性和魯棒性。

從圖4中可以看出,在機(jī)器人運(yùn)行過(guò)程中,路標(biāo)點(diǎn)變化情況主要會(huì)有如下3種類型:

(Ⅰ)在t時(shí)刻已經(jīng)匹配成功,在t+1時(shí)刻仍可以觀測(cè)并且匹配成功,如圖4中路標(biāo)點(diǎn)A-A'。

(Ⅱ)在t時(shí)刻匹配成功,但是在t+1時(shí)刻沒(méi)有再觀測(cè)到該路標(biāo)點(diǎn),如圖4中路標(biāo)點(diǎn)B-B'。

(Ⅲ)在t時(shí)刻沒(méi)有觀測(cè)到,而在t+1時(shí)刻第一次觀測(cè)到并匹配成功,如圖4中路標(biāo)點(diǎn)C。

圖4 空間域內(nèi)路標(biāo)點(diǎn)類型

(1)對(duì)于類型Ⅰ中的特征點(diǎn),各子濾波器中正常采用UPF完成對(duì)機(jī)器人位姿的估計(jì),采用無(wú)跡變換按如下過(guò)程實(shí)時(shí)更新濾波器中的粒子:

步驟1 進(jìn)行粒子的初始化,利用先驗(yàn)分布{xi0,i=0, 1,2,…,N}在每個(gè)子濾波器中產(chǎn)生粒子群,并通過(guò)無(wú)跡變換選取粒子

步驟2 時(shí)間更新,把每個(gè)粒子對(duì)機(jī)器人狀態(tài)估計(jì)的擴(kuò)展向量分別代入機(jī)器人運(yùn)動(dòng)模型和路標(biāo)點(diǎn)觀測(cè)模型得到每個(gè)粒子的一步預(yù)測(cè)狀態(tài)向量和一步預(yù)測(cè)觀測(cè)向量根據(jù)每個(gè)粒子的權(quán)系數(shù),計(jì)算每個(gè)子濾波器中所有粒子的狀態(tài)向量的一步預(yù)測(cè)加權(quán)和和觀測(cè)向量的一步預(yù)測(cè)加權(quán)和z-j

t|t-1。

步驟3 量測(cè)更新,對(duì)于預(yù)測(cè)的粒子分布情況,需要加入量測(cè)更新過(guò)程,從而根據(jù)實(shí)測(cè)路標(biāo)點(diǎn)更新粒子狀態(tài)分布:

(2)對(duì)于類型Ⅱ的特征點(diǎn),若子濾波器中仍有其他路標(biāo)點(diǎn)能夠被觀測(cè)到,則利用此路標(biāo)點(diǎn)構(gòu)建單路標(biāo)點(diǎn)觀測(cè)的子濾波器繼續(xù)進(jìn)行估計(jì)及重構(gòu),該結(jié)構(gòu)有利于新加入特征點(diǎn)的初始化,設(shè)jL為濾波器中仍能夠觀測(cè)到的路標(biāo)點(diǎn),當(dāng)新的路標(biāo)點(diǎn)進(jìn)入空間域,此類jL可以為其提供先驗(yàn)概率分布。

(3)對(duì)于類型Ⅲ特征點(diǎn),若除了重構(gòu)的路標(biāo)點(diǎn)外,子濾波器仍能夠通過(guò)空間域內(nèi)的第Ⅱ類路標(biāo)點(diǎn)進(jìn)行量測(cè)更新過(guò)程,則子濾波器中的粒子分布情況由本次量測(cè)的路標(biāo)點(diǎn)進(jìn)行調(diào)整,那么式(19)中的一步預(yù)測(cè)誤差zi,jt|t-1-z-jt|t-1的量測(cè)更新采用式(20)進(jìn)行計(jì)算:

通過(guò)使新加入路標(biāo)點(diǎn)繼承未參與重構(gòu)的路標(biāo)點(diǎn)粒子分布的方法,能夠充分利用Ⅱ類特征點(diǎn)提供的量測(cè)信息達(dá)到優(yōu)化調(diào)整粒子分布的目的,而普通的分布式SLAM中新加入的路標(biāo)點(diǎn)則只能通過(guò)新建立子濾波器進(jìn)行初始化,收斂時(shí)間過(guò)長(zhǎng),影響系統(tǒng)的精度。此外,如果新加入的路標(biāo)點(diǎn)不被納入存在jL類型路標(biāo)點(diǎn)的子空間域內(nèi),則以此路標(biāo)點(diǎn)為基礎(chǔ)建立的新濾波器,初始化后等待下一步觀測(cè)重復(fù)式(19)的過(guò)程。

空間域中的粒子調(diào)整算法主要是利用無(wú)跡變換,從原先狀態(tài)分布中按某一規(guī)則提取一些點(diǎn),將這些點(diǎn)帶入非線性函數(shù)中,取得相應(yīng)的非線性函數(shù)點(diǎn)集,最后通過(guò)求取變化后的均值和協(xié)方差進(jìn)行粒子調(diào)整。從而在路標(biāo)點(diǎn)的更替過(guò)程中,單一路標(biāo)點(diǎn)對(duì)應(yīng)單個(gè)子濾波器在路標(biāo)點(diǎn)丟失或加入時(shí)面臨的子濾波器重構(gòu)問(wèn)題得以改善,從而改善系統(tǒng)的整體性能。

4 仿真實(shí)驗(yàn)

本文采用澳大利亞悉尼大學(xué)研究人員測(cè)得的實(shí)際數(shù)據(jù)[17]進(jìn)行仿真實(shí)驗(yàn),該數(shù)據(jù)是由車輛穿過(guò)維多利亞公園內(nèi)各種不同區(qū)域,利用車輛自身裝配的里程計(jì)和激光測(cè)距傳感器獲取車輛的當(dāng)前里程信息和車輛距路標(biāo)點(diǎn)(樹)之間的距離,并同步使用GPS系統(tǒng)獲得車輛位置信息。

在本文的仿真實(shí)驗(yàn)中,分別采用基于分布式UPF的分布式SLAM(distributed unscented particle filer-SLAM, DUPF-SLAM)與本文提出的基于空間域劃分的分布式SLAM(distributed unscented particle filer-SLAM based on subspace,SDUPF-SLAM)分別進(jìn)行估計(jì),兩種算法的仿真結(jié)果如圖5所示。其中,虛線為DUPF算法的估計(jì)結(jié)果,實(shí)線為SDUPF算法的估計(jì)結(jié)果。星號(hào)為GPS數(shù)據(jù),由于公園內(nèi)樹木遮擋存在GPS覆蓋盲區(qū),因此GPS數(shù)據(jù)并不連續(xù)。

圖5 兩種算法的估計(jì)結(jié)果與GPS數(shù)據(jù)對(duì)比

根據(jù)上述實(shí)驗(yàn)結(jié)果可以看出,SDUPF算法與DUPF算法相比,在粒子數(shù)目相同的前提下,隨著車輛運(yùn)動(dòng)時(shí)間的增加,DUPF算法的精度逐漸變差。在初始階段,兩者的估計(jì)精度相差不大,SDUPF算法精度稍高,隨后GPS信號(hào)被遮擋,當(dāng)再次接收到GPS信號(hào)時(shí)(x=35.92 m,y=61.45 m,圖中G1標(biāo)識(shí)處),隨后的一段時(shí)間內(nèi),DUPF估計(jì)的位置在x方向上明顯偏離GPS數(shù)據(jù)達(dá)3.53 m(圖中G2標(biāo)識(shí)處),而SDUPF算法仍能很好地跟蹤車輛的位置軌跡,在x和y軸方向偏離均小于1 m。當(dāng)車輛再次接收到GPS信號(hào)時(shí)(x=66.34 m,y=75.02 m,圖中G3標(biāo)識(shí)處),此后SDUPF算法的跟蹤效果要優(yōu)于DUPF的結(jié)果(圖中G4標(biāo)識(shí)處),如圖5中所示。因此,可以證明采用空間域劃分的SDUPF算法的位姿估計(jì)精度明顯高于DUPF算法。

為了對(duì)比兩種算法的估計(jì)誤差,本文采用式(21)對(duì)于實(shí)驗(yàn)結(jié)果中進(jìn)行誤差分析,給出兩種算法在整個(gè)車輛定位過(guò)程中的估計(jì)結(jié)果和GPS數(shù)據(jù)之間在x方向以及y方向上的平均誤差值如表1所示。由表1可知,在整個(gè)估計(jì)過(guò)程中,x方向和y方向上均是SDUPF的估計(jì)結(jié)果誤差更小。

表1 估計(jì)結(jié)果與GPS數(shù)據(jù)的平均誤差

在初始階段GPS有效時(shí),兩種算法的估計(jì)誤差如圖6所示。其中,虛線為DUPF算法估計(jì)誤差,實(shí)線為SDUPF估計(jì)誤差。從圖6中可以看出,從濾波估計(jì)初始階段6.5 s開始,SDUPF算法估計(jì)結(jié)果在x方向以及y方向上誤差均低于DUPF算法的估計(jì)誤差?？梢奡DUPF的初始濾波估計(jì)精度就高于DUPF算法。

圖6 濾波初始階段與GPS的誤差

5 結(jié) 論

針對(duì)SLAM算法實(shí)現(xiàn)過(guò)程中存在的問(wèn)題,本文提出了一種基于空間域劃分的分布式SLAM算法,該算法先將觀測(cè)到的路標(biāo)點(diǎn)劃分到各個(gè)子空間域,并依據(jù)子空間域構(gòu)建組合觀測(cè)模型,以分布式UPF和FKF分別實(shí)現(xiàn)機(jī)器人位姿和路標(biāo)點(diǎn)的實(shí)時(shí)估計(jì)。同時(shí),在每個(gè)子濾波器中利用無(wú)味變換的方法進(jìn)行粒子分布的調(diào)整,利用空間域中無(wú)需重構(gòu)的路標(biāo)點(diǎn)進(jìn)行粒子調(diào)整可以有效減少子濾波器的重構(gòu)次數(shù),該算法不僅借鑒了分布式結(jié)構(gòu)可以使每個(gè)濾波器計(jì)算量下降,增強(qiáng)容錯(cuò)性的特點(diǎn),又同時(shí)具備了集中式粒子濾波器在單步估計(jì)上的精度優(yōu)勢(shì)。仿真結(jié)果表明,本文提出的基于空間域劃分的分布式SLAM算法的定位精度優(yōu)于DUPF算法,并能夠有效減少DUPF在長(zhǎng)時(shí)間運(yùn)行后由于子濾波器退化嚴(yán)重及重構(gòu)次數(shù)增多造成的精度和穩(wěn)定性問(wèn)題。

[1]Durant W H,Bailey T.Simultaneous localization and mapping: part I the essential algorithms[J].Robotics and Automation Magazine,2006,13(2):99-110.

[2]Guivant J,Nebot E.Optimization of the simultaneous localization and map building algorithm for real time implementation[J].Proceedingsof the IEEE Transactions of Robotics and Automation, 2001,17(3):242-257.

[3]Csorba M.Simultaneous localization and map building[R].Oxford:University of Oxford,1997.

[4]Smith R C,Cheeseman P.On the representation and estimation of spatial uncertainty[J].Robotics Research,1986,5(4):231-238.

[5]Julier SJ,Uhlmann J K.Unscented filtering and nonlinear estimation[J].Proceeding of the IEEE Aerospace and Electronic Systems,2004,92(3):401-422.

[6]Bolic M,Djuric P M,Hong S,et al.New resampling algorithms for particle filters[C]∥Proc.of the IEEE International Conference on Robotics and Automation,2003:589-592.

[7]Montemerlo M,Thrun S,Koller D,et al.RBPF-SLAM:A factored solution to the simultaneous localization and mapping problem[C]∥Proc.of the National Conference on Artificial Intelligence,2002:593 598.

[8]Montemerlo M,Thrun S,Koller D,et al.RBPF-SLAM 2.0:animproved particle filtering algorithm for simultaneous localization and mapping that probably converges[C]∥Proc.of the International Conference on Artificial Intelligence,2003:1151-1156.

[9]Won D,Chun S,Sung S,et al.INS/SLAM system using distributed particle filter[J].International Journal of Control, Automation and Systems,2010,8(6):1232-1240.

[10]Pei F J,Wu M,Zhang S M,Distributed SLAM usingimproved particle filter for mobile robot localization[J].The Scientific World Journal,2014:DOI:10.1155/2014/239531.

[11]Pei F J,Wu M,Zhang S M.Advanced distributed unscented particle filter for simultaneous localization and mapping[J]. WIT Trans.on Engineering Sciences,2014,86:DOI:10. 2495/ICFCIT131101.

[12]Thrun S,Burgard W,Fox D.Probabilistic robotics[M].Cambridge,MA:MIT Press,2006:148 151.

[13]Hlinka O,Hlawatsch F,Druric P M.Distributed particle filtering in agent networks[J].IEEE Signal Processing Magazine,2013,30(1):61-78.

[14]Perera L D L,Eric Netteleton.On the nonlinear observability and the information form of the SLAM problem[C]∥Proc.of the IEEE International Conference on Intelligent Robots and Systems,2009:2061 2067.

[15]Farahmand S,Roumeliotis S I,Giannakis G B.Set-membership constrained particle filter:distributed adaption for sensor network[J].IEEE Trans.on Signal Processing,2011,59 (9):4127-4135.

[16]Zhu Z Y.The algorithms and implementation for particle filters[M].Beijing:Science Press,2010:37-39.(朱志宇.粒子濾波算法及其應(yīng)用[M].北京:科學(xué)出版社,2010:37-39.)

[17]Jose G.Victoria Park[EB/OL].[2013-10-18].http:∥www-personal.acfr.usyd.edu.au/nebot/victoria_park.htm.

Distributed simultaneous localization and mapping algorithm based on partition of space-region

PEI Fu-jun1,2,CHENG Yu-hang1,2,LI Hao-yang1,2,JU He-hua1,2
(1.School of Electronic Information&Control Engineering,Beijing University of Technology,Beijing 100124,China; 2.Beijing Key Laboratory of Computational Intelligence and Intelligence System,Beijing 100124,China)

To solve the problem of the simultaneous localization and mapping(SLAM)under the complex circumstance,a distributed algorithm of the SLAM based on partition of space-region is proposed considering the respective advantages of centralized configuration and distributed structure.The region is formed according to the angle between two landmarks and the robot,which is designed in case of the collinearity between two landmarks.The landmarks in each region are combined to establish the corresponding observation model.Besides,the position of the robot is obtained by applying the distributed unscented particle filter and the positions of the landmarks are estimated simultaneously by employing the Kalman filter.Meanwhile,the accuracy and the stability are improved through constructing the adjustment of particle distribution during the dynamic reconfiguration process.Eventually,the better real-time capability and filter accuracy of the proposed SLAM algorithm are proved through simulation experiments which are supported by actual data.

simultaneous localization and mapping(SLAM);partition of space-region;distributed structure;dynamic reconfiguration

TP 242.6

A

10.3969/j.issn.1001-506X.2015.03.26

裴?？?1976-),男,副教授,博士,主要研究方向?yàn)閼T性導(dǎo)航、自主導(dǎo)航及信息融合。

E-mail:PFJ@bjut.edu.cn

程雨航(1990),男,碩士研究生,主要研究方向?yàn)闄C(jī)器人自主導(dǎo)航算法。

E-mail:chengyuhangzxl@sina.com

李昊洋(1989-),女,碩士研究生,主要研究方向?yàn)闄C(jī)器人自主導(dǎo)航算法。E-mail:2548590420@qq.com

居鶴華(1969),男,教授,博士,主要研究方向?yàn)闄C(jī)器人自主導(dǎo)航控制。

E-mail:juhehua@bjut.edu.cn

網(wǎng)址:www.sys-ele.com

1001-506X(2015)03-0639-07

2014 06 26;

2014 09 18;網(wǎng)絡(luò)優(yōu)先出版日期:2014 10 30。

網(wǎng)絡(luò)優(yōu)先出版地址:http://w ww.cnki.net/kcms/detail/11.2422.TN.20141030.0939.002.html

國(guó)家自然科學(xué)基金(60975065);北京市青年拔尖人才培育計(jì)劃(CITTCD201304046)資助課題