【SLAM】(一)Google Cartographer的初步嘗試

【SLAM】(一)Google Cartographer的初步嘗試

最近剛定了畢業設計的題目,大概是做SLAM方向,恰好Google不久前開源了他的室內SLAM庫Cartographer,我就拿來用實驗室的Robot試了一下,總體效果還是很好的。

Cartographer是Google的開源SLAM庫,並且包含了ROS介面,主要依賴的感測器是鐳射雷達(Lidar)和慣性導航儀(IMU),用於2D和3D的室內SLAM。
它的設計目標是實現低計算資源消耗,實時優化,不追求高精度(可以達到r=5cm級別的精度)
Paper中說了,它主要的貢獻並不在演算法層面,而是提供了工程上高效率、Robust的程式碼實現(所以更要把它程式碼跑起來試一試~)。它的一個關鍵點在於將branch and bound(分支限界演算法)引入到 loop closure的過程中,提高與之前submap比較的效率。

演算法的基本思路可以看這個圖:

這裡寫圖片描述

Scan是鐳射掃描的單幀資料,通過累加Scan來構建區域性地圖(Submap)。Cartographer的實現並沒有採用濾波方法,而是採用了類似圖優化的模型進行Pose estimation,具體的實現是用了Ceres scan matching(Ceres是Google自家的庫) 。
這樣構造出來的很多很多submap是會產生累計誤差的,最後通過Loop closing來消除這些誤差,完成閉環。

Cartographer的官方文件中提供了很多Demo,基本的ROS框架如下圖所示:

這裡寫圖片描述

其中,Cartographer_node是它的主要模組,它訂閱了很多topic(ROS作業系統中的概念,如果不熟悉ROS可以參考它的tutorial:http://wiki.ros.org/ROS/Tutorials),其中/scan是鐳射雷達的資料,/imu是IMU的資料,我所做的主要工作就是參考Google的demo實現了Laser publisher和IMU publisher,並做了一些測試。

我們的Robot上使用的是hokuyo的鐳射雷達,它提供了現成的ROS node,叫做urg_node,可以在ROS wiki上查詢使用方法。
IMU的publisher使用了python實現,它publish的資料型別是sensor_msgs/Imu。目前我只輸出了orientation,這個publisher還不夠完善,不過已經基本能用了。publisher的實現等完善後再放上來。

我分別在室內和室外測試了該演算法:
室內測試的視訊可以看這裡:百度網盤

室內

這裡寫圖片描述

在室外測試的時候很卡,不知道是因為我們的Robot上CPU不太夠用還是視覺化佔用的記憶體太大,這個我還要繼續研究下(因為離線跑Google資料的時候非常流暢),另外接下來還要補充SLAM的理論知識,並繼續瞭解其他的SLAM演算法。