LIO-SAM 紧耦合激光惯性里程计因子图融合 4 类传感器约束的数学推导在机器人自主导航领域激光雷达与惯性测量单元IMU的融合已成为提升定位精度的关键技术。LIO-SAM 作为这一领域的代表性算法通过因子图框架实现了多传感器数据的紧耦合优化。本文将深入解析其数学本质揭示四种约束因子如何协同构建高精度状态估计系统。1. 因子图框架的数学基础因子图作为一种概率图模型将状态估计问题转化为最大后验概率MAP估计。在 LIO-SAM 中机器人状态序列 $X{x_1,...,x_n}$ 与传感器观测 $Z{z_1,...,z_m}$ 的关系可表示为$$ p(X|Z) \propto \prod_i p(x_i) \prod_j p(z_j|X_j) $$其中 $p(z_j|X_j)$ 为观测因子对应不同传感器的测量模型。系统通过高斯噪声假设将 MAP 问题转化为非线性最小二乘优化$$ X^* \arg\min_X \sum_k ||h_k(X_k)-z_k||^2_{\Sigma_k} $$式中 $h_k(\cdot)$ 为观测函数$\Sigma_k$ 为协方差矩阵。LIO-SAM 的创新在于设计了四类特殊因子因子类型数学形式作用频率误差项维度IMU预积分因子$r_I(x_i,x_j,z^{ij}_I)$200-500Hz15激光里程计因子$r_L(x_i,x_j,z^{ij}_L)$5-20Hz6GPS因子$r_G(x_i,z^i_G)$1-10Hz3回环检测因子$r_C(x_i,x_j,z^{ij}_C)$0.1-1Hz62. IMU预积分因子的推导IMU 原始测量模型包含加速度计和陀螺仪噪声$$ \begin{aligned} \tilde{a}_t a_t b^a_t n^a_t \ \tilde{\omega}_t \omega_t b^g_t n^g_t \end{aligned} $$在时间间隔 $[t_k,t_{k1}]$ 内预积分量计算涉及三重积分$$ \begin{aligned} \Delta R_{ij} \prod_{ki}^{j-1} \text{Exp}((\tilde{\omega}k - b^g_k)\Delta t) \ \Delta v{ij} \sum_{ki}^{j-1} \Delta R_{ik}(\tilde{a}k - b^a_k)\Delta t \ \Delta p{ij} \sum_{ki}^{j-1} [\Delta v_{ik}\Delta t \frac{1}{2}\Delta R_{ik}(\tilde{a}_k - b^a_k)\Delta t^2] \end{aligned} $$预积分残差定义为$$ r_I \begin{bmatrix} \text{Log}(\Delta R_{ij}^T R_i^T R_j) \ R_i^T(v_j - v_i - g\Delta t_{ij}) - \Delta v_{ij} \ R_i^T(p_j - p_i - v_i\Delta t_{ij} - \frac{1}{2}g\Delta t_{ij}^2) - \Delta p_{ij} \end{bmatrix} $$关键实现细节采用Manifold上的李代数扰动避免旋转矩阵过参数化使用BCH公式进行噪声传播MatrixXd A MatrixXd::Identity(15,15); A.block3,3(3,0) -R_ik_1 * skew(a_k - b_a) * dt; A.block3,3(6,0) -0.5 * R_ik_1 * skew(a_k - b_a) * dt*dt; A.block3,3(6,3) MatrixXd::Identity(3,3) * dt;零偏采用随机游走模型更新 $$ b_{k1}^g b_k^g n^{bg}\Delta t $$3. 激光里程计因子的构建激光特征匹配分为两个阶段3.1 局部体素地图构建选取最近 $n$ 个关键帧构建边缘/平面特征地图$$ \begin{aligned} \mathcal{M}^e \downarrow {T_{ik}F^e_k}{ki-n}^i \ \mathcal{M}^p \downarrow {T{ik}F^p_k}_{ki-n}^i \end{aligned} $$其中 $\downarrow$ 表示体素下采样典型参数边缘特征分辨率0.2m平面特征分辨率0.4m滑动窗口大小 $n25$3.2 点到几何距离计算对于边缘特征点 $p^e$匹配直线 $(p_a,p_b)$ 的距离残差$$ d^e \frac{|(p^e-p_a)\times(p^e-p_b)|}{|p_a-p_b|} $$对于平面特征点 $p^p$匹配平面 $(p_a,p_b,p_c)$ 的距离残差$$ d^p \frac{(p^p-p_a)\cdot((p_b-p_a)\times(p_c-p_a))}{|(p_b-p_a)\times(p_c-p_a)|} $$通过高斯牛顿法最小化总残差$$ T^* \arg\min_T \sum (d^e)^2 \sum (d^p)^2 $$优化结果转换为位姿因子gtsam::Pose3 lidarFactor(T_ij); graph.add(BetweenFactorPose3(x_i, x_j, lidarFactor, noiseModel));4. GPS与回环因子的融合策略4.1 GPS因子的自适应融合GPS测量引入绝对位置约束但需处理两个问题时间同步对非同步GPS数据进行线性插值 $$ p_{GPS}(t) p_{k} \frac{t-t_k}{t_{k1}-t_k}(p_{k1}-p_k) $$协方差阈值仅当位姿不确定度超过GPS精度时引入gpsCovThreshold: 0.5 # 位置方差阈值(m^2) poseCovThreshold: 25 # 位姿不确定度阈值GPS残差模型 $$ r_G R_i^T(p_i - p_{GPS}) - p_{ext} $$ 其中 $p_{ext}$ 为GPS天线到IMU中心的杆臂值。4.2 回环检测的欧氏搜索采用基于KD-Tree的最近邻搜索实现回环检测构建位姿空间KD-Treekdtree KDTree(keyframe_positions)半径搜索候选帧std::vectorint candidates; kdtree.radiusSearch(current_pose, 15.0, candidates);通过ICP精配准 $$ T_{icp} \arg\min_T \sum ||Tp_i - q_i||^2 $$回环残差采用相对位姿形式 $$ r_C \text{Log}(T_{ij}^{-1}T_{icp}) $$5. 系统实现的关键优化技术5.1 增量平滑与映射采用iSAM2进行增量优化核心步骤贝叶斯树更新isam.update(graph, initial_estimate)边缘化旧状态isam.marginalizeLeaves(marginalize_keys)滑窗优化保持窗口内15-20个关键帧边缘化窗口外帧但保留其信息矩阵5.2 计算效率优化关键技术对比技术传统方法LIO-SAM改进速度提升点云匹配范围全局地图局部子关键帧5-8x关键帧选择固定间隔运动阈值触发3x优化频率全图优化增量优化10x实测性能指标Intel i7-11800H单帧处理时间≤50ms内存占用≤1.5GB轨迹误差KITTI 000.78%在实际部署中建议采用以下参数组合# 关键帧选择阈值 keyframe_linear: 1.0 # 位移阈值(m) keyframe_angular: 10 # 旋转阈值(deg) # 优化配置 optimization_rate: 1.0 # 优化频率(Hz) max_keyframes: 25 # 最大关键帧数通过这种紧耦合的因子图框架LIO-SAM 在保持实时性的同时实现了厘米级的定位精度。其数学推导揭示了多传感器融合的本质——将不同时空特性的观测统一转化为概率图模型中的约束因子通过优化理论实现状态的最优估计。