1緒論1.1研究背景與意義隨著社會(huì)科技的發(fā)展，人類的基本活動(dòng)大部分在室內(nèi)進(jìn)行。室外環(huán)境下，利用全球衛(wèi)星導(dǎo)航系統(tǒng)（GlobalNavigationSatelliteSystems，GNSS）可以提供厘米級(jí)的定位精度，但在室內(nèi)或近地面環(huán)境下，由于受到障礙物的遮擋和復(fù)雜環(huán)境的影響，接收到的GNSS信號(hào)較弱，無法滿足室內(nèi)高精度定位的需求。因此，如何在室內(nèi)實(shí)現(xiàn)精確的定位是定位技術(shù)研究領(lǐng)域中的一個(gè)重要方向。實(shí)現(xiàn)室內(nèi)定位和地圖構(gòu)建對(duì)我們現(xiàn)代化的發(fā)展有重要的意義和作用，可幫助我們進(jìn)行安全管理、目標(biāo)跟蹤、消防工作等工作。在世界各國(guó)專家學(xué)者的研究下，產(chǎn)生了許多室內(nèi)定位的技術(shù)。當(dāng)前主要應(yīng)用的定位技術(shù)有基于超寬帶（ultra-wideband，UWB）、藍(lán)牙、無線局域網(wǎng)（WirelessLocalAreaNetwork，WLAN）、視覺和激光雷達(dá)（LightDetectionandRanging，LiDAR）等技術(shù)。室內(nèi)定位大部分情況是在部分已知或者未知環(huán)境中獲取目標(biāo)位置和周圍環(huán)境地圖，而同步定位與制圖技術(shù)是實(shí)現(xiàn)室內(nèi)定位的關(guān)鍵技術(shù)，二者關(guān)系如圖1-1所示。圖1-1室內(nèi)定位技術(shù)與SLAM技術(shù)關(guān)系1.2國(guó)內(nèi)外的研究現(xiàn)狀1.2.1國(guó)外研究現(xiàn)狀1986年，美國(guó)舊金山舉辦的國(guó)際機(jī)器人與自動(dòng)化會(huì)議（InternationalConferenceonRoboticsandAutomation，ICRA）上，RajaChatila和Randelsmith等科學(xué)家提出了SLAM這一概念，科學(xué)家們?cè)跁?huì)議上討論了與機(jī)器人學(xué)相關(guān)的問題，許多科學(xué)家就基于概率優(yōu)化的環(huán)境地圖創(chuàng)建的方法展開了激烈討論，并重點(diǎn)研究其基本理論和算法，同年，Raja

Chatila和Peter提出了基于卡爾曼濾波（Kalman

Filters，KF）的機(jī)器人定位算法，該算法的原理是在機(jī)器人運(yùn)行過程中，假定其狀態(tài)噪聲和觀測(cè)噪聲都服從高斯正態(tài)分布，需通過卡爾曼濾波進(jìn)行優(yōu)化，通常，狀態(tài)方程對(duì)機(jī)器人位姿進(jìn)行預(yù)測(cè)，再根據(jù)當(dāng)前時(shí)刻的內(nèi)部傳感器測(cè)量和外部傳感器的觀測(cè)，在預(yù)測(cè)基礎(chǔ)上引出觀測(cè)模型進(jìn)行矯正，從而實(shí)現(xiàn)機(jī)器人位姿狀態(tài)的最優(yōu)估計(jì)?？柭鼮V波應(yīng)用的系統(tǒng)為線性系統(tǒng)，然而現(xiàn)實(shí)中的系統(tǒng)絕大多數(shù)都為非線性系統(tǒng),它的應(yīng)用條件有很大的局限性，所以在此基礎(chǔ)上對(duì)其進(jìn)行了延展——擴(kuò)展卡爾曼濾波（Extended

Kalman

Filter，EKF），該算法的思想是由Bucy和Sunahara等人提出的。其原理為：將非線性系統(tǒng)通過一階泰勒展開，使其接近于近線性系統(tǒng)，再進(jìn)行卡爾曼濾波的應(yīng)用，以此為基礎(chǔ)，SYavuz、ZKurt和MSbicer等人提出基于擴(kuò)展卡爾曼濾波的SLAM的方法，該算法同時(shí)利用里程計(jì)和外部傳感器對(duì)系統(tǒng)狀態(tài)進(jìn)行預(yù)測(cè)和矯正，實(shí)現(xiàn)了機(jī)器人定位精度和增量式構(gòu)建地圖。當(dāng)然，擴(kuò)展卡爾曼濾波也有不足，在實(shí)際應(yīng)用過程中，隨著機(jī)器人探測(cè)范圍的不斷擴(kuò)大，系統(tǒng)狀態(tài)觀測(cè)量不斷增加，這樣誤差就會(huì)不斷積累，系統(tǒng)的時(shí)效性會(huì)變差。此外，觀測(cè)地標(biāo)與地圖間的數(shù)據(jù)關(guān)聯(lián)問題也尚未解決。與卡爾曼濾波算法所不同的是，粒子濾波算法（ParticleFilter,PF）的應(yīng)用范圍更廣，并不局限于線性系統(tǒng)，能夠應(yīng)用于非高斯和非線性系統(tǒng)，具有更好的穩(wěn)定性和準(zhǔn)確性。1993年，Gordon等人提出了基于序貫重要性采樣粒子濾波器的非線性濾波算法，廣泛應(yīng)用到包括目標(biāo)識(shí)別、大數(shù)據(jù)分析和視覺跟蹤等各領(lǐng)域。不久，F(xiàn)ox.D提出了蒙特卡洛定位的思想，這個(gè)思想是基于馬爾可夫定位原理。這個(gè)定位思想使機(jī)器人實(shí)現(xiàn)了在未知環(huán)境進(jìn)行室內(nèi)定位和地圖構(gòu)建。Murphy等學(xué)者在此基礎(chǔ)上,將Rao-Blackellization算法與粒子濾波算法相結(jié)合,利用降維的方法,大大降低了計(jì)算量,形成了RBPF算法的雛形,并應(yīng)用于SLAM的研究當(dāng)中。在RBPF算法雛形的基礎(chǔ)上,利用擴(kuò)展卡爾曼濾波和粒子濾波器相融合的原理,Montemerlo提出了能夠?qū)崿F(xiàn)快速建圖定位的Fastslam

1.0算法和Fastslam

