基于激光雷達(dá)的智能車定位

自二十世紀(jì)七十年代初期美國斯坦福大學(xué)(SRI)的第一臺(tái)被叫做Shakey的移動(dòng)機(jī)器人研究成功，成為了我們?nèi)祟惪茖W(xué)技術(shù)在二十世紀(jì)以來的重要成果。移動(dòng)機(jī)器人的問世是現(xiàn)代自動(dòng)化技術(shù)與人工智能技術(shù)發(fā)展與融合的典型代表，也昭示著制造技術(shù)發(fā)達(dá)的水平。在過去50年中，在計(jì)算機(jī)技術(shù)飛速發(fā)展的帶動(dòng)下，人工智能技術(shù)取得了重大的突破性進(jìn)展。越來越多的領(lǐng)域開始嘗試使用人工智能技術(shù)提高生產(chǎn)效率，其中模式識(shí)別技術(shù)取代傳統(tǒng)低效率識(shí)別方式、智能控制技術(shù)代替人工操作、傳感器技術(shù)和數(shù)據(jù)融合讓數(shù)據(jù)利用越來越精確。這一系列技術(shù)推動(dòng)著智能機(jī)器人技術(shù)的飛速發(fā)展。在傳統(tǒng)的工業(yè)系統(tǒng)當(dāng)中，原有工業(yè)機(jī)器人對(duì)生產(chǎn)生活效率的提高起著微乎其微的作用。如今的智能機(jī)器人更加自動(dòng)化并且工作環(huán)境不再局限于室內(nèi)等構(gòu)造簡單的環(huán)境，而能勝任各種復(fù)雜多變的工作環(huán)境，如海陸空等。應(yīng)用的領(lǐng)域也越來越廣泛和無法估量，目前已經(jīng)運(yùn)用到了工農(nóng)業(yè)自動(dòng)化、軍事、醫(yī)療等。智能移動(dòng)機(jī)器人作為機(jī)器人學(xué)和人工智能領(lǐng)域結(jié)合的重點(diǎn)研究對(duì)象，以其突出的研究價(jià)值及在各領(lǐng)域潛在的廣闊前景受到了全世界的高校和圖1-1自主機(jī)器人在各個(gè)領(lǐng)域的應(yīng)用智能車，即能實(shí)現(xiàn)不需要駕駛員，僅僅通過計(jì)算機(jī)和無線傳感器控制就能實(shí)現(xiàn)自己行動(dòng)的車輛，是移動(dòng)智能機(jī)器人在交通領(lǐng)域的重要應(yīng)用。智能車使用多種車載傳感器，例如視覺相機(jī)、GPS導(dǎo)航、激光雷達(dá)測距儀、里程計(jì)等，獲取車輛周圍的環(huán)境情況，并通過傳感器的數(shù)據(jù)創(chuàng)建出表示環(huán)境的地圖，包括路徑、障礙物的位置以及自身的位置，再這些的基礎(chǔ)上完成定位、導(dǎo)航和避障等等行為。無人駕駛車輛顧名思義它具有智能控制的方式，它不再需要駕駛員這個(gè)可變因素較高的成分，而用了相對(duì)來說穩(wěn)定的電腦來控制車輛行駛。我們給智能車安裝了導(dǎo)航、定位、避障等等功能，使其能夠有效穩(wěn)定的完成任務(wù)，大大的提高了交通的效率和安全系數(shù)。這些技術(shù)都有同樣的目的——保障智能車的安全可靠。而要實(shí)現(xiàn)智能車自主行駛最基本的技術(shù)便是定位，同時(shí)它也是研究中的最重點(diǎn)的問題。本課題中最關(guān)鍵的兩個(gè)問題是對(duì)智能車的建圖和定位，他們是相互依存的。智能車想要定位，那么需要有一個(gè)有效的環(huán)境地圖，同時(shí)地圖的創(chuàng)建又需要智能車當(dāng)前的定位為基礎(chǔ)。因?yàn)樗鼈冎按嬖谥@樣密不可分的關(guān)系，所以我們稱這個(gè)過程為同步定位與地圖創(chuàng)建(simultaneouslocalizationandmapbuilding,SLAM),它是指智能車在當(dāng)前未知的環(huán)境中一邊建圖一邊在所創(chuàng)建的地圖上定位自身的過程。這就是智能車能在未知環(huán)境中有效的移動(dòng)而不是傻傻的到處亂跑的關(guān)鍵所在，在這個(gè)基礎(chǔ)上智能車還可以更進(jìn)一步的研究制造，完成更加復(fù)雜的任本次畢業(yè)設(shè)計(jì)的題目是基于激光雷達(dá)的智能車定位，我個(gè)人認(rèn)為本次設(shè)計(jì)的選題意義在于對(duì)ROS平臺(tái)的理解和應(yīng)用，因?yàn)槿缃裰悄苘囈约爸悄軝C(jī)器人等智能的。況且能將所學(xué)到的應(yīng)用到實(shí)際當(dāng)中更是最好的。其次是從激光雷達(dá)傳感器中獲取數(shù)據(jù)的學(xué)習(xí)，因?yàn)橹皩?duì)此是一無所知的狀態(tài)，所以要想學(xué)習(xí)并理解激光雷達(dá)的數(shù)據(jù)獲取還是要下很多功夫的，數(shù)據(jù)的獲取對(duì)于智能車定位來說是必不可少的一個(gè)環(huán)節(jié)，因?yàn)檫@也是對(duì)其地圖搭建和實(shí)現(xiàn)定位重要基礎(chǔ)。最后就是對(duì)基于粒子濾波的SLAM算法進(jìn)行一定學(xué)習(xí)和理解了，此次設(shè)計(jì)的定位算法所用到的方法就是粒子濾波進(jìn)行定位。智能車的定位對(duì)于我們?nèi)粘Ｉ钪械墓ぷ骱蛯W(xué)習(xí)是至關(guān)重要的，在生活中智能車可以代替人類完成很多人不能完成的工作和任務(wù)，尤其是在一些特殊環(huán)境下的工作。對(duì)智能車的研究意義重大，它可以解決人類的生活困難，在學(xué)習(xí)過程中也能加深我們對(duì)智能車的了解以便于更好的控制他為我們工作當(dāng)前時(shí)代，各個(gè)世界強(qiáng)國都對(duì)智能技術(shù)趨之若鶩，特別體現(xiàn)在對(duì)智能車技術(shù)的需求，在這種背景下智能車技術(shù)成為了評(píng)價(jià)一個(gè)國家科技綜合國力的標(biāo)準(zhǔn)之一，美國、日本、歐盟等都走在了世界前列。上世紀(jì)70年代末世界上首輛智能車在日本研發(fā)并制作成功。該車上采集信號(hào)的傳感器由2臺(tái)TV攝像機(jī)組成并裝有專用的信號(hào)處理器，并在當(dāng)時(shí)已經(jīng)可以達(dá)到30公里每小時(shí)的速度。美國也在智能車領(lǐng)域進(jìn)行了深入的研究并為此投入了巨額的費(fèi)用，同時(shí)也是最早開始智能車研究的國家之一。從1980年開始，美國的卡內(nèi)基梅隆大學(xué)、麻省理工大學(xué)、斯坦福大學(xué)就得到國家的支持開始了智能車方面的科學(xué)研究。很快他們就在智能車方面取得了重大的研究成果，研制出了如圖1-2所示的NavLba11智能車。該出配備有目前世界上主流的傳感器，通過視覺相機(jī)、里程計(jì)、激光雷達(dá)、GPS導(dǎo)航等完成了自主移動(dòng)，并為后來智能車的發(fā)展做出了良好的鋪墊，具有很重大的意義。圖1-2卡內(nèi)基梅隆大學(xué)的NavLab11智能車在歐洲，智能車的研究發(fā)展以歐盟為代表取得了卓越的成績，各國之間也逐漸形成了統(tǒng)一的合作關(guān)系并建立組織。他們開發(fā)了自己的智能車框架項(xiàng)目，并且在歐盟各個(gè)國家的科學(xué)家組建的團(tuán)隊(duì)通力合作研究下，這些項(xiàng)目有了很卓著的成果。其中最具有代表性的實(shí)際應(yīng)用是1997年12月Frog公司的ParkShuttle在荷蘭阿姆斯特丹Schiphol機(jī)場的應(yīng)用，如圖1-3所示，雖然那只是早期的智能車技術(shù)，但在當(dāng)時(shí)已經(jīng)是很了不起的成就。它能在出發(fā)點(diǎn)和終點(diǎn)按一定的路線來1999年2月，在荷蘭的鹿特丹，相同的智能車技術(shù)用于ParkShuttle在Rivium商業(yè)區(qū)的應(yīng)用上。2002年，CyberCab以荷蘭園藝博覽會(huì)為平臺(tái)完成了試驗(yàn)，任務(wù)是短距離輸送當(dāng)時(shí)的游客。如圖1-4所示，這時(shí)技術(shù)已經(jīng)比早期成熟許多，雖然同樣是在規(guī)定的路線上完成對(duì)游客的運(yùn)輸工作，智能車系統(tǒng)能夠完成2004年6月，法國借鑒歐盟的智能車項(xiàng)目，開發(fā)出了自己的智能車并進(jìn)行了大規(guī)模的演示。2005年6月，該項(xiàng)目有了突破性的進(jìn)展隨之而來是法國智能車項(xiàng)目越來越多的展示，這些各種各樣的成果展示都向世界展示了他們?cè)谥悄苘囇芯可先〉玫某删秃臀磥戆l(fā)展的速度將越來越快。圖1-3Frog公司CyberCar在機(jī)場的應(yīng)用圖1-4雅馬哈公司的智能車輛CyberCab我們國家對(duì)智能車項(xiàng)目的研究起步比世界上其他大國晚，期初的規(guī)模也比不上國際水平，但我們投入了大量的精力現(xiàn)在也取得了不錯(cuò)的進(jìn)展和成果。如上海交大、國防科大、清華、浙大等高校及其研究單位在這方面工作上做出了巨大的付出，同時(shí)也取得了杰出的成果。在1991年到1995年期間，我國的第一款智能車面世，取名為ALVLAB1。對(duì)程中自主進(jìn)行道路的偵查和選擇，能有效躲避障礙物并能夠在野外泥濘等復(fù)雜環(huán)境中保持良好運(yùn)行狀態(tài)。在直路狀態(tài)下最高時(shí)速21.6km/h,當(dāng)在轉(zhuǎn)彎以及躲避障礙物時(shí)也能保持12km/h的時(shí)速。總體上達(dá)到了國際智能車的性能水平。在大好形勢下，我國在第九個(gè)“五年計(jì)劃”期間成功研制出了我國的第二代智能車異，在第一代智能車基礎(chǔ)上增加了支持臨場感遙控駕駛及戰(zhàn)場偵查等功能。其在復(fù)雜道路環(huán)境和無人駕駛狀態(tài)下最高時(shí)速達(dá)到了74km/h,平均正常行駛速度為30.6km/h。在特殊路段如越野環(huán)境中，白天行駛時(shí)速最高達(dá)到24km/h,夜晚行駛時(shí)速達(dá)到了15km/h。在前兩代智能車的基礎(chǔ)上，由中科院與清華大學(xué)計(jì)算機(jī)系合作提供技術(shù)的支撐，在國家重點(diǎn)實(shí)驗(yàn)室中研制成功了我國新一代智能車THMR-V(TsingHuaMobileRobotV)。在原有基礎(chǔ)上改良了智能車的道路適應(yīng)系統(tǒng)，讓其可以同時(shí)兼顧高速公路和一般道路的行駛。智能車THMR-V車體由道奇7座廂式車改裝而來，設(shè)計(jì)其在高速公路上的時(shí)速為80km/h,在一般道路上的時(shí)速為20km/h。目前此智能車已經(jīng)能夠完成在非結(jié)構(gòu)化道路環(huán)境下進(jìn)行道路的偵查和選擇和躲吉林大學(xué)也在中國智能車發(fā)展的道路上緊緊跟隨上述各個(gè)單位的腳步，在組建自己的科研小組后成功的研制出了自己的智能車JUTIV-II。該智能車于上世紀(jì)九十年代初研制成功，具備了當(dāng)時(shí)智能車普遍的功能，在國內(nèi)是較早跟上當(dāng)時(shí)技術(shù)水平的單位之一。還有2013年我國首次送上月球的智能車“玉兔號(hào)”(如圖1-5所示),其代表這我國在智能車方面的研究已經(jīng)躋身世界前列，是非常具有意義的。該車采用太陽能為動(dòng)力源，具備了在外太空爬坡、避障和導(dǎo)航等功能。上海交通大學(xué)主持了歐盟CyberC3項(xiàng)目，并參加了歐盟CyberCars-2項(xiàng)目，已經(jīng)研制出多輛智能車，如圖1-6所示，該智能車性能穩(wěn)定、優(yōu)異，已經(jīng)在上海市向市民進(jìn)行了演示，并且投入實(shí)際工作和生活中使用。圖1-5玉兔號(hào)月球車圖1-6上海交通大學(xué)Cyberc3智能車輛本課題將把激光雷達(dá)集成到ROS平臺(tái)上開展工作，以提高智能車的地圖定位本課題利用ROS搭建智能車軟件平臺(tái)，利用TurtleBot和激光雷達(dá)搭建智能法設(shè)計(jì)能力、C++語言應(yīng)用開發(fā)能力和綜合應(yīng)用能力。第一章：主要介紹本課題的研究背景和意義，以及智能車在國內(nèi)外的發(fā)展概況第二章：介紹了智能車定位過程中創(chuàng)建的各種地圖類型及其優(yōu)缺點(diǎn)，詳細(xì)介紹了本次課題采用的地圖——基于激光雷達(dá)傳感器的線段特征采集以及局部地第四章：介紹了實(shí)驗(yàn)智能車平臺(tái)，實(shí)驗(yàn)的過程中重要步驟和代碼的分析。第五章：對(duì)本課題的總結(jié)和展望。實(shí)現(xiàn)智能車在未知環(huán)境中的定位與其實(shí)現(xiàn)自主創(chuàng)建地圖是互相依存的。地圖是智能車所在環(huán)境的信息表現(xiàn)方式，它依據(jù)智能車所配傳感器收集的信息而建立。根據(jù)創(chuàng)建使用的地圖不同，有三種不同的方法創(chuàng)建地圖來供智能車使用：拓?fù)浞椒?、柵格法、特征法。這三個(gè)方向?qū)?yīng)著不同的環(huán)境建圖。本文主要討論的是室內(nèi)環(huán)境下的智能車即時(shí)定位與同步建圖(SLAM),所以選擇采用創(chuàng)建基于特征的地圖。本章重點(diǎn)講述各種方法創(chuàng)建的地圖類型和它們各自的優(yōu)缺點(diǎn)，也介紹了智能車傳感器的模型并詳細(xì)介紹了本課題所使用的地圖創(chuàng)建過程——特征地圖的創(chuàng)建。對(duì)于智能車來說，創(chuàng)建地圖的方法是它能否快速有效的完成SLAM過程的關(guān)鍵。在如今智能車研究發(fā)展的結(jié)果表明一張切實(shí)有效的地圖，都應(yīng)滿足以下三個(gè)(2)地圖的更新迭代不困難，新的信息隨時(shí)可以吸收；(3)智能車可以使用該地圖完成我們對(duì)它布置的任務(wù)。前文介紹了創(chuàng)建地圖有三種方法，而每種方法創(chuàng)建出的地圖都有自己的特點(diǎn)和適用范圍，應(yīng)該根據(jù)實(shí)際情況選擇合適的地圖表示方法。2.1.1拓?fù)涞貓D基于拓?fù)鋱D表示法是將智能車所在的外部環(huán)境表示為一張拓?fù)鋱D，周圍環(huán)境中的每一個(gè)路標(biāo)點(diǎn)(可以自行做標(biāo)記)對(duì)應(yīng)于地圖中的一個(gè)節(jié)點(diǎn)，若果地圖中的節(jié)點(diǎn)兩兩之間有一段弧則說明在真實(shí)環(huán)境中的2個(gè)路標(biāo)點(diǎn)之間存在可以通行的路徑。如下圖2-1所示。拓?fù)涞貓D通過非常抽象的拓?fù)鋱D表示環(huán)境，不追求環(huán)境中具體的情況、甚至不同的節(jié)點(diǎn)之間它們實(shí)際的位置關(guān)系也是不精確的，所以創(chuàng)建時(shí)很方便。因?yàn)橥負(fù)鋱D具有這樣的特點(diǎn)，所以它很適合用于創(chuàng)建大范圍、室外、環(huán)境情況復(fù)雜的地圖，如果需要對(duì)環(huán)境中的情況有需要的話可以將這些信息儲(chǔ)存為對(duì)應(yīng)節(jié)點(diǎn)的描述文件?；谕?fù)涞貓D的智能車SLAM有著難以創(chuàng)建和更新的缺點(diǎn)，維護(hù)也比較麻煩。特別是環(huán)境中又有兩個(gè)地方很相似，而傳感器的誤差是不可避免的，通常這樣的話我們就無法分析它們是否為相同的節(jié)點(diǎn)，創(chuàng)建的地圖可能就出現(xiàn)非常大的誤差，導(dǎo)致智能車無法完成任務(wù)。圖2-1拓?fù)涞貓D柵格法對(duì)地圖的表示已經(jīng)越來越多的應(yīng)用在了智能車自主地圖的創(chuàng)建，并在柵格地圖上進(jìn)行自主定位。該方法將智能車所在的環(huán)境信息收集起來，建立一個(gè)坐標(biāo)系，并將用一個(gè)一個(gè)的柵格來儲(chǔ)存收集的各種信息，每一個(gè)柵格有自己的坐標(biāo)，也有一個(gè)數(shù)值用來表示該方格代表的環(huán)境中存在障礙物的概率。柵格法創(chuàng)建地圖的優(yōu)點(diǎn)在于創(chuàng)建的過程不復(fù)雜，數(shù)據(jù)的結(jié)構(gòu)是坐標(biāo)系也很容易計(jì)算，使得地圖在需要維護(hù)和更新時(shí)更加的方便。如圖2-2。圖2-2占有柵格地圖來創(chuàng)建整個(gè)地圖，而全局地圖的更新是通過更新局部柵格來實(shí)現(xiàn)的，再通過將局部地圖與全局地圖匹配，可以確定智能車的位置。柵格地圖非常適合于智能車的路徑規(guī)劃是該方法最突出的優(yōu)點(diǎn)，因?yàn)榭尚谢虿豢尚械膮^(qū)域顏色對(duì)比非常明顯。顏色越深的地方代表障礙物存在的概率越大，反之概率就越小。不過它的創(chuàng)建難點(diǎn)在于確定每一個(gè)柵格的大小，這是顯而易見的。如果要求地圖的精度高，那么我們需要選擇尺度較小的柵格，那么創(chuàng)建的地圖數(shù)據(jù)結(jié)構(gòu)很很大，導(dǎo)致需求的儲(chǔ)存空間和計(jì)算的時(shí)間復(fù)雜度很高；如果選擇大尺寸的柵格，那么地圖的誤差就會(huì)難以估計(jì)。因此柵格法在大規(guī)模地圖中應(yīng)用具將傳感器置于需要檢測的環(huán)境中，智能車通過傳感器得到環(huán)境的具體數(shù)據(jù)并對(duì)這些數(shù)據(jù)進(jìn)行分析，從而建立抽象的幾何模型，如點(diǎn)、線、面等模型來表示環(huán)境。這就是特征地圖。如圖2-3所示為提取線段創(chuàng)建的特征地圖。圖2-3基于線段的特征地圖特征地圖具有表示緊密細(xì)致并且讓定位目標(biāo)位置和識(shí)別其信息變得更加方便的優(yōu)點(diǎn)。比如在辦公室等室內(nèi)的封閉環(huán)境中，多數(shù)都是由顯著的幾何特性線段組成，這時(shí)就可以利用環(huán)境中的幾何特征進(jìn)行建模，可以簡單明了的將環(huán)境具體的表現(xiàn)出來。由這種方法創(chuàng)建出來的特征地圖能有效的對(duì)目標(biāo)進(jìn)行精確定位且能直觀體現(xiàn)目標(biāo)的位置信息。特征地圖有效解決了在不同坐標(biāo)系中難以轉(zhuǎn)換和不同傳感器之間的數(shù)據(jù)難以有效交叉相容的難題，更有利于在真實(shí)的室內(nèi)環(huán)境中有效實(shí)施，很適用于智能車導(dǎo)航中的自定位，很多智能車系統(tǒng)都采用了這種方法。因?yàn)槭覂?nèi)環(huán)境有很多的幾何特征，便于采集，并且地圖表達(dá)簡單，存儲(chǔ)空間較小。所以本課題采用特征地圖來進(jìn)行智能車的定位?；谔卣鞯貓D的智能車SLAM需要從傳感器收集的數(shù)據(jù)中采集出所需要的線段特征，再進(jìn)行下一步的工里程計(jì)(Odometry)具有價(jià)格不高、采樣速率高、短距離內(nèi)的定位誤差極小的特點(diǎn)，成為智能車很常用的一種定位工具。它的工作原理是通過光電編碼器獲得車輪在實(shí)驗(yàn)過程中旋轉(zhuǎn)過的度數(shù)，(如圖2-4所示)再結(jié)合車輪的周長，推算出智能車行走的路程距離來得到智能車距離出發(fā)點(diǎn)的位置情況。不過缺點(diǎn)在于誤差較大，通常會(huì)結(jié)合其他的傳感器完成數(shù)據(jù)的采集。本文采用常見的增量式正交光電編碼器。在一個(gè)周期T內(nèi)采樣可以推算智能車左輪和右輪的位移分別是：假設(shè)智能車的2個(gè)車輪之間的寬度為D,那么在一個(gè)周期T內(nèi)智能車位置和上式左側(cè)就是里程計(jì)的輸出量。2.2.2激光雷達(dá)測距儀模型激光雷達(dá)測距儀是移動(dòng)智能車研究中常用的測距傳感器，屬于外部傳感器。激光雷達(dá)測距儀采用的是激光束，它不受光照影響，采集到的環(huán)境信息誤差相比里程計(jì)得到的信息精確度很高，并且具有質(zhì)量輕、體積小、距離遠(yuǎn)、實(shí)時(shí)性高、抗干擾能力強(qiáng)等優(yōu)點(diǎn)使其可以作為自主智能車主要的的環(huán)境信息獲取傳感器。工作原理和聲吶類似如圖2-5所示。由發(fā)射口發(fā)出一束激光信號(hào)，在信號(hào)傳播的過程中如果遇到障礙物它就會(huì)反射回來，激光雷達(dá)傳感器有自己的接收器，可以接受被反射回來的激光信號(hào)。通過發(fā)射和接收之間的時(shí)間間隔來計(jì)算出障礙物的距離，主要用于收集創(chuàng)建地圖的數(shù)據(jù)。本文采用的激光傳感器是日本北陽公司生產(chǎn)的2D激光掃描儀Hokuyo指標(biāo)名稱參數(shù)指標(biāo)名稱參數(shù)電源電壓DC12V±10%光源半導(dǎo)體雷射入=870nm測距范圍掃描角度測距精度0.1-10m:±30mm;角度分解度約0.25(360/1,440分割)掃描時(shí)間噪音接口USB2.0(全速)環(huán)境溫濕度濕度：<85%RH(無凝露)激光掃描測距產(chǎn)品，測量距離0.1to30m,測量范圍為270°,角度分辨率為0.25°(360°/1,440steps),精度為0.1toZ,=(p?+o,,α?+o?)'i=1,2,3…,N2.3線段特征提取和局部地圖匹配預(yù)期的特征或者特征模板進(jìn)行對(duì)比并配對(duì)出相同的特征。上述過程中，通常傳感器檢測到的幾何特征參數(shù)數(shù)目會(huì)大于被估計(jì)的數(shù)目。因?yàn)閭鞲衅鞯臏y量都存在不可避免的誤差，所以不會(huì)存在相同的情況，而要考慮的就是如何優(yōu)化我們的檢測到的結(jié)果。例如我們用最小平方值來估計(jì)，可以得到一個(gè)有效的估計(jì)參數(shù)，使得我們得到的估計(jì)值與由傳感器采集數(shù)據(jù)計(jì)算而來的測量值之間差異最小。2.3.2線段特征檢測特征方法創(chuàng)建地圖的具體流程如圖2-6所示。通過激光雷達(dá)和里程計(jì)得到的原始數(shù)據(jù)提取出特征線段，然后進(jìn)行濾波、聚類、擬合等步驟得到局部地圖，再通過選取的角度對(duì)局部地圖進(jìn)行掃描，然后一一匹配來減少誤差。最終的環(huán)境地?fù)?jù)匹配聚類濾波擬合據(jù)匹配聚類濾波擬合數(shù)圖2-7激光數(shù)據(jù)處理流程圖激光雷達(dá)測距儀得到的數(shù)據(jù)并不是直接的特征地圖，而是一系列的點(diǎn)。我們需要去除誤差較大的點(diǎn)后才可以進(jìn)行線段特征的提取。這一過程稱為濾波，能提高地圖的準(zhǔn)確性。由于室內(nèi)環(huán)境下線段特征容易采集，所以本課題采用線段特征的提取方法構(gòu)建室內(nèi)地圖。這里我們使用的是聚類法提取特征，該方法主要是給各個(gè)采集點(diǎn)賦于不同的權(quán)值為數(shù)據(jù)處理手段，因此具有更高的精確性。得到了環(huán)境中的線段特征，我們需要通過聚類法將它們擬合成直線，然后才可以創(chuàng)建環(huán)境的特征地圖。因?yàn)閭鞲衅鞯脑蚝蛿?shù)據(jù)計(jì)算過程中難免存在大量難以估計(jì)的誤差，那么為了提高地圖的精確度，對(duì)得到的線段特征使用一定的算法進(jìn)行擬合得到直線特征，減少誤差。實(shí)驗(yàn)得到的觀測數(shù)據(jù)與之前的預(yù)測值的匹配問題，也稱作數(shù)據(jù)關(guān)聯(lián)。比如說通過掃描環(huán)境模型中特征值對(duì)應(yīng)的路標(biāo)點(diǎn)，這樣做可以確認(rèn)我們的測量值是否和預(yù)測值相同，或者說相同的測量值和預(yù)測值是否來自于同一個(gè)路標(biāo)點(diǎn)。這樣做可以防止錯(cuò)誤的測量值被我們用于地圖創(chuàng)建過程中的聚類，提高地圖的可靠性。那么我們就需要更加復(fù)雜的數(shù)據(jù)關(guān)聯(lián)技術(shù)進(jìn)行關(guān)聯(lián)匹配。Besl和McKay在1992年提出了ICP(IterativeClosestPoint)迭代最近點(diǎn)算法。這是一種從不同的角度對(duì)模型中距離最近的2個(gè)采集點(diǎn)進(jìn)行不同的掃描匹配，找到對(duì)全局最合適的匹配方案，如圖2-7所示。這個(gè)匹配問題可以描述為：對(duì)智能車從一個(gè)采集點(diǎn)到下一個(gè)采集點(diǎn)運(yùn)動(dòng)前后對(duì)同一路標(biāo)點(diǎn)的兩組激光測量數(shù)據(jù)，求出一個(gè)運(yùn)動(dòng)系數(shù)以使這兩組數(shù)據(jù)指向的都是這個(gè)相同的路標(biāo)點(diǎn)。然后繼續(xù)重復(fù)上述的匹配過程。圖2-8在橢圓模型中進(jìn)行ICP匹配(1)計(jì)算距離最近點(diǎn)(2)計(jì)算配準(zhǔn)(3)更新點(diǎn)集(4)迭代終止本章講述在智能車定位過程所涉及到的幾種地圖類型及根據(jù)它們各自的特點(diǎn)，本課題采用的了其中的特征地圖。介紹了在室內(nèi)環(huán)境下采用基于特征方法表示智能車所處環(huán)境，并建立特征地圖。特征地圖使用里程計(jì)和激光雷達(dá)測距儀采集的數(shù)據(jù)，提取線段特征描述環(huán)境，簡單明了便于儲(chǔ)存。在創(chuàng)建地圖的過程中由于激光雷達(dá)傳感器的功能不穩(wěn)定，地圖有時(shí)會(huì)出現(xiàn)錯(cuò)誤，這時(shí)需要等待地圖的更新以改正。2007年WillowGarage公司和美國斯坦福的聯(lián)合項(xiàng)目誕生了ROS操作系統(tǒng)。ROS從誕生開始就是面向全世界的智能機(jī)器人愛好者和研究人員，開源并且不收費(fèi)，因此被廣泛的使用在全世界各種各樣的機(jī)器人上，是一種可靠優(yōu)秀的分布式框架操作系統(tǒng)。它可以提供類似Windows這樣的操作系統(tǒng)所具備的大多數(shù)功能，并且自帶有許多簡單有效的工具軟件。最重要的一點(diǎn)是她支持多種編程語言編寫的程序，能在機(jī)器人領(lǐng)域輕松加愉快的實(shí)現(xiàn)代碼復(fù)用，因此ROS還支持代碼庫的上述所有功能都能由ROS的基礎(chǔ)工具實(shí)現(xiàn)。本次課題就會(huì)用到R供的用于機(jī)器人導(dǎo)航的完整包navigation,其中包含了本次實(shí)驗(yàn)所使用到的重AMCL模塊實(shí)現(xiàn)的。我們還會(huì)用到ROS系統(tǒng)自帶的一個(gè)可視化3D軟件RVIZ,它提供給我們地圖的可視化界面。ROS的主要特點(diǎn)有點(diǎn)對(duì)點(diǎn)設(shè)計(jì)、不依賴編程語言、精簡與集成、便于測試、開源并且規(guī)模大。這一級(jí)主要包括幾個(gè)重要概念：節(jié)點(diǎn)，節(jié)點(diǎn)管理器，參數(shù)服務(wù)器，消息，主題，服務(wù)(1)節(jié)點(diǎn)：節(jié)點(diǎn)執(zhí)行計(jì)算的進(jìn)程，ROS上一個(gè)機(jī)器人控制系統(tǒng)有許多節(jié)點(diǎn)組成。(2)消息：每個(gè)不同節(jié)點(diǎn)間相互聯(lián)絡(luò)是通過傳送消息進(jìn)行通訊的，每一個(gè)消息(3)主題：消息發(fā)布訂閱傳輸系統(tǒng)來傳送，節(jié)點(diǎn)之前的通信是依靠主題來互通消息的。主題是用于識(shí)別消息內(nèi)容的名稱，發(fā)布消息必須伴隨著一條主題。(4)服務(wù)：與主題類似也是一種消息傳輸機(jī)制。雖然基于主題的發(fā)布或者訂閱模型是很靈活的通信模式，但是它對(duì)于同步傳輸模式并不適合。在ROS中服務(wù)被互行為表現(xiàn)為類似一個(gè)遠(yuǎn)程程序的調(diào)用。subscribeROS節(jié)點(diǎn)管理器通過RPC(RemoteProcedureCallProtocol,遠(yuǎn)程過程調(diào)用)提供了主題和服務(wù)的注冊(cè)信息以及對(duì)其他計(jì)算圖表的查找。節(jié)點(diǎn)之間是可以直接相連的，通過節(jié)點(diǎn)管理器提供的表信息，如同IP地址一樣。節(jié)點(diǎn)就能找到其他節(jié)點(diǎn)并確定一種協(xié)議進(jìn)行通信。比如控制節(jié)點(diǎn)訂閱和發(fā)布消息的模型如下：XML/RPC8○3)connect("scan*.TCP)4)TCPserver:foo:23456)dataXML/RPC:foo:1234TCPdata:foo:2345messages句二二XML/RPC:foo:1234ROSRPC:foo:3456XML/RPC0圖3-4ROS控制器控制服務(wù)ROS文件系統(tǒng)級(jí)類似于Windows系統(tǒng)，可以通過硬盤分區(qū)，通過圖形界面來查看ROS系統(tǒng)中的文件，比如源代碼文件等等。圖3-5ROS系統(tǒng)文件種類ROS計(jì)算圖中包含的內(nèi)容類似于Windows中各種各樣的文件，它們統(tǒng)一包含在ROS文件系統(tǒng)結(jié)構(gòu)中，主要有以下2個(gè)重要的組織形式：包和堆。(1)包圖3-6包圖3-7堆功能包功能包清單消息服務(wù)代碼其他功能包集圖3-8ROS文件結(jié)構(gòu)是開源的并且希望最大速度讓它發(fā)展，所以采用軟件倉庫的管理模式來處理。這樣做使得ROS愛好者和研究人員可以在通過其他人的創(chuàng)意來豐富自己的機(jī)器人，同時(shí)也可以共享自己的想法，達(dá)到共同推進(jìn)ROS發(fā)展的目的。因?yàn)榇祟愖陨系较陆Y(jié)構(gòu)的設(shè)計(jì)，使得一個(gè)能夠?qū)崿F(xiàn)個(gè)體自主工作和根據(jù)環(huán)境的不同進(jìn)行優(yōu)化。這種結(jié)構(gòu)上分布的優(yōu)點(diǎn)，讓獨(dú)立地發(fā)展和實(shí)施工作成為可能，以指數(shù)級(jí)增加。社區(qū)級(jí)文件系統(tǒng)級(jí)社區(qū)級(jí)程序分布式管理程序如何組織構(gòu)建描述程序如何運(yùn)行3.2粒子濾波理論分析粒子濾波的核心是對(duì)智能車的路徑位姿及地圖進(jìn)行分解的一種混合濾波算法。根據(jù)該思想，將聯(lián)合后驗(yàn)概率分解：p(S,m/Z,,U,)=p(S,/Z,,U,)p(m/Z,S,)是t-1時(shí)刻前的控制量，實(shí)際反映為里程計(jì)的反饋。具體的算法流程如后面小節(jié)介紹。粒子濾波器是一種基本統(tǒng)計(jì)工具，其核心是序列重要采樣(SequentialImportanceSampling,SIS)通常也稱之為蒙特卡洛濾波器。目前已經(jīng)成功應(yīng)用在目標(biāo)跟蹤、智能車定位、地圖創(chuàng)建等各個(gè)相關(guān)領(lǐng)域。通過采樣樣本表示概率分布，每一個(gè)樣本稱作一個(gè)粒子，每一個(gè)粒子賦予不同的權(quán)值。它提供了一種方便有效的計(jì)算后驗(yàn)概率的方法。粒子濾波器包括四部分：1)生成粒子集(采樣步驟);4)重采樣。根據(jù)先驗(yàn)概率隨機(jī)選取N個(gè)粒子，作為初始狀態(tài)，每個(gè)粒子對(duì)應(yīng)自己的權(quán)值為W?=1/N,根據(jù)規(guī)則和目標(biāo)的先驗(yàn)概率得到初始狀態(tài)集為X。=P(X?)。積，即P(x?,x?)=P(x?|x?)P(x?)。如果P(x?,x?)可以解析的表示，那么只需要對(duì)對(duì)于每一個(gè)粒子，根據(jù)特定的粒子歷史計(jì)算預(yù)期分布，并且根據(jù)該分布采集X”~π(X?|XO,Zx,U)并將該樣本加入到粒子的歷史中。3.2.3粒子權(quán)重的計(jì)算每個(gè)粒子的權(quán)值可以由如下的遞推式得到：xw;P(z,ls,,z_,)其中，Z'1代表t-1時(shí)刻及之前對(duì)環(huán)境特征的觀測值，Z'表示t時(shí)刻的觀測值，S'表示t時(shí)刻的智能車的位置。如果t時(shí)刻前的估計(jì)值不在測量值計(jì)算出來目標(biāo)區(qū)間，那么計(jì)算會(huì)變得非常復(fù)雜，而且不一定能有結(jié)果。這里有一個(gè)優(yōu)化的方案：將t時(shí)刻的觀測值直接引入到對(duì)下一代的粒子集合的產(chǎn)生中。得到：p(s,m_,,s,,z,,u,,)的粒子樣本集合，那么可以推出新的權(quán)值的計(jì)算方式為：3.2.4重采樣當(dāng)?shù)玫剿械臉颖玖Ｗ雍?，我們并不是要使用所有的樣本粒子。我們?huì)用權(quán)值高的粒子來代替那些權(quán)值較低的粒子，得到更加精確的定位信息。這個(gè)過程就稱為重采樣。這樣做又存在一個(gè)關(guān)鍵的問題，當(dāng)我們多次迭代之后，樣本粒子中只剩下那些重復(fù)的權(quán)值大的粒子，我們的樣本就會(huì)退化，即那些權(quán)值大的粒子就3.2.5狀態(tài)參數(shù)的估計(jì)根據(jù)前文提到的各種模型得到粒子樣本，再根據(jù)粒子樣本估計(jì)得到的結(jié)果，更新對(duì)應(yīng)的地圖參數(shù)，通過激光數(shù)據(jù)得到的當(dāng)前估計(jì)的位姿來對(duì)下一步的粒子濾波進(jìn)行采樣。具體高斯估計(jì)的參數(shù)如下：其中的為歸一化參數(shù)：設(shè)智能車與設(shè)定的特征路標(biāo)點(diǎn)的距離為r,行駛路線與路標(biāo)的夾角為a,那其中的q為：q=(0m,y-S?y)2+(0m,x-s,)2SLAM算法的過程，主要使用特征地圖進(jìn)行定位，對(duì)粒子的重要性權(quán)值計(jì)算和重本次實(shí)驗(yàn)平臺(tái)是由實(shí)驗(yàn)室老師和研究生師兄自主搭建的一個(gè)智能小車，如圖4-1、4-2、4-3所示。4-1輪式智能車4-2輪式智能車4-3輪式智能車該機(jī)器人采用前文介紹的激光掃描儀HokuyoUTM-30LX,基于TurtleBot智能機(jī)器人平臺(tái)。TurtleBot是一個(gè)較為簡單的基礎(chǔ)機(jī)器人開發(fā)平臺(tái)，自帶有軟硬件，使用ROS作為操作系統(tǒng)，能實(shí)現(xiàn)機(jī)器人的自主建圖、導(dǎo)航、定位等功能。本次實(shí)驗(yàn)采用JoyStick手柄進(jìn)行操控智能車，實(shí)驗(yàn)的環(huán)境為西南科技大學(xué)東7A座8樓的走廊。(1)打開ROS(2)在turtlesim控制節(jié)點(diǎn) $rosrunturtlesimturtlesimnode啟動(dòng)小海龜仿真平面(3)發(fā)布控制命令在新的terminal中輸入以下指令： $rostopicpub-1/turtlel/cmd_velgeometry_msgs/Twist-可以得到小海龜繪制8字形狀命令，效果如下：4.2.2TurtleBot實(shí)現(xiàn)建圖在使用完成TurtleBot對(duì)ROS進(jìn)行初步認(rèn)識(shí)之后，現(xiàn)在我們就應(yīng)該把本課題的主體軟件程序應(yīng)用到此平臺(tái)上來。在第二章中，我們介紹了激光雷達(dá)傳感器采集環(huán)境。現(xiàn)在只要將數(shù)據(jù)添加到TurtleBot上，我們就可以用運(yùn)用這些數(shù)據(jù)進(jìn)行創(chuàng)建地圖，加載地圖的工作了。具體的各個(gè)命令順序如下：I$roslaunchturtlebot_bringupminimal.launch $roslaunchturtlebot_navigationgmapping_demo.launch $roslaunchturtlebot_rviz_launchersview_navigation.launch $roslaunchturtlebot_teleopkeyboard_teleop.launch V如果還沒實(shí)現(xiàn)存儲(chǔ)地圖工作，不能關(guān)上gmapping.launch運(yùn)行保存的地圖。4.3.1創(chuàng)造一個(gè)簡單的ROS包ROS的軟件以包的方式存在。包可以用自己寫的軟件封裝，也可使用ROS提供的工具，例如roscreate-pkg來創(chuàng)建package:生成manifest.xml文件，如圖4-4所示。<!--Theexporttagcontainsother,unspecified,tags--><!--Youcanspecifythatthispackageisameta<!--Youcanspecifythatthispackageisameta<!--<metapackage/>-->(2)將該路徑添加到ros系統(tǒng)中(即更新ros查找路徑):通過將改路徑添加到ros的環(huán)境變量中來實(shí)現(xiàn)。export.ROS_PACKAGE_PATH=YOUR_BEGINNER_TUTORIALS_PATH:$ROS_PACKAGEPATHcatmanifest.xml,得到結(jié)果如圖4-5。<!--Theexporttagcontainsother,unspecified,tags---><!--Youcanspecify<!--Youcanspecifythatthispackageisa<!--<metapackage/>-->圖4-5申明顯示結(jié)果rosdep命令：根據(jù)ROSpackages來安裝系統(tǒng)變量msg:msg文件是儲(chǔ)存了ROS消息的內(nèi)容，可以生成各種適合于ROS的源代分開。srv文件存儲(chǔ)在srv文件路徑。(2)創(chuàng)建一個(gè)msg:_a.在已經(jīng)創(chuàng)建好的beginner_tutorials_num”>msg/num”>msg/Num.msgpackage上創(chuàng)建msg:然后，用“geditCMakeLists.txt”打開package的CMakeList.txt,取b.查看msg文件：圖風(fēng)《《《《《□《《圖風(fēng)《《《《《□《《建立完成packge,相關(guān)程序及進(jìn)行操作其文檔應(yīng)該在~/catkin_ws/src文件下，等價(jià)的打開命令通常是如圖4-6所示的。$catkin_init_workspace$cmake…/Src-DCMAKE_INSTALL_PREFIX=../install-DCATKIN_DEVEL_PREFIX=../devel圖4-6存放目錄等價(jià)命令4.3.3用ruiz包實(shí)時(shí)顯示機(jī)器人建立的地圖Rosrunrvizrviz創(chuàng)建地圖過程中截圖如圖4-7所示。GlobalStatus:okGeldTFLocalMapStatus:OkTop北PotionToleranceAngleToleranceKeepLengthLengthofeacharrow.3030fps圖4-7創(chuàng)建地圖的過程截圖在上圖中的黑點(diǎn)即為移動(dòng)智能車的定位所在。(1)通過RVIZ對(duì)智能車進(jìn)行直線定位，通過手柄控制智能車在走廊上進(jìn)行直線行駛一定的距離，記錄智能車在地圖上與路路標(biāo)的距離，使用卷尺測量智能車與路標(biāo)的實(shí)際距離。(2)通過RVIZ對(duì)機(jī)器人進(jìn)行非直線定位，通過手柄控制智能車在走廊上進(jìn)行非直線行駛一定的距離，記錄智能車在地圖上與路路標(biāo)的距離，使用卷尺根據(jù)數(shù)據(jù)可以對(duì)定位精度進(jìn)行測試。測試數(shù)據(jù)截圖如下所示：orlentation:y:0.0covartance:[6.1,6.0,0z:0.cstanp:Z:0.0orlentatlon:Y8O.thear:X:0.00.0,0.0,Y:OZ:0.0X:X:0.0,0.0,0.0,1.7976931348623157G0.0,1.7976931348623157e+308,0.0,0.0,0.0,0.0,turtlebotOturtlebotturtlebotOturtlebot:~Pose:x:0.0y:0.6tisttx:0.0x:0.Cy:0.0covarlance:[6.1,0.0.0.0.0.0.0.0,對(duì)數(shù)據(jù)進(jìn)行整理可以得到：理論值(cm)實(shí)際值誤差誤差范圍(cm)起始坐標(biāo)終止坐標(biāo)距離82定位精度在進(jìn)行多次測量中，大部分誤差范圍均維持在10cm以內(nèi)，僅少數(shù)誤差范圍超過10cm。驗(yàn)證了本文討論的基于激光雷達(dá)的智能車定位算法是有效4.4重要源代碼及分析$roslaunchturtlebot_navgmapping_demo.launch文件是初始化算法的文件，其中：入加載算法所需要的參數(shù)，并初始化；gmapping.launch.xml文件包含所有需要用到的參數(shù)，其中：<nodepkg="gmapping"type="slam_gmapping"name="slam_gmapping"output="screen">加載名為gmapping的包并初始化，生成名為slam_gmapping的可執(zhí)行文件，參算法的主函數(shù)main.cpp:#include<ros/ros.h>#include"slam_gmapping.h"main(intargc,char**argv){ros::init(argc,argv,"slam_gmapping");gn.startLiveSlam();ros::spin();其中ros::init(argc,argv,"slam_gmapping");初始化gmapping節(jié)點(diǎn)；SlamGMappinggn;定義了一個(gè)類的對(duì)象gn;gn.startLiveSlam();調(diào)用gn里的一個(gè)功能函數(shù)來創(chuàng)建地圖并定位。slam_gmapping.h中封裝了一個(gè)很重要的類SlamGMapping:(部分重要的代碼)classSlamGMapping{public:SlamGMapping();SlamGMapping(unsignedlongintseed,unsignedlongintmax_duration_buffer);~SlamGMapping();voidvoidvoidvoidsensor_msgssensor_msgs::LaserScan::ConstPtr&scan);laserCallback(const上述代碼定義了激光數(shù)據(jù)的回調(diào)，以及地圖的保存和更新。在SlamGMapping這個(gè)類中的構(gòu)造函數(shù)SlamGMapping:SlamGMapping::SlamGMappingSlamGMapping::SlamGMapping():map_to_odom(tf::Transform(tf::createQuaternionFromRPY(0,0,0),tf::Point(0,laser_count_(0),private_nh_("~"),scan_filter_sub_(NULL),scan_filter_(NULL),}它的主要功能就是實(shí)現(xiàn)各種參數(shù)的初始化SlamGMapping