SLAM on 阿波的博客

多层长走廊三维SLAM建图实战

Sun, 19 Nov 2023 18:10:12 +0800

前面把算法在仿真环境都跑通过后，决定拿雷达在真实世界跑一下。先介绍一下最近又发现的两个巨牛的算法：Point-Lio与Faster-lio。

Point-Lio部署

Point-Lio是一种鲁棒且高带宽的LIO算法，具备在极端剧烈运动条件下稳定估计的能力，能够提供准确的、高频的里程计测量（4-8 kHz），可应对严重振动和高角速度或线速度的情况。但对算力的要求较高、CPU负载较大。

先安装livox_ros_driver ，单独创一个工作空间，或者和Point-Lio一个工作空间也行。这里新建一个工作空间：

mkdir -p livox_ros_driver_ws/src #-p 代表递归创建文件夹
cd livox_ros_driver_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver.git
cd ..
catkin_make

然后安装Point-Lio

mkdir -p Point_Lio_ws/src
cd Point_Lio_ws/src
git clone https://github.com/hku-mars/Point-LIO.git
cd Point-LIO
git submodule update --init
cd ../..

编译前先source一下livox_ros_driver的工作空间（如果point-lio的src下有livox_ros_driver则省去此步骤）

source /home/leo/livox_ros_driver_ws/devel/setup.bash

然后编译

catkin_make

由于官方只对avia等固态激光雷达做了启动文件的适配，并没有对mid360雷达做适配，但我们可以将avia的启动文件中的一些雷达参数改为mid360的参数，主要就是线数、IMU外参这些。以下是我在src/config文件夹下增加的mid360.yaml文件配置：

common:
 lid_topic: "/livox/lidar" 
 imu_topic: "/livox/imu" 
 con_frame: false # true: if you need to combine several LiDAR frames into one
 con_frame_num: 1 # the number of frames combined
 cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
 cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
 time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
 # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value

preprocess:
 lidar_type: 1 
 scan_line: 4
 timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
 blind: 0.5 

mapping:
 imu_en: true
 start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
 extrinsic_est_en: false # for aggressive motion, set this variable false
 imu_time_inte: 0.005 # = 1 / frequency of IMU
 satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
 satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units
 acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
 lidar_meas_cov: 0.001 # 0.001; 0.01
 acc_cov_output: 500
 gyr_cov_output: 1000 
 b_acc_cov: 0.0001 
 b_gyr_cov: 0.0001 
 imu_meas_acc_cov: 0.1 #0.1 # 0.1
 imu_meas_omg_cov: 0.1 #0.01 # 0.1
 gyr_cov_input: 0.01 # for IMU as input model
 acc_cov_input: 0.1 # for IMU as input model
 plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
 match_s: 81
 fov_degree: 360 
 det_range: 100
 gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
 gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned
 gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
 extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
 extrinsic_R: [ 1, 0, 0,
 0, 1, 0,
 0, 0, 1 ]

odometry: 
 publish_odometry_without_downsample: false

publish:
 path_en: true # false: close the path output
 scan_publish_en: true # false: close all the point cloud output
 scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame

pcd_save:
 pcd_save_en: false
 interval: -1 # how many LiDAR frames saved in each pcd file; 
 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

之后在src/launch文件夹下增加mapping_mid360.launch文件：

<launch>
<!-- Launch file for Livox AVIA LiDAR -->

	<arg name="rviz" default="true" />

	<node pkg="point_lio" type="pointlio_mapping" name="laserMapping" output="screen">
	<rosparam command="load" file="$(find point_lio)/config/mid360.yaml" />
	<param name="use_imu_as_input" type="bool" value="1"/> <!--change to 1 to use IMU as input of Point-LIO-->
	<param name="prop_at_freq_of_imu" type="bool" value="1"/>
	<param name="check_satu" type="bool" value="1"/>
	<param name="init_map_size" type="int" value="10"/>
	<param name="point_filter_num" type="int" value="1"/> <!--4, 3--> 
	<param name="space_down_sample" type="bool" value="1" /> 
	<param name="filter_size_surf" type="double" value="0.3" /> <!--0.5, 0.3, 0.2, 0.15, 0.1--> 
	<param name="filter_size_map" type="double" value="0.2" /> <!--0.5, 0.3, 0.15, 0.1-->
	<param name="cube_side_length" type="double" value="2000" /> <!--1000-->
	<param name="runtime_pos_log_enable" type="bool" value="0" /> <!--1-->
	</node>
	<group if="$(arg rviz)">
	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find point_lio)/rviz_cfg/loam_livox.rviz" />
	</group>

	launch-prefix="gdb -ex run --args"

