SLAM+語音機器人DIY系列：（六）SLAM建圖與自主避障導航——2.google-cartographer機器人SLAM建圖
溫馨提示
摘要
通過前面的基礎學習，本章進入最為激動的機器人自主導航的學習。在前面的學習鋪墊后，終于迎來了最大樂趣的時刻，就是賦予我們的miiboo機器人能自由行走的生命。本章將圍繞機器人SLAM建圖、導航避障、巡航、監控等內容展開。本章內容：
2.google-cartographer機器人SLAM建圖
2.google-cartographer機器人SLAM建圖
主流的激光SLAM算法有hector、gmapping、karto、cartographer。
hector是一種結合了魯棒性較好的掃描匹方法2D_SLAM方法和使用慣性傳感系統的導航技術。傳感器的要求較高，高更新頻率小測量噪聲的激光掃描儀，不需要里程計。使空中無人機與地面小車在不平坦區域運行存在運用的可能性。作者利用現代激光雷達的高更新率和低距離測量噪聲，通過掃描匹配實時地對機器人運動進行估計。所以當只有低更新率的激光傳感器時，即便測距估計很精確，對該系統都會出現一定的問題。
gmapping是一種基于粒子濾波的激光SLAM算法，它已經集成在ROS中，是移動機器人中使用最多的SLAM算法。基于粒子濾波的算法用許多加權粒子表示路徑的后驗概率，每個粒子都給出一個重要性因子。但是，它們通常需要大量的粒子才能獲得比較好的的結果，從而增加該算法的的計算復雜性。此外，與PF重采樣過程相關的粒子退化耗盡問題也降低了算法的準確性。
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。
（圖29）地圖格式轉換后的結果
后記
------SLAM+語音機器人DIY系列【目錄】快速導覽------
第1章：Linux基礎
第2章：ROS入門
第3章：感知與大腦
第4章：差分底盤設計
第5章：樹莓派3開發環境搭建
第6章：SLAM建圖與自主避障導航
2.google-cartographer機器人SLAM建圖
第7章：語音交互與自然語言處理
第8章：高階拓展
2.centos7下部署Django(nginx+uwsgi+django+python3)
參考文獻
[1] 張虎，機器人SLAM導航核心技術與實戰[M]. 機械工業出版社，2022.
浙公網安備 33010602011771號