機器人SLAM與自主導航（三）——SLAM功能包

目錄

• • 一、gmapping
  • • 1、gmapping功能包
    • 2、柵格地圖取值原理
    • 3、gmapping安裝
    • 4、配置gmapping節點
    • 5、啟動gmapping演示（深度信息基于激光雷達）
    • 6、啟動gmapping（kinect）
  • 二、hector_slam
  • • 1、hector_slam功能包
    • 2、安裝hector_slam
    • 3、配置hector_mapping節點
    • 4、啟動hector_slam演示（深度信息基于激光雷達）
    • 5、打滑現象
  • 三、cartographer
  • • 1、cartographer功能包
    • 2、cartographer安裝
    • 3、配置cartographer節點
    • 4、啟動2D slam demo演示
    • 5、啟動3D slam demo演示
    • 6、參數配置
    • 7、啟動cartographer仿真
  • 四、Karto_slam
  • • 1、Karto_slam概況
    • 2、Karto_slam具體介紹
    • 3、karto_slam總結
  • 五、ORB_SLAM
  • • 1、ORB_SLAM功能包
    • 2、ORB_SLAM2安裝
    • 3、啟動單目SLAM示例（基于數據包）
    • 4、啟動AR示例（基于數據包）
    • 5、啟動ORB_SLAM示例（真實攝像頭）
    • 6、啟動AR示例（真實攝像頭）

一、gmapping

1、gmapping功能包

輸入：1.深度信息 2.IMU信息 3.里程計信息
輸出：柵格地圖

$ rosmsg show nav_msgs/OccupancyGrid

2、柵格地圖取值原理

3、gmapping安裝

$ sudo apt-get install ros-kinetic-gmapping

4、配置gmapping節點

參數說明可參考： http://wiki.ros.org/gmapping

5、啟動gmapping演示（深度信息基于激光雷達）

分別開啟三個終端運行以下命令：

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch # 啟動仿真環境 $ roslaunch mbot_navigation gmapping_demo.launch # 啟動建圖節點，灰色地圖建成，黑色障礙物 $ roslaunch mbot_teleop mbot_teleop.launch #啟動鍵盤控制節點

建圖完畢，保存地圖

$ rosrun map_server map_saver -f cloister_gmapping # cloister_gmapping是文件名的意思，是自己保存文件名的意思 # 保存的路徑在當前/home文件夾下,有兩個文件.pgm和.yaml

查看生成地圖文件

6、啟動gmapping（kinect）

建圖效果不佳（不推薦）

$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch $ roslaunch mbot_navigation gmapping_demo.launch $ roslaunch mbot_teleop mbot_teleop.launch

二、hector_slam

1、hector_slam功能包

輸入：深度信息
輸出：柵格地圖

2、安裝hector_slam

$ sudo apt-get install ros-kinetic-hector-slam

3、配置hector_mapping節點

參數說明可參考：http://wiki.ros.org/hector_slam

4、啟動hector_slam演示（深度信息基于激光雷達）

第一個節點啟動了gazebo仿真，它使用仿真的雷達給出了一個scan掃描數據；而后下面的節點訂閱了這個數據并進行處理，最后得到一個二維柵格地圖。

5、打滑現象

hector_slam功能包使用時要求機器人移速不能過快，否則運算跟不上會出現打滑

hector這個算法有一個很大的問題，就是在于它的自定位，它利用自身的激光雷達數據進行定位，如果上一次的掃描數據與下一次的掃描數據基本一致的話，它就很難確定自己是否在運動，例如當我給一個足夠大的空間，這里有一條沒有任何特征點的長廊，長度遠遠超過激光掃描的最大距離時，hector基本就失去了建圖的能力。

三、cartographer

1、cartographer功能包

2、cartographer安裝

方法一（新方法）：

sudo apt-get updatesudo apt-get install ros-<your ros version>-cartographer* # 安裝全部關于cartographer的包

方法二：
cartographer功能功能包沒有集成到ROS的軟件源里面，所以需要采用源碼編譯方式進行安裝。為了不與其他安裝包沖突，最好為cartographer專門創建一個工作空間，這里我們創建了一個工作空間catkin_google_ws：

cartographer功能包可能出現的問題

3、配置cartographer節點

4、啟動2D slam demo演示

5、啟動3D slam demo演示

6、參數配置

7、啟動cartographer仿真

四、Karto_slam

1、Karto_slam概況

評價：
Karto_SLAM是基于圖優化的思想，用高度優化和非迭代 cholesky分解進行稀疏系統解耦作為解。圖優化方法利用圖的均值表示地圖，每個節點表示機器人軌跡的一個位置點和傳感器測量數據集，每個新節點加入，就會進行計算更新。
Karto_SLAM的ROS版本，其中采用的稀疏點調整（the Spare Pose Adjustment(SPA)）與掃描匹配和閉環檢測相關。landmark越多,內存需求越大,然而圖優化方式相比其他方法在大環境下制圖優勢更大，因為他僅包含點的圖(robot pose)，求得位姿后再求map。

