今天开始尝试使用Lidar+IMU进行3D SLAM建图，本文记录下实验过程。
一、概述
1.测试环境
硬件设备：
- 激光雷达：Hesai Pandar40P
- IMU：Yesense H30（9轴，200HZ）
环境信息：
- Ubuntu 22.04
- ROS 2 Humble
SLAM算法：
- LIO_SAM
2. LIO-SAM简介
LIO-SAM（Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping）是一个利用GT-SAM通过平滑和映射实现紧耦合的激光雷达惯性里程计，实现了高精度、实时的移动机器人的轨迹估计和建图。
架构设计：
二、传感器准备工作
1. Hesai 40P
1.1 ROS2点云调通
1.2 时钟同步
由于暂时没有增加时钟同步功能，目前Lidar默认输出的是1970-07-14的时间，我们需要使用PTP对雷达的时间进行同步，方法详见Hesai Lidar PTP。
启动并检查传感器时钟是否正常：
# check hesai timestamp
sudo ptp4l -m -4 -i eth0
cd ~/ros2_src ros2 launch hesai_ros_driver start.py
# topic
ros2 topic echo /sensors/lidar/points2. H30 IMU
2.1 ROS2数据输出
2.2 时钟同步
由于H30模块不具备时钟同步功能，我们暂时采用sensor精振和系统时间的gap来修正IMU数据时间，方法详见：H30 IMU时间校准。
启动并检查传感器时钟是否正常：
# check H30 timestamp
cd ~/ros2_src
ros2 launch yesense_std_ros2 yesense_node.launch.py
# topic
ros2 topic echo /sensors/imu/data3. 安装结构
为便于测试，我们可以把IMU安装在激光雷达的底部或顶部，让它们的 x 轴指向同一方向。
注意Pandar40P的X方向，X方向并不是线束的正前方，而是90度位置：
IMU的X轴需要与Lidar的X轴对齐，一起朝前安装。
三、LIO_SAM
1. 编译源码
# download code
cd ~/ros2_ws/src
git clone -b ros2 --recursive git@github.com:yanjingang/LIO-SAM.git
# depend
sudo apt install -y ros-humble-perception-pcl ros-humble-pcl-msgs ros-humble-vision-opencv ros-humble-xacro
# Add GTSAM-PPA
sudo add-apt-repository ppa:borglab/gtsam-release-4.1
sudo apt install libgtsam-dev libgtsam-unstable-dev
# build
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release2. 配置
根据你的雷达幸好，配置对应的参数我这里用的是Pardar40P，根据官方产品手册进行配置：
vim src/LIO-SAM/config/params.yaml
# Topics
pointCloudTopic: "/sensors/lidar/points" # Point cloud data
imuTopic: "/sensors/imu/data" # IMU data
# Sensor Settings
sensor: hesai # lidar sensor type, either 'velodyne', 'ouster' or 'livox'
N_SCAN: 40 # Pardar40P有40个通道 number of lidar channels (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
Horizon_SCAN: 1800 # Pardar40P水平分辨率 = 水平视场角 / 水平角分辨率 = 360 / 0.2(10hz时) = 1800 lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
3. Hesai-Veloyne点云格式转换
LIO-SAM、FAST-LIO2算法，对机械雷达只适配了Veloyne雷达，对国内的雷达型号均不适配。国内的速腾、禾赛等雷达需要将点云数据结构转化为Veloyne结构，进而运行LIO-SAM、FAST-LIO2等建图算法。
3.1 添加Hesai Lidar类型
vim src/LIO-SAM/include/lio_sam/utility.hpp
enum class SensorType { VELODYNE, OUSTER, LIVOX , HESAI};
ParamServer(std::string node_name, const rclcpp::NodeOptions & options) : Node(node_name, options){
...
else if (sensorStr == "hesai")
{
sensor = SensorType::HESAI;
}
...3.2 点云格式转换
激光雷达坐标系要符合REP105标准，即XYZ代表前/左/上方向。不同雷达的可能不一样，注意点云的结构跟你的激光雷达驱动有关。禾赛激光雷达的坐标系与传统的REP105不同，其XYZ分别代表左/后/上方向。我们在cachePointCloud()里转换为Velodyne数据格式的同时，修正这个坐标系偏差。
vim src/LIO-SAM/src/imageProjection.cpp
// Hesai DataType
struct PandarPointXYZIRT {
PCL_ADD_POINT4D;
float intensity;
double timestamp;
uint16_t ring; ///< laser ring number
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(PandarPointXYZIRT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(double, timestamp, timestamp)
(uint16_t, ring, ring)
)
...
class ImageProjection : public ParamServer{
private:
pcl::PointCloud<PandarPointXYZIRT>::Ptr tmpPandarCloudIn;
...
public:
void allocateMemory(){
tmpPandarCloudIn.reset(new pcl::PointCloud<PandarPointXYZIRT>());
...
bool cachePointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr& laserCloudMsg){
else if (sensor == SensorType::HESAI) {
// Convert to Velodyne format
pcl::moveFromROSMsg(currentCloudMsg, *tmpPandarCloudIn);
laserCloudIn->points.resize(tmpPandarCloudIn->size());
laserCloudIn->is_dense = tmpPandarCloudIn->is_dense;
double time_begin = tmpPandarCloudIn->points[0].timestamp;
for (size_t i = 0; i < tmpPandarCloudIn->size(); i++) {
auto &src = tmpPandarCloudIn->points[i];
auto &dst = laserCloudIn->points[i];
dst.x = src.y * -1; // 后->前
dst.y = src.x; // 左
//dst.x = src.x;
//dst.y = src.y;
dst.z = src.z;
dst.intensity = src.intensity;
dst.ring = src.ring;
//dst.tiSme = src.t * 1e-9f;
dst.time = src.timestamp - time_begin; // s
}
}
...
void projectPointCloud()
if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER || sensor == SensorType::HESAI)
...调整后，Hesai Pandar激光雷达的base_link变为REP105标准的，你只需要把它当作Velodyne激光雷达来调整IMU之间的外部参数。
4. IMU-Lidar标定
在使用LIO-SAM进行Lidar+IMU建图之前，需要做两个标定工作，IMU内参标定和Lidar-IMU外参标定。IMU内参标定，可以解决其固有的测量误差问题；Lidar和IMU的外参标定(以下简称“外参标定”)。外参标定的目的是获得激光雷达和IMU之间的位置转换关系，其中包括平移关系和旋转关系，分别对应最终输出结果中的平移向量与旋转矩阵。需要使用Ubuntu20.04 + ROS1环境进行标定。
4.1 坐标系
LIO-SAM 将 IMU 原始数据从 IMU 框架转换为激光雷达框架，所以IMU同样需要遵循 ROS REP-105 约定的坐标系（x – 向前，y – 向左，z – 向上）。由于H30 IMU的XYZ同样为前左上，因此不需要调整。
Lidar跟IMU一起都绑定到底盘link上：
vim src/LIO-SAM/config/robot.urdf.xacro
<link name="lidar_link"> </link>
<parent link="chassis_link" />4.2 IMU内参
内参的获取方法详见：H30 IMU + Pandar40P Lidar联合标定
IMU内参配置：
vim src/LIO-SAM/config/params.yaml
# IMU Settings (IMU内参)
# imuAccNoise: 3.9939570888238808e-03
# imuGyrNoise: 1.5636343949698187e-03
# imuAccBiasN: 6.4356659353532566e-05
# imuGyrBiasN: 3.5640318696367613e-05
imuAccNoise: 7.5656614363139437e-03 # acc_n
imuGyrNoise: 1.2050330151032497e-03 # gyr_n
imuAccBiasN: 2.9325075138204079e-04 # acc_w
imuGyrBiasN: 1.4177677047615486e-05 # gyr_w
imuGravity: 9.80511
imuRPYWeight: 0.014.3 IMU-Lidar外参
为了使系统正常运行，需要在“params.yaml”文件中提供正确的外部转换，我们需要自己根据Lidar-IMU标定情况修改这里的标定外参。（配置里有两个外部参数的原因是作者用的 IMU Microstrain 3DM-GX5-25加速度和姿态具有不同的坐标，用于分别设置）
- extrinsicTrans：Lidar坐标系的原点在imu坐标系下的坐标值。
- extrinsicRot：表示imu->lidar的坐标变换, 用于旋转imu坐标系下的加速度和角速度到lidar坐标系下
- extrinsicRPY：旋转imu坐标系下的欧拉角到lidar坐标下
外参的标定方法详见：H30 IMU + Pandar40P Lidar联合标定
外参配置：
vim src/LIO-SAM/config/params.yaml
# Extrinsics (lidar -> IMU 外参)
# extrinsicTrans: [ 0.0, 0.0, 0.0 ]
# extrinsicRot: [-1.0, 0.0, 0.0,
# 0.0, 1.0, 0.0,
# 0.0, 0.0, -1.0 ]
# extrinsicRPY: [ 0.0, 1.0, 0.0,
# -1.0, 0.0, 0.0,
# 0.0, 0.0, 1.0 ]
extrinsicTrans: [0.0169827, -0.0285108, 0.097318]
extrinsicRot: [-0.03966705, -0.99917971, -0.00815956,
0.99920914, -0.0396428, -0.00311358,
0.00278756, -0.00827661, 0.99996186]
extrinsicRPY: [-0.03966705, -0.99917971, -0.00815956,
0.99920914, -0.0396428, -0.00311358,
0.00278756, -0.00827661, 0.99996186]
5. 重新编译
# build
cd ~/ros2_ws
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=lio_sam三、SLAM建图
1. 采集数据建图
# 启动传感器
ros2 launch hesai_ros_driver start.py
ros2 launch yesense_std_ros2 yesense_node.launch.py
# 启动PTP时钟同步，观察Lidar/IMU时间是否一致
sudo ptp4l -m -4 -i eth0
# 启动SLAM建图
ros2 launch lio_sam run.launch.py
# 保存地图
ros2 service call /lio_sam/save_map lio_sam/srv/SaveMap2. 常见问题
2.1 折线或螺旋
如果激光雷达和 IMU 数据格式符合 LIO-SAM 的REP-105格式要求，那么这个问题可能是由激光雷达和 IMU 数据的时间戳不同步或同步精度不足引起的。
解决方法：
- 确认激光雷达和IMU的X轴方向保持一致
- 调整激光雷达点云坐标格式为REP-105格式
- 保持开着ptp时钟同步，同时降低IMU与系统时钟同步的误差到5-10ms
实测完成以上三步后，折线和螺旋旋转现象消失，开始出现初始化跳动问题。
2.2 上下跳动
如果刚开始base_link 立即开始上下跳动，那么IMU extrinsics 很可能是错误的。例如，重力加速度为负值。
解决方法：
- IMU frame使用base_link。
- IMU内参标定：设置params.yaml中的imuAccNoise、imuGyrNoise、imuAccBiasN、imuGyrBiasN。
- IMU-Lidar外参标定：设置params.yaml中的extrinsicTrans、extrinsicRot、extrinsicRPY矩阵(一定要设置正确，否则各种跑飞和抖动现象)。
2.3 移动距离不准
实际移动与地图上的距离不对应,地图上走的更慢,走着走着就会重叠。extrinsicRot和extrinsicRPY矩阵如果设置不正确则建出的地图或出现重叠。
解决方法:
- IMU内参标定
- IMU-Lidar外参标定
2.4 保存地图过程中topic消失
地图过大，保存地图文件时耗时较长，但是建图程序的map topic输出不会等那么就就退出了，需要手动调下ros的默认退出时间：
sudo vim /opt/ros/humble/lib/python3.10/site-packages/launch/actions/execute_local.py
sigterm_timeout: SomeSubstitutionsType = LaunchConfiguration(
'sigterm_timeout', default=30),
sigkill_timeout: SomeSubstitutionsType = LaunchConfiguration(
'sigkill_timeout', default=30),
建图效果：
四、总结
总体来说LIO-SAM算法对精度非常敏感，要想正常建图，需要注意以下事项：
- Lidar和IMU时间对齐
- Lidar、IMU数据格式符合 LIO-SAM 的REP-105格式要求，尤其注意坐标系方向
- Lidar-IMU联合标定外参需要精准
- 道路宽度大于两个车体宽度
- 局部有足够的特征点，喜欢建筑物（如果公园那种没有建筑，只有道路和树木的，无法建图）
- 采集过程中车辆的晃动或其他车辆或行人的进入都很容易导致地图建花
- 更多激光雷达的线数、更高的IMU帧率、增加RTK数据，对建图效果都有改善作用
参考：
https://github.com/TixiaoShan/LIO-SAM/tree/ros2
十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）
IMU+激光雷达融合使用LIO-SAM建图学习笔记——详细、长文、多图、全流程
禾赛、速腾三维雷达适配lio-sam、fast-lio2建图算法
LIO-SAM运行自己数据包遇到的问题解决–SLAM不学无数术小问题
LIO-SAM Hesai PandarXT32雷达 imu：N100 戴世组合惯导GPS
