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

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

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

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

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

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

同步定位與地圖構建;空間域劃分;分布式結構;動態(tài)重構

0 引 言

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

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

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

1 SLAM模型

1.1 過程模型

SLAM系統(tǒng)狀態(tài)模型可看作一個馬爾可夫過程,即t時刻狀態(tài)與t-1時刻前的狀態(tài)無關。系統(tǒng)中機器人的運動控制信息由里程計提供,如圖1所示,為機器人的狀態(tài)轉移過程及實際機器人坐標系。

圖1 機器人狀態(tài)轉移過程及坐標系定義

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

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

1.2 量測模型

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

將機器人狀態(tài)信息與觀測信息相關聯(lián),可以獲得路標點的觀測模型[10]:

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

2 集中式與分布式SLAM算法對比分析

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

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

其次,集中式SLAM濾波器中的狀態(tài)維數隨著路標點個數M時變,根據控制信息ut得到粒子預測分布后,將量測信息融入構成N個粒子的權重值,M個路標點的權值計算過程如式(6)所示,其中粒子標號i=1,2,…,N,路標點標號j=1,2,…,M。

根據權重值更新,可輸出位姿估計的計算結果如式(7)。

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

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

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

通過對兩者的比較,對于兩者的算法流程對估計結果的影響,分析如下:

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

3 基于空間域組合觀測的SLAM算法

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

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

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

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

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

步驟1 將激光傳感器輸入的原始數據轉化為路標點信息。機器人通過激光傳感器掃描前方180°范圍內的障礙物,在返回值中篩選t時刻范圍內的路標點z(t)={m1,m2,…,mn}。

步驟2 匹配t時刻與t-1時刻路標點并判斷是否為新加入路標點。對比t-1時刻路標點集,設定鄰域Δ=,根據最小鄰域法進行兩個時刻的路標點匹配,若在鄰域內則認為t時刻與t-1時刻所測量的信息來源于同一路標點,否則視為新觀測到的路標點。

步驟3 按照角度劃分空間域。機器人與路標點的相對位置如圖2所示,定義路標點方向角度θi為:在水平面內,由0°開始沿逆時針方向旋轉到第i個路標點所在位置所形成的角度。

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

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

min,θ1

mid}劃分入一個子空間域中。依照上述算法流程,設經過n步劃分之后則將剩余路標點劃歸同一個子空間域,至此劃分過程結束。

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

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

基于上述空間域劃分方法建立的各子空間域及對應的系統(tǒng)方程,對各子系統(tǒng)的濾波器進行設計,各子濾波器的設計均采用集中式濾波結構如式(14):

對于子濾波器的融合,采用分布式濾波結構,對量測信息進行融合如式(15):

式中,Mk為所劃分的子空間總數。

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

式中,xL(t)為t時刻的路標點位置;v為觀測噪聲。

從式(16)可以看出,路標點的估計過程轉換為了一個線性過程,完全可以采用FKF完成對路標點的估計和更新。基于空間域劃分的SLAM系統(tǒng)框圖如圖3所示。

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

3.2 動態(tài)重構過程分析

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

從圖4中可以看出,在機器人運行過程中,路標點變化情況主要會有如下3種類型:

(Ⅰ)在t時刻已經匹配成功,在t+1時刻仍可以觀測并且匹配成功,如圖4中路標點A-A'。

(Ⅱ)在t時刻匹配成功,但是在t+1時刻沒有再觀測到該路標點,如圖4中路標點B-B'。

(Ⅲ)在t時刻沒有觀測到,而在t+1時刻第一次觀測到并匹配成功,如圖4中路標點C。

圖4 空間域內路標點類型

(1)對于類型Ⅰ中的特征點,各子濾波器中正常采用UPF完成對機器人位姿的估計,采用無跡變換按如下過程實時更新濾波器中的粒子:

步驟1 進行粒子的初始化,利用先驗分布{xi0,i=0, 1,2,…,N}在每個子濾波器中產生粒子群,并通過無跡變換選取粒子

步驟2 時間更新,把每個粒子對機器人狀態(tài)估計的擴展向量分別代入機器人運動模型和路標點觀測模型得到每個粒子的一步預測狀態(tài)向量和一步預測觀測向量根據每個粒子的權系數,計算每個子濾波器中所有粒子的狀態(tài)向量的一步預測加權和和觀測向量的一步預測加權和z-j

t|t-1。

步驟3 量測更新,對于預測的粒子分布情況,需要加入量測更新過程,從而根據實測路標點更新粒子狀態(tài)分布:

(2)對于類型Ⅱ的特征點,若子濾波器中仍有其他路標點能夠被觀測到,則利用此路標點構建單路標點觀測的子濾波器繼續(xù)進行估計及重構,該結構有利于新加入特征點的初始化,設jL為濾波器中仍能夠觀測到的路標點,當新的路標點進入空間域,此類jL可以為其提供先驗概率分布。

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

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

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

4 仿真實驗

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

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

圖5 兩種算法的估計結果與GPS數據對比

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

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

表1 估計結果與GPS數據的平均誤差

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

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

5 結 論

針對SLAM算法實現(xiàn)過程中存在的問題,本文提出了一種基于空間域劃分的分布式SLAM算法,該算法先將觀測到的路標點劃分到各個子空間域,并依據子空間域構建組合觀測模型,以分布式UPF和FKF分別實現(xiàn)機器人位姿和路標點的實時估計。同時,在每個子濾波器中利用無味變換的方法進行粒子分布的調整,利用空間域中無需重構的路標點進行粒子調整可以有效減少子濾波器的重構次數,該算法不僅借鑒了分布式結構可以使每個濾波器計算量下降,增強容錯性的特點,又同時具備了集中式粒子濾波器在單步估計上的精度優(yōu)勢。仿真結果表明,本文提出的基于空間域劃分的分布式SLAM算法的定位精度優(yōu)于DUPF算法,并能夠有效減少DUPF在長時間運行后由于子濾波器退化嚴重及重構次數增多造成的精度和穩(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.(朱志宇.粒子濾波算法及其應用[M].北京:科學出版社,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-),男,副教授,博士,主要研究方向為慣性導航、自主導航及信息融合。

E-mail:PFJ@bjut.edu.cn

程雨航(1990),男,碩士研究生,主要研究方向為機器人自主導航算法。

E-mail:chengyuhangzxl@sina.com

李昊洋(1989-),女,碩士研究生,主要研究方向為機器人自主導航算法。E-mail:2548590420@qq.com

居鶴華(1969),男,教授,博士,主要研究方向為機器人自主導航控制。

E-mail:juhehua@bjut.edu.cn

網址:www.sys-ele.com

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

2014 06 26;

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

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

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