移動(dòng)機(jī)器人FASTSLAM算法研究

《移動(dòng)機(jī)器人FASTSLAM算法研究》由會(huì)員分享，可在線閱讀，更多相關(guān)《移動(dòng)機(jī)器人FASTSLAM算法研究（37頁(yè)珍藏版）》請(qǐng)?jiān)谘b配圖網(wǎng)上搜索。

1、 本科畢業(yè)設(shè)計(jì) 題目： 移動(dòng)機(jī)器人FastSLAM算法研究 學(xué) 院: 專 業(yè): 學(xué) 號(hào): 學(xué)生姓名: 指導(dǎo)教師: 日 期: 武漢科技大學(xué)本科畢業(yè)設(shè)計(jì) 摘 要 移動(dòng)機(jī)器人同時(shí)定位與地圖創(chuàng)建是實(shí)現(xiàn)未知環(huán)境下機(jī)器人自主導(dǎo)航的關(guān)鍵性技術(shù)，具有廣泛的應(yīng)用前景，也是目前機(jī)器人研究的熱門課題之一?；诳柭鼮V波器的SLAM算法有計(jì)算的復(fù)雜性以及對(duì)數(shù)據(jù)融合誤差非常敏感的缺點(diǎn)，使其不能再實(shí)際環(huán)境中得到廣泛應(yīng)用。 為解決傳統(tǒng)SLAM算法的缺陷，介紹了一種基于Rao-Black

2、wellized粒子濾波器的FastSLAM方法，該方法將SLAM問題分解為對(duì)機(jī)器人姿態(tài)和路標(biāo)在地圖中的位置的遞歸算法。每一粒粒子都有對(duì)應(yīng)的地圖，再將地圖估計(jì)分解成N個(gè)獨(dú)立的特征估計(jì)，路徑估計(jì)采用粒子濾波器，地圖估計(jì)采用擴(kuò)展卡爾曼濾波器。 FastSLAM有機(jī)地將粒子濾波器與卡爾曼濾波器集成在一起，魯棒性地解決數(shù)據(jù)關(guān)聯(lián)和多目標(biāo)跟蹤問題，其時(shí)間消耗與路標(biāo)的數(shù)量成對(duì)數(shù)關(guān)系，計(jì)算量小，用時(shí)短。 基于Rao-Blackwellized粒子濾波器的FastSLAM算法是一種高效的機(jī)器人同步定位和繪制地圖的算法，其具有高效性和準(zhǔn)確性，該方法使用提高了機(jī)器人地圖創(chuàng)建的實(shí)時(shí)性，增強(qiáng)了避障能力。 關(guān)鍵

3、詞： 移動(dòng)機(jī)器人； FastSLAM算法； 路徑估計(jì)； 地圖估計(jì) Abstract Mobile robot localization and mapping the key technologies of the robot autonomous navigation in unknown environment, has broad application prospects, is currently a hot research topic of the robo

4、t. The Kalman filter-based SLAM algorithm to calculate the complexity and the error is very sensitive to the shortcomings of the data fusion, so that it can not be widely applied in the actual environment. FastSLAM based on Rao-Blackwellized particle filter SLAM problem is decomposed into the posit

5、ion of the robot pose and landmarks on the map recursive algorithm to solve the defects of the traditional SLAM algorithm. A particle has a corresponding map, and then map the estimated decomposed into N independent characteristics estimated path estimation using particle filter, map estimated using

6、 extended Kalman filter. The FastSLAM organic particle filter and Kalman filter integrated, robust solution to data association and multi-target tracking, the time consumption and the number of landmarks logarithmic small amount of calculation, with short. FastSLAM algorithms based on Rao-Blackwel

7、lized particle filter is an efficient robot simultaneous localization and mapping algorithm, its efficiency and accuracy, the method to improve the robot map created real-time, and enhance the ability to obstacle avoidance. Key words: Mobile robot; The FastSLAM algorithm; The path estimation;

8、 The map estimation 目 錄 1 緒論 1 1.1 移動(dòng)機(jī)器人定位和地圖創(chuàng)建問題 2 1.1.1 移動(dòng)機(jī)器人國(guó)內(nèi)外發(fā)展?fàn)顩r 2 1.1.2 移動(dòng)機(jī)器人的地圖構(gòu)建問題 3 1.1.3 機(jī)器人的定位方法 5 2 基于粒子濾波器的SLAM算法 7 2.1 SLAM的通用框架和理論模型 7 2.2 粒子濾波器定位的基本原理 8 2.3 擴(kuò)展卡爾曼濾波器算法 9 2.3 粒子重采樣 10 2.4 移動(dòng)機(jī)器人SLAM問題描述 10 2.4.1 SLAM計(jì)算復(fù)雜度 10 2.4.2 SLAM的聯(lián)合估計(jì) 11

9、 2.4.3 SLAM的后驗(yàn)估計(jì)表示 11 2.4.4 SLAM公式推導(dǎo) 13 2.4.5 有效的數(shù)據(jù)關(guān)聯(lián) 14 2.4.6 FastSLAM的粒子表示形式 15 2.4.7 FastSLAM的計(jì)算時(shí)間復(fù)雜度 16 3 模型建立 17 3.1 運(yùn)動(dòng)模型 17 3.2 觀測(cè)模型 18 4 FastSLAM算法步驟 19 4.1 FastSLAM算法步驟 19 4.2 新位姿采樣 20 4.3 環(huán)境特征估計(jì)的更新 21 5　算法流程圖和代碼 22 5.1 FastSLAM算法偽代碼 22 5.2 FastSLAM算法流程 22 6 仿真環(huán)

10、境建立和仿真結(jié)果 24 6.1 仿真環(huán)境介紹 24 6.2 仿真結(jié)果 24 7 總結(jié)和展望 29 參考文獻(xiàn) 30 致謝 31 33 1 緒論 移動(dòng)機(jī)器人的同時(shí)定位與地圖創(chuàng)建(SLAM)問題有重要的理論與應(yīng)用價(jià)值，被很多學(xué)者認(rèn)為是實(shí)現(xiàn)真正全自主移動(dòng)機(jī)器人的關(guān)鍵。在火災(zāi)、地震或者極地考察現(xiàn)場(chǎng)存在一些潛在危險(xiǎn)，可能會(huì)對(duì)進(jìn)入人員生命構(gòu)成危險(xiǎn)，就需要移動(dòng)機(jī)器人來代替人進(jìn)入這些環(huán)境，進(jìn)行探測(cè)地形和檢查是否有傷員的工作。民用方面可以用于自主行駛車輛或者車輛輔助駕駛系統(tǒng)，還有服務(wù)機(jī)器人，智能機(jī)器人，智能玩具等。軍用方面可以用于對(duì)敵方控制區(qū)偵查和對(duì)危險(xiǎn)易爆炸物的搜索和

11、拆除?？茖W(xué)方面可以用于外星探索和采礦等。SLAM問題研究是實(shí)現(xiàn)機(jī)器人完全自主以及服務(wù)人類的關(guān)鍵,因此,對(duì)移動(dòng)機(jī)器人SLAM的研究具有重要的理論意義和應(yīng)用價(jià)值,由于自主移動(dòng)機(jī)器人具有廣泛的應(yīng)用、巨大的經(jīng)濟(jì)利益和深遠(yuǎn)的科學(xué)價(jià)值，各國(guó)家和科研工作者對(duì)其相關(guān)技術(shù)發(fā)展都投入了大量的研究。在20世紀(jì)70年代末期，法國(guó)的系統(tǒng)分析與架構(gòu)實(shí)驗(yàn)室（LAAS）研制了HILARE機(jī)器人，這是歐洲的第一個(gè)移動(dòng)機(jī)器人研究項(xiàng)目，HILARE安裝了視覺傳感器、激光測(cè)距儀和超聲波傳感器等設(shè)備，用二維多邊形環(huán)境模型和全局坐標(biāo)系統(tǒng)實(shí)現(xiàn)導(dǎo)航和路徑規(guī)劃功能。多年來國(guó)內(nèi)外有大量的科技工作致力于這方面的研究工作，對(duì)許多問題的認(rèn)識(shí)和求解都取

