SLAM+語音機器人DIY系列：（六）SLAM建圖與自主避障導航——2.google-cartographer機器人SLAM建圖

時間 2019-11-10

標籤 slam 語音機器人 diy 系列自主導航 2.google google cartographer 欄目 Google 简体版

原文原文鏈接

摘要

經過前面的基礎學習，本章進入最爲激動的機器人自主導航的學習。在前面的學習鋪墊後，終於迎來了最大樂趣的時刻，就是賦予咱們的miiboo機器人能自由行走的生命。本章將圍繞機器人SLAM建圖、導航避障、巡航、監控等內容展開。本章內容：html

1.在機器人上使用傳感器node

2.google-cartographer機器人SLAM建圖python

3.ros-navigation機器人自主避障導航nginx

4.多目標點導航及任務調度git

5.機器人巡航與現場監控github

2.google-cartographer機器人SLAM建圖

主流的激光SLAM算法有hector、gmapping、karto、cartographer。算法

hector是一種結合了魯棒性較好的掃描匹方法2D_SLAM方法和使用慣性傳感系統的導航技術。傳感器的要求較高，高更新頻率小測量噪聲的激光掃描儀，不須要里程計。使空中無人機與地面小車在不平坦區域運行存在運用的可能性。做者利用現代激光雷達的高更新率和低距離測量噪聲，經過掃描匹配實時地對機器人運動進行估計。因此當只有低更新率的激光傳感器時，即使測距估計很精確，對該系統都會出現必定的問題。django

gmapping是一種基於粒子濾波的激光SLAM算法，它已經集成在ROS中，是移動機器人中使用最多的SLAM算法。基於粒子濾波的算法用許多加權粒子表示路徑的後驗機率，每一個粒子都給出一個重要性因子。可是，它們一般須要大量的粒子才能得到比較好的的結果，從而增長該算法的的計算複雜性。此外，與PF重採樣過程相關的粒子退化耗盡問題也下降了算法的準確性。ubuntu

karto是基於圖優化的SLAM算法，用高度優化和非迭代cholesky矩陣進行稀疏系統解耦做爲解。圖優化方法利用圖的均值表示地圖，每一個節點表示機器人軌跡的一個位置點和傳感器測量數據集，箭頭的指向的鏈接表示連續機器人位置點的運動，每一個新節點加入，地圖就會依據空間中的節點箭頭的約束進行計算更新。路標landmark越多,內存需求越大,然而圖優化方式相比其餘方法在大環境下製圖優點更大。後端

cartographer是google開發的實時室內SLAM項目，cartographer採用基於google自家開發的ceres非線性優化的方法，cartographer的量點在於代碼規範與工程化，很是適合於商業應用和再開發。而且cartographer基於submap子圖構建全局地圖的思想，能有效的避免建圖過程當中環境中移動物體的干擾。而且cartographer支持多傳感器數據（odometry、IMU、LaserScan等）建圖，支持2D_SLAM和3D_SLAM建圖。因此，我果斷採用cartographer來建圖，個人樹莓派3主板跑cartographer實時建圖是十分的流暢，這一點很欣慰^_^

2.1.google-cartographer建圖算法原理分析

cartographer採用的是主流的SLAM框架，也就是特徵提取、閉環檢測、後端優化的三段式。由必定數量的LaserScan組成一個submap子圖，一系列的submap子圖構成了全局地圖。用LaserScan構建submap的短期過程累計偏差不大，可是用submap構建全局地圖的長時間過程就會存在很大的累計偏差，因此須要利用閉環檢測來修正這些submap的位置，閉環檢測的基本單元是submap，閉環檢測採用scan_match策略。cartographer的重點內容就是融合多傳感器數據（odometry、IMU、LaserScan等）的submap子圖建立以及用於閉環檢測的scan_match策略的實現。

（圖18）cartographer算法系統框圖

2.2.cartographer_ros安裝

咱們直接參考google-cartographer官方教程安裝就行，官方教程分爲cartographer和cartographer_ros，其實cartographer就是核心算法層、cartographer_ros是核心算法層的ros調用層。官方教程以下：

https://google-cartographer.readthedocs.io/en/latest/index.html#

https://google-cartographer-ros.readthedocs.io/en/latest/index.html#

直接按照第二個連接cartographer_ros的安裝教程，就可將cartographer_ros、cartographer、以及各類依賴都安裝了。不過特別說明一點，爲了解決從官網下載ceres-solver速度慢的問題，我將ceres-solver的下載地址換到了github源；我須要將官方教程中生成的src/.rosinstall替換成了本身的內容，如圖19。其他安裝過程和官方教程如出一轍。

（1）安裝編譯工具

我來編譯cartographer_ros，咱們須要用到wsool和rosdep。爲了加快編譯，咱們使用ninja工具進行編譯。

sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build

（2）建立存放cartographer_ros的專門工做空間

mkdir catkin_ws_carto
cd catkin_ws_carto
wstool init src

wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall

wstool update -t src

特別說明，在執行wstool update -t src以前，須要將src/.rosinstall文件修改爲如下內容,以解決ceres-solver下載不了的問題，如圖19。

