一種移動(dòng)機(jī)器人SLAM中的多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法

陳白帆，蔡自興，鄒智榮

(中南大學(xué) 信息科學(xué)與工程學(xué)院, 湖南 長(zhǎng)沙，410083)

移動(dòng)機(jī)器人同時(shí)定位與建圖(Simultaneous localization and mapping，SLAM)問題是移動(dòng)機(jī)器人研究領(lǐng)域的基本問題與研究熱點(diǎn)，也是移動(dòng)機(jī)器人真正實(shí)現(xiàn)自主的最重要的條件之一。所謂同時(shí)定位與建圖，是指機(jī)器人在移動(dòng)過程中根據(jù)位姿估計(jì)和傳感器數(shù)據(jù)進(jìn)行自身定位，同時(shí)建造增量式地圖[1]。數(shù)據(jù)關(guān)聯(lián)問題也稱一致性問題，本是目標(biāo)跟蹤中的問題，用于確定傳感器的測(cè)量信息和目標(biāo)源之間的對(duì)應(yīng)關(guān)系[2]。在移動(dòng)機(jī)器人SLAM中數(shù)據(jù)關(guān)聯(lián)是指建立在不同時(shí)間、不同地段獲得的傳感器測(cè)量之間、傳感器測(cè)量與地圖特征之間或者地圖特征之間的對(duì)應(yīng)關(guān)系，以確定它們是否源于環(huán)境中同一物理實(shí)體的過程。它是SLAM本身面臨的挑戰(zhàn)之一，對(duì)于SLAM的狀態(tài)估計(jì)至關(guān)重要，并直接影響到SLAM的計(jì)算復(fù)雜度和結(jié)果的正確性。目前，移動(dòng)機(jī)器人SLAM中的數(shù)據(jù)關(guān)聯(lián)的研究主要有3個(gè)方面：

(1) 局部數(shù)據(jù)關(guān)聯(lián)。當(dāng)前觀測(cè)信息與已有地圖中的某個(gè)特征匹配或 2個(gè)連續(xù)的觀測(cè)幀間的匹配(Scan matching)從而進(jìn)行特征或目標(biāo)的跟蹤問題的匹配問題，這是SLAM過程中必不可少的基本問題，也是本文的主要研究問題。

(2) 循環(huán)閉合。主要針對(duì)環(huán)境中存在循環(huán)(Loop)地形的情況。當(dāng)移動(dòng)機(jī)器人繞循環(huán)地形1周時(shí)，綜合應(yīng)用各方面信息確認(rèn)機(jī)器人回到了循環(huán)起點(diǎn)，并獲得當(dāng)前觀測(cè)與循環(huán)起點(diǎn)處地圖特征間的關(guān)聯(lián)關(guān)系，對(duì)機(jī)器人定位及地圖進(jìn)行誤差修正。

(3) 地圖合并。對(duì)于大范圍的環(huán)境探索，需要用到多個(gè)機(jī)器人協(xié)作以減少環(huán)境探索時(shí)間、提高環(huán)境建圖的精度。在多個(gè)機(jī)器人SLAM時(shí)，為了生成全局地圖，就出現(xiàn)了各機(jī)器人的局部地圖間相同物理實(shí)體的路標(biāo)關(guān)聯(lián)問題，即地圖合并問題。

