激光雷达(LiDAR)光束法平差(Bundle Adjustment,BA)已经被用于构建大尺度的一致性点云地图，显著提升了建图的精度，但还未被用于满足状态估计一致性的递推导航系统中。在我们先前开源的FF-LINS中，通过采用基于LiDAR帧间数据关联的方式，解决了传统LiDAR帧到地图数据关联而造成的状态估计不一致性问题(理论方差过于乐观)。在此基础上，我们提出了一种基于帧间同平面点BA的LiDAR/惯性导航估计器BA-LINS，用更紧密的LiDAR同平面点BA约束替代了FF-LINS的点到平面距离约束，并构建了同平面点的自适应协方差估计方法。基于同平面点观测，我们提出了一种LiDAR帧间的同平面点BA测量模型，在图优化框架下实现了LiDAR/惯性紧耦合导航状态估计。BA-LINS适用于多种不同类型LiDAR，包括机械扫描式和固态LiDAR，在复杂环境下实现了更精确、更高效的LiDAR惯性里程计(LIO)。

随着新型固态、半固态激光雷达(LiDAR)的迅速发展，LiDAR已经被广泛应用于自动驾驶车辆、移动机器人等无人系统中。针对陌生复杂环境下可靠精确的导航定位需求，提升无人系统的自主导航定位能力具有重要意义。在紧耦合的导航定位框架下，为了保证状态估计的一致性，LiDAR应该构建基于帧间数据关联的测量模型，从而避免采用帧到地图关联造成位姿协方差出现不发散的假象。在我们的先前研究中，我们提出了FF-LINS[1]，通过构建帧间数据关联，提出了一种LiDAR帧间测量模型，从而建立了一种满足递推导航状态估计一致性的LiDAR惯性里程计(LIO)。然而，FF-LINS本质上基于点到平面距离构建了两两LiDAR帧的相对位姿约束。这就导致LiDAR测量的约束不够紧密，没有实现LiDAR测量的跨帧约束。与此同时，这种两两帧的观测形式会造成观测数量成千上万，进而导致观测更新的计算量非常大。因此，FF-LINS存在着状态估计精度受限、计算耗时偏大的问题。

BALM在视觉光束法平差的启发下，基于自适应体素地图实现了LiDAR边线特征点的跨帧关联，构建了LiDAR的BA测量模型，实现了更精确的LiDAR建图[2]。尽管如此，BALM基于体素地图的数据关联方式，实际上还是一种帧到地图的方式，不适用于满足一致性的导航状态估计。在帧间数据关联的前提上，如果能够建立LiDAR同名点的跨帧关联，就可以实现精确的LiDAR帧间BA，且能够减少LiDAR观测数量而提高估计效率。因此，我们在FF-LINS的研究基础上，提出了一种基于帧间同平面点BA的LiDAR惯性导航状态估计器[3]，BA-LINS的系统设计原理见图1所示。

图1 BA-LINS算法的实现框图

（不同颜色表示不同的数据源，填充的模块表示本工作的主要内容）

工作主要亮点如下：

1、提出了一种LiDAR帧间的同平面点BA测量模型，约束准则为同平面点所形成的平面最薄，所提出的LiDAR BA测量模型构建了一种多状态的相对位姿约束，如图2所示。

2、 基于直接的帧间点云关联方法，实现了同平面点在滑动窗口内的跨帧关联。得益于这种特殊的同平面点数据关联方式，提出了一种LiDAR帧间同平面点BA测量的自适应协方差估计方法。

3、在图优化的框架下，建立了LiDAR同平面点BA和IMU预积分测量的紧耦合状态估计模型，同时实现了LiDAR-IMU时空参数的在线估计。

图2：LiDAR同平面点BA示意图

（LiDAR帧间同平面点BA测量模型构建了一种对多帧位姿的约束，是一种多状态约束的形式，类似于视觉导航中的BA）

(a) Ouster OS1-64

(b) Livox AVIA

(c) Livox Mid-70

图3：多种不同LiDAR的建图效果(基于BA-LINS估计的位姿投影)

图3展示了基于BA-LINS估计位姿的LiDAR建图效果，可以看到BA-LINS在建筑、街道、树林等多种不同场景都能够实现精确的位姿估计，因此构建的LiDAR点云地图结构清晰，能够明显区分不同的景物特征。机器人数据集上的位姿误差如表1所示，相比于目前的SOTA算法FAST-LIO2、FF-LINS和BALM，BA-LINS实现了更高的位姿精度。相对于基准系统FF-LINS，BA-LINS的绝对平移精度提升了29.5%。得益于采用BA测量模型而带来的观测数量显著减少，BA-LINS的状态估计耗时相比于FF-LINS减少了28.7%。在此基础上，通过进一步采用多线程技术，BA-LINS的等效运行效率是FF-LINS的2倍，实现了运行效率的巨大提升，如表2所示。

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

表2：机器人数据集上的状态估计耗时和等效运行速率对比

BA-LINS基于我们开源的LiDAR惯性导航算法软件FF-LINS开发(https://github.com/i2Nav-WHU/FF-LINS)。在FF-LINS的基础上，BA-LINS实现了LiDAR同平面点的跨帧数据关联，构建了同平面点的帧间BA测量模型，并实现了BA测量协方差的自适应估计，从而将精确的LiDAR BA方法应用到了满足状态估计一致性的LIO递推导航系统中。开源和自采集数据集的充分测试结果表明，BA-LINS适用于多种不同类型的LiDAR，实现了定位精度和估计效率的显著提升。BA-LINS具有广泛的应用前景：通过引入回环约束，BA-LINS能够实现大尺度场景的LiDAR建图；BA-LINS满足状态估计的一致性，能够无缝地集成到包含GNSS、UWB、高精地图等绝对测量的多源融合导航系统中。

更详细的算法方案和测试结果可参考我们的论文。论文已被SCI 一区TOP期刊IEEE Transactions on Intelligent Transportation Systems 接收。

参考文献

[1] 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.

[2] Z. Liu and F. Zhang, “BALM: Bundle Adjustment for Lidar Mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3184–3191, Apr. 2021, doi: 10.1109/LRA.2021.3062815.

[3] Hailiang Tang, Tisheng Zhang, Liqiang Wang, Man Yuan, and Xiaoji Niu, “BA-LINS: A Frame-to-Frame Bundle Adjustment for LiDAR-Inertial Navigation,” IEEE Transactions on Intelligent Transportation Systems, pp. 1-14, 2025, doi: 10.1109/TITS.2024.3524569.