💻 computer science

SE(3)-LIO: Smooth IMU Propagation With Jointly Distributed Poses on SE(3) Manifold for Accurate and Robust LiDAR-Inertial Odometry

本文提出了 SE(3)-LIO，一种在 SE(3) 流形上联合分布位姿以实现平滑 IMU 传播并引入不确定性感知运动补偿的激光惯性里程计方法，有效解决了现有方法在运动预测中旋转与平移解耦以及运动补偿中相对变换不确定性建模不足的问题。

原作者： Gunhee Shin, Seungjae Lee, Jei Kong, Youngwoo Seo, Hyun Myung

发布于 2026-03-18

📖 1 分钟阅读☕ 轻松阅读

原作者： Gunhee Shin, Seungjae Lee, Jei Kong, Youngwoo Seo, Hyun Myung

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

这篇论文介绍了一种让机器人（比如无人机或自动驾驶汽车）在移动时能更精准地“知道自己在哪里”的新方法。这项技术叫做 SE(3)-LIO。

为了让你更容易理解，我们可以把机器人想象成一个在黑暗中蒙眼走路的探险家，他手里拿着两个工具：

激光雷达（LiDAR）：像是一双能看清周围环境的眼睛，但眨眼频率慢（每秒扫几次）。
惯性测量单元（IMU）：像是内耳的前庭系统，能感知加速度和旋转，反应极快（每秒几百次），但时间久了会“晕头转向”（产生误差）。

这篇论文主要解决了两个老问题，并提出了一个聪明的新方案：

1. 旧方法的痛点：旋转和移动“分家”了

比喻：像是一个笨拙的舞者
以前的方法在预测机器人下一步去哪时，把“转圈”（旋转）和“走路”（平移）分开处理。

问题：想象你在旋转的同时向前跑。如果你只算“向前跑”的距离，却忽略了“转圈”带来的方向变化，你最终的位置就算错了。就像那个笨拙的舞者，转圈时没把脚步带对，最后肯定踩错地方。
后果：在机器人快速急转弯或剧烈运动时，这种分开计算的方法会让位置预测越来越不准，就像滚雪球一样，误差越积越大。

2. 新方案的核心：把“转”和“走”绑在一起（SE(3) 流形）

比喻：像是一个训练有素的体操运动员
作者提出了一种新的数学方法（在 SE(3) 流形上传播），把旋转和移动看作一个整体。

做法：不再把转圈和走路分开算，而是把它们融合在一起。当机器人转圈时，系统会自动调整脚步的方向和距离。
效果：就像体操运动员在做空翻时，身体和脚步完美协调。这样即使机器人动作很剧烈（比如无人机急转弯），它也能精准地知道自己下一秒在哪里，不会“晕”过去。

3. 第二个痛点：修正误差时的“盲目自信”

比喻：像是一个不确定的翻译官
机器人用 IMU 预测位置后，需要用这些预测值来修正激光雷达看到的图像（因为机器人动的时候，激光雷达扫出来的图是扭曲的，这叫“运动畸变”）。

问题：以前的方法在修正图像时，假设 IMU 预测的两个位置是完全独立的。这就像两个翻译官，明明刚才还在一起说话，互相知道对方的想法，但翻译时却假装互不认识，导致翻译出来的意思偏差很大。
后果：这种“假装独立”的做法，会让系统对误差的估计要么太保守（觉得误差很大，不敢用），要么太乐观（觉得误差很小，结果用错了）。特别是在看远处的物体时，一点点角度偏差都会导致巨大的位置错误。

4. 新方案的亮点：知道“不确定性”的修正（UAMC）

比喻：像是一个懂得“留有余地”的导航员
作者提出了一种**“感知不确定性的运动补偿”（UAMC）**。

做法：系统不仅计算位置，还计算“我和刚才那个位置有多大的关联”。它知道这两个预测值是“亲戚”（有相关性），所以在修正图像时，会把这些关联考虑进去，给每个修正后的点都贴上“可信度标签”。
效果：就像导航员知道“刚才那段路有点颠簸，所以现在的定位可能稍微有点飘”，于是他在修正地图时，会小心翼翼地处理那些不确定的点。这样生成的“无畸变点云”（修正后的地图）更加真实可靠。

