[28]。（2-SEQ2-\*ARABIC6）上文求出的Neff是一個(gè)可以很好估計(jì)重要性權(quán)重分散的度量，當(dāng)Neff小于Cartographer算法原理Cartographer算法的核心部分包括傳感器數(shù)據(jù)輸入（InputSensorData）、局部建圖（LocalSLAM）和全局建圖（GlobalSLAM）。首先Cartographer結(jié)合了許多不同的傳感器數(shù)據(jù)（慣性測量單元數(shù)據(jù)、雷達(dá)數(shù)據(jù)、里程計(jì)數(shù)據(jù)等）創(chuàng)建Submap子圖，具體過程是將一個(gè)激光掃描幀（Laserscan）插入相應(yīng)的Submap，利用Submap子圖已有的Laserscan以及其他的傳感器估計(jì)其在Submap中最佳的位置和姿態(tài)，這其實(shí)是一種局部的位姿優(yōu)化。隨著Submap數(shù)量的不斷增多，累計(jì)誤差不斷的增大，這時(shí)就需要對位姿進(jìn)行進(jìn)一步的優(yōu)化。Cartographer算法會隔一段時(shí)間，進(jìn)行全局優(yōu)化，也就是當(dāng)新的激光掃描幀插入時(shí)，利用Scanmatch策略尋找匹配的閉環(huán)（閉環(huán)會考慮之前所有創(chuàng)建的Submap），并將匹配的閉環(huán)加入位姿優(yōu)化圖2-SEQ圖2-\*ARABIC3Cartographer算法原理流程傳感器數(shù)據(jù)首先經(jīng)過體素濾波器（VoxelFilter）和自適應(yīng)體素濾波器（AdaptiveVoxelFilter）過濾的距離信息（RangeData），其中體素濾波器的作用：激光雷達(dá)掃描點(diǎn)云數(shù)目大，重疊相近的點(diǎn)會占用很多內(nèi)存空間，體素濾波器在保證點(diǎn)云的形狀特征的前提下對點(diǎn)云采取降采樣操作,極大縮減了點(diǎn)云的數(shù)量量級。里程計(jì)數(shù)據(jù)為輪式移動機(jī)器人左右輪的行駛距離，通過解算可提供機(jī)器人位置信息和轉(zhuǎn)角信息，可用于估計(jì)位姿。IMU數(shù)據(jù)可提供線加速度和角速度信息，通過IMUTracker進(jìn)行數(shù)據(jù)解算，也可得到機(jī)器人的位姿PoseExtrapolator結(jié)合里程計(jì)和IMU的位姿數(shù)據(jù)，得到更準(zhǔn)確的位姿數(shù)據(jù)。局部SLAM局部SLAM的作用是優(yōu)化掃描幀位姿建立每一個(gè)良好的子圖(Submap)。Submap構(gòu)造是將激光雷達(dá)數(shù)據(jù)scans坐標(biāo)幀與Submap坐標(biāo)幀(簡稱幀)反復(fù)對齊的迭代過程。掃描坐標(biāo)系下的位姿ξ在子圖坐標(biāo)系要經(jīng)過轉(zhuǎn)換??,它將掃描點(diǎn)從掃描幀嚴(yán)格變換為子圖幀，定義為公式REF_Ref71920827\h（2-7）所示，左邊矩陣為旋轉(zhuǎn)矩陣，右邊矩陣代表平移矩陣。（2-SEQ2-\*ARABIC7）每一個(gè)掃描幀在形成子圖前都要經(jīng)過靜止濾波器的過濾，靜止濾波器的作用是：為了避免在每個(gè)子圖像中插入過多的激光掃描數(shù)據(jù)，一旦掃描匹配器生成一個(gè)新的姿態(tài)，它計(jì)算姿態(tài)和姿態(tài)變化，并調(diào)用靜態(tài)過濾器。如果更改不夠重要（或更改非常?。?，這一幀掃描數(shù)據(jù)將被刪除。減少了數(shù)據(jù)處理量，降低了內(nèi)存消耗。局部地圖是以分辨率為邊長的概率柵格，由連續(xù)的激光掃描幀匹配組合而成，當(dāng)創(chuàng)建完子圖時(shí)，遍歷所有柵格的概率值，當(dāng)柵格概率＜pmin則該柵格處于自由空間，當(dāng)概率在pmin,圖2-SEQ圖2-\*ARABIC4概率柵格激光掃描的每一幀數(shù)據(jù)都會兩組數(shù)據(jù)：“擊中柵格點(diǎn)”與“未擊中柵格點(diǎn)”,如REF_Ref71921122\hREF_Ref72154362\h圖2-5概率柵格地圖所示。帶叉的是激光掃中的柵格，陰影的是原點(diǎn)到激光掃中點(diǎn)的線段掠過的所有柵格。圖中既在陰影范圍內(nèi)有有×符號的柵格點(diǎn)為“擊中柵格點(diǎn)”,只帶陰影不帶×符號的柵格點(diǎn)為“未擊中柵格點(diǎn)”,每個(gè)“擊中柵格點(diǎn)”都初始化概率值,每個(gè)“未擊中柵格點(diǎn)”被初始化概率值為。如果該柵格點(diǎn)賦值之前已經(jīng)有了相應(yīng)的概率值，那么將此柵格點(diǎn)的概率值進(jìn)行如公式REF_Ref71921212\h（2-8）的更新，其中clamp限定了函數(shù)的輸出數(shù)值在一定的區(qū)域范圍內(nèi)。（2-SEQ2-\*ARABIC8）圖2-SEQ圖2-\*ARABIC5概率柵格概率更新方法Scanmatching指將不同時(shí)刻的傳感器數(shù)據(jù)關(guān)聯(lián)起來，尋找其對應(yīng)關(guān)系，獲得被測物體的完整幾何信息。每一個(gè)掃描幀在插入到子圖中都要經(jīng)過一個(gè)掃描匹配優(yōu)化器，通過這個(gè)優(yōu)化器來獲取掃描幀相對于當(dāng)前子圖位姿的最佳估計(jì)值ξ。掃描匹配器尋找最佳位姿的估計(jì)的本質(zhì)也是非線性最小二乘優(yōu)化為題，當(dāng)掃描真以這個(gè)估計(jì)姿態(tài)ξ插入到子圖中時(shí)，可以最大化掃描點(diǎn)在子圖中的概率值。最小二乘問題由谷歌自家的ceres庫解決。其問題可以描述為：（2-SEQ2-\*ARABIC9）Tξ將點(diǎn)云轉(zhuǎn)換到概率柵格坐標(biāo)下，M全局SLAM全局SLAM的目的是將所有已經(jīng)創(chuàng)建好的子圖的位姿進(jìn)行全局優(yōu)化，將子圖正確拼接成環(huán)境地圖。Cartographer在構(gòu)建巨大場景的地圖時(shí)以子圖為小的建圖單元，子圖在相對較短的時(shí)間內(nèi)時(shí)相對準(zhǔn)確的，隨著子圖數(shù)目的增加誤差會不斷的積累，為了減少隨著時(shí)間而積累的誤差項(xiàng)，需要全局的回環(huán)檢測來提供約束關(guān)系進(jìn)行全局的圖優(yōu)化修正。閉環(huán)檢測是指：如果目前正在處理的掃描和所有已創(chuàng)建完成的子圖中的某個(gè)掃描幀的位置和姿態(tài)足夠的接近，就可以檢測到閉環(huán)，本質(zhì)上是新的scan與已構(gòu)成submap的scan的一種匹配。圖優(yōu)化的過程：根據(jù)當(dāng)前掃描的姿態(tài)和最近的一個(gè)子地圖優(yōu)化所有子地圖的姿態(tài)，即使殘差E最小，殘差E的計(jì)算如REF_Ref72246544\h（2-10所示，實(shí)際上也是一個(gè)非線性最小二乘優(yōu)化的過程。ξim為子圖的位姿；ξis為掃描的位姿；Σij相關(guān)協(xié)方差矩陣；（2-SEQ2-\*ARABIC10）圖優(yōu)化的效果如REF_Ref72149117\h圖2-6所示，cartographer官方的整體框架如REF_Ref72149133\h圖2-7所示(a)圖優(yōu)化之前(b)圖優(yōu)化之后圖2-SEQ圖2-\*ARABIC6全局SLAM效果圖2-SEQ圖2-\*ARABIC7Cartographer算法官方框架Cartographer的整體代碼結(jié)構(gòu)如REF_Ref72149198\h圖2-8所示，Cartographer_ros作為ROS環(huán)境下的接口，可以調(diào)用ROS中的工具tf，訂閱ROS環(huán)境下相應(yīng)傳感器的話題（/odom、/imu、/scan），并且發(fā)布相應(yīng)的建圖數(shù)據(jù)到/map話題中。而Cartographer_ros調(diào)用