12、得了長(zhǎng)足的進(jìn)步，但是仍然有很多問題需要解決。由于移動(dòng)機(jī)器人系統(tǒng)運(yùn)動(dòng)模型難以精確建模，再加上其他傳感器感知誤差存在高度不確定性和外界干擾因素等，從而造成了機(jī)器人系統(tǒng)的高度復(fù)雜性和不確定性。由于在很多情況下，環(huán)境的信息是部分、甚至是完全未知的，此時(shí)必須將定位和地圖構(gòu)建結(jié)合起來，使得移動(dòng)機(jī)器人利用對(duì)自身的定位來構(gòu)建地圖，同時(shí)利用構(gòu)建地圖來定位，在這一問題中，移動(dòng)機(jī)器人定位與地圖構(gòu)建是密切相關(guān)的，地圖創(chuàng)建過程依賴于精確的機(jī)器人位姿信息，任何一方都無法單獨(dú)實(shí)現(xiàn)，必須同時(shí)加以考慮，近年來，SLAM算法越來越受到人們的重視，針對(duì)移動(dòng)機(jī)器人的SLAM問題，主要有基于粒子濾波器的SLAM方法和基于擴(kuò)展卡爾曼濾波

13、以及EKF的各種變種SLAM方法。SLAM(Simultaneous Localization And Mapping）問題是指把移動(dòng)機(jī)器人放在未知環(huán)境中，機(jī)器人增量式地創(chuàng)建未知環(huán)境的連續(xù)地圖，同時(shí)確定它在地圖中的位置，由于SLAM問題的解法使機(jī)器人實(shí)現(xiàn)了真正的自主導(dǎo)航，所以在過去幾十年中逐漸成為了移動(dòng)機(jī)器人領(lǐng)域的研究熱點(diǎn)。SLAM一方面為了實(shí)現(xiàn)精確定位，機(jī)器人需要利用位置相對(duì)確定的環(huán)境特征不斷校正自身位置，一方面為了確定環(huán)境特征的位置，機(jī)器人需要了解自身所在的精確位姿。機(jī)器人的定位與地圖構(gòu)建有利于進(jìn)一步協(xié)助運(yùn)動(dòng)規(guī)劃和躲避障礙物，體現(xiàn)機(jī)器人對(duì)環(huán)境探測(cè)效率的同時(shí)又體現(xiàn)出對(duì)環(huán)境地圖信息描述的精準(zhǔn)程

14、度。SLAM也稱CML(Concurrent Mapping and Localization),最初于上世界80年代早期由Smith Self和Cheeseman給出了基礎(chǔ)概率方法的解決方案[ ]， SLAM 問題的解決方法大致分為兩類:基于概率估計(jì)的方法和非概率估計(jì)的方法?；诟怕誓Ｐ偷姆椒ㄕ贾鲗?dǎo)地位[ ]。 1.1 移動(dòng)機(jī)器人定位和地圖創(chuàng)建問題 1.1.1 移動(dòng)機(jī)器人國(guó)內(nèi)外發(fā)展?fàn)顩r 1950年和1951年，William Grey Walter研制了兩個(gè)可完成自動(dòng)避障等智能行為的移動(dòng)機(jī)器人。在20世紀(jì)50年代初期，Marvin Minsky等人研制了具有跟蹤功能的移動(dòng)機(jī)器人

15、。1956年，在由美國(guó)國(guó)防部高級(jí)研究計(jì)劃局（DARPA）主辦的Dartmouth會(huì)議上，正式提出了“人工智能”概念，認(rèn)為移動(dòng)機(jī)器人應(yīng)能夠執(zhí)行和學(xué)習(xí)有意義的任務(wù)，一定程度上能夠模仿動(dòng)物或人的能力。1969年，Nilsson等人在斯坦福機(jī)器人研究所（SRI）研制了第一個(gè)智能移動(dòng)機(jī)器人Shakey，它裝備了攝像機(jī)、測(cè)距儀和接觸傳感器等設(shè)備，并通過無線連接與DEC．PDP 10計(jì)算機(jī)相連，其任務(wù)是自動(dòng)避障和運(yùn)動(dòng)目標(biāo)跟蹤。在20世紀(jì)70年代末期，Hans Moravec在SRI研制了CART移動(dòng)機(jī)器人，它能夠利用攝像機(jī)進(jìn)行避障，并能利用連續(xù)的多幅圖像構(gòu)建二維環(huán)境模型，并可以實(shí)現(xiàn)路徑規(guī)劃。在20世紀(jì)70年

16、代末期，法國(guó)的系統(tǒng)分析與架構(gòu)實(shí)驗(yàn)室（LAAS）研制了HILARE機(jī)器人，這是歐洲的第一個(gè)移動(dòng)機(jī)器人研究項(xiàng)目，HILARE安裝了視覺傳感器、激光測(cè)距儀和超聲波傳感器等設(shè)備，用二維多邊形環(huán)境模型和全局坐標(biāo)系統(tǒng)實(shí)現(xiàn)導(dǎo)航和路徑規(guī)劃功能。 目前，移動(dòng)機(jī)器人的研究范圍已經(jīng)擴(kuò)展到水下、空中、地面、甚至太空和行星表面等多種環(huán)境，如美國(guó)馬里蘭系統(tǒng)工程實(shí)驗(yàn)室（Marine Systems Engineering Laboratory）研制的EAVE III型自主式水下移動(dòng)機(jī)器人，美國(guó)海軍研制的新型戰(zhàn)場(chǎng)準(zhǔn)備自主水下航行器（BPAUV）、美國(guó)無人車輛協(xié)會(huì)（the Joint of Unmanned Vehicle）

17、組織開展的自主式航空飛行器的研究，世界著名移動(dòng)機(jī)器人制造商IROBOT公司的金字塔探索者Pyramid Rover、月球車，以及卡耐基-梅隆大學(xué)（CMU）研制的CMU-Rover，日本Tsukuba大學(xué)的Yamabiko和MELDOG，法國(guó)系統(tǒng)分析與結(jié)構(gòu)實(shí)驗(yàn)室（LAAS）的Hilare2，德國(guó)Karlsruhe大學(xué)的KAMRO等。美國(guó)航空航天局（NASA）研制的第一代火星探測(cè)機(jī)器人Sojourner于1997年成功登陸火星，開啟了人類利用移動(dòng)機(jī)器人技術(shù)進(jìn)行空間探測(cè)和開發(fā)的序幕。為了在火星上進(jìn)行長(zhǎng)距離探險(xiǎn)，NASA又開始了新一代樣機(jī)的研制，命名為Rocky7（如圖1.2所示），并在Lavic湖的

18、巖溶流上和干枯的湖床上進(jìn)行了成功的實(shí)驗(yàn)。最值得一提的是，2004年1月美國(guó)NASA火星探測(cè)移動(dòng)機(jī)器人Spirit（如圖1.3所示）和Opportunity先后在火星表面順利登陸。Spirit和Opportunity的成功著陸標(biāo)志著人類發(fā)射的星際探測(cè)器自動(dòng)化程度提升到了前所未有的水平，同時(shí)也標(biāo)志著智能移動(dòng)機(jī)器人的研究工作也已經(jīng)提高到了一個(gè)前所未有的高度，正在為人類的進(jìn)步發(fā)展貢獻(xiàn)力量。我國(guó)也針對(duì)這一趨勢(shì)制定了以月球?yàn)榻谀繕?biāo)的空間探測(cè)計(jì)劃。研究和發(fā)展我國(guó)的月球探測(cè)移動(dòng)機(jī)器人技術(shù)，不但對(duì)于我國(guó)在激烈的空間技術(shù)和資源競(jìng)爭(zhēng)中取得有力地位具有關(guān)鍵意義，同時(shí)也對(duì)包括移動(dòng)機(jī)器人導(dǎo)航控制在內(nèi)的相關(guān)技術(shù)的發(fā)展有巨

19、大的促進(jìn)作用。 我國(guó)對(duì)移動(dòng)機(jī)器人的研究雖然起步較晚，但是越來越受到重視，國(guó)家863智能機(jī)器人組把智能移動(dòng)機(jī)器人的研究作為今后發(fā)展的重點(diǎn)。以中科院自動(dòng)化研究所、清華大學(xué)、國(guó)防科技大學(xué)、東北大學(xué)、上海交通大學(xué)、哈爾濱工業(yè)大學(xué)為代表的我國(guó)機(jī)器人研究基地，已經(jīng)取得了令人矚目的成果。 清華大學(xué)研制的THMR系列移動(dòng)機(jī)器人系統(tǒng)；自動(dòng)化所研制的集多種傳感器、視覺、語音識(shí)別與會(huì)話功能于一體的智能移動(dòng)機(jī)器人CASIA-I[ ]；此外，沈陽(yáng)自動(dòng)化研究所機(jī)器人實(shí)驗(yàn)室研制的激光導(dǎo)引AGV、防爆機(jī)器人、“海極號(hào)”和“探索者號(hào)”水下機(jī)器人，香港城市大學(xué)智能設(shè)計(jì)、自動(dòng)化以及制造研究中心的自動(dòng)導(dǎo)航車和服務(wù)機(jī)器人，哈爾濱