2.0算法?；趫D優(yōu)化（Graph-basedOptimization）的SLAM也是應(yīng)用非常廣泛的一種技術(shù)，它克服了EKF-SLAM和PF-SLAM的某些劣勢(shì)。1997年，Lu和Milosl提出了基于圖優(yōu)化的SLAM的概念，是用一個(gè)圖表示同步定位和地圖構(gòu)建的問題，其原理是構(gòu)建地圖，并且找到一個(gè)最優(yōu)的機(jī)器人位姿，最小化預(yù)測(cè)誤差和觀測(cè)誤差，從而對(duì)整個(gè)圖形進(jìn)行優(yōu)化。1999年，Gutmann和Konolige提出了運(yùn)行增量式地圖算法，同時(shí)構(gòu)建圖形和檢測(cè)閉環(huán)的有效方法。2002年，Gauss-Sedel松弛方法來解決最小化誤差問題是由Duckett等人提出的。隨后，F(xiàn)rese等人提出了多層次松弛方法，在不同的分辨率下使用不同的松弛。同年，Olson等提出了一種應(yīng)用隨機(jī)梯度下降法解決問題的一種有效的優(yōu)化方法。大部分基于圖優(yōu)化的方法都僅僅關(guān)注于得到最優(yōu)的地圖，他們假設(shè)機(jī)器人位姿和約束是已知的。最開始基于圖優(yōu)化的SLAM被認(rèn)為非常耗時(shí)，隨著對(duì)SLAM問題結(jié)構(gòu)的深入研究以及稀疏矩陣在線性代數(shù)領(lǐng)域的發(fā)展，使得解決圖優(yōu)化問題成為了可能。近些年，已經(jīng)有許多基于圖優(yōu)化的算法被提出，著名的有Osoll，TreeMap，TORO和iSAM,還有一些基于圖優(yōu)化的開源框架g2o和Cartographer。相較于濾波方法，由于利用了SLAM問題的稀疏特性，使得基于圖優(yōu)化的非線性優(yōu)化算法具有很高的效率[6]。1.2.2國(guó)內(nèi)研究現(xiàn)狀在SLAM方面，雖然國(guó)內(nèi)研究起步較晚，但發(fā)展較快。在移動(dòng)機(jī)器人同步定位和地圖構(gòu)建的問題上，宋宇提出了基于平方根的RBPF-SLAM算法，可在大環(huán)境中完成地圖構(gòu)建，其算法因?qū)⑵椒礁蜃蛹尤胗?jì)算權(quán)值和協(xié)方差，從而減少了大量的運(yùn)算時(shí)間。同年，閆德立等人基于Cubature卡爾曼濾波，提出了一種基于概率密度的方法。該方法在SLAM過程中精確了移動(dòng)機(jī)器人的位姿更新，從而提高了定位和構(gòu)圖的精度[10]。陳白帆扥人將粒子群引入FastSLAM中，對(duì)預(yù)估粒子進(jìn)行更新，使其更集中于機(jī)器人的真實(shí)位姿附近，這有效的提高了定位的精度同時(shí)大大減少了運(yùn)算時(shí)間[7]。洪炳熔等人在Rao-Blackellized粒子濾波的基礎(chǔ)上提出了結(jié)合自適應(yīng)的重采樣方法[3]。1.3本文主要研究?jī)?nèi)容本文主要基于EKF和圖優(yōu)化的SLAM算法，在UbuntuLinux14.04操作系統(tǒng)下Indigo版本的機(jī)器人操作系統(tǒng)(RobotOperatingSystem，ROS)平臺(tái)下進(jìn)行實(shí)驗(yàn)，使用RPLIDARA1實(shí)現(xiàn)室內(nèi)定位和地圖構(gòu)建，對(duì)比兩者的室內(nèi)定位精度，全文分為六個(gè)章節(jié)，各章安排如下：第一章是緒論，主要介紹了室內(nèi)定位的研究背景和意義，以及SLAM技術(shù)的國(guó)內(nèi)外研究現(xiàn)狀，最后闡述了論文的主要研究?jī)?nèi)容。第二章是LiDAR系統(tǒng)概述與數(shù)據(jù)處理，主要介紹了LiDAR的系統(tǒng)概述、工作原理以及LiDAR的數(shù)據(jù)采集和傳輸流程。第三章是SLAM原理與方法流程，主要介紹了SLAM系統(tǒng)模型，重點(diǎn)分析了擴(kuò)展卡爾曼濾波與圖優(yōu)化的原理以及環(huán)境地圖的表示方法。第四章是基于ROS的SLAM研究與設(shè)計(jì)，主要介紹了ROS的基本概念，在ROS上實(shí)現(xiàn)了基于激光雷達(dá)的室內(nèi)定位，以及在ROS中常用的SLAM算法。第五章是實(shí)驗(yàn)與分析，設(shè)計(jì)并搭建了移動(dòng)定位平臺(tái)，進(jìn)行了室內(nèi)定位實(shí)驗(yàn)，分析了定位和制圖精度。第六章是總結(jié)與展望，總結(jié)了本文的主要研究?jī)?nèi)容，展望了后續(xù)的研究工作。