总结：SE(3)-LIO 到底强在哪？

这就好比给机器人的大脑装上了一个超级导航系统：

动作更协调：转圈和走路不再分家，剧烈运动时也不容易迷路。
修正更聪明：在修正图像扭曲时，它知道哪些数据是“铁哥们”（有相关性），从而更精准地消除误差，而不是盲目猜测。

实验结果：
作者在无人机（飞得很快、转得很急）和越野车（在崎岖不平的森林里开）上做了测试。结果显示，SE(3)-LIO 比目前最先进的方法更准、更稳，即使在环境很乱、数据很少的情况下，也能让机器人稳稳地找到回家的路。

简单来说，这篇论文就是教机器人如何更优雅地跳舞（处理运动），并更聪明地看路（处理误差），从而在复杂的世界里不再“迷路”。

这篇论文提出了一种名为 SE(3)-LIO 的激光雷达 - 惯性里程计（LiDAR-Inertial Odometry, LIO）框架，旨在解决现有 IMU 传播方法在运动预测和运动补偿方面的局限性。该方法通过在 SE(3) 流形上进行平滑的 IMU 传播，并结合不确定性感知运动补偿（UAMC），显著提高了里程估计的准确性和鲁棒性。

以下是该论文的详细技术总结：

1. 研究背景与问题 (Problem)

现有的 LIO 系统通常利用 IMU 的高频测量进行运动预测（作为先验）和运动补偿（消除运动畸变）。然而，传统方法存在两个主要缺陷：

运动预测不准确（Motion Prediction）： 传统方法通常将 6-DoF 位姿分解为旋转（SO(3)）和平移（ $\mathbb{R}^3$ ），并在各自的流形上分别传播。这种做法忽略了旋转变化对平移传播的影响，导致在剧烈运动（如快速旋转伴随平移）下，运动预测出现较大误差。
运动补偿中的不确定性处理不当（Motion Compensation）： 为了消除 LiDAR 扫描过程中的运动畸变，需要将不同时刻的采样点变换到参考位姿。由于 IMU 传播本身存在误差，预测位姿之间的相对变换也带有不确定性。现有方法（如 MA-LIO）在计算相对变换的不确定性时，往往假设预测位姿是独立的，忽略了它们之间的相关性，从而导致对不确定性的高估（Overestimation），特别是在远距离测量中，这会降低系统的精度。

2. 核心方法论 (Methodology)

A. 基于 SE(3) 流形的平滑 IMU 传播 (Smooth IMU Propagation on SE(3) Manifold)

状态表示： 论文不再将位姿分离为旋转和平移，而是直接在 SE(3) 流形上表示 6-DoF 位姿（使用变换矩阵 $T$ ）。状态向量包含 $T$ 、机体坐标系下的速度 $B_v$ 以及偏置。
运动学模型： 引入了刚体运动模型，使用旋量（Twist, $\zeta = [v, \omega]^T$ ）来描述运动。
传播机制： 在离散化传播过程中，平移项的计算显式地包含了旋转变化（通过左雅可比矩阵 $J_l$ ）。公式表明，传播后的平移 $t_{i+1}$ 不仅依赖于速度，还依赖于当前的旋转 $\omega$ 和左雅可比矩阵。
优势： 这种方法确保了在传播平移时能正确考虑旋转变化，从而在剧烈运动下生成更平滑、更准确的轨迹先验。

B. 联合分布位姿与相对变换不确定性 (Jointly Distributed Poses & Relative Transformation Uncertainty)

联合分布推导： 由于 IMU 传播是递归进行的（当前状态基于前一状态和 IMU 输入），预测出的不同时刻位姿之间存在内在的相关性。论文推导了这些预测位姿的联合分布（Joint Distribution）。
不确定性建模： 在计算相对变换（用于运动补偿）的协方差时，论文利用 Baker-Campbell-Hausdorff (BCH) 公式展开，并明确保留了交叉项（Cross terms）。
- 传统方法假设位姿独立，忽略交叉项，导致不确定性被高估。
- SE(3)-LIO 利用联合分布计算交叉项，从而更精确地刻画相对变换的不确定性。

