月照银海似蛟龙的博客

边包括预积分观测边，两个时刻的GNSS观测边，前一个时刻的先验边，两个零偏随机游走边。最后利用前面定义的图优化边，来实现一个类似于ESKF的GNSS惯性导航融合定位。收到GNSS一帧数据，如果是第一帧则先用该帧gnss数据赋值当前帧位姿。在收到一帧 gnss数据 和 imu数据 后，才能进行预积分的累积。设置当前时刻顶点 分别为pose v bg ba。不是第一帧了则先将imu预积分积分至当前时间。GINS中实际使用的图优化结构如下图。收到新的IMU数据后的处理函数。创建g2o图优化的优化器。

15维或18维状态变量应该对应到图优化的顶点。使用广义加法来实现矩阵流形与切空间上的操作。下面使用散装的形式，每个状态被分为位姿、速度、陀螺仪零偏、加速度零偏四种顶点。函数设置为虚函数，后面陀螺仪和加速度的零偏顶点，与速度都属于R3。相比滤波器框架，图优化框架要复杂些，但使用更加灵活。下面是陀螺仪零偏和加速度计零偏的顶点，从速度顶点继承。旋转在前 SO3 位移在后 ，6自由度。接下来实现预积分相关的图优化。需要对旋转部分做流形态的加法。位移部分则是正常的加法。

需要注意的是，如果不进行优化，则预积分和直接积分的效果是完全一致的，都是将IMU数据一次性地积分。除此之外，也可以将IMU的读数记录在预积分类中（当然，也可以不记录，因为都已经积分过了）。与ESKF不同的是，预积分可以对多个IMU数据进行预测，可以从任意起始时刻向后预测，而ESKF通常只在当前状态下，针对单个IMU数据，向下一时刻预测。的构成就是陀螺仪和加计的噪声构成的对角矩阵，在构造函数中构成的。计算A矩阵中对应的各个块，分别对应公式如下，A矩阵中的。下面来看单个IMU的积分函数，首先在类中进行声明。

预积分：是一种十分常见的IMU数据处理方法。与传统的IMU运动学积分不同，预积分可以将一段时间内的IMU测量数据累积，建立预积分测量，同时还能保证测量值与状态变量无关。如果以吃饭来比喻的话，ESKF像是一口口地吃菜，而预积分则是从锅里先把菜一块块夹到碗里，再把碗里的菜一口气吃掉。无论是LIO系统还是VIO系统，预积分已经称为诸多紧耦合IMU系统的标准方法。在ESKF中，将两个GNSS观测之间的IMU数据进行积分，作为ESKF的预测过程。

IMU&GNSS的误差状态卡尔曼滤波器（ESKF）的后续处理)

在传统的EKF中，可以直观地对观测方程线性化，求出观测方程相对与状态变量的雅克比矩阵，进而更新卡尔曼滤波器。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的h,那么可以写为。其它几项都是平凡的，只有旋转部分，因为 delat theat 定义为R的右乘，用右乘的BCH即可。下面就是如何计算H，大部分的观测数据是对名义状态的观测。其中，K为卡尔曼增益，Ppred为预测的协方差矩阵，最后的P为修正后的协方差矩阵。其中z为观测数据，v为观测噪声，V为该噪声的协方差矩阵。

至此，完成了在ESKF中进行IMU递推的过程，对应卡尔曼滤波器中的状态方程。为了让滤波器收敛，需要外部的观测对卡尔曼滤波器进行修正，也是所谓的组合导航。当然组合导航的方法很多，从传统的EKF，到本节介绍的ESKF，以及预积分和图优化都可以用于组合导航中。连续时间的噪声项可以视为随机过程的能量谱密度，而离散时间下的噪声 变量就是我们日常看到的随机变量。其中前两式的 delta t 是由于积分关系导致的，后两式则和 离散时间下的 零偏游走的标准差公式 一致。其中w为噪声，按照前面的定义，Q应该为。

