機(jī)器人地圖創(chuàng)建方法研究

本科學(xué)生畢業(yè)論文論文題目：機(jī)器人地圖創(chuàng)立方法研究學(xué)院：電子工程學(xué)院年級(jí)：2007級(jí)專業(yè)：自動(dòng)化姓名：關(guān)妍妍學(xué)號(hào)：20076208指導(dǎo)教師：王欣2010年5月16日摘要移動(dòng)機(jī)器人同時(shí)定位和地圖創(chuàng)立是實(shí)現(xiàn)移動(dòng)機(jī)器人完全自主導(dǎo)航的關(guān)鍵。自主移動(dòng)機(jī)器人在未知環(huán)境下作業(yè)時(shí)，首先要解決的根本問題就是其自身的定位問題，而定位問題與環(huán)境地圖的創(chuàng)立又是相輔相成的。本文從相關(guān)理論和關(guān)鍵技術(shù)等方面，系統(tǒng)地總結(jié)了同步自定位和地圖創(chuàng)立的研究現(xiàn)狀，從環(huán)境特征提取、定位與地圖創(chuàng)立、數(shù)據(jù)相關(guān)等多個(gè)方面對(duì)移動(dòng)機(jī)器人即時(shí)定位與地圖創(chuàng)立問題進(jìn)行了綜述。著重介紹了基于EKF模型的SLAM算法、基于尺度不變特征變換算法和基于概率論的方法，分析了目前存在的難題，并指出了未來研究的開展方向。關(guān)鍵詞移動(dòng)機(jī)器人；特征提??；地圖創(chuàng)立；數(shù)據(jù)相關(guān)；EKFAbstractThemobilerobotsimultaneouslocalizationandmappingiscriticaltorealizingthefullyautonomousnavigation.Whenautonomousmobilerobotsoperateinanuncertainenvironment.oneofthemostfundamentaltasksistolocalizeitself.Thelocationisassociatedwithmapbuilding.Basedonrelatedtheoriesandtechnologies,Thispapersummarizestheachievementsinsimultaneouslocalizationandmappingsystematically,andthepapersummarizesthecurrentresearchonSLAMinsomeaspects,suchasfeatureextraction,methodsanddataroducesSLAMalgorithmsbasedonEKFmodel,scaleconstantfeaturetransformationandprobabilisticapproachindetail,analyzesthecurrentopenissuesandindicatestheprospectiveresearchdirectioninthefuturedevelopment.KeywordsMobilerobot；Featureextraction；Filter；Dataassociation；EKF目錄摘要IAbstractII第一章緒論11.1機(jī)器人同時(shí)定位與地圖創(chuàng)立(SLAM)概述1地圖的表示方法2特征提取3機(jī)器人地圖創(chuàng)立的研究及開展趨勢(shì)4第二章地圖創(chuàng)立的一些關(guān)鍵性問題5不確定性信息處理方法5數(shù)據(jù)關(guān)聯(lián)5累計(jì)誤差6算法復(fù)雜度72.5機(jī)器人地圖創(chuàng)立的導(dǎo)航問題7探索規(guī)劃8第三章移動(dòng)機(jī)器人地圖創(chuàng)立方法9基于卡爾曼濾波(KF)的SLAM算法9算法根本原理9擴(kuò)展卡爾曼濾波(EKF)10仿真實(shí)驗(yàn)11基于尺度不變特征變換(SIFT)算法12移動(dòng)機(jī)器人模型12估計(jì)與更新方程143.2.3SIFT算法15實(shí)驗(yàn)結(jié)果15基于概率的SLAM算法163.3.1SLAM問題的概率描述16基于概率論的SLAM17稀疏擴(kuò)展信息濾波18FastSLAM算法19結(jié)論20參考文獻(xiàn)21致謝23第一章緒論1.1機(jī)器人同時(shí)定位與地圖創(chuàng)立(SLAM)概述在完全未知的環(huán)境中由機(jī)器人自主地依靠其自身攜帶的傳感器提供的信息建立環(huán)境模型正成為自主移動(dòng)機(jī)器人研究中的一個(gè)熱點(diǎn)問題。為了能夠有效地探索未知區(qū)域并完成給定的任務(wù)，機(jī)器人需要自主創(chuàng)立地圖的能力。下面對(duì)這個(gè)問題作些詳細(xì)的解釋：完全未知環(huán)境，即機(jī)器人對(duì)環(huán)境一無所知，不存在任何先驗(yàn)信息，包括環(huán)境大小、形狀、障礙物位置等等，且環(huán)境中不存在諸如路標(biāo)、燈塔等人為設(shè)定的參照物。在這種未知環(huán)境中，機(jī)器人創(chuàng)立地圖行為的完成必須依賴于其傳感器所獲得的信息，如里程儀、聲納、激光測(cè)距儀、視覺等等。由于傳感器自身的限制，感知信息存在不同程度的不確定性。直接使用感知信息進(jìn)行地圖創(chuàng)立很難得到準(zhǔn)確的環(huán)境模型，因此通常需要對(duì)感知信息再處理，通過多感知信息的融合獲得較為準(zhǔn)確的環(huán)境信息。機(jī)器人根據(jù)在運(yùn)動(dòng)過程中獲取的傳感器數(shù)據(jù)，利用相關(guān)地圖模型和集成算法，生成關(guān)于環(huán)境的地圖表示。對(duì)于環(huán)境中的機(jī)器人自主定位和機(jī)器人位置的地圖創(chuàng)立已經(jīng)有了一些實(shí)用的解決方法，然而在很多環(huán)境中機(jī)器人不能利用全局定位系統(tǒng)進(jìn)行定位，而且事先獲取機(jī)器人工作環(huán)境的地圖很困難，甚至是不可能的。這時(shí)機(jī)器人需要在自身位置不確定的條件下，在完全未知環(huán)境中創(chuàng)立地圖，同時(shí)利用地圖進(jìn)行自主定位和導(dǎo)航，這就是移動(dòng)機(jī)器人的同時(shí)定位與地圖創(chuàng)立(SLAM)問題。定位問題是移動(dòng)機(jī)器人的根本問題，是指移動(dòng)機(jī)器人通過對(duì)內(nèi)部狀態(tài)的檢測(cè)或?qū)ν獠凯h(huán)境的感知估算自身位置和姿態(tài)的過程。移動(dòng)機(jī)器人地圖創(chuàng)立問題描述如下：移動(dòng)機(jī)器人從初始位置，經(jīng)過一系列的位置并且在每一個(gè)位置獲得傳感器對(duì)環(huán)境的感知信息，移動(dòng)機(jī)器人的目標(biāo)是處理這些傳感器數(shù)據(jù),確定機(jī)器人的位姿，并且同時(shí)創(chuàng)立環(huán)境地圖。要想開發(fā)一個(gè)有效的SLAM系統(tǒng)，關(guān)鍵是選擇好環(huán)境的描述方法。目前，應(yīng)用衛(wèi)星全球定位系統(tǒng)等裝置進(jìn)行室外機(jī)器人的定位已經(jīng)取得成功，但由于環(huán)境、任務(wù)等因素移動(dòng)機(jī)器人大多采用機(jī)載傳感器定位。它主要由位姿跟蹤、地圖獲取組成。位姿跟蹤指機(jī)器人在走行中對(duì)其位置和姿態(tài)的估計(jì)過程。SLAM方法在過去的十幾年中逐漸成為機(jī)器人導(dǎo)航問題的研究熱點(diǎn)，吸引了大量的研究人員，并取得了很多實(shí)用性的成果，是否具備并發(fā)建圖與定位的能力被許多人認(rèn)為是機(jī)器人是否能夠?qū)崿F(xiàn)自主的關(guān)鍵的前提條件。SmithR、SelfM和CheesemanP于1986年提出基于EKF(ExtendedKalmanFilter)StochasticMapping方法，揭開了SLAM研究的序幕。在以后的十幾年中,研究范圍不斷擴(kuò)大，從有人工路標(biāo)到完全自主、從戶內(nèi)到戶外，出現(xiàn)許多SLAM方法。由于自主機(jī)器人固有的特點(diǎn)〔缺乏自身位置和環(huán)境的先驗(yàn)信息；靠外部和內(nèi)部傳感器獲得知識(shí)；環(huán)境、傳感器信息及機(jī)器人運(yùn)動(dòng)本身具有不確定性〕，各種方法歸納起來都是一個(gè)“估計(jì)—校正”的過程，必須解決像如何估計(jì)機(jī)器人及環(huán)境特征狀態(tài)，如何校正和更新地圖、地圖如何表示、不確定性如何處理等問題。地圖的表示方法機(jī)器人地圖可以分為兩大類：幾何地圖和拓?fù)涞貓D。幾何地圖又可分為柵格地圖和特征地圖?；跂鸥竦牡貓D表示方法[1]即將整個(gè)環(huán)境分為假設(shè)干相同大小的柵格，對(duì)于每個(gè)柵格指出其中是否存在障礙物。這種方法已經(jīng)在許多機(jī)器人系統(tǒng)中得到應(yīng)用，是使用較為成功的一種方法。柵格地圖的缺點(diǎn)是：當(dāng)柵格數(shù)量增大時(shí)〔在大規(guī)模環(huán)境或?qū)Νh(huán)境劃分比擬詳細(xì)時(shí)〕，對(duì)于地圖的維護(hù)所占用的內(nèi)存和CPU時(shí)間迅速增長，使計(jì)算機(jī)的實(shí)時(shí)處理變得很困難。柵格地圖是一個(gè)近似的解決方案，但其對(duì)特定感知系統(tǒng)的假設(shè)參數(shù)不敏感，具有較強(qiáng)的魯棒性?；趲缀翁卣鞯牡貓D表示方法[2,3]是指機(jī)器人收集對(duì)環(huán)境的感知信息，從中提取更為抽象的幾何特征，例如線段或曲線，使用這些幾何信息描述環(huán)境。這種表示方法更為緊湊，且便于位置估計(jì)和目標(biāo)識(shí)別。幾何特征的提取需要對(duì)感知信息作額外的處理，且需要一定數(shù)量的感知數(shù)據(jù)才能得到結(jié)果。特征地圖由一系列包含位置信息的特征組成。拓?fù)鋱D也是一種緊湊的地圖表示方法[4]，特別在環(huán)境大而簡單時(shí)。這種方法將環(huán)境表示為一張拓?fù)鋱D(graph)，圖中的節(jié)點(diǎn)對(duì)應(yīng)于環(huán)境中的一個(gè)特征狀態(tài)、地點(diǎn)〔由感知決定〕，如果節(jié)點(diǎn)間存在直接連接的路徑那么相當(dāng)于圖中連接節(jié)點(diǎn)的弧。這種表示方法可以實(shí)現(xiàn)快速的路徑規(guī)劃。由于拓?fù)鋱D通常不需要機(jī)器人準(zhǔn)確的位置信息，對(duì)于機(jī)器人的位置誤差也就有了更好的魯棒性。但當(dāng)環(huán)境中存在兩個(gè)很相似的地方時(shí)，拓?fù)鋱D的方法將很難確定這是否為同一節(jié)點(diǎn)〔特別是機(jī)器人從不同的路徑到達(dá)這些節(jié)點(diǎn)時(shí)〕。拓?fù)涞貓D由環(huán)境中特征位置或區(qū)域組成的節(jié)點(diǎn)及其連接關(guān)系成，根據(jù)連接關(guān)系信息，移動(dòng)機(jī)器人可從一個(gè)節(jié)點(diǎn)區(qū)域運(yùn)動(dòng)到另一節(jié)點(diǎn)域。拓?fù)涞貓D抽象度高，適合于大環(huán)境的結(jié)構(gòu)化描述。地圖創(chuàng)立最簡單的方法為增進(jìn)式方案。首先利用里程計(jì)數(shù)據(jù)估計(jì)機(jī)器人的位置，然后根據(jù)機(jī)器人獲取的新信息建立局部地圖并對(duì)全局地圖更新。移動(dòng)機(jī)器人配備多種內(nèi)部和外部傳感器，內(nèi)部傳感器主要有里程計(jì)、陀螺儀等。外部傳感器有視覺、激光、聲納、紅外等。移動(dòng)機(jī)器人融合內(nèi)部傳感器信息和外部環(huán)境特征信息進(jìn)行精定位和地圖創(chuàng)立，外部傳感器信息經(jīng)過處理形成外部環(huán)境的幾何特征，以長度、寬度、位置等參量表示。由于單目視覺系統(tǒng)不能提供環(huán)境特征的深度信息，無法表征環(huán)境特征的位置，因此移動(dòng)機(jī)器人即時(shí)定位與地圖創(chuàng)立中，主要通過聲納或激光傳感器在2D平面上的掃描信息〔距離、角度、強(qiáng)度等〕獲得外部環(huán)境的輪廓特征及其位置信息。聲納傳感器具有本錢低、波束覆蓋范圍寬的特點(diǎn)，但角度分辨率低，不精確，且容易產(chǎn)生虛假和多重反射回波信號(hào)，增加特征匹配的難度。而激光傳感器以其快速、掃描精度高、角分辨率高、方向性強(qiáng)等特點(diǎn)，得到了越來越多的應(yīng)用。環(huán)境特征的選擇和描述不追求細(xì)節(jié)，注重特征的魯棒性和穩(wěn)定性，主要采用室內(nèi)環(huán)境標(biāo)識(shí)的最小環(huán)境模型，提取能夠反映大范圍環(huán)境的結(jié)構(gòu)化特征，如直線和線段、角、點(diǎn)、垂直線等，分別與墻、墻角、凸角、門等特征對(duì)應(yīng)。特征提取的方法很多，對(duì)于直線和點(diǎn)特征主要采取哈夫變換方法，如加權(quán)哈夫變換法、直方圖法[5]等。特征的選擇非常廣泛，多面體、平面體、直線、角點(diǎn)、目標(biāo)邊緣等均可作為地圖特征[6,7]。然而單個(gè)點(diǎn)特征對(duì)路標(biāo)的唯一性識(shí)別和定義非常困難，可記錄目標(biāo)外表分布的特征點(diǎn)集合組成空間結(jié)構(gòu)用于目標(biāo)定位。Ayache等采用三目立體攝像機(jī)獲取環(huán)境中的直線段信息，最終獲取更高層的特征信息。Leonard等利用RCD模型從屢次掃描的聲納數(shù)據(jù)序列中提取距離信息恒定的區(qū)域所對(duì)應(yīng)的固定特征。IpY.Lip等采用EAFC算法從原始聲納測(cè)量數(shù)據(jù)中提取線段特征。Gross—mann、Tardos等用Hough變換和地圖聯(lián)接技術(shù)實(shí)現(xiàn)感知特征的提取，以減少鏡面反射引起的錯(cuò)誤特征檢測(cè)。Chong將離散聲納測(cè)量數(shù)據(jù)不斷累計(jì)合并至局部平面，提取平面、角點(diǎn)和邊緣等地圖特征，消除聲納鏡面反射現(xiàn)象引起的虛假目標(biāo)。多傳感器融合是提高特征檢測(cè)能力的重要手段。Castellanos等對(duì)激光和攝像機(jī)數(shù)據(jù)在特征級(jí)進(jìn)行融合，利用冗余數(shù)據(jù)提高特征表示可靠性，羅榮華[8]等也提出基于信息融合的地圖創(chuàng)立方案。準(zhǔn)確定位同時(shí)建造環(huán)境地圖的能力是自主移動(dòng)機(jī)器人的重要特征之一。為使自主型移動(dòng)機(jī)器人在真實(shí)環(huán)境部署和生存，真正實(shí)現(xiàn)完全自主，地圖創(chuàng)立的研究還需要在下面幾個(gè)方面取得突破：(1)實(shí)時(shí)性：人類處理地圖信息和理解環(huán)境可以在非常短暫的時(shí)間完成，而現(xiàn)在計(jì)算機(jī)定位和建造地圖的方法無論基EKF、基于概率還是其它方法都遠(yuǎn)遠(yuǎn)做不到實(shí)時(shí)處理。實(shí)時(shí)性要求兩方面的工作：一方面開發(fā)專用的建圖芯片或板卡，通過與主CPU并行工作提高處理速度；另一方面需要改良現(xiàn)有算法的效率或研究新的更好的SLAM方法，降低算法的時(shí)間和空間需求，提高運(yùn)算速度。(2)魯棒性：SLAM處理的主要對(duì)象就是系統(tǒng)中存在的各種不確定性。不確定性的存在也使SLAM算法很難保證穩(wěn)定性。如基于EKF算法非常依賴數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性，如果關(guān)聯(lián)不準(zhǔn)確，很容易使算法發(fā)散?；诔叨炔蛔兲卣髯儞Q的SLAM算法很大地改良了算法的穩(wěn)定性，但其對(duì)相關(guān)性的處理使它很難作為一種通用的解決方案。(3)準(zhǔn)確性：建立于各種不確定性上的準(zhǔn)確定位確實(shí)是個(gè)很難的問題。為此，需要盡量減少系統(tǒng)中的不確定因素。傳感器方面，由于聲納的噪聲數(shù)據(jù)很難處理，近年很多研究傾向于激光雷達(dá)和視覺傳感器。未來建立在激光雷達(dá)和視覺上的SLAM研究將越來越深入和完善。此外，SLAM算法包括地圖表示的方法等，也必須改良以減小位置誤差。概括來講，自主移動(dòng)機(jī)器人的SLAM研究的主要開展趨勢(shì)可以歸納為以下幾點(diǎn)：(1)機(jī)器人作業(yè)環(huán)境由的結(jié)構(gòu)化環(huán)境向未知的非結(jié)構(gòu)環(huán)境拓展。(2)環(huán)境感知方面采用能提供更大信息量的多傳感器系統(tǒng)〔如激光雷達(dá)和視覺系統(tǒng)等〕和多機(jī)器人系統(tǒng)〔也稱為分布式可移動(dòng)感知系統(tǒng)〕。(3)更加注重概率、模糊數(shù)學(xué)等不確定性處理工具在地圖描述和數(shù)據(jù)關(guān)聯(lián)算法中的應(yīng)用。(4)在實(shí)驗(yàn)研究中強(qiáng)調(diào)實(shí)機(jī)途徑，開發(fā)在真實(shí)環(huán)境下具有高魯棒性和高適應(yīng)能力的實(shí)用技術(shù)。(5)特種應(yīng)用和效勞業(yè)的需求日益迫切，可以預(yù)見面對(duì)真實(shí)世界的非結(jié)構(gòu)化和動(dòng)態(tài)特點(diǎn)，為了提高移動(dòng)機(jī)器人的自主能力，高適應(yīng)性、高魯棒性、高效率的SLAM理論、方法與技術(shù)將是今后的研究熱點(diǎn)。第二章地圖創(chuàng)立的一些關(guān)鍵性問題該領(lǐng)域所涉及的關(guān)鍵性問題可以歸結(jié)為六點(diǎn)，分別為不確定信息處理方法、數(shù)據(jù)關(guān)聯(lián)、累計(jì)誤差、算法復(fù)雜度、機(jī)器人導(dǎo)航問題、探索規(guī)劃。這些問題并不是孤立考慮的，彼此間存在緊密的關(guān)聯(lián)和滲透。下面分別對(duì)上述問題根據(jù)國內(nèi)外研究的現(xiàn)狀加以闡述。不確定性信息處理方法事實(shí)上，機(jī)器人地圖創(chuàng)立的困難就在于系統(tǒng)中無處不在的不確定性。機(jī)器人本身機(jī)械性能或未知外力〔綁架〕造成的不確定性將導(dǎo)致運(yùn)動(dòng)估計(jì)出現(xiàn)誤差，觀測(cè)的不確定性〔包括傳感器的不確定性和數(shù)據(jù)關(guān)聯(lián)的不確定性〕將導(dǎo)致校正失效，更不必說動(dòng)態(tài)環(huán)境中的環(huán)境的不確定性。如何處理不確定性，盡量減少各種客觀存在的不確定性引起的誤差是SLAM的關(guān)鍵，也是各種算法的精髓。作為SLAM的信息來源，機(jī)器人本體的動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)模型及其攜帶的各種傳感器的準(zhǔn)確性是決定地圖精度的一個(gè)關(guān)鍵。實(shí)際上這些渠道獲得的信息都帶有不同程度的不確定性。感知信息的不確定必然導(dǎo)致地圖和定位雙方的不確定。而同樣的，當(dāng)依據(jù)地圖和感知決策后續(xù)移動(dòng)時(shí)也就帶有不確定性，造成不確定性的逐步傳遞。因此必須解決以下問題：·在地圖和位置的表示中，如何描述運(yùn)動(dòng)和感知信息的不確定性？·在迭代過程中，如何處理舊信息與新信息的關(guān)系，連續(xù)更新地圖與位置？·如何依據(jù)不確定的信息進(jìn)行決策？對(duì)于上述問題，目前使用的主要是模糊邏輯[9]和概率[10]的方法，如Bayes估計(jì)，Kalman濾波和Markov推理等。以概率描述信息的不確定性其優(yōu)點(diǎn)在于適應(yīng)不確定模型，對(duì)于性能差的傳感器也能表示其感知信息，在實(shí)際應(yīng)用中魯棒性較好，在很多實(shí)現(xiàn)中得到了應(yīng)用，缺點(diǎn)是計(jì)算量較大。模糊邏輯提供了一個(gè)處理不確定性〔特別是聲納感知的不確定性〕較為魯棒且有效的工具。同時(shí)，在對(duì)多種不確定性信息進(jìn)行建模和融合時(shí)可以有更多的操作符供選擇。數(shù)據(jù)關(guān)聯(lián)為了獲得全局的環(huán)境地圖和實(shí)現(xiàn)定位，還需要將不同時(shí)間、不同地點(diǎn)的感知信息進(jìn)行匹配和聯(lián)合，存在局部數(shù)據(jù)之間的關(guān)聯(lián)問題，也存在局部數(shù)據(jù)與全局?jǐn)?shù)據(jù)的關(guān)聯(lián)與匹配問題。在SLAM中使用的數(shù)據(jù)關(guān)聯(lián)算法也多種多樣，如近鄰算法[11]和聯(lián)合相容性算法。由于根據(jù)機(jī)器人的里程儀提供的位置信息一般存在累計(jì)誤差，感知數(shù)據(jù)通常要融合其他外部傳感器的數(shù)據(jù)，如聲納、激光測(cè)距儀或視覺傳感器等等。這些傳感器提供的豐富和多樣的感知數(shù)據(jù)既為數(shù)據(jù)關(guān)聯(lián)提供了資源，也對(duì)數(shù)據(jù)描述的一致性和數(shù)據(jù)關(guān)聯(lián)算法的適用性提出了挑戰(zhàn)。上述研究可一定程度上提高特征選擇的魯棒性，然而特征檢測(cè)仍具有不確定性。數(shù)據(jù)關(guān)聯(lián)是指不同時(shí)刻對(duì)環(huán)境的觀測(cè)是否來源于同一特征。對(duì)于EKF模型來說，數(shù)據(jù)關(guān)聯(lián)的不準(zhǔn)確將導(dǎo)致算法無法收斂。在大多數(shù)模型中，這種關(guān)聯(lián)通常表達(dá)在特征之間的相似度和它們之間的相對(duì)距離。常見的數(shù)據(jù)關(guān)聯(lián)多采用最鄰近算法，方法認(rèn)為傳感器觀測(cè)值和特征之間的每個(gè)匹配均是獨(dú)立的，沒有考慮相鄰特征標(biāo)志之問的相互關(guān)系，而事實(shí)上測(cè)量預(yù)測(cè)誤差是相關(guān)的。當(dāng)機(jī)器人誤差增加或者陷入混亂狀態(tài)時(shí)，該方法將容易導(dǎo)致錯(cuò)誤匹配。Moutarlier等采用啟發(fā)式方法從機(jī)器人估計(jì)位置出發(fā)搜尋機(jī)器人旋轉(zhuǎn)和位移變化，實(shí)現(xiàn)感知特征與已存儲(chǔ)特征的最正確匹配。Neria等提出基于JCCB的約束搜索算法，把環(huán)境中的相關(guān)特征聯(lián)合形成有意義的環(huán)境標(biāo)志，如門、墻、角點(diǎn)等，然后進(jìn)行最正確關(guān)聯(lián)處理，成功剔除錯(cuò)誤匹配信息。Ba1ev等將數(shù)據(jù)關(guān)聯(lián)視為圖的匹配問題，通過求取最大公共子圖來實(shí)現(xiàn)特征標(biāo)志的匹配。數(shù)據(jù)關(guān)聯(lián)是對(duì)兩個(gè)特征標(biāo)志進(jìn)行匹配，確定它們是否對(duì)應(yīng)環(huán)境中的同一物體。SLAM中的數(shù)據(jù)關(guān)聯(lián)主要需要完成三個(gè)任務(wù)：新特征標(biāo)的檢測(cè)、特征標(biāo)志的匹配以及地圖之間的匹配。雖然在目標(biāo)跟蹤、傳感融合等領(lǐng)域，數(shù)據(jù)關(guān)聯(lián)已經(jīng)得到較好的解決，但是這些方法的計(jì)算量大，不能滿足SLAM的實(shí)時(shí)性要求。實(shí)現(xiàn)個(gè)標(biāo)志與擁有個(gè)標(biāo)志的地圖之間的數(shù)據(jù)關(guān)聯(lián)的復(fù)雜度與之間呈指數(shù)關(guān)系，假設(shè)每個(gè)觀測(cè)到的標(biāo)志有個(gè)可能的匹配，那么對(duì)于個(gè)標(biāo)志需要在指數(shù)空間中搜索正確的匹配。數(shù)據(jù)關(guān)聯(lián)的搜索空間與環(huán)境的復(fù)雜程度以及機(jī)器人的定位誤差有關(guān)，環(huán)境的復(fù)雜程度的增加會(huì)使增大，而誤差的增大會(huì)使增大。累計(jì)誤差SLAM中的誤差主要來自三個(gè)方面：觀測(cè)誤差、里程計(jì)的誤差和錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)帶來的誤差。當(dāng)機(jī)器人在地圖的環(huán)境中進(jìn)行定位時(shí)，機(jī)器人可以通過觀測(cè)位置的特征標(biāo)志對(duì)里程計(jì)的誤差進(jìn)行補(bǔ)償，每一次觀測(cè)使機(jī)器人的位置誤差趨向于觀測(cè)誤差與特征標(biāo)志的位置誤差之和。然而在SLAM中，由于機(jī)器人的位置和環(huán)境中的特征標(biāo)志的位置都是未知的，觀測(cè)信息不能有效糾正里程計(jì)的誤差，機(jī)器人的位置誤差隨著機(jī)器人的運(yùn)動(dòng)距離而增大，而機(jī)器人的位置誤差的增大將導(dǎo)致錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)，從而增大特征標(biāo)志的位置誤差；反過來，特征標(biāo)志的誤差又將增大機(jī)器人的位置誤差。因此，機(jī)器人的位置誤差與特征標(biāo)志的位置誤差密切相關(guān)，它們之間的相互影響使機(jī)器人和特征標(biāo)志的位置估計(jì)產(chǎn)生累計(jì)誤差，難以保證地圖的一致性。算法復(fù)雜度真正完全自主的機(jī)器人要求建圖與定位必須能夠在線計(jì)算，因而對(duì)計(jì)算復(fù)雜度的要求是非?？量痰摹，F(xiàn)在的算法最多能夠運(yùn)行幾天或幾個(gè)星期，可存儲(chǔ)和計(jì)算的環(huán)境特征到達(dá)104級(jí)，但仍然與真實(shí)世界有很大差距。單從運(yùn)算速度角度來說，現(xiàn)有計(jì)算機(jī)與人相比雖然有差距，但這種差距呈日漸減小趨勢(shì)。但進(jìn)行定位判斷等處理時(shí)，計(jì)算機(jī)卻遠(yuǎn)遠(yuǎn)不能與人相比。原因之一就是人腦的并行計(jì)算及分布式存儲(chǔ)。將神經(jīng)網(wǎng)絡(luò)技術(shù)或其它并行算法應(yīng)用于SLAM，極有可能較大幅度提高SLAM算法在真實(shí)環(huán)境部署的可能性。基于卡爾曼濾波器的SLAM的計(jì)算量主要來源于系統(tǒng)協(xié)方差矩陣的更新，即維持機(jī)器人與特征標(biāo)志之間以及各特征標(biāo)志之間的相關(guān)關(guān)系。這些相關(guān)關(guān)系對(duì)于SLAM算法的收斂性十分重要。研究說明，忽略這些相關(guān)關(guān)系不能保證地圖的一致性，使創(chuàng)立的地圖失去意義。為了降低SLAM的時(shí)間復(fù)雜度，需要在不影響地圖一致性的條件下限制這些相關(guān)關(guān)系。在機(jī)器人剖建地圖的過程中必然涉及到機(jī)器人的導(dǎo)航問題，包括自定位、運(yùn)動(dòng)規(guī)劃和避障。避障可以通過設(shè)計(jì)其行為來實(shí)現(xiàn)與地圖創(chuàng)立關(guān)系。比擬緊密的主要是定位和規(guī)劃。存在全局地圖的情況下，自身的定位比擬容易解決，當(dāng)能夠獲得精確的自身位置時(shí)，創(chuàng)立地圖只是一個(gè)積累信息的過程。但在完全未知的環(huán)境里，機(jī)器人沒有任何的參照物，其能依靠自己并不十分準(zhǔn)確的傳感器來獲取外界信息，由于計(jì)算實(shí)時(shí)性的限制，視覺傳感器也不能提供更多的信息，如同一個(gè)盲人在建筑物里摸索的情況，這種情況下，定位是比擬困難的。有地圖的定位和有定位的地圖創(chuàng)立都是容易解決的，但無地圖的定位和未解決定位的地圖刨建如同“無雞生蛋、無蛋生雞”。實(shí)際上目前機(jī)器人地圖刨建的一個(gè)主要難點(diǎn)也正是難以解決機(jī)器人自身精確定位的問題。已有的研究中對(duì)這個(gè)問題的解決方法可分為兩類，一類利用自身攜帶的多種內(nèi)部傳感器〔包括里程儀、羅盤、加速度計(jì)、陀螺儀等等〕，通過多種傳感信息的融合減小定位的誤差，使用的融合算法多為基于卡爾曼濾波的方法。這類方法由于沒有參考外部信息，在長時(shí)間的漫游后誤差的積累仍然會(huì)比擬大。另一類方法在依靠內(nèi)部傳感器估計(jì)自身運(yùn)動(dòng)的同時(shí)，使用外部傳感器〔如激光測(cè)距儀、視覺等等〕感知環(huán)境，對(duì)獲得的信息進(jìn)行分析抽取環(huán)境特征并保存，在下一步通過對(duì)環(huán)境特征的比擬對(duì)自身位置進(jìn)行校正。但是這種方法依賴于能夠取得環(huán)境特征，而環(huán)境特征[12，13]有時(shí)是很難提取出的，如：〔1〕環(huán)境特征不夠明顯時(shí)，如在茫茫沙漠中，人也無法完成哪怕大約的定位?！?〕傳感器信息比擬少，難以從一次感知信息中獲得環(huán)境特征。例如使用聲納或紅外傳感器。在地圖創(chuàng)立中，為了提高效率，希望在盡短的時(shí)間內(nèi)獲得盡可能多的未知環(huán)境信息。那么就需要解決這樣一些問題：如何確定未知區(qū)域，在最開始時(shí)所有方向都是未知區(qū)域，但一段時(shí)間后，已經(jīng)建立了局部地圖，這時(shí)就要尋找并標(biāo)注未知區(qū)域。選擇在當(dāng)前情況下最值得探索的區(qū)域，比擬探索該區(qū)域所能獲得的信息量和探索該區(qū)域需要付出的代價(jià)〔能量、時(shí)間等等〕。規(guī)劃前往該區(qū)域的路徑，尋找一條能保證平安的最短路徑。在這方面進(jìn)行的研究目前還比擬少太多數(shù)地圖創(chuàng)立的研究還主要著眼于解決創(chuàng)立地圖的主要問題。研究探索規(guī)劃的主要目的是提高地圖創(chuàng)立的效率，使機(jī)器人在較短的時(shí)間內(nèi)感知范圍覆蓋盡可能大的區(qū)域，在這方面的研究成果較少，一般遵循的策略是：讓機(jī)器人總是沿著最短路徑到最近的未探索區(qū)域[14]。這種方法原理上效率較高，但考慮到信息的不確定性，精心設(shè)計(jì)的路徑未必可行，而且復(fù)雜的算法也會(huì)影響計(jì)算的實(shí)時(shí)性。因此，需要設(shè)計(jì)更高效的控制策略，實(shí)現(xiàn)精確性和實(shí)時(shí)性的折衷，既能利用重復(fù)定位獲取的關(guān)聯(lián)數(shù)據(jù)來提高地圖和定位精度，又能保證全局的收斂性和較高的探索效率。針對(duì)大規(guī)模環(huán)境，為了提高作業(yè)速度，多機(jī)器人的SLAM問題的研究也得到關(guān)注。但總的來講現(xiàn)有的研究還是對(duì)單機(jī)器人方法的一種簡單擴(kuò)展。在多機(jī)器人的探索策略方面存在兩種思路：一種是完全分布私的，機(jī)器人之間共享信息，但獨(dú)立作出探索決策；另一種策略是分散探索，集中決策和創(chuàng)立地圖。第三章移動(dòng)機(jī)器人地圖創(chuàng)立方法基于卡爾曼濾波(KF)的SLAM算法擴(kuò)展Kalman濾波是移動(dòng)機(jī)器人即時(shí)定位與地圖創(chuàng)立中最重要的理論方法，最早的應(yīng)用是Smith等人提出基于擴(kuò)展卡爾曼濾波器的隨機(jī)地圖創(chuàng)立(stochasticmapping)方法[15]。方法用一包含機(jī)器人位姿向量和環(huán)境特征向量的高維向量表示空間環(huán)境，機(jī)器人控制信號(hào)或里程計(jì)信息輸入到系統(tǒng)狀態(tài)方程，完成對(duì)位姿和環(huán)境特征的預(yù)測(cè)，環(huán)境特征那么用于更新位姿和特征地圖。預(yù)測(cè)與更新之間，要進(jìn)行特征匹配或數(shù)據(jù)關(guān)聯(lián)處理，只有匹配的特征才用來更新位姿和特征地圖。隨機(jī)地圖將機(jī)器人的位姿向量和環(huán)境特征向量組織在一個(gè)高維狀態(tài)向量之中，用擴(kuò)展Kalman濾波作最小均方差估計(jì)。機(jī)器人的控制信號(hào)或里程計(jì)的信息輸入到系統(tǒng)狀態(tài)方程之中，完成對(duì)位姿和地圖特征的預(yù)測(cè)。上述成果奠定該類算法的理論研究框架，但是算法實(shí)施中存在兩大問題：〔1)計(jì)算量大。方法需要計(jì)算系統(tǒng)協(xié)方差矩陣，即維持機(jī)器人與特征標(biāo)志之間以及各特征標(biāo)志之間的相互關(guān)系。(2)不確定性高。由于機(jī)器人系統(tǒng)和傳感器自身的局限性、外界環(huán)境的干擾造成機(jī)器人定位和特征識(shí)別的不確定性。針對(duì)這些問題，研究人員提出了多種改良方案以保證地圖創(chuàng)立的精確度和可靠性，并降低空間和計(jì)算復(fù)雜度。目前，基于擴(kuò)展卡爾曼濾波的SLAM算法已成為該領(lǐng)域最廣泛最重要的研究方向之一。算法根本原理建立非線性離散時(shí)間系統(tǒng)的系統(tǒng)模型如下：(3-1)(3-2)其中為系統(tǒng)輸入控制向量，和為0為均值高斯白噪聲序列。即(3-3)(3-4)其中，為系統(tǒng)過程噪聲的協(xié)方差矩陣，為系統(tǒng)觀測(cè)噪聲的協(xié)方差矩陣。EKF將非線性函數(shù)f(·)和h(·)圍繞當(dāng)前系統(tǒng)狀態(tài)估計(jì)值擴(kuò)展成一階泰勒公式，實(shí)現(xiàn)非線性系統(tǒng)的線性化，并運(yùn)用KF算法遞推計(jì)算系統(tǒng)的狀態(tài)估計(jì)。其實(shí)現(xiàn)步驟如下：k-1和協(xié)方差矩陣預(yù)測(cè)當(dāng)前時(shí)刻的狀態(tài)估計(jì)和協(xié)方差矩陣：(3-5)(3-6)其中，為系統(tǒng)狀態(tài)方程的jacobian矩陣在處的值。預(yù)算Kalman增益矩陣(3-7)(3-8)其中為系統(tǒng)觀測(cè)協(xié)方差矩陣，為系統(tǒng)觀測(cè)方程的jacobian矩陣在處的值。根據(jù)狀態(tài)的預(yù)測(cè)估計(jì)和實(shí)際觀測(cè)值修正系統(tǒng)的狀態(tài)估計(jì)并計(jì)算系統(tǒng)的協(xié)方差矩陣：(3-9)(3-10)擴(kuò)展卡爾曼濾波(EKF)卡爾曼濾波器假設(shè)系統(tǒng)是線性系統(tǒng)，但實(shí)際的機(jī)器人運(yùn)動(dòng)模型與觀測(cè)模型是非線性的。因此，通常采用擴(kuò)展卡爾曼濾波器，基于EKF的建圖與定位可以歸納為一個(gè)循環(huán)迭代的估計(jì)校正過程，EKF算法在處理不確定信息方面有獨(dú)到之處，因此EKF成為最廣泛的SLAM方法。根據(jù)以上的公式和假設(shè)，可以設(shè)計(jì)卡爾曼濾波器公式為：(3-11)(3-12)其中和為互不相關(guān)且均值為零的正態(tài)白噪聲序列，方差分別為和，而K時(shí)刻的狀態(tài)向量和量測(cè)向量可表示為如下形式：(3-13)(3-14)量測(cè)值是從狀態(tài)預(yù)測(cè)向量中計(jì)算得到的目標(biāo)位置，它是指目標(biāo)點(diǎn)在機(jī)器人坐標(biāo)系中的具體位置，可以表示為：(3-15)所以系統(tǒng)的狀態(tài)方程和量測(cè)方程為：(3-16)(3-17)其中，(3-18)仿真實(shí)驗(yàn)實(shí)驗(yàn)在Matlab環(huán)境下進(jìn)行，移動(dòng)機(jī)器人的尺寸為：左右驅(qū)動(dòng)輪的半徑，寬度，初始位姿為，激光觀測(cè)的最大有效距離設(shè)為30m，隨機(jī)選擇了29個(gè)特征點(diǎn)和14個(gè)航點(diǎn)，其所構(gòu)建的仿真環(huán)境模型如圖2-1所示，通過上文提出的同時(shí)定位與地圖創(chuàng)立框架進(jìn)行仿真實(shí)驗(yàn)，結(jié)果如圖2-2所示。由于系統(tǒng)融合了內(nèi)部和外表傳感器的信息，預(yù)測(cè)與量測(cè)進(jìn)行了數(shù)據(jù)關(guān)聯(lián)匹配，并通過EKF進(jìn)行遞歸最優(yōu)估計(jì)，極大的改善了只用航位推算方法實(shí)現(xiàn)定位所帶來的誤差積累，從而使得移動(dòng)機(jī)器人的運(yùn)行軌跡與航線之間具有很好的擬合性，說明了EKF算法具有很強(qiáng)的魯棒性；同時(shí)由于實(shí)時(shí)的捕獲了當(dāng)前地圖信息，并對(duì)地圖不斷的更新，使得在未知環(huán)境下移動(dòng)機(jī)器人的自主導(dǎo)航成為可能，說明框架具有很大的實(shí)際應(yīng)用。驗(yàn)證了框架的可行性。本實(shí)驗(yàn)采用〔EKF〕對(duì)地圖或機(jī)器人位姿進(jìn)行更新，匹配成功那么對(duì)機(jī)器人位姿進(jìn)行更新，實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)定位；匹配失敗那么認(rèn)為測(cè)量到得特征為新特征，將其參加到環(huán)境中，作為下一時(shí)刻的參照物，實(shí)現(xiàn)了地圖的實(shí)時(shí)更新。圖3-1基于環(huán)境特征的地圖圖3-2同時(shí)定位與地圖創(chuàng)立基于尺度不變特征變換(SIFT)算法移動(dòng)機(jī)器人模型在二維環(huán)境中，移動(dòng)機(jī)器人的位姿使用，，表示，其中，表示移動(dòng)機(jī)器人相對(duì)世界坐標(biāo)位置，表示機(jī)器人的朝向，在位姿跟蹤問題中，移動(dòng)機(jī)器人的初始位姿是的。地圖的創(chuàng)立過程是指移動(dòng)機(jī)器人在自身位姿的情況下的環(huán)境地圖創(chuàng)立過程。在SLAM中，機(jī)器人利用自身攜帶的傳感器識(shí)別未知環(huán)境的特征標(biāo)志，然后根據(jù)機(jī)器人與特征標(biāo)志之間的相對(duì)位置和里程計(jì)的讀數(shù)估計(jì)機(jī)器人和特征標(biāo)志的世界坐標(biāo)。這種在線的定位與地圖創(chuàng)立需要保持機(jī)器人與特征標(biāo)志之間的詳細(xì)信息。在室內(nèi)特定環(huán)境下，基于環(huán)境特征SLAM方法的根本思想是將移動(dòng)機(jī)器人的位姿和環(huán)境特征坐標(biāo)表達(dá)在一個(gè)狀態(tài)向量中，在機(jī)器人的移動(dòng)過程中通過對(duì)環(huán)境特征的觀測(cè)做最準(zhǔn)確的估計(jì)。假設(shè)移動(dòng)機(jī)器人在世界坐標(biāo)系〔用，表示〕中為一個(gè)點(diǎn)，機(jī)器人的起始位置為世界坐標(biāo)系原點(diǎn)，前進(jìn)的方向〔機(jī)器人的朝向〕為機(jī)器人坐標(biāo)系中的橫坐標(biāo)，即軸，逆時(shí)針旋轉(zhuǎn)90度為縱坐標(biāo)軸。如圖3-3所示圖3-3機(jī)器人的移動(dòng)過程因此世界坐標(biāo)系中的方程為：(3-19)(3-20)(3-21)其中和表示時(shí)刻機(jī)器人在世界坐標(biāo)系中的位置；表示時(shí)刻機(jī)器人的朝向；是時(shí)刻移動(dòng)機(jī)器人的位移；k表示時(shí)刻機(jī)器人的偏航角。由于目標(biāo)是靜止物體，所以在世界坐標(biāo)系中的位置可以表示為：，(3-22)和是第個(gè)坐標(biāo)在世界坐標(biāo)系中的位置。機(jī)器人獲得的目標(biāo)位置是指第個(gè)目標(biāo)在機(jī)器人坐標(biāo)系中的位置，因此還需要將其轉(zhuǎn)換成世界坐標(biāo)，可表示為：(3-23)(3-24)估計(jì)與更新方程采用EKF實(shí)現(xiàn)系統(tǒng)狀態(tài)的估計(jì)與更新，當(dāng)機(jī)器人在運(yùn)動(dòng)過程中發(fā)現(xiàn)新的特征時(shí)，需要根據(jù)新特征的觀測(cè)矢量和機(jī)器人的當(dāng)前狀態(tài)計(jì)算新特征標(biāo)志的初始狀態(tài)，并更新狀態(tài)矢量和協(xié)方差矩陣。其中，在SLAM中，系統(tǒng)的狀態(tài)包括機(jī)器人和環(huán)境特征在機(jī)器人坐標(biāo)系中的位置的估計(jì)，而協(xié)方差矩陣表示估計(jì)的誤差，擴(kuò)展卡爾曼濾波方法對(duì)信息的處理一般分為預(yù)測(cè)和更新，并且此方法對(duì)信息的估計(jì)是無偏估計(jì)?？柭鼮V波預(yù)測(cè)方程為：(3-25)(3-26)(3-27)更新方程為：(3-28)(3-39)(3-30)其中，是狀態(tài)噪聲協(xié)方差，是測(cè)量噪聲協(xié)方差，是誤差協(xié)方差，是增益矩陣。預(yù)測(cè)方程用來預(yù)測(cè)當(dāng)前狀態(tài)，誤差協(xié)方差矩陣為下一時(shí)刻的先驗(yàn)估計(jì)。新方程將先驗(yàn)估計(jì)和測(cè)量值結(jié)合，得到一個(gè)較精確的后驗(yàn)估計(jì)。3.2.3SIFT算法LoweDG在2004年總結(jié)了現(xiàn)有的基于不變量技術(shù)的特征檢測(cè)方法，并正式提出了一種基于尺度空間的，對(duì)圖像縮放，旋轉(zhuǎn)甚至仿射變換保持不便性的圖像局部特征描述算子—SIFT算子，即尺度不變特征變換。SIFT算法首先在尺度空間進(jìn)行特征檢測(cè)，并確定關(guān)鍵點(diǎn)的位置和關(guān)鍵點(diǎn)所處的尺度，然后使用關(guān)鍵點(diǎn)領(lǐng)域梯度的主方向做為該點(diǎn)的方向特征，以實(shí)現(xiàn)算子對(duì)尺度和方向的無關(guān)性，梯度和方向的計(jì)算方程為：(3-31)(3-32)上式為處梯度的模值和方向公式，其中L所用的尺度為每個(gè)關(guān)鍵點(diǎn)各自所在的尺度。在實(shí)際計(jì)算時(shí)，可在以關(guān)鍵點(diǎn)為中心的鄰域窗口內(nèi)采樣，并用直方圖統(tǒng)計(jì)鄰域像素的梯度方向。直方圖的峰值代表了該關(guān)鍵點(diǎn)處鄰域梯度的主方向，即作為該關(guān)鍵點(diǎn)的方向。實(shí)驗(yàn)結(jié)果在試驗(yàn)室內(nèi)對(duì)移動(dòng)機(jī)器人進(jìn)行試驗(yàn)，讓機(jī)器人沿著長為三米的正方形行走，機(jī)器人行走的路線和地圖創(chuàng)立的結(jié)果如3-4所示。機(jī)器人實(shí)際路線和估計(jì)路線根本一致，而與里程計(jì)定位有一定差距，是由于里程計(jì)本身存在的缺陷，比方機(jī)器人行走過程輪子打滑，就會(huì)產(chǎn)生實(shí)際移動(dòng)距離和機(jī)器人內(nèi)部里程表數(shù)據(jù)不一致，因此，隨著時(shí)間的推移，里程計(jì)的累積誤差逐漸增大，從而導(dǎo)致自我定位與實(shí)際相差較大。圖3-4基于尺度不變特征變換的SLAM實(shí)驗(yàn)結(jié)果基于概率的SLAM算法由于SLAM問題涉及未知且不確定的環(huán)境描述和傳感器噪聲，一般使用概率方法來描述該問題。擴(kuò)展Kalman濾波方法是多種SLAM算法的根底，用于同步創(chuàng)立地圖和估計(jì)機(jī)器人的位置。但是其計(jì)算量大和對(duì)數(shù)據(jù)關(guān)聯(lián)問題處理困難的缺點(diǎn)也十清楚顯，盡管不如EKF那樣流行，但由于用概率表達(dá)機(jī)器人定位問題的不確定性非常自然合理，基于概率的SLAM也吸引了很多人的目光。同樣是基于概率，也有多種地圖表示及建圖方法。3.3.1SLAM問題的概率描述首先介紹一下相關(guān)的符號(hào)表示[16]：離散時(shí)間k=1,2,?，是機(jī)器人在時(shí)刻的實(shí)際位姿〔位姿包括機(jī)器人位置和朝向〕，表示控制變量，它使得機(jī)器人位姿從改變到，表示第個(gè)路標(biāo)的實(shí)際位置；表示時(shí)刻在位姿處對(duì)第個(gè)路標(biāo)的測(cè)量值；表示時(shí)刻得到的總測(cè)量值。另外，我們用大寫字母來表示以上變量的集合：狀態(tài)集合，控制輸入集合，所有路標(biāo)集合(即地圖)，其中l(wèi)是整個(gè)地圖上路標(biāo)的個(gè)數(shù);觀測(cè)集合。基于上面的符號(hào)表示，我們可用概率表示SLAM中兩個(gè)重要模型：運(yùn)動(dòng)模型和測(cè)量模型。運(yùn)動(dòng)模型：是在時(shí)刻機(jī)器人控制輸入和時(shí)刻位姿的條件下求取時(shí)刻位姿的條件概率。運(yùn)動(dòng)模型可由機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性求得。測(cè)量模型：是在時(shí)刻機(jī)器人位姿和路標(biāo)位置集合的條件下求取k時(shí)刻測(cè)量值的條件概率。測(cè)量模型在使用柵格地圖的情況下可通過反復(fù)測(cè)量傳感器數(shù)據(jù)而求得。整個(gè)SLAM問題可以用概率來表示，其含義是在所有控制輸入和觀測(cè)值的條件下求取機(jī)器人位姿和地圖的聯(lián)合條件概率?；跈C(jī)器人運(yùn)動(dòng)是Markov過程和周圍環(huán)境是靜態(tài)的這兩個(gè)假設(shè)，應(yīng)用Bayes公式，得到下式：(3-33)這是一個(gè)回歸算式，其中是時(shí)刻的條件概率，是時(shí)刻的條件概率，是歸一化常數(shù)。假設(shè)運(yùn)動(dòng)模型和測(cè)量模型，就可以通過上式迭代求解SLAM問題，其中最根本的方法就是前面所介紹的擴(kuò)展Kalman濾波?；诟怕收摰腟LAM在SLAM問題中，整個(gè)狀態(tài)矢量包含機(jī)器人位姿和地圖。移動(dòng)機(jī)器人的工作環(huán)境可看成一個(gè)平面，機(jī)器人位姿包括3個(gè)變量：直角坐標(biāo)和,姿態(tài)朝向即。每個(gè)路標(biāo)可以由其直角坐標(biāo)來表示：，i=1,2,?,l，路標(biāo)可以是周圍環(huán)境中的人工路標(biāo)或其他物體。假設(shè)在一張地圖中有個(gè)路標(biāo)，相應(yīng)的狀態(tài)矢量可以表示成以下維矢量。在擴(kuò)展Kalman濾波算法中，位姿和地圖的聯(lián)合估計(jì)可由其均值和協(xié)方差矩陣來確定。其中，均值矢量是維的，協(xié)方差矩陣是維的。假設(shè)環(huán)境是靜態(tài)的，那么地圖分量M滿足線性要求，但是機(jī)器人位姿通常是前一時(shí)刻和控制輸入的非線性函數(shù)。這在計(jì)算上增加了難度,而擴(kuò)展Kalman濾波將非線性函數(shù)使用泰勒級(jí)數(shù)展開作線性化近似。對(duì)于大多數(shù)移動(dòng)機(jī)器人來說，這種線性化處理不會(huì)引起較大的偏差。線性化的結(jié)果就是狀態(tài)方程可以由一個(gè)線性函數(shù)再加上高斯噪聲來表示：(3-34)這里，模型中的噪聲用來表示，并假設(shè)它服從均值為零，協(xié)方差矩陣是的正態(tài)分布。移動(dòng)機(jī)器人觀測(cè)模型通常也是非線性的，并且具有非高斯噪聲，同樣可以用泰勒級(jí)數(shù)展開來近似描述。即(3-35)這里，模型中的噪聲用來表示，并假設(shè)它服從均值為零，協(xié)方差矩陣是的正態(tài)分布。然后，就可以應(yīng)用標(biāo)準(zhǔn)Kalman濾波方法來計(jì)算。(3-36)(3-37)(3-38)(3-39)稀疏擴(kuò)展信息濾波稀疏擴(kuò)展信息濾波(SEIF)算法基于Kalman濾波的信息形式，即擴(kuò)展信息濾波(extendedinformationfilter)。EIF在數(shù)學(xué)表示形式上與EKF一致，但在地圖表示上采用路標(biāo)之間的兩兩約束。實(shí)際應(yīng)用時(shí)，這些約束通常表示為稀疏矩陣形式。SEIF更新時(shí)間是固定的，這明顯快于復(fù)雜度為的EKF算法。另外，SEIF利用最大似然法解決數(shù)據(jù)關(guān)聯(lián)問題。但是，SEIF算法的精確性較EKF方法差。FastSLAM算法FastSLAM算法假設(shè)機(jī)器人確實(shí)切路徑，那么確定路標(biāo)位置問題可分解成l個(gè)獨(dú)立估計(jì)問題，每個(gè)對(duì)應(yīng)一個(gè)路標(biāo)，整個(gè)SLAM問題可分解成下式：(3-40)FastSLAM算法使用粒子濾波(Particlefilter)估計(jì)機(jī)器人路徑，類似于MonteCarlo定位算法。從運(yùn)動(dòng)模型中采樣多條路徑，每條路徑就是一個(gè)粒子。數(shù)據(jù)關(guān)聯(lián)問題對(duì)于每個(gè)粒子都是單獨(dú)考慮的，關(guān)聯(lián)問題的影響降低。FastSLAM的時(shí)間復(fù)雜度為，其中N表示粒子數(shù)，可見其計(jì)算復(fù)雜度較EKF大為降低，但是由于每個(gè)粒子都要考慮一次數(shù)據(jù)關(guān)聯(lián)問題，相關(guān)計(jì)算又變得復(fù)雜了。結(jié)論在SLAM中，機(jī)器人利用自身攜帶的傳感器識(shí)別未知環(huán)境中的特征標(biāo)志，然后根據(jù)機(jī)器人與特征標(biāo)志之間的相對(duì)位置和里程計(jì)的讀數(shù)估計(jì)機(jī)器人和特征標(biāo)志的全局坐標(biāo)。這種在線的定位與地圖創(chuàng)立需要保持機(jī)器人與特征標(biāo)志之間的詳細(xì)信息。近幾年來，隨著計(jì)算機(jī)技術(shù)、傳感器技術(shù)的開展，SLAM的研究取得了很大的進(jìn)展，并已應(yīng)用于各種不同的環(huán)境國外學(xué)者在這方面做了大量的研究工作，移動(dòng)機(jī)器人的定位技術(shù)取得了較大的進(jìn)展。針對(duì)SLAM問題存在的難點(diǎn)，近20年來人們先后提出了多種解決方案，使該領(lǐng)域從根底研究逐步走向應(yīng)用，使得機(jī)器人逐步向未知非結(jié)構(gòu)化大規(guī)模環(huán)境下的智能自主機(jī)器人方向延伸。本文主要論述了該領(lǐng)域的主要理論方法及相關(guān)技術(shù)的現(xiàn)狀和最新進(jìn)展，指出SLAM面臨的問題，介紹了SLAM的根本實(shí)現(xiàn)方法，提出了一個(gè)基于特征的通用同時(shí)定位與地圖創(chuàng)立框架，并進(jìn)行了基于EKF的結(jié)構(gòu)化環(huán)境下同時(shí)定位與地圖創(chuàng)立仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果說明該框架的可行性和其實(shí)際應(yīng)用價(jià)值。但離實(shí)現(xiàn)移動(dòng)機(jī)器人完全的自主導(dǎo)航仍有很多問題需要研究。但是SLAM的各種實(shí)現(xiàn)方法都不是很完善，基于多機(jī)器人協(xié)作的SLAM的研究還剛剛起步，可以將人工智能、智能控制等領(lǐng)域的方法引入到SLAM中，開發(fā)更有效的SLAM算法。開發(fā)其他更好的地圖表示方式，特別是復(fù)雜地形中的地圖表達(dá)方式是SLAM中值得研究的問題。參考文獻(xiàn)[1]王衛(wèi)華，陳衛(wèi)華，席裕庚.基于不確定信息的移動(dòng)機(jī)器人地圖創(chuàng)立研究進(jìn)展[J].機(jī)器人.2001,23(6):563-568.[2]PRESCOTTTJ.Spatialrepresentationfornavigationinanimats[J].AdaptiveBehavior.1995,4(2)：85—123．[3]FEDERH，LEONARDJ，SMITHC．Adaptivemobilerobotnavigationandmapping[J]．IntJofRoboticsResearch.1999，l8(7)：650—668．[4]CsorbaM.SimultaneousLocalizationandMapBuilding[D].Oxford:U2niversityof[5]ELFESA，MORAVECH.Highresolutionmapsfromwideanglesonar[C].ProcoftheIEEEIntConfonRoboticsandAutomation.St.LouisMO:IEEEPress,1985:116-121.[6]OHYAA,NAGASHIMAY,YUTAS.Exploreunknownenvironmentandmapconstructionusingultrasonicsensingofnormaldirectionofwalls[C]ProcoftheIEEEIntConfonRoboticsandAutomation.SanDiegoCA:IEEEPress,1994:485-492.[7]CHONGKS,KLEEMANL.Mobile-robotmapbuildingfroman