在视觉惯性里程计中，多状态约束卡尔曼滤波器(MSCKF)已被证明比图优化更高效，且能达到相近的精度。然而，基于MSCKF的激光雷达惯性里程计(LIO)尚未被充分考虑和研究。我们提出了一种基于MSCKF的紧耦合LIO(MSC-LIO），设计了一种无需显式特征提取的高效LiDAR同平面簇(LSPC)跟踪方法，构建了一种多状态约束的LSPC测量模型，并提出了一种基于同平面簇速度信息的LiDAR-IMU时延估计方法。实测结果表明，MSC-LIO相较于当前主流开源方法具有更优的定位精度和更高的计算效率。消融实验表明，提出的LSPC跟踪方法将数据关联效率提高了近三倍。

激光雷达(LiDAR)具备优越的空间感知能力，随着低成本固态LiDAR的快速发展，已在自动驾驶和机器人领域中发挥越来越关键的作用。在紧耦合导航定位框架下，为了与绝对定位传感器如全球导航卫星系统(GNSS)、超宽带(UWB)[1]无缝集成，LiDAR应进行帧间数据关联、构建相对约束，以确保状态估计的一致性。我们前期提出了基于因子图优化的FF-LINS [2]和BA-LINS [3]，通过构建帧间数据关联，分别建立了LiDAR帧间测量模型和LiDAR同平面点BA测量模型，从而实现了状态估计一致的激光雷达惯性里程计(LIO)。然而，FF-LINS和BA-LINS的帧间数据关联方式在每次选择LiDAR关键帧时，需要将最新LiDAR关键帧点云投影到滑窗中所有的历史LiDAR关键帧点云地图，然后进行点到平面ICP。此外，FF-LINS和BA-LINS都基于因子图优化框架进行状态估计，比滤波方法效率低。因此，FF-LINS和BA-LINS存在着数据关联效率低、状态估计耗时偏大的问题。

多状态约束卡尔曼滤波器(MSCKF)能够构建多状态相对约束，同时估计滑窗内多个时刻的位姿，从而在无需迭代的情况下保持较高的精度。LIC-Fusion 2.0等现有MSCKF方法通过将平面地标和多个LiDAR帧关联，构建了多状态相对约束的紧耦合LIO，但其针对传统机械LiDAR显式地提取平面特征，不仅存在传感器适用性不足、数据关联效率低问题，还会造成非结构化场景下的定位精度退化。为此，我们提出了一种无需显式特征提取的高效的LiDAR同平面簇(LiDAR Same-Plane Cluster, LSPC)跟踪方法，同时适用于机械旋转式和新型固态LiDAR。基于LSPC跟踪获取的观测信息，构建了基于MSCKF的紧耦合LIO(MSC-LIO)[4]，兼顾了效率和精度的。MSC-LIO的系统框图如图1所示。

图1 MSC-LIO的系统框图

MSC-LIO的主要贡献和亮点如下：

1. 设计了一种高效的LSPC跟踪方法(见图2)，避免了显式的平面特征提取，适用于机械旋转式和新型固态LiDAR。提出的方法通过滑窗内逐帧跟踪LSPC，避免了多次重复平面点关联，显著提升了前端数据关联的效率。

2. 基于LSPC跟踪方法的特性，近似计算LiDAR平面点的移动速度，并据此提出了一种有效的LiDAR-IMU时延估计方法，实现了时延的在线估计和补偿，提高了LIO对数据的适应性。

3. 在MSCKF框架下，实现了LSPC测量模型与IMU测量值的紧耦合，且支持LiDAR-IMU时空参数在线估计。LSPC测量模型构建了多个LiDAR关键帧之间的精确相对约束，显著提升了LIO的定位精度。

图2 LSPC跟踪示意图

(图中，每个黄色点都是上一关键帧中红色点的投影，所有红色点组成一个LSPC)

RobNav数据集上评估的位姿误差如表1所示。相比目前的SOTA算法FAST-LIO2和FF-LINS，MSC-LIO实现了更高的位姿精度，这主要得益于MSCKF和LSPC测量模型提供的高精度相对位姿估计。算法的运行效率如表2所示。采用LSPC跟踪后，MSC-LIO的数据关联效率提高了近3倍，总运行时间减少了36%；相比于基于因子图优化的FF-LINS(运行时间平均值为194s)，MSC-LIO总运行时间(平均值为69s)减少了64.4%，运行效率显著提高。

表1：RobNav数据集上的绝对旋转误差和绝对平移误差

表 2 每个关键帧的平均运行时间以及数据关联和状态估计的总时间

总的来说，MSC-LIO利用LSPC高效地实现了多LiDAR关键帧的同平面点关联，构建了多状态约束的LSPC测量模型，并基于MSCKF建立了与IMU的紧耦合定位，实现了高效率、高精度的位姿估计。在开源数据集MCD、WHU-Helmet和自采数据集RobNav的测试结果表明，MSC-LIO适用于机械旋转式LiDAR和固态LiDAR，并实现了定位精度和运行效率的显著提升。MSC-LIO满足状态估计的一致性，我们未来会集成包含GNSS等绝对测量信息。

成果被SCI一区TOP期刊IEEE/ASME Transactions on Mechatronics接收。

参考文献

[1] Tisheng Zhang, Man Yuan, Linfu Wei, Yan Wang, Hailiang Tang, and Xiaoji Niu, “MR-ULINS: A Tightly-Coupled UWB-LiDAR-Inertial Estimator With Multi-Epoch Outlier Rejection,” IEEE Robotics and Automation Letters, vol. 9, no. 12, pp. 11786–11793, Dec. 2024, doi: 10.1109/LRA.2024.3498780.

[2] Hailiang Tang, Tisheng Zhang, Xiaoji Niu, Liqiang Wang, Linfu Wei, and Jingnan Liu, “FF-LINS: A Consistent Frame-to-Frame Solid-State-LiDAR-Inertial State Estimator,” IEEE Robotics and Automation Letters, vol. 8, no. 12, pp. 8525–8532, Dec. 2023, doi: 10.1109/LRA.2023.3329625.

[3] Hailiang Tang, Tisheng Zhang, Liqiang Wang, Man Yuan, Xiaoji Niu, “BA-LINS: A frame-to-frame bundle adjustment for LiDAR-inertial navigation,” IEEE Transactions on Intelligent Transportation Systems, vol. 26, no. 5, pp. 6621–6634, May. 2025, doi: 10.1109/TITS.2024.3524569.

[4] Tisheng Zhang, Man Yuan, Linfu Wei, Hailiang Tang, and Xiaoji Niu, “MSC-LIO: An MSCKF-Based LiDAR-Inertial Odometry With Same-Plane Cluster Tracking,” IEEE/ASME Transactions on Mechatronics, 2025, doi: 10.1109/TMECH.2025.3574307.