F中需要计算旋转矩阵R相对于某个扰动的导数，而在不引入张量的情况下，是无法表达矩阵对向量导数的形式的。不过这样的矩阵形式将含有很多的零项，相比上式并不会有明显简化，所以先使用这种散开的公式。滤波器仅需考虑误差状态如何运动，如何观测，最后如何滤波，和名义状态的关系不大。为更新量，应是位于切空间中的矢量，中间的加法应为流形与切空间指数映射的广义加法。不光是平移和旋转，把所有的状态都用误差状态来表达，这就是典型ESKF的做法。把噪声的处理放到误差状态变量中，可以认为名义状态变量的方程是不含噪声的。

最后讨论预积分相对状态变量的雅克比矩阵。由于预积分测量已经归纳了IMU在短时间内的读数，因此残差相对于状态变量的雅克比矩阵推导则简单。接下来，考虑速度项的雅克比矩阵。速度项与vi,vj呈线性关系，可以得到。由于速度的残差项与它只相差一个负号，所以只需要在下式中加个负号。然后，它们的关系大多为线性关系，雅克比矩阵可以得出。，在某一步迭代时，当前估计出来的零偏修正为。，而当前修正得到的预积分旋转观测量为。为了求导，又在上面两项的基础上加上了。零偏的残差在下面基础上加负号。假设优化初始的零偏为。

因此，如果把预积分残差看作同一个，那么它与状态顶点关联应该如下图所示，除了预积分因子本身，还需要约束IMU的随机游走，因此在IMU的不同时刻，零偏会存在一个约束因子。前面定义了预积分的测量模型，推导了它的噪声模型和协方差矩阵，并说明了随着零偏量的更新，预积分应该怎么更新。在IMU相关的应用中，通常把每个时刻的状态建模为包含旋转、平移、线速度、IMU零偏的变量，构成状态变量集合。在优化过程中，如果对i时刻的零偏进行更新，那么预积分观测量也应该线性地发生改变，从而影响残差项的取值。,但它们显然是和残差相关的。

整理上面的雅克比矩阵，得到更整齐的结果由于后面四种雅克比矩阵本身就是累加的，总结成递推形式。

上面推导了，如何从j-1时刻将噪声项递推至j时刻。也可以将上面格式整理成矩阵形式。将三个噪声项合并成同一个：并且把IMU的零偏噪声定义为那么从ηij−1ηij−1​至ηijηij​的递推式可以写作：其中，系数矩阵Aj−1Bj−1Aj−1​Bj−1​为矩阵形式更清晰地显示了几个噪声项之间累积的递推关系。

通过上面的推导，定义了预积分的三个观测量和它们的噪声。将它们代回最初的定义式，可以简单写为从公式中可以看到预积分的几大优点。它的左侧是可以通过传感器数据积分得到的观测量，右侧是根据状态变量推断出来的预测值，再加上（或乘上）一个随机噪声。左侧变量的定义方式非常适合程序实现。一方面，△Rik△Rik​可以通过IMU的读数得到，△vik△vik​可以由k时刻IMU的读数加上△Rik△Rik​算得，而△pik△p​ik​。

旋转R平移p角速度w线速度v加速度a这些变量的运动学关系可以写成：在t到tΔttΔt时间内，对上式进行欧拉积分，可得：其中，角速度和加速度可以被IMU测量到，但受噪声与重力影响。令测量值为ωω和a\tilde{a}a，则有噪声与零偏与重力的测量公式如下：其中bgb_{g}bg​和bab_{a}ba​为陀螺仪和加速度零偏ηg\eta_{g}ηg​和ηa\eta_{a}ηa​为测量的高斯噪声用测量值表示t到tΔ。

全球卫星导航系统(Global Navigation Satelite System,GNSS)，简称卫星导航，是室外机器人定位的一个主要信息来源。GNSS定位原理：GNSS通过测量自身与地球周围各卫星的距离来确定自身位置，与卫星的距离主要通过测量时间间隔来确定。一个卫星信号从卫星上发出时，带有一个发送时间,而GNSS接收机接收到它时，有一个接收时间，通过比较时间间隔，就能估算各卫星离我们的距离。GNSS本质上可以看成一种高精度的授时系统。