數(shù)據(jù)間的關(guān)聯(lián)通常利用統(tǒng)計(jì)估計(jì)的方法來確定。目前，在SLAM領(lǐng)域中也提出了許多數(shù)據(jù)關(guān)聯(lián)算法，大多采用門限法，其中比較經(jīng)典的算法包括單匹配最近鄰(Individual compatibility nearest neighbor，ICNN)、分枝限界聯(lián)合匹配(Joint compatibility branch and bound，JCBB)[3]。此外，還有Bailey等[4]引入圖論思想，通過尋找2幅完全圖間的最大公共子圖獲得觀測(cè)與地圖間的數(shù)據(jù)關(guān)聯(lián)。該方法具很強(qiáng)的抗干擾能力，但是，搜索最大公共子圖問題很難，且構(gòu)造完全圖需要提取觀測(cè)特征和觀測(cè)間的約束，當(dāng)觀測(cè)增多時(shí)計(jì)算量會(huì)顯著增加。Zhang等[5]將多維分配數(shù)據(jù)關(guān)聯(lián)算法應(yīng)用于SLAM中單幀觀測(cè)的數(shù)據(jù)關(guān)聯(lián)，Wijesoma等[6]又進(jìn)一步將該方法應(yīng)用于多幀觀測(cè)的數(shù)據(jù)關(guān)聯(lián)。Hahnel等[7]提出了一種惰性數(shù)據(jù)關(guān)聯(lián)方法，該方法通過回溯修正過去錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)，但需要計(jì)算維數(shù)較大矩陣的逆，很難實(shí)時(shí)實(shí)現(xiàn)。黃慶成等[8]應(yīng)用基于KD樹的最近鄰算法實(shí)現(xiàn)了局部地圖間的特征點(diǎn)數(shù)據(jù)關(guān)聯(lián)；王婷婷[9]研究了模擬退火算法求解SLAM中的數(shù)據(jù)關(guān)聯(lián)方法；Ji等[10]提出了一種關(guān)聯(lián)樹模型，并對(duì)關(guān)聯(lián)樹進(jìn)行有限深度回溯搜索實(shí)現(xiàn)數(shù)據(jù)關(guān)聯(lián)，該方法適用于基于最小二乘的完全SLAM。在目標(biāo)跟蹤方面，Reid[11]針對(duì)多目標(biāo)跟蹤問題提出了一種多假設(shè)跟蹤的數(shù)據(jù)關(guān)聯(lián)方法，該方法對(duì)所有滿足約束條件后可能的關(guān)聯(lián)進(jìn)行假設(shè)，并對(duì)假設(shè)進(jìn)行跟蹤，在一定時(shí)間以后才真正確定最優(yōu)或次優(yōu)關(guān)聯(lián)對(duì)集合。在理想條件下，多假設(shè)跟蹤方法被認(rèn)為是處理數(shù)據(jù)關(guān)聯(lián)的最優(yōu)方法[12]，并且由于其獨(dú)有的跟蹤特性，使得在動(dòng)態(tài)環(huán)境下也能保證其有效性。上述數(shù)據(jù)關(guān)聯(lián)方法中，一旦觀測(cè)量和路標(biāo)間的關(guān)聯(lián)假設(shè)被確定，大多數(shù)方法就不能進(jìn)行修改。由于當(dāng)前時(shí)刻的信息量缺乏導(dǎo)致關(guān)聯(lián)假設(shè)錯(cuò)誤，而這個(gè)錯(cuò)誤將影響后面的移動(dòng)機(jī)器人位置估計(jì)，從而導(dǎo)致后續(xù)的數(shù)據(jù)關(guān)聯(lián)錯(cuò)誤。如果能在一段時(shí)間后修正之前發(fā)生錯(cuò)誤觀測(cè)，就可以獲得更好的SLAM結(jié)果。少數(shù)方法能通過回溯進(jìn)行修正，但需要大量的計(jì)算，實(shí)時(shí)性不強(qiáng)。如果能夠維持多個(gè)關(guān)聯(lián)假設(shè)，便可增加獲得最優(yōu)或次優(yōu)關(guān)聯(lián)假設(shè)的概率。在此，本文作者提出一種多假設(shè)的數(shù)據(jù)關(guān)聯(lián)方法。在某時(shí)刻維持多個(gè)數(shù)據(jù)關(guān)聯(lián)假設(shè)，設(shè)定每個(gè)假設(shè)計(jì)算代價(jià)的函數(shù)，選擇當(dāng)前時(shí)刻數(shù)據(jù)關(guān)聯(lián)假設(shè)代價(jià)最小的為當(dāng)前時(shí)刻SLAM系統(tǒng)的數(shù)據(jù)關(guān)聯(lián)結(jié)果。該關(guān)聯(lián)確定后，并沒有丟棄其他的假設(shè)。實(shí)際上，采用該方法要經(jīng)過一段時(shí)間才獲得真正的關(guān)聯(lián)。為了減少計(jì)算量，用基本剪枝技術(shù)濾除錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)假設(shè)。在此，本文作者利用多個(gè)粒子維持多種數(shù)據(jù)關(guān)聯(lián)假設(shè)，即采用粒子濾波器來實(shí)現(xiàn)多假設(shè)數(shù)據(jù)關(guān)聯(lián)算法。