2LiDAR系統(tǒng)概述與數(shù)據(jù)處理2.1LiDAR工作原理LiDAR系統(tǒng)包括一個(gè)激光器和一個(gè)接收系統(tǒng)，激光器產(chǎn)生并發(fā)射激光束，打在物體上并反射回來，反射回來的激光被接收系統(tǒng)接收。接收器可以準(zhǔn)確地測(cè)量激光束從發(fā)射到反射回來的傳播時(shí)間。由于光速已知，通過傳播時(shí)間即可轉(zhuǎn)換為L(zhǎng)IDAR與被照射到的目標(biāo)物體的距離值。本文介紹的是SLAMTEC公司所研發(fā)的RPLIDARA1，是一種低成本的、采用非接觸測(cè)距技術(shù)的二維激光雷達(dá)，可實(shí)時(shí)獲取所在環(huán)境的高精度輪廓信息，是機(jī)器人定位導(dǎo)航，環(huán)境測(cè)量，同步地位和地圖構(gòu)建、安保安防等領(lǐng)域必不可少的傳感器。其基本參數(shù)如表2-1所示。表2-1RPLiDARA1基本參數(shù)項(xiàng)目單位最小值典型值最大值備注測(cè)距范圍米待定0.15-12待定基于白色高反光物體測(cè)得掃描角度度-0-360-測(cè)距分辨率毫米-＜0.5-測(cè)量物體在1.5米以內(nèi)<實(shí)際距離的1%全部量程范圍內(nèi)角度分辨率度-≤1-5.5hz掃描時(shí)單次測(cè)距時(shí)間毫秒-0.5-測(cè)量頻率赫茲2000≥40008000掃描頻率赫茲5.51010掃描一周的頻率典型值為一次掃描恰好360采樣點(diǎn)的情況RPLIDARA1主要由激光測(cè)距核心、掃描電機(jī)和傳動(dòng)以及USB三部分組成，給各個(gè)子系統(tǒng)接入電源之后，RPLIDARA1的掃描電機(jī)進(jìn)行360°的旋轉(zhuǎn)，激光發(fā)射頭發(fā)射激光束，通過反射回來的激光脈沖獲取周圍環(huán)境的特征信息。可以通過USB接口來向外部系統(tǒng)傳輸掃描外部環(huán)境的特征信息，如圖2-1所示。圖2-1RPLIDARA1硬件結(jié)構(gòu)RPLIDARA1主要采用了激光三角測(cè)距技術(shù)，在掃描過程中，RPLIDARA1發(fā)射經(jīng)過調(diào)制的紅外激光束到周圍環(huán)境并接收反射回來的激光信號(hào)，經(jīng)過RPLIDARA1內(nèi)部的數(shù)字信號(hào)處理器實(shí)時(shí)處理，即采集到的信息為RPLIDARA1與被照射到的目標(biāo)物體的距離值以及當(dāng)前夾角信息。圖2-2RPLIDARA1工作原理單個(gè)激光照射點(diǎn)數(shù)據(jù)采集的過程如圖2-2所示，其中是掃描點(diǎn)相對(duì)于RPLIDARA1正前方（0°）的夾角，d為被照射物體與RPLIDARA1中心的距離。因此，當(dāng)RPLIDARA1被安裝在實(shí)驗(yàn)移動(dòng)平臺(tái)上中心位置時(shí)，以RPLIDARA1的中心為極坐標(biāo)原點(diǎn)，可以得到各個(gè)掃描點(diǎn)的坐標(biāo)為：(2-1)式中，N為L(zhǎng)iDAR掃描一周得到的測(cè)量數(shù)據(jù)個(gè)數(shù)，隨掃描頻率設(shè)定的不同而不同。RPLIDARA1采樣信息如表2-2所示。表2-2RPLIDARA1采樣信息數(shù)據(jù)類型單位描述距離值毫米PRLiDAR距離當(dāng)前采樣點(diǎn)之間的實(shí)際距離夾角度當(dāng)前采樣點(diǎn)相對(duì)于PRLiDAR自身朝向的夾角信號(hào)強(qiáng)度級(jí)當(dāng)前采樣點(diǎn)的信號(hào)強(qiáng)度起始信號(hào)(布爾值)表示當(dāng)前采集點(diǎn)是否屬于一次新的掃描2.2LiDAR數(shù)據(jù)獲取LiDAR的數(shù)據(jù)采集是實(shí)現(xiàn)SLAM的一部分，機(jī)器人通過激光雷達(dá)在未知環(huán)境中獲取數(shù)據(jù)，融合SLAM的系統(tǒng)模型等實(shí)現(xiàn)室內(nèi)定位和地圖構(gòu)建。主要介紹LiDAR的數(shù)據(jù)采集與數(shù)據(jù)傳輸流程，其中數(shù)據(jù)采集包括LiDAR的命令請(qǐng)求和數(shù)據(jù)掃描，數(shù)據(jù)傳輸流程主要是指將LiDAR的數(shù)據(jù)向外部設(shè)備傳輸。2.2.1LiDAR數(shù)據(jù)采集2.2.1.1LiDAR請(qǐng)求命令（1）停止掃描（STOP）命令請(qǐng)求該命令屬于單次請(qǐng)求/無應(yīng)答模式，即外部系統(tǒng)發(fā)送請(qǐng)求報(bào)文，LIDAR接收到之后進(jìn)行相關(guān)操作，此時(shí)外部系統(tǒng)等待LiDAR處理，而LiDAR對(duì)此請(qǐng)求命令不給予響應(yīng)。停止掃描命令為：LiDAR正處于掃描采樣狀態(tài)時(shí)，外部系統(tǒng)可通過發(fā)送停止掃描的請(qǐng)求電文，使其關(guān)閉測(cè)距系統(tǒng)和激光器，進(jìn)入空閑狀態(tài)。若如果LIDAR先前已經(jīng)在空閑狀態(tài)或者保護(hù)停機(jī)狀態(tài)下，則該命令則會(huì)被忽略。若外部系統(tǒng)需要再發(fā)送該請(qǐng)求命令，延遲1ms以上時(shí)間后發(fā)送下一次請(qǐng)求，否則第二次請(qǐng)求將會(huì)被LIDAR刪除。（2）測(cè)距核心重啟（RESET）命令請(qǐng)求該命令屬于單次請(qǐng)求/無應(yīng)答模式，即LIDAR接收到由外部系統(tǒng)發(fā)送請(qǐng)求報(bào)文，執(zhí)行相關(guān)操作，但對(duì)此請(qǐng)求命令不給予響應(yīng)。測(cè)距核心重啟命令請(qǐng)求為：當(dāng)LiDAR因故障進(jìn)入了保護(hù)性停機(jī)后，外部系統(tǒng)就可以嘗試發(fā)送RESET命令嘗試將LiDAR恢復(fù)至正常工作狀態(tài)。對(duì)其發(fā)送RESET命令時(shí)，測(cè)距核心將進(jìn)行軟件重啟操作，LiDAR將會(huì)呈現(xiàn)為正常的通電狀態(tài)。圖2-3單次請(qǐng)求/無應(yīng)答模式（3）掃描采樣（SCAN）命令請(qǐng)求與回應(yīng)數(shù)據(jù)格式該命令屬于單次請(qǐng)求/多次應(yīng)答模式，即LiDAR在接收到外部系統(tǒng)發(fā)送的測(cè)距要求和PWM控制信號(hào)時(shí)，會(huì)先進(jìn)入初始化狀態(tài)，隨后發(fā)送起始信號(hào)給外部系統(tǒng)，表示LiDAR接受了進(jìn)入掃描采樣狀態(tài)的請(qǐng)求，外部系統(tǒng)可以準(zhǔn)備接受采樣點(diǎn)。當(dāng)LiDAR的掃描電機(jī)穩(wěn)定后會(huì)開始測(cè)距操作，不停的向外發(fā)送采樣數(shù)據(jù)。直至外部系統(tǒng)發(fā)送停止掃描的請(qǐng)求命令或者LiDAR出現(xiàn)故障時(shí)，LiDAR才會(huì)停止掃描。掃描采樣（SCAN）命令為：外部系統(tǒng)若使LiDAR進(jìn)入掃描狀態(tài)時(shí)，需對(duì)其發(fā)送掃描采樣命令請(qǐng)求的報(bào)文，接收到掃描命令，LIDAR將會(huì)進(jìn)入測(cè)距采樣狀態(tài)，隨后外部系統(tǒng)可接收來自LIDAR的發(fā)送的采樣數(shù)據(jù)。如果LIDAR先前已經(jīng)處于測(cè)距采樣狀態(tài)，則正在進(jìn)行的測(cè)距采樣功能將被停止，系統(tǒng)會(huì)重新開始測(cè)距掃描操作[9]。如果LiDAR已經(jīng)處于保護(hù)性停機(jī)狀態(tài)，其接收到的命令將會(huì)被刪除。圖2-4單次請(qǐng)求/多次應(yīng)答模式（4）強(qiáng)制掃描采樣（FORCE_SCAN）命令請(qǐng)求與回應(yīng)數(shù)據(jù)格式該命令屬于單次請(qǐng)求/單次應(yīng)答模式，即外部系統(tǒng)向LiDAR發(fā)送請(qǐng)求報(bào)文，LiDAR在內(nèi)部處理后執(zhí)行請(qǐng)求命令，發(fā)送應(yīng)答報(bào)文給外部系統(tǒng)，之后再次等待外部系統(tǒng)的下一個(gè)命令。強(qiáng)制掃描采樣（FORCE_SCAN）使得LiDAR忽略當(dāng)前掃描電機(jī)的工作狀態(tài)而強(qiáng)行進(jìn)行掃描測(cè)距并發(fā)送數(shù)據(jù)應(yīng)答。LiDAR執(zhí)行強(qiáng)制掃描采樣請(qǐng)求與開始掃描采樣命令處理邏輯一致。起始應(yīng)答報(bào)文與數(shù)據(jù)應(yīng)答報(bào)文均與掃描采樣的應(yīng)答報(bào)文一致。圖2-5單次請(qǐng)求/單次應(yīng)答模式2.2.1.2LiDAR的數(shù)據(jù)掃描激光雷達(dá)建立坐標(biāo)系，以LiDAR的重心為原點(diǎn)沿著雷達(dá)引出線方向?yàn)闄M軸，垂直方向?yàn)榭v軸，使在掃描過程中不同位置對(duì)同一環(huán)境得到的數(shù)據(jù)相同。由LiDAR的工作原理可知，利用激光束發(fā)射與反射時(shí)間求解目標(biāo)距離，d=ct/2，其中c為光速。還需知道當(dāng)前采集方向與橫軸之間的夾角。則所測(cè)的LiDAR與目標(biāo)的距離為d，角度為，則轉(zhuǎn)換到直角坐標(biāo)系下的坐標(biāo)為：(2-2)2.2.2LiDAR數(shù)據(jù)傳輸流程數(shù)據(jù)傳輸流程為：圖2-4LiDAR數(shù)據(jù)傳輸流程