20、工業(yè)大學(xué)機(jī)器人研究所研制的能在未知、復(fù)雜、多變、惡劣的環(huán)境下運(yùn)行的全方位輪式移動(dòng)機(jī)器人平臺(tái)等都說明了我國(guó)移動(dòng)機(jī)器人技術(shù)取得的突出成就。但從總體水平上看，我國(guó)移動(dòng)機(jī)器人技術(shù)比起發(fā)達(dá)國(guó)家還有一定的差距。 1.1.2 移動(dòng)機(jī)器人的地圖構(gòu)建問題 移動(dòng)機(jī)器人通過自身所帶的傳感器實(shí)現(xiàn)對(duì)外部環(huán)境的感知。由傳感器獲得的數(shù)據(jù)經(jīng)處理后要以某種抽象的形式來對(duì)環(huán)境空間進(jìn)行表達(dá)，并作為地圖信息存儲(chǔ)在移動(dòng)機(jī)器人中。移動(dòng)機(jī)器人利用對(duì)環(huán)境的感知信息實(shí)現(xiàn)環(huán)境的建模，自動(dòng)的構(gòu)建一個(gè)地圖，利用地圖給出的信息在外部環(huán)境中運(yùn)動(dòng)，并持續(xù)感知周圍環(huán)境，進(jìn)一步完善和更新地圖。在移動(dòng)機(jī)器人導(dǎo)航并自主定位過程中，按照一定的空間表達(dá)規(guī)則將傳

21、感器信息轉(zhuǎn)換成地圖的過程是移動(dòng)機(jī)器人導(dǎo)航并自主定位的基礎(chǔ)，而空間表達(dá)的方法又決定了地圖的表達(dá)形式。因此，采用何種空間表達(dá)方法對(duì)移動(dòng)機(jī)器人自主定位和地圖的持續(xù)更新具有很大影響。目前地圖創(chuàng)建方法很多大致可以歸為四類:柵格表示法、幾何信息表示法、拓?fù)鋱D表示法和混合表示法。 1. 基于柵格的地圖表示法 柵格地圖表示法最先是由Moravec和Elfes于1985年提出的。它將機(jī)器人工作環(huán)境劃分為一系列柵格，其中每個(gè)柵格都被分配一個(gè)概率值。此概率值表示該柵格被障礙物占據(jù)的可能性大小。柵格地圖的優(yōu)點(diǎn)在于易于重建和維護(hù)，對(duì)任何一個(gè)柵格的感知信息可以直接與環(huán)境中的某個(gè)區(qū)域相對(duì)應(yīng)，特別適合于處理超聲測(cè)

22、量數(shù)據(jù)。但是環(huán)境空間的分辨率和柵格大小相關(guān)，當(dāng)需要增加分辨率時(shí)就必須縮小柵格的大小，進(jìn)而增加了運(yùn)算的時(shí)間和空間復(fù)雜度。另外，在傳統(tǒng)的柵格地圖表示法中，每個(gè)柵格之間被認(rèn)為是相互獨(dú)立的，因此，利用柵格表示的地圖和實(shí)際環(huán)境地圖具有很大的不一致性[9]。為改進(jìn)傳統(tǒng)柵格地圖表示法的這種缺點(diǎn)，Thrun提出了一種采用期望最大化方法進(jìn)行環(huán)境地圖表示的方法，同時(shí)還考慮了各個(gè)相鄰柵格之間的依賴關(guān)系。在Bert等人的研究中，對(duì)環(huán)境采用全局柵格和局部柵格結(jié)合的方法進(jìn)行建模，同時(shí)利用霍夫變換對(duì)柵格地圖中的線段進(jìn)行抽象描述，并采用卡爾曼濾波器進(jìn)行位置估計(jì)。Stepan等人提出了一種基于單目攝像頭和激光測(cè)距儀的柵格地圖構(gòu)

23、造方法，對(duì)柵格地圖的質(zhì)量利用機(jī)器人的歷史路徑信息進(jìn)行優(yōu)劣評(píng)估。Noykov等人提出了一種利用聲納傳感器數(shù)據(jù)進(jìn)行柵格地圖構(gòu)造的方法，利用統(tǒng)計(jì)方法對(duì)聲納數(shù)據(jù)概率模型進(jìn)行建模并結(jié)合模糊理論進(jìn)行聲納數(shù)據(jù)融合。Grisetti等人采用Rao-Blackwellized粒子濾波器對(duì)柵格地圖進(jìn)行估計(jì)，采取自適應(yīng)方法獲取粒子數(shù)，同時(shí)，在粒子采樣函數(shù)的選取時(shí)考慮了最近多次傳感器的測(cè)量數(shù)據(jù)。 2. 基于特征的地圖表示法 基于特征的地圖表示法是指利用可以識(shí)別的環(huán)境路標(biāo)進(jìn)行環(huán)境地圖的表示，其中每個(gè)路標(biāo)特征都用幾何原型進(jìn)行表示。此種地圖表示方法局限于可以進(jìn)行參數(shù)化的路標(biāo)環(huán)境，或者是可建模的對(duì)象環(huán)境，比如點(diǎn)、線、面等

24、。對(duì)特征地圖的重建大多都是基于外部傳感器對(duì)環(huán)境的檢測(cè)數(shù)據(jù)，然后利用這些檢測(cè)數(shù)據(jù)進(jìn)行環(huán)境路標(biāo)特征的提取。對(duì)結(jié)構(gòu)化的室內(nèi)環(huán)境，可以利用一些簡(jiǎn)單的集合模型進(jìn)行環(huán)境空間的表示，對(duì)于室外環(huán)境，可以采用曲面進(jìn)行環(huán)境空間的表示。Drumhellor利用線段進(jìn)行地圖構(gòu)造，并利用激光測(cè)距儀的測(cè)量數(shù)據(jù)進(jìn)行機(jī)器人自身定位。Arras等人用激光測(cè)距儀數(shù)據(jù)提取水平直線特征，同時(shí)利用視覺系統(tǒng)進(jìn)行垂直直線特征的提取，最后綜合這些直線進(jìn)行環(huán)境地圖的構(gòu)造。Roumeliotis等人針對(duì)激光測(cè)距儀對(duì)環(huán)境的檢測(cè)數(shù)據(jù)提出了一種分層的路標(biāo)檢測(cè)方法，在底層采用兩個(gè)卡爾曼濾波器分別進(jìn)行環(huán)境拐角和線段等基本特征的檢查，在上層再對(duì)底層檢測(cè)到的

25、特征進(jìn)行較為復(fù)雜的路標(biāo)特征合成，比如墻，門，地面等。Luis Pedraza等人提出利用B-樣條的方法進(jìn)行非結(jié)構(gòu)化環(huán)境中的路標(biāo)描述，然后運(yùn)用擴(kuò)展卡爾曼濾波器進(jìn)行機(jī)器人定位。Luis Pedraza等人另外還提出采用B-樣條曲線進(jìn)行環(huán)境邊界表示的方法，即BS-SLAM算法，BS-SLAM算法的意義在于它是第一個(gè)運(yùn)用參數(shù)化方法進(jìn)行環(huán)境曲線特征描述的算法。 3. 基于拓?fù)涞牡貓D表示法 利用拓?fù)涞貓D進(jìn)行機(jī)器人運(yùn)行環(huán)境描述最先是由Brooks提出的。拓?fù)涞貓D表示法中的地圖是根據(jù)實(shí)際環(huán)境結(jié)構(gòu)，由位置節(jié)點(diǎn)和節(jié)點(diǎn)之間的連線組成。其中節(jié)點(diǎn)表示環(huán)境中的特定地點(diǎn)，節(jié)點(diǎn)之間的連線表示的是不同地點(diǎn)之間的路徑信息。拓

26、撲地圖可以組織為層次結(jié)構(gòu)，例如在底層，一個(gè)位置可能就是一個(gè)房間，但是在更上一層則可能為一個(gè)建筑物。為了利用拓?fù)涞貓D方法進(jìn)行環(huán)境地圖的表示，必須有效的識(shí)別環(huán)境中各個(gè)特定的地點(diǎn)。對(duì)結(jié)構(gòu)化的環(huán)境而言，各個(gè)特定的地點(diǎn)可以容易進(jìn)行識(shí)別，但是在非結(jié)構(gòu)化的環(huán)境中，節(jié)點(diǎn)的識(shí)別會(huì)非常復(fù)雜。利用拓?fù)涞貓D進(jìn)行環(huán)境表示的關(guān)鍵在于拓?fù)涞貓D中各個(gè)頂點(diǎn)的選取，David等人提出了一種基于聲納數(shù)據(jù)和視覺信息相結(jié)合的拓?fù)涞貓D頂點(diǎn)選擇方法，相對(duì)于單獨(dú)采用聲納數(shù)據(jù)進(jìn)行頂點(diǎn)檢測(cè)，此方法可以得到更加精確的拓?fù)涞貓D描述。Choset等人提出利用通用Voronoi圖（Generalized Voronoi graph，GVG）進(jìn)行環(huán)境地圖

