五. 激光雷达建图和定位方案-引言

前面内容：

一. 器件选型心得（系统设计）--1_goldqiu的博客-CSDN博客

一. 器件选型心得（系统设计）--2_goldqiu的博客-CSDN博客

二. 多传感器时间同步方案（时序闭环）--1

三. 多传感器标定方案（空间同步）--1_goldqiu的博客-CSDN博客

三. 多传感器标定方案（空间同步）--2

三. 多传感器标定方案（空间同步）--3

四. 点云着色（真彩点云）方案

五. 激光雷达建图和定位方案-引言

首先先介绍下我们项目中SLAM是怎么去做的，它的功能是？

在我们实际应用中，或者说在我们的这个项目中，机器人或者车的位姿获取，或者说状态估计，主要可以分为三块任务，第一块就是SLAM，边建图边定位这种；第二块是mapping，就主要是建地图；第三块是localization，主要是做实时定位；这三块任务的核心原理都是一致的，本质上就是状态估计，然后我们可以通过不同的算法工具去做，可通过滤波器，有ESKF，IESKF；也可通过优化的方式，图优化，因子图优化，滑窗优化这些；这些方法各有利弊，得根据具体的任务来看。

在mapping任务中，我们的要求是要建立一个一致性比较好的地图或者是可规划的那种地图，类似代价地图；在这个任务中对时间的要求其实是不太敏感的，但是对精度要求会比较高；这种任务就比较适合用优化的方式来做，因为它可以批量地把各个优化边加进来用最小二乘来优化。这里面我们想要让地图一致性更好可以有几个提升的方式，第一个就是增加回环检测，就是构建当前帧与历史地图的数据关联，得到的回环可以作为优化变量加进去。第二个就是增加全局观测，也就是RTK，这样会约束整个建图的位姿，就是约束视觉或者雷达里程计的位姿，因为这些里程计很容易在高程上、yaw角上飘。第三个就是提升优化的算法，比如用更鲁棒的LM法这些。

在localization任务中，我们的要求是要实时得到一个鲁棒的、可靠的机器人在世界中的位姿，这种实时性比较强的任务就适合用滤波的方式来做；这里我们用IMU的测量做预测，用其他来做观测，主要有雷达自身的里程计、雷达当前帧和全局地图的点云匹配、GNSS全局观测、轮速计等，这些都可作为滤波器的观测，一起估计一个更加可靠的位姿。（PS：而如果没有雷达里程计和全局地图的点云匹配，只有GNSS全局观测，那么就是一个GINS系统，也就是组合导航，其实是一样的，求解的状态变量都是一致的，都是三个维度的位置、速度、旋转、角速度零偏、加速度零偏和重力加速度；同样视觉观测也是一致的）

那么有没有需要同时用滤波和优化的任务呢？是有的。在SLAM任务中，也就是建图和定位一起的任务中，我们可以在前端采用滤波，后端采用优化的方式来做；当然也可以前后端都用滤波，前后端都用优化。为什么前端用滤波，后端用优化的方式呢？因为前端一般来说我们是需要提供一个高频的位姿信息，它需要是实时、鲁棒的；所以我们用滤波。而后端一般我们是对位姿、地图的一致性要求比较高，对位姿和地图进行一个矫正，往往会加实时建好的地图的匹配、回环检测等一些全局约束。在这样的一个背景下，就可以做这么一个改进，前端采用fast-lio2的ikd-tree做雷达匹配生成里程计；以IMU测量作为预测，观测为雷达帧间匹配和GNSS全局位置观测，实时生成一个世界系下的一个位姿；后端就在不断地保存关键帧，生成地图；然后对地图做匹配、构建回环检测（回环是用Intensity-SC或者SC来做）；将这些优化项加到后端优化节点中去，矫正位姿，修正地图。

但是这个位姿在一些环境下会遇到很多问题，比如经过天桥、隧道、多层天桥这些；需要增加很多异常检测、状态切换；比如在隧道中没有GNSS，也要能继续利用雷达里程计、IMU持续输出位姿；出来后，后端要及时能够修正回去，就可以采用优化的方式。

总结：在这里面，建图任务是为了建出一致性高、精度高的全局地图，对时间不敏感；在定位任务中，精度要求是其次，鲁棒性、实时性是首要。而SLAM边建图边定位的任务中，滤波和优化的结合就是为了能输出实时性高、鲁棒的位姿，也能够在后端对位姿和地图及时矫正。

而多传感器融合在这里主要的作用就是提升位姿估计的鲁棒性和精度，其实就是利用多个传感器对环境中机器人位姿的观测信息进行融合，得到一个确定性更高的位姿；在算法中的表现形式就是滤波器中的P阵，优化中的信息矩阵，其方差会降低，不确定性会降低。