</launch>

然后就可以启动Point-Lio算法了：

source devel/setup.bash
roslaunch point_lio mapping_mid360.launch

Faster-Lio部署

Faster-Lio是一种基于增量体素的激光惯性里程计的方法,是Fast-Lio2 的基础上发表的工作。优点是流程短、计算快，扫描频率高可快速跟踪旋转。缺点也是对算力要求略高。

编译的时候会出现寻找livox_ros_driver驱动的过程，如果之前已经单独安装过，或在其他工作空间安装过，那就耐心等待编译过程寻找就行，不用手动结束编译（实在不行就source一下livox_ros_driver的工作空间）。

mkdir -p faster_lio_ws/src
cd faster_lio_ws/src
git clone https://github.com/gaoxiang12/faster-lio.git
cd ..
catkin_make

同样的在src/config文件夹下增加的mid360.yaml文件配置：

common:
 lid_topic: "/livox/lidar"
 imu_topic: "/livox/imu"
 time_sync_en: false # ONLY turn on when external time synchronization is really not possible

preprocess:
 lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
 scan_line: 4
 blind: 0.5
 time_scale: 1e-3

mapping:
 acc_cov: 0.1
 gyr_cov: 0.1
 b_acc_cov: 0.0001
 b_gyr_cov: 0.0001
 fov_degree: 360
 det_range: 100.0
 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic
 extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
 extrinsic_R: [ 1, 0, 0,
 0, 1, 0,
 0, 0, 1 ]

publish:
 path_publish_en: false
 scan_publish_en: true # false: close all the point cloud output
 scan_effect_pub_en: true # true: publish the pointscloud of effect point
 dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.
 scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame

path_save_en: true # 保存轨迹，用于精度计算和比较

pcd_save:
 pcd_save_en: true
 interval: -1 # how many LiDAR frames saved in each pcd file;
 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

feature_extract_enable: false
point_filter_num: 3
max_iteration: 3
filter_size_surf: 0.5
filter_size_map: 0.5 # 暂时未用到，代码中为0， 即倾向于将降采样后的scan中的所有点加入map
cube_side_length: 1000

ivox_grid_resolution: 0.5 # default=0.2
ivox_nearby_type: 18 # 6, 18, 26
esti_plane_threshold: 0.1 # default=0.1

在src/launch文件夹下增加mapping_mid360.launch：

<launch>
<!-- Launch file for Livox AVIA LiDAR -->

	<arg name="rviz" default="true" />

	<rosparam command="load" file="$(find faster_lio)/config/mid360.yaml" />

	<param name="feature_extract_enable" type="bool" value="0"/>
	<param name="point_filter_num_" type="int" value="3"/>
	<param name="max_iteration" type="int" value="3" />
	<param name="filter_size_surf" type="double" value="0.5" />
	<param name="filter_size_map" type="double" value="0.5" />
	<param name="cube_side_length" type="double" value="1000" />
	<param name="runtime_pos_log_enable" type="bool" value="1" />
 <node pkg="faster_lio" type="run_mapping_online" name="laserMapping" output="screen" />

	<group if="$(arg rviz)">
	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find faster_lio)/rviz_cfg/loam_livox.rviz" />
	</group>

</launch>

之后就可以启动Faster-Lio了：

source devel/setup.bash
roslaunch faster_lio mapping_mid360.launch

实物扫描楼层测试

将所有算法移植到NUC上，配置好环境并编译完成后，手持NUC与MID360激光雷达在北京某高层十三楼与十二楼环绕一圈。

随便粘的，像不像个大钻石😁

最后附上对比视频，从效果上看Faster-Lio的定位建图最准确。

CMU-自主探索导航系统（TARE & FAR Planner）部署

Sun, 12 Nov 2023 19:00:09 +0800

去年CMU机器人实验室团队开源了整套的自主探索导航系统，相关文章还荣获IROS2022 最佳学生论文。因此想在实验室的设备上试一下，先跑个官方demo试试。但中文互联网搜索结果的文章质量果然不出我所料，全都是互相抄，下载下来的源码CmakeList文件都是错的😅，最后还是去CMU这个项目的官网才把demo跑起来了。下面记录一下部署过程，我的系统环境是Ubuntu20.04、ROS1 Noetic。

一、安装仿真环境

先安装CMU团队制作的仿真环境，仿真环境包含多楼层停车场、隧道、森林、校园等多种复杂环境，能把这些环境跑好说明这个自主探索导航系统还是很有普适性的。

先安装依赖环境

sudo apt update
sudo apt install libusb-dev

克隆开源存储库

