機(jī)器人SLAM技術(shù)及其ROS系統(tǒng)應(yīng)用 第2版 課件 第3、4章 基于ROS系統(tǒng)的SLAM技術(shù)、基于特征估計(jì)的激光SLAM技術(shù)

基于ROS系統(tǒng)的SLAM技術(shù)第三章目錄CONTENTS01ROS系統(tǒng)02基于ROS系統(tǒng)的機(jī)器人實(shí)踐03基于ROS系統(tǒng)的SLAM開源方案01ROS系統(tǒng)ROS從某種意義上說是一個(gè)操作系統(tǒng)（OS），它能夠提供類似操作系統(tǒng)所能提供的功能，包括硬件抽象描述、底層啟動(dòng)管理、公共功能的執(zhí)行、程序間消息的傳遞以及程序發(fā)行包管理，它也能提供工具程序和庫，用于獲取、建立、編寫和運(yùn)行多機(jī)整合的程序。ROS的首要設(shè)計(jì)目標(biāo)是在機(jī)器人研發(fā)領(lǐng)域提高代碼的復(fù)用率。ROS系統(tǒng)的應(yīng)用1.ROS的起源2.ROS的五個(gè)特點(diǎn)（1）點(diǎn)對(duì)點(diǎn)設(shè)計(jì)節(jié)點(diǎn)單元分布式網(wǎng)絡(luò)RPC+TCP/UDP通信系統(tǒng)（2）多語言支持支持Python、C++、Java等編程語言語言無關(guān)的接口定義（3）精簡與集成每個(gè)功能節(jié)點(diǎn)可以單獨(dú)編譯集成眾多開源項(xiàng)目接口統(tǒng)一、提高軟件復(fù)用性（4）工具包豐富3D可視化工具—rviz物理仿真環(huán)境—gazebo數(shù)據(jù)記錄工具—rosbagQT工具箱—rqt_*rvizrqt_plot（5）免費(fèi)且開源BSD許可，可修改、可復(fù)用、可商用軟件包數(shù)量指數(shù)級(jí)增加，良好的生態(tài)系統(tǒng)3.ROS文件系統(tǒng)級(jí)文件系統(tǒng)級(jí)是用于描述可以在硬盤上查找到的代碼及可執(zhí)行文件程序，在這一級(jí)中將使用一組概念來解釋ROS的內(nèi)部組成、文件架構(gòu)以及工作所需的核心文件。ROS文件系統(tǒng)主要概念（1）功能包（package）：功能包是ROS軟件組織的基本形式，是構(gòu)成ROS的基本單元。一個(gè)功能包可以包含多個(gè)可執(zhí)行文件（節(jié)點(diǎn)）。（2）功能包清單（packagemanifest）：記錄功能包的基本信息。（3）綜合功能包（Metapackage）：也稱元功能包，將幾個(gè)具有某些功能的功能包組織在一起。（4）綜合功能包清單（Metapackagemanifest）：會(huì)包含運(yùn)行時(shí)所需要依賴的功能包或者聲明一些引用的標(biāo)簽。（5）消息類型（.msg）：消息是ROS節(jié)點(diǎn)之間發(fā)布/訂閱的通信消息。（6）服務(wù)類型（.srv）：服務(wù)類型定義了ROS服務(wù)器/客戶端通信模型下的請(qǐng)求與應(yīng)答數(shù)據(jù)類型。文件系統(tǒng)級(jí)4.ROS計(jì)算圖級(jí)計(jì)算圖級(jí)體現(xiàn)的是進(jìn)程與系統(tǒng)之間的通信，描述程序是如何運(yùn)行的。ROS會(huì)創(chuàng)建一個(gè)連接所有進(jìn)程的網(wǎng)絡(luò)，子系統(tǒng)中的任何節(jié)點(diǎn)都可以訪問此網(wǎng)絡(luò)，并通過該網(wǎng)絡(luò)與其他節(jié)點(diǎn)交互，獲取其他節(jié)點(diǎn)發(fā)布的消息，并將自身的數(shù)據(jù)發(fā)布到網(wǎng)絡(luò)上。計(jì)算圖級(jí)（1）節(jié)點(diǎn)（Node）:執(zhí)行運(yùn)算任務(wù)的進(jìn)程。在ROS系統(tǒng)中，節(jié)點(diǎn)是最小的進(jìn)程單元。一個(gè)軟件包里面可以有多個(gè)可執(zhí)行文件，可執(zhí)行文件在運(yùn)行之后就成了一個(gè)進(jìn)程（這個(gè)進(jìn)程便是一個(gè)節(jié)點(diǎn)）。（2）節(jié)點(diǎn)管理器(NodeMaster):master在整個(gè)網(wǎng)絡(luò)通信架構(gòu)里相當(dāng)于管理中心，管理著各個(gè)node。node首先在master處進(jìn)行注冊(cè)，之后master會(huì)將該node納入整個(gè)ROS程序中。（3）消息（Message）:節(jié)點(diǎn)之間最重要的通信機(jī)制就是基于發(fā)布/訂閱模型的消息通信。消息包含一個(gè)節(jié)點(diǎn)發(fā)送到其他節(jié)點(diǎn)的數(shù)據(jù)信息，節(jié)點(diǎn)是通過消息實(shí)現(xiàn)彼此的邏輯聯(lián)系與數(shù)據(jù)交換。（4）主題（Topic）:單向異步通信機(jī)制，傳輸消息(Message)。在這種機(jī)制中，Message以一種發(fā)布/訂閱的方式進(jìn)行傳遞(Publish-Subscribe).每個(gè)Topic的消息類型都是強(qiáng)類型，發(fā)布到其上的Message都必須與Topic的ROS消息類型匹配，而且節(jié)點(diǎn)只能接收類型匹配的消息。單向異步通信機(jī)制（5）服務(wù)（Service）:雙向同步通信機(jī)制，ROS中稱其為“服務(wù)”，傳輸請(qǐng)求/應(yīng)答數(shù)據(jù)，是一個(gè)request-reply模型.與主題不同的是，ROS中只允許有一個(gè)節(jié)點(diǎn)提供指定命名的服務(wù)。這種機(jī)制不僅可以發(fā)送消息，還存在反饋。雙向同步通信機(jī)制（6）參數(shù)服務(wù)器（ParameterServer）:參數(shù)服務(wù)器能夠是數(shù)據(jù)通過關(guān)鍵詞存儲(chǔ)在一個(gè)系統(tǒng)的核心位置（節(jié)點(diǎn)管理器）。通過使用參數(shù)就能夠在運(yùn)行時(shí)配置節(jié)點(diǎn)或改變節(jié)點(diǎn)的工作任務(wù)。（7）消息記錄包（Bag）:用于記錄和回放ROS消息數(shù)據(jù)的文件格式，保存在.bag文件中。Bag是一種用于存儲(chǔ)數(shù)據(jù)的重要機(jī)制，它可以獲取并且記錄各種難以收集的傳感器數(shù)據(jù)，程序可以通過Bag反復(fù)獲取實(shí)驗(yàn)數(shù)據(jù)，常用于調(diào)試算法。rosparam支持參數(shù)6.ROS開源社區(qū)級(jí)（1）發(fā)行版（Distribution）：ROS發(fā)行版包括一系列帶有版本號(hào)、可以直接安裝的功能包。（2）軟件源（Repository）：ROS依賴于共享網(wǎng)絡(luò)上的開源代碼，不同的組織機(jī)構(gòu)可以開發(fā)或者共享自己的機(jī)器人軟件。（3）ROS維基（ROSwiki）：記錄ROS信息文檔的主要論壇。（4）郵件列表（Mailinglist）：交流ROS更新的主要渠道，同時(shí)也可以交流ROS開發(fā)的各種疑問。（5）ROS問答（ROSAnswer）：咨詢ROS相關(guān)問題的網(wǎng)站。（6）博客（Blog）：發(fā)布ROS社區(qū)中的新聞、圖片、視頻等，網(wǎng)址是/news。ROS開源社區(qū)ROS系統(tǒng)實(shí)現(xiàn)三個(gè)層次02基于ROS系統(tǒng)的機(jī)器人實(shí)踐

