LIO_SAM程序实现原理学习笔记（一）

目录

一、前言

二、前期标定

IMU标定

IMU与激光雷达的联合标定

三、程序结构

---

一、前言

经过前文的流程步骤之后，配置使用LIo_sam建立一个点云地图应该是没什么问题了，接下来还是需要对涉及到的程序进行原理上包括代码上的学习，毕竟单纯跑通一个程序对于自己的提升实在是有限，介于自身水平有限，下文我将尽量以通俗一点的方式记录下我的学习过程，其中可能可能存在着不少错误。

二、前期标定

首先还是对于前期硬件配置时对IMU以及IMU与激光雷达的联合标定所采用的标定方法进行了原理方面的学习，由于时间有限，标定并不是我学习的重点，所以对该部分的大体了解了原理流程方面，对于其中一些比较复杂的，需要花较多时间去学习或者使用比较复杂的数学公式等部分没有进行深入研究。

IMU标定

我在前期对IMU标定采用的是港科大的imu_utils标定工具，使用Allan方差标定高斯白噪声和随机游走噪声。imu的误差种类可大致分为两类:确定性误差和随机误差。其中确定性误差主要有零偏误差、尺度误差、安装误差和温度误差。随机性误差主要有测量噪声和随机游走噪声。所谓零偏误差是指IMU在静止状态的时候仍然会有一些测量输出，在应用时要减去这个量；尺度误差是指传感器的真实值与测量值之间有一个比例关系；而安装误差是指IMU器件在生产过程中可能会造成的加速度计和陀螺仪坐标轴的不正交以及坐标轴不重合所产生的误差量。在确定性误差中，主要以零偏和尺度误差为主。

在前文配置Lio_sam的yaml文件中我们可以注意到算法需要imuAccNoise、imuGyrNoise、imuAccBiasN与imuGyrBiasN四个参数。他们是加速度计和陀螺仪的噪声与偏差。

对于确定性的偏差方法原理，主要参考了下面这篇文章：

https://zhuanlan.zhihu.com/p/523610221

在文章中可以比较清楚的明白加速度计与陀螺仪的标定原理，其中许多公式可以在slam十四讲中有公式的证明和给出。我将原理用比较通俗的方式叙述一下，具体的数学公式可以参考上文。

首先在IMU的数据输出值与实际值是不同的，他们之间的包含着各种各样的误差，每种误差都考虑的话是不现实且极难计算的。因此我们应该抓住主要矛盾，将误差中占比较大的误差表达出来且未知数尽可能的少。因此选择尺度误差和零偏误差；两种误差建立模型。

首先对于加速度计：

公式左边为imu输出值，因为有xyz轴三个方向的数据，故记作向量的形式。式子右边对应着尺度误差，理论真值和最后的零偏。在这里尺度误差需要化为3x3矩阵与真值相乘做缩放，但零偏只需要加减做一个偏移即可矫正。这个与坐标变换中的旋转矩阵和平移向量的形式颇为相似。

在有了误差模型之后开始进行标定，从原理上来说是不难的，就是像解方程一样把式子中右侧真值解出来即可，首先将零偏误差移到式子左侧，再将尺度误差的逆左乘在式子左侧。最后将式子化为：

因为尺寸误差的逆矩阵因为是对角阵，矩阵整体的逆可以化为每个对角元素的逆写到矩阵内部，这里为了符号简洁（其实是写文电脑没有好用的公式编辑器）将每个对角元素的逆看为一个整体，仍然记回原来的符号即为上式。如果不好理解的话，不妨将上式的S右上角重新加上逆的标记。

关于求解这个问题，就不展开叙述了，详细原理可以参考引用的文章。其实原理就是利用的静止时的测量值的三个方向加速度的分量是与重力加速度相等。我们可以利用重力加速度构造一个等式。由于右侧包含测量值是不能完全等于等式对侧的重力加速的值的，因此解这个公式变成了一个最优化问题。

至于陀螺仪的标定，它与加速度的误差模型是差不多的。唯一的问题在于标定他的时候不能采用加速度计的办法。它的零偏可以采用静止一段时间取平均值的办法进行标定。而对于尺度误差标定。需要用到加速度计的数值辅助标定。这个时候找出加速度与角速度之间的变化关系即可进行标定，当然了，因为基于加速度计标定，所以需要用到的加速度计数值是需要视为真实值的，这点需要注意。

而噪音的数据求取原理参考了很多网上的博客，粗略了解了Allan方差采取样本的方法，具体原理也是有些云里雾里，感觉对于这个方法学习起来缺少一些必要的数学知识，所以这部分的原理分析暂时略过。