（圖19）我修改後的src/.rosinstall文件內容

（3）安裝依賴項

安裝cartographer_ros的依賴項proto3、deb包等。若是執行sudo rosdep init報錯，能夠直接忽略。

src/cartographer/scripts/install_proto3.sh
sudo rosdep init
rosdep update

rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

（4）編譯和安裝

上面的配置和依賴都完成後，就能夠開始編譯和安裝cartographer_ros整個項目工程了。

catkin_make_isolated --install --use-ninja

特別提醒，之後對cartographer_ros中的配置文件或源碼有改動時，都須要執行這個編譯命令使修改生效。

2.3.cartographer_ros使用

cartographer_ros總體代碼結構分析：

最頂層的是cartographer_ros，做爲rosj接口調用層，經過調用cartographer核心算法，訂閱多傳感器數據（/scan、/imu、/odom等），併發布地圖、機器人位置信息（/map、/tf等）；其次是cartographer，做爲SLAM算法的核心實現，特徵提取、子圖構建、閉環檢測、全局優化都在這裏實現，其中優化過程須要調用ceres-solver非線性優化庫；最後是ceres-solver，是非線性優化庫，用於求解SLAM中的優化問題。

（圖20）cartographer_ros總體代碼結構

在miiboo機器人上用cartographer_ros多傳感器建圖進行配置：

通過前面對cartographer_ros進行安裝後，咱們確定火燒眉毛想在實際的miiboo機器人上使用cartographer_ros進行SLAM建圖了。爲了最大限度的提升SLAM建圖的性能，咱們的miiboo機器人提供了激光雷達、IMU、輪式里程計（/scan、/imu、/odom）這三種傳感器的數據，因此咱們須要先將cartographer_ros配置成對應的工做模式。

cartographer算法是一個很是通用和適應不一樣平臺的開放框架算法，因此支持多種配置與工做模式。咱們就來看看cartographer_ros如何進行配置。配置文件由*.lua書寫被放在路徑cartographer_ros/configuration_files/，咱們須要創建一個咱們本身的配置文件，取名就叫miiboo_mapbuild.lua吧，具體內容如圖21。因爲咱們的miiboo機器人採用激光雷達、IMU、輪式里程計三種傳感器融合建圖，因此如下參數必定要設置正確：

參數tracking_frame設置爲imu_link，由於咱們使用/imu的數據；

參數published_frame設置爲odom，由於咱們使用/odom的數據；

參數provide_odom_frame設置爲false，由於咱們使用外部/odom，因此這裏不須要內部提供；

參數use_odometry設置爲true，由於咱們使用外部/odom的數據；

參數use_imu_data設置爲true，由於咱們使用/imu的數據；

參數imu_gravity_time_constant設置爲10，這個是IMU的重力加速度常數。

其他參數根據須要自行調整，因爲cartographer是發展很迅速的算法，因此不少代碼和文檔一直在更新，因此參考官方文檔來解讀這些參數的含義是最好的選擇，官方文檔鏈接地址我貼在下面了。

https://google-cartographer-ros.readthedocs.io/en/latest/index.html

（圖21）咱們miiboo機器人的建圖配置文件miiboo_mapbuild.lua

而後須要配置*.launch文件，咱們給miiboo機器人創建啓動文件取名叫miiboo_mapbuild.launch，存放路徑在cartographer_ros/launch/裏面，具體內容如圖22。

不難發現launch文件中包含三個node啓動項，即urdf模型啓動項、cartographer_node啓動項、cartographer_occupancy_grid_node啓動項。

第一個啓動項是啓動urdf模型，這個接口是提供給那些只使用cartographer單獨建圖的應用場景，因爲咱們miiboo機器人創建完地圖後還須要繼續進行自動導航任務，因此咱們使用miiboo底盤提供的urdf模型，而不使用這裏的urdf模型，因此這個啓動項被註釋掉了，這樣建圖和導航就更容易管理。

第二個啓動項是啓動cartographer_node建圖節點，這個是SLAM建圖主節點，咱們創建的配置miiboo_mapbuild.lua將在這裏被載入，同時這裏能夠對建圖輸入數據scan、imu、odom的topic名稱作重映射。

第三個啓動項是啓動cartographer_occupancy_grid_node地圖格式轉換節點，因爲cartographer_node建圖節點提供的地圖是submapList格式的，須要轉換成GridMap格式才能在ROS中顯示和使用。這裏面有兩個可配參數，resolution用來設置GridMap地圖的分辨率，publish_period_sec用來設置GridMap地圖發佈的頻率。

（圖22）咱們miiboo機器人的建圖啓動文件miiboo_mapbuild.launch

配置參數修改好後，不要忘了再編譯一次整個catkin_ws_carto工做空間，切換到catkin_ws_carto目錄，執行下面的編譯命令。

catkin_make_isolated --install --use-ninja

啓動cartographer_ros建圖：

要在miiboo機器人上，啓動cartographer_ros建圖，分爲這幾個步驟：啓動機器人上的各個傳感器、啓動cartographer_ros、在PC端啓動鍵盤控制機器人運動並啓動rviz觀察地圖（或者在Android手機端用miiboo機器人APP控制機器人運動和觀察地圖）。

