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

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

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

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

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

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

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

●目標是降低計算資源消耗、實時優(yōu)化并且不追求高精度?！裾麄€算法的核心代碼依賴很少Cartographer地圖2.基于視覺的SLAM算法MonoSLAM以擴展卡爾曼濾波為后端，追蹤前端非常稀疏的特征點。

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

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

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

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

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

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

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

時刻的位姿和控制輸入通過運動模型得出。2）機器人基于地圖

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

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

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

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

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