用单台Livox Horizon (含内置IMU) 实现高鲁棒性的激光-惯性里程计，可在各类极端场景下鲁棒运行，并达到高精度的定位和建图效果。(城区拥堵、高速公路、幽暗隧道)

注：该系统主要面向大型室外环境中的汽车平台设计。用户可以使用 Livox Horizon 或 HAP LiDAR 轻松运行该系统。在室内环境中系统同样表现稳定，我们目前也支持面向机器人设计的 Mid-360 雷达。

1.系统框架

该系统由两个 ROS 节点组成：ScanRegistration 和 PoseEstimation。

ScanRegistration 节点中的类 “LidarFeatureExtractor” 从原始点云中提取角点特征、平面特征和不规则特征。

PoseEstimation 节点中，主线程用于估计传感器位姿，而类 “Estimator” 中的另一个线程则利用 “MapManager” 类构建和管理特征地图。

2. 配置文件

在启动文件中包含以下参数：

IMU_Mode：用于选择 IMU 信息融合策略，有三种模式可选：

• 0：不使用 IMU 信息，纯激光里程计，使用匀速模型进行运动畸变补偿；

• 1：使用 IMU 预积分进行运动畸变补偿；

• 2：将 IMU 与激光信息进行紧耦合融合。

Extrinsic_Tlb：激光雷达与 IMU 之间的外参，使用 SE3 形式表示。如果使用的是外部 IMU，则需要自行完成传感器标定，并将该参数设置为您系统中的外参值。

Used_Line：HAP和Horizon等效为6线，MID360等效为4线。

Use_seg：选择动态物体滤除的分割方式，有两种模式可选：

• 0：不使用分割方法，如果数据中几乎没有动态物体，可以选择该模式；

• 1：使用分割方法来剔除动态物体。

3. ScanRegistration

系统从节点 “ScanRegistration” 开始运行，该节点负责提取特征点。在特征提取之前，会先从原始点云中剔除动态物体，因为在城市场景中通常存在大量动态物体，这些动态物体会影响系统的鲁棒性和精度。为了过滤动态物体，采用了一种快速的点云分割方法。首先应用欧几里得聚类将点云划分为多个聚类，然后将原始点云分为地面点、背景点和前景点。前景点被视为动态物体，从特征提取过程中排除。通过动态物体滤除模块，系统在动态场景中获得了较高的鲁棒性。

使用欧式聚类方法将原始点云聚类为地面点（蓝色），背景点（绿色），和前景点（黄色），如下图所示。其中前景点(黄色)被视为动态物体，并在特征提取过程中去除。去除之后，系统在动态场景中可保持很高的鲁棒性，即便激光雷达的大部分视场角被动态物体遮挡，也可以准确地定位和建图。

注：具体实现在函数AbvGndSeg()的标记背景点SegBG()和SegObjects()中，这部分代码没有使用PCL提供的相关聚类方法（在DoSeg()函数最开始的降采样也是自己实现的，这部分内容以后可以借鉴）。还有一个是每个点地面在KD树找到相邻点后，对邻近点进行PCA分析，计算这些点的特征值和特征向量，求平均得到地面的法向量 CalGndPos_cor()。

在开阔场景中，通常只能提取较少的特征点，容易导致某些自由度上的退化。为了解决这个问题，设计了一套特征提取流程，使得特征点在空间中分布更广泛且更加均匀。分布广泛且均匀的特征点可以在六个自由度上提供更多约束，有助于消除退化问题。此外，在特征稀疏的场景中，一些不规则点也能提供有用的信息，因此将不规则点也作为一个类别提取，用于点云配准。

根据局部几何属性，特征点被分为三类：角点特征、平面特征和不规则特征。我们首先在每条扫描线上提取曲率较大且孤立的点作为角点特征，然后使用主成分分析（PCA）对其余点进行分类，划分为平面特征和不规则特征，如下图所示。针对不同距离的点，设置不同的阈值，以尽可能保证特征点在空间中的分布均匀。

4. PoseEstimation

在 “PoseEstimation” 节点中，系统首先通过 IMU 预积分或匀速模型对点云的运动畸变进行补偿。随后进入 IMU 初始化模块。如果初始化成功，系统将切换至 LIO（激光惯性里程计）模式；否则，将以 LO（激光里程计）模式运行，并进行 IMU 状态初始化。

在 LO 模式下，系统采用帧到模型（frame-to-model）的点云配准方法来估计传感器位姿。受 ORB-SLAM3 启发，系统引入了一种最大后验（MAP）估计方法，用于联合初始化 IMU 的偏置、速度和重力方向。该方法不依赖繁琐的初始化流程，支持任意运动轨迹下的初始化，同时考虑了传感器的不确定性，从而在最大后验概率意义下获得最优解，具备高效、鲁棒和高精度的特点。

完成初始化后，系统将运行一个基于滑动窗口的紧耦合传感器融合模块，估计滑动窗口内的 IMU 位姿、偏置和速度。同时，另一个线程并行地构建和维护全局地图。

参考：览沃 LIO-Livox 算法开源 一个鲁棒的激光-惯性里程计