2、Karto_slam具體介紹

整體程序框架

從上圖中可以看出，其實流程還是相當簡單的，slam傳統軟實時的運行機制，每進入一幀數據，即進行處理，然后返回。
1）掃描匹配
關鍵詞：
興趣區域：
匹配的方式是scanTomap，興趣區域就是矩形形狀的submap，也可以將這塊區域理解為參考模型。
搜索區域：
以里程計估計的位置為中心的一個矩形區域，用以表示最終位置的可能范圍，在匹配時，遍歷搜索區域，獲取響應值最高的位置。
查找表：
對于激光獲得的數據信息，以一定的角分辨率和角偏移值進行投影，獲取查找表，用以匹配。
Running-scans：
實時維護的局部激光數據鏈，首末兩幀距離在一定距離范圍內，且滿足一定數據規模，否則需要刪除末端數據幀。維護當前局部數據鏈。
生成submap(因為匹配的方式是ScanToMap)

生成查找表：
查找表存在的意義就是相比于暴力匹配，不要每次都重新計算每個激光數據信息，相同角度不同位置的激光數據信息只需要被索引一次。

由里程計預測可以得到當前預估的姿態角，真實的姿態角必定在附近；
以一定的分辨率和偏移值對原機器人坐標系下（局部坐標系）表示的激光信息進行不同角度的映射，獲得一個包含n個角度的查找表。
其中：

查找表->submap（匹配）
移動機器人自身的狀態包括 (x,y,θ)，通過查找表的方式有效解決了角度的問題，眼下仍需解決位置的問題，采用離散化搜索區域進行定位，并利用多分辨率提高搜索速度。

響應值的計算如下描述：將查找表以一定的為宜投到submap上，此時的submap已有running-scans生成，并利用高斯進行模糊，假設總共有n個點被查找表擊中，擊中的每個點得分不同（高斯模糊的作用），累加得分并除以可以達到的最高得分。

查找表->submap（粗匹配->精匹配）
為提高搜索效率，采用多分辨率的方式，即粗匹配時采用較低的分辨率搜索得到候選區域，再對候選區域進行劃分得到精確求解。

計算節點均值
在每次匹配過程中（粗匹配和精匹配），選取幾個擁有最優響應值的位姿狀態，取平均值作為匹配結果。
最優響應值為一個，設立一個10^-6作為容忍度，一旦有其他位姿狀態的響應值與最優響應值在容忍度內，即為相等；最終的節點為它們的平均值。
迭代：
粗匹配（獲得均值）->（作為初值）精匹配（得到最終均值）
計算節點協方差——位置協方差（粗匹配）

計算節點協方差——角度協方差（粗匹配）

上述協方差存在和理論相背的情況，主要每次計算的分量需要乘上一個響應值。
2）添加頂點和邊
添加頂點：即關鍵幀的位姿
添 加 邊： 當前幀與前一幀、running-scans、near-chains建立連接。

添加邊主要經過三個步驟：
（1）Link to previsous scan
（2）Link to Running scans
RunningScan chain：一定數量且距當前一定距離內的激光數據鏈。
（3）Link to other near chains
NearChain: 以當前節點開始廣度優先的方式從graph中遍歷相鄰的一定距離范圍內所有節點，依據當前id從sensorManager中分別遞增與遞減尋找一定范圍內的chain， 生成nearLinkScans。
前兩個步驟都好理解，主要是第三個，以當前節點為搜索中心，以一定范圍（比如說4m）進行廣度搜索，得到了所有關聯的節點，然后利用節點生成數據鏈（數據鏈需要包含約5個ID連續的關鍵幀）（不包括當前結點，否則放棄該數據鏈），用當前節點進行匹配，若響應值達到一定閾值，產生一條邊，這條邊一端是當前節點，還有一端是數據鏈中質心距離當前節點質心最近的節點。

3）回環檢測
回環檢測的操作和添加鄰近邊類似，步驟較為繁瑣：
（1）依據當前的節點, 從Graph中找到與之相鄰的所有節點(一定距離范圍內)。
（2）采取廣度優先搜索的方式，將相鄰（next）與相連（adjacentVertices）添加進nearLinkedScans。
（3）從sensorManager中取從前到后，依據id序號挑選與當前在一定距離范圍內，且不在nearLinkedScans中的candidateScans, 當數量達到一定size，返回。
（4）loopScanMatcher進行scanTomap的匹配，當匹配response 和covariance達到一定要求認為閉環檢測到。得到調整的correct pose。
（5）Add link to loop : 調整邊（全局閉環）。
（6）觸發correctPose: spa優化
第一步，首先去除那些和當前節點的時間相鄰的節點，針對那些在搜索范圍內，且在時間點上并不相鄰的節點產生一個數據鏈，然后進行匹配，若響應值大于閾值，添加回環并進行全局優化。

4）優化求解

整個優化算法使用了LM優化算法，原文中，作者大篇幅的都是介紹如何得到海森矩陣，以及高效進行優化求解，LM系統如下：