在进行GPS加IMU的组合导航或者Lidar加IMU的组合导航时，用EKF或者ESKF的滤波方法时，需要提前知道惯导的测量噪声、初始零偏、重力方向等信息。此时就需要对惯导进行一个初始化，来获取以上信息，常见的初始化方法为静止初始化法。例如无人机在上电后要进行自检，此时需要无人机静止一段时间，通过指示灯来提示自检是否完毕，在静止的过程中，则对惯导进行了初始化的方法。

下面是在imu数据一帧一帧进来，如何实现状态的积分，在使用的时候则是来一帧IMU数据，调用一次这个函数。与真实的IMU 噪声模型差别很大，用rotors的imu插件，可以尽可能模拟IMU噪声模型。在gazebo中的IMU插件对惯导的噪声描述很简单，仅有一个高斯噪声可以设置。声明一个用与 IMU积分的类 ，来实现 短时间内的航迹推算。更新时间，用于判断下一帧数据，在时间间隔上是否满足要求。所以需要在IMU的回调函数里面，赋值上面的结构体数据。然后将数据加入，在该函数内部，即完成航迹的推算。

随机过程可以从直观上理解，由于高斯过程的协方差随时间变得越来越大，IMU的测量值会随着采样时间变长而变得更加不准确，因此采样频率越高的IMU，其精度也会相对较高。若从高一级的系统层面来看，角速度就是角度的导数，加速度又是速度的导数，所以IMU的测量噪声，也可以解释为角度的随机游走和速度的随机游走。由于各种各样的原因，即使在车辆静止时，IMU的角速度和加速度输出也不一定形成均值为0的白噪声，而是带有一定的。这个偏移量是由IMU内部的机电测量装置导致的，有些IMU的偏移较小，也有的会比较大。

注意另一种情况：如果不把IMU放在车辆中心，当车辆方式旋转和平移时，IMU还应该测量到由车辆旋转导致的离心力、科氏力和角加速度，最后体现在加速度计的读数上。，但其实现手段非常多样，从低成本的MEMS（Micro-electromechanical Systems，微0087机电）惯性导航到昂贵的光纤陀螺，它们的作用都是精确的测量物体的惯性。它们的体积很小，安装在设备内部，假设IMU静止，由于物体加速度的测量实际上是通过测量受力情况得到的，这个IMU应该受到一个反向的支撑力，所以应该测到一个-g方向的重力。

览沃科技有限公司（Livox）成立于2016年。为了革新激光雷达行业，Livox致力于提供高性能、低成本的激光雷达传感器。通过降低使用门槛和生产成本，Livox将激光雷达技术集成到更多产品和应用之中，从而为自动驾驶、智慧城市、测绘、移动机器人等行业带来创新性改变。Livox产品已销往包括美国、加拿大、中国、日本和欧盟在内的 26 个国家和地区。面向智能移动机器人市场，Livox 推出最新一代 3D 激光雷达 Mid-360，开启混合固态激光雷达 360° 立体感知新篇章。

bit5 bit4 回波序号 : 00: 第 0 个回波 01: 第 1 个回波 10: 第 2 个回波 11: 第 3 个回波 由于Livox Avia 采用同轴光路，即使外部无被测物体，其内部的光学系统也会产生一个回波，该回波记为第 0个回波。随后，若激光出射方向存在可被探测的物体，则最先返回系统的激光回波记为第 1 个回波，随后为第 2个回波，以此类推。bit3 bit2 基于强度的点属性： 00：正常点 01：回波能量噪点置信度高 10：回波能量噪点置信度中 11：保留位。

Faster-LIO是基于FastLIO2开发的。FastLIO2是开源LIO中比较优秀的一个，前端用了增量的kdtree（ikd-tree），后端用了迭代ESKF（IEKF），流程短，计算快。Faster-LIO则把ikd-tree替换成了iVox，顺带优化了一些代码逻辑，实现了更快的LIO。在典型的32线激光雷达中可以取得100-200Hz左右的计算频率，在固态雷达中甚至可以达到1000-2000Hz，能够达到FastLIO2的1.5-2倍左右的速度。当然具体数值和计算平台相关。