27、表示的方法，在GVG地圖表示中添加了機(jī)器人傳感器的一些度量信息，通過對(duì)各個(gè)GVG圖的匹配進(jìn)行機(jī)器人自身定位。Ranganathan等人提出利用GVG圖和視覺傳感器檢測(cè)到的路標(biāo)進(jìn)行地圖表示的方法。另外，為更好地進(jìn)行環(huán)境地圖模型的描述，研究者還提出了拓?fù)涞貓D和尺度地圖混合進(jìn)行環(huán)境表示的方法，通過在拓?fù)涞貓D中加入尺度信息來補(bǔ)償拓?fù)涞貓D中的信息丟失。GVG地圖表示法已經(jīng)被用于美國(guó)國(guó)家航空和宇宙航天局空間站中的機(jī)器人三維定位問題研究中，這樣的地圖表示方法具有拓?fù)涞貓D的高效性和尺度地圖的一致性以及精確性等特點(diǎn)。 4. 基于視圖的地圖表示法 在基于視圖的地圖表示方法中，對(duì)環(huán)境并不是利用拓?fù)湫问交蛘呗窐?biāo)特

28、征進(jìn)行表示，而是采用機(jī)器人的一系列歷史路徑估計(jì)信息進(jìn)行表示?；谝晥D的地圖表示方法首先是由Lu和Milios提出的，在他們的研究中，環(huán)境地圖是由多次激光掃描數(shù)據(jù)和各個(gè)掃描數(shù)據(jù)之間的關(guān)系組成。Fleischer等人提出了基于視覺的視圖地圖表示方法，Malauchlan提出了一種基于批處理和迭代處理的三維環(huán)境視圖表示方法。Eustice等人針對(duì)機(jī)器人在海底環(huán)境運(yùn)行的特殊性，提出了一種新的地圖表示方法VAN（Visually Augmented Navigation），在VAN的機(jī)器人定位與地圖重建算法中，SLAM的狀態(tài)向量由機(jī)器人在不同時(shí)刻的位姿組成。Eustice等人還對(duì)基于視圖的地圖表示方法中

29、的信息矩陣，稀疏擴(kuò)展信息濾波器（Sparse Extended Information Filter，SEIF）以及稀疏聯(lián)合樹濾波器（Thin Junction Tree，TJT）中的信息矩陣進(jìn)行了比較，從比較結(jié)果可以看出，基于視圖的地圖表示方法中的信息矩陣相對(duì)于SEIF和TJT中的信息矩陣有兩個(gè)優(yōu)點(diǎn)，一是信息矩陣的稀疏性要更好，二是它對(duì)應(yīng)信息矩陣的稀疏性是自然的，并不需要像SEIF那樣經(jīng)過強(qiáng)制的信息丟失處理?；谝晥D的地圖表示法適合于那些并不容易進(jìn)行特征抽象的復(fù)雜地圖環(huán)境。 1.1.3 機(jī)器人的定位方法 在自主移動(dòng)機(jī)器人中，無論是局部實(shí)時(shí)避障還是全局規(guī)劃，都要精確地獲得機(jī)器人及障礙物的

30、當(dāng)前狀態(tài)，因此對(duì)于移動(dòng)機(jī)器人來說，精確地位置估計(jì)是實(shí)現(xiàn)自主導(dǎo)航的必要內(nèi)容，對(duì)移動(dòng)機(jī)器人定位的研究具有非常重要的意義。目前，移動(dòng)機(jī)器人多采用各種機(jī)載傳感器進(jìn)行定位。 1． 里程計(jì)定位 在移動(dòng)機(jī)器人的兩個(gè)驅(qū)動(dòng)電機(jī)組上均裝有里程計(jì)，它們是使用最為廣泛的導(dǎo)航工具之一，能在短距離內(nèi)提供精確地定位，但它的誤差是隨著時(shí)間增加而累計(jì)增長(zhǎng)的，尤其是方向誤差。當(dāng)用來長(zhǎng)距離和長(zhǎng)時(shí)間定位時(shí)，由于誤差的不斷累加，會(huì)導(dǎo)致移動(dòng)機(jī)器人在行駛過程中發(fā)生很大的偏離，從而影響定位效果[9]。盡管有這個(gè)缺點(diǎn)，由于其在短距離內(nèi)依然能夠獲得較好的效果。因此絕大多數(shù)的研究者還是認(rèn)為里程計(jì)是機(jī)器人導(dǎo)航系統(tǒng)中的一個(gè)重要組成部分。 2．

31、路標(biāo)定位 基于人工路標(biāo)、自然路標(biāo)或者“燈塔”，一般來說，路標(biāo)都有固定的已知位 置，機(jī)器人自身的傳感器能對(duì)路標(biāo)進(jìn)行識(shí)別，參照路標(biāo)進(jìn)行相對(duì)定位。為了使問題簡(jiǎn)化，經(jīng)常假設(shè)機(jī)器人的當(dāng)前位置和方向角已經(jīng)劃定，機(jī)器人只需要在有限的區(qū)域內(nèi)尋找路標(biāo)。缺點(diǎn)是需要人工干預(yù)，在現(xiàn)場(chǎng)設(shè)置路標(biāo)，這是人為對(duì)環(huán)境的再結(jié)構(gòu)化，移動(dòng)機(jī)器人的自主性不高。路標(biāo)對(duì)信息的反饋存在一定的誤差，這也影響了機(jī)器人對(duì)于自身位置的確定。 3． 視覺定位 視覺定位技術(shù)主要是利用攝像機(jī)獲取工作空間的圖像，通過圖像處理、模式 識(shí)別等手段對(duì)獲取的圖片進(jìn)行分析，提取有用的環(huán)境信息，為機(jī)器人的定位、路徑規(guī)劃、避障以及創(chuàng)建地圖提供數(shù)據(jù)。其優(yōu)點(diǎn)是具有

32、很高的空間分辨率。探測(cè)范圍廣、精度高、能夠獲得場(chǎng)景的大部分信息。缺點(diǎn)是攝像頭在使用過程中容易受到光線和環(huán)境的影響。且獲得的圖像提取和識(shí)別選用困難，耗費(fèi)的計(jì)算了很大，實(shí)時(shí)性也不易滿足。 4． GPS定位 GPS全球定位技術(shù)是一種應(yīng)用范圍較廣的絕對(duì)定位技術(shù)。它利用環(huán)繞在地球上空的24顆衛(wèi)星準(zhǔn)確定位出使用者的準(zhǔn)確位置。若是已知地面接收器與三個(gè)衛(wèi)星之間的距離，從理論上來講，就可以計(jì)算出接收器的經(jīng)緯度和高度，單純利用GPS定位精度還比較低，所以在移動(dòng)機(jī)器人定位和導(dǎo)航中一般還同時(shí)融合里程計(jì)和其他傳感器的數(shù)據(jù)，其缺點(diǎn)是不適合室內(nèi)環(huán)境、水下環(huán)境以及對(duì)精度要求較高的移動(dòng)機(jī)器人定位。 5． 地圖匹配定位

33、 地圖構(gòu)建和匹配算法是基于地圖匹配定位的關(guān)鍵，首先要獲得機(jī)器人工 作環(huán)境的全局地圖，在進(jìn)行地圖匹配定位時(shí)，利用把機(jī)器人機(jī)載傳感器獲取其局部地圖，將局部地圖和先存放在寄存器中的全局地圖做比較，如果是匹配的，那么機(jī)器人就能確定在這個(gè)環(huán)境中的全局位置和方向。地圖匹配的缺點(diǎn)是它對(duì)于機(jī)載傳感器獲得的局部地圖要求比較高，地圖上要有足夠多的與全局特征匹配的特征點(diǎn)，而這些特征點(diǎn)在一般相對(duì)簡(jiǎn)單的環(huán)境里還是比較少的。 6． 慣性定位 慣性定位是一種不依賴于任何外部信息的自主式定位。慣性傳感器是移動(dòng)機(jī)器人常用的一類內(nèi)部傳感器，一般利用陀螺儀計(jì)算移動(dòng)機(jī)器人的姿態(tài)，利用加速儀計(jì)算出移動(dòng)機(jī)器人的位置，將測(cè)量結(jié)果積分