1 SLAM中數(shù)據(jù)關(guān)聯(lián)問題描述

在移動(dòng)機(jī)器人SLAM過程中，數(shù)據(jù)關(guān)聯(lián)發(fā)生在預(yù)測(cè)階段，用于判斷已構(gòu)建的地圖中與當(dāng)前觀測(cè)信息相匹配的特征子集。設(shè)移動(dòng)機(jī)器人構(gòu)建的地圖中有n個(gè)特征{F1，F(xiàn)2，…，F(xiàn)n}，傳感器的測(cè)量值E有m個(gè)即{E1，E2，…，Em}。數(shù)據(jù)關(guān)聯(lián)就是找到地圖特征與測(cè)量值間的關(guān)系，可描述為

其中：ji為地圖中第j個(gè)特征Fj與第i個(gè)測(cè)量值Ei的相關(guān)值，當(dāng)兩者完全不相關(guān)時(shí)，ji=0。

在SLAM過程中，根據(jù)前一時(shí)刻移動(dòng)機(jī)器人的位姿和運(yùn)動(dòng)模型可以預(yù)測(cè)當(dāng)前時(shí)刻移動(dòng)機(jī)器人位姿：

其中：f為狀態(tài)傳遞函數(shù)；vk是預(yù)測(cè)噪聲，包括系統(tǒng)的隨機(jī)噪聲和模型本身的不確定性，一般采用服從高斯分布噪聲。

根據(jù)k時(shí)刻觀測(cè)模型可以預(yù)測(cè)地圖特征的位置：

其中：h為測(cè)量函數(shù)；wk,j為測(cè)量噪聲，包括表示測(cè)量過程中的傳感器的誤差和模型本身的不確定性。第 i個(gè)觀測(cè)值和已有地圖中第j個(gè)特征間的距離可用新息及其協(xié)方差表示：

Ei和 Fj之間是否相關(guān)就要看它們之間的 Mahalanobis距離是否小于某個(gè)閾值，即

若新息υk,ij服從高斯分布，則標(biāo)準(zhǔn)化后的距離應(yīng)滿足2χ分布。

為了很好地維持多種數(shù)據(jù)關(guān)聯(lián)假設(shè)，并能從多種假設(shè)中尋找最優(yōu)的關(guān)聯(lián)假設(shè)，首先需要建立描述多數(shù)據(jù)關(guān)聯(lián)假設(shè)方法。本文不采用式(1)所示的數(shù)據(jù)關(guān)聯(lián)描述方法，而另外定義關(guān)聯(lián)假設(shè)變量為rij，當(dāng)觀測(cè)量與地圖中某個(gè)特征相關(guān)聯(lián)時(shí)即為1，否則為0，則SLAM中的數(shù)據(jù)關(guān)聯(lián)問題就轉(zhuǎn)化成了0?1整數(shù)規(guī)劃問題。在一般情況下設(shè)定關(guān)聯(lián)變量應(yīng)滿足單源約束條件，即 1個(gè)觀測(cè)量最多只源于1個(gè)物理路標(biāo)特征，1個(gè)特征最多只產(chǎn)生1個(gè)觀測(cè)量，因此，數(shù)據(jù)關(guān)聯(lián)變量定義如下：

