【LIO-SAM 技术深度解析：原理、实现与部署全指南】-CSDN博客

本文链接：https://blog.csdn.net/weixin_47429781/article/details/148136899

一、LIO-SAM核心原理深度剖析

在这里插入图片描述

1.1 紧耦合因子图优化框架（核心创新）

LIO-SAM最核心的创新在于其紧耦合的激光-惯性里程计框架，这个设计理念贯穿整个系统。要深入理解这一点，我们需要从几个关键层面来分析：

1.1.1 因子图理论基础

LIO-SAM构建了一个多因子约束的图优化模型，将不同传感器的观测数据统一转化为因子图中的约束条件。这种设计带来了两大优势：

传感器优势互补：IMU提供高频率的运动估计，激光雷达提供高精度的环境观测，GPS提供全局定位参考
误差自动分配：系统能够根据各传感器的可靠性自动分配权重

因子图结构详解：

节点（变量）：
- xₜ：t时刻的机器人位姿
- vₜ：t时刻的速度
- bₜ：IMU偏置

因子（约束）：
- IMU预积分因子：连接相邻位姿节点
- 激光匹配因子：连接当前位姿与地图
- GPS因子：提供全局位置约束
- 回环因子：连接历史相似位姿

1.1.2 紧耦合 vs 松耦合

传统松耦合方法存在的问题：

各传感器独立处理，信息损失严重
无法处理传感器之间的时空对齐误差
误差累积难以控制

LIO-SAM紧耦合方案的优势：

时间对齐：严格处理各传感器的时间戳同步

// 代码位置：src/imuPreintegration.cpp
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuRaw) {
    // 严格的时间同步处理
    if (imuRaw->header.stamp.toSec() <= lastImuT) {
        ROS_WARN("IMU message disordered!");
        return;
    }
    lastImuT = imuRaw->header.stamp.toSec();
}

空间校准：在线估计传感器之间的外参

// 代码位置：include/utility.h
struct Transformation {
    Eigen::Quaterniond rotation;
    Eigen::Vector3d translation;
    // 外参标定参数
    static Transformation extrin_imu_to_lidar; 
};

1.1.3 优化过程详解

LIO-SAM采用**增量平滑和建图(iSAM2)**算法进行优化，其核心流程：

前端处理：
- IMU数据预积分
- 激光特征提取
- 帧间匹配初估计
因子图更新：

// 代码位置：src/mapOptmization.cpp
void updateFactorGraph() {
    // 添加IMU因子
    addIMUFactor();
    
    // 添加激光因子
    addLidarFactor();
    
    // 添加GPS因子（当可用时）
    if(gpsAvailable) addGPSFactor();
    
    // 执行增量优化
    isam->update(gtSAMgraph, initialEstimate);
}

边缘化处理：
- 保留关键帧节点
- 边缘化非关键帧信息
- 维护稀疏因子图结构

1.2 IMU预积分深度优化

LIO-SAM对IMU预积分的改进是其第二大创新点，这些改进显著提升了系统性能。

1.2.1 预积分理论模型

传统IMU积分存在的问题：

积分误差随时间的二次方增长
对初始条件敏感
难以处理长时间积分

LIO-SAM采用的预积分理论：

ΔR = Πexp((ω - b_w)Δt)  // 相对旋转
Δv = ΣR(a - b_a)Δt      // 相对速度
Δp = Σ(ΔvΔt + 0.5R(a - b_a)Δt²) // 相对位移

1.2.2 关键改进点

偏置自适应估计：

// 代码位置：src/imuPreintegration.cpp
void integrateMeasurement(
    const Eigen::Vector3d& acceleration,
    const Eigen::Vector3d& angular_velocity) {
    // 考虑偏置变化的预积分
    Eigen::Vector3d un_acc_0 = delta_q * (acceleration_0 - bias_a) - g;
    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - bias_g;
    delta_q = delta_q * Quaterniond(1, un_gyr(0)*dt/2, un_gyr(1)*dt/2, un_gyr(2)*dt/2);
}

协方差传播：

精确建模IMU噪声特性
实时更新预积分协方差

// 代码位置：src/imuPreintegration.cpp
Matrix9d A = Matrix9d::Identity();
Matrix<double,9,6> B = Matrix<double,9,6>::Zero();
// 更新协方差矩阵
covariance = A * covariance * A.transpose() + B * noise_gyr_acc * B.transpose();

