关于人工智能:基于多传感器融合的定位和建图系统

转载自:深蓝AI
分享嘉宾:林家荣
文稿整顿:William
编辑:一点人工一点智能
原文:基于多传感器交融的定位和建图零碎

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树以供下一次的搜寻。

图2 loam-livox零碎框图
点云去帧内静止含糊如图3所示,一开始应用线性插值(图3(b)),发现成果不太好,前面应用分段解决(图3(c)),即把一帧分为三段,而后去从新预计姿势,因为三小段上帧内的静止含糊不显著。图3(d)是开源手持设施和采集场景。

图3 去帧内静止含糊

2.2 激光雷达-惯导紧耦合的里程计(FAST-LIO)

在此项工作之前,也存在许多优良的工作。LIO-mapping 是香港科技大学刘明组所提出的一个紧耦合的雷达惯导交融办法,在LiDAR Odometry 交融了IMU信息以进一步提高定位精度和鲁棒性。此外,为了获取雷达位姿的更牢靠预计,提出了一种旋转束缚精炼算法(LIO-mapping)来进一步对齐雷达位姿和和全局地图。LIO-Mapping借鉴LOAM和VINS-Mono的思维进行的一个联结状态预计问题。工程层面和VINS-Mono一样,只不过前端视觉局部换成了lidar的前端进行特色关联和odometry的位姿预计。其次要分为两个局部:tightly coupled lidar-IMU odometry与the rotation constrained refinement。LIO-SAM是Tixiao Shan,在LeGO的根底上,提出了一种紧耦合激光-惯性里程计办法,通过优化蕴含LiDAR里程计因子,IMU预积分因子,GPS因子和回环因子来失去机器人的全局统一的位姿。作者应用帧-部分地图匹配代替LOAM的帧-全局地图匹配,进步了帧图匹配的效率。然而他们都或多或少存在一些问题,导致他们在理论应用中并不够鲁棒,精度不够高,运行速度慢的问题。
因而在此类背景下提出了FAST-LIO系列。FAST-LIO是一个高效的紧耦合的雷达惯导交融框架,是基于迭代卡尔曼滤波器,滤波器里提供雷达观测和IMU观测,次要奉献在于升高了大规模观测数量导致的计算量。此外,还提出了FAST-LIO2,其次要翻新在于应用了一个增量式KD树去保护地图的更新,无效解决了传统办法中重建时导致地图越来越大的问题。
FAST-LIO框架如图4(a)所示,首先以雷达点云作为数据,而后进行特征提取后进行去帧内点云含糊(图4(b)),一帧内的点云通过IMU预积分的形式将其对立到帧末,以此打消掉真正的点云静止含糊。最初通过迭代卡尔曼滤波器办法去紧耦合的交融激光雷达和IMU观测。

图4 FAST-LIO框架
试验是在实验室中拿一个手持设施一直地剧烈运动来失去周围环境建图,后果如图5所示,能够看到无论是精度上,还有计算效率上都会比同行的办法有比拟显著的劣势。

图5 FAST-LIO试验比照
FAST-LIO2的零碎框图如图6所示,其次要翻新的第一点是引入增量式更新的KD树,第二点是应用的原始的雷达点,没有特定点提取的步骤,因为应用了更多的点,反而在精度上比应用特色点的办法有所晋升,第三点是重力加速度上应用了S2更新。

图6 FAST-LIO2框架
增量式更新的KD树之所以会比同行的办法有显著的劣势,次要因为增量式K的引进。传统的KD树里,去搜寻K最近邻的点,而后形成一个点到面的残差时,会用到KD树,它是一种均衡的数据结构,也是最优的knn搜寻,但并不反对减少式的。于是,提出了增量式K的数据结构,首先是反对增量式的更新,其次是能够在线的做动静的树均衡。
模型耗时评估后果如图7所示,即计算比拟每雷达帧的解决工夫。通过跑雷同解决的雷达帧的后果比照,能够发现存在非常明显的劣势。

图7 模型耗时评估比照

03  多传感器交融(激光雷达-惯导-视觉)

3.1 首个开源的多传感器紧耦合计划(R2LIVE)