3SLAM原理與方法流程激光雷達(dá)作為傳感器可以獲取高精度的環(huán)境特征，提供的定位信息包括了位置、姿態(tài)等，根據(jù)是否有先驗(yàn)的地圖信息，激光雷達(dá)定位分為兩種模式，一種是基于已知地圖信息的絕對(duì)定位，另外一種是沒有地圖信息時(shí)的相對(duì)定位。絕對(duì)定位模式是根據(jù)激光雷達(dá)采集的點(diǎn)云與已有的地圖信息進(jìn)行匹配，從而獲取傳感器的絕對(duì)位置和姿態(tài)。相對(duì)定位模式也可以稱為同步定位與制圖技術(shù)，其地圖信息和定位是同步的：第一步基于起始?xì)v元的位置、傳感器姿態(tài)及觀測(cè)點(diǎn)云生成初始的地圖信息；第二步在隨后的歷元中將激光雷達(dá)采集的點(diǎn)云與該地圖信息匹配推算傳感器相對(duì)于起始狀態(tài)的位置和姿態(tài)，這一步為定位過程；第三步，利用基于當(dāng)前歷元的位置、姿態(tài)和觀測(cè)點(diǎn)云對(duì)初始地圖進(jìn)行更新，這一步為制圖過程，隨著激光雷達(dá)不斷觀測(cè)，不斷重復(fù)定位和制圖的過程[2]，如圖3-1所示。圖3-1SLAM流程圖3.1SLAM系統(tǒng)運(yùn)動(dòng)模型3.1.1系統(tǒng)模型對(duì)于SLAM的描述，采用已知的運(yùn)動(dòng)模型，在未知環(huán)境中，機(jī)器人通過自身攜帶的傳感器感知環(huán)境信息進(jìn)行自定位并不斷的更新周圍的環(huán)境地圖。機(jī)器人可根據(jù)運(yùn)動(dòng)模型確定初始位姿，利用觀測(cè)得到的環(huán)境特征進(jìn)行局部地圖的構(gòu)建，在機(jī)器人位姿改變的過程中，可以利用機(jī)器人內(nèi)部傳感器信息進(jìn)行狀態(tài)的更新，并利用觀測(cè)信息對(duì)狀態(tài)進(jìn)行矯正，同時(shí)增量式構(gòu)建地圖環(huán)境[7]。機(jī)器人在實(shí)現(xiàn)同時(shí)定位和地圖構(gòu)建的過程中，兩者的關(guān)系是相輔相成，不可或缺的。機(jī)器人根據(jù)自身的位姿信息預(yù)測(cè)出環(huán)境路標(biāo)特征的位置，然后根據(jù)激光雷達(dá)的觀測(cè)信息和內(nèi)部傳感器的測(cè)量信息，在預(yù)測(cè)的基礎(chǔ)上引出了觀測(cè)模型進(jìn)行矯正，實(shí)現(xiàn)室內(nèi)定位和地圖構(gòu)建。機(jī)器人在SLAM過程中，其狀態(tài)包括機(jī)器人自身姿態(tài)及路標(biāo)的相對(duì)位置[3]。3.1.2運(yùn)動(dòng)模型這里的運(yùn)動(dòng)模型為實(shí)驗(yàn)室移動(dòng)定位平臺(tái)。移動(dòng)定位平臺(tái)的位姿和環(huán)境地圖都是處于未知的狀態(tài)，不能直接觀測(cè)得到，需要依靠已知的傳感器信息和運(yùn)動(dòng)模型進(jìn)行估計(jì)。而機(jī)器人的運(yùn)動(dòng)模型就是用來表示機(jī)器人狀態(tài)隨時(shí)間變化的模型，其中，機(jī)器人的運(yùn)動(dòng)模型坐標(biāo)系是由機(jī)器人位姿，傳感器位置和環(huán)境特征相對(duì)機(jī)器人在環(huán)境中的位置這三個(gè)坐標(biāo)系組成的[3]。圖3-2移動(dòng)機(jī)器人運(yùn)動(dòng)模型根據(jù)運(yùn)動(dòng)模型，假定其初始位姿、傳感器位置以及環(huán)境特征相對(duì)機(jī)器人在環(huán)境中的位置，三者的初始狀態(tài)為0，運(yùn)動(dòng)狀態(tài)方程如下：(3-1)式中，L為移動(dòng)平臺(tái)車輪之間的距離，為運(yùn)動(dòng)控制中的噪聲。假設(shè)系統(tǒng)動(dòng)態(tài)方程為隨機(jī)非線性離散方程，系統(tǒng)方程為(3-2)式中，為k時(shí)刻系統(tǒng)在環(huán)境中的狀態(tài)向量，為系統(tǒng)運(yùn)動(dòng)過程中控制噪聲向量，為系統(tǒng)在k時(shí)刻的非線性狀態(tài)轉(zhuǎn)移矩陣。3.1.3觀測(cè)模型一般來說，觀測(cè)量是某個(gè)環(huán)境特征相對(duì)于傳感器的距離和方向[8]，但激光雷達(dá)傳感器或多或少都存在測(cè)量誤差，在機(jī)器人移動(dòng)位姿的過程中，誤差也在不斷地積累。因此，僅依靠機(jī)器人的內(nèi)部傳感器來確定機(jī)器人的位姿是不準(zhǔn)確的，需要融合外部傳感器的信息對(duì)環(huán)境進(jìn)行觀測(cè)[9]。而機(jī)器人在運(yùn)動(dòng)過程中的運(yùn)動(dòng)模型包含機(jī)器人的運(yùn)動(dòng)控制信息和觀測(cè)時(shí)的位置信息，這是完成地圖構(gòu)建與定位必不可少的條件信息，需結(jié)合運(yùn)動(dòng)模型，來估計(jì)機(jī)器人當(dāng)前時(shí)刻的位姿[3]。外部環(huán)境的觀測(cè)值包含了特征點(diǎn)到機(jī)器人的距離r和相對(duì)機(jī)器人的偏轉(zhuǎn)角度，可表示為：(3-3)觀測(cè)模型主要是根據(jù)傳感器測(cè)量單個(gè)路標(biāo)與機(jī)器人之間的相對(duì)距離，從而確定機(jī)器人在環(huán)境地圖中的相對(duì)位置，根據(jù)觀測(cè)量的不同其表示形式也不同，觀測(cè)方程也會(huì)隨之變化，一般的觀測(cè)模型可表示為：(3-4)式中，為k時(shí)刻傳感器觀測(cè)值，為傳感器測(cè)量函數(shù)，通常為非線性，為傳感器觀測(cè)噪聲，用于描述測(cè)量中的噪聲和模型本身的誤差。本文采用的是激光雷達(dá)傳感器，其觀測(cè)模型可寫為：(3-5)式中，為觀測(cè)到的第i個(gè)特征點(diǎn)的位置坐標(biāo)。此時(shí)，觀測(cè)模型的雅可比矩陣為：(3-6)式中，，，。3.2環(huán)境地圖表示方法機(jī)器人定位的精度與構(gòu)建環(huán)境地圖的精度直接相關(guān)，在機(jī)器人運(yùn)行過程中，機(jī)器人通過傳感器來對(duì)感知外部環(huán)境，從而對(duì)實(shí)現(xiàn)對(duì)所處環(huán)境地圖的描述和更新，它們之間的關(guān)系是相輔相成，互相依賴的。最常見的地圖構(gòu)建方法有柵格地圖、特征地圖和拓?fù)涞貓D。3.2.1柵格地圖最早是由Moravec和Elfes等提出了柵格地圖的概念。柵格地圖是將獲取到的周圍環(huán)境信息離散化為一個(gè)個(gè)的柵格單元，每個(gè)柵格單元有各自的概率值，介于0到1之間，每個(gè)柵格單元的值是對(duì)柵格內(nèi)障礙物所占比例的描述，當(dāng)柵格被完全占用時(shí)，其概率值為1，當(dāng)柵格內(nèi)無障礙物時(shí)，其概率值為0，當(dāng)柵格內(nèi)具體情況未知時(shí)其概率值為0.5，柵格地圖相較于其他兩種地圖的優(yōu)勢(shì)體現(xiàn)在：容易創(chuàng)建、且確定位置的唯一性、短路徑的規(guī)劃方便的優(yōu)點(diǎn),特別適合處理激光數(shù)據(jù)；但它的缺點(diǎn)是規(guī)劃效率不高,隨著地圖模型的擴(kuò)大，其所占的內(nèi)存和處理時(shí)間會(huì)增加,易造成空間浪費(fèi)。圖3-3柵格地圖3.2.2幾何特征地圖與柵格特征地圖相比，幾何特征地圖更適合于結(jié)構(gòu)化的環(huán)境，它是將環(huán)境內(nèi)的信息用一些離散的點(diǎn)或者線段來表示，通過觀測(cè)來對(duì)周圍環(huán)境進(jìn)行提取特征點(diǎn)，對(duì)于非結(jié)構(gòu)化的環(huán)境，幾何特征地圖很難達(dá)到其效果，很多環(huán)境特征很難用點(diǎn)、線精確的描述，此外，幾何特征地圖在描述細(xì)節(jié)方面不是很好，細(xì)節(jié)的缺失導(dǎo)致系統(tǒng)在進(jìn)行地圖匹配時(shí)失敗，這樣會(huì)對(duì)機(jī)器人進(jìn)行下一步的操作產(chǎn)生影響，位置和姿態(tài)信息估計(jì)不準(zhǔn)確。圖3-4幾何特征地圖3.2.3拓?fù)涞貓D拓?fù)涞貓D最早是由Brooks和Mataric等人提出，拓?fù)涞貓D是把室內(nèi)環(huán)境表示為節(jié)點(diǎn)和相關(guān)連接線的拓?fù)浣Y(jié)構(gòu)圖，其中節(jié)點(diǎn)表示環(huán)境中重要的特征點(diǎn)，線是不同節(jié)點(diǎn)之間的連接關(guān)系，在結(jié)構(gòu)化環(huán)境中，拓?fù)涞貓D是一種有效的表示方法，它的優(yōu)點(diǎn)是允許有效的路徑規(guī)劃，應(yīng)對(duì)復(fù)雜的空間環(huán)境，且不需要機(jī)器人的精確的位置信息,但同時(shí)由于節(jié)點(diǎn)表示的相似性，拓?fù)涞貓D在構(gòu)建大環(huán)境下的地圖時(shí)，在某些角度比較敏感，這樣在對(duì)某些特征點(diǎn)進(jìn)行識(shí)別時(shí)容易誤判，甚至不能識(shí)別。圖3-5拓?fù)涞貓D3.3基于EKF的SLAM3.3.1SLAM問題傳感器的觀測(cè)都會(huì)受到噪聲的干擾，所以在SLAM的過程中需要處理來自噪聲源的隨機(jī)誤差，從而實(shí)現(xiàn)最優(yōu)估計(jì)，概率論是最優(yōu)估計(jì)研究領(lǐng)域中一個(gè)重要理論工具，目前主流的SLAM建模方法都是基于概率模型來對(duì)噪聲建模，本質(zhì)上看，SLAM問題是一個(gè)基于貝葉斯的非線性狀態(tài)估計(jì)問題。假設(shè)一個(gè)在未知環(huán)境中移動(dòng)的傳感器觀測(cè)到一些位置未知，靜止的相對(duì)地標(biāo)點(diǎn)，SLAM問題就是通過觀測(cè)到的這些相對(duì)地標(biāo)點(diǎn)估計(jì)自身的運(yùn)動(dòng)軌跡及地標(biāo)點(diǎn)的位置。圖3-6SLAM問題描述如圖所示，在k時(shí)刻時(shí)，為傳感器的狀態(tài)向量（位置、姿態(tài)），為控制向量，為地標(biāo)點(diǎn)的位置向量，地標(biāo)點(diǎn)為靜止?fàn)顟B(tài)，位置不隨時(shí)間變化，為觀測(cè)的地標(biāo)點(diǎn)信息向量（包括多個(gè)地標(biāo)點(diǎn)的觀測(cè)信息等）。目前，解決SLAM問題，最常見的表示形式是基于高斯噪聲模型的高斯濾波器，主要使用擴(kuò)展卡爾曼濾波器（EKF）來解決SLAM問題，其將系統(tǒng)狀態(tài)向量的后驗(yàn)概率分布表示為多維高斯分布，后驗(yàn)概率分布轉(zhuǎn)換為求取系統(tǒng)狀態(tài)向量的期望和協(xié)方差矩陣，而EKF-SLAM相對(duì)于傳統(tǒng)的EKF有所不同，需要考慮狀態(tài)增廣和數(shù)據(jù)關(guān)聯(lián)等問題。3.3.2EKF-SLAM算法原理在EKF-SLAM算法中，k時(shí)刻的系統(tǒng)狀態(tài)向量包括傳感器的在參考坐標(biāo)下的位置，激光雷達(dá)坐標(biāo)系相對(duì)于參考坐標(biāo)系的姿態(tài)角和當(dāng)前已觀測(cè)到的所有點(diǎn)在參考坐標(biāo)系下的位置向量：(3-7)系統(tǒng)狀態(tài)向量的后驗(yàn)概率密度為多元高斯分布，表示為，其均值和協(xié)方差矩陣分別為高斯分布的一階矩和二階矩：(3-8)(3-9)EKF-SLAM與傳統(tǒng)的EKF算法相似，不同點(diǎn)在于當(dāng)觀測(cè)到新的地標(biāo)點(diǎn)時(shí)，需要將新地標(biāo)點(diǎn)的位置向量增廣到系統(tǒng)狀態(tài)向量中，其EKF-SLAM流程如下：第一步，根據(jù)狀態(tài)方程進(jìn)行時(shí)間更新：(3-10)(3-11)式中：(3-12)(3-13)Q為過程噪聲矩陣。第二步進(jìn)行數(shù)據(jù)關(guān)聯(lián)：(3-14)這一步是將當(dāng)前觀測(cè)到的地標(biāo)點(diǎn)與已觀測(cè)到的地標(biāo)點(diǎn)進(jìn)行匹配，分別找出觀測(cè)值中已存在的地標(biāo)點(diǎn)和新觀測(cè)到的地標(biāo)點(diǎn)。第三步根據(jù)已存在的地標(biāo)觀測(cè)值進(jìn)行量測(cè)更新：(3-15)(3-16)式中：(3-17)(3-18)(3-19)(3-20)為已存在地標(biāo)的觀測(cè)噪聲矩陣。第四步為系統(tǒng)狀態(tài)增廣：(3-21)(3-22)式中：(3-23)(3-24)(3-25)(3-26)(3-27)為新地標(biāo)的觀測(cè)噪聲矩陣。這一步是將觀測(cè)到的新地標(biāo)點(diǎn)的位置向量增廣到系統(tǒng)狀態(tài)向量中。3.4基于圖優(yōu)化的SLAM基于圖優(yōu)化理論的SLAM算法最早由LU和Milions在1997年提出，由于使用標(biāo)準(zhǔn)技術(shù)解決誤差最小化問題具有相當(dāng)高的復(fù)雜性，因此當(dāng)時(shí)沒有引起重視。直到2010年左右，隨著一些開源的稀疏矩陣運(yùn)算算法的出現(xiàn)，大大提高了基于圖優(yōu)化SLAM的計(jì)算效率。才逐漸成為了研究的熱點(diǎn)。基于圖優(yōu)化理論的SLAM算法目前已經(jīng)在速度和準(zhǔn)確性方面屬于最先進(jìn)的技術(shù)之一[1]。該方法應(yīng)用所有保存的觀測(cè)數(shù)據(jù)估計(jì)機(jī)器人的所有的軌跡，這解決了完全SLAM問題。該方法從觀測(cè)數(shù)據(jù)提取一組約束條件，用一個(gè)稀疏圖表示，如圖3-7所示，圖的節(jié)點(diǎn)表示機(jī)器人在該時(shí)刻的位姿與環(huán)境地圖，連接兩個(gè)節(jié)點(diǎn)的邊表示兩個(gè)節(jié)點(diǎn)之間的非線性約束關(guān)系，實(shí)線為任意兩個(gè)連續(xù)的機(jī)器人位姿間約束，虛線為位姿與機(jī)器人假定處于該位姿時(shí)觀測(cè)到的特征的約束關(guān)系。圖3-7位姿圖表示·3.4.1圖優(yōu)化理論圖3-8圖優(yōu)化流程基于圖優(yōu)化理論的SLAM分為兩個(gè)部分：一部分是前端（構(gòu)圖過程），前端根據(jù)傳感器數(shù)據(jù)來構(gòu)建圖并計(jì)算約束，約束由兩種形式構(gòu)成，一種是通過相鄰的幾個(gè)連續(xù)觀測(cè)的對(duì)應(yīng)關(guān)系得到，另一種是通過閉環(huán)檢測(cè)，檢測(cè)機(jī)器人是否回到了曾經(jīng)訪問過的位置從而添加閉環(huán)約束；另一部分是后端（優(yōu)化過程），后端根據(jù)前端提供的約束對(duì)圖進(jìn)行優(yōu)化，它通過將這些約束條件解析為全局一致的估計(jì)值來獲得地圖和機(jī)器人路徑。由于新的觀測(cè)加入到已有的位姿圖中時(shí)，只是考慮最近的幾次觀測(cè)，因此位姿圖會(huì)逐漸累積誤差，通過全局優(yōu)化問題可以將累積誤差除去，從而得到在給定的約束條件下的機(jī)器人運(yùn)動(dòng)軌跡的最優(yōu)估計(jì)。如圖3-8所示，基于圖優(yōu)化的SLAM算法交錯(cuò)執(zhí)行前端和后端，前端需要在部分優(yōu)化的地圖上進(jìn)行操作，以限制誤差的積累，當(dāng)前位姿估計(jì)越精確，前端生成的約束條件的魯棒性就越強(qiáng)，得到的地圖就越精準(zhǔn)。3.4.2Graph-basedSLAMGraph用一個(gè)圖來表示SLAM的過程，由于觀測(cè)噪聲和配準(zhǔn)誤差的存在，通過觀測(cè)信息配準(zhǔn)得到的位姿圖往往不具備一致性，如圖3-9所示。圖3-9實(shí)際軌跡與機(jī)器人軌跡外框?qū)嵕€部分為實(shí)際軌跡，內(nèi)框虛線為機(jī)器人移動(dòng)路線，從節(jié)點(diǎn)4節(jié)點(diǎn)1通過回環(huán)檢測(cè)可知，節(jié)點(diǎn)1和4有共視區(qū)域，通過幀間匹配，可得節(jié)點(diǎn)1和節(jié)點(diǎn)4之間的相對(duì)位姿，該位姿為觀測(cè)位姿z。節(jié)點(diǎn)1、節(jié)點(diǎn)2、節(jié)點(diǎn)3和節(jié)點(diǎn)4本身有位姿信息，通過里程計(jì)積分，則節(jié)點(diǎn)1和節(jié)點(diǎn)4之間的相對(duì)位姿為，該位姿為預(yù)測(cè)位姿。因?yàn)榻▓D過程中的誤差累積，，則，其中表示在以為原點(diǎn)的坐標(biāo)系中的坐標(biāo)，其設(shè)為b；節(jié)點(diǎn)1與節(jié)點(diǎn)4通過幀間匹配，兩者之間也有相對(duì)位姿（觀測(cè)位姿），也有在以為原點(diǎn)的坐標(biāo)系中的坐標(biāo),其設(shè)為a；則表示b在a坐標(biāo)系下的坐標(biāo)，其表示位姿誤差，通過非線性最小二乘最小化兩者誤差，得到一個(gè)位姿。3.4.3非線性最小二乘原理（1）給定一個(gè)系統(tǒng)，其狀態(tài)方程有描述，如圖3-10所示圖3-10狀態(tài)量的對(duì)應(yīng)函數(shù)其中：x為該系統(tǒng)的狀態(tài)向量—即需要估計(jì)的值（在激光SLAM中，為機(jī)器人位姿）；f(x)是一個(gè)非線性的映射函數(shù)，為傳感器的觀測(cè)模型；狀態(tài)向量x，可以通過非線性函數(shù)f(x)映射得到z；z表示系統(tǒng)的觀測(cè)值，可以通過傳感器進(jìn)行直接觀測(cè)，為激光數(shù)據(jù)或者圖像特征點(diǎn)。給定該系統(tǒng)的n個(gè)混有噪聲的觀測(cè)值（），估計(jì)狀態(tài)向量x,使得其經(jīng)過f(x)映射之后的預(yù)測(cè)值和觀測(cè)值的誤差（）最小。（2）誤差函數(shù)目標(biāo)為最小化預(yù)測(cè)和觀測(cè)的誤差，即誤差函數(shù)為；(3-28)假設(shè)誤差服從高斯分布，其對(duì)應(yīng)的信息矩陣為，因此該觀測(cè)值誤差的平方定義為： (3-29)因此，非線性最小二乘的目標(biāo)函數(shù)為：(3-30)(3-31)為關(guān)于x的非線性方程，可通過泰勒公式把其化為關(guān)于x的線性方程，是關(guān)于x的非線性函數(shù)的原因是，誤差函數(shù)是非線性函數(shù)，因此可直接對(duì)誤差函數(shù)進(jìn)行線性化：(3-32)式中，J為映射函數(shù)對(duì)狀態(tài)向量x的導(dǎo)數(shù)，稱之為Jacobian矩陣：(3-33)因此，函數(shù)可化解為：(3-34)式中，；；為關(guān)于變量的二次函數(shù)，則的極值可通過令其關(guān)于的導(dǎo)數(shù)等于0，求解得到：(3-35)(3-36)(3-37)令，然后不斷迭代，直至收斂即可。