海森矩陣的稀疏：

3、karto_slam總結

1）單從論文角度看，論文并沒有關于整個框架的介紹，大部分的內容都是關于優化方法、黑森矩陣的構造等。關于掃描匹配、頂點和邊、回環檢測等所提甚少。
2）源代碼包含主函數和庫函數兩部分，框架清晰，可讀性強；除了eigen，不需要額外的依賴庫。
3）關于karto_slam的介紹不如hector和gmapping多，可能是 沒有采用主流的地圖更新機制（概率地圖）。只保存了pose節點和激光數據，作為pose-graph。

五、ORB_SLAM

1、ORB_SLAM功能包

基于特征點的實時單目SLAM系統 實時解算攝像機的移動軌跡 構建三維點云地圖 不僅適用于手持設備獲取的一組連續圖像，也可以應用于汽車行駛過程中獲取的連續圖像Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年發表在IEEE Transactions on Robotics上

2、ORB_SLAM2安裝

1）、安裝工具&下載源碼：

sudo apt-get install libboost-all-dev libblas-dev liblapack-dev git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2 # 克隆到home下

2）、安裝eigen3.2.10

去官網下載：http://eigen.tuxfamily.org/index.php?title=Main_Page解壓源碼包，并進入目錄：mkdir buildcd buildcmake ..makesudo make install

3）、編譯g2o

cd ~/ORB_SLAM2/Thirdparty/g2o/mkdir buildcd buildcmake .. -DCMAKE_BUILD_TYPE=Releasemake

4）、編譯DBoW2

cd ~/ORB_SLAM2/Thirdparty/DBoW2/mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Releasemake

5）、編譯Pangolin

sudo apt-get install libglew-dev git clone https://github.com/stevenlovegrove/Pangolin.gitcd Pangolinmkdir build && cd build cmake ..cmake --build .

6）、編譯ORM_SLAM

cd ~/ORB_SLAM2mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=Releasemake

7）、編譯功能包（配置ROS環境）

# 首先修改.bashrc文件 $ cd ~ $ gedit .bashrc# 打開.bashrc文件在最后一行加入 source ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash$ source ~/.bashrc

注意：第7步，編譯功能包 出現的問題：

解決方法：

修改 ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt,添加 -lboost_system

3、啟動單目SLAM示例（基于數據包）

1）、首先下載數據包TUM：這個主要是室內，單目和RGB-D

https://vision.in.tum.de/data/datasets/rgbd-dataset/download#freiburg1_desk

在ORB-SLAM2下新建文件夾Data，把測試的數據解壓在這里。 TUM數據集分為相機fr1，fr2，fr3，對應TUM1-3.yaml； 一般第一次測試用fr1/xyz這個數據集，這個就是x，y，z方向來回動，用來檢測一下系統出沒出什么問題。 其他的數據看名字就知道，比如desk就是在桌子附近來回轉，room就是在房間里面掃來掃去。 值得注意的是，運行其他數據集的時候，單目不一定能追蹤成功，在臺式機上能成功的在虛擬機上也不一定能成功，這就需要我們進行一些調整，比如調整初始化需求數量等，這個關系到對SLAM系統的理解。

2）、分別打開三個終端運行下面的三個命令：

$ roscore# 進入ORB_SLAM2目錄下啟動Mono功能節點。 # 看到開啟的等待數據的可視化建圖界面 $ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml# 進入數據包所在目錄下，運行命令 # /camera/image_raw表示播放的數據要發布的話題名稱，此時可以在界面中看到建圖的效果 $ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw PATH_TO_VOCABULARY：算法參數文件，在ORB_SLAM2/Vocabulary中，將其中的壓縮包解壓即可；PATH_TO_SETTINGS_FILE：相機參數設置文件，需要對camera進行標定產生，也可以使用ORB_SLAM2/Examples/ROS/ORB_SLAM2中已有的設置文件Asus.yaml。

4、啟動AR示例（基于數據包）

$ roscore# 進入ORB_SLAM2目錄下啟動Mono功能節點。 # 看到開啟的等待數據的可視化建圖界面 $ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml# 進入數據包所在目錄下，運行命令 # /camera/image_raw表示播放的數據要發布的話題名稱，此時可以在界面中看到建圖的效果 $ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw

點擊界面Insert Cube，看到正方形插入進來，展示AR效果

5、啟動ORB_SLAM示例（真實攝像頭）

$ roslaunch mbot_navigation usb_cam_remap.launch# 進入ORB_SLAM2目錄下啟動Mono功能節點。 # 看到開啟的等待數據的可視化建圖界面 $ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

6、啟動AR示例（真實攝像頭）

攝像頭要運動起來，來完成地圖構建

$ roslaunch mbot_vision usb_cam_remap.launch# 進入ORB_SLAM2目錄下啟動Mono功能節點。 # 看到開啟的等待數據的可視化建圖界面 $ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

總結

以上是生活随笔為你收集整理的机器人SLAM与自主导航（三）——SLAM功能包的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。