IMU与激光雷达的联合标定

第二部分的标定就是激光雷达与IMU的联合标定，在网上找了很久，大部分文章都是在讲如何使用瑞士苏黎世联邦理工大学自动驾驶实验室开发标定工具的使用方法，没有找到关于其原理与代码的分析。在这里我简单记录下我的理解，仅供参考。

在两个设备单独使用时都会有其自己独立的坐标系，每种设备单独使用时倒是问题不大，可如果两个设备协同工作时各自使用各自的坐标系肯定会有问题。所以需要知道两个坐标系间的相对位置以便于数据在不同坐标系间的传递，标定的结果也是两个坐标系间的旋转矩阵与平移向量。因此其原理可能与机械臂的手眼标定类似，但因为激光雷达的信息格式，所以具体的实施方式有所不同。

下面是从古月居摘录的实现步骤（https://www.guyuehome.com/35393）：

        首先$(RT)^{-1}$是imu初始坐标系，imu做积分可获得任一时刻相对初始位姿的位姿变换矩阵C,则$(RT)^{-1}C(RT)$表示此时刻激光雷达原点位姿在世界坐标系中的表示，假设某一点在激光雷达坐标系下的表示为$S$，则其在世界坐标系下的绝对坐标为$(RT)^{-1}C(RT)S$。

        点云扫描前后2帧有很大一部分点云重合，利用点云配准算法，可以估计前后2帧之间的移动转动，从而将每2帧之间的变换$C_i$估计出来,$C_iS$表示$i$点在世界绝对坐标系下的坐标。

        我们使用ICP最近邻迭代算法将2种表示方法描述的同一片点云区域配准，构建并优化最近邻误差，这里不直接对获得的laser坐标系构建优化目标，而对坐标系内的点云构建优化目标，运用了统计误差平均效应，直至收敛，获得RT变换矩阵的估计值，从而获得激光雷达与imu的外参变换矩阵

三、程序结构

LIO_SAM的程序结构还是比较简洁的。总体来说程序的主要文件只有六个。

首先是头文件：utility.h

然后是一个参数文件：params.yaml

最后是四个cpp文件：  feastureExtraction.cpp

                                        imageProjection.cpp

                                        imuPreintegration.cpp

                                        mapOptmization.cpp

头文件除了include了很多其他头文件，定义了许多变量，还设置了一些参数服务器的参数，定义了几个函数。

参数文件记录了许多参数的初值，对于数据来源的话题名和保存数据的开关等都在此定义。

四个cpp文件根据文件名其对应的结点功能也能看个七七八八。

                feastureExtraction.cpp（特征提取）

                imageProjection.cpp（图像映射）

                imuPreintegration.cpp（imu预积分）

                mapOptmization.cpp （建图优化）

全部的文件结构如下所示：

（在这里插一句，非常建议使用tree命令或者慢慢点开查看一下新程序的结构，我就是因为拿到新程序懒得查看结构，多花了不少时间。比如说config文件夹下的doc文件夹里面，里面就存放了作者的测试平台照片，以及我花了不少时间找了好久的论文和程序流程图！！！）

 文件结构了解的差不多之后我们看一下官方给的程序流程图：

在这里我再描述下上文的流程图，毕竟相信和我一样不那么喜欢阅读英文流程图的人不在少数。

        首先程序需要最少需要IMU与激光雷达点云数据两个数据源（GPS为可选数据源），原始的IMU数据流入imuPreintegration与imageProjection，也就是IMU预积分和图像映射。原始的点云数据流入图像映射环节。

        imu预积分环节订阅imu原始数据与雷达里程计数据，发布IMU里程计数据，主要实现因子图优化和预估IMU的bias功能。该环节对外以IMU原始消息频率发布里程计信息。

        图像映射环节订阅了点云原始信息、imu原始信息和imu预积分环节发布的IMU里程计信息，发布点云消息。主要的功能是获得初始变化估计、组织点云以及矫正点云的功能。

        特征提取环节只订阅葱花图像映射环节发布的点云消息，发布另外一组二次处理点云信息，主要        功能是提取角点与面点。

        最后的建图优化环节订阅由特征提取环节发布的二次处理的点云信息以及原始GPS信息，发布雷达里程计消息。主要功能是匹配点云，计算雷达里程计以及图优化。

到目前为止已经对程序的大致流程有了比较直观的认识，那么接下来就开始研究每个环节是如何利用代码实现的。

参考文章：https://blog.youkuaiyun.com/qq_32761549/article/details/126140151