LIO-Livox 算法开源: 一个鲁棒的激光-惯性里程计

Livox Pioneer

凭着独特的类固态旋镜式扫描方式和极致的产品性价比，Livox 一经面世便颠覆了业内对高性能激光雷达价格昂贵的认知。为帮助用户迅速上手这种新型的激光雷达，Livox陆续开源了各类感知算法的适配，用Livox 激光雷达实现了优秀的性能。




同时，我们也发现部分用户在高速场景SLAM时遇到一些挑战。为帮助用户打开思路，本文尝试仅用单台Livox Horizon (含内置IMU) 实现高鲁棒性的激光-惯性里程计，可在各类极端场景下鲁棒运行，并达到高精度的定位和建图效果。


图1：系统以 80km/h 左右时速穿过长达 4 公里的隧道



图2: Demo车顶的前向的Horizon激光雷达

上传视频封面

视频1: Demo车横穿隧道实时Slam
​

If it doesn't play normally, please click here to open in a new window

下面对本开源系统做进一步的说明。




系统由ScanRegistration和PoseEstimation两个模块组成:

ScanRegistration模块负责从原始点云中去除动态物体并提取特征点供位姿估计使用;




PoseEstimation模块，主线程进行IMU初始化和状态估计，同时另一个线程构建和管理特征地图。





图3: 系统流程图

• 去除动态物体 ScanRegistration模块
  
  

在城市场景中，通常会有很多动态物体，比如车辆，行人，这些物体会影响激光雷达或者相机的定位鲁棒性和精度。因此需要一种快速去除动态物体的点云分割方法。




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


图4：点云分割示意图。蓝色点：地面；绿色点：背景；黄色点：前景动态物体；


2. 提取特征点 ScanRegistration模块

我们需要设计一种特征提取方法，使得特征点在空间中尽可能广泛和均匀地分布在6个自由度上面，以提供更多约束来稳住系统。这样即使在一些空旷场景里可提取的特征点非常少，也能防范系统在某些自由度上发生退化。




此外，在特征点少的场景中，一些不规则的点也可以提供有用的约束。我们将这些不规则的点提取出来单独作为一类进行点云匹配和地图维护。


图5：特征提取流程图


如下图所示，根据点的局部几何性质我们将特征点分为三类：边缘特征，平面特征，和不规则特征。首先将每条扫描线上的大曲率点和孤立点提取出来作为边缘特征。然后使用主成分分析法区分平面特征和不规则特征。对于不同的距离，特征提取使用不同的阈值，这样便可使能特征点在空间中的分布尽可能均匀。


图6 特征点示例绿点为边缘特征点；粉色为平面特征点；蓝色为不规则特征点；


3. IMU 初始化 PoseEstimation模块

在PoseEstimation模块中，根据不同的融合模式，系统可使用IMU预积分或者匀速运动模型来消除点云的自运动畸变，然后进行IMU初始化。如果初始化成功了，系统将从LO模式切换到LIO模式，否则系统将继续运行LO模式并尝试进行初始化。




在LO模式中，我们使用帧到模型的点云配准方法来估计传感器位姿。受ORB-SLAM3算法的启发，我们将最大后验概率估计的方法适配到IMU初始化中，一步联合估计IMU偏置，速度，以及重力方向。这种方法不需要特定的运动过程，系统可在任意运动状态下（静止、运动或混合状态）完成初始化。既考虑了传感器自身的不确定性，也可得到最大后验概率意义下的最优解。




在初始化完成之后，系统将执行一个基于滑动窗口的紧耦合传感器融合模块来优化窗口内的IMU位姿、速度和偏置。




与此同时，系统使用一个额外的线程来并行地构建和维护一个全局地图。




4. 鲁棒性验证 整体系统

我们在各类城市场景中做了大量的测试，系统表现出了良好的鲁棒性。在各种极端的场景中，都可以正常运行。




如上文隧道场景中，由于场景单一，激光SLAM极易发生退化。而我们的系统可穿过4公里长的隧道不发生退化，这主要得益于Livox Horizon足够远的量程和准确的测量精度，仅靠 81.7°的前向FOV, 便可获得场景中的各类信息，如隧道里零散的指示牌和墙壁上的凹槽。这些信息与IMU的紧耦合给容易发生退化的方向提供了约束，实现了对退化的抵御。




而在城际公路场景中，场景较空旷，结构化程度较低，可利用的信息不多，而且高速路上车辆运动速度快，很容易发生退化。但由于我们选用的特征提取方式，灯杆，路牌，植被等信息都得到了充分的利用，相关特征点提供了前进方向上的约束, 使系统在城际公路场景中也有着出色的鲁棒性。




如下视频所示，我们穿越了十公里以上的城市路段，其中经过了密集的高楼区，住宅区，高架桥，快速路，隧道，山林等，期间遇到多次堵车，并持续伴有很多动态车辆。系统表现良好，鲁棒性极高，各种极端场景均未失效，且精度较高，构建的点云地图与谷歌地球配准后表现良好。

If it doesn't play normally, please click here to open in a new window

在城市场景中，往往有大量的动态物体。特别是发生堵车的时候，大量的动态车辆一方面会造成激光雷达的视野遮挡，一方面容易带偏位姿估计。我们的系统去除了动态物体，极大程度地避免了动态物体的影响，能很好地应对高动态的城市场景。

If it doesn't play normally, please click here to open in a new window

5. 结语

我们以鲁棒性为目标，开发了一个适用于室外场景的车载平台激光惯性里程计。Livox Horizon的优秀性能为我们的算法提供了最合适的硬件平台，最终实现各种极端场景中的高鲁棒性。即使只用了一台激光雷达Horizon和内置的6轴IMU，我们的系统依然得到了出色的表现。为了让用户更好地了解和使用我们激光雷达，本系统代码已开源到github上，并提供详细的使用说明。