其中：r0j=1表示特征j在當(dāng)前幀中沒有與之相關(guān)聯(lián)的觀測(cè)量；ri0=1表示觀測(cè)量i與所有特征都不匹配，有可能是新路標(biāo)或虛警(不是真實(shí)物理路標(biāo)的反映，而是傳感器噪聲或鏡面反射等引起的)。

設(shè)k時(shí)刻觀測(cè)量與路標(biāo)間有L種關(guān)聯(lián)假設(shè)集合，其關(guān)聯(lián)假設(shè)的域?yàn)镽k，Rk,l為其中的一組關(guān)聯(lián)假設(shè)集，即

Rk,l可看成由0和1組成的滿足約束條件的二維矩陣；l=1，…，L。數(shù)據(jù)關(guān)聯(lián)問題就變成了尋找關(guān)聯(lián)假設(shè)集合中最優(yōu)的一組假設(shè)集，即可等價(jià)為以下離散優(yōu)化問題：

rk,ij滿足式(7)～(9)。

2 基于粒子濾波的多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法

在基于粒子濾波的移動(dòng)機(jī)器人SLAM方法中，采用粒子來保存機(jī)器人位姿的可能分布和地圖中路標(biāo)的可能分布，通過不斷采樣和更新跟蹤整個(gè)SLAM過程，按粒子權(quán)重獲得最接近真實(shí)情況的機(jī)器人路徑軌跡和地圖。若將移動(dòng)機(jī)器人的位姿和環(huán)境中路標(biāo)位置看作移動(dòng)機(jī)器人SLAM系統(tǒng)狀態(tài)，則該方法的實(shí)質(zhì)就是對(duì)移動(dòng)機(jī)器人SLAM狀態(tài)進(jìn)行多種假設(shè)并跟蹤。本文將利用粒子濾波器來實(shí)現(xiàn)多假設(shè)數(shù)據(jù)關(guān)聯(lián)。

采用多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法，在每次觀測(cè)后將生成當(dāng)前時(shí)刻的多個(gè)數(shù)據(jù)關(guān)聯(lián)假設(shè)，并計(jì)算每種數(shù)據(jù)關(guān)聯(lián)假設(shè)的代價(jià)，以獲得當(dāng)前時(shí)刻SLAM的數(shù)據(jù)關(guān)聯(lián)結(jié)果?；诹Ｗ訛V波的多假設(shè)數(shù)據(jù)關(guān)聯(lián)算法，設(shè)定每個(gè)粒子包含了當(dāng)前的假設(shè)關(guān)聯(lián)集 Rk,l和粒子權(quán)重 w，即s={Rk,l，w}。若有N個(gè)粒子，則可維持并跟蹤N種假設(shè)關(guān)聯(lián)。多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法流程中主要模塊對(duì)應(yīng)于粒子濾波的模塊如圖1所示。

圖1 基于粒子濾波的多假設(shè)數(shù)據(jù)關(guān)聯(lián)算法流程Fig.1 Multiple hypotheses data association algorithm flow based on particle filter

獲得假設(shè)生成是多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法中最關(guān)鍵的步驟，也決定了整個(gè)算法的復(fù)雜度，而數(shù)據(jù)關(guān)聯(lián)假設(shè)的代價(jià)計(jì)算則決定了當(dāng)前時(shí)刻SLAM的地圖和機(jī)器人位姿更新的正確性。

在數(shù)據(jù)關(guān)聯(lián)過程中，每幀觀測(cè)和地圖的關(guān)聯(lián)與之前的數(shù)據(jù)關(guān)聯(lián)并非完全獨(dú)立，有一定關(guān)系。假設(shè)第 k時(shí)刻所有的關(guān)聯(lián)假設(shè)集合Rk是由k?1時(shí)刻的關(guān)聯(lián)假設(shè)集合Rk-1和k時(shí)刻獲取的觀測(cè)數(shù)據(jù)Zk相關(guān)形成：新的關(guān)聯(lián)假設(shè)集合Rk,l由Rk-1,l中每個(gè)先驗(yàn)的假設(shè)與觀測(cè)量zk,i相關(guān)形成，如此循環(huán)，直到所有的先驗(yàn)假設(shè)和觀測(cè)量集處理完成為止，形成最終的假設(shè)集Rk。其中，zk,i有可能是源于已構(gòu)建地圖中的路標(biāo)、新觀測(cè)到的路標(biāo)或者虛警。因此，可設(shè)關(guān)聯(lián)假設(shè)集合 Rk,l為先驗(yàn)假設(shè)Rk?1,l與 φk的聯(lián)合假設(shè)：

