Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一)

article/2025/7/22 20:56:31

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一)

  • 1. LiDAR inertial odometry and mapping简介
  • 2. Tightly coupled LiDAR inertial odometry
    • 2.1 LiDAR-IMU odometry overview
    • 2.2 IMU and pre-integration
    • 2.3 De-skewing and feature extraction
    • 2.4 Relative lidar measurements

1. LiDAR inertial odometry and mapping简介

在利用3D LiDAR进行SLAM计算时,有时会遇到一些问题,例如两帧之间的运动较为剧烈(大转角)可能会导致点云配准算法失效,在某些场景如长廊场景下有效测量点云较少使运动估计精度下降等,这些现象影响了SLAM算法的鲁棒性。而IMU(Inertial Measurement Unit)作为一种惯性测量单元,可以精确地测量载体的三轴加速度以及三轴角速度,在上述的场景中(大转角,长廊环境)可以为LiDAR提供额外的先验信息,从而增强LiDAR算法的鲁棒性,因此LiDAR-IMU fusioin成为一种近年来备受关注的SLAM方法。事实上,camera-IMU融合已经有一些开源的SLAM算法,例如VINS Mono。在ICRA 2019 上,终于迎来了第一个开源的LiDAR-IMU SLAM算法LIOM(Lidar-Inertial Odometry and mapping),由香港科技大学的刘明老师及学生共同完成,其主要贡献如下:

  • 提出了一种紧耦合的LiDAR-IMU里程计算法,可实现实时、高精度以及高更新率(High update rate)的里程计计算
  • 利用LiDAR-IMU里程计的先验信息,基于旋转约束的优化方法进一步优化位姿和点云地图,以生成全局一致、鲁邦的地图和位姿估计
  • 进行了大量的室内外实验测试,实验结果优于LiDAR-only或LiDAR-IMU松耦合方法
  • 第一个开源的LiDAR-IMU紧耦合SLAM算法(LIO-Mapping)

2. Tightly coupled LiDAR inertial odometry

2.1 LiDAR-IMU odometry overview

整个LiDAR-IMU里程计的算法流程如下图所示:
在这里插入图片描述
为了更好的理解LIO算法,我们一起看下面这幅时序图:
在这里插入图片描述
假设当前的时间戳是 i i i,则由于一般IMU设备的频率(100-1000Hz)会远远高于LiDAR点云数据的更新频率(10Hz),因此在下一帧点云即时间戳 j j j到来之前,会有大量的IMU数据 τ i , j \tau_{i,j} τi,j读入,如上图中紫色线条所示。则可以根据这段时间内IMU的数据对IMU的状态进行估计,即流程图中的state-prediction部分,同时利用这段时间间隔内的IMU测量量进行预积分操作,即对应流程图中的Pre-integration。当到达时间戳 j j j也即新的激光雷达数据帧过来后,由于激光雷达为连续测量,即在时间戳 i i i到时间戳 j j j过程中,激光雷达的测量是在不断运动且位姿不断变化的过程中测量得到的,因此会产生一定的运动畸变(motion distortion)。此时,在获得时间戳 j j j对应的激光点云数据 S j S_j Sj后,应首先根据IMU的state-prediction消除运动畸变即流程图中的De-skewing部分从而得到未畸变的点云 S ‾ j \overline{S}_j Sj
在得到畸变矫正后的点云 S ‾ j \overline{S}_j Sj后,为减少计算量,采用了LOAM中的方法从点云提取了两种特征 F L j F_{L_j} FLj,分别是角点以及平面特征点,即对应流程图中的Feature Extraction部分。然后就是根据局部地图 M L o , i L p M_{L_{o,i}}^{L_p} MLo,iLp,确定特征与局部地图的对应关系,也即流程图中的Find relative lidar measurements部分。所谓的局部地图,也就是将上图所示的从时间戳 o o o到时间戳 i i i的特征点都投影到一个坐标系下形成一个点云局部地图,在本文中,每次都是投影到中间的时间戳即 p p p对应的坐标系下,即对应流程图中的Local map management。最后在确定了测量约束和IMU的预积分约束后,建立非线性最小二乘的目标函数并进行联合优化求解,即对应流程图中的Joint non-linear optimization部分。接下来,我们对LIO中的每一个子部分进行详细解析。