当初的激光雷达–惯导—视觉的交融还是学术界的一个十分热门的方向,很多中央的问题还解决的不是很彻底。LIC-Fusion 是19年提出的雷达-惯导-相机的多传感器交融算法,能够无效地交融IMU测量,稠密的视觉特色和提取的激光雷达点。此外,还可能在三个异步传感器之间执行在线的空间和工夫传感器标定,以弥补可能的标定偏差。随后,LIC-Fusion 2.0又提出了一种新的立体特色跟踪算法来更加无效地解决激光雷达测量,以进一步提高算法的精度和鲁棒性。LVI-SAM是在LIO-SAM的根底上,交融了VINS-MONO的观测,整个零碎分为VIO和LIO零碎,VIO零碎通过独自解决相机+IMU数据给出一个里程计后果, LIO零碎通过提取匹配雷达特色给出一个雷达里程计后果,最初,通过联结优化实现零碎的状态预计。
在这样的背景下,提出了R2LIVE的多传感器交融计划,零碎框图如图8所示。该模型也是VIO和LIO两个子系统,LIO跟FAST-LIO框架差异不大,VIO是有两步观测,第一步是用迭代卡尔曼滤波器实现3D点的观测,第二步去更新相机的状态预计。因为卡尔曼滤波器耗时比拟低,所以可能及时的计算出相机姿势,而后后端有个图优化是因为须要一直优化视觉的3D点地位,否则更新进去的地图会产生较大误差。

图8 R2LIVE框架
迭代卡尔曼滤波器如图9所示,当输出一帧雷达点云时,上一帧是相机的更新帧,预积分到一个时刻,而后提供一个姿势预计的先验,之后雷达点云达到时就实现实现一次迭达卡尔曼滤波器的更新。当几个状态预计实现之后,下一帧如果来的还是那个视觉的观测,那就把雷达观测的预积分跟到相机来的工夫戳上,再去实现更新。要做视觉和雷达的交融的起因在于进步零碎整体的鲁棒性。

图9 迭代卡尔曼滤波器示意图

3.2 实时真彩地图重建(R3LIVE)

R3LIVE的零碎框架如图10所示,也是分为LIO和VIO两个子系统,LIO是基于FAST-LIO2改良的一个零碎,VIO是思考的一个交融视觉相机的计划。R3LIVE相比于R2LIVE的区别在于,R3LIVE会思考一些相机的光度学模型,即相机的镜头暗影、相机的响应函数等方面。

图10 R3LIVE零碎框架
光度学模型如图11所示,在建图中,认为一个点在不同相机观测中的色彩值是一样的,然而没有思考相机曝光的影响或镜头暗影及响应应函数的影响,导致它有可能一个点插在不同的相机帧里。R3LIVE++每个点是重建的辐射光场,即反射自然光中的一个点的辐射水平通过黑白相机的模型,而后去最终计算出相机的灰度值观测。

图11 R3LIVE++光度学模型
VIO中存在两个更新,一个是frame-to-frame(图12左),还有一个是frame-to-map(图12右)。之所以须要frame-to-frame更新是因为间接应用frame-to-map的更新是比较慢的,因为一开始可能没有收敛到一个适合的收敛值。frame-to-frame的VIO更新是上一帧跟踪的点,通过光流的办法去预测下一帧的以后帧,通过迭代优化全状态,使得两个观测点的像素间隔最小,最初实现更新。

图12 VIO中的两个更新形式

04  激光雷达的实时定位和网格(mesh)重建零碎(ImMesh)

4.1 ImMesh的介绍和试验演示

R3LIVE重建的黑白点云是要失去雷达在地图外面全环境的辐射信息,才可能重建出地图的纹理,且必须要求每个点笼罩到了才会有。然而,点云的密度无限,即不能在没有点的中央看着这个点,现有一种解决方案是重建地图的网格(mesh)模型,而后用相机失去图像RGB信息后贴到网格上,这样可能无损的去重建地图纹理。在这个背景下,做了一个在线激光雷达的实时定位和网格重建零碎。次要奉献是实现了一个实时的、在线的网格重建。网格重建的相干工作首先一个是泊松重建,其原理是通过一个3D的标量方程,而后去拟合3D空间中的点云,使得拟合的标量方程在每个采样点的梯度方向上和输出点的靶向量是尽可能的匹配,再通过优化3D的标量方程,之后去提取等值面的形式把这个面给提取进去,示意图如图13所示。然而泊松重建的速度比较慢,另一方面是在点间距的边缘上会呈现一些莫名凸包。

图13 泊松重建示意图
离线办法中比拟典型的是德劳内三角剖分+图宰割的办法,如图14所示,基本原理是第一步先用3D的德劳内三角剖分对3D空间做四面体的切割,而后再通过相机和观测点的可视化关系,再去做图宰割,以此辨别去面片的inside和outside,最初是提取这些等值面。这些等值面就是重建的进去的mesh。