其中：φk定義為Zk中所有觀測(cè)量與先驗(yàn)假設(shè)之間的數(shù)據(jù)關(guān)聯(lián)。關(guān)聯(lián)假設(shè)集合Rk,l的概率為：

通常，先驗(yàn)分布p(Rk,l)為常數(shù)，則式(11)的數(shù)據(jù)關(guān)聯(lián)整數(shù)規(guī)劃的極大化問題就變成了極小化問題。代價(jià)函數(shù)越小，則其相對(duì)應(yīng)的關(guān)聯(lián)假設(shè)越接近真實(shí)的數(shù)據(jù)關(guān)聯(lián)。

基于粒子濾波的多假設(shè)數(shù)據(jù)關(guān)聯(lián)算法是一個(gè)包含粒子采樣、權(quán)重計(jì)算和重采樣步驟不斷迭代的過程：首先對(duì)觀測(cè)數(shù)據(jù)進(jìn)行聚類；根據(jù)式(13)對(duì)當(dāng)前粒子集合進(jìn)行采樣，獲得下一時(shí)刻的粒子集合。通過下式計(jì)算粒子的權(quán)重：

粒子中保存的假設(shè)數(shù)據(jù)關(guān)聯(lián)集的代價(jià)函數(shù)值越小，該粒子權(quán)重越高；在粒子重采樣過程中，利用剪枝技術(shù)去掉錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)假設(shè)。

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

實(shí)驗(yàn)采用經(jīng)典的 Neira的仿真實(shí)驗(yàn)數(shù)據(jù)[13]。為簡(jiǎn)化，在實(shí)驗(yàn)過程中將本文提出的基于粒子濾波的多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法簡(jiǎn)稱為PFMH。機(jī)器人通過前向運(yùn)動(dòng)和轉(zhuǎn)向運(yùn)動(dòng)進(jìn)行控制，前向運(yùn)動(dòng)速度為0.312 5 m/s，轉(zhuǎn)速為9 °/s。設(shè)定機(jī)器人的傳感器的感知距離為3.5 m，角度為180°，傳感器觀測(cè)周期為1 s。設(shè)機(jī)器人初始運(yùn)動(dòng)控制誤差的方差 σveh=(σv，σθ)(其中：前向運(yùn)動(dòng)速度控制誤差方差 σv=0.01，轉(zhuǎn)向運(yùn)動(dòng)控制誤差方差σθ=2)。傳感器初始觀測(cè)誤差的方差 σsensor=(σρ，σφ)(其中：觀測(cè)距離誤差方差σρ=0.01，轉(zhuǎn)向運(yùn)動(dòng)控制誤差方差σφ=0.125)。圖2和圖3所示分別為采用基于粒子濾波的多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法的 SLAM(實(shí)驗(yàn)中簡(jiǎn)稱PFMH-SLAM)的定位誤差和數(shù)據(jù)關(guān)聯(lián)結(jié)果，其中SLAM解決方法采用擴(kuò)展卡爾曼濾波方法。

圖2 PFMH-SLAM的定位誤差Fig.2 Localization errors of PFMH-SLAM

圖3 PFMH-SLAM的數(shù)據(jù)關(guān)聯(lián)結(jié)果Fig.3 Data association results of PFMH-SLAM

