LIO-SAM—IMU+Lidar+GNSS构建高精地图

发表评论

本文以6轴IMU、Liar、GPS传感器为基础，记录LIO-SAM建图的过程，以及需要注意的事项。

目录 隐藏
一、概述
二、LIO-SAM
1. 编译LIO-SAM
2. 卫星地图加载测试
3. 播包建图测试
三、实时建图
1. 硬件设备
1.1 Lidar的扫描距离/水平和垂直角度
1.2 IMU的安装位置
2. LIO-SAM配置
2.1 初始化配置
2.2 传感器Topics/Frames
2.3 GPS
2.4 Debug
2.5 Lidar配置
2.6 IMU内参
2.7 IMU-Lidar外参
2.8 点云下采样
3. 实时建图
四、总结

一、概述

LIO-SAM（Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping）是一个利用GT-SAM通过平滑和映射实现紧耦合的激光雷达惯性里程计，实现了高精度、实时的移动机器人的轨迹估计和建图。

架构设计：

工作原理：接收3D激光雷达、IMU以及GPS作为输入，引入四种因子去构建因子图。

  • IMU预积分因子
  • 激光里程计因子
  • GPS因子
  • 闭环因子

论文研读：LIO-SAM—通过平滑和映射实现紧耦合激光雷达惯性里程计

二、LIO-SAM

软件环境：

1. 编译LIO-SAM

# source
mkdir -p ~/catkin_ws/src
cd ~/project
git clone https://github.com/JokerJohn/LIO_SAM_6AXIS.git  # 6轴IMU版本

# modify
cd ~/project/LIO_SAM_6AXIS
# 删除不存在的Geoid.hpp
vim LIO-SAM-6AXIS/src/simpleGpsOdom.cpp
    // #include <GeographicLib/Geoid.hpp>
vim LIO-SAM-6AXIS/src/mapOptmizationGps.cpp
    // #include <GeographicLib/Geoid.hpp>