34、合成得到機(jī)器人的位姿。這種方式的優(yōu)點(diǎn)是不需要外在的參照物，可以自主定位。缺點(diǎn)是慣性傳感器數(shù)據(jù)會(huì)隨著時(shí)間產(chǎn)生偏差，每次積分后原先的位置和方向上的微小誤差都會(huì)隨之放大，且有誤差累計(jì)效應(yīng)。 2 基于粒子濾波器的SLAM算法 2.1 SLAM的通用框架和理論模型 圖2-1 SLAM通用框架 SLAM問題是把定位和地圖構(gòu)建結(jié)合在一起的，下圖為SLAM的理論模型： 圖2-2 SLAM的理論模型 2.2 粒子

35、濾波器定位的基本原理 粒子濾波（PF）定位方法用粒子集描述概率分布，而不用概率分布函數(shù)本身表示[ ]?？柭鼮V波器（KF）和擴(kuò)展卡爾曼濾波器（EKF）利用參數(shù)化模型(一個(gè)高維的高斯函數(shù))來表示概率密度分布；粒子濾波器采用一系列有限的采樣粒子代表概率密度分布，高概率區(qū)域含有高概率的粒子，反之亦然。粒子濾波以樣本集合的方法逼近概率分布，如果粒子數(shù)趨于無窮大，那么這種非參數(shù)化的粒子分布能夠逼近任何復(fù)雜的概率分布。因此，粒子濾波能夠比較精確地表達(dá)基于觀測(cè)量和控制量的后驗(yàn)概率分布。它采用一組帶有權(quán)值的隨機(jī)樣本來描述概率分布。在概率高的區(qū)域，粒子的密度就大，相反，在概率低的區(qū)域，粒子的密度就小。

36、 粒子濾波定位方法優(yōu)點(diǎn)很突出：能夠處理多峰分布；能夠很大地降低內(nèi)存耗費(fèi)；能夠更精確地定位，且便于實(shí)現(xiàn)；不受限于高斯分布，在很多定位問題中，如位姿跟蹤、全局定位甚至綁架問題，都取得較好的應(yīng)用。粒子濾波方法作為一種基于貝葉斯估計(jì)的非線性濾波算法，在處理非高斯非線性時(shí)變系統(tǒng)的參數(shù)估計(jì)和狀態(tài)濾波問題方面具有獨(dú)特的優(yōu)勢(shì)。 EKF只適用于高斯噪聲模型，對(duì)于非線性的模型表現(xiàn)非常差。而粒子濾波則適用于任意噪聲模型。利用EKF只能在濾波誤差和預(yù)測(cè)誤差很小的情況下通過對(duì)系統(tǒng)的非線性進(jìn)行近似以獲得令人滿意的估計(jì)精度，否則濾波初期估計(jì)協(xié)方差下降太快會(huì)導(dǎo)致濾波不穩(wěn)定甚至發(fā)散。此外EKF還要求噪聲必須服從高斯分布，大大

37、限制它的應(yīng)用范圍。而PF在測(cè)量的基礎(chǔ)上，通過調(diào)節(jié)個(gè)粒子的權(quán)值大小和樣本的位置，來近似實(shí)際的概率分布，并以樣本的均值作為系統(tǒng)的估計(jì)值。因此，PF不受線性化誤差和高斯噪聲假定的限制，適用于任何狀態(tài)轉(zhuǎn)換和觀測(cè)模型，有效克服了EKF的缺點(diǎn)。 PF也有缺點(diǎn)：相對(duì)于EKF，粒子濾波的計(jì)算了較大，特別是在采用非常多的粒子數(shù)目時(shí)。隨著計(jì)算機(jī)處理能力的不斷增強(qiáng)，以及各種改進(jìn)的實(shí)時(shí)粒子濾波算法不斷涌現(xiàn)，PF算法計(jì)算了大等障礙正在逐漸消失；在采用PF算法時(shí)，在粒子濾波中，普遍存在的問題可能是存在的退化現(xiàn)象[ ]。因?yàn)镻F抽取N個(gè)具有權(quán)值的粒子后，系統(tǒng)如接收到新的觀測(cè)數(shù)據(jù)，則實(shí)時(shí)更新每個(gè)粒子的權(quán)值。重要性權(quán)值

38、的分布會(huì)變得越來越傾斜，這時(shí)有可能出現(xiàn)粒子匱乏現(xiàn)象。即經(jīng)過幾次迭代后，少數(shù)樣本會(huì)占去總體樣本的絕大部分權(quán)重，除了一個(gè)粒子以外，其余粒子均只有微小的權(quán)值，可忽略不計(jì)，這樣使得樣本集不能體現(xiàn)所要表達(dá)的后驗(yàn)概率密度，同樣使得大量計(jì)算浪費(fèi)在幾乎不起任何作用的粒子的更新上。 2.3 擴(kuò)展卡爾曼濾波器算法 卡爾曼濾波器是由Kalman在20世紀(jì)60年代提出的一種遞推濾波器算法，它不需要保留使用過的測(cè)量數(shù)據(jù)，當(dāng)系統(tǒng)檢測(cè)到新的測(cè)量數(shù)據(jù)后，可以利用遞推公式獲得新的系統(tǒng)狀態(tài)估計(jì)。擴(kuò)展卡爾曼濾波器是在卡爾曼濾波器的基礎(chǔ)上針對(duì)非線性系統(tǒng)輸出估計(jì)提出的一種改進(jìn)方法。在擴(kuò)展卡爾曼濾波器中，對(duì)非線性系統(tǒng)首先采用一級(jí)泰

39、勒展開進(jìn)行線性化近似，然后利用最小均方誤差準(zhǔn)則對(duì)非線性系統(tǒng)狀態(tài)方程和測(cè)量方程的輸出進(jìn)行估計(jì)。在基于擴(kuò)展卡爾曼濾波器的機(jī)器人同步定位與地圖重建（Extended Kalman Filter SLAM，EKF-SLAM）算法中，系統(tǒng)噪聲均被默認(rèn)為符合高斯分布，因此利用系統(tǒng)狀態(tài)的均值和協(xié)方差進(jìn)行系統(tǒng)狀態(tài)輸出的描述。在EKF-SLAM中，系統(tǒng)狀態(tài)中包括機(jī)器人的位姿和已檢測(cè)到所有路標(biāo)的位置，因此，協(xié)方差矩陣的大小隨著路標(biāo)數(shù)量成平方比而增大。在EKF-SLAM算法中，根據(jù)機(jī)器人的運(yùn)動(dòng)狀態(tài)方程，并結(jié)合機(jī)器人當(dāng)前位姿和運(yùn)動(dòng)控制輸入利用擴(kuò)展卡爾曼濾波器對(duì)機(jī)器人下一時(shí)刻的位姿進(jìn)行預(yù)測(cè)估計(jì)。當(dāng)機(jī)器人傳感器檢測(cè)到外部

40、路標(biāo)時(shí)，根據(jù)傳感器測(cè)量方程，結(jié)合機(jī)器人當(dāng)前位姿、路標(biāo)位置以及傳感器的測(cè)量信息采用擴(kuò)展卡爾曼濾波器進(jìn)行系統(tǒng)狀態(tài)的更新。 傳統(tǒng)的EKF-SLAM算法存在如下兩個(gè)典型問題，一是計(jì)算復(fù)雜度，由于在EKF-SLAM中維護(hù)著系統(tǒng)狀態(tài)的協(xié)方差，并且在每次接受到傳感器測(cè)量數(shù)據(jù)時(shí)需要進(jìn)行系統(tǒng)狀態(tài)協(xié)方差的更新，因此EKF-SLAM算法的計(jì)算復(fù)雜度為（其中為已檢測(cè)到的路標(biāo)數(shù)目）。二是系統(tǒng)狀態(tài)估計(jì)的一致性，由于EKF-SLAM算法中利用擴(kuò)展卡爾曼濾波器對(duì)機(jī)器人運(yùn)動(dòng)方程和傳感器測(cè)量方程的輸出進(jìn)行估計(jì)，然而在擴(kuò)展卡爾曼濾波器中，對(duì)系統(tǒng)輸出采用的是一級(jí)泰勒公式展開近似，因此當(dāng)系統(tǒng)的非線性非常嚴(yán)重時(shí)，采用擴(kuò)展卡爾曼濾波器對(duì)

41、系統(tǒng)輸出估計(jì)的精確性會(huì)嚴(yán)重降低，甚至導(dǎo)致輸出估計(jì)的發(fā)散。針對(duì)EKF-SLAM算法計(jì)算復(fù)雜度大的問題，Newman等人提出了一種基于局部子地圖序列的SLAM算法，它的核心是重建一系列相互獨(dú)立而且具有固定大小的局部地圖，然后再利用擴(kuò)展卡爾曼濾波器將各個(gè)局部地圖合并為一個(gè)完整的全局地圖，從而保證SLAM過程中的耗時(shí)不會(huì)隨著檢測(cè)到路標(biāo)數(shù)目的增大而無限增大。Lina等人從機(jī)器人位姿估計(jì)一致性最大化角度對(duì)局部地圖的大小取值進(jìn)行了分析。Chong等人同樣提出利用子地圖進(jìn)行環(huán)境地圖描述的方法，他的做法是對(duì)子地圖之間的相對(duì)位置進(jìn)行單獨(dú)估計(jì)，當(dāng)機(jī)器人回到先前的子地圖區(qū)域時(shí)，采用搜索算法對(duì)機(jī)器人進(jìn)行重新定位。子地圖