图14 离线重建办法
在线办法比拟经典的是TSDF+Marching cubes,如图15所示,其原理是通过并行更新每个体素的TSDF,前面用Marching cubes的办法把面片给提取进去。TSDF的更新能够做到实时,如采纳GPU减速,但在提取外表时候的速度会比较慢一些,因为是隐式的重建办法。

图15 TSDF+Marching cubes在线重建
只管当初有这些工作,但它们在激光雷达上仍旧无奈做到实时性。因而,心愿去构建一个实时的网格重建,因为一方面是是效率快,第二方面是可能实时的给你一个预览反馈。ImMesh零碎框架如图16所示,它是间接以雷达和转换器为输出,如果有IMU则也会拿来做雷达点云的去静止含糊。定位模块第一步是去点云的静止含糊,第二步是做状态预计,而后第三步是做点云配准,实现之后存入数据结构中,即启动在线的match模块,首先取点云,而后进行降维解决,第三步是做增量式的三角形面片更新,最初把更新后果推送到全局地图里。所以,ImMesh可能同时输入零碎的一个实时状态,还有实时输入雷达匹配准之后的点云,以及重建的三角形面片。

图16 ImMesh零碎框架
这里在线网格重建的办法先是应用分层体素,如图17所示,首先是用分层体素对空间进行一些划分,比如说有L1,L2,L3,之后用哈希表的模式将这些体素给储存起来,而后分类管理。所以,每次雷达点云配准之后,会更新一些部分的地图,这时每个体素里存在一个状态,如果体素中插入新的顶点,体素中会有一个标记位去标记,让网格重建模块去更新这个体素外面数据。

图17 分层体素示意图
在取出点之后,将3D的一个个点通过图形的办法去投影到一个适合的立体内,这个适合的立体是由voxel-map预计的一个地图在此点的立体法向量,而后进入到那个立体去。因为雷达点自身对环境进行测量的时候,这个点原本是在一个间断的立体上,也在一个区域内,如果这时在3D上间接做match,复杂度会很高,而且目前还没有成熟的办法。所以,这里采纳间接用2D的视角去解决3D的这个问题,实现降维操作,之后再去进行部分区域的更新,最初利用增量式的machine进行同步到全局地图外面。
这个零碎可能实现在数据采集的时候,同时对周围环境做在线的网格重建,演示如图18所示。这里有三个视图,蓝色框的是第三人称拍的,红色框是雷达点云的录入,黄色框是前置相机的预览,而后把这三个视图通过视频剪辑的形式将工夫给合在一起,进行同步。

图18 零碎演示
该办法不仅仅实用于固态雷达,还有旋转机械式雷达等。在不同的数据集上测试了这个算法的性能,后果如图19所示。能够发现在不同的数据集,不同的场景和不同的雷达上对该算法进行测试,结果表明这个算法都可能在这些测试区域中实现一个实时的重建。

图19 试验后果

4.2 基于ImMesh开发的利用

1)激光雷达点云加强

激光雷达点云加强利用如图20所示,第二行的雷达的扫描是一种玫瑰花色的扫描,导致这种雷达做learning算法的时候会呈现一些问题。因为该零碎可能实时的重建出周围环境的三角形面片,那就能够把这个三角形面片给察看化到以后帧的坐标系外面,这时就可能失去一个规定的深度图,而后再从深度图外面反投影。所以,雷达加强之后的点云就是一个一个规定排列的加强点云。如果相机的四张角变得更大,那么就能够取得更大范畴的3D感知的雷达点云,就能够对远处观测的更浓密的一些。

图20 激光雷达点云加强

2)疾速无损的场景纹理重建

方才提到R3LIVE是须要每个点去记录环境的纹理,如果有match的模式,能够间接把采集的图像给贴图到面片上,因而就可能无损的对环境进行纹理重建,这里在线的疾速纹理重建如图21所示。

图21 疾速场景纹理重建
1、 书籍举荐-《机器人编程:应用树莓派3和Python构建和管制自主机器人》
2、 最新|3D SLAM对于z轴精度的优化思路整顿
3、 书籍举荐-《卡尔曼滤波与信息交融》
4、 机器人静止|浅谈Time Elastic Band算法
5、 书籍举荐-《大规模SLAM技术》
6、 ROS与挪动底盘通信

【腾讯云】轻量 2核2G4M,首年65元

阿里云限时活动-云数据库 RDS MySQL  1核2G配置 1.88/月 速抢

本文由乐趣区整理发布,转载请注明出处,谢谢。

您可能还喜欢...

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注

此站点使用Akismet来减少垃圾评论。了解我们如何处理您的评论数据