1. 项目背景与核心价值
去年接触LIO-SAM时就被激光雷达惯性里程计的精度惊艳到了,但一直苦于其计算资源消耗过大。直到发现Fast-LIO2这个号称"计算效率提升10倍"的开源方案,才真正找到了能在Jetson Xavier NX这类边缘设备上实时运行的SLAM解决方案。经过两个月的实际部署和调参,记录下这些关键修改点,特别适合需要在资源受限环境下实现高精度定位的开发者参考。
Fast-LIO2的核心突破在于:
- 采用iEKF(迭代扩展卡尔曼滤波)替代传统EKF
- 引入紧耦合的激光-IMU融合架构
- 创新性地使用ikd-Tree进行高效点云管理
实测在Intel NUC11(i7-1165G7)上单线程运行仅占用35% CPU,而定位精度在室内环境下能达到厘米级,这使其成为服务机器人、AGV等应用的理想选择。
2. 环境配置优化记录
2.1 依赖库编译技巧
官方推荐的Ubuntu 20.04 + ROS Noetic组合确实最稳定,但有几个依赖项需要特别注意:
bash复制# Eigen建议手动编译3.4.0版本(重要!)
git clone https://gitlab.com/libeigen/eigen.git
mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local/
make -j4 && sudo make install
PCL库的1.10.0版本存在线程安全问题,推荐使用apt安装的1.10.0+dfsg-5ubuntu1版本:
bash复制sudo apt install libpcl-dev=1.10.0+dfsg-5ubuntu1
踩坑提醒:如果在ARM架构设备(如Jetson)上编译,务必添加-march=native编译选项,否则ikd-Tree的性能会下降40%以上。
2.2 传感器驱动适配
针对不同型号的激光雷达,需要修改laserMapping.cpp中的点云预处理逻辑。以Livox MID-360为例:
cpp复制// 原始代码中的通用处理
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
*laserCloud = *pclMsg;
// 修改为Livox专用处理
if (lidar_type == LIVOX) {
pcl::PointCloud<LivoxPoint>::Ptr livoxCloud(new pcl::PointCloud<LivoxPoint>());
pcl::fromROSMsg(*pclMsg, *livoxCloud);
// 添加去畸变处理...
}
实测发现Livox雷达的时间戳对齐特别重要,建议在imageProjection.cpp中添加:
cpp复制double time_diff = fabs(laser_time - imu_time);
if (time_diff > 0.005) { // 5ms阈值
ROS_WARN("Time sync error: %.3fms", time_diff*1000);
}
3. 核心算法调参指南
3.1 噪声参数矩阵调整
配置文件config/xxx.yaml中这几个参数对精度影响最大:
yaml复制# IMU噪声参数(需要根据实际IMU型号调整)
imu_acc_noise: 0.001 # 典型值:Xsens MTi-670 - 0.0008
imu_gyro_noise: 0.0001 # 典型值:BMI088 - 0.00015
imu_acc_bias_n: 0.0002
imu_gyro_bias_n: 0.00001
# 激光雷达参数
laser_point_coveriance: 0.001 # 室外可增大至0.005
建议先用Allan方差工具标定IMU参数,实测某款国产IMU的噪声参数比默认值高3倍,直接使用默认参数会导致轨迹漂移。
3.2 ikd-Tree参数优化
在laserMapping.cpp中找到ikd-Tree构建部分,关键参数:
cpp复制// 原始设置
KD_TREE<PointType>::Ptr ikdtree_ptr(new KD_TREE<PointType>(0.3, 0.6, 0.2));
// 优化建议(针对不同场景):
// 高动态环境(如AGV)
new KD_TREE<PointType>(0.2, 0.4, 0.15);
// 静态环境(如仓储机器人)
new KD_TREE<PointType>(0.5, 1.0, 0.3);
参数说明:
- 第一个参数:叶子节点最小尺寸(米)
- 第二个参数:降采样体素尺寸
- 第三个参数:动态点移除阈值
经验法则:环境动态性越强,参数应该设置得越小。但过小会导致计算量激增,需要在NX上实测保持<50ms/帧。
4. 实际部署问题排查
4.1 内存泄漏问题
长时间运行后发现内存缓慢增长,通过valgrind检测发现是ikd-Tree的节点删除不彻底。解决方案:
cpp复制// 在laserMapping.cpp的~LaserMapping()析构函数中添加
void LaserMapping::freeMemory() {
if (ikdtree_ptr != nullptr) {
ikdtree_ptr->~KD_TREE();
ikdtree_ptr.reset();
}
}
4.2 点云丢失问题
当处理高速移动场景时,偶尔出现点云丢失。通过修改imageProjection.cpp中的缓存策略解决:
cpp复制// 原始代码
pcl::PointCloud<PointType> pl_full;
// 修改为带时间戳的缓存队列
std::deque<pcl::PointCloud<PointType>> cloud_buffer;
const int MAX_BUFFER_SIZE = 10;
if (cloud_buffer.size() > MAX_BUFFER_SIZE) {
cloud_buffer.pop_front();
}
4.3 轨迹漂移修正
发现Z轴方向随时间会有缓慢漂移,在laserMapping.cpp的update函数中添加高度约束:
cpp复制// 在StateEkf()函数中添加
if (use_altitude_constraint) {
Eigen::Matrix<double, 1, 1> z_meas;
z_meas << current_altitude; // 从气压计或已知地图获取
ekf.update(z_meas, H_alt, R_alt);
}
5. 性能优化技巧
5.1 多线程加速
通过修改CMakeLists.txt开启OpenMP并行:
cmake复制find_package(OpenMP REQUIRED)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
target_link_libraries(laserMapping OpenMP::OpenMP_CXX)
然后在laserMapping.cpp的降采样函数中添加:
cpp复制#pragma omp parallel for
for (size_t i = 0; i < pl_surf.size(); ++i) {
// 降采样处理...
}
实测在i7-11800H上处理时间从12ms降至7ms。
5.2 ROS2迁移要点
正在进行的ROS2迁移遇到的主要问题是tf2替换tf1:
cpp复制// 原始ROS1代码
tf::TransformBroadcaster tf_broadcaster;
// 修改为ROS2版本
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
需要特别注意rclcpp的时钟接口变化,建议使用:
cpp复制builtin_interfaces::msg::Time stamp = now();
6. 典型应用场景参数
6.1 服务机器人配置
yaml复制# config/service_robot.yaml
max_iteration: 3
cube_side_length: 200.0
filter_size_surf: 0.3 # 增大滤波尺寸减少计算量
filter_size_map: 0.4
6.2 无人机高速场景
yaml复制# config/drone.yaml
max_iteration: 5 # 增加迭代次数
cube_side_length: 300 # 扩大搜索范围
dense_map_enable: false # 关闭稠密建图
6.3 室内AGV配置
yaml复制# config/agv.yaml
extrinsic_est_enable: true # 开启外参在线标定
plane_estimation: true # 利用地面平面约束
7. 调试工具推荐
- evo轨迹评估:
bash复制evo_ape bag output.bag /ground_truth /laser_odom -va --plot
- rqt_graph优化:
bash复制rosrun rqt_graph rqt_graph
# 特别关注/laser_mapping节点的订阅关系
- 自定义调试话题:
在laserMapping.cpp中添加:
cpp复制pub_debug_ = nh.advertise<sensor_msgs::PointCloud2>("/debug_cloud", 10);
8. 关键问题速查表
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 启动即崩溃 | Eigen版本不匹配 | 重装3.4.0版本 |
| 轨迹跳变 | IMU时间戳不同步 | 检查硬件时间同步 |
| CPU占用100% | ikd-Tree参数过小 | 增大叶子节点尺寸 |
| Z轴漂移 | 缺少高度约束 | 启用plane_estimation |
| 建图模糊 | 点云协方差过大 | 减小laser_point_covariance |