42、分割法可以有效降低計(jì)算的復(fù)雜度，但它帶來的最大問題是數(shù)據(jù)關(guān)聯(lián)的確認(rèn)上有一定的困難。 2.3 粒子重采樣 通過增加采樣點(diǎn)數(shù)可以有效地克服退化現(xiàn)象，采樣點(diǎn)數(shù)越小退化現(xiàn)象越嚴(yán)重，然而采樣點(diǎn)數(shù)大時(shí)又影響計(jì)算的實(shí)時(shí)性。可見通過增加采樣點(diǎn)數(shù)來解決退化問題是不現(xiàn)實(shí)的。 目前消除退化問題的兩個(gè)關(guān)鍵技術(shù)分別是：選取適當(dāng)?shù)闹匾芏群瘮?shù)和進(jìn)行再采樣。粒子濾波的收斂精度和收斂速度直接關(guān)系到它能否實(shí)現(xiàn)應(yīng)用及應(yīng)用的范圍，是另一個(gè)值得重點(diǎn)研究的關(guān)鍵內(nèi)容。為了避免粒子匱乏，Gorden等人提出了重采樣方案，其主要思想史除去那些權(quán)值小的粒子，并復(fù)制權(quán)值大的粒子，重采樣通過過濾樣本集中權(quán)值較低的樣本。保留高權(quán)重樣本，使權(quán)

43、重集中在權(quán)重大的樣本上，來達(dá)到解決退化問題，一般根據(jù)權(quán)重對(duì)后驗(yàn)概率密度進(jìn)行離散近似處理。 重采樣原理：時(shí)刻的先驗(yàn)概率可由帶有權(quán)值的粒子近似表示。經(jīng)過系統(tǒng)觀測(cè)并重新計(jì)算權(quán)值，那些權(quán)值大的粒子可以分裂出若干新的粒子，而權(quán)值很小的粒子將被丟棄，如此即可得到一組新的粒子。這些新的粒子加入隨機(jī)量后即可預(yù)測(cè)在t時(shí)刻的狀態(tài)，即系統(tǒng)狀態(tài)轉(zhuǎn)移過程。最后再次進(jìn)入系統(tǒng)觀測(cè)過程，并預(yù)測(cè)下一時(shí)刻的狀態(tài)。重采樣階段會(huì)造成樣本有效性和多樣性的損失，導(dǎo)致樣本貧化現(xiàn)象。如何保持粒子的有效性和多樣性，克服樣本貧化也是應(yīng)該考慮的問題之一。 2.4 移動(dòng)機(jī)器人SLAM問題描述 2.4.1 SLAM計(jì)算復(fù)雜度 SLAM解法

44、在實(shí)際應(yīng)用中需要滿足實(shí)時(shí)性要求，即傳感器的數(shù)據(jù)采集周期應(yīng)大于SLAM算法處理一幀傳感器數(shù)據(jù)的時(shí)間。全狀態(tài)SLAM算法如EKF-SLAM的計(jì)算復(fù)雜度與狀態(tài)向量的維數(shù)成指數(shù)關(guān)系，在路標(biāo)數(shù)目較大時(shí)全狀態(tài)SLAM算法不能滿足實(shí)時(shí)性要求，因而不適合大范圍環(huán)境中的SLAM問題，Guivant等人提出了CEKF-SLAM（Compressed EKF）算法，采用近似的EKF全方差計(jì)算方法，將大范圍環(huán)境分成若干局部子區(qū)域，在大部分時(shí)間僅用EKF更新狀態(tài)向量中的局部估計(jì)，在機(jī)器人從一個(gè)局部區(qū)域轉(zhuǎn)向另一個(gè)局部區(qū)域時(shí)對(duì)狀態(tài)向量的均值和方差進(jìn)行全局更新，從而大大提高了計(jì)算效率。此外，Leonard等將整個(gè)地圖分解為一

45、系列子地圖，然后對(duì)每個(gè)子地圖進(jìn)行濾波估計(jì)，降低了狀態(tài)向量方差更新過程中的計(jì)算復(fù)雜度。擴(kuò)展信息濾波器（Extended Information Filter，EIF）是EKF的對(duì)稱形式，它保持了EKF數(shù)學(xué)嚴(yán)謹(jǐn)、估計(jì)準(zhǔn)確的特點(diǎn)，并且，其信息矩陣幾乎稀疏，特別適合SLAM問題，Eustice等從稀疏信息矩陣的角度提出了SEIF-SLAM算法，估計(jì)精度也得到了保證。Montemerlo等人采用Rao-Blackweuise粒子濾波器解決SLAM問題，并提出了FastSLAM算、法它的計(jì)算量與某一時(shí)刻觀測(cè)到的地圖特征數(shù)目成線性關(guān)系，尤其適用于大范圍環(huán)境中的SLAM問題。 2.4.2 SLAM的聯(lián)合估

46、計(jì) 機(jī)器人運(yùn)動(dòng)誤差如何干擾傳感器誤差的先后不確定會(huì)導(dǎo)致定位和構(gòu)建地圖之間先后的關(guān)系的不確定。機(jī)器人運(yùn)動(dòng)，位姿估計(jì)被運(yùn)動(dòng)誤差干擾。環(huán)境特征在全中的位置也基于粒子濾波器的同時(shí)定位與地圈構(gòu)件的方法研究被觀測(cè)誤差和位姿估計(jì)誤差所干擾。然而，與觀測(cè)噪聲不同的是，位姿誤差系統(tǒng)性地作用于全局地圖。這個(gè)作用也就是，路徑的誤差與地圖的誤差是相互關(guān)聯(lián)的。因此，沒有正確路徑估計(jì)的條件下無法估計(jì)地圖的誤差。盡管，路徑誤差和地圖誤差的關(guān)系使SLAM問題在變得更加復(fù)雜和困難，下面就要講怎樣把這個(gè)關(guān)系分解，把SLAM問題轉(zhuǎn)換成一些小的問題的乘積，然后可以用更有效的方法解決這些問題。 2.4.3 SLAM的后驗(yàn)估計(jì)表示

47、 在機(jī)器人的控制過程中會(huì)有噪聲干擾，可以把每個(gè)控制量或者觀測(cè)量以及一些噪聲模型當(dāng)成一個(gè)概率約束，每個(gè)控制量概率地約束兩個(gè)連續(xù)的機(jī)器人位姿，觀測(cè)量約束機(jī)器人和目標(biāo)的相對(duì)位置，新的觀測(cè)量會(huì)更新地圖特征和機(jī)器人位姿以及以前所觀測(cè)的地圖特征，開始，這些約束可能很不確定。然而，隨著地圖的特征被反復(fù)的觀測(cè)，約束越來越確定。在無限次的觀測(cè)和控制后，所有的地圖特征的位置就完全相關(guān)。 可以通過估計(jì)算法估計(jì)最可能的機(jī)器人位姿和地圖。但是這些方法需要處理的是一組變化無常的觀測(cè)量和控制量，不適合在線操作，這些算法一般不能估計(jì)地圖的不同部分的確定性。最流行的在線解決SLAM問題的方法是：已知傳感器讀數(shù)，對(duì)所有可能的

48、地圖和路徑估計(jì)后驗(yàn)概率分布。 SLAM算法的一些基本參數(shù)：地圖含有N個(gè)特征，記為： （2.1） 在t時(shí)刻，加入的控制量： （2.2） 機(jī)器人軌跡： （2.3） 在平面環(huán)境中包含三個(gè)參數(shù)機(jī)器人的坐標(biāo)和方向角， t時(shí)刻以前所有路標(biāo)的觀測(cè)量： （2.4） 本文假設(shè)每個(gè)觀測(cè)量

49、提供的信息是一個(gè)環(huán)境特征相對(duì)于目前位置的確切位置。變量代表了某個(gè)被觀察到的環(huán)境特征。實(shí)際上，并不會(huì)觀察到環(huán)境特征的唯一性，因?yàn)楹芏喹h(huán)境特征看起來是一樣的。對(duì)應(yīng)觀測(cè)量的唯一的環(huán)境特征寫做，相關(guān)性變量： （2.5） 表示所有數(shù)據(jù)關(guān)聯(lián)的集合，確定的過程叫數(shù)據(jù)關(guān)聯(lián)，表示在時(shí)刻t，第個(gè)路標(biāo)是否被觀測(cè)到。路徑信息和地圖信息兩個(gè)量都依賴于觀測(cè)量，必須把構(gòu)建地圖和定位過程一起考慮。機(jī)器人依靠傳感器得到的觀測(cè)量、自身的控制量以及相關(guān)性變量來得到路徑信息和地圖信息。解決SLAM問題的核心是在于應(yīng)該把定位和