TurtleBot第一代發(fā)布于2010年，兩年后發(fā)布了第二代產(chǎn)品。前兩代TurtleBot都使用iRobot的機(jī)器人作為底盤，在底盤上可以裝載激光雷達(dá)、Kinect等傳感器，使用PC搭載基于ROS的控制系統(tǒng)。

TurtleBot3徹底顛覆了原有TurtleBot的外形設(shè)計(jì)，成本進(jìn)一步降低，而且模塊化更強(qiáng)，可以根據(jù)開發(fā)者的需求自由改裝。TurtleBot2主要結(jié)構(gòu)示意圖在本書中，主要介紹的版本是TurtleBot2，TurtleBot2主要構(gòu)成如圖所示，其主要包含三個(gè)部分，Kobuki底盤、主機(jī)以及傳感器Kinect。Kobuki底盤：Kobuki是Yujin公司開發(fā)的移動(dòng)機(jī)器人底盤，機(jī)器人接口控制板有32個(gè)內(nèi)置傳感器，兩個(gè)驅(qū)動(dòng)輪，2個(gè)從動(dòng)輪共四個(gè)可移動(dòng)小輪，110度/秒單軸陀螺儀，一對(duì)編碼器，一個(gè)可以擴(kuò)大的輸入輸出端口和一個(gè)后擋板。開放式接口可以直接實(shí)現(xiàn)對(duì)機(jī)器人的移動(dòng)、聲音、顯示以及輸入傳感器的操作。Kobuki底盤