首先，啓動機器人上的各個傳感器，爲了操做方便，我已經將要啓動的傳感器都寫入miiboo_bringup/launch/miiboo_all_sensor.launch這個啓動文件了，文件內容如圖23。這個啓動文件包含機器人urdf啓動項、miiboo底盤啓動項、激光雷達啓動項、IMU啓動項、攝像頭啓動項、廣播IP啓動項。

（圖23）各個傳感器啓動文件miiboo_all_sensor.launch

打開終端，經過下面的命令直接啓動就好了。

source ~/catkin_ws/devel/setup.bash 
roslaunch miiboo_bringup miiboo_all_sensor.launch

而後，啓動cartographer_ros，因爲前面已經作好了相應的配置，因此直接使用命令啓動就好了。

source ~/catkin_ws_carto/install_isolated/setup.bash
roslaunch cartographer_ros miiboo_mapbuild.launch

這裏給個小提示，爲了查看cartographer_ros建圖算法有沒有正常開始工做，咱們能夠用rosrun rqt_tf_tree rqt_tf_tree查看整個tf樹的結構，正常的tf樹如圖24。map->odom之間的關係由cartographer建圖節點提供，odom->base_footprint之間的關係由miiboo底盤的輪式里程計提供，base_footprint->imu_link和base_link和base_laser_link之間的關係由miiboo機器人的urdf模型提供。從tf樹不難看出整個建圖過程當中機器人定位的實現原理，cartographer建圖節點經過維護map->odom之間的關係最終實現全局定位，miiboo底盤的輪式里程計經過維護odom->base_footprint之間的關係來實現局部定位，傳感器之間的安裝關係由urdf模型提供，這個靜態關係主要用於多傳感器數據融合。

（圖24）cartographer運行時正常的tf樹

最後，在PC端啓動鍵盤控制機器人運動並啓動rviz觀察地圖（或者在Android手機端用miiboo機器人APP控制機器人運動和觀察地圖）。若是用PC端控制和觀察，啓動命令以下。

在PC端打開一個新終端，運行rviz啓動命令。

rosrun rviz rviz

在rviz窗口中添加訂閱/map，就能夠看到建圖效果了，如圖25。

（圖25）在PC端用rviz觀察地圖

在PC端再打開一個新終端，運行鍵盤控制啓動命令。

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

在該終端下，用鍵盤就能夠控制機器人前進、後退、左轉、右轉了。

若是是在Android手機端用miiboo機器人APP控制機器人運動和觀察地圖，直接就能使用，如圖26。

（圖26）在Android手機端用miiboo機器人APP控制機器人運動和觀察地圖

保存cartographer_ros建圖結果：

當咱們在房間裏面掃描一圈，地圖創建的差很少了，就能夠將建圖結果保存下來了，cartographer_ros提供了將建圖結果保存爲*.pbstream專門的方法，其實就是一條命令。

source ~/catkin_ws_carto/install_isolated/setup.bash
rosservice call /write_state  /home/ubuntu/map/carto_map.pbstream

其實就是調用cartographer_ros提供的叫/write_state這個名字的服務，服務傳入參數/home/ubuntu/map/carto_map.pbstream爲地圖的保存路徑。保存成功後，會返回相應的狀態信息，如圖27。

（圖27）調用/write_state服務保存建圖結果

地圖格式轉換：

因爲用cartographer_ros提供的/write_state方法保存的地圖是*.pbstream的格式，而要在後續的自主導航中使用這個地圖，咱們須要將其轉換爲ROS中通用的GridMap格式。其實很簡單，cartographer_ros已經跟咱們提供了cartographer_pbstream_to_ros_map這個節點用於轉換的實現。因此，咱們只須要寫一個啓動文件啓動這個節點就好了，我給這個啓動文件取名miiboo_pbstream2rosmap.launch，存放路徑是cartographer_ros/launch/，啓動文件的內容如圖28。在使用這個啓動文件進行啓動時，須要從外部傳入兩個參數，參數pbstream_filename爲待轉換的*.pbstream文件路徑，參數map_filestem爲轉換後存放結果的文件路徑。

（圖28）pbstream轉GridMap啓動文件

配置參數修改好後，不要忘了再編譯一次整個catkin_ws_carto工做空間，切換到catkin_ws_carto目錄，執行下面的編譯命令。

catkin_make_isolated --install --use-ninja

最後，就能夠打開終端，使用啓動這個啓動文件，對地圖格式進行轉換了，命令以下。

roslaunch cartographer_ros miiboo_pbstream2rosmap.launch pbstream_filename:=/home/ubuntu/map/carto_map.pbstream map_filestem:=/home/ubuntu/map/carto_map

保存結束後，節點會自動退出，這時咱們能夠獲得轉換後的地圖，轉換後的GridMap地圖由*.pgm和*.yaml兩部分構成，這時標準的ROS格式地圖，能夠被ROS導航框架中的map_server節點直接調用，轉換後的地圖結果如圖29。