50、地圖構(gòu)建視為整體。實(shí)際上，這是因?yàn)槁窂叫畔⒑偷貓D信息兩個(gè)量都依賴于觀測(cè)量。所以要想找到解決方法，必須把構(gòu)建地圖和定位過程一起考慮。在實(shí)際應(yīng)用中，這些點(diǎn)的環(huán)境特征可以對(duì)應(yīng)傳感器提取特征的位置。 SLAM問題的實(shí)質(zhì)就是根據(jù)已記錄的從時(shí)刻1到t的觀測(cè)量、控制信息以及初始狀態(tài)信息來決定所有路標(biāo)位置集合和機(jī)器人位姿。若在t時(shí)刻所測(cè)得的路標(biāo)數(shù)已知，那么SLAM問題將變得簡(jiǎn)單得多，概率分布描述成在時(shí)刻t的路標(biāo)位置和小車狀態(tài)的聯(lián)合后驗(yàn)密度函數(shù)，即SLAM被定義為一個(gè)關(guān)于路徑和路標(biāo)的后驗(yàn)估計(jì)： (2.6

51、) 如果數(shù)據(jù)關(guān)聯(lián)的集合也已知，那么SLAM后驗(yàn)概率簡(jiǎn)化為： (2.7) SLAM在t時(shí)刻的后驗(yàn)概率可以用時(shí)刻后驗(yàn)概率函數(shù)來遞歸計(jì)算，這個(gè)遞歸的更新算法，稱為SLAM的貝葉斯濾波器。 下面的遞歸公式，也就是貝耶斯濾波器，可以用于已知在的后驗(yàn)概率情況下，計(jì)算時(shí)刻的后驗(yàn)概率： ( 2.8) 是代表概率分布的歸一化因子。一般式中的積分是無法計(jì)算的，可以假設(shè)后驗(yàn)概率分布為一個(gè)特定形式來計(jì)算。機(jī)器人在環(huán)境移動(dòng)時(shí)，采集了它自身運(yùn)動(dòng)的相關(guān)信息，這個(gè)信息可以用機(jī)器人輪

52、子上帶的里程計(jì)、內(nèi)部的導(dǎo)航單元或通過觀測(cè)機(jī)器人執(zhí)行的控制命令得到。 FastSLAM是RBPF(Rao-Blackwellised Particle Fitering)濾波器的一個(gè)實(shí)例[ ] ，Murphy等人對(duì)動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)（Dynamic Bayesian Networks）進(jìn)行分析得出結(jié)論：若機(jī)器人的路徑估計(jì)已知，則陸標(biāo)估計(jì)（,,）之間相互獨(dú)立。由貝葉斯公式及特征估計(jì)之間的獨(dú)立性假設(shè)，對(duì)后驗(yàn)概率分布分解： (2.9) 其中右邊第一部分用粒子濾波器估計(jì)，第二部分用擴(kuò)展卡爾曼濾波器估計(jì)，每個(gè)粒子都有自己?jiǎn)为?dú)的地圖，在每個(gè)粒子中

53、，如果環(huán)境特征的個(gè)數(shù)是已知的，那么每個(gè)環(huán)境特征的位置都可以用一次EKF獨(dú)立的進(jìn)行估計(jì)。 假設(shè)需要k個(gè)粒子實(shí)現(xiàn)SLAM，F(xiàn)astSLAM總共有kn個(gè)卡爾曼濾波器，粒子濾波器的每個(gè)粒子代表機(jī)器人的一條可能運(yùn)動(dòng)路徑，利用觀測(cè)信息計(jì)算每個(gè)粒子的權(quán)重： (2.10) FastSLAM 的EKF與傳統(tǒng)的EKF相似，它近似線性高斯函數(shù)測(cè)量模型，線性高斯測(cè)量模型得到的分布 是高斯分布的，即使動(dòng)作模型不是線性的，這也是對(duì)機(jī)器人姿態(tài)使用采樣來近似分布的結(jié)果。 2.4.4 SLAM公式推導(dǎo) SLAM分解可以從SLAM的路徑后驗(yàn)概率

54、中直接推導(dǎo)。用條件概率的定義，SLAM后驗(yàn)概率可以寫成： (2.11) 因此，要得到分解的后驗(yàn)概率，對(duì)所有的非負(fù)值t需要以下方程： (2.12) 這個(gè)論斷的證據(jù)可以從歸納中得到。要得到這個(gè)結(jié)果，需要兩個(gè)中間結(jié)果．第一個(gè)量是觀測(cè)到的環(huán)境特征的概率。這個(gè)量可以用貝耶斯定理寫成： (2.13) 目前的觀測(cè)量?jī)H僅與機(jī)器人的狀態(tài)和被觀測(cè)到的環(huán)境特征有關(guān)。在（2.13）最右邊的項(xiàng)，在沒有新的觀測(cè)量的情況下，當(dāng)前的位姿、當(dāng)前的控制量、當(dāng)前的數(shù)據(jù)關(guān)聯(lián)對(duì)沒有影響。因此，所有這

55、些變量可以省去得到： (2.14) 需要的第二個(gè)中間結(jié)果是，沒有觀測(cè)到的任意環(huán)境特征的概率這很簡(jiǎn)單，如果沒有觀測(cè)到環(huán)境特征，環(huán)境特征后驗(yàn)概率不變。因此，環(huán)境特征的后驗(yàn)概率在時(shí)刻等于時(shí)刻。首先，在時(shí)刻做歸納假設(shè)： (2.15) 歸納的開始情況,沒有加入觀測(cè)量。因此，，分解因式是對(duì)的。在,再一次用貝耶斯定理來擴(kuò)展（2.12）的左邊可以得到： (2.16) 等于單獨(dú)的環(huán)境特征后驗(yàn)概率的乘積。 (2.17) 2.4.5 有效的數(shù)據(jù)關(guān)聯(lián) 在實(shí)際的SLAM應(yīng)用中，很少能觀

56、測(cè)到數(shù)據(jù)關(guān)聯(lián)。然而，如果環(huán)境特征位置的不確定性相對(duì)于環(huán)境特征間的平均距離低，確定正確的約束關(guān)聯(lián)的簡(jiǎn)單提示很見效。數(shù)據(jù)關(guān)聯(lián)的最普通的方法是用最大似然定理來給每個(gè)觀測(cè)量指派。換句話說，每個(gè)觀測(cè)量是被指派給最可能的產(chǎn)生它的環(huán)境特征。如果最大的概率仍比固定的限值低，就被考慮加入一個(gè)新的環(huán)境特征。 在EKF的情況下，觀測(cè)量的概率可以寫成觀測(cè)量和期望觀測(cè)量的差值的函數(shù)。這個(gè)差值被稱為修正值： (2.18) 數(shù)據(jù)關(guān)聯(lián)的提示經(jīng)常用負(fù)的對(duì)數(shù)似然方程表示，如下： (2.19) 方程的第二部分就是Mahalanobis距離：通過觀測(cè)量和環(huán)境

57、特征估計(jì)的協(xié)方差米制規(guī)格化的距離。由于這個(gè)原因，米制的數(shù)據(jù)關(guān)聯(lián)通常被稱為最鄰近的數(shù)據(jù)關(guān)聯(lián)。最大似然數(shù)據(jù)關(guān)聯(lián)主要用在數(shù)據(jù)關(guān)聯(lián)比不正確的數(shù)據(jù)關(guān)聯(lián)顯著合適的時(shí)候。然而，如果環(huán)境特征的位置很不確定，多個(gè)數(shù)據(jù)關(guān)聯(lián)的概率很高。如果選取的數(shù)據(jù)關(guān)聯(lián)是錯(cuò)誤地。對(duì)于結(jié)果地圖的精確性會(huì)有影響。如果機(jī)器人的傳感器噪聲嚴(yán)重的話，很容易產(chǎn)生這種數(shù)據(jù)關(guān)聯(lián)的不確定性。 解決這個(gè)問題的一個(gè)方法是只加入那些得到明確數(shù)據(jù)關(guān)聯(lián)的觀測(cè)量。然而，如果SLAM環(huán)境是有噪聲的，大多數(shù)的觀測(cè)量得不到處理。而且，沒有加入觀測(cè)量會(huì)導(dǎo)致高估環(huán)境特征的協(xié)方差，使以后的數(shù)據(jù)關(guān)聯(lián)變得更加不確定。 2.4.6 FastSLAM的粒子表示形式 在Fa