# include
mkdir LIO-SAM-6AXIS/include/GeographicLib
cp LIO-SAM-6AXIS/ThirdParty/GeographicLib/include/* LIO-SAM-6AXIS/include/GeographicLib/
# 清理
rm LIO-SAM-6AXIS/src/mapOptmization.cpp
# cp to catkin_ws
cp -R LIO-SAM-6AXIS/  rviz_satellite/  ~/catkin_ws/src

# build
cd ~/catkin_ws/
catkin_make

source ~/catkin_ws/devel/setup.bash

2. 卫星地图加载测试

# 修改测试配置
vim src/rviz_satellite/launch/demo.gps   # 测试用坐标（默认美国费城）
    latitude: 22.3387116703647
    longitude: 114.263642735396
    altitude: 107.128592180088
vim src/rviz_satellite/launch/demo.rviz  # 卫星图像服务切换为MapBox、topic修改为/gps/fix
    Object URI: https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=pk.eyJ1IjoieGh1YmQiLCJhIjoiY2w4YWo4OXJ6MGJuMzN3bHl4YXh3OW12YyJ9.Y_5wSWax9RSJ8zgGIWQPxA
    Topic: /gps/fix

# 启动测试（根据GPS位置加载卫星地图的rviz插件）
roslaunch rviz_satellite demo.launch

3. 播包建图测试

首先下载香港科技大学地图bag包（密码：m8g4），包内数据使用的传感器为VLP-16，STIM300 IMU，摄像头，普通GPS。我们把包房

# 配置bag包目录
vim LIO-SAM-6AXIS/launch/test_vlp16.launch
    <!-- <arg name="bag_path" default=""/> -->
    <arg name="bag_path" default="/home/work/data/bag/hkust/hkust_20201105full.bag"/>
    <arg name="sequence" default="hkust_campus"/>  # 地图保存目录

# 启动slam
roslaunch lio_sam_6axis test_vlp16.launch

# 保存地图
rosservice call /lio_sam_6axis/save_map

# 查看地图
pcl_viewer ~/catkin_ws/src/LIO-SAM-6AXIS/data/hkust_campus/globalmap_lidar_feature.pcdhkust_campus/

生成的点云地图：

# 查看订阅关系
rosrun rqt_graph rqt_graph

# 查看TF
rosrun rqt_tf_tree rqt_tf_tree

订阅关系：

TF：

三、实时建图

1. 硬件设备

本文使用以下传感器，可根据对应说明进行传感器安装、配置和调试：

  • Lidar：镭神C16（16线，200米）
  • IMU：Yesense H30（9轴，200HZ）
  • GNSS：G70 + 千寻RTK差分
  • Camera：暂不参与建图，用于回放确认场景
  • 底盘：任意底盘均可

传感器安装调试后，需要确保满足以下几点要求：

  • 数据：传感器正常输出到Topic，数据格式符合要求；
  • 时间：传感器时钟经过同步，时间戳满足对齐条件；
  • 外参：传感器相对后轴中心的安装位置进行过校准，并通过TF输出变换关系；
  • 其他：低线数Lidar尽量具有360度视角；IMU尽量处于后轴中心点；IMU/Lidar的X轴朝前；

关于传感器几个特殊/说明：

1.1 Lidar的扫描距离/水平和垂直角度

调整ls16 lidar扫描距离为200米：

# 点云最近和最远距离：
vim robot/lslidar_ros/lslidar_cx_driver/launch/lslidar_c16.launch
    <！--param name="min_range" value="0.15"/>
    <param name="max_range" value="150.0"/-->
    <param name="min_range" value="0.01"/>
    <param name="max_range" value="200.0"/>

# 修改后启动lidar和rviz，确保激光雷达底座或者线束不会被扫到（会触发障碍物导致无法行走）
roslaunch lslidar_cx_driver lslidar_c16.launch
rviz

调整ls16 lidar的点云屏蔽水平角度：

# 点云屏蔽角度区间（值为度数*100；正前方为0/360度，正后方为180度，向右后方顺时针旋转度数逐渐增加）：
vim robot/lslidar_ros/lslidar_cx_driver/launch/lslidar_c16.launch
    <!-- <param name="angle_disable_min" value="12000"/>
    <param name="angle_disable_max" value="24000"/> -->
    <param name="angle_disable_min" value="0"/>
    <param name="angle_disable_max" value="0"/>
# 0,0意思是不屏蔽，360度的点云都要。如果填写0,18000，表示屏蔽所有右半侧的点云，大家根据实际情况修改即可。

# 启动lidar和rviz，检查屏蔽角度是否生效：
roslaunch lslidar_cx_driver lslidar_c16.launch
rviz

调整激光雷达点云扫描垂直角度：

# 点云扫描最低和最高角度：
vim robot/pointcloud_to_laserscan/launch/pointcloud_scan.launch
    min_height: -0.1
    max_height: 2.0

调整激光雷达位置：

修改base_footprint到laser的tf：

  • x：Lidar中心点到后轴中心点的前后距离差，单位为米。Lidar相较后轴更朝前安装为正值，往后为负值。我这里实际测量是0.26
  • y：Lidar中心点到轴中心点的左右距离差，单位为米。Lidar相较后轴中心点往左安装为正值，往右安装为负值。因为我的Lidar左右是居中安装的，所以这里值为0
  • z：Lidar中心点距离前/后轴中心点的高度差，单位为米。我这里加高前Lidar距地面高0.298，加高后距地面0.438。减去前/后轴中心点距离地面高度0.063，所以加高前z为0.235，加高后z为0.375。
vim robot/turn_on_wheeltec_robot/launch/robot_model_visualization_cx.launch
    <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.26 0.00 0.375 -1.57 0 0  base_footprint velodyne 100" />
1.2 IMU的安装位置

IMU默认应安装在后轴中心点，如果不是，必须输出对应的外参偏移量：

  • x：IMU中心点到后轴中心点的前后距离差，单位为米。IMU相较后轴更朝前安装为正值，往后为负值。我这里实际测量是0.105
  • y：IMU中心点到轴中心点的左右距离差，单位为米。IMU相较后轴中心点往左安装为正值，往右安装为负值。因为我的IMU实际测试为0.052
  • z：IMU中心点距离前/后轴中心点的高度差，单位为米。我这里IMU距地面高0.115，减去前/后轴中心点距离地面高度0.063，z为0.052。
vim robot/turn_on_wheeltec_robot/launch/robot_model_visualization_cx.launch
    <node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0.105 0.052 0.052 0 0 0 base_footprint gyro_link 100" />

2. LIO-SAM配置

2.1 初始化配置

由于C16雷达输出点云格式与Veloyne一致，因此不需要格式转换，可以复用vlp16的配置进行微调。

cd ~/catkin_ws/src/LIO-SAM-6AXIS
cp config/params_vlp.yaml config/params_ls.yaml
cp launch/test_vlp16.launch launch/test_ls.launch

# launch配置
vim launch/test_ls16.launch
      <launch>
      <arg name="project" default="lio_sam_6axis"/>
      <arg name="bag_path" default=""/>
      <arg name="lat" default=""/>
      <arg name="lng" default=""/>
      <arg name="alt" default=""/>
      <arg name="rviz"  default="true"/>

      <!-- rosservice call /lio_sam_6axis/save_map save data path -->
      <arg name="sequence" default="xiangyansi"/>

      <!--set your own Parameters -->
      <rosparam file="$(find lio_sam_6axis)/config/params_ls16.yaml" command="load"/>

      <!--- LOAM -->
      <param name="saveDirectory" type="string" value="$(find lio_sam_6axis)/data/"/>
      <param name="configDirectory" type="string" value="$(find lio_sam_6axis)/config/"/>
      <rosparam param="sequence" subst_value="true">$(arg sequence)</rosparam>
      <include file="$(find lio_sam_6axis)/launch/include/module_loam.launch"/>

      <!--- Robot State TF -->
      <group unless="$(eval bag_path == '')">
            <include file="$(find lio_sam_6axis)/launch/include/module_robot_state_publisher.launch"/>
      </group>

      <!--show satellite-->
      <!--set your orgin gps lla  22.3387279108228 114.263622199692 87.7310562180355 -->
      <group unless="$(eval lat == '' or lng == '' or alt == '')">
            <node pkg="rostopic" type="rostopic" name="fake_gps_fix" args="pub gps/fix sensor_msgs/NavSatFix '{ header: auto, latitude: $(arg lat), longitude: $(arg lng), altitude:  $(arg alt)}'" output="screen"/>
      </group>
       
      <!--- Run Navsat -->
      <node pkg="lio_sam_6axis" type="lio_sam_6axis_gpsOdometry" name="lio_sam_6axis_gpsOdometry" output="log"/>

      <!--- Run Rviz-->
      <group if="$(eval rviz == true)">
            <node pkg="rviz" type="rviz" name="$(arg project)_rviz" args="-d $(find lio_sam_6axis)/launch/include/config/vlp.rviz"/>
      </group>

      <!--- Play Bag-->
      <group unless="$(eval bag_path == '')">
            <node pkg="rosbag" type="play" name="bag_play" args="$(arg bag_path) --clock -d 5 -r 1.0"/>
      </group>
      </launch>
2.2 传感器Topics/Frames
vim config/params_ls16.yaml
    # Topics
    pointCloudTopic: "points_raw" # Point cloud data
    imuTopic: "imu_raw"           # IMU data
    gpsTopic: "gps/fix"           # GPS odometry topic from navsat, see module_navsat.launch file
    odomTopic: "odometry/imu"     # 由imageProjection激光里程计因子生成，实际pub topic时会后缀_incremental，非传感器驱动topic。IMU pre-preintegration odometry, same frequency as IMU

    # Frames
    lidarFrame: "base_link"
    baselinkFrame: "base_link"
    odometryFrame: "odom"
    mapFrame: "map"
2.3 GPS

启用GPS，注意使用gps海拔高度，打开GPS协方差噪声过滤和GPS距离过滤。

 # GPS Settings
    useGPS: true
    # updateOrigin: true                # 停用。when you can not get a reliable gps origin points(RTK fixed)
    gpsFrequence: 10                    # gps topic帧率（单位hz，10hz表示只使用激光雷达时间戳前后100ms的gps数据）set proper gps frequence to align gps and lidar timestamp
    useImuHeadingInitialization: false  # if using GPS data, set to "false"
    useGpsElevation: true               # 是否使用gps海拔高度（Fix=1质量好时，设置true可以解决分层问题） if GPS elevation is bad, set to "false"
    gpsCovThreshold: 8.0                # GPS协方差噪声阈值。 m^2, threshold for using GPS data
    # poseCovThreshold: 25.0            # 停用。m^2, threshold for using GPS data
    gpsDistance: 5.0                    # 当最新有效gps位置与上一帧有效gps位置之间的距离小于n米时，丢弃数据。设计本意是为了降低rtk-lidar位置纠偏频率（因为段距离imu还是比较准的） the distance between two gps point used for optimization
2.4 Debug

按需打开对应的debug开关，方便调试相关数据。

# debug setting
  debugLidarTimestamp: false
  debugImu: false
  debugGps: true
2.5 Lidar配置

lidar的扫描线数、角度、帧数等配置。由于c16点云格式与velodyne一致，我们这里直接使用vlp16的配置。

# Sensor Settings
  sensor: velodyne                            # lidar sensor type, either 'velodyne' or 'ouster'
  N_SCAN: 16                                  # number of lidar channel (i.e., 16, 32, 64, 128)
  Horizon_SCAN: 2000                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
  downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
  lidarMinRange: 0.15                          # default: 1.0, minimum lidar range to be used
  lidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used
2.6 IMU内参

内参的获取方法详见：H30 IMU + Pandar40P Lidar联合标定

  # IMU Settings
  imuAccNoise: 1.3728471070253096e-02
  imuGyrNoise: 8.0812003774619718e-04
  imuAccBiasN: 3.2132560329046421e-04
  imuGyrBiasN: 4.4240582574498520e-05
  imuGravity: 9.80511
  imuRPYWeight: 0.01
2.7 IMU-Lidar外参

为了使系统正常运行，需要在“params.yaml”文件中提供正确的外部转换，我们需要自己根据Lidar-IMU标定情况修改这里的标定外参。

  • extrinsicTrans：Lidar坐标系的原点在imu坐标系下的坐标值。
  • extrinsicRot：表示imu->lidar的坐标变换, 用于旋转imu坐标系下的加速度和角速度到lidar坐标系下
  • extrinsicRPY：旋转imu坐标系下的欧拉角到lidar坐标下

外参的标定方法详见：H30 IMU + Pandar40P Lidar联合标定

我们这里由于lidar和imu之间并不存在旋转，只有前后和上下的平移，因此单独手标并配置extrinsicTrans即可：

  # Extrinsics (lidar -> IMU)
  imu_type: 0   # 0: 6axis, 1:9 axis
  extrinsicTrans: [ 0.25, 0, 0.15 ]
  extrinsicRot: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]
  extrinsicRPY: [ 1, 0, 0,
                  0, 1, 0,
                  0, 0, 1 ]
2.8 点云下采样

如果想保存更精细的地图，建议设置为0.1。否则建议设置0.2。

 # map
  globalMapLeafSize: 0.1                        # 体素下采样大小 saved map voxgrid size

3. 实时建图

# 配置不启用bag
vim LIO-SAM-6AXIS/launch/test_ls16.launch
    <arg name="bag_path" default=""/>
# 启动slam
roslaunch lio_sam_6axis test_ls16.launch

# 保存地图
rosservice call /lio_sam_6axis/save_map

# 查看地图
pcl_viewer ~/catkin_ws/src/LIO-SAM-6AXIS/data/xiangyansi/globalmap_lidar_feature.pcd

实时建图：

查看点云地图：

四、总结

使用廉价的6轴IMU+16线激光雷达，在低速场景，生成的点云地图可以满足导航时的局部点云定位需求。在长距离场景，增加RTK的位置修正，可以改善IMU里程计误差问题。

相比NDT Mapping或不包含GPS修正的LOAM/LIOM等算法，LIO-SAM在同时包含高耸建筑（多路径干扰）、狭窄道路、宽阔广场（特征少）、树木枝叶密集（GPS信号差）、上方建筑遮挡（完全无GPS信号）环境中，所构建出的地图质量更高，且可以与Google地图完全匹配，方便进行城市级的大地图采集和拼接。

如果你希望将本文中的硬件安装在高速车辆上，建议更换线数更高的激光雷达，其他传感器可以保持不变。有什么问题欢迎留言沟通。

 

yan 25.9.7

发表评论

您的电子邮箱地址不会被公开。