從圖2和圖3可以看出：PFMH-SLAM在實(shí)驗(yàn)中表現(xiàn)出很高的定位精度和數(shù)據(jù)關(guān)聯(lián)正確率。較大的關(guān)聯(lián)漏檢率主要是由于機(jī)器人角度預(yù)測(cè)出現(xiàn)較大誤差。

圖4所示為3倍初始運(yùn)動(dòng)控制誤差方差時(shí)，基于單匹配最近鄰的SLAM(ICNN-SLAM)、基于分枝限界聯(lián)合匹配的 SLAM(JCBB-SLAM)和本文提出的PFMH-SLAM這3種方法的數(shù)據(jù)關(guān)聯(lián)正確率、錯(cuò)誤率和漏檢率。從圖4可以看出：在運(yùn)動(dòng)控制誤差增大到3倍時(shí)，ICNN數(shù)據(jù)關(guān)聯(lián)正確率低，錯(cuò)誤率很高，JCBB和PFMH則保持著較高的數(shù)據(jù)關(guān)聯(lián)正確率和較低的錯(cuò)誤率。由于漏檢率僅與傳感器觀測(cè)的誤差密切相關(guān)，因此，3種方法的漏檢率基本相同。

圖4 3倍σveh時(shí)3種方法數(shù)據(jù)關(guān)聯(lián)結(jié)果比較Fig.4 Experimental results comparison of three times σveh

在不同的運(yùn)動(dòng)誤差和觀測(cè)誤差下，以初始的運(yùn)動(dòng)控制誤差方差和初始觀測(cè)誤差為基礎(chǔ)分別進(jìn)行倍增調(diào)整，對(duì)上述3種方法進(jìn)行多次實(shí)驗(yàn)。在相同誤差條件下，對(duì)每種方法進(jìn)行20次實(shí)驗(yàn)，每次實(shí)驗(yàn)重新隨機(jī)設(shè)置仿真環(huán)境中的路標(biāo)，其實(shí)驗(yàn)結(jié)果如圖5所示。

從圖5(a)可見：3種方法的平均數(shù)據(jù)關(guān)聯(lián)正確率都會(huì)隨著觀測(cè)誤差的增大而下降。這是由于前2種方法本質(zhì)上都是一種門限約束的關(guān)聯(lián)判斷方法，PFMH在計(jì)算關(guān)聯(lián)代價(jià)時(shí)也使用了這一約束。由于PFMH方法對(duì)過去錯(cuò)誤數(shù)據(jù)關(guān)聯(lián)有一定修正能力，因此，在同樣觀測(cè)誤差條件下會(huì)獲得比前兩者更高的正確率。從圖5(b)可見：當(dāng)機(jī)器人運(yùn)動(dòng)控制誤差大于2倍初始誤差時(shí)，ICNN方法的平均數(shù)據(jù)關(guān)聯(lián)正確率急速下降；當(dāng)大于4倍初始誤差時(shí)，JCBB方法的平均數(shù)據(jù)關(guān)聯(lián)正確率也開始迅速下降，而PFMH方法在運(yùn)動(dòng)控制誤差大于6倍初始誤差時(shí)才開始下降，并保持最高的正確率。機(jī)器人運(yùn)動(dòng)控制誤差方差不斷增加時(shí)，基于這3種方法的SLAM對(duì)機(jī)器人在X方向、Y方向和角度的定位誤差的變化見圖5(c)。從圖5(c)可見：與ICNN方法相比，JCBB方法和PFMH方法平均定位誤差增長(zhǎng)較緩慢，且PFMH方法獲得的平均定位誤差最小。

圖5 3種數(shù)據(jù)關(guān)聯(lián)方法結(jié)果比較Fig.5 Experimental results comparison of three data association methods

4 結(jié)論

(1) 針對(duì)經(jīng)典數(shù)據(jù)關(guān)聯(lián)方法一旦關(guān)聯(lián)假設(shè)確定就不能修改的不足，將數(shù)據(jù)關(guān)聯(lián)問題轉(zhuǎn)換成離散優(yōu)化問題，利用多個(gè)粒子來維持多種數(shù)據(jù)關(guān)聯(lián)假設(shè)。