零速检测与补偿：
- 自动检测静止状态
- 减少静止时的积分漂移

1.2.3 与激光数据的紧耦合

IMU预积分结果作为激光匹配的初始值：

// 代码位置：src/mapOptmization.cpp
void updateInitialGuess() {
    // 使用IMU预积分结果初始化位姿
    transformTobeMapped[0] = imuRollInit;
    transformTobeMapped[1] = imuPitchInit;
    transformTobeMapped[2] = imuYawInit;
    
    // 使用IMU速度估计初始化运动
    transformTobeMapped[5] += imuVeloXInit * scanPeriod;
}

1.3 激光雷达处理创新

1.3.1 改进的特征提取

LIO-SAM对LOAM的特征提取方法进行了重要改进：

自适应曲率阈值：

// 代码位置：src/imageProjection.cpp
void calculateSmoothness() {
    // 动态调整曲率阈值
    float threshold = (cloudSmoothness[maxIndex].value + cloudSmoothness[minIndex].value) / 2.0;
    if(cloudSmoothness[i].value > threshold) {
        // 标记为边缘特征
    }
}

多尺度特征提取：
- 近处点云：高分辨率特征
- 远处点云：低分辨率特征
动态物体过滤：
- 基于点云聚类分析
- 结合运动一致性检测

1.3.2 关键帧管理策略

LIO-SAM的关键帧策略是其高效性的重要保证：

关键帧选择条件：

运动量超过阈值（平移或旋转）
场景变化显著（基于特征分布）
时间间隔超过最小值

// 代码位置：src/mapOptmization.cpp
bool checkKeyframe() {
    // 计算运动量
    float delta_trans = (current_pose.translation() - last_keyframe_pose.translation()).norm();
    float delta_angle = current_pose.rotation().angularDistance(last_keyframe_pose.rotation());
    
    // 自适应阈值
    float trans_thresh = (isFastMoving ? 1.5 : 0.5) * keyframeMeterGap;
    float angle_thresh = (isFastMoving ? 1.5 : 0.5) * keyframeRadGap;
    
    return (delta_trans > trans_thresh) || (delta_angle > angle_thresh);
}

关键帧边缘化策略：

基于信息矩阵的稀疏化
保留强约束的关键帧
动态调整关键帧窗口大小

二、Ubuntu 18.04详细部署指南

2.1 系统环境配置

1. ROS Melodic安装：

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt update
sudo apt install ros-melodic-desktop-full

2. 依赖库安装：

# GTSAM 4.0安装
wget https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip
unzip 4.0.0-alpha2.zip && cd gtsam-4.0.0-alpha2
mkdir build && cd build
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..
make -j$(nproc)
sudo make install

# 其他依赖
sudo apt-get install -y libgoogle-glog-dev libgflags-dev libsuitesparse-dev

2.2 LIO-SAM编译与配置

1. 创建工作空间：

mkdir -p ~/lio_sam_ws/src
cd ~/lio_sam_ws/src
git clone https://github.com/TixiaoShan/LIO-SAM.git
cd ..

2. 修改配置参数：

# 编辑配置文件
nano ~/lio_sam_ws/src/LIO-SAM/config/params.yaml

# 重要参数调整：
# 1. IMU话题名称
imuTopic: "/imu/data"
# 2. 激光雷达参数
pointCloudTopic: "/points_raw"
# 3. 传感器外参（根据实际标定修改）
extrinsicTrans: [0.0, 0.0, 0.0]
extrinsicRot: [1, 0, 0, 0, 1, 0, 0, 0, 1]

3. 编译与运行：

catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash
roslaunch lio_sam run.launch

2.3 常见问题解决方案

1. GTSAM版本冲突：

# 如果系统中有多个GTSAM版本
sudo rm -rf /usr/local/lib/cmake/GTSAM/
sudo rm -rf /usr/local/include/gtsam/

2. IMU数据不同步：
修改params.yaml中的时间偏移参数：

imuAccNoise: 1e-2
imuGyrNoise: 1e-4
imuAccBiasN: 1e-6
imuGyrBiasN: 1e-8

3. 点云显示异常：
检查雷达与IMU的外参标定，特别是旋转部分：

extrinsicRot: 
  - 1
  - 0
  - 0
  - 0
  - 1
  - 0
  - 0
  - 0
  - 1