C. 不确定性感知运动补偿 (Uncertainty-Aware Motion Compensation, UAMC)

概率化去畸变： 在将 LiDAR 点云去畸变到参考位姿时，不仅进行几何变换，还将相对变换的不确定性（协方差）融合到每个点的测量噪声中。
结果： 生成一个概率化的去畸变点云（Probabilistic Undistorted Point Cloud）。距离参考位姿越远（即时间差越大）或运动越剧烈，点的测量噪声越大。这使得优化器能够更合理地处理低质量或高不确定性的测量数据。

D. SE(3)-LIO 框架集成

基于**误差状态卡尔曼滤波（ESKF）**构建。
流程：
1. 利用 IMU 在 SE(3) 上传播得到先验分布。
2. 利用 UAMC 处理 LiDAR 点云，构建包含不确定性信息的测量分布。
3. 结合先验和测量分布进行 MAP 估计（最大后验估计），更新位姿。
4. 使用体素地图（VoxelMap）进行特征匹配和残差计算。

3. 主要贡献 (Key Contributions)

SE(3) 流形上的平滑 IMU 传播： 提出了一种在 SE(3) 流形上直接传播位姿的方法，解决了传统分离传播中旋转变化未被有效纳入平移传播的问题，显著提升了剧烈运动下的预测精度。
不确定性感知运动补偿 (UAMC)： 推导了预测位姿的联合分布，精确刻画了相对变换的不确定性（考虑了位姿间的相关性），并将其融入测量噪声，构建了概率化的去畸变点云。
SE(3)-LIO 系统： 集成了上述两种技术的完整 LIO 框架，实现了高精度的里程估计。
广泛的实验验证： 在多种具有挑战性的数据集（包括无人机剧烈运动、手持设备稀疏测量、地面车辆粗糙地形）上进行了验证，证明了其优越性。

4. 实验结果 (Experimental Results)

数据集： 使用了 NTU-VIRAL（无人机剧烈运动）、Newer-College（手持稀疏点云）以及作者自建的粗糙地形地面车辆数据集。
对比基线： 与 FAST-LIO2, PV-LIO, DLIO, LIO-EKF, MA-LIO 等 SOTA 方法进行了对比。
定量结果：
- 在 NTU-VIRAL 数据集的 spms 序列（极高动态，旋转>1.6 rad/s，平移>5.0 m/s）中，其他方法（如 FAST-LIO2）出现严重漂移甚至发散，而 SE(3)-LIO 保持了最低误差（RMSE 0.321m vs 3.12m）。
- 在 Newer-College 和自建数据集的粗糙地形中，SE(3)-LIO 在所有序列中均取得了最佳或次佳性能，特别是在长距离和复杂地形下表现出极强的鲁棒性。
消融实验： 证明了在 SE(3) 上传播比在 $SO(3) \times \mathbb{R}^3$ 上传播有显著提升，且加入 UAMC 后精度进一步提升。
计算效率： 虽然引入了额外的数学推导，但 SE(3)-LIO 的平均计算时间（约 20ms）与 FAST-LIO2 相当，仅增加了约 10% 的额外开销，满足实时性要求。

5. 意义与总结 (Significance)

SE(3)-LIO 通过数学上的严谨性（SE(3) 流形传播和联合分布不确定性建模）解决了 LIO 系统中两个长期存在的痛点：

运动模型更物理： 承认旋转和平移的耦合，避免了在剧烈运动下的模型失配。
误差传播更准确： 不再保守地假设位姿独立，而是利用相关性精确量化不确定性，使得优化器能更智能地处理噪声数据。

这项工作为高动态、复杂环境下的机器人定位导航提供了一种更准确、更鲁棒的解决方案，特别是在传感器数据质量下降（如点云稀疏、剧烈震动）的场景下表现尤为突出。未来的工作计划将其扩展至视觉 - 惯性或雷达里程计等其他传感器配置。