(2) 基于粒子濾波的多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法實(shí)際上是在一段時(shí)間后才獲得真正最優(yōu)或次優(yōu)的關(guān)聯(lián)結(jié)果，具有正確的數(shù)據(jù)關(guān)聯(lián)結(jié)果和更高的定位精度。

(3) 采用該方法在復(fù)雜環(huán)境下如何減小計(jì)算量同時(shí)維持其準(zhǔn)確度有待進(jìn)一步研究。

[1]王璐, 蔡自興. 未知環(huán)境中移動(dòng)機(jī)器人并發(fā)建圖與定位(CML)的研究進(jìn)展[J]. 機(jī)器人, 2004, 26(4): 380?384.WANG Lu, CAI Zi-xing. Progress of CML for mobile robots in unknown environments[J]. Robot, 2004, 26(4):380?384.

[2]韓崇昭, 朱洪艷, 段戰(zhàn)勝, 等. 多源信息融合[M]. 北京: 清華大學(xué)出版社, 2006: 290?334.HAN Chong-zhao, ZHU Hong-yan, DUAN Zhan-sheng. Multisource information fusion[M]. Beijing: Tsinghua University Press, 2006: 290?334.

[3]Neira J, Tardos J D. Data association in stochastic mapping using the joint compatibility test[J]. IEEE Transactions on Robotics and Automation, 2001, 17(6): 890?897.

[4]Bailey T, Nebot E M, Rosenblatt J K, et al. Data association for mobile robot navigation: A graph theoretic approach[C]//Proceedings of the IEEE International Conference on Robotics and Automation. San Francisco, USA, 2000: 2512?2517.

[5]ZHANG Sen, XIE Li-hua, Martin A. An efficient data association approach to simultaneous localization and map building[C]// Proceedings of the IEEE International Conference on Robotics and Automation. New Orleans, USA, 2004:854?859.

[6]Wijesoma W S, Perera L D L, Adams M D. Toward multidimensional assignment data association in robot localization and mapping[J]. IEEE Transactions on Robotics and Automation, 2006, 22(2): 350?365.

[7]Hahnel D, Thrun S, Wegbreit B, et al. Towards lazy data association in SLAM[C]//Proceedings of the International Symposium on Robotics Research. Sienna, Italy, 2003: 421?431

[8]黃慶成, 洪炳熔, 厲茂海, 等. 基于主動(dòng)環(huán)形閉合約束的移動(dòng)機(jī)器人分層同時(shí)定位和地圖創(chuàng)建[J]. 計(jì)算機(jī)研究與發(fā)展,2007, 44(4): 636?642.HUANG Qing-cheng, HONG Bing-rong, LI Mao-hai. Mobile robot hierarchical simultaneous localization and mapping based on active loop closure constraint[J]. Journal of Computer Research and Development, 2007, 44(4): 636?642.

[9]王婷婷. 移動(dòng)機(jī)器人SLAM中的數(shù)據(jù)關(guān)聯(lián)算法研究[D]. 天津:南開大學(xué)軟件學(xué)院, 2007: 10?50.WANG Ting-ting. Research on data association algorithm in mobile robot SLAM[D]. Tianjin: Nankai University. College of Software, 2007: 10?50.

[10]JI Xiu-cai, ZHANG Hui, HAI Dan, et al. Incremental simultaneous localization and mapping with backtracking data association for mobile robots[C]//Proceedings of the IEEE International Conference on Information and Automation.Zhangjiajie, China, 2008: 634?639.

[11]Reid D. An algorithm for tracking multiple targets[J]. IEEE Transactions on Automatic Control, 1979, 24(6):843?854.

[12]Stone L D, Corwin T L, Barlow C A. Bayesian multiple target tracking[M]. Boston: Artech Print on Demand, 1999: 123?140.

[13]Neira J. Continuous SLAM [EB/OL]. http://webdiis.unizar.es/～neira/5007439/dalab.zip. 2008?12?10.