4基于ROS的SLAM研究與設(shè)計(jì)4.1ROS的簡(jiǎn)介ROS是一個(gè)用于開發(fā)機(jī)器人應(yīng)用程序的機(jī)器人軟件平臺(tái)。ROS提供開發(fā)機(jī)器人應(yīng)用程序時(shí)所需的硬件抽象，子設(shè)備控制，以及機(jī)器人工程中廣泛使用的傳感、識(shí)別、繪圖、運(yùn)動(dòng)規(guī)劃等功能。此外ROS還提供進(jìn)程之間的消息解析、功能管理、庫(kù)和豐富的開發(fā)及調(diào)試工具。4.1.1機(jī)器人系統(tǒng)操作構(gòu)建主要介紹在使用UbuntuLinux14.04操作系統(tǒng)下Indigo版本的機(jī)器人操作平臺(tái)時(shí)，涉及到的功能模塊。ROS的主要功能模塊如下：（1）節(jié)點(diǎn)(Node)：是指在ROS中運(yùn)行的最小處理器單元?？梢园阉醋饕粋€(gè)可執(zhí)行的程序，一個(gè)節(jié)點(diǎn)表示一個(gè)運(yùn)行的程序，有唯一的目的性。比如，在移動(dòng)機(jī)器人的情況下，為了驅(qū)動(dòng)機(jī)器人，將每個(gè)細(xì)節(jié)細(xì)分化，也就是說，使用傳感器驅(qū)動(dòng)、傳感器數(shù)據(jù)轉(zhuǎn)換、障礙物判斷、電機(jī)驅(qū)動(dòng)、編碼器輸入和導(dǎo)航等多個(gè)細(xì)分節(jié)點(diǎn)。（2）功能包（package）：是構(gòu)成ROS的基本單元。ROS應(yīng)用程序是以功能包為單位開發(fā)的。功能包包括至少一個(gè)以上的節(jié)點(diǎn)或擁有用于運(yùn)行其他功能包的節(jié)點(diǎn)的配置文件。它還包含功能包所需的所有文件，例如ROS依賴庫(kù)可在各種進(jìn)程中運(yùn)行、數(shù)據(jù)集和配置文件等。在ROS運(yùn)行下可同時(shí)操作一個(gè)或多個(gè)功能。（3）消息(Message):節(jié)點(diǎn)之間通過消息（Message）來發(fā)送和接收數(shù)據(jù)。消息是如整型、浮點(diǎn)型、布爾型等變量。其中用戶還可以使用消息的簡(jiǎn)單數(shù)據(jù)結(jié)構(gòu)和列舉消息的消息數(shù)組的結(jié)構(gòu)。使用消息的通信方法包括TCPROS和UDPROS等，根據(jù)情況使用單向消息發(fā)送/接收方式的話題（topic）和雙向消息請(qǐng)求（request）/響應(yīng)（response）方式的服務(wù)（service）。（4）主題（Topic）：用于在節(jié)點(diǎn)之間傳輸數(shù)據(jù)的總線，節(jié)點(diǎn)通過向相應(yīng)主題發(fā)送消息來實(shí)現(xiàn)消息的發(fā)布，而主題是用于識(shí)別消息內(nèi)容的名稱，節(jié)點(diǎn)在工作中需要某種類型的數(shù)據(jù)，需要訂閱相關(guān)的主題，主題可以對(duì)應(yīng)多個(gè)主題訂閱者，即一個(gè)節(jié)點(diǎn)可以發(fā)布和訂閱多個(gè)主題。一般情況下，主題發(fā)布者和主題訂閱者在通信之間不知道對(duì)方的存在，只有當(dāng)發(fā)布者在工作空間中發(fā)布信息時(shí)，訂閱者會(huì)發(fā)現(xiàn)該消息是它所訂閱，即可以接收該消息。（5）服務(wù)（service）：服務(wù)（service）消息通信是服務(wù)客戶端（serviceclient）與服務(wù)服務(wù)器（serviceserver）之間的同步雙向消息通信。服務(wù)是一次性的消息通信，當(dāng)服務(wù)服務(wù)器和服務(wù)客戶端通信之后，兩個(gè)節(jié)點(diǎn)的連接會(huì)被斷開。其中，請(qǐng)求和回復(fù)通過服務(wù)來進(jìn)行，兩者被定義為一對(duì)消息結(jié)構(gòu)：一個(gè)表示請(qǐng)求，另一個(gè)表示回復(fù)，當(dāng)一個(gè)節(jié)點(diǎn)提供某一個(gè)服務(wù)時(shí)，所有的節(jié)點(diǎn)都可以通過ROS客戶端編寫的代碼與之通信來使用此服務(wù)。（6）roscore：roscore是運(yùn)行ROS主節(jié)點(diǎn)的命令?？稍谕痪W(wǎng)絡(luò)中不同的計(jì)算機(jī)上運(yùn)行。但除了支持多rosccore的某些特殊情況，roscore一般在網(wǎng)絡(luò)中只能運(yùn)行一個(gè)，在運(yùn)行ROS時(shí)，將使用在ROS_MASTER_URI變量中列出的URI地址和端口。（7）roslaunch：如果rosrun是執(zhí)行一個(gè)節(jié)點(diǎn)的命令，那么roslaunch是運(yùn)行多個(gè)節(jié)點(diǎn)的概念。該命令允許運(yùn)行多個(gè)確定的節(jié)點(diǎn)。其他功能還包括一些專為執(zhí)行具有諸多選項(xiàng)的節(jié)點(diǎn)的ROS命令，比如包括更改功能包參數(shù)或節(jié)點(diǎn)名稱、配置節(jié)點(diǎn)命名空間、設(shè)置ROS_ROOT和ROS_PACKAGE_PATH以及更改環(huán)境變量等。4.1.2遠(yuǎn)程調(diào)用下的通信機(jī)制在ROS中有一個(gè)主控制器Master，負(fù)責(zé)管理和協(xié)調(diào)各個(gè)不同模塊在運(yùn)行過程中的執(zhí)行任務(wù)。主要包括節(jié)點(diǎn)與節(jié)點(diǎn)之間的連接和消息通信。類似于名稱注冊(cè)器（Nameserver）。roscore是主節(jié)點(diǎn)的運(yùn)行命令。在運(yùn)行主節(jié)點(diǎn)時(shí)，需要根據(jù)獲取的信息注冊(cè)節(jié)點(diǎn)的名字。沒有主節(jié)點(diǎn)，則節(jié)點(diǎn)之間不能建立消息通信和相互訪問。主節(jié)點(diǎn)使用XML遠(yuǎn)程過程調(diào)用與節(jié)點(diǎn)進(jìn)行通信，節(jié)點(diǎn)在需要注冊(cè)自己的信息或向其他節(jié)點(diǎn)發(fā)送請(qǐng)求信息時(shí)才能訪問主節(jié)點(diǎn)并獲取信息。一般情況下，不檢查彼此的連接狀態(tài)。由于這些特點(diǎn)，ROS可用于相對(duì)較大的環(huán)境，且支持多種語言。主節(jié)點(diǎn)控制節(jié)點(diǎn)間訂閱和發(fā)布消息的模型如圖4-1所示。圖4-1訂閱發(fā)布信息控制示意圖圖中的Talker和Listener各為一個(gè)節(jié)點(diǎn)，Talker節(jié)點(diǎn)向ROSMaster申請(qǐng)注冊(cè)名為bar的Topic，其接受端口為：1234，Listener節(jié)點(diǎn)ROSMaster發(fā)送訂閱該主題的請(qǐng)求，收到請(qǐng)求后，ROSMaster將主題端口發(fā)送給Listener節(jié)點(diǎn)，通過端口號(hào)，兩個(gè)節(jié)點(diǎn)就可以實(shí)現(xiàn)信息交互了。4.2ROS中常用的SLAM算法4.2.1Cartographer算法原理Cartographer是google公司發(fā)布的SLAM解決方案，這一方案的主要思路是利用閉環(huán)檢測(cè)來削弱構(gòu)圖過程中出現(xiàn)的誤差累積。子圖（submap）是閉環(huán)檢測(cè)的基本單元，由特定數(shù)目的雷達(dá)掃描數(shù)據(jù)組成。將雷達(dá)數(shù)據(jù)插入相應(yīng)的子圖時(shí)，可以假設(shè)子圖的創(chuàng)建在短時(shí)間內(nèi)誤差累積是足夠小的，然后基于子圖已有的激光雷達(dá)或者其它傳感器數(shù)據(jù)估算其在該子圖中的最適合位置。創(chuàng)建的子圖數(shù)量會(huì)隨著時(shí)間增多，子圖之間的累積誤差則會(huì)越來越大。因此需要利用閉環(huán)檢測(cè)來改善這些子圖的位姿以消除累計(jì)的誤差，從而將問題轉(zhuǎn)變成一個(gè)位姿的改善問題。當(dāng)一個(gè)子圖的構(gòu)造結(jié)束后，即不再有新的雷達(dá)數(shù)據(jù)加入到該子圖時(shí)，該子圖就會(huì)進(jìn)入到閉環(huán)檢測(cè)中。當(dāng)一個(gè)實(shí)時(shí)的雷達(dá)掃描數(shù)據(jù)插入到地圖中時(shí)，如果該雷達(dá)掃描的估算位姿與地圖中某個(gè)子圖的某個(gè)雷達(dá)掃描的位姿相對(duì)差別不大的話，通過掃描匹配就可以找到該閉環(huán)。Cartographer的掃描匹配方案是采用在新插入地圖的激光雷達(dá)掃描估算位姿附近選定一個(gè)區(qū)間窗，然后在該窗口中查找該掃描的一個(gè)大概率匹配，當(dāng)查找到最適宜的匹配，就在位姿改善的問題中添加該匹配的閉環(huán)約束[5]，系統(tǒng)框圖如圖4-2所示。圖4-2Cartographer系統(tǒng)框圖系統(tǒng)在實(shí)現(xiàn)平面SLAM問題上結(jié)合了局部和全局的方法，兩者都對(duì)雷達(dá)的觀測(cè)位姿進(jìn)行了優(yōu)化，可設(shè)激光雷達(dá)的觀測(cè)點(diǎn)包含坐標(biāo)的轉(zhuǎn)換和方位角，也稱為一個(gè)掃描點(diǎn)。局部方法中，每個(gè)連續(xù)的掃描點(diǎn)被用來和子地圖做匹配，匹配的過程中是使用一種非線性的優(yōu)化方法將掃描點(diǎn)和子圖聯(lián)系在一起，這一局部方法最終會(huì)累積誤差，但在全局方法中誤差將會(huì)濾除。在掃描點(diǎn)插入子圖之前，利用Ceres掃描匹配方法相對(duì)于局部子地圖來優(yōu)化掃描點(diǎn)，這一過程將找到掃描點(diǎn)在子圖中最可能出現(xiàn)的位置[5]。而全局優(yōu)化是通過閉環(huán)檢測(cè)來實(shí)現(xiàn)的。激光雷達(dá)掃描幀僅與一些相鄰的掃描幀的子地圖匹配，因此，會(huì)在構(gòu)建地圖中造成緩慢累積誤差，這就需要全局優(yōu)化消除累積誤差。當(dāng)激光雷達(dá)掃描幀與其子地圖相匹配時(shí)，插入子地圖中的掃描姿態(tài)會(huì)存儲(chǔ)在內(nèi)存中。當(dāng)創(chuàng)建子地圖時(shí)，相應(yīng)的掃描幀和子地圖會(huì)用于閉環(huán)檢測(cè)，并且所有的掃描匹配都優(yōu)化過程中進(jìn)行。在全局優(yōu)化過程中需要有良好的閉環(huán)匹配。4.2.2HectorSLAM算法原理HectorSLAM是在2DSLAM的基礎(chǔ)上，通過采用更高頻率以及更小噪聲的激光掃描儀，配合更加穩(wěn)定且抗變換性強(qiáng)的掃描方法，使得實(shí)驗(yàn)移動(dòng)定位平臺(tái)在顛簸的區(qū)域中和無人機(jī)在復(fù)雜的環(huán)境中完成SLAM成為可能，因此Hector無法使用里程計(jì)。具體過程可理解為激光掃描儀通過對(duì)周圍環(huán)境進(jìn)行掃描，得到激光點(diǎn)集與已經(jīng)獲取的地圖進(jìn)行匹配，對(duì)其點(diǎn)陣進(jìn)行優(yōu)化，匹配時(shí)用高斯牛頓的方法進(jìn)行解算，這樣就可以得到激光點(diǎn)集在現(xiàn)有地圖上的位置(x,y,θ),匹配過程中為了排除出現(xiàn)最小而非全局最優(yōu)的情況，地圖多用多種分辨率的形式。4.2.3Gmapping算法原理Gmapping是一種基于LiDARSLAM的算法，可以在ROS平臺(tái)下調(diào)用，是機(jī)器人領(lǐng)域中使用最廣的SLAM算法，Gmapping在實(shí)現(xiàn)室內(nèi)定位和地圖構(gòu)建時(shí)，其突出的優(yōu)勢(shì)為在構(gòu)建小環(huán)境地圖時(shí)，可保證其定位的精度且計(jì)算量較小，與HectorSLAM算法相比，對(duì)激光雷達(dá)頻率要求較低，可利用里程計(jì)數(shù)據(jù)來預(yù)測(cè)機(jī)器人的位姿先驗(yàn)，且有較好的穩(wěn)定性。在構(gòu)建小地圖環(huán)境時(shí)，其不需要太多的粒子，并不需要回環(huán)檢測(cè)。Gmapping是由Grisetti等人提出的基于Rao-Blackellized的粒子濾波的SLAM方法。即將定位和建圖過程分離，先進(jìn)行定位再進(jìn)行建圖。粒子濾波的方法一般需要大量的粒子才能獲得較好的結(jié)果，在使用Gmapping算法構(gòu)建大環(huán)境地圖時(shí)，由于每個(gè)粒子都會(huì)攜帶一副地圖，隨著掃描場(chǎng)景的增加，從而會(huì)增加該算法的計(jì)算復(fù)雜性和內(nèi)存。而且沒有回環(huán)檢測(cè)，會(huì)造成大環(huán)境地圖會(huì)閉合錯(cuò)位。在此基礎(chǔ)上，專家學(xué)者引入了選擇性重采樣在Gmapping中，選擇性重采樣的過程相關(guān)的粒子退化耗盡問題也降低了算法的準(zhǔn)確性，為了避免粒子耗散問題，引入自適應(yīng)重采樣技術(shù)。計(jì)算粒子分布的時(shí)候不單單僅依靠機(jī)器人的里程計(jì)，同時(shí)將當(dāng)前觀測(cè)考慮進(jìn)去，減少了機(jī)器人位置在粒子濾波步驟的不確定性[4]。本文分別使用Cartographer算法與Gmapping算法來實(shí)現(xiàn)室內(nèi)定位與構(gòu)圖，其中Gmapping算法在構(gòu)建小環(huán)境地圖時(shí)，計(jì)算量較小且精度較高。對(duì)激光雷達(dá)頻率要求較低，魯棒性高，但若使用在大環(huán)境中時(shí)，所需的粒子會(huì)增加，內(nèi)存和計(jì)算量會(huì)隨之上升，并且沒有回環(huán)檢測(cè)，可能會(huì)造成地圖錯(cuò)位；相比較而言Cartographer累計(jì)誤差較低，能天然的輸出協(xié)方差矩陣，后端優(yōu)化的輸入項(xiàng)。且在構(gòu)建大環(huán)境地圖時(shí)，其優(yōu)化相當(dāng)于使用一個(gè)粒子來攜帶地圖，其計(jì)算量相對(duì)較小，節(jié)約內(nèi)存空間。且對(duì)于傳感器沒有較高的要求，先有一定數(shù)量的激光掃描構(gòu)建子地圖，由子地圖拼接成地圖，所謂的回環(huán)檢測(cè)，就是用最近的掃描幀進(jìn)行匹配，找出幀的約束關(guān)系，起到減少累積誤差的作用，進(jìn)行所有子地圖的圖優(yōu)化。