最初TurtleBot2所帶的傳感器是微軟在2009年發(fā)布的kinectV1深度傳感器在2014年微軟公司發(fā)布了Kinect的二代產(chǎn)品?，F(xiàn)在的TurtleBot2所配備的傳感器Kinect2.0。

kinect1Kinect2Kinect1，彩色攝像頭為640*480，深度攝像頭為320*240，可以同時(shí)檢測到2名用戶的姿勢可以檢測到人體20個(gè)關(guān)節(jié)點(diǎn)檢測范圍為0.8m-4.0m角度（水平57°垂直43°）Kinect2，彩色攝像頭為1920*1080，深度攝像頭為512*424，可以同時(shí)檢測到6名用戶的姿勢可以檢測到人體25個(gè)關(guān)節(jié)點(diǎn)檢測范圍為0.5m-4.5m角度（水平70°垂直60°）03基于ROS系統(tǒng)的SLAM開源方案基于ROS的開源SLAM方案就非常多，總共可以分為兩個(gè)大類，分別是基于激光雷達(dá)的開源方案以及基于視覺的開源方案。1.基于激光雷達(dá)的SLAM算法（1）HectorSLAMHectorSLAM是一種結(jié)合了魯棒性較好的掃描匹配（scanmatching）算法的2D激光SLAM方法，同時(shí)在該方案中使用了慣性傳感系統(tǒng)的導(dǎo)航技術(shù)。要求：高更新頻率、低測量噪聲的激光掃描儀，不需要里程計(jì)特點(diǎn)：內(nèi)存中有不同的地圖，同時(shí)使用之前估計(jì)的姿勢來更新它們，計(jì)算成本很低，且地圖總是保持一致。HectorSLAM多分辨率地圖（2）Gmapping這是一種基于Rao-Blackwellized的粒子濾波SLAM方法。基于粒子濾波的算法用許多加權(quán)粒子表示路徑的后驗(yàn)概率，每個(gè)粒子都給出一個(gè)重要性因子。特點(diǎn)：●通常需要大量的粒子才能獲得比較好的的結(jié)果，從而增加該算法的計(jì)算復(fù)雜性。

●與PF重采樣過程相關(guān)的粒子退化耗盡問題降低了算法的準(zhǔn)確性。30粒子80粒子Gmapping基于不同粒子數(shù)的地圖（3）CartographerCartographer是谷歌推出的一套基于圖優(yōu)化的SLAM算法，該算法以柵格的形式來建立地圖，并且將局部匹配直接表示成一個(gè)非線性優(yōu)化問題，后端用圖來優(yōu)化。特點(diǎn)：

●目標(biāo)是降低計(jì)算資源消耗、實(shí)時(shí)優(yōu)化并且不追求高精度。●整個(gè)算法的核心代碼依賴很少Cartographer地圖2.基于視覺的SLAM算法MonoSLAM以擴(kuò)展卡爾曼濾波為后端，追蹤前端非常稀疏的特征點(diǎn)。

●第一個(gè)實(shí)時(shí)的視覺SLAM系統(tǒng)