2.2 IMU and pre-integration

一般来讲,一个三轴正交的IMU设备可以测量三轴的加速度以及角速度,由牛顿运动定律,若初始状态(位置,姿态,速度)已知,则可以根据加速度以及角速度的测量跟踪一个物体的状态(位置,姿态,速度),因此广泛的应用于惯性导航(Inertial Navigation System)中。在涉及到IMU的导航系统中,通常IMU的状态可以建模为如下向量:
X B i W = [ p B i W T , v B i W T , q B i W T , b a i T , b ω i T ] T X_{B_i}^{W}=[{p_{B_i}^W}^T,{v_{B_i}^W}^T,{q_{B_i}^W}^T,b_{a_i}^T,b_{\omega_i}^T]^T XBiW=[pBiWT,vBiWT,qBiWT,baiT,bωiT]T
其中 p B i W T {p_{B_i}^W}^T pBiWT表示的是IMU在 i i i时刻在世界坐标系 W W W下的位置, v B i W T {v_{B_i}^W}^T vBiWT表示的是IMU在 i i i时刻在世界坐标系 W W W下的速度,而 q B i W T {q_{B_i}^W}^T qBiWT表示的是IMU在 i i i时刻在世界坐标系 W W W下的姿态(四元数),最后两个 b a i T , b ω i T b_{a_i}^T,b_{\omega_i}^T baiT,bωiT则是IMU原始测量数据即加速度以及角速度测量的偏置,可以理解为一种IMU原始测量数据的误差建模。在本文中,由于涉及到LiDAR-IMU融合,LiDAR与IMU之间为刚性链接且未先前标定,因此状态向量除了IMU状态向量 X B i W X_{B_i}^{W} XBiW外,还要估计LiDAR和IMU之间的相对位姿关系,即:
T B L = [ p B L , q B L ] T T_B^L=[p_B^L,q_B^L]^T TBL=[pBL,qBL]T
根据高中学过的牛顿运动定律 x = x 0 + v 0 t + 1 2 a t 2 , v = v 0 + a t x=x_0+v_0t+\frac{1}{2}at^2,v=v_0+at x=x0+v0t+21at2,v=v0+at,我们可以根据IMU的原始测量数据以及前一时刻 i i i,IMU的状态,对当前时刻 j j j,IMU的状态进行更新,即:
p B j W = p B i W + ∑ k = i j − 1 ( v k Δ t + 1 2 ( g W + R k ( a ^ k − b a k ) ) Δ t 2 ) v B j W = v B i W + ∑ k = i j − 1 ( g W + R k ( a ^ k − b a k ) ) Δ t q B j W = q B i W ∏ k = i j − 1 δ q k p_{B_j}^W=p_{B_i}^W+\sum_{k=i}^{j-1}(v_k\Delta t+\frac{1}{2}(g^W+R_k(\hat{a}_k-b_{a_k}))\Delta t^2)\\ v_{B_j}^W=v_{B_i}^W+\sum_{k=i}^{j-1}(g^W+R_k(\hat{a}_k-b_{a_k}))\Delta t\\ q_{B_j}^W=q_{B_i}^W\prod_{k=i}^{j-1}\delta q_k pBjW=pBiW+k=ij1(vkΔt+21(gW+Rk(a^kbak))Δt2)vBjW=vBiW+k=ij1(gW+Rk(a^kbak))ΔtqBjW=qBiWk=ij1δqk
因此,时间戳 i i i到时间戳 j j j内IMU的运动可以利用预积分建模为:
z i , j = { Δ p i j , Δ v i j , Δ q i j } z_{i,j}=\{\Delta p_{ij},\Delta v_{ij},\Delta q_{ij}\} zi,j={Δpij,Δvij,Δqij}

