基于多传感器融合的定位和建图系统

基于多传感器融合的定位和建图系统

01  传感器介绍

IMU(加速度计)的测量频率高，即可以精确的测量到物体的姿态运动，对运动灵敏，同时成本低，体积小，抗干扰能力强，基本上在多传感器融合中是一个必备的传感器。

但IMU也有局限性，第一点是测量值存在随机游走的bias，第二点是由于通过积分的形式计算出那个姿态，因此会存在累计误差，导致长时间的积分结果不可靠。因此常是IMU+雷达或IMU+视觉相机进行计算。

因为视觉相机比较轻小，价格便宜，所以一般是基于视觉相机的SLAM被广泛应用在无人机，AR/VR等轻载重的平台上。但视觉相机也存在局限性，第一是单目相机无法直接获得可靠的3D几何信息，第二是基于多视图的方法可以获得3D几何信息，但需要消耗大量的计算资源，而且在视觉纹理缺失、大量纹理重复场景下的效果比较差，第三是相机的SLAM方法对环境光照的要求会比较苛刻。

激光雷达(LiDAR)传感器可以提供高精度稠密的3D点云，因此可以直接获得六自由度的状态估计，还能重建出周围环境高精度的3D地图。但同样也存在局限性，首先是激光雷达传感器本身就又贵又大又重，使得无法用在一些轻量的平台上，第二是分辨率相比视觉相机是比较低，同时刷新率也低，约为12Hz，第三是帧内的点云会存在运动畸变，第四点是在 周围环境特征不足的情况下，会发生退化现象。

在市区里面，到底是用视觉相机跑SLAM还是用激光雷达跑SLAM实验更合适点？这里更偏向采用视觉相机+激光雷达的方式，相机用来观测周围环境，激光雷达直接测量环境的3D几何形状。在特性排序上，相机获得的图像是有序的，连续的2D测量，雷达获得的是无序的，离散的3D测量。

附赠自动驾驶最全的学习资料和量产经验：链接

02  激光雷达(惯导)SLAM

2.1 首个基于固态激光雷达的SLAM系统(loam-livox)

近期激光雷达的发展一般都致力于降低设备成本的同时提高有效性，其中备受关注的是各种固态雷达，比如微电机系统扫描，光相位阵列，Risley棱镜等。因此，在这样的背景下，loam-livox工作是以livox-MID40为代表的首个固态激光雷达的SLAM框架。livox-MID40和传统雷达对比如图1所示，可以看出来红色livox-MID40的FoV比蓝色的传统雷达是小的多，右图中是livox-MID40的扫描轨迹图。

图1 livox-MID40和传统雷达对比

在此项工作之前，有许多优秀的方法，比如：2014年提出的使用激光雷达完成定位与三维建图的LOAM算法, 提出了一种特征点的提出方法，使用点-面ICP的形式，完成了基于一个单线激光的3D定位和建图工作; 2018年提出LeGO-LOAM，其相对于LOAM的提升主要在于轻量级和地面优化，即在前端使用了分割模块来区分地面点和非地面点，接着对这些点进行特征提取，后端是设计了一个加速算法，进一步提高算法的运行速度。使得LeGO-LOAM在各种平台甚至是Nvidia的Tx2平台上实现了实时的定位和建图。

由于固态雷达的特性，例如非重复扫描的模式不能作frame-to-frame的匹配，小FoV的雷达对feature点要求更高，对异常值更敏感，更容易受动态物体的影响，因此这些优秀的算法并不能直接适用了固态激光雷达。所以，实现了首个适用于固态激光雷达，能够实时鲁棒低漂移的SLAM系统。系统框图如图2所示，先做特征点提取，然后进行面到面的匹配后去更新地图，重建KD树以供下一次的搜索。