●應(yīng)用場景窄，路標(biāo)數(shù)量有限，稀疏特征點(diǎn)非常容易丟失PTMA實(shí)現(xiàn)了跟蹤與建圖過程的并行化，跟蹤部分需要實(shí)時(shí)響應(yīng)圖像數(shù)據(jù)，而對(duì)地圖的優(yōu)化則沒必要實(shí)時(shí)的計(jì)算。也就意味著后端優(yōu)化可以在后端進(jìn)行，然后在必要的時(shí)候進(jìn)行線程同步即可。 ●首次將前后端的概念區(qū)分出來；引入了關(guān)鍵幀機(jī)制 ●應(yīng)用場景小，跟蹤容易丟失ORB-SLAM代表著特征點(diǎn)SLAM的一個(gè)高峰，支持單目、雙目、RGB-D三種模式，整個(gè)系統(tǒng)圍繞ORB特征進(jìn)行計(jì)算，包括視覺里程計(jì)與回環(huán)檢測的ORB字典。●ORB回環(huán)檢測可以有效的降低算法運(yùn)行過程中的累積誤差，并且在丟失后可以迅速找回●創(chuàng)新式的使用了三個(gè)線程完成SLAM

基于特征估計(jì)的激光SLAM技術(shù)第四章目錄CONTENTS01rviz和Gazebo工具02激光雷達(dá)傳感器03基于激光的Gmapping算法04基于激光的HectorSLAM算法05基于激光的Cartographer算法01rviz和Gazebo工具

機(jī)器人系統(tǒng)中存在大量數(shù)據(jù)，這些數(shù)據(jù)在計(jì)算過程中往往都處于數(shù)據(jù)形態(tài)，比如圖像數(shù)據(jù)中0~255的RGB值。但是這種數(shù)據(jù)形態(tài)的值往往不利于開發(fā)者去感受數(shù)據(jù)所描述的內(nèi)容，所以常常需要將數(shù)據(jù)可視化顯示，例如機(jī)器人模型的可視化、圖像數(shù)據(jù)的可視化、地圖數(shù)據(jù)的可視化等。rviz是ROS可視化（ROSvisualization）的縮寫，是ROS中強(qiáng)有力的3D可視化工具。它使得用戶能夠查看模擬機(jī)器人模型、來自機(jī)器人傳感器的傳感器日志信息，并且重放已記錄的傳感器信息。用戶能夠?qū)C(jī)器人應(yīng)用程序進(jìn)行調(diào)試，調(diào)試內(nèi)容涵蓋從傳感器輸入到計(jì)劃動(dòng)作(或計(jì)劃外動(dòng)作)的整個(gè)過程。rvizrviz主界面劃分為以下幾個(gè)主要的顯示區(qū)域:1.中心窗口2.