git clone https://github.com/HongbiaoZ/autonomous_exploration_development_environment.git

更换分支，并编译。

cd autonomous_exploration_development_environment
git checkout noetic #如果是melodic 将noetic更换为melodic
catkin_make

如果可以科学上网，运行脚本下载模拟环境，由于网络环境不同，下载可能需要一会。

./src/vehicle_simulator/mesh/download_environments.sh

当然也可以下载官方整理的百度网盘：

链接：https://pan.baidu.com/share/init?surl=7PFWGbQGLLfPy1mHNiiS4A
提取码：qm45

将在百度网盘下载的"autonomous_exploration_environments.zip"解压之后放在src/vehicle_simulator/mesh文件夹下。最终的mesh文件夹结构应该和下面的一致：

mesh
 campus
 indoor
 garage
 tunnel
 forest
 download_environments.sh

然后运行仿真环境

source devel/setup.sh
roslaunch vehicle_simulator system_garage.launch

现在，可以通过点击RVIZ中的“waypoint”按钮发送航路点，然后点击一个点来设置航路点。车辆将导航到航路点，避开沿途的障碍物。请注意，航路点应该是可到达的，并且在车辆附近。

或者，可以运行ROS节点来发送一系列路点。在另一个终端中，转到文件夹并获取ROS工作区，然后使用下面的命令行运行ROS节点。：

roslaunch waypoint_example waypoint_example_garage.launch

导航至航路点

存储库包含一组不同类型和规模的模拟环境。要在特定环境下启动系统，请使用下面的命令行。将“environment”替换为环境名称，即’campus’,、‘indoor’,、‘garage’、 ’tunnel’和’forest’。现在，用户可以使用RVIZ中的“Waypoint”按钮来导航车辆。要在Gazebo GUI中查看环境中的车辆，在启动文件中设置’gazebo_gui = true’，该文件位于’src/vehicle_simulator/launch’中。

roslaunch vehicle_simulator system_environment.launch

二、TARE Exploration Planner部署

同样的，先克隆仓库：

git clone https://github.com/caochao39/tare_planner.git

编译后就可以运行了

cd tare_planner

catkin_make

去上一步的工作空间下运行仿真环境：

source devel/setup.sh
roslaunch vehicle_simulator system_garage.launch

然后在现在的工作空间下运行TARE自主探索算法：

source devel/setup.sh
roslaunch tare_planner explore_garage.launch

现在，应该看到自主探索的行动。同样的，要在不同的环境中启动，使用下面的命令行，并将“environment”替换为开发环境中的一个环境名称，即’campus’, ‘indoor’, ‘garage’, ’tunnel’, 和 ‘forest’。

roslaunch vehicle_simulator system_environment.launch

roslaunch tare_planner explore_environment.launch

TARE自主探索

三、FAR Planner部署

克隆仓库

git clone https://github.com/MichaelFYang/far_planner

编译

cd far_planner
catkin_make

同样的，去第一个工作空间下运行仿真环境：

source devel/setup.sh
roslaunch vehicle_simulator system_indoor.launch

然后在现在的工作空间下运行FAR-Planner算法

source devel/setup.sh
roslaunch far_planner far_planner.launch

现在，我们可以发送一个目标，通过单击在RVIZ的“Goalpoint”按钮，然后点击一个点设置的目标。车辆将导航到目标，并在沿途建立一个可见性图表(青色)。可见性图所覆盖的区域变成了自由空间。当在自由空间中导航时，计划者使用构建的可见性图，当在未知空间中导航时，计划者试图发现通往目标的方法。通过点击“Reset Visibility Graph”按钮，规划器将重新初始化可见性图。通过取消选中“Planning Attemptable”复选框，规划器将首先尝试在空闲空间中找到一条路径。这条路将以绿色显示。如果不存在这样的路径，规划器会一起考虑未知空间。路径将以蓝色显示。通过取消选中”Update Visibility Graph“复选框，规划器将停止更新可见性图。

Far-Planner 导航

如果是实车可以用手柄遥控，这里是仿真环境，Rviz右下角有一个虚拟遥控器，可以遥控车辆移动，并且移动过程中会自动调用局部路径规划自动避开障碍物。

虚拟遥控器与真实遥控器

四、实物部署（待更新）

MID360激光雷达适配LIO-SAM与FAST-LIO2指南

Sun, 12 Nov 2023 16:07:35 +0800

实验室有一个大疆的MID360半固态激光雷达，需要我来探索一个建图效果较好的三维SLAM算法。但是由于是半固态雷达，雷达输出的点云数据格式与普通的多线激光雷达有一些区别，目前业界的激光雷达算法好多都是基于Velodyne的多线雷达格式做的适配，因此在适配LIO-SAM时会遇到诸多问题。