2.3 De-skewing and feature extraction

这一部分主要实现点云畸变矫正以及从畸变矫正的点云中提取特征,主要借鉴了LOAM的畸变矫正以及特征提取方法,因此在这里仅简单描述一下畸变矫正以及特征提取的思路,细节还请阅读LOAM源码。
我们都知道,激光雷达的测量与摄像头的测量不太相同。摄像头的拍照可以理解为one-shot,即快门曝光的时刻。而激光雷达的测量则是连续测量,也即每一帧点云中的不同点其实都是在不同时刻测量得到的。而车载的激光雷达又都是在不断运动的,这就导致该帧点云中的每一个点其实都是在不同的时刻测量得到的。因此需要根据IMU的加速度以及角速度信息(插值等方法)预测每一个激光扫描点测量时,该点对应的激光雷达的位置。具体这部分具体实现也可以参考LOAM源码。

2.4 Relative lidar measurements

这部分非常有趣,刚看这篇论文时十分困惑,我去relative lidar measurements,以前没听过啊我靠,好高端,得好好研究研究,甚至去逐行看了源码(源码质量不错)。研究了一番后,发现其实也不过是为了给优化函数添加约束建立了一种特征点到局部地图的关联罢了,其实从这个角度看还是蛮好理解的。
如下图所示,这篇论文采用了滑动窗口(sliding window)的方法,图中B表示的是IMU坐标系,L表示的是激光雷达坐标系,在运动过程中假设两者之间的相对位姿关系不发生变化(刚性连接)。蓝色的大窗口即是论文中说的sliding window,在源码中该window设置为包含了15个激光雷达帧大小的窗口,其中从 o o o p p p为之前优化过的帧,而从 p p p i i i对应的帧为待优化的帧。
在这里插入图片描述
为了建立特征点到局部地图的关联关系,当然应该首先建立局部地图对吧。有的同学可能会问了,直接进行点云帧之间的关联不就可以了吗,这样不就是一个简答的激光里程计吗?问题在于啊,一帧的点云通常还是比较稀疏的,尤其是线数比较低的点云,建立稠密局部地图可以使得特征之间的匹配更为准确。现在假设我们根据2.3节中所述LOAM的特征提取方法,分别提取出来了每一帧的平面特征点,但是这些特征点都是定义在自己的局部激光雷达坐标系 O L i O_{L_i} OLi内的哦。因此为了建立坐标系统一的局部地图,我们首先应该要找一个基准,然后大家把自己的点云都投影到这个基准下面。在论文中,这个基准就是 p p p时刻对应的雷达坐标系 O L p O_{L_p} OLp。接下来的问题就是怎么投影了,其实就是一些简单的坐标变化关系,我们一起来看一下。

现在已知 i i i时刻和 p p p时刻IMU坐标系到世界坐标系的位姿变化关系即 T B i W T_{B_i}^{W} TBiW T B p W T_{B_p}^{W} TBpW,且已知激光雷达到IMU坐标系之间的变换关系 T L B T_L^B TLB(外参:待标定),则根据坐标变换的链式法则,可以得到 i i i时刻激光雷达坐标系到 p p p时刻激光雷达坐标系之间的变换关系为:
T L P = T L B T B i W T B p W − 1 T L B − 1 T_L^P=T_L^BT_{B_i}^W{T_{B_p}^W}^{-1}{T_L^B}^{-1} TLP=TLBTBiWTBpW1TLB1
在建立完局部地图(Local map)后,就要寻找特征到局部地图的对应关系了。对于待优化窗口(红色)内的某一帧,我们可以根据2.2节中IMU的预积分得到该帧的IMU位姿,然后就可以根据上式的坐标变换关系将该帧内的特征点 F l F_l Fl也投影到 p p p对应的激光雷达坐标系内。对于 F l F_l Fl内的每一个平面特征点,我们需要确定其与特征地图的对应关系。也就是说,如果所有测量都没有误差,由于局部地图是由平面特征点构成的,则 F l F_l Fl中的每一个特征点都应该落在局部地图的平面上。然而,实际情况是IMU和雷达的测量都会存在误差,导致预估的位姿不准确,同时激光雷达与IMU的相对位姿关系也存在误差,因此该特征点不是严格的落在局部地图的平面上。因此我们可以建立点到面的对应关系,首先在局部地图内搜索5个紧邻点,然后这5个近邻点可以拟合出一个平面,这样就可以用点到面的距离来作为约束啦。看起来比较简单,不过实现的时候还是有很多细节的,将在后面的源码博客中一一介绍,目前这篇博客先专注于LiDAR-IMU紧耦合的原理。
我们再回头看2.1节中的算法流程图,似乎只有联合优化(Joint non-linear optimization)没有讲了,考虑到这部分比较重要,且这篇博客篇幅有点长,将在下一篇博客中介绍。