58、stSLAM中，路標(biāo)估計(jì)利用獨(dú)立的EKF實(shí)現(xiàn)，一個(gè)路標(biāo)對(duì)應(yīng)一個(gè)EKF濾波器，地圖是一組N個(gè)2維的高斯模型，而不是一個(gè)單獨(dú)的聯(lián)合2*N維高斯模型。共有M個(gè)粒子濾波器用來估計(jì)路徑，每個(gè)粒子含有N個(gè)獨(dú)立的EKF濾波器，用來估計(jì)N個(gè)路標(biāo)，第i個(gè)粒子表示： （2.20） 其中包含一個(gè)路徑和基于該路徑上N個(gè)路標(biāo)高斯估計(jì)，第N個(gè)路標(biāo)的高斯表征的均值和方差分別是和，如圖2-3所示： 圖2-3 FastSLAM的粒子表示 2.4.7 FastSLAM的計(jì)算時(shí)間復(fù)雜度 FastSLAM有機(jī)地將粒子濾波器與卡爾曼濾

59、波器集成在一起，魯棒性地解決數(shù)據(jù)關(guān)聯(lián)和多目標(biāo)跟蹤問題，F(xiàn)astSLAM算法將狀態(tài)估計(jì)分解成了采樣部分和解析部分。由于這種分解，其計(jì)算復(fù)雜度僅為O，其中N,M分別為FastSLAM算法所用的粒子數(shù)和未知環(huán)境中的特征個(gè)數(shù),每個(gè)粒子中的高斯組用穩(wěn)定二叉樹表示，高斯參數(shù)和在樹的葉子上，訪問每個(gè)高斯模型所需要的時(shí)間與K成對(duì)數(shù)增長(zhǎng)關(guān)系。在復(fù)制粒子時(shí)，所有高斯模型樹中只有一個(gè)路徑必須被修改，為了完成這個(gè)樹，對(duì)于所有與之分開的枝干，所有的相關(guān)點(diǎn)從生成的粒子復(fù)制過來。所有與之分開的枝干就指向了與生成樹同樣的子樹，生成一個(gè)這樣的不完全的書只用了K對(duì)數(shù)時(shí)間，此外，由于需要用來達(dá)到葉子的步驟數(shù)與路徑的長(zhǎng)度相等，訪問高

60、斯模型也需要K對(duì)時(shí)間。在每個(gè)更新步驟需要?jiǎng)?chuàng)建M個(gè)新粒子，所以更新總共需要0時(shí)間。 FastSLAM算法是基于粒子濾波的，它對(duì)于機(jī)器人位姿和數(shù)據(jù)融合采樣，在每個(gè)粒子的基礎(chǔ)上計(jì)算路標(biāo)的位置，用一些平衡的二叉樹來表示路標(biāo)，使得更新路標(biāo)只需要路標(biāo)數(shù)的對(duì)數(shù)次，每個(gè)粒子對(duì)位姿的不確定性是來自數(shù)據(jù)融合中，能更清楚的在噪聲環(huán)境中區(qū)分出來路標(biāo)，甚至能在很簡(jiǎn)單的數(shù)據(jù)融合條件下進(jìn)行更新路標(biāo)。 圖2-4 從上一時(shí)刻復(fù)制和更新樹的過程圖 3 模型建立 3.1 運(yùn)動(dòng)模型 在SLAM中有兩個(gè)系統(tǒng)模型：觀測(cè)模型和運(yùn)動(dòng)模型。運(yùn)動(dòng)模型描述了t時(shí)刻機(jī)器人位姿的概率分

61、布。機(jī)器人運(yùn)動(dòng)模型如圖3.1所示[ ]： 圖3.1 機(jī)器人運(yùn)動(dòng)模型 機(jī)器人的運(yùn)動(dòng)模型見式[ ]： （3.1） 其中：、、分別為時(shí)刻第i個(gè)粒子所表征的機(jī)器人質(zhì)心坐標(biāo)和方向角；為時(shí)刻到t時(shí)刻的時(shí)間間隔；、分別是機(jī)器人的速度和角速度；、、分別為相應(yīng)的噪聲項(xiàng)。 3.2 觀測(cè)模型 觀測(cè)模型即描述了當(dāng)機(jī)器人位姿為時(shí)，觀測(cè)到的信息為的概率。其中：為時(shí)刻里程計(jì)給出的控制信息。和分別表示時(shí)刻機(jī)器人的速度和角速度。 機(jī)器人利用自身配

62、備的距離/方向傳感器探測(cè)陸標(biāo)，得到陸標(biāo)的距離和方向角。其觀測(cè)模型見式[ ]： （3.2） 其中：分別是機(jī)器人的位置；是機(jī)器人的方向角；、分別是對(duì)應(yīng)于測(cè)量距離和方向的噪聲。機(jī)器人觀測(cè)模型如圖3.2所示： 圖3.2 機(jī)器人觀測(cè)模型 4 FastSLAM算法步驟 4.1 FastSLAM算法步驟 基于粒子濾波器和EKF的FastSLAM算法如下： 1　 根據(jù)初始坐標(biāo)，隨機(jī)生成個(gè)粒子、粒子權(quán)值組成的粒子集，見式 、、：

63、 （4.1） （4.2） （4.3） 2　 預(yù)測(cè)機(jī)器人位姿[ ]，根據(jù)時(shí)刻粒子集中的每個(gè)粒子及運(yùn)動(dòng)模型，計(jì)算時(shí)刻粒子。 3　 計(jì)算權(quán)重，重要性權(quán)值計(jì)算方法見式 （4

64、.4） 4　 根據(jù)觀測(cè)模型計(jì)算粒子對(duì)應(yīng)的特征點(diǎn)坐標(biāo)，并將其與環(huán)境陸標(biāo)集中的某一陸標(biāo)進(jìn)行關(guān)聯(lián)。 5　 對(duì)粒子權(quán)值進(jìn)行更新，見式[ ]: （4.5） 式中，函數(shù)G為高斯概率計(jì)算公式，可根據(jù)對(duì)粒子集權(quán)值進(jìn)行更新。 6　 歸一化見式： （4.6） 7　 計(jì)算有效粒子數(shù)，設(shè)定一個(gè)閾值，若有效粒子數(shù)小于這個(gè)閾值，根據(jù)粒子權(quán)值對(duì)粒子進(jìn)行重采樣[ ]，去除權(quán)值小的粒子，復(fù)制權(quán)值大的粒子。 8　 計(jì)算權(quán)重。 9　 環(huán)境地圖更

65、新，利用對(duì)進(jìn)行更新。 10　 通過公式計(jì)算粒子k時(shí)刻的位姿，若還要計(jì)算下一時(shí)刻的位姿，則轉(zhuǎn)到步驟?；否則結(jié)束定位。 4.2 新位姿采樣 粒子集是遞增更新的，是根據(jù)時(shí)刻的粒子和路標(biāo)觀測(cè)量，控制量得來的。機(jī)器人的新位置是由上一個(gè)位置和控制輸入推算得出的，然后結(jié)合新的觀測(cè)值來更新路標(biāo)的位置。FastSLAM對(duì)上一時(shí)刻的路徑和最近的控制量進(jìn)行采樣,t時(shí)刻中的每個(gè)粒子生成由可能性運(yùn)動(dòng)模型取樣中獲得的一個(gè)對(duì)機(jī)器人姿態(tài)估計(jì)的可能性估計(jì)，見式[ ]： （4.7） 然后將這種估

66、計(jì)連同路徑加入到一個(gè)臨時(shí)粒子組中，假設(shè)中的粒子組根據(jù)來分配，則新的粒子也根據(jù)分配。通過這種方法生產(chǎn)M個(gè)粒子后，通過對(duì)臨時(shí)粒子組取樣獲得新組。每個(gè)粒子通過權(quán)重因數(shù)更新獲得。 4.3 環(huán)境特征估計(jì)的更新 環(huán)境特征估計(jì)是以機(jī)器人的位姿為條件的，個(gè)EKF加到集合的每個(gè)粒子上。第個(gè)環(huán)境特征的后驗(yàn)概率很容易得到。它的計(jì)算是要看是否，也就是，是否環(huán)境特征在時(shí)刻被觀察到。對(duì)于被觀察到的環(huán)境特征，根據(jù)通常用貝耶斯定理的分解后驗(yàn)概率的程序。 （4.8） （4.9） 為第m個(gè)粒子的觀測(cè)量概率函數(shù)的倒數(shù)。 若，則簡(jiǎn)單地使用高斯公式不變[ ]： （4.10） t時(shí)刻的均值和協(xié)方差矩陣更新： （4.11） （4.12）