Displays控制面板3.工具欄4.Views控制面板5.Time控制面板6.主窗口菜單欄。常見的display類型類型描述消息類型Axes顯示坐標(biāo)系－Gamera從相機(jī)視角顯示圖像sensor_msgs/Imagesensor_msgs/CameraInfoGrid顯示網(wǎng)格－Image顯示圖像sensor_msgs/ImageLaserScan顯示激光雷達(dá)數(shù)據(jù)sensor_msgs/LaserScanImage顯示圖像sensor_msgs/ImagePointClode2顯示點(diǎn)云數(shù)據(jù)sensor_msgs/PointCloud2Odomerty顯示里程計(jì)數(shù)據(jù)nav_msgs/OdometryPointClode2顯示點(diǎn)云數(shù)據(jù)sensor_msgs/PointCloud2RobotModel顯示機(jī)器人模型－PointClode2顯示TF樹－rviz的視角1.Orbital軌道攝像機(jī)視角，圍繞一個(gè)焦點(diǎn)旋轉(zhuǎn)，Ｌ:單擊并拖動(dòng)圍繞焦點(diǎn)旋轉(zhuǎn)Ｍ:在相機(jī)向上和向右向量形成平面，按下中鍵并拖動(dòng)焦點(diǎn)R:單擊拖動(dòng)鼠標(biāo)放大/縮小焦點(diǎn)向上拖動(dòng)放大，向下拖動(dòng)縮小滾輪:放大縮小焦點(diǎn)2.FPS第一視角FPVＬ:單擊選中對(duì)象，并拖動(dòng)旋轉(zhuǎn)Ｍ:移動(dòng)沿著相機(jī)向上、向右向量形成的平面移動(dòng)R:右擊并拖動(dòng)，前后移動(dòng)，上拖向前移動(dòng)，下拖向后移動(dòng)滾輪:前后移動(dòng)3.top-downOrthographic自上而下的正視圖Ｌ:單擊并拖動(dòng)繞Ｚ軸旋轉(zhuǎn)Ｍ:在ＸＹ平面移動(dòng)R:右擊并拖動(dòng)，縮放圖像滾輪:縮放圖像4.ＸＹOrthographic限制在XY平面GazeboGazebo是一款3D動(dòng)態(tài)模擬器，能夠在復(fù)雜的室內(nèi)和室外環(huán)境中準(zhǔn)確有效地模擬機(jī)器人群。與游戲引擎提供高保真度的視覺模擬類似，Gazebo提供高保真度的物理模擬，其提供一整套傳感器模型，以及對(duì)用戶和程序非常友好的交互方式。1.Gazebo的典型用途●測試機(jī)器人算法●設(shè)計(jì)機(jī)器人●用現(xiàn)實(shí)場景進(jìn)行回歸測試2.Gazebo的一些主要特點(diǎn)●包含多個(gè)物理引擎●包含豐富的機(jī)器人模型和環(huán)境庫●包含各種各樣的傳感器●程序設(shè)計(jì)方便和具有簡單的圖形界面3.系統(tǒng)要求Gazebo官方建議，Gazebo目前最好在Ubuntu或者其他的Linux發(fā)行版上運(yùn)行。同時(shí)您的計(jì)算機(jī)需要具有以下功能：●專用GPU：Nvidia卡往往在Ubuntu中運(yùn)行良好●至少是IntelI5或同等產(chǎn)品的CPU●至少500MB的可用磁盤空間●安裝盡可能高版本的UbuntuTrusty02激光雷達(dá)傳感器激光雷達(dá)探測原理激光雷達(dá)（Lidar）類似于雷達(dá)（Radar），但是分辨率更高，因?yàn)榧す獾牟ㄩL大約比無線電的波長小10萬倍。可以用來區(qū)分真實(shí)移動(dòng)中的行人和人物海報(bào)、在三維立體空間中建模、檢測靜態(tài)物體、精確測距。激光雷達(dá)是通過發(fā)射激光束來探測目標(biāo)位置、速度等特征量的雷達(dá)系統(tǒng)，具有測量精度高、方向性好等優(yōu)點(diǎn)。隼鳥2號(hào)LiDAR結(jié)合光學(xué)相機(jī)影像獲取龍宮全球三維地形信息iPadPro探測點(diǎn)云示意圖激光雷達(dá)優(yōu)點(diǎn)●具有極高的分辨率●抗干擾能力強(qiáng)●獲取的信息量豐富●可全天時(shí)工作激光雷達(dá)缺點(diǎn)●工作時(shí)受天氣和大氣影響大。激光一般在晴朗的天氣里衰減較小，傳播距離較遠(yuǎn)。而在大雨、濃煙、濃霧等壞天氣里，衰減急劇加大，傳播距離大受影響?！窦す饫走_(dá)的波束極窄,在空間搜索目標(biāo)非常困難，直接影響對(duì)非合作目標(biāo)的截獲概率和探測效率,只能在較小的范圍內(nèi)搜索、捕獲目標(biāo)，激光雷達(dá)RPLIDARA1高度60毫米直徑98.5毫米重量170克測量半徑范圍0.15-12米掃描測距角度360度測量頻率最大可達(dá)每秒8000次掃描頻率5.5赫茲。激光雷達(dá)RPLIDARA2高度41毫米直徑190毫米重量76克測量半徑范圍0.15-18米掃描測距角度360度測量頻率最大可達(dá)每秒8000次掃描頻率15赫茲。激光雷達(dá)RPLIDARA1/2是思嵐科技（SLAMTEC）有限公司的產(chǎn)品,能夠準(zhǔn)確測量試場中物體輪廓邊沿與設(shè)備間的相對(duì)距離，可快速獲得環(huán)境輪廓信息，這些輪廓信息組成所謂的點(diǎn)云并繪制出3D環(huán)境地圖，可以幫助機(jī)器人實(shí)現(xiàn)自主構(gòu)建地圖、實(shí)時(shí)路徑規(guī)劃與自動(dòng)避開障礙物。03基于激光的Gmapping算法Gmapping是一種基于Rao-Blackwellized粒子濾波器并適用于網(wǎng)格地圖的SLAM算法，也是一種有效解決同時(shí)定位和建圖的算法，它將定位和建圖分離，并且每一個(gè)粒子都攜帶一幅地圖。