livox mid360 在官网一直没有货，在gazebo里可以仿真该雷达形式的点云。但是其只发布雷达的数据，没有imu数据，实际的雷达是可以发布既有雷达也有imu的数据的运行 FAST-LIO2 也需要雷达和惯导的数据本篇博客在gazebo中搭建了一个有livox mid360 和惯导的平台，并成功运行了FAST-LIO2。

览沃科技有限公司（Livox）成立于2016年。为了革新激光雷达行业，Livox致力于提供高性能、低成本的激光雷达传感器。通过降低使用门槛和生产成本，Livox将激光雷达技术集成到更多产品和应用之中，从而为自动驾驶、智慧城市、测绘、移动机器人等行业带来创新性改变。Livox产品已销往包括美国、加拿大、中国、日本和欧盟在内的 26 个国家和地区。面向智能移动机器人市场，Livox 推出最新一代 3D 激光雷达 Mid-360，开启混合固态激光雷达 360° 立体感知新篇章。

览沃科技有限公司（Livox）成立于2016年。为了革新激光雷达行业，Livox致力于提供高性能、低成本的激光雷达传感器。通过降低使用门槛和生产成本，Livox将激光雷达技术集成到更多产品和应用之中，从而为自动驾驶、智慧城市、测绘、移动机器人等行业带来创新性改变。Livox产品已销往包括美国、加拿大、中国、日本和欧盟在内的 26 个国家和地区。面向智能移动机器人市场，Livox 推出最新一代 3D 激光雷达 Mid-360，开启混合固态激光雷达 360° 立体感知新篇章。

览沃科技有限公司（Livox）成立于2016年。为了革新激光雷达行业，Livox致力于提供高性能、低成本的激光雷达传感器。通过降低使用门槛和生产成本，Livox将激光雷达技术集成到更多产品和应用之中，从而为自动驾驶、智慧城市、测绘、移动机器人等行业带来创新性改变。Livox产品已销往包括美国、加拿大、中国、日本和欧盟在内的 26 个国家和地区。面向智能移动机器人市场，Livox 推出最新一代 3D 激光雷达 Mid-360，开启混合固态激光雷达 360° 立体感知新篇章。

LI-Init是一种鲁棒、实时的激光雷达惯性系统初始化方法。该方法可校准激光雷达与IMU之间的时间偏移量和外部参数，以及重力矢量和IMU偏差。我们的方法不需要任何目标或额外的传感器，特定的结构化环境，先前的环境点图或初始值的外在和时间偏移。

LI-Init是一种鲁棒、实时的激光雷达惯性系统初始化方法。该方法可校准激光雷达与IMU之间的时间偏移量和外部参数，以及重力矢量和IMU偏差。我们的方法不需要任何目标或额外的传感器，特定的结构化环境，先前的环境点图或初始值的外在和时间偏移。

Ceres Solver是谷歌开源的C++非线性优化库，能够解决有约束或无约束条件下的非线性最小二乘问题。2010年之后大量的运用在谷歌的产品开发中，尤其在谷歌开源的cartographer中被大量的使用。ceres可以在Linux，Windows，macOS，Andrioid，IOS系统进行安装使用ceres被命名是由于高斯使用了最小二乘方法成功的预测了绕行至太阳背后的小行星ceres的位置。本篇博客在 ubuntu 20.04 环境下安装 Ceres-solver-2.0.0。

在之前将ouster-32的激光雷达设置成了静态IP。因为设置了固定的雷达IP，所以就在launch文件中设置其参数，不用在用命令行的方式传入该参数修改内容如下：改为参数配置launch文件中有几个参数，可以看下，其原文件如下：ouster_ns 不用管，设置命名空间的sensor_hostname 已经改为了静态IPudp_dest 电脑的ip可以不填入，程序会自动获取lidar_port和imu_port 不用动udp_profile_lidar 雷达的数据格式，默认 的是LEGACY

此 ROS 软件包支持所有固件 v2.0 或更高版本的 Ouster 传感器。启动后，驱动程序将配置并连接到选定的传感器设备，连接后，驱动程序将处理传入的IMU和激光雷达数据包，解码激光雷达帧并发布有关/ouster/imu和/ouster/points主题的相应ROS消息。如果传感器支持双回波并且已配置为使用此功能，则将发布另一个名为 /ouster/points2 的主题，该主题对应于第二个点云。驱动支持Melodic 和 Noetic的ROS版本除了基础的ROS环境，驱动还需要的ROS功能包有：通过如