5實(shí)驗(yàn)與分析5.1實(shí)驗(yàn)平臺(tái)搭建5.1.1硬件設(shè)備本次實(shí)驗(yàn)所采用的是RPLIDARA1、一臺(tái)全站儀（含棱鏡）、一臺(tái)筆記本電腦，移動(dòng)定位平臺(tái)。首先，將移動(dòng)定位平臺(tái)上的LiDAR通過USB接口連接至電腦，實(shí)時(shí)獲取數(shù)據(jù)并進(jìn)行相關(guān)數(shù)據(jù)的存儲(chǔ)；其次利用LeicaTS50全站儀確定LiDAR起始點(diǎn)坐標(biāo)；最后移動(dòng)定位平臺(tái)從起始點(diǎn)出發(fā)，全站儀實(shí)時(shí)跟蹤并測(cè)量移動(dòng)定位平臺(tái)的位置，提供參考軌跡。實(shí)驗(yàn)平臺(tái)搭建如圖5-1所示。圖5-1實(shí)驗(yàn)設(shè)備搭建5.1.2實(shí)驗(yàn)軟件LiDAR的軟件平臺(tái)采用UbuntuLinux14.04操作系統(tǒng)下Indigo版本的ROS，其具有點(diǎn)對(duì)點(diǎn)設(shè)計(jì)、支持多種編程語言以及代碼免費(fèi)與開源等優(yōu)點(diǎn)，是機(jī)器人領(lǐng)域的研究熱點(diǎn)。LiDARSLAM的調(diào)試以及界面顯示使用ROS系統(tǒng)自帶的可視化三維操作工具RVIZ，如圖5-2所示，其可構(gòu)建LiDARSLAM系統(tǒng)模型和環(huán)境地圖，也可實(shí)時(shí)查看LiDARSLAM導(dǎo)航定位的全部過程。圖5-2可視化操作工具RVIZ5.2室內(nèi)定位與制圖實(shí)驗(yàn)LiDAR定位與制圖實(shí)驗(yàn)是在測(cè)繪樓414進(jìn)行的，實(shí)驗(yàn)場(chǎng)景如圖5-3所示。其中移動(dòng)定位平臺(tái)并未設(shè)定固定軌跡，沿地板瓷磚推行，使其運(yùn)動(dòng)軌跡呈環(huán)狀，移動(dòng)軌跡如圖5-4所示。圖5-3實(shí)驗(yàn)場(chǎng)景1圖5-4移動(dòng)軌跡圖通過ROS平臺(tái)利用EKF-SLAM和圖優(yōu)化算法進(jìn)行室內(nèi)定位和地圖構(gòu)建，生成的定位軌跡與參考軌跡進(jìn)行比較，分析不同算法的優(yōu)缺點(diǎn)。首先在電腦上啟動(dòng)ROS操作系統(tǒng)，利用命令分別調(diào)用EKF-SLAM和圖優(yōu)化算法程序，啟動(dòng)所需的節(jié)點(diǎn)系統(tǒng)，具體操作過程如下：（1）啟動(dòng)節(jié)點(diǎn)管理器：$roscore，在啟動(dòng)ROS操作系統(tǒng)之前首先啟動(dòng)該程序，才能節(jié)點(diǎn)信息進(jìn)行管理，保證節(jié)點(diǎn)之間的通信情況良好。（2）輸入命令，啟動(dòng)地圖操作，保證能夠接收激光雷達(dá)數(shù)據(jù)，并能夠進(jìn)行實(shí)時(shí)構(gòu)建地圖與更新，如圖5-5所示。圖5-5命令行模式顯示第一個(gè)命令行模式：給予ROS平臺(tái)授權(quán)，可讀取執(zhí)行LIDAR數(shù)據(jù)；第二個(gè)命令行模式：驅(qū)動(dòng)LIDAR，使其開始采集數(shù)據(jù)；第三個(gè)命令行模式：輸入命令，進(jìn)入ROS平臺(tái)的RVIZ軟件，開始進(jìn)行定位和制圖；第四個(gè)命令行模式：實(shí)時(shí)保存定位數(shù)據(jù)；第五個(gè)命令行模式：保存已構(gòu)建地圖數(shù)據(jù)。在測(cè)繪414的室內(nèi)定位與制圖實(shí)驗(yàn)中，可得：LiDARSLAM的制圖結(jié)果如圖5-5、5-6所示，本文兩種方法解算的軌跡以及全站儀測(cè)量的參考軌跡如圖5-7所示，藍(lán)色線段表示EKF-SLAM算法獲取的移動(dòng)定位平臺(tái)的移動(dòng)軌跡，紅色線段表示圖優(yōu)化算法獲取的移動(dòng)定位平臺(tái)的移動(dòng)軌跡，黑色線段表示全站儀獲得的移動(dòng)定位平臺(tái)的移動(dòng)軌跡。圖5-5EKF-SLAM制圖結(jié)果圖5-6圖優(yōu)化SLAM的制圖結(jié)果圖5-7