一、适配LIO-SAM

LIO-SAM算法对激光雷达的数据格式有着较为严格的要求，以往的单激光雷达建图的算法没注意到这一点，一般要求的是XYZI(x, y, z, intensity ) 格式即可，但是LIO-SAM要求的是 XYZIRT（x, y, z, intensity, ring, timestamp）格式，即算法内使用了激光雷达的通道数ring参数和时间戳timestep参数，启动算法时会检查是否具有这两个参数，而MID360雷达的输出格式中没有ring与time这两个参数。进一步的，LIO_SAM要用9轴IMU，而MID360内置的IMU是六轴IMU😭。

因此要想适配MID360需要改源码，感谢万能的github，已经有人做好了适配，链接如下:https://github.com/nkymzsy/LIO-SAM-MID360.git也可输入以下命令gitclone到ROS工作空间下的src文件夹下内：

git clone https://github.com/nkymzsy/LIO-SAM-MID360.git

如果gitclone的速度太慢，这里有一个小技巧，那就是将github改为githubfast，亲测有效。

git clone https://githubfast.com/nkymzsy/LIO-SAM-MID360.git

之后要安装Livox-SDK（在工作空间外安装）

git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
cd build && cmake ..
make
sudo make install

还要在src下下载livox_ros_driver 也就是livox雷达的驱动包：

git clone https://github.com/Livox-SDK/livox_ros_driver.git

接下来还需要对LIO-SAM的代码做一点修改，第一处位于src/LIO-SAM-MID360-master目录下，双击打开CMakeLists.txt文件，请将第5行的c++11改为c++14，保存后退出，如下图所示：

第二处位于src/LIO-SAM-MID360-master/include目录下，双击打开utility.h文件，请将第18行的#include <opencv/cv.h>使用”//“注释掉，并添加以下内容：

”#include <opencv2/opencv.hpp>“

还有一个可能报错的地方，解决方法是将下图中的26行的内容注释掉放到第18行。

接下来在工作空间目录下catkin _make应该没有报错了。最后就可以运行LIO-SAM建图了：

source devel/setup.bash
roslaunch lio_sam run6axis.launch

然后启动MID360的驱动

source devel/setup.bash
roslaunch livox_ros_driver<span class="hljs-number">2</span> msg_MID<span class="hljs-number">360</span>.launch

如果手里现在没有雷达，也可以播包：

rosbag play mid360.bag

录制rosbag包的命令是（以下是录制所有的主题，-o 后的参数要换成你自己的bag包储存地址）：

rosbag record -O /home/lingao/mid360_bag/my.bag `rostopic list`

如果只录制imu和雷达数据则输入命令：

rosbag record -O /home/lingao/mid360_bag/my.bag /livox/imu /livox/lidar /clock

二、适配FAST-LIO2

由于FAST-LIO2本身就对LIVOX系列的雷达做了一定的适配，因此对源码基本不需要修改，直接在src里gitclone下来：

 git clone https://github.com/hku-mars/FAST_LIO.git

同样的也需要下载livox_ros_driver：

git clone https://github.com/Livox-SDK/livox_ros_driver.git

然后初始化和更新一个仓库中的子模块：

git submodule update --init

编译

cd ../..
catkin_make

这里我的环境是Ubuntu 20.04 ROS Noetic，所以需要将src/FAST_LIO中的CMakeLists.txt中的C++版本改为14，这样编译就能通过了。

接下来运行fast-lio2：

source devel/setup.bash
roslaunch fast_lio mapping_mid360.launch

然后播包

rosbag play mid360.bag

可以看到建图效果还是不错的，整个楼层没有大的漂移。而反观LIO-SAM这边，顶楼走廊到楼梯那建的都还可以，但是下了楼后高度定位出现了问题，一直还定位到顶楼，最后直接建飞了，把楼层建成了平行宇宙🤣。。。。有可能是我用的是别人录制的rosbag包的原因，IMU与雷达内参没有校准。

LIO-SAM建出平行宇宙

建图对比视频

评论区有大佬指出关闭LIO-SAM回环会好很多，我找了一下，配置文件在lio_sam_mid360_ws/src/LIO-SAM-MID360-master/config下的paramsLivoxIMU.yaml里。将“loopClosureEnableFlag”的“false”改为“true”，然后不用编译直接运行看一下。

回环配置文件

果然有用诶，成功建图，而且从图中效果可以看出细节上比FAST-LIO2要好很多，楼旁边的树和周围楼的墙壁都给建出来了。

关闭回环后LIO-SAM建图效果