http://chatgpt.dhexx.cn/article/LSL4P2IA.shtml

相关文章

SLAM——入门到放弃:ROS里程计(odometry)

ROS gmapping导航包,要求有2 个 输入,一个是激光数据,另一个就是里程计信息。 里程计又包含2 个方面的信息: 位姿(位置和转角),即(x,y,θ)是速度(前进速度和…

Ros Odometry获取机器人位置-python 代码打印

纪念一下,辛苦了一天多,终于找出来了,经过网上的资料反向推出结构。 博主需要通过机器人的实时位置来进行计算。从网上查阅得知nav_msgs/Odometry消息中存储机器人的位置和速度估计等。 下面展示一下操作流程: 1.首先通过 ros…

ROS里程计消息nav_msgs/Odometry的可视化方法

ROS中里程计的消息类型为nav_msgs/Odometry,该消息类型具有以下结构: 可以看到,里程计消息中的pose包含了位置pose.position和姿态pose.orientation 在ROS中,有一种常用消息类型为nav_msgs/Path,可视化的方法为&#…

基于rf2o_laser_odometry纯激光里程计的gmapping建图

ROS环境:ubuntu16.04 & ROS kinetic激光雷达:EAI-X4 or RPlidar-A1激光里程计:rf2o_laser_odometry建图:gmapping 对于很多刚入门的同学,购买一台带有高精度轮式里程计的ROS小车经济上往往不允许。但是大多数同学…

nav_msgs/Odometry消息的发布和tf变换

一。ROS使用tf来决定机器人的位置和静态地图中的传感器数据,但是tf中没有机器人的速度信息,所以导航功能包要求机器人 能够通过里程计信息源发布包含速度信息的里程计nav_msgs/Odometry 消息。 本篇将介绍nav_msgs/Odometry消息,并且通过代码…

【视觉SLAM】DM-VIO: Delayed Marginalization Visual-Inertial Odometry

L. v. Stumberg and D. Cremers, “DM-VIO: Delayed Marginalization Visual-Inertial Odometry,” in IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1408-1415, April 2022, doi: 10.1109/LRA.2021.3140129. 论文阅读方法:Title,Abstract…

DSO(Direct Sparse Odometry)

DSO(Direct Sparse Odometry) 文章目录 1. 简述2. 概述3. 框架流程3.1 代码框架与数据表示3.2 VO流程 4. DSO详细介绍4.1 残差的构成与雅可比4.2 滑动窗口的维护与边缘化4.3 零空间,FEJ4.4 其他零散的模块和算法 5. 光度标定6. 评述7. 资料与…

SVO(SVO: fast semi-direct monocular visual odometry)

SVO2系列之深度滤波DepthFiltersvo_noteSVO(SVO: fast semi-direct monocular visual odometry)SVO 半直接视觉里程计【DepthFilter】深度滤波器【svopro】代码梳理 SVO(SVO: fast semi-direct monocular visual odometry)翻译 1、…

航迹推演

​ 做机器人底层程序的时候,经常用到航迹推演(Odometry),无论是定位导航还是普通的方向控制。航迹推演中除了对机器人位姿进行估计,另一个很重要的关系是移动机器人前进速度、转向角速度与左轮速度、右轮速度之间的转换…

航迹推演(Odometry)

做机器人底层程序的时候,经常用到航迹推演(Odometry),无论是定位导航还是普通的方向控制。航迹推演中除了对机器人位姿进行估计,另一个很重要的关系是移动机器人前进速度、转向角速度与左轮速度、右轮速度之间的转换。…

大数据基础编程+实验

本文仅用于分析和记录在校期间学习大数据分析课程的一点心得体会。 1、Ubuntu系统的安装和使用 本文采用Ubuntu16.04系统,安装系统省略,选择镜像后一直点击下一步 1.1 进入系统后,调整输入法,将输入法切换至中英文切换 1.2为…

Python数据库编程pymysql

Python数据库编程pymysql 一、数据库编程介绍 数据库编程就是针对数据库的操作,通过编写程序的方式,让程序做为数据库的客户端进行数据库操作。 对于MySQL的操作我们可以通过SQL语句,但是有很多情况下我们需要写入MySQL的数据非常多&#…

过程化SQL数据库编程

一、过程化SQL的块结构 基本的SQL是高度非过程化的语言。嵌入式SQL将SQL语句嵌入程序设计语言,借助高级语言的控制功能实现过程化。过程化SQL是对SQL的扩展,使其增加了过程化语句功能。 过程化SQL程序的基本结构是块。所有的过程化SQL程序都是由块组成的…

Java的数据库编程:JDBC

目录 一、JDBC是什么? 二、使用步骤 1.首先将JDBC的包引进java中 2.创建新的类来写代码 3.描述你的服务器 4.设置你的数据库地址,数据库用户名,数据库密码 5.连接数据库 6.书写你所要执行的SQL语句 7.把字符串风格的sql转化成一个对象 8.执行语句 9.回收资…

Python数据库编程

操作SQLite3数据库 从Python3.x版本开始,在标准库中已经内置了SQLlite3模块,它可以支持SQLite3数据库的访问和相关的数据库操作。在需要操作SQLite3数据库数据时,只须在程序中导入SQLite3模块即可。Python语言操作SQLite3数据库的基本流程如…

实验7 数据库编程

第1关 定义一个名为PROC_COUNT的无参数存储过程 任务描述 定义一个名为PROC_COUNT的无参数存储过程,查询工程名称中含有“厂”字的工程数量,并调用该存储过程。 相关知识 1、工程项目表J由工程项目代码(JNO)、工程项目名(JNAME)、工程项目所在城市(CITY)…

C++数据库编程简介

C数据库编程简介 C数据库编程 ODBC简介 C数据库编程 ODBC连接SQL Server数据库 C数据库编程 ODBC插入数据 C数据库编程 ODBC查询数据 C数据库编程 ODBC删除数据 C数据库编程 ODBC修改数据 C数据库编程 ODBC连接MySQL增删改查数据 C数据库编程 MySQL Connecttor C简介 …

游戏设计模式——面向数据编程(转)

作者:KillerAery 出处:http://www.cnblogs.com/KillerAery/ 随着软件需求的日益复杂发展,远古时期面的向过程编程思想才渐渐萌生了面向对象编程思想。 当人们发现面向对象在应对高层软件的种种好处时,越来越沉醉于面向对象&…

数据科学必备Python编程基础

前言 对于Python复杂的编程语言中提取了数据分析常用的数据处理以及数据可视化等数据分析师常用的内容,区别与其他的Python编程教程,如果是纯开发的小伙伴,看完本系列的文章仅仅只能掌握数据相关处理的内容,并不能完全掌握开发方…

数据结构中的C语言编程基础

​ 在学习数据结构时,需要我们编写许多的程序,对于一些变量的定义、结构体的声明、指针的使用,需要有一个统一的标准,这样才能方便我们使用、并简化记忆难度。 ​ 本文结合自身的编程经验和高分笔记中对考研数据结构编程的一些建…