ROS中階筆記(七):機器人SLAM與自主導航—SLAM功能包的使用
ROS中階筆記(七):機器人SLAM與自主導航—SLAM功能包的使用
目錄
1 機器人必備條件
1.1 硬體要求
(1)差分輪式機器人,可使用twist速度指令控制
$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear # linear:xyz方向上的線速度,單位是m/s;
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular # angular:xyz方向上的角速度,單位是rad/s。
float64 x
float64 y
float64 z
(2)機器人必須安裝鐳射雷達等測距裝置,可以獲取環境深度資訊。
(3)最好使用正方形和圓形的機器人,其他外形的機器人雖然可以使用但是效果可能不佳。
1.2 深度資訊
1.2.1 鐳射雷達
$ rosmsg show sensor_msgs/LaserScan # 檢視鐳射雷達訊息結構
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
angle_min:可檢測範圍的起始角度; (—180——180度 )
angle_max:可檢測範圍的終止角度,與angle_min組成鐳射雷達的可檢測範圍;
angle_increment:相鄰資料幀之間的角度步長;
time_incremen:採集到相鄰資料幀之間的時間步長,當感測器處於相對運動狀態時進行補償使用。
scan_time:採集一幀資料所需要的視覺;
rang_min:最近可檢測深度的閾值;
rang_max:最遠可檢測深度的閾值;
ranges:一幀深度資料的儲存陣列。
intensities:每個鐳射點的強度
1.2.2 kinect
Kinect等GRB-D攝像頭,也可以通過紅外攝像頭獲取周圍環境的深度資訊。
depthimage_to_laserscan功能包:將三維點雲資料轉換為二維鐳射雷達資料;
<!--depthimage_to_laserscan節點,將點雲深度資料轉換成鐳射資料-->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
<remap from="image" to="/kinect/depth/image_raw"/>
<remap from="camera_info" to=/kinect/depth/camera_info"/>
<remap froam="scan" to="/scan"/>
<param name="output_frame_id" value="/camera_link"/>
</node>
1.3 里程計資訊
$ rosmsg show nav_msgs/odometry
pose:機器人當前位置座標,包括機器人的XYZ三軸位置與方向引數,以及用於校正誤差的方差矩陣
twist:機器人當前的運動狀態,包括XYZ三軸的線速度與角速度,以及用於校正誤差的方差矩陣。
注意:ROS中所有的座標系都是右手座標系。
1.4 模擬環境
在視訊中給出了一個模擬環境:
$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch # 啟動模擬環境
# 使用Building Editor建立模擬環境cloister.world
2 SLAM功能包的使用方法
論文參考:https://openslam-org.github.io/gmapping.html
2.1 gmapping
2.1.1 gmapping 功能包
基於鐳射雷達
Rao-Blackwellized 粒子濾波演算法
二維柵格地圖
需要機器人提供里程計資訊OpenSlam開源演算法
輸出地圖話題:nav_msgs/OccupancyGrid
$ rosmsg show nav_msgs/OccupancyGrid
2.1.2 柵格地圖取值原理
2.1.3 gmapping安裝
$ sudo apt-get install ros-kinetic-gmapping
2.1.4 配置gmapping節點
參考: http://wiki.ros.org/gmapping
catkin_ws/src/mbot_navigation/launch/gmapping.launch
<launch>
<arg name="scan_topic" default="scan" />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
<param name="odom_frame" value="odom"/>
<param name="map_update_interval" value="5.0"/>
<!-- Set maxUrange < actual maximum range of the Laser -->
<param name="maxRange" value="5.0"/>
<param name="maxUrange" value="4.5"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>
<param name="xmin" value="-1.0"/>
<param name="ymin" value="-1.0"/>
<param name="xmax" value="1.0"/>
<param name="ymax" value="1.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
2.1.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
2.1.6 啟動gmapping(kinect)
建圖效果不佳(不推薦)
$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch
2.2 hector_slam
2.2.1 hector_slam功能包
基於鐳射雷達
高斯牛頓方法
二維柵格地圖
不需要里程計資料
輸出地圖話題:
nav_msgs/OccupancyGrid
2.2.2 安裝hector_slam
$ sudo apt-get install ros-kinetic-hector-slam
2.2.3 配置hector_mapping節點
引數說明可參考:http://wiki.ros.org/hector_slam
catkin_ws/src/mbot_navigation/launch/hector.launch
<launch>
<node pkg = "hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_footprint" />
<param name="odom_frame" value="odom" />
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>
<!-- Map size / start point -->
<param name="map_resolution" value="0.05"/>
<param name="map_size" value="2048"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="laser_z_min_value" value = "-1.0" />
<param name="laser_z_max_value" value = "1.0" />
<param name="map_multi_res_levels" value="2" />
<param name="map_pub_period" value="2" />
<param name="laser_min_dist" value="0.4" />
<param name="laser_max_dist" value="5.5" />
<param name="output_timing" value="false" />
<param name="pub_map_scanmatch_transform" value="true" />
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.7" />
<param name="map_update_distance_thresh" value="0.2"/>
<param name="map_update_angle_thresh" value="0.06" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="5"/>
<param name="scan_topic" value="scan"/>
</node>
</launch>
2.2.4 啟動hector_slam演示
分別開啟三個終端執行以下命令:
$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch # 動一個gazebo模擬節點
$ roslaunch mbot_navigation hector_demo.launch # hector建圖節點
$ roslaunch mbot_teleop mbot_teleop.launch # 啟動一個鍵盤控制節點
第一個節點啟動了gazebo模擬,它使用模擬的雷達給出了一個scan掃描資料;而後下面的節點訂閱了這個資料並進行處理,最後得到一個二維柵格地圖。
hector這個演算法有一個很大的問題,就是在於它的自定位,它利用自身的鐳射雷達資料進行定位,如果上一次的掃描資料與下一次的掃描資料基本一致的話,它就很難確定自己是否在運動,例如當我給一個足夠大的空間,這裡有一條沒有任何特徵點的長廊,長度遠遠超過鐳射掃描的最大距離時,hector基本就失去了建圖的能力。
2.3 cartographer
2.3.1 cartographer功能包
2016年10月5日,谷歌開源
基於圖網路的優化方法
二維或三維條件下的定位及建圖功能
設計目的是在計算資源有限的情況下,實時獲取相對較高精度的2D地圖
主要基於鐳射雷達
後續會支援更多感測器和機器人平臺,同時不斷增加新的功能。
2.3.2 cartographer安裝
方法一:(新方法)
sudo apt-get update
sudo apt-get install ros-<your ros version>-cartographer* # 安裝全部關於cartographer的包
方法二:原來的方法(有好處)
cartographer功能功能包沒有整合到ROS的軟體源裡面,所以需要採用原始碼編譯方式進行安裝。為了不與其他安裝包衝突,最好為cartographer專門建立一個工作空間,這裡我們建立了一個工作空間catkin_google_ws:
1、安裝工具:
$ sudo apt-get update
$ sudo apt-get install -y python-wstool python-rosdep ninja-build
2、初始化工作空間:
$ mkdir google_ws # 建立工作空間,名稱為google_ws
$ cd google_ws
$ wstool init src
3、設定下載地址(掛VPN最好)
在新建的工作空間下輸入下邊指令:
# 如果這一步錯誤,見下面方法一、方法二
$ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
$ wstool update -t src
說明:配置Cartographer需要三個包,分別是:Cartographer、Cartographer_ros 和ceres-solver。
方法一:
# 如果無法從google取出程式碼可以從github提取code:
# 在~/google_ws/src下目錄下面ctrl+h開啟隱藏資料夾,修改裡面第三個包的下載地址:
https://github.com/ceres-solver/ceres-solver.git
方法二:
# 輸入完第一個命令後需要改檔案的下載地址,另起終端輸入:
$ gedit google_ws/src/.rosinstall
# 將最後一個git來源網址由https://ceres-solver.googlesource.com/ceres-solver.git改為https://github.com/ceres-solver/ceres-solver.git 如該果可以掛VPN可以不改。
4、安裝功能包依賴
# 安裝 proto3.sh
進入protobuf的release版本下載頁,link:https://github.com/protocolbuffers/protobuf/releases
# 在google_ws目錄下
# protobuf的下載太慢,可以在官網手動下載好,放到cartographer目錄下(和src同級,具體操作如下)
$ src/cartographer/scripts/install_proto3.sh
# 安裝 deb 依賴,出現錯誤可以忽略
$ sudo rosdep init
$ rosdep update
$ rosdep install --from-paths src --ignore-src --rosdistro=${kinetic} -y
5、編譯功能包
$ catkin_make_isolated --install --use-ninja # 在google_ws目錄下,編譯
6、在當前終端,設定環境變數
source install_isolated/setup.bash #設定環境變數,在當前終端中設定環境變數,只能在當前終端查詢
建議把該句source命令直接寫入系統的bashrc環境裡
$ vim ./bashrc
source install_isolated/setup.bash # 然後把該句source命令新增到最後一行即可
2.3.3 啟動2D、3D 演示
# Download the 2D backpack example bag.
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
# Launch the 2D backpack demo.
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag
# Download the 3D backpack example bag.
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag
# Launch the 3D backpack demo.
roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag
啟動Revo LDS demo演示
啟動PR2 demo填示
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/pr2/2011-09-15-08-32-46.bag
roslaunch cartographer_ros demo_pr2.launch bag_filename:=~/Downloads/2011-09-15-08-32-46.bag
2.3.4 把cartographer功能包執行到自己的機器人上
由於感測器的rostopic不同,我們需要對cartographer訂閱的名稱進行修改,需要在launch檔案中進行修改,但是修改後發現沒有任何作用,這是因為修改後需要重新按照上面的流程進行編譯安裝,這個可能是因為在安裝的時候其實已經把launch檔案安裝到系統中了,所以我們呼叫的時候還是呼叫系統中的launch檔案
1、配置cartographer節點
mbot_navigation/launch/cartographer_demo_rplidar.launch
2、引數配置
mbot_navigation/config/rplidar.lua
2.3.5 啟動cartographer模擬
catkin_make_isolated --install--use-ninja
roslaunch mbot_ gazebo mbot_laser_nav_gazebo.launch
roslaunch cartographer_ros cartographer_demo_rplidar.launch
roslaunch mbot_teleop mbot_teleop.launch
2.4 ORB_SLAM
2.4.1 ORB_SLAM功能包
基於特徵點的實時單目SLAM系統
實時解算攝像機的移動軌跡
構建三維點雲地圖
不僅適用於手持裝置獲取的一組連續影象,也可以應用於汽車行駛過程中獲取的連續影象Raul Mur-Artal,J.M.M.Montiel和Juan D.Tardos於2015年發表在IEEE Transactions on Robotics上
2.4.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 build
cd build
cmake ..
make
sudo make install
3、編譯g2o
cd ~/ORB_SLAM2/Thirdparty/g2o/
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make
4、編譯DBoW2
cd ~/ORB_SLAM2/Thirdparty/DBoW2/
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make
5、編譯Pangolin
sudo apt-get install libglew-dev
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build && cd build
cmake ..
cmake --build .
6、編譯ORM_SLAM
cd ~/ORB_SLAM2
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make
7、編譯功能包(配置ROS環境)
# 首先修改.bashrc檔案
$ cd ~
$ gedit .bashrc
# 開啟.bashrc檔案在最後一行加入
source ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash
$ source ~/.bashrc
$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/ggk/ORB_SLAM2/Examples/ROS
$ cd ~/ORB_SLAM2
$ chmod +x build_ros.sh
$ ./build_ros.sh
第7步,編譯功能包 出現的問題:
解決方法:
修改 ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt,新增 -lboost_system
2.4.3 啟動單目SLAM示例(基於資料包)
KITTI:http://www.cvlibs.net/datasets/kitti/eval_odometry.php,是室外,雙目
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。
2.4.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效果
2.4.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
2.4.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
3 參考資料
2.3 參考資料
Compiling Cartographer ROS
https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html#building-installation
ROS slam-google cartographer編譯和demo執行
https://blog.csdn.net/x_r_su/article/details/52927564
Google Cartographer安裝教程
https://blog.csdn.net/xmy306538517/article/details/81455625
ubuntu18.04配置cartographer
https://blog.csdn.net/SimileciWH/article/details/82939752
微信公眾號:喵哥解說
公眾號介紹:主要研究機器學習、計算機視覺、深度學習、ROS等相關內容,分享學習過程中的學習筆記和心得!期待您的關注,歡迎一起學習交流進步!同時還有1200G的Python視訊和書籍資料等你領取!!!