查看当前topic消息种类终端输出imu数据可以看到ouster 是带有内置imu的imu消息的名称是 /ouster/imu查看其消息类型：终端输出雷达数据查看点云数据类型终端输出查看点云标签其中一包数据为：所以在雷达数据中的标签有：在该驱动文件中，启动rviz后，默认有3个tf这3个方向是不一致的，其中os_lidar 坐标系os_sensor 坐标系驱动包默认的rviz显示其中包含了

在之前将ouster-32的激光雷达设置成了静态IP。因为设置了固定的雷达IP，所以就在launch文件中设置其参数，不用在用命令行的方式传入该参数修改内容如下：改为参数配置launch文件中有几个参数，可以看下，其原文件如下：ouster_ns 不用管，设置命名空间的sensor_hostname 已经改为了静态IPudp_dest 电脑的ip可以不填入，程序会自动获取lidar_port和imu_port 不用动udp_profile_lidar 雷达的数据格式，默认 的是LEGACY

此 ROS 软件包支持所有固件 v2.0 或更高版本的 Ouster 传感器。启动后，驱动程序将配置并连接到选定的传感器设备，连接后，驱动程序将处理传入的IMU和激光雷达数据包，解码激光雷达帧并发布有关/ouster/imu和/ouster/points主题的相应ROS消息。如果传感器支持双回波并且已配置为使用此功能，则将发布另一个名为 /ouster/points2 的主题，该主题对应于第二个点云。将“发布”指定为生成类型对于获得驱动程序的功能非常重要。在实际使用中需要将 < >改为’ ’

查询出雷达IPv4地址后，如果电脑IP与雷达IP不在同一个网段，此时电脑还不能连接上雷达（ping通雷达IP），这样的话无法通过电脑对雷达进行任何参数查询和设置。另外，如果新的IP与老IP不在一个网段内，电脑后续如果需要继续连接雷达，那还需要同样修改网卡的IP。注意：如果IP修改后发现IP网段与电脑无线网卡的IP也在一个网段里，那就会引起混淆，此时请暂时关闭无线网卡功能。因此需要下面的工作，将雷达的IP改为静态IP，即每次上电雷达的IP不变。注意修改雷达的IP后，需要再次配置电脑的IP为同一网段。

功能包名称github地址效果展示b站地址功能包简介：LiDAR_IMU_Init 是一个鲁棒的、实时的激光雷达与惯导系统的初始化方法。所提出的方法校准了 LiDAR 和 IMU 之间的时间偏移和外部参数，以及重力矢量和 IMU 偏差。该方法不需要任何目标或额外的传感器、特定的结构化环境、先验环境点图或外部和时间偏移的初始值。从 FAST-LIO2 修改的稳健的 LiDAR 里程计（Fast LO）。无需任何硬件设置即可在 LiDAR 和 IMU 之间进行快速且稳健的时间偏移和外部参数校准。

LIO-SAM从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。LIO-SAM提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。实现了高精度、实时的移动机器人的轨迹估计和建图。其中建图优化节点整体如下图在之前的博客中，我们分部完成了帧间点云匹配回环检测及位姿计算。

图优化本身有成形的 开源的库 例如g2oceresgtsamlio-sam 中就是 通过 gtsam 库 进行 图优化的，其中约束因子就包括回环检测因子本篇主要解析lio-sam框架下，是如何进行回环检测及位姿计算的。lio-sam回环检测的方式构建关键帧，将关键帧的位姿存储。以固定频率进行回环检测。每次处理最新的关键帧，通过kdtree寻找历史关键帧中距离和时间满足条件的一个关键帧。然后就认为形成了回环。

ubuntu20.04 实测 机械式激光雷达与相机联合标定

LIO-SAM从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。LIO-SAM提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。实现了高精度、实时的移动机器人的轨迹估计和建图。其中建图优化节点整体如下图在这篇博客中 https://www.guyuehome.com/39723 分析了点云匹配前戏之初值计算及局部地图构建。