机器人操作系统ROS—使用Cartographer进行激光雷达SLAM建图

机器人 SLAMROS  收藏
0 / 86

主流的激光 SLAM 算法有 hector、gmapping、karto、cartographer。很多同学使用 ROS 默认自带的 gmapping、hector 等比较多,这次带大家一起尝试下传说中的 Google cartographer,看看效果差别有多大。

一、概述

1.主流激光 SLAM 算法

  • 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 建图。

2.google-cartographer 建图算法原理分析

Cartographer 主要是通过闭环检测来消除构图过程中产生的累积误差。用于闭环检测的基本单元是 submap。一个 submap 是由一定数量的 laser scan 构成。将一个 laser scan 插入其对应的 submap 时,会基于 submap 已有的 laser scan 及其它传感器数据估计其在该 submap 中的最佳位置。submap 的创建在短时间内的误差累积被认为是足够小的。然而随着时间推移,越来越多的 submap 被创建后,submap 间的误差累积则会越来越大。因此需要通过闭环检测适当的优化这些 submap 的位姿进而消除这些累积误差,这就将问题转化成一个位姿优化问题。当一个 submap 的构建完成时,也就是不会再有新的 laser scan 插入到该 submap 时,该 submap 就会加入到闭环检测中。闭环检测会考虑所有的已完成创建的 submap。当一个新的 laser scan 加入到地图中时,如果该 laser scan 的估计位姿与地图中某个 submap 的某个 laser scan 的位姿比较接近的话,那么通过某种 scan match 策略就会找到该闭环。Cartographer 中的 scan match 策略通过在新加入地图的 laser scan 的估计位姿附近取一个窗口,进而在该窗口内寻找该 laser scan 的一个可能的匹配,如果找到了一个足够好的匹配,则会将该匹配的闭环约束加入到位姿优化问题中。Cartographer 的重点内容就是融合多传感器数据的局部 submap 创建以及用于闭环检测的 scan match 策略的实现。

主要由 Local SLAM 和 Global SLAM 两部分组成:

Local SLAM:

  • 利用里程计(Odometry)和 IMU 数据进行轨迹推算,给出小车位姿估计值
  • 将位姿估计值作为初值,对雷达数据进行匹配,并更新位姿估计器的值
  • 雷达一帧帧数据经过运动滤波后,进行叠加,形成子图(submap)

Global SLAM:

  • 回环检测
  • 后端优化,全部子图形成一张完整可用的地图

二、cartographer 安装

Google 开源的代码包含两个部分:cartographer 和 cartographer_ros。cartographer 主要负责处理来自雷达、IMU 和里程计的数据并基于这些数据进行地图的构建,是 cartographer 理论的底层实现。cartographer_ros 则基于 ros 的通信机制获取传感器的数据并将它们转换成 cartographer 中定义的格式传递给 cartographer 处理,与此同时也将 cartographer 的处理结果发布用于显示或保存,是基于 cartographer 的上层应用。

源码:

接下来我们安装 cartographer+cartographer_ros,环境为 ubuntu20.04+noetic+pi4b:

# 1.安装编译工具wstool、rosdep、ninjd
sudo apt update
# on Noetic
sudo apt install -y python3-wstool python3-rosdep ninja-build stow
# other distribution
sudo apt-get install -y python-wstool python-rosdep ninja-build stow

# 2.下载cartographer_ros源码
mkdir ~/catkin_ws_carto  &&  cd ~/catkin_ws_carto
wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src

# 3.安装依赖包proto3、deb等
sudo rosdep init   #如果执⾏报错,可以直接忽略
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

./src/cartographer/scripts/install_debs_cmake.sh  #req libs
./src/cartographer/scripts/install_abseil.sh    #abseil
./src/cartographer/scripts/install_proto3.sh    #proto3
./src/cartographer/scripts/install_ceres.sh     #ceres-solver 如果代码下不下来,修改.sh里git clone https://github.com/ceres-solver/ceres-solver.git

# 4.修改配置
vim ~/catkin_ws_carto/src/cartographer_ros/cartographer_ros/configuration_files/revo_lds.lua
  tracking_frame = "base_footprint",  #底盘坐标frame
  published_frame = "odom",      #机器人odom frame(tf关联关系:cartographer map -> odom -> base_footprint -> base_laser_link)
  provide_odom_frame = false,    #是否使用cartographer提供的坐标系
  use_odometry = true,           #是否使用机器人的odom

# 5.编译和安装cartographer_ros整个项⽬⼯程
cd  ~/catkin_ws_carto
catkin_make_isolated --install --use-ninja
source install_isolated/setup.bash

下方是 tf 关联关系示例(使用带底盘的机器人):
cartographer-tf.png

*注:如果你只是 pc 直接接上激光雷达进行测试,那么就只把 revo_lds.lua 里的 tracking_frame、published_frame 改为 laser 的 frame_id 即可。

三、Bag 测试

# 2D bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag
# revo_lds bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/revo_lds/cartographer_paper_revo_lds.bag 
roslaunch cartographer_ros demo_revo_lds.launch bag_filename:=${HOME}/Downloads/cartographer_paper_revo_lds.bag

四、实车测试

1.配置 cartographer slam

vim cartographer.launch
    <launch>  
      <arg name="simulation" default= "false"/> 
      <arg name="version" default="$(env ROS_DISTRO)"/>
      <param name="/use_sim_time" value="$(arg simulation)" />  

      <!-- cartographer_node -->
      <node name="cartographer_node" pkg="cartographer_ros"  
            type="cartographer_node" args="  
                -configuration_directory $(find cartographer_ros)/configuration_files
                -configuration_basename revo_lds.lua"  
            output="screen">  
      </node>  

      <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node"/>

    </launch>

2.SLAM 建图测试

2.1.2D 雷达建图

# 无地图场景导航并建图
roslaunch robot_navigation robot_slam_laser.launch slam_methods:=cartographer
#  pc端
roslaunch robot_navigation navigation_rviz.launch
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

# 保存地图
roscd robot_navigation/maps
rosrun map_server map_saver -f map

# 使用地图
roslaunch robot_navigation robot_navigation.launch
roslaunch robot_navigation navigation_rviz.launch

cartographer-slam.png

2.2. 2D 雷达 + 摄像头联合建图

# 启动激光雷达+base_control
roslaunch robot_navigation robot_lidar.launch 
# 启动深度摄像头
roslaunch robot_vslam camera.launch 
# 启动rgbd+lidar联合建图
roslaunch robot_vslam rtabmap_rgbd_lidar.launch

# pc端查看
roslaunch robot_vslam rtabmap_rviz.launch
# pc端控制移动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py


# 重启rtabmap_rgbd_lidar,打开movebase+自动定位(允许后退) 
roslaunch robot_vslam rtabmap_rgbd_lidar.launch localization:=true planner:=dwa move_forward_only:=false

五、结论

从实际测试来看,与 gmapping/hector 必须控制移动速度并且容易出现衔接问题相比,cartographer 即使在非常快速的移动时,建出的图还原度依然很高,基本都是一次到位,形状非常规整,不同区域衔接的地方做的很好,同时建图速度很快,地图文件很小,实在是 slam 建图的利器!

参考:

Compiling Cartographer ROS

Google Docs » Cartographer

SLAM 建图与自主避障导航——google-cartographer

原文出处