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

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

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

3、詞： 移動機(jī)器人； FastSLAM算法； 路徑估計； 地圖估計 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 移動機(jī)器人定位和地圖創(chuàng)建問題 2 1.1.1 移動機(jī)器人國內(nèi)外發(fā)展?fàn)顩r 2 1.1.2 移動機(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 移動機(jī)器人SLAM問題描述 10 2.4.1 SLAM計算復(fù)雜度 10 2.4.2 SLAM的聯(lián)合估計 11

9、 2.4.3 SLAM的后驗(yàn)估計表示 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的計算時間復(fù)雜度 16 3 模型建立 17 3.1 運(yùn)動模型 17 3.2 觀測模型 18 4 FastSLAM算法步驟 19 4.1 FastSLAM算法步驟 19 4.2 新位姿采樣 20 4.3 環(huán)境特征估計的更新 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 緒論 移動機(jī)器人的同時定位與地圖創(chuàng)建(SLAM)問題有重要的理論與應(yīng)用價值，被很多學(xué)者認(rèn)為是實(shí)現(xiàn)真正全自主移動機(jī)器人的關(guān)鍵。在火災(zāi)、地震或者極地考察現(xiàn)場存在一些潛在危險，可能會對進(jìn)入人員生命構(gòu)成危險，就需要移動機(jī)器人來代替人進(jìn)入這些環(huán)境，進(jìn)行探測地形和檢查是否有傷員的工作。民用方面可以用于自主行駛車輛或者車輛輔助駕駛系統(tǒng)，還有服務(wù)機(jī)器人，智能機(jī)器人，智能玩具等。軍用方面可以用于對敵方控制區(qū)偵查和對危險易爆炸物的搜索和

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

34、合成得到機(jī)器人的位姿。這種方式的優(yōu)點(diǎn)是不需要外在的參照物，可以自主定位。缺點(diǎn)是慣性傳感器數(shù)據(jù)會隨著時間產(chǎn)生偏差，每次積分后原先的位置和方向上的微小誤差都會隨之放大，且有誤差累計效應(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ù)化模型(一個高維的高斯函數(shù))來表示概率密度分布；粒子濾波器采用一系列有限的采樣粒子代表概率密度分布，高概率區(qū)域含有高概率的粒子，反之亦然。粒子濾波以樣本集合的方法逼近概率分布，如果粒子數(shù)趨于無窮大，那么這種非參數(shù)化的粒子分布能夠逼近任何復(fù)雜的概率分布。因此，粒子濾波能夠比較精確地表達(dá)基于觀測量和控制量的后驗(yàn)概率分布。它采用一組帶有權(quán)值的隨機(jī)樣本來描述概率分布。在概率高的區(qū)域，粒子的密度就大，相反，在概率低的區(qū)域，粒子的密度就小。

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

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

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

39、勒展開進(jìn)行線性化近似，然后利用最小均方誤差準(zhǔn)則對非線性系統(tǒng)狀態(tài)方程和測量方程的輸出進(jìn)行估計。在基于擴(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ī)器人的位姿和已檢測到所有路標(biāo)的位置，因此，協(xié)方差矩陣的大小隨著路標(biāo)數(shù)量成平方比而增大。在EKF-SLAM算法中，根據(jù)機(jī)器人的運(yùn)動狀態(tài)方程，并結(jié)合機(jī)器人當(dāng)前位姿和運(yùn)動控制輸入利用擴(kuò)展卡爾曼濾波器對機(jī)器人下一時刻的位姿進(jìn)行預(yù)測估計。當(dāng)機(jī)器人傳感器檢測到外部

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

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

42、分割法可以有效降低計算的復(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í)時性?？梢娡ㄟ^增加采樣點(diǎn)數(shù)來解決退化問題是不現(xiàn)實(shí)的。 目前消除退化問題的兩個關(guān)鍵技術(shù)分別是：選取適當(dāng)?shù)闹匾芏群瘮?shù)和進(jìn)行再采樣。粒子濾波的收斂精度和收斂速度直接關(guān)系到它能否實(shí)現(xiàn)應(yīng)用及應(yīng)用的范圍，是另一個值得重點(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)重對后驗(yàn)概率密度進(jìn)行離散近似處理。 重采樣原理：時刻的先驗(yàn)概率可由帶有權(quán)值的粒子近似表示。經(jīng)過系統(tǒng)觀測并重新計算權(quán)值，那些權(quán)值大的粒子可以分裂出若干新的粒子，而權(quán)值很小的粒子將被丟棄，如此即可得到一組新的粒子。這些新的粒子加入隨機(jī)量后即可預(yù)測在t時刻的狀態(tài)，即系統(tǒng)狀態(tài)轉(zhuǎn)移過程。最后再次進(jìn)入系統(tǒng)觀測過程，并預(yù)測下一時刻的狀態(tài)。重采樣階段會造成樣本有效性和多樣性的損失，導(dǎo)致樣本貧化現(xiàn)象。如何保持粒子的有效性和多樣性，克服樣本貧化也是應(yīng)該考慮的問題之一。 2.4 移動機(jī)器人SLAM問題描述 2.4.1 SLAM計算復(fù)雜度 SLAM解法

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

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

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

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

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

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

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

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

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

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

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

55、些變量可以省去得到： (2.14) 需要的第二個中間結(jié)果是，沒有觀測到的任意環(huán)境特征的概率這很簡單，如果沒有觀測到環(huán)境特征，環(huán)境特征后驗(yàn)概率不變。因此，環(huán)境特征的后驗(yàn)概率在時刻等于時刻。首先，在時刻做歸納假設(shè)： (2.15) 歸納的開始情況,沒有加入觀測量。因此，，分解因式是對的。在,再一次用貝耶斯定理來擴(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、測到數(shù)據(jù)關(guān)聯(lián)。然而，如果環(huán)境特征位置的不確定性相對于環(huán)境特征間的平均距離低，確定正確的約束關(guān)聯(lián)的簡單提示很見效。數(shù)據(jù)關(guān)聯(lián)的最普通的方法是用最大似然定理來給每個觀測量指派。換句話說，每個觀測量是被指派給最可能的產(chǎn)生它的環(huán)境特征。如果最大的概率仍比固定的限值低，就被考慮加入一個新的環(huán)境特征。 在EKF的情況下，觀測量的概率可以寫成觀測量和期望觀測量的差值的函數(shù)。這個差值被稱為修正值： (2.18) 數(shù)據(jù)關(guān)聯(lián)的提示經(jīng)常用負(fù)的對數(shù)似然方程表示，如下： (2.19) 方程的第二部分就是Mahalanobis距離：通過觀測量和環(huán)境

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

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

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

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

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

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

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

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

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

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