Gmapping算法流程圖重要性重采樣（SamplingImportanceResampling,SIR）濾波器

Gmapping算法的實(shí)現(xiàn)步驟1）第i個(gè)粒子在k時(shí)刻的預(yù)測位姿可以由k-1

時(shí)刻的位姿和控制輸入通過運(yùn)動(dòng)模型得出。2）機(jī)器人基于地圖

，從預(yù)測的位姿處執(zhí)行掃描匹配算法。掃描匹配執(zhí)行的搜索區(qū)域被限定在附近的一個(gè)有限區(qū)域內(nèi)。如果掃描匹配失敗，則根據(jù)運(yùn)動(dòng)模型計(jì)算位姿和權(quán)重（忽略步驟3和4）。3）通過掃描匹配器在位姿周圍選取一組采樣點(diǎn)，其中是掃描匹配器通過將當(dāng)前觀測值與已構(gòu)建的地圖進(jìn)行匹配，找到最有可能的位姿?；谶@些點(diǎn)，通過逐點(diǎn)評(píng)估采樣位姿中的目標(biāo)分布來計(jì)算建議的平均值和協(xié)方差矩陣。在此階段計(jì)算加權(quán)系數(shù)。4）根據(jù)改進(jìn)的建議分布得出粒子i

的新位姿，位姿服從高斯分布的形式。5）更新粒子權(quán)重。6）根據(jù)機(jī)器人位姿和最新觀測，更新粒子i的地圖。7）得到新粒子樣本后，根據(jù)閾值執(zhí)行重采樣步驟。04基于激光的HectorSLAM算法HectorSLAM使用一種快速在線學(xué)習(xí)的占用柵格地圖系統(tǒng)，它是結(jié)合了激光雷達(dá)系統(tǒng)和基于慣性傳感的三維姿態(tài)估計(jì)系統(tǒng)的魯棒掃描匹配方法。通過使用快速近似的地圖梯度和多分辨率柵格，實(shí)現(xiàn)了可靠的定位和建圖。HectorSLAM功能包使用高斯牛頓的方法，不需要里程計(jì)數(shù)據(jù)，只根據(jù)激光信息便可構(gòu)建地圖。

SLAM一般分為前端和后端系統(tǒng)。SLAM前端用于實(shí)時(shí)在線估計(jì)機(jī)器人運(yùn)動(dòng)，后端用于對(duì)位姿圖進(jìn)行優(yōu)化。前端快速掃描匹配步驟用于位姿估計(jì)，而較慢的后端建圖步驟在后臺(tái)或遠(yuǎn)程計(jì)算機(jī)上運(yùn)行。HectorSLAM方法主要關(guān)注前端系統(tǒng)，沒有提供相應(yīng)的位姿圖優(yōu)化，主要是由于這種優(yōu)化在真實(shí)環(huán)境下建圖是不需要的，因?yàn)镠ectorSLAM方法對(duì)于機(jī)器人執(zhí)行任務(wù)來說已經(jīng)足夠精確。HectorSLAM算法原理HectorSLAM系統(tǒng)需要用到6自由度運(yùn)動(dòng)的平臺(tái)，這不同于其它2D網(wǎng)格SLAM算法所假設(shè)的3自由度運(yùn)動(dòng)，因此系統(tǒng)必須估計(jì)由平臺(tái)的平移和旋轉(zhuǎn)組成的全6自由度狀態(tài)。為此，該系統(tǒng)由兩個(gè)主要組件組成，導(dǎo)航濾波器子系統(tǒng)融合來自慣性測量單元和其他可用傳感器的信息，形成一致的3D解決方案，而2DSLAM子系統(tǒng)用于提供地平面內(nèi)的位姿和航向信息。這兩種估計(jì)都是單獨(dú)更新的，并且只是松散耦合的，因此它們隨著時(shí)間的推移保持同步。Hectorslam算法的實(shí)現(xiàn)步驟（1）在2DSLAM中，為了能夠表示任意環(huán)境，HectorSLAM使用占用柵格地圖，占用柵格地圖的離散特性限制了地圖表達(dá)精度，也不允許直接計(jì)算插值或?qū)?shù)。給定一個(gè)連續(xù)的地圖坐標(biāo)，占用值以及梯度可以如圖用四個(gè)近鄰點(diǎn)來近似，沿著x軸和y軸進(jìn)行線性插值，然后得到:導(dǎo)數(shù)可以近似為（2）掃描匹配是將激光掃描相互對(duì)齊或與現(xiàn)有地圖對(duì)齊的過程，HectorSLAM實(shí)質(zhì)上是一個(gè)將掃描末端點(diǎn)與已知地圖對(duì)準(zhǔn)優(yōu)化方法,其基本思路是使用高斯-牛頓法。通過解最小二乘函數(shù)，使得激光掃描與地圖有最佳的對(duì)齊：代表機(jī)器人的位姿，代表i號(hào)激光束在機(jī)器人姿態(tài)下掃描點(diǎn)的世界坐標(biāo)，其坐標(biāo)變化為：根據(jù)優(yōu)化測量誤差來實(shí)現(xiàn)對(duì)的估計(jì)：對(duì)做一階泰勒展開得：對(duì)上式求偏導(dǎo)并設(shè)置為，可得上述最小化問題的高斯牛頓方程：其中（3）HectorSLAM使用擴(kuò)展卡爾曼濾波器(EKF)。在掃描時(shí)的卡爾曼估計(jì)可以用均值和協(xié)方差來表示，位姿估計(jì)

