本科畢業(yè)設計題目：移動機器人FastSLAM算法研究學 院:專 業(yè):學 號:學生姓名:指導教師:日 期:武漢科技大學本科畢業(yè)設計摘 要移動機器人同時定位與地圖創(chuàng)建是實現(xiàn)未知環(huán)境下機器人自主導航的關(guān)鍵性技術(shù)，具有廣泛的應用前景，也是目前機器人研究的熱門課題之一。基于卡爾曼濾波器的SLAM算法有計算的復雜性以及對數(shù)據(jù)融合誤差非常敏感的缺點，使其不能再實際環(huán)境中得到廣泛應用。為解決傳統(tǒng)SLAM算法的缺陷，介紹了一種基于Rao-Blackwellized粒子濾波器的FastSLAM方法，該方法將SLAM問題分解為對機器人姿態(tài)和路標在地圖中的位置的遞歸算法。每一粒粒子都有對應的地圖，再將地圖估計分解成N個獨立的特征估計，路徑估計采用粒子濾波器，地圖估計采用擴展卡爾曼濾波器。FastSLAM有機地將粒子濾波器與卡爾曼濾波器集成在一起，魯棒性地解決數(shù)據(jù)關(guān)聯(lián)和多目標跟蹤問題，其時間消耗與路標的數(shù)量成對數(shù)關(guān)系，計算量小，用時短?；赗ao-Blackwellized粒子濾波器的FastSLAM算法是一種高效的機器人同步定位和繪制地圖的算法，其具有高效性和準確性，該方法使用提高了機器人地圖創(chuàng)建的實時性，增強了避障能力。關(guān)鍵詞： 移動機器人； 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 robot. 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 position 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 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-Blackwellized 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;The map estimation目 錄1 緒論11.1 移動機器人定位和地圖創(chuàng)建問題21.1.1 移動機器人國內(nèi)外發(fā)展狀況21.1.2 移動機器人的地圖構(gòu)建問題31.1.3 機器人的定位方法52 基于粒子濾波器的SLAM算法72.1 SLAM的通用框架和理論模型72.2 粒子濾波器定位的基本原理82.3 擴展卡爾曼濾波器算法92.3 粒子重采樣102.4 移動機器人SLAM問題描述102.4.1 SLAM計算復雜度102.4.2 SLAM的聯(lián)合估計112.4.3 SLAM的后驗估計表示112.4.4 SLAM公式推導132.4.5 有效的數(shù)據(jù)關(guān)聯(lián)142.4.6 FastSLAM的粒子表示形式152.4.7 FastSLAM的計算時間復雜度163 模型建立173.1 運動模型173.2 觀測模型184 FastSLAM算法步驟194.1 FastSLAM算法步驟194.2 新位姿采樣204.3 環(huán)境特征估計的更新215算法流程圖和代碼225.1 FastSLAM算法偽代碼225.2 FastSLAM算法流程226 仿真環(huán)境建立和仿真結(jié)果246.1 仿真環(huán)境介紹246.2 仿真結(jié)果247 總結(jié)和展望29參考文獻30致謝31331 緒論移動機器人的同時定位與地圖創(chuàng)建(SLAM)問題有重要的理論與應用價值，被很多學者認為是實現(xiàn)真正全自主移動機器人的關(guān)鍵。在火災、地震或者極地考察現(xiàn)場存在一些潛在危險，可能會對進入人員生命構(gòu)成危險，就需要移動機器人來代替人進入這些環(huán)境，進行探測地形和檢查是否有傷員的工作。民用方面可以用于自主行駛車輛或者車輛輔助駕駛系統(tǒng)，還有服務機器人，智能機器人，智能玩具等。軍用方面可以用于對敵方控制區(qū)偵查和對危險易爆炸物的搜索和拆除?？茖W方面可以用于外星探索和采礦等。SLAM問題研究是實現(xiàn)機器人完全自主以及服務人類的關(guān)鍵,因此,對移動機器人SLAM的研究具有重要的理論意義和應用價值,由于自主移動機器人具有廣泛的應用、巨大的經(jīng)濟利益和深遠的科學價值，各國家和科研工作者對其相關(guān)技術(shù)發(fā)展都投入了大量的研究。在20世紀70年代末期，法國的系統(tǒng)分析與架構(gòu)實驗室（LAAS）研制了HILARE機器人，這是歐洲的第一個移動機器人研究項目，HILARE安裝了視覺傳感器、激光測距儀和超聲波傳感器等設備，用二維多邊形環(huán)境模型和全局坐標系統(tǒng)實現(xiàn)導航和路徑規(guī)劃功能。多年來國內(nèi)外有大量的科技工作致力于這方面的研究工作，對許多問題的認識和求解都取得了長足的進步，但是仍然有很多問題需要解決。由于移動機器人系統(tǒng)運動模型難以精確建模，再加上其他傳感器感知誤差存在高度不確定性和外界干擾因素等，從而造成了機器人系統(tǒng)的高度復雜性和不確定性。由于在很多情況下，環(huán)境的信息是部分、甚至是完全未知的，此時必須將定位和地圖構(gòu)建結(jié)合起來，使得移動機器人利用對自身的定位來構(gòu)建地圖，同時利用構(gòu)建地圖來定位，在這一問題中，移動機器人定位與地圖構(gòu)建是密切相關(guān)的，地圖創(chuàng)建過程依賴于精確的機器人位姿信息，任何一方都無法單獨實現(xiàn)，必須同時加以考慮，近年來，SLAM算法越來越受到人們的重視，針對移動機器人的SLAM問題，主要有基于粒子濾波器的SLAM方法和基于擴展卡爾曼濾波以及EKF的各種變種SLAM方法。SLAM(Simultaneous Localization And Mapping）問題是指把移動機器人放在未知環(huán)境中，機器人增量式地創(chuàng)建未知環(huán)境的連續(xù)地圖，同時確定它在地圖中的位置，由于SLAM問題的解法使機器人實現(xiàn)了真正的自主導航，所以在過去幾十年中逐漸成為了移動機器人領(lǐng)域的研究熱點。SLAM一方面為了實現(xiàn)精確定位，機器人需要利用位置相對確定的環(huán)境特征不斷校正自身位置，一方面為了確定環(huán)境特征的位置，機器人需要了解自身所在的精確位姿。機器人的定位與地圖構(gòu)建有利于進一步協(xié)助運動規(guī)劃和躲避障礙物，體現(xiàn)機器人對環(huán)境探測效率的同時又體現(xiàn)出對環(huán)境地圖信息描述的精準程度。SLAM也稱CML(Concurrent Mapping and Localization),最初于上世界80年代早期由Smith Self和Cheeseman給出了基礎(chǔ)概率方法的解決方案， SLAM 問題的解決方法大致分為兩類:基于概率估計的方法和非概率估計的方法?；诟怕誓Ｐ偷姆椒ㄕ贾鲗У匚?。1.1 移動機器人定位和地圖創(chuàng)建問題1.1.1 移動機器人國內(nèi)外發(fā)展狀況1950年和1951年，William Grey Walter研制了兩個可完成自動避障等智能行為的移動機器人。在20世紀50年代初期，Marvin Minsky等人研制了具有跟蹤功能的移動機器人。1956年，在由美國國防部高級研究計劃局（DARPA）主辦的Dartmouth會議上，正式提出了“人工智能”概念，認為移動機器人應能夠執(zhí)行和學習有意義的任務，一定程度上能夠模仿動物或人的能力。1969年，Nilsson等人在斯坦福機器人研究所（SRI）研制了第一個智能移動機器人Shakey，它裝備了攝像機、測距儀和接觸傳感器等設備，并通過無線連接與DECPDP 10計算機相連，其任務是自動避障和運動目標跟蹤。在20世紀70年代末期，Hans Moravec在SRI研制了CART移動機器人，它能夠利用攝像機進行避障，并能利用連續(xù)的多幅圖像構(gòu)建二維環(huán)境模型，并可以實現(xiàn)路徑規(guī)劃。在20世紀70年代末期，法國的系統(tǒng)分析與架構(gòu)實驗室（LAAS）研制了HILARE機器人，這是歐洲的第一個移動機器人研究項目，HILARE安裝了視覺傳感器、激光測距儀和超聲波傳感器等設備，用二維多邊形環(huán)境模型和全局坐標系統(tǒng)實現(xiàn)導航和路徑規(guī)劃功能。目前，移動機器人的研究范圍已經(jīng)擴展到水下、空中、地面、甚至太空和行星表面等多種環(huán)境，如美國馬里蘭系統(tǒng)工程實驗室（Marine Systems Engineering Laboratory）研制的EAVE III型自主式水下移動機器人，美國海軍研制的新型戰(zhàn)場準備自主水下航行器（BPAUV）、美國無人車輛協(xié)會（the Joint of Unmanned Vehicle）組織開展的自主式航空飛行器的研究，世界著名移動機器人制造商IROBOT公司的金字塔探索者Pyramid Rover、月球車，以及卡耐基-梅隆大學（CMU）研制的CMU-Rover，日本Tsukuba大學的Yamabiko和MELDOG，法國系統(tǒng)分析與結(jié)構(gòu)實驗室（LAAS）的Hilare2，德國Karlsruhe大學的KAMRO等。美國航空航天局（NASA）研制的第一代火星探測機器人Sojourner于1997年成功登陸火星，開啟了人類利用移動機器人技術(shù)進行空間探測和開發(fā)的序幕。為了在火星上進行長距離探險，NASA又開始了新一代樣機的研制，命名為Rocky7（如圖1.2所示），并在Lavic湖的巖溶流上和干枯的湖床上進行了成功的實驗。最值得一提的是，2004年1月美國NASA火星探測移動機器人Spirit（如圖1.3所示）和Opportunity先后在火星表面順利登陸。Spirit和Opportunity的成功著陸標志著人類發(fā)射的星際探測器自動化程度提升到了前所未有的水平，同時也標志著智能移動機器人的研究工作也已經(jīng)提高到了一個前所未有的高度，正在為人類的進步發(fā)展貢獻力量。我國也針對這一趨勢制定了以月球為近期目標的空間探測計劃。研究和發(fā)展我國的月球探測移動機器人技術(shù)，不但對于我國在激烈的空間技術(shù)和資源競爭中取得有力地位具有關(guān)鍵意義，同時也對包括移動機器人導航控制在內(nèi)的相關(guān)技術(shù)的發(fā)展有巨大的促進作用。我國對移動機器人的研究雖然起步較晚，但是越來越受到重視，國家863智能機器人組把智能移動機器人的研究作為今后發(fā)展的重點。以中科院自動化研究所、清華大學、國防科技大學、東北大學、上海交通大學、哈爾濱工業(yè)大學為代表的我國機器人研究基地，已經(jīng)取得了令人矚目的成果。清華大學研制的THMR系列移動機器人系統(tǒng)；自動化所研制的集多種傳感器、視覺、語音識別與會話功能于一體的智能移動機器人CASIA-I；此外，沈陽自動化研究所機器人實驗室研制的激光導引AGV、防爆機器人、“海極號”和“探索者號”水下機器人，香港城市大學智能設計、自動化以及制造研究中心的自動導航車和服務機器人，哈爾濱工業(yè)大學機器人研究所研制的能在未知、復雜、多變、惡劣的環(huán)境下運行的全方位輪式移動機器人平臺等都說明了我國移動機器人技術(shù)取得的突出成就。但從總體水平上看，我國移動機器人技術(shù)比起發(fā)達國家還有一定的差距。1.1.2 移動機器人的地圖構(gòu)建問題移動機器人通過自身所帶的傳感器實現(xiàn)對外部環(huán)境的感知。由傳感器獲得的數(shù)據(jù)經(jīng)處理后要以某種抽象的形式來對環(huán)境空間進行表達，并作為地圖信息存儲在移動機器人中。移動機器人利用對環(huán)境的感知信息實現(xiàn)環(huán)境的建模，自動的構(gòu)建一個地圖，利用地圖給出的信息在外部環(huán)境中運動，并持續(xù)感知周圍環(huán)境，進一步完善和更新地圖。在移動機器人導航并自主定位過程中，按照一定的空間表達規(guī)則將傳感器信息轉(zhuǎn)換成地圖的過程是移動機器人導航并自主定位的基礎(chǔ)，而空間表達的方法又決定了地圖的表達形式。因此，采用何種空間表達方法對移動機器人自主定位和地圖的持續(xù)更新具有很大影響。目前地圖創(chuàng)建方法很多大致可以歸為四類:柵格表示法、幾何信息表示法、拓撲圖表示法和混合表示法。1. 基于柵格的地圖表示法 柵格地圖表示法最先是由Moravec和Elfes于1985年提出的。它將機器人工作環(huán)境劃分為一系列柵格，其中每個柵格都被分配一個概率值。此概率值表示該柵格被障礙物占據(jù)的可能性大小。柵格地圖的優(yōu)點在于易于重建和維護，對任何一個柵格的感知信息可以直接與環(huán)境中的某個區(qū)域相對應，特別適合于處理超聲測量數(shù)據(jù)。但是環(huán)境空間的分辨率和柵格大小相關(guān)，當需要增加分辨率時就必須縮小柵格的大小，進而增加了運算的時間和空間復雜度。另外，在傳統(tǒng)的柵格地圖表示法中，每個柵格之間被認為是相互獨立的，因此，利用柵格表示的地圖和實際環(huán)境地圖具有很大的不一致性9。為改進傳統(tǒng)柵格地圖表示法的這種缺點，Thrun提出了一種采用期望最大化方法進行環(huán)境地圖表示的方法，同時還考慮了各個相鄰柵格之間的依賴關(guān)系。在Bert等人的研究中，對環(huán)境采用全局柵格和局部柵格結(jié)合的方法進行建模，同時利用霍夫變換對柵格地圖中的線段進行抽象描述，并采用卡爾曼濾波器進行位置估計。Stepan等人提出了一種基于單目攝像頭和激光測距儀的柵格地圖構(gòu)造方法，對柵格地圖的質(zhì)量利用機器人的歷史路徑信息進行優(yōu)劣評估。Noykov等人提出了一種利用聲納傳感器數(shù)據(jù)進行柵格地圖構(gòu)造的方法，利用統(tǒng)計方法對聲納數(shù)據(jù)概率模型進行建模并結(jié)合模糊理論進行聲納數(shù)據(jù)融合。Grisetti等人采用Rao-Blackwellized粒子濾波器對柵格地圖進行估計，采取自適應方法獲取粒子數(shù)，同時，在粒子采樣函數(shù)的選取時考慮了最近多次傳感器的測量數(shù)據(jù)。2. 基于特征的地圖表示法基于特征的地圖表示法是指利用可以識別的環(huán)境路標進行環(huán)境地圖的表示，其中每個路標特征都用幾何原型進行表示。此種地圖表示方法局限于可以進行參數(shù)化的路標環(huán)境，或者是可建模的對象環(huán)境，比如點、線、面等。對特征地圖的重建大多都是基于外部傳感器對環(huán)境的檢測數(shù)據(jù)，然后利用這些檢測數(shù)據(jù)進行環(huán)境路標特征的提取。對結(jié)構(gòu)化的室內(nèi)環(huán)境，可以利用一些簡單的集合模型進行環(huán)境空間的表示，對于室外環(huán)境，可以采用曲面進行環(huán)境空間的表示。Drumhellor利用線段進行地圖構(gòu)造，并利用激光測距儀的測量數(shù)據(jù)進行機器人自身定位。Arras等人用激光測距儀數(shù)據(jù)提取水平直線特征，同時利用視覺系統(tǒng)進行垂直直線特征的提取，最后綜合這些直線進行環(huán)境地圖的構(gòu)造。Roumeliotis等人針對激光測距儀對環(huán)境的檢測數(shù)據(jù)提出了一種分層的路標檢測方法，在底層采用兩個卡爾曼濾波器分別進行環(huán)境拐角和線段等基本特征的檢查，在上層再對底層檢測到的特征進行較為復雜的路標特征合成，比如墻，門，地面等。Luis Pedraza等人提出利用B-樣條的方法進行非結(jié)構(gòu)化環(huán)境中的路標描述，然后運用擴展卡爾曼濾波器進行機器人定位。Luis Pedraza等人另外還提出采用B-樣條曲線進行環(huán)境邊界表示的方法，即BS-SLAM算法，BS-SLAM算法的意義在于它是第一個運用參數(shù)化方法進行環(huán)境曲線特征描述的算法。3. 基于拓撲的地圖表示法利用拓撲地圖進行機器人運行環(huán)境描述最先是由Brooks提出的。拓撲地圖表示法中的地圖是根據(jù)實際環(huán)境結(jié)構(gòu)，由位置節(jié)點和節(jié)點之間的連線組成。其中節(jié)點表示環(huán)境中的特定地點，節(jié)點之間的連線表示的是不同地點之間的路徑信息。拓撲地圖可以組織為層次結(jié)構(gòu)，例如在底層，一個位置可能就是一個房間，但是在更上一層則可能為一個建筑物。為了利用拓撲地圖方法進行環(huán)境地圖的表示，必須有效的識別環(huán)境中各個特定的地點。對結(jié)構(gòu)化的環(huán)境而言，各個特定的地點可以容易進行識別，但是在非結(jié)構(gòu)化的環(huán)境中，節(jié)點的識別會非常復雜。利用拓撲地圖進行環(huán)境表示的關(guān)鍵在于拓撲地圖中各個頂點的選取，David等人提出了一種基于聲納數(shù)據(jù)和視覺信息相結(jié)合的拓撲地圖頂點選擇方法，相對于單獨采用聲納數(shù)據(jù)進行頂點檢測，此方法可以得到更加精確的拓撲地圖描述。Choset等人提出利用通用Voronoi圖（Generalized Voronoi graph，GVG）進行環(huán)境地圖表示的方法，在GVG地圖表示中添加了機器人傳感器的一些度量信息，通過對各個GVG圖的匹配進行機器人自身定位。Ranganathan等人提出利用GVG圖和視覺傳感器檢測到的路標進行地圖表示的方法。另外，為更好地進行環(huán)境地圖模型的描述，研究者還提出了拓撲地圖和尺度地圖混合進行環(huán)境表示的方法，通過在拓撲地圖中加入尺度信息來補償拓撲地圖中的信息丟失。GVG地圖表示法已經(jīng)被用于美國國家航空和宇宙航天局空間站中的機器人三維定位問題研究中，這樣的地圖表示方法具有拓撲地圖的高效性和尺度地圖的一致性以及精確性等特點。4. 基于視圖的地圖表示法在基于視圖的地圖表示方法中，對環(huán)境并不是利用拓撲形式或者路標特征進行表示，而是采用機器人的一系列歷史路徑估計信息進行表示?；谝晥D的地圖表示方法首先是由Lu和Milios提出的，在他們的研究中，環(huán)境地圖是由多次激光掃描數(shù)據(jù)和各個掃描數(shù)據(jù)之間的關(guān)系組成。Fleischer等人提出了基于視覺的視圖地圖表示方法，Malauchlan提出了一種基于批處理和迭代處理的三維環(huán)境視圖表示方法。Eustice等人針對機器人在海底環(huán)境運行的特殊性，提出了一種新的地圖表示方法VAN（Visually Augmented Navigation），在VAN的機器人定位與地圖重建算法中，SLAM的狀態(tài)向量由機器人在不同時刻的位姿組成。Eustice等人還對基于視圖的地圖表示方法中的信息矩陣，稀疏擴展信息濾波器（Sparse Extended Information Filter，SEIF）以及稀疏聯(lián)合樹濾波器（Thin Junction Tree，TJT）中的信息矩陣進行了比較，從比較結(jié)果可以看出，基于視圖的地圖表示方法中的信息矩陣相對于SEIF和TJT中的信息矩陣有兩個優(yōu)點，一是信息矩陣的稀疏性要更好，二是它對應信息矩陣的稀疏性是自然的，并不需要像SEIF那樣經(jīng)過強制的信息丟失處理?；谝晥D的地圖表示法適合于那些并不容易進行特征抽象的復雜地圖環(huán)境。1.1.3 機器人的定位方法在自主移動機器人中，無論是局部實時避障還是全局規(guī)劃，都要精確地獲得機器人及障礙物的當前狀態(tài)，因此對于移動機器人來說，精確地位置估計是實現(xiàn)自主導航的必要內(nèi)容，對移動機器人定位的研究具有非常重要的意義。目前，移動機器人多采用各種機載傳感器進行定位。1 里程計定位在移動機器人的兩個驅(qū)動電機組上均裝有里程計，它們是使用最為廣泛的導航工具之一，能在短距離內(nèi)提供精確地定位，但它的誤差是隨著時間增加而累計增長的，尤其是方向誤差。當用來長距離和長時間定位時，由于誤差的不斷累加，會導致移動機器人在行駛過程中發(fā)生很大的偏離，從而影響定位效果9。盡管有這個缺點，由于其在短距離內(nèi)依然能夠獲得較好的效果。因此絕大多數(shù)的研究者還是認為里程計是機器人導航系統(tǒng)中的一個重要組成部分。2 路標定位基于人工路標、自然路標或者“燈塔”，一般來說，路標都有固定的已知位置，機器人自身的傳感器能對路標進行識別，參照路標進行相對定位。為了使問題簡化，經(jīng)常假設機器人的當前位置和方向角已經(jīng)劃定，機器人只需要在有限的區(qū)域內(nèi)尋找路標。缺點是需要人工干預，在現(xiàn)場設置路標，這是人為對環(huán)境的再結(jié)構(gòu)化，移動機器人的自主性不高。路標對信息的反饋存在一定的誤差，這也影響了機器人對于自身位置的確定。3 視覺定位視覺定位技術(shù)主要是利用攝像機獲取工作空間的圖像，通過圖像處理、模式識別等手段對獲取的圖片進行分析，提取有用的環(huán)境信息，為機器人的定位、路徑規(guī)劃、避障以及創(chuàng)建地圖提供數(shù)據(jù)。其優(yōu)點是具有很高的空間分辨率。探測范圍廣、精度高、能夠獲得場景的大部分信息。缺點是攝像頭在使用過程中容易受到光線和環(huán)境的影響。且獲得的圖像提取和識別選用困難，耗費的計算了很大，實時性也不易滿足。4 GPS定位GPS全球定位技術(shù)是一種應用范圍較廣的絕對定位技術(shù)。它利用環(huán)繞在地球上空的24顆衛(wèi)星準確定位出使用者的準確位置。若是已知地面接收器與三個衛(wèi)星之間的距離，從理論上來講，就可以計算出接收器的經(jīng)緯度和高度，單純利用GPS定位精度還比較低，所以在移動機器人定位和導航中一般還同時融合里程計和其他傳感器的數(shù)據(jù)，其缺點是不適合室內(nèi)環(huán)境、水下環(huán)境以及對精度要求較高的移動機器人定位。5 地圖匹配定位 地圖構(gòu)建和匹配算法是基于地圖匹配定位的關(guān)鍵，首先要獲得機器人工作環(huán)境的全局地圖，在進行地圖匹配定位時，利用把機器人機載傳感器獲取其局部地圖，將局部地圖和先存放在寄存器中的全局地圖做比較，如果是匹配的，那么機器人就能確定在這個環(huán)境中的全局位置和方向。地圖匹配的缺點是它對于機載傳感器獲得的局部地圖要求比較高，地圖上要有足夠多的與全局特征匹配的特征點，而這些特征點在一般相對簡單的環(huán)境里還是比較少的。6 慣性定位慣性定位是一種不依賴于任何外部信息的自主式定位。慣性傳感器是移動機器人常用的一類內(nèi)部傳感器，一般利用陀螺儀計算移動機器人的姿態(tài)，利用加速儀計算出移動機器人的位置，將測量結(jié)果積分合成得到機器人的位姿。這種方式的優(yōu)點是不需要外在的參照物，可以自主定位。缺點是慣性傳感器數(shù)據(jù)會隨著時間產(chǎn)生偏差，每次積分后原先的位置和方向上的微小誤差都會隨之放大，且有誤差累計效應。2 基于粒子濾波器的SLAM算法2.1 SLAM的通用框架和理論模型 圖2-1 SLAM通用框架SLAM問題是把定位和地圖構(gòu)建結(jié)合在一起的，下圖為SLAM的理論模型： 圖2-2 SLAM的理論模型2.2 粒子濾波器定位的基本原理粒子濾波（PF）定位方法用粒子集描述概率分布，而不用概率分布函數(shù)本身表示?？柭鼮V波器（KF）和擴展卡爾曼濾波器（EKF）利用參數(shù)化模型(一個高維的高斯函數(shù))來表示概率密度分布；粒子濾波器采用一系列有限的采樣粒子代表概率密度分布，高概率區(qū)域含有高概率的粒子，反之亦然。粒子濾波以樣本集合的方法逼近概率分布，如果粒子數(shù)趨于無窮大，那么這種非參數(shù)化的粒子分布能夠逼近任何復雜的概率分布。因此，粒子濾波能夠比較精確地表達基于觀測量和控制量的后驗概率分布。它采用一組帶有權(quán)值的隨機樣本來描述概率分布。在概率高的區(qū)域，粒子的密度就大，相反，在概率低的區(qū)域，粒子的密度就小。粒子濾波定位方法優(yōu)點很突出：能夠處理多峰分布；能夠很大地降低內(nèi)存耗費；能夠更精確地定位，且便于實現(xiàn)；不受限于高斯分布，在很多定位問題中，如位姿跟蹤、全局定位甚至綁架問題，都取得較好的應用。粒子濾波方法作為一種基于貝葉斯估計的非線性濾波算法，在處理非高斯非線性時變系統(tǒng)的參數(shù)估計和狀態(tài)濾波問題方面具有獨特的優(yōu)勢。EKF只適用于高斯噪聲模型，對于非線性的模型表現(xiàn)非常差。而粒子濾波則適用于任意噪聲模型。利用EKF只能在濾波誤差和預測誤差很小的情況下通過對系統(tǒng)的非線性進行近似以獲得令人滿意的估計精度，否則濾波初期估計協(xié)方差下降太快會導致濾波不穩(wěn)定甚至發(fā)散。此外EKF還要求噪聲必須服從高斯分布，大大限制它的應用范圍。而PF在測量的基礎(chǔ)上，通過調(diào)節(jié)個粒子的權(quán)值大小和樣本的位置，來近似實際的概率分布，并以樣本的均值作為系統(tǒng)的估計值。因此，PF不受線性化誤差和高斯噪聲假定的限制，適用于任何狀態(tài)轉(zhuǎn)換和觀測模型，有效克服了EKF的缺點。PF也有缺點：相對于EKF，粒子濾波的計算了較大，特別是在采用非常多的粒子數(shù)目時。隨著計算機處理能力的不斷增強，以及各種改進的實時粒子濾波算法不斷涌現(xiàn)，PF算法計算了大等障礙正在逐漸消失；在采用PF算法時，在粒子濾波中，普遍存在的問題可能是存在的退化現(xiàn)象。因為PF抽取N個具有權(quán)值的粒子后，系統(tǒng)如接收到新的觀測數(shù)據(jù)，則實時更新每個粒子的權(quán)值。重要性權(quán)值的分布會變得越來越傾斜，這時有可能出現(xiàn)粒子匱乏現(xiàn)象。即經(jīng)過幾次迭代后，少數(shù)樣本會占去總體樣本的絕大部分權(quán)重，除了一個粒子以外，其余粒子均只有微小的權(quán)值，可忽略不計，這樣使得樣本集不能體現(xiàn)所要表達的后驗概率密度，同樣使得大量計算浪費在幾乎不起任何作用的粒子的更新上。2.3 擴展卡爾曼濾波器算法卡爾曼濾波器是由Kalman在20世紀60年代提出的一種遞推濾波器算法，它不需要保留使用過的測量數(shù)據(jù)，當系統(tǒng)檢測到新的測量數(shù)據(jù)后，可以利用遞推公式獲得新的系統(tǒng)狀態(tài)估計。擴展卡爾曼濾波器是在卡爾曼濾波器的基礎(chǔ)上針對非線性系統(tǒng)輸出估計提出的一種改進方法。在擴展卡爾曼濾波器中，對非線性系統(tǒng)首先采用一級泰勒展開進行線性化近似，然后利用最小均方誤差準則對非線性系統(tǒng)狀態(tài)方程和測量方程的輸出進行估計。在基于擴展卡爾曼濾波器的機器人同步定位與地圖重建（Extended Kalman Filter SLAM，EKF-SLAM）算法中，系統(tǒng)噪聲均被默認為符合高斯分布，因此利用系統(tǒng)狀態(tài)的均值和協(xié)方差進行系統(tǒng)狀態(tài)輸出的描述。在EKF-SLAM中，系統(tǒng)狀態(tài)中包括機器人的位姿和已檢測到所有路標的位置，因此，協(xié)方差矩陣的大小隨著路標數(shù)量成平方比而增大。在EKF-SLAM算法中，根據(jù)機器人的運動狀態(tài)方程，并結(jié)合機器人當前位姿和運動控制輸入利用擴展卡爾曼濾波器對機器人下一時刻的位姿進行預測估計。當機器人傳感器檢測到外部路標時，根據(jù)傳感器測量方程，結(jié)合機器人當前位姿、路標位置以及傳感器的測量信息采用擴展卡爾曼濾波器進行系統(tǒng)狀態(tài)的更新。傳統(tǒng)的EKF-SLAM算法存在如下兩個典型問題，一是計算復雜度，由于在EKF-SLAM中維護著系統(tǒng)狀態(tài)的協(xié)方差，并且在每次接受到傳感器測量數(shù)據(jù)時需要進行系統(tǒng)狀態(tài)協(xié)方差的更新，因此EKF-SLAM算法的計算復雜度為（其中為已檢測到的路標數(shù)目）。二是系統(tǒng)狀態(tài)估計的一致性，由于EKF-SLAM算法中利用擴展卡爾曼濾波器對機器人運動方程和傳感器測量方程的輸出進行估計，然而在擴展卡爾曼濾波器中，對系統(tǒng)輸出采用的是一級泰勒公式展開近似，因此當系統(tǒng)的非線性非常嚴重時，采用擴展卡爾曼濾波器對系統(tǒng)輸出估計的精確性會嚴重降低，甚至導致輸出估計的發(fā)散。針對EKF-SLAM算法計算復雜度大的問題，Newman等人提出了一種基于局部子地圖序列的SLAM算法，它的核心是重建一系列相互獨立而且具有固定大小的局部地圖，然后再利用擴展卡爾曼濾波器將各個局部地圖合并為一個完整的全局地圖，從而保證SLAM過程中的耗時不會隨著檢測到路標數(shù)目的增大而無限增大。Lina等人從機器人位姿估計一致性最大化角度對局部地圖的大小取值進行了分析。Chong等人同樣提出利用子地圖進行環(huán)境地圖描述的方法，他的做法是對子地圖之間的相對位置進行單獨估計，當機器人回到先前的子地圖區(qū)域時，采用搜索算法對機器人進行重新定位。子地圖分割法可以有效降低計算的復雜度，但它帶來的最大問題是數(shù)據(jù)關(guān)聯(lián)的確認上有一定的困難。2.3 粒子重采樣通過增加采樣點數(shù)可以有效地克服退化現(xiàn)象，采樣點數(shù)越小退化現(xiàn)象越嚴重，然而采樣點數(shù)大時又影響計算的實時性?？梢娡ㄟ^增加采樣點數(shù)來解決退化問題是不現(xiàn)實的。目前消除退化問題的兩個關(guān)鍵技術(shù)分別是：選取適當?shù)闹匾芏群瘮?shù)和進行再采樣。粒子濾波的收斂精度和收斂速度直接關(guān)系到它能否實現(xiàn)應用及應用的范圍，是另一個值得重點研究的關(guān)鍵內(nèi)容。為了避免粒子匱乏，Gorden等人提出了重采樣方案，其主要思想史除去那些權(quán)值小的粒子，并復制權(quán)值大的粒子，重采樣通過過濾樣本集中權(quán)值較低的樣本。保留高權(quán)重樣本，使權(quán)重集中在權(quán)重大的樣本上，來達到解決退化問題，一般根據(jù)權(quán)重對后驗概率密度進行離散近似處理。重采樣原理：時刻的先驗概率可由帶有權(quán)值的粒子近似表示。經(jīng)過系統(tǒng)觀測并重新計算權(quán)值，那些權(quán)值大的粒子可以分裂出若干新的粒子，而權(quán)值很小的粒子將被丟棄，如此即可得到一組新的粒子。這些新的粒子加入隨機量后即可預測在t時刻的狀態(tài)，即系統(tǒng)狀態(tài)轉(zhuǎn)移過程。最后再次進入系統(tǒng)觀測過程，并預測下一時刻的狀態(tài)。重采樣階段會造成樣本有效性和多樣性的損失，導致樣本貧化現(xiàn)象。如何保持粒子的有效性和多樣性，克服樣本貧化也是應該考慮的問題之一。2.4 移動機器人SLAM問題描述2.4.1 SLAM計算復雜度SLAM解法在實際應用中需要滿足實時性要求，即傳感器的數(shù)據(jù)采集周期應大于SLAM算法處理一幀傳感器數(shù)據(jù)的時間。全狀態(tài)SLAM算法如EKF-SLAM的計算復雜度與狀態(tài)向量的維數(shù)成指數(shù)關(guān)系，在路標數(shù)目較大時全狀態(tài)SLAM算法不能滿足實時性要求，因而不適合大范圍環(huán)境中的SLAM問題，Guivant等人提出了CEKF-SLAM（Compressed EKF）算法，采用近似的EKF全方差計算方法，將大范圍環(huán)境分成若干局部子區(qū)域，在大部分時間僅用EKF更新狀態(tài)向量中的局部估計，在機器人從一個局部區(qū)域轉(zhuǎn)向另一個局部區(qū)域時對狀態(tài)向量的均值和方差進行全局更新，從而大大提高了計算效率。此外，Leonard等將整個地圖分解為一系列子地圖，然后對每個子地圖進行濾波估計，降低了狀態(tài)向量方差更新過程中的計算復雜度。擴展信息濾波器（Extended Information Filter，EIF）是EKF的對稱形式，它保持了EKF數(shù)學嚴謹、估計準確的特點，并且，其信息矩陣幾乎稀疏，特別適合SLAM問題，Eustice等從稀疏信息矩陣的角度提出了SEIF-SLAM算法，估計精度也得到了保證。Montemerlo等人采用Rao-Blackweuise粒子濾波器解決SLAM問題，并提出了FastSLAM算、法它的計算量與某一時刻觀測到的地圖特征數(shù)目成線性關(guān)系，尤其適用于大范圍環(huán)境中的SLAM問題。2.4.2 SLAM的聯(lián)合估計機器人運動誤差如何干擾傳感器誤差的先后不確定會導致定位和構(gòu)建地圖之間先后的關(guān)系的不確定。機器人運動，位姿估計被運動誤差干擾。環(huán)境特征在全中的位置也基于粒子濾波器的同時定位與地圈構(gòu)件的方法研究被觀測誤差和位姿估計誤差所干擾。然而，與觀測噪聲不同的是，位姿誤差系統(tǒng)性地作用于全局地圖。這個作用也就是，路徑的誤差與地圖的誤差是相互關(guān)聯(lián)的。因此，沒有正確路徑估計的條件下無法估計地圖的誤差。盡管，路徑誤差和地圖誤差的關(guān)系使SLAM問題在變得更加復雜和困難，下面就要講怎樣把這個關(guān)系分解，把SLAM問題轉(zhuǎn)換成一些小的問題的乘積，然后可以用更有效的方法解決這些問題。2.4.3 SLAM的后驗估計表示在機器人的控制過程中會有噪聲干擾，可以把每個控制量或者觀測量以及一些噪聲模型當成一個概率約束，每個控制量概率地約束兩個連續(xù)的機器人位姿，觀測量約束機器人和目標的相對位置，新的觀測量會更新地圖特征和機器人位姿以及以前所觀測的地圖特征，開始，這些約束可能很不確定。然而，隨著地圖的特征被反復的觀測，約束越來越確定。在無限次的觀測和控制后，所有的地圖特征的位置就完全相關(guān)?？梢酝ㄟ^估計算法估計最可能的機器人位姿和地圖。但是這些方法需要處理的是一組變化無常的觀測量和控制量，不適合在線操作，這些算法一般不能估計地圖的不同部分的確定性。最流行的在線解決SLAM問題的方法是：已知傳感器讀數(shù)，對所有可能的地圖和路徑估計后驗概率分布。SLAM算法的一些基本參數(shù)：地圖含有N個特征，記為： （2.1）在t時刻，加入的控制量： （2.2）機器人軌跡： （2.3）在平面環(huán)境中包含三個參數(shù)機器人的坐標和方向角， t時刻以前所有路標的觀測量： （2.4）本文假設每個觀測量提供的信息是一個環(huán)境特征相對于目前位置的確切位置。變量代表了某個被觀察到的環(huán)境特征。實際上，并不會觀察到環(huán)境特征的唯一性，因為很多環(huán)境特征看起來是一樣的。對應觀測量的唯一的環(huán)境特征寫做，相關(guān)性變量： （2.5）表示所有數(shù)據(jù)關(guān)聯(lián)的集合，確定的過程叫數(shù)據(jù)關(guān)聯(lián)，表示在時刻t，第個路標是否被觀測到。路徑信息和地圖信息兩個量都依賴于觀測量，必須把構(gòu)建地圖和定位過程一起考慮。機器人依靠傳感器得到的觀測量、自身的控制量以及相關(guān)性變量來得到路徑信息和地圖信息。解決SLAM問題的核心是在于應該把定位和地圖構(gòu)建視為整體。實際上，這是因為路徑信息和地圖信息兩個量都依賴于觀測量。所以要想找到解決方法，必須把構(gòu)建地圖和定位過程一起考慮。在實際應用中，這些點的環(huán)境特征可以對應傳感器提取特征的位置。SLAM問題的實質(zhì)就是根據(jù)已記錄的從時刻1到t的觀測量、控制信息以及初始狀態(tài)信息來決定所有路標位置集合和機器人位姿。若在t時刻所測得的路標數(shù)已知，那么SLAM問題將變得簡單得多，概率分布描述成在時刻t的路標位置和小車狀態(tài)的聯(lián)合后驗密度函數(shù)，即SLAM被定義為一個關(guān)于路徑和路標的后驗估計： (2.6)如果數(shù)據(jù)關(guān)聯(lián)的集合也已知，那么SLAM后驗概率簡化為： (2.7)SLAM在t時刻的后驗概率可以用時刻后驗概率函數(shù)來遞歸計算，這個遞歸的更新算法，稱為SLAM的貝葉斯濾波器。下面的遞歸公式，也就是貝耶斯濾波器，可以用于已知在的后驗概率情況下，計算時刻的后驗概率： ( 2.8) 是代表概率分布的歸一化因子。一般式中的積分是無法計算的，可以假設后驗概率分布為一個特定形式來計算。機器人在環(huán)境移動時，采集了它自身運動的相關(guān)信息，這個信息可以用機器人輪子上帶的里程計、內(nèi)部的導航單元或通過觀測機器人執(zhí)行的控制命令得到。FastSLAM是RBPF(Rao-Blackwellised Particle Fitering)濾波器的一個實例 ，Murphy等人對動態(tài)貝葉斯網(wǎng)絡（Dynamic Bayesian Networks）進行分析得出結(jié)論：若機器人的路徑估計已知，則陸標估計（,）之間相互獨立。由貝葉斯公式及特征估計之間的獨立性假設，對后驗概率分布分解： (2.9)其中右邊第一部分用粒子濾波器估計，第二部分用擴展卡爾曼濾波器估計，每個粒子都有自己單獨的地圖，在每個粒子中，如果環(huán)境特征的個數(shù)是已知的，那么每個環(huán)境特征的位置都可以用一次EKF獨立的進行估計。假設需要k個粒子實現(xiàn)SLAM，F(xiàn)astSLAM總共有kn個卡爾曼濾波器，粒子濾波器的每個粒子代表機器人的一條可能運動路徑，利用觀測信息計算每個粒子的權(quán)重： (2.10) FastSLAM 的EKF與傳統(tǒng)的EKF相似，它近似線性高斯函數(shù)測量模型，線性高斯測量模型得到的分布 是高斯分布的，即使動作模型不是線性的，這也是對機器人姿態(tài)使用采樣來近似分布的結(jié)果。2.4.4 SLAM公式推導SLAM分解可以從SLAM的路徑后驗概率中直接推導。用條件概率的定義，SLAM后驗概率可以寫成： (2.11)因此，要得到分解的后驗概率，對所有的非負值t需要以下方程： (2.12)這個論斷的證據(jù)可以從歸納中得到。要得到這個結(jié)果，需要兩個中間結(jié)果第一個量是觀測到的環(huán)境特征的概率。這個量可以用貝耶斯定理寫成： (2.13)目前的觀測量僅僅與機器人的狀態(tài)和被觀測到的環(huán)境特征有關(guān)。在（2.13）最右邊的項，在沒有新的觀測量的情況下，當前的位姿、當前的控制量、當前的數(shù)據(jù)關(guān)聯(lián)對沒有影響。因此，所有這些變量可以省去得到： (2.14)需要的第二個中間結(jié)果是，沒有觀測到的任意環(huán)境特征的概率這很簡單，如果沒有觀測到環(huán)境特征，環(huán)境特征后驗概率不變。因此，環(huán)境特征的后驗概率在時刻等于時刻。首先，在時刻做歸納假設： (2.15)歸納的開始情況,沒有加入觀測量。因此，分解因式是對的。在,再一次用貝耶斯定理來擴展（2.12）的左邊可以得到： (2.16)等于單獨的環(huán)境特征后驗概率的乘積。 (2.17)2.4.5 有效的數(shù)據(jù)關(guān)聯(lián)在實際的SLAM應用中，很少能觀測到數(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)常用負的對數(shù)似然方程表示，如下： (2.19)方程的第二部分就是Mahalanobis距離：通過觀測量和環(huán)境特征估計的協(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é)果地圖的精確性會有影響。如果機器人的傳感器噪聲嚴重的話，很容易產(chǎn)生這種數(shù)據(jù)關(guān)聯(lián)的不確定性。解決這個問題的一個方法是只加入那些得到明確數(shù)據(jù)關(guān)聯(lián)的觀測量。然而，如果SLAM環(huán)境是有噪聲的，大多數(shù)的觀測量得不到處理。而且，沒有加入觀測量會導致高估環(huán)境特征的協(xié)方差，使以后的數(shù)據(jù)關(guān)聯(lián)變得更加不確定。2.4.6 FastSLAM的粒子表示形式在FastSLAM中，路標估計利用獨立的EKF實現(xiàn)，一個路標對應一個EKF濾波器，地圖是一組N個2維的高斯模型，而不是一個單獨的聯(lián)合2*N維高斯模型。共有M個粒子濾波器用來估計路徑，每個粒子含有N個獨立的EKF濾波器，用來估計N個路標，第i個粒子表示： （2.20）其中包含一個路徑和基于該路徑上N個路標高斯估計，第N個路標的高斯表征的均值和方差分別是和，如圖2-3所示： 圖2-3 FastSLAM的粒子表示2.4.7 FastSLAM的計算時間復雜度FastSLAM有機地將粒子濾波器與卡爾曼濾波器集成在一起，魯棒性地解決數(shù)據(jù)關(guān)聯(lián)和多目標跟蹤問題，F(xiàn)astSLAM算法將狀態(tài)估計分解成了采樣部分和解析部分。由于這種分解，其計算復雜度僅為O，其中N,M分別為FastSLAM算法所用的粒子數(shù)和未知環(huán)境中的特征個數(shù),每個粒子中的高斯組用穩(wěn)定二叉樹表示，高斯參數(shù)和在樹的葉子上，訪問每個高斯模型所需要的時間與K成對數(shù)增長關(guān)系。在復制粒子時，所有高斯模型樹中只有一個路徑必須被修改，為了完成這個樹，對于所有與之分開的枝干，所有的相關(guān)點從生成的粒子復制過來。所有與之分開的枝干就指向了與生成樹同樣的子樹，生成一個這樣的不完全的書只用了K對數(shù)時間，此外，由于需要用來達到葉子的步驟數(shù)與路徑的長度相等，訪問高斯模型也需要K對時間。在每個更新步驟需要創(chuàng)建M個新粒子，所以更新總共需要0時間。FastSLAM算法是基于粒子濾波的，它對于機器人位姿和數(shù)據(jù)融合采樣，在每個粒子的基礎(chǔ)上計算路標的位置，用一些平衡的二叉樹來表示路標，使得更新路標只需要路標數(shù)的對數(shù)次，每個粒子對位姿的不確定性是來自數(shù)據(jù)融合中，能更清楚的在噪聲環(huán)境中區(qū)分出來路標，甚至能在很簡單的數(shù)據(jù)融合條件下進行更新路標。 圖2-4 從上一時刻復制和更新樹的過程圖3 模型建立3.1 運動模型在SLAM中有兩個系統(tǒng)模型：觀測模型和運動模型。運動模型描述了t時刻機器人位姿的概率分布。機器人運動模型如圖3.1所示： 圖3.1 機器人運動模型機器人的運動模型見式： （3.1）其中：、分別為時刻第i個粒子所表征的機器人質(zhì)心坐標和方向角；為時刻到t時刻的時間間隔；、分別是機器人的速度和角速度；、分別為相應的噪聲項。3.2 觀測模型 觀測模型即描述了當機器人位姿為時，觀測到的信息為的概率。其中：為時刻里程計給出的控制信息。和分別表示時刻機器人的速度和角速度。機器人利用自身配備的距離/方向傳感器探測陸標，得到陸標的距離和方向角。其觀測模型見式： （3.2） 其中：分別是機器人的位置；是機器人的方向角；、分別是對應于測量距離和方向的噪聲。機器人觀測模型如圖3.2所示： 圖3.2 機器人觀測模型 4 FastSLAM算法步驟4.1 FastSLAM算法步驟基于粒子濾波器和EKF的FastSLAM算法如下：1 根據(jù)初始坐標，隨機生成個粒子、粒子權(quán)值組成的粒子集，見式 、： （4.1） （4.2） （4.3）2 預測機器人位姿 ，根據(jù)時刻粒子集中的每個粒子及運動模型，計算時刻粒子。3 計算權(quán)重，重要性權(quán)值計算方法見式 （4.4）4 根據(jù)觀測模型計算粒子對應的特征點坐標，并將其與環(huán)境陸標集中的某一陸標進行關(guān)聯(lián)。5 對粒子權(quán)值進行更新，見式: （4.5）式中，函數(shù)G為高斯概率計算公式，可根據(jù)對粒子集權(quán)值進行更新。6 歸一化見式： （4.6）7 計算有效粒子數(shù)，設定一個閾值，若有效粒子數(shù)小于這個閾值，根據(jù)粒子權(quán)值對粒子進行重采樣 ，去除權(quán)值小的粒子，復制權(quán)值大的粒子。8 計算權(quán)重。9 環(huán)境地圖更新，利用對進行更新。10 通過公式計算粒子k時刻的位姿，若還要計算下一時刻的位姿，則轉(zhuǎn)到步驟；否則結(jié)束定位。4.2 新位姿采樣粒子集是遞增更新的，是根據(jù)時刻的粒子和路標觀測量，控制量得來的。機器人的新位置是由上一個位置和控制輸入推算得出的，然后結(jié)合新的觀測值來更新路標的位置。FastSLAM對上一時刻的路徑和最近的控制量進行采樣,t時刻中的每個粒子生成由可能性運動模型取樣中獲得的一個對機器人姿態(tài)估計的可能性估計，見式： （4.7） 然后將這種估計連同路徑加入到一個臨時粒子組中，假設中的粒子組根據(jù)來分配，則新的粒子也根據(jù)分配。通過這種方法生產(chǎn)M個粒子后，通過對臨時粒子組取樣獲得新組。每個粒子通過權(quán)重因數(shù)更新獲得。4.3 環(huán)境特征估計的更新環(huán)境特征估計是以機器人的位姿為條件的，個EKF加到集合的每個粒子上。第個環(huán)境特征的后驗概率很容易得到。它的計算是要看是否，也就是，是否環(huán)境特征在時刻被觀察到。對于被觀察到的環(huán)境特征，根據(jù)通常用貝耶斯定理的分解后驗概率的程序。 （4.8） （4.9）為第m個粒子的觀測量概率函數(shù)的倒數(shù)。 若，則簡單地使用高斯公式不變： （4.10） t時刻的均值和協(xié)方差矩陣更新： （4.11） （4.12）