移動(dòng)機(jī)器人fastslam算法研究畢業(yè)設(shè)計(jì)(存儲(chǔ)版)

　　

【正文】 境特征和運(yùn)動(dòng)控制信息。分解成一些低維數(shù)問(wèn)題的集合，粒子濾波器SLAM對(duì)于機(jī)器入的路徑和數(shù)據(jù)關(guān)聯(lián)的采樣，以每個(gè)粒子為條件計(jì)算獨(dú)立的環(huán)境特征估計(jì)。2）研究更有效的自適應(yīng)采樣策略，進(jìn)一步提高粒子濾波算法的性能。他們的支持與情感，是我永遠(yuǎn)的財(cái)富。感謝我的室友們，他們?yōu)槲业膶W(xué)習(xí)和生活提供了良好環(huán)境，同時(shí)給與我很多支持和鼓勵(lì)。機(jī)器人領(lǐng)域的研究者對(duì)SLAM進(jìn)行了大量的研究，特別是降低復(fù)雜度，提高魯棒性等方面取得了很大的進(jìn)展。7 總結(jié)和展望 本文介紹了解決SLAM問(wèn)題的方法：粒子濾波器SLAM?，F(xiàn)在MATLAB軟件不但廣泛應(yīng)用于控制領(lǐng)域，也應(yīng)用于其它的工程和非工程領(lǐng)域。它的計(jì)算是要看是否，也就是，是否環(huán)境特征在時(shí)刻被觀察到。10　 通過(guò)公式計(jì)算粒子k時(shí)刻的位姿，若還要計(jì)算下一時(shí)刻的位姿，則轉(zhuǎn)到步驟?；否則結(jié)束定位。機(jī)器人利用自身配備的距離/方向傳感器探測(cè)陸標(biāo)，得到陸標(biāo)的距離和方向角。在每個(gè)更新步驟需要?jiǎng)?chuàng)建M個(gè)新粒子，所以更新總共需要0時(shí)間。解決這個(gè)問(wèn)題的一個(gè)方法是只加入那些得到明確數(shù)據(jù)關(guān)聯(lián)的觀測(cè)量。在EKF的情況下，觀測(cè)量的概率可以寫成觀測(cè)量和期望觀測(cè)量的差值的函數(shù)。首先，在時(shí)刻做歸納假設(shè)： ()歸納的開(kāi)始情況,沒(méi)有加入觀測(cè)量。假設(shè)需要k個(gè)粒子實(shí)現(xiàn)SLAM，F(xiàn)astSLAM總共有kn個(gè)卡爾曼濾波器，粒子濾波器的每個(gè)粒子代表機(jī)器人的一條可能運(yùn)動(dòng)路徑，利用觀測(cè)信息計(jì)算每個(gè)粒子的權(quán)重： () FastSLAM 的EKF與傳統(tǒng)的EKF相似，它近似線性高斯函數(shù)測(cè)量模型，線性高斯測(cè)量模型得到的分布 是高斯分布的，即使動(dòng)作模型不是線性的，這也是對(duì)機(jī)器人姿態(tài)使用采樣來(lái)近似分布的結(jié)果。在實(shí)際應(yīng)用中，這些點(diǎn)的環(huán)境特征可以對(duì)應(yīng)傳感器提取特征的位置。變量代表了某個(gè)被觀察到的環(huán)境特征。盡管，路徑誤差和地圖誤差的關(guān)系使SLAM問(wèn)題在變得更加復(fù)雜和困難，下面就要講怎樣把這個(gè)關(guān)系分解，把SLAM問(wèn)題轉(zhuǎn)換成一些小的問(wèn)題的乘積，然后可以用更有效的方法解決這些問(wèn)題。擴(kuò)展信息濾波器（Extended Information Filter，EIF）是EKF的對(duì)稱形式，它保持了EKF數(shù)學(xué)嚴(yán)謹(jǐn)、估計(jì)準(zhǔn)確的特點(diǎn)，并且，其信息矩陣幾乎稀疏，特別適合SLAM問(wèn)題，Eustice等從稀疏信息矩陣的角度提出了SEIFSLAM算法，估計(jì)精度也得到了保證。經(jīng)過(guò)系統(tǒng)觀測(cè)并重新計(jì)算權(quán)值，那些權(quán)值大的粒子可以分裂出若干新的粒子，而權(quán)值很小的粒子將被丟棄，如此即可得到一組新的粒子。子地圖分割法可以有效降低計(jì)算的復(fù)雜度，但它帶來(lái)的最大問(wèn)題是數(shù)據(jù)關(guān)聯(lián)的確認(rèn)上有一定的困難。在EKFSLAM中，系統(tǒng)狀態(tài)中包括機(jī)器人的位姿和已檢測(cè)到所有路標(biāo)的位置，因此，協(xié)方差矩陣的大小隨著路標(biāo)數(shù)量成平方比而增大。隨著計(jì)算機(jī)處理能力的不斷增強(qiáng)，以及各種改進(jìn)的實(shí)時(shí)粒子濾波算法不斷涌現(xiàn)，PF算法計(jì)算了大等障礙正在逐漸消失；在采用PF算法時(shí)，在粒子濾波中，普遍存在的問(wèn)題可能是存在的退化現(xiàn)象[]。粒子濾波方法作為一種基于貝葉斯估計(jì)的非線性濾波算法，在處理非高斯非線性時(shí)變系統(tǒng)的參數(shù)估計(jì)和狀態(tài)濾波問(wèn)題方面具有獨(dú)特的優(yōu)勢(shì)。缺點(diǎn)是慣性傳感器數(shù)據(jù)會(huì)隨著時(shí)間產(chǎn)生偏差，每次積分后原先的位置和方向上的微小誤差都會(huì)隨之放大，且有誤差累計(jì)效應(yīng)。4． GPS定位GPS全球定位技術(shù)是一種應(yīng)用范圍較廣的絕對(duì)定位技術(shù)。為了使問(wèn)題簡(jiǎn)化，經(jīng)常假設(shè)機(jī)器人的當(dāng)前位置和方向角已經(jīng)劃定，機(jī)器人只需要在有限的區(qū)域內(nèi)尋找路標(biāo)。基于視圖的地圖表示法適合于那些并不容易進(jìn)行特征抽象的復(fù)雜地圖環(huán)境。Ranganathan等人提出利用GVG圖和視覺(jué)傳感器檢測(cè)到的路標(biāo)進(jìn)行地圖表示的方法。3. 基于拓?fù)涞牡貓D表示法利用拓?fù)涞貓D進(jìn)行機(jī)器人運(yùn)行環(huán)境描述最先是由Brooks提出的。此種地圖表示方法局限于可以進(jìn)行參數(shù)化的路標(biāo)環(huán)境，或者是可建模的對(duì)象環(huán)境，比如點(diǎn)、線、面等。但是環(huán)境空間的分辨率和柵格大小相關(guān)，當(dāng)需要增加分辨率時(shí)就必須縮小柵格的大小，進(jìn)而增加了運(yùn)算的時(shí)間和空間復(fù)雜度。移動(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)一步完善和更新地圖。我國(guó)也針對(duì)這一趨勢(shì)制定了以月球?yàn)榻谀繕?biāo)的空間探測(cè)計(jì)劃。1969年，Nilsson等人在斯坦福機(jī)器人研究所（SRI）研制了第一個(gè)智能移動(dòng)機(jī)器人Shakey，它裝備了攝像機(jī)、測(cè)距儀和接觸傳感器等設(shè)備，并通過(guò)無(wú)線連接與DEC．PDP 10計(jì)算機(jī)相連，其任務(wù)是自動(dòng)避障和運(yùn)動(dòng)目標(biāo)跟蹤。SLAM(Simultaneous Localization And Mapping）問(wèn)題是指把移動(dòng)機(jī)器人放在未知環(huán)境中，機(jī)器人增量式地創(chuàng)建未知環(huán)境的連續(xù)地圖，同時(shí)確定它在地圖中的位置，由于SLAM問(wèn)題的解法使機(jī)器人實(shí)現(xiàn)了真正的自主導(dǎo)航，所以在過(guò)去幾十年中逐漸成為了移動(dòng)機(jī)器人領(lǐng)域的研究熱點(diǎn)。民用方面可以用于自主行駛車輛或者車輛輔助駕駛系統(tǒng)，還有服務(wù)機(jī)器人，智能機(jī)器人，智能玩具等。SLAM問(wèn)題研究是實(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ā)展都投入了大量的研究。SLAM也稱CML(Concurrent Mapping and Localization),最初于上世界80年代早期由Smith Self和Cheeseman給出了基礎(chǔ)概率方法的解決方案[]， SLAM 問(wèn)題的解決方法大致分為兩類:基于概率估計(jì)的方法和非概率估計(jì)的方法。目前，移動(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ó)無(wú)人車輛協(xié)會(huì)（the Joint of Unmanned Vehicle）組織開(kāi)展的自主式航空飛行器的研究，世界著名移動(dòng)機(jī)器人制造商IROBOT公司的金字塔探索者Pyramid Rover、月球車，以及卡耐基梅隆大學(xué)（CMU）研制的CMURover，日本Tsukuba大學(xué)的Yamabiko和MELDOG，法國(guó)系統(tǒng)分析與結(jié)構(gòu)實(shí)驗(yàn)室（LAAS）的Hilare2，德國(guó)Karlsruhe大學(xué)的KAMRO等。以中科院自動(dòng)化研究所、清華大學(xué)、國(guó)防科技大學(xué)、東北大學(xué)、上海交通大學(xué)、哈爾濱工業(yè)大學(xué)為代表的我國(guó)機(jī)器人研究基地，已經(jīng)取得了令人矚目的成果。目前地圖創(chuàng)建方法很多大致可以歸為四類:柵格表示法、幾何信息表示法、拓?fù)鋱D表示法和混合表示法。在Bert等人的研究中，對(duì)環(huán)境采用全局柵格和局部柵格結(jié)合的方法進(jìn)行建模，同時(shí)利用霍夫變換對(duì)柵格地圖中的線段進(jìn)行抽象描述，并采用卡爾曼濾波器進(jìn)行位置估計(jì)。Drumhellor利用線段進(jìn)行地圖構(gòu)造，并利用激光測(cè)距儀的測(cè)量數(shù)據(jù)進(jìn)行機(jī)器人自身定位。拓?fù)涞貓D可以組織為層次結(jié)構(gòu)，例如在底層，一個(gè)位置可能就是一個(gè)房間，但是在更上一層則可能為一個(gè)建筑物。4. 基于視圖的地圖表示法在基于視圖的地圖表示方法中，對(duì)環(huán)境并不是利用拓?fù)湫问交蛘呗窐?biāo)特征進(jìn)行表示，而是采用機(jī)器人的一系列歷史路徑估計(jì)信息進(jìn)行表示。1． 里程計(jì)定位在移動(dòng)機(jī)器人的兩個(gè)驅(qū)動(dòng)電機(jī)組上均裝有里程計(jì)，它們是使用最為廣泛的導(dǎo)航工具之一，能在短距離內(nèi)提供精確地定位，但它的誤差是隨著時(shí)間增加而累計(jì)增長(zhǎng)的，尤其是方向誤差。3． 視覺(jué)定位視覺(jué)定位技術(shù)主要是利用攝像機(jī)獲取工作空間的圖像，通過(guò)圖像處理、模式識(shí)別等手段對(duì)獲取的圖片進(jìn)行分析，提取有用的環(huán)境信息，為機(jī)器人的定位、路徑規(guī)劃、避障以及創(chuàng)建地圖提供數(shù)據(jù)。5． 地圖匹配定位 地圖構(gòu)建和匹配算法是基于地圖匹配定位的關(guān)鍵，首先要獲得機(jī)器人工作環(huán)境的全局地圖，在進(jìn)行地圖匹配定位時(shí)，利用把機(jī)器人機(jī)載傳感器獲取其局部地圖，將局部地圖和先存放在寄存器中的全局地圖做比較，如果是匹配的，那么機(jī)器人就能確定在這個(gè)環(huán)境中的全局位置和方向。粒子濾波以樣本集合的方法逼近概率分布，如果粒子數(shù)趨于無(wú)窮大，那么這種非參數(shù)化的粒子分布能夠逼近任何復(fù)雜的概率分布。利用EKF只能在濾波誤差和預(yù)測(cè)誤差很小的情況下通過(guò)對(duì)系統(tǒng)的非線性進(jìn)行近似以獲得令人滿意的估計(jì)精度，否則濾波初期估計(jì)協(xié)方差下降太快會(huì)導(dǎo)致濾波不穩(wěn)定甚至發(fā)散。即經(jīng)過(guò)幾次迭代后，少數(shù)樣本會(huì)占去總體樣本的絕大部分權(quán)重，除了一個(gè)粒子以外，其余粒子均只有微小的權(quán)值，可忽略不計(jì)，這樣使得樣本集不能體現(xiàn)所要表達(dá)的后驗(yàn)概率密度，同樣使得大量計(jì)算浪費(fèi)在幾乎不起任何作用的粒子的更新上。傳統(tǒng)的EKFSLAM算法存在如下兩個(gè)典型問(wèn)題，一是計(jì)算復(fù)雜度，由于在EKFSLAM中維護(hù)著系統(tǒng)狀態(tài)的協(xié)方差，并且在每次接受到傳感器測(cè)量數(shù)據(jù)時(shí)需要進(jìn)行系統(tǒng)狀態(tài)協(xié)方差的更新，因此EKFSLAM算法的計(jì)算復(fù)雜度為（其中為已檢測(cè)到的路標(biāo)數(shù)目）。目前消除退化問(wèn)題的兩個(gè)關(guān)鍵技術(shù)分別是：選取適當(dāng)?shù)闹匾芏群瘮?shù)和進(jìn)行再采樣。重采樣階段會(huì)造成樣本有效性和多樣性的損失，導(dǎo)致樣本貧化現(xiàn)象。機(jī)器人運(yùn)動(dòng)，位姿估計(jì)被運(yùn)動(dòng)誤差干擾。在無(wú)限次的觀測(cè)和控制后，所有的地圖特征的位置就完全相關(guān)。路徑信息和地圖信息兩個(gè)量都依賴于觀測(cè)量，必須把構(gòu)建地圖和定位過(guò)程一起考慮。下面的遞歸公式，也就是貝耶斯濾波器，可以用于已知在的后驗(yàn)概率情況下，計(jì)算時(shí)刻的后驗(yàn)概率： ( ) 是代表概率分布的歸一化因子。要得到這個(gè)結(jié)果，需要兩個(gè)中間結(jié)果．第一個(gè)量是觀測(cè)到的環(huán)境特征的概率。 () 有效的數(shù)據(jù)關(guān)聯(lián)在實(shí)際的SLAM應(yīng)用中，很少能觀測(cè)到數(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í)候。 FastSLAM的粒子表示形式在FastSLAM中，路標(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維高斯模型。運(yùn)動(dòng)模型描述了t時(shí)刻機(jī)器人位姿的概率分布。3　 計(jì)算權(quán)重，重要性權(quán)值計(jì)算方法見(jiàn)式 （）4　 根據(jù)觀測(cè)模型計(jì)算粒子對(duì)應(yīng)的特征點(diǎn)坐標(biāo)，并將其與環(huán)境陸標(biāo)集中的某一陸標(biāo)進(jìn)行關(guān)聯(lián)。FastSLAM對(duì)上一時(shí)刻的路徑和最近的控制量進(jìn)行采樣,t時(shí)刻中的每個(gè)粒子生成由可能性運(yùn)動(dòng)模型取樣中獲得的一個(gè)對(duì)機(jī)器人姿態(tài)估計(jì)的可能性估計(jì)，見(jiàn)式[]： （） 然后將這種估計(jì)連同路徑加入到一個(gè)臨時(shí)粒子組中，假設(shè)中的粒子組根據(jù)來(lái)分配，則新的粒子也根據(jù)分配。 若，則簡(jiǎn)單地使用高斯公式不變[]： （） t時(shí)刻的均值和協(xié)方差矩陣更新： （） （） （）