直接得出，融合結(jié)果如下：觀測器矩陣C將整個(gè)狀態(tài)空間投影到SLAM系統(tǒng)的三維子空間中，參數(shù)用來調(diào)整SLAM更新的效果。于是，進(jìn)一步有：05基于激光的Cartographer算法基于激光的SLAM方法中，掃描--掃描匹配（Scan-to-scanmatching）經(jīng)常用于計(jì)算相對(duì)姿態(tài)變化。然而，掃描--掃描匹配本身會(huì)很快積累誤差。掃描--地圖匹配（Scan-to-mapmatching）有助于限制這種誤差積累，可使用高斯-牛頓方法在線性插值圖上尋找局部最優(yōu)解。當(dāng)高精度頻率激光雷達(dá)提供良好的位姿初始估計(jì)時(shí)，局部優(yōu)化的掃描--地圖匹配是有效和魯棒的。

像素精度的掃描匹配方法，可以進(jìn)一步減少了局部誤差積累。解決剩余局部誤差累積的兩種常見方法是粒子濾波和基于圖形的SLAM。Cartographer是谷歌推出的一套基于圖優(yōu)化的SLAM算法。Cartographer的設(shè)計(jì)目的是在計(jì)算資源有限的情況下，實(shí)時(shí)獲取相對(duì)較高精度的2D地圖?？紤]到基于模擬策略的粒子濾波方法在較大環(huán)境下對(duì)內(nèi)存和計(jì)算資源的需求較高，Cartographer采用基于圖優(yōu)化方法。Cartographer是一個(gè)實(shí)時(shí)的室內(nèi)建圖算法，能生成分辨率的柵格地圖。在前端將最新的激光雷達(dá)掃描數(shù)據(jù)在相鄰的子圖上(整個(gè)地圖的一小塊)完成掃描匹配，得到一個(gè)在短時(shí)間內(nèi)準(zhǔn)確的最佳插入位置(位姿)后，將掃描插入到子圖中。掃描匹配中，位姿估計(jì)的誤差會(huì)在整個(gè)地圖中隨時(shí)間逐漸累積，在后端中,通過回環(huán)檢測加約束進(jìn)行優(yōu)化消除誤差。Cartographer算法框架圖Cartographer算法原理Cartographer系統(tǒng)將獨(dú)立的局部和全局方法綜合來實(shí)現(xiàn)二維的定位與地圖構(gòu)建。在局部方法中，每個(gè)連續(xù)的掃描都與世界坐標(biāo)系上的一小塊區(qū)域（稱為子圖）進(jìn)行匹配，使用非線性優(yōu)化將掃描與子圖對(duì)齊，此過程被稱為掃描匹配。子圖的構(gòu)建就是將掃描結(jié)果不斷與子圖的坐標(biāo)系對(duì)齊的迭代過程。