LIRO：紧耦合激光-惯性-测距里程计

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

LIRO: Tightly Coupled Lidar-Inertia-Ranging Odometry  紧耦合激光-惯性-测距里程计

下载链接: https://arxiv.org/abs/2010.13072.pdf

作者: Thien-Minh Nguyen, Muqing Cao, Shenghai Y uan, Yang Lyu, Thien Hoang Nguyen, and Lihua Xie

编辑：周朋伟

审核：张海哈

摘要

近年来，由于3D LiDAR 成本和重量不断的降低，在机器人领域中得到广泛的应用。尽管有许多进展，但是轨迹漂移和跟踪失效仍然是这些系统中普遍存在的现象。在理论上，可以利用环境中固定的路标信息作为观测来解决。因此，我们提出一种基于超宽带UWB，激光和惯性测量单元的紧耦合方案。首先，与机器人位姿相关的IMU、LiDAR和UWB的数据在滑动窗口中产生关联。随后，我们构建代价函数融合来自UWB、LiDAR和IMU预积分的测量值。最后，执行优化过程以估计机器人的位置和姿态。通过一些现实世界实验证明了该方法可以有效地解决漂移问题，同时只需要在环境照顾中部署两到三个定位标签。

背景与贡献

在目前的自主导航方案中，由于近年来激光雷达的传感器的成本和重量降低后，基于3D LiDAR的方案得到广泛使用。最初雷达重量超过10公斤，数万美元，因此大多数情况下只能用于大型自动轿车。相比之下，目前的一些商用产品只有几百克的重量, 只需几百美元。对比于基于视觉的定位方案，采用激光雷达有更多优势。例如，雷达可以直接观测公制的几何特征，因此，他可以直接用于机器人的定位，而基于单目相机的视觉SLAM方案仅能够提供模糊的尺度信息。相比于RGBD相机或视觉相机可以同时检测到公制尺度特征，LiDAR具有更远的检测范围。尽管拥有上述优点，由于环境中缺乏几何特征，仍然会使LOAM系列的方案发生轨迹漂移。此外，在这些方法下，机器人只能估计相对于初始时刻的位置。这些问题在诸如3D结构的检查之类的应用中非常不便。其中通常选择所谓的世界坐标系完成轨迹预测。一种方案是利用GPS融合LOAM算法。但是仅仅能够在一些空旷区域有效。另外的方法是通过场景重识别或者回环检测来矫正位姿漂移。在本文中，我们通过使用从机器人到两到三个UWB定位标签的测量值，矫正长时间导航中位置和姿态的漂移现象。位置和姿态也可以通过标签部署定义的坐标系下使用。因此，本文的里程计方案是零漂移和全局的。本文的主要贡献如下：

• 我们将灵活便捷的测距方案与激光-惯性里程计系统集成，以实现无漂移和全局的位姿估计。

• 我们设计了一个高效的紧耦合的传感器融合框架，将载体的距离偏移，IMU预积分和激光里程计融合在一起，以便实时的完成机器人的位姿估计。

• 我们在实际环境中进行了广泛的试验，验证方案的有效性。

方法 1. 符号标记

我们记 时刻机器人的状态包括

其中表示姿态，位置和速度。表示加速度计和陀螺仪的偏置。这里的表示世界坐标系，是由使用者的定位标签决定的。我们每k步执行一次滑动窗口的优化。

在我们的实验中，滑动窗口的长度被设置为10。在我们实验中不对外参进行优化，提前手动标定好。

图1 激光惯性里程计示意图

2. UWB测距和通信网络

如图1所示，我们通过UWB测量路标点节点之间的距离，以及路标点和机器人和路标点之间的距离。

图2 三次距离测量示意图

首先，我们在固定区域放置定位标签。为了简化任务，我们仅在距离地面相同高度上放置两个或三个定位标签记为。可以看出两个以上的定位标签足够定义一个坐标系统。通常我们第0个标签假设在。同时从标签0到标签1的方向被定义为x轴正向。因此，标签1的坐标可以设定为。第三个标签的位置可以通过几何运算直接得到。测距方案的下一个步骤是机器人上部署UWB测距节点，多个UWB节点被安装在机器人上。他们在载体上的位置是已知的，因此可以通过他们直接获得全局姿态。

3. UWB测距工作流程

在标签位置确定以后，机器人到标签的距离可以直接被用于估计位置。首先通过信噪比、视距、变动率等移除不可靠的测量。当系统获得时刻的点云，对应的状态被创建。检查时间段中UWB的缓存，所有的测量数据已经到达。这些数据将会和IMU预测状态一起移除异常值。获得有效的测量值集合。

4. IMU预积分测量流程

IMU测量值的处理流程相对简单，同样提取时间段的数据，通过预积分得机器人的状态and 之间的测量值。同样，IMU预积分结果也被用于优化的初始值。

5. LiDAR点云处理流程

该部分的流程与LIO-Mapping算法类似。从原始的点云中提取平面特征和角点特征后，存储在缓存中。并构建局部子图。不同的特征将会被直接构建成雷达特征因子。

图3 因子图示意图

 6. 多传感器融合联合位姿优化 

图3中展示算法的因子图结构，其中每个UWB测量值，IMU预积分结果和LiDAR特征因子以及先验约束被用于联合位姿优化，残差函数的形式如下

其中表示UWB，IMU和雷达的测量残差。是测量误差的协方差矩阵。##### 6.1 IMU预积分因子 当我们获得IMU的加速度和角速度以及Bias的名义值以后，IMU预积分结果可以通过以下式子得到 

名义值和真实值以及误差值的关系如下

其中表示位移误差，速度误差，姿态误差。表示预积分的结果关于Bias的雅克比。

 6.2 IMU预积分残差

 6.3 雷达特征因子 在时刻时，我们有一系列的特征点云，首先将其合并到局部子图中去。

6.4 UWB测量因子 每个时间段，我们可以一系列UWB的测量数据集合，其具体形式如下：

其中表示距离测量值，，表示UWB标签的位置坐标。表示载体系下的距离节点。如果我们假设载体在时间段内的速度和姿态是以恒定的速率变化。那么在时刻从标签到UWB距离节点被定义为如下形式

我们考虑在时刻的距离测量值作为一个向量的范数，其受高斯噪声的影响，如。综上，UWB距离因子被定义如下

实验部分

图4 飞行测试中的试验设备

图5 不同算法雷达水平运动的轨迹示意图

图6 不同算法雷达水平运动姿态变化

表1 水平运动中的三次实验，不同算法的均方误差对比

表2 垂直运动中的三次实验，不同算法的均方误差对比

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~