基于多传感器融合的系统自我定位与地图重建

郝 睿，李 瑞，史莹晶，龚美凤，张智容

(1.电子科技大学 自动化工程学院，四川 成都 611731；2.电子科技大学长三角研究院(湖州)，浙江 湖州 313001；3.重庆大学 自动化学院，重庆 400044)

0 引言

随着传感器技术和人工智能的不断发展,诸多全新的移动测量系统空间数据获取技术应运而生,其中自身位姿数据和运动轨迹信息是完成空间数据获取的基础,其准确性直接影响到三维空间场景数据的精度及有效性。但是当面临各种复杂环境时,如何充分发挥传感器的优势,通过自我定位和地图构建(Simultaneous Localization and Mapping,SLAM)算法提高移动测量系统整体的精度和鲁棒性,成为近年来移动测量领域的研究热点。

针对以上问题,本文研究设计基于激光雷达(LiDAR)、相机和惯性测量单元(IMU)多传感器融合的移动背包测量系统,目的在于完成各种环境中系统的精准自我运动估计和对目标环境的三维稠密建图。主要工作如下:

① 构建“传感器联合—激光惯导前端定位器—激光视觉后端三维重建—平台”的多传感融合SLAM架构体系。研究多传感器融合带来的描述一致性、位置姿态的准确性和实时建图的有效性等问题。

② 对多传感器融合SLAM算法加以改进,提出了一种基于因子图优化框架的雷达惯导融合定位算法。首先,改进点云提取及预处理的方式;其次,围绕帧间配准算法时效性和精度问题,设计关键帧选取策略和点云配准算法,构建多个约束因子在局部滑动窗口中进行联合优化,得到初步定位值最后设计基于帧到地图(scan-to-submap)的位姿优化方法,优化定位结果。

③ 对激光-视觉融合方法进行改进。在后端闭环检测图优化部分,利用视觉词袋辅助三维激光雷达完成闭环候选帧选取,同时基于三维激光雷达,进一步融合视觉信息丰富雷达点云的纹理特征,基于融合点云构建新的点云表达形式,进行点云网格化实现稀疏点云稠密化重建效果,以便更好地对移动测量系统周围环境进行评估。

最终设计系统整体评估策略。通过研究领域内公开的标准数据集(KITTI)进行多场景实验,从轨迹精度、算法对比地图一致性等多个角度,全面分析本文SLAM方案中位姿估计方法的精确性和三维重建的有效性。同时设计构建出低成本高性能的移动背包扫描系统(BLS),通过本地数据集验证该移动背包系统的有效性和适用性。

1 相关工作

早在1986年的IEEE机器人与自动化国际会议(ICRA)上,为了将基于估计理论的方法引入到移动测量系统的运动估计与建图问题中,Cheeseman等提出了自我定位和地图构建的概念。经过30余年的理论创新和传感器技术发展,同时伴随着电子探测、智能机器人和自动驾驶等领域的兴起,SLAM作为这些领域中基本的技术模块,完成了众多的工程实践和应用推广任务,成为许多高校实验室和企业的研究重点。

为此,研究者提出了使用诸如激光雷达等有源传感器来完成相关SLAM任务。通过传感器获取现实世界数据,反映空间实体单元的几何信息,记录实体表面在观测点上的物理参数,其中包括观测物体最基本的各离散点三维坐标,单元表面的纹理特性等;将此类观测到的数据进行解算来得到自身状态结果和周围环境信息。由于目前系统所面临的各种复杂环境,难以在使用单一传感器的前提下保证实时完成环境中的SLAM任务。基于纯激光雷达的SLAM系统由于雷达的更新频率低(5～30 Hz),在快速运动场景下性能较低,以及雷达点云的稀疏性且其基于几何呈现的特点,在具有重复结构(如隧道或狭窄走廊)的环境中容易导致SLAM任务失败;基于纯视觉的SLAM系统由于传感器的光敏性特点和不稳定性,当环境信息退化时,系统性能会产生明显的弱化[1]。

经典激光SLAM方案LOAM由Zhang等[2]于2014年提出,作为基于几何三维激光雷达的SLAM方案,目前在 KITTI 数据集上测试状态轨迹精度位列第二。但由于缺少闭环,对于累积误差和发生位置重访的时候无法进行位姿信息的更新。对此,当前激光SLAM领域内的研究学者基于LOAM展开了多种SLAM方案的研究,通过在雷达前端定位部分融入高频IMU(100～500 Hz)数据去除由雷达连续时间运动产生的畸变。2018年Shan等[3]提出的开源LeGo-LOAM算法,提出了具有闭环检测和后端优化的激光惯导融合SLAM方案,团队随之在2020年提出了基于滑窗的激光雷达惯导紧耦合SLAM方案LIO-SAM,该方案构建因子图并将异源传感器测量值作为因子合并到系统中,实现高精度、实时的移动系统轨迹估计和地图构建[4]。

在SLAM实际任务中,为了能够有效地融合多个传感器,需要考虑多传感器数据融合和时空同步。激光SLAM对扫描环境的特征具有较强依赖性,在连续运动期间的不同时间会存在点云的运动失真,点云扫描配准在非结构化场景中容易失败;视觉SLAM虽然在位置重访闭环检测方面取得很大进展,但在能见度降低,以及不同的视角导致同一场景外观的变化等感知退化场景下,产生的特征点稀疏性会导致数据关联模糊和感知理解混叠[5]。而IMU因具备模块小成本低、自主感知性强、高精度(每秒数千个样本)等优势,可以通过自积分跟踪载体姿态,稳定的IMU能为系统提供关键且可靠的位姿信息。当前多传感器的融合也更偏向于低成本和高性能的结合,在融合方案中更加合理利用各传感器数据,同时减少传感器自身给系统整体带来的影响,使得系统整体在性价比高的方案下,能够保证系统性能以及稳定性。

随着信息技术研究的深入、传感器的迭代及数字化城市等相关技术的发展,高精地图的重要性也日益显现。与仅包含宽、高信息的二维图像相比,三维地图重建技术的发展将移动测量系统的感知提高了一个维度。从传感器融合角度来看,微软将红外光传感器与单目相机相结合,在2009年推出了KinectV1,其中空间距离信息由红外光传感器主动提供,环境纹理信息由被动传感器相机提供。这类融合方式的出现使得室内三维重建技术得到了迅速的发展。近年来已经开发了将LiDAR和相机数据结合起来进行3D映射或重建的各种算法。使用LiDAR数据进行重建已用于不同的项目,来自ETH ASL实验室2020年发布的基于体积元素和基于截断的带符号距离函数(TSDF)的建图工作[6],在机器人的避障、导航领域的三维稠密重建上有重要作用。

2 多传感器融合定位与建图

本地移动BLS系统信息采集主要由VLP-16激光雷达、单目相机和六自由度(6-DOF)IMU组成,背包平台携带由电源和在CPU上、Ubuntu开发环境下的笔记本电脑,共同组成信息处理系统。系统整体具备移动采集、自我定位与建图能力,如图1所示。BLS系统的平均运动速度为1 m/s,由于平台没有搭载全球导航卫星系统(GNSS)类相关传感器,无法获取运动轨迹真实值。因此在本地数据采集过程中,将有意识走出一个规则区域,形成路线闭环,以此定性评估系统整体定位效果和建图效果。

图1 背包扫描系统Fig.1 Backpack scanning system

本文所设计的背包式移动测量系统旨在自动收集和解算传感器观测数据,完成自我运动估计和环境重建。按照“多传感器联合—位姿定位—闭环检测—位姿地图更新—点云网格化—三维地图重建”的算法流程,构建包含“传感器硬件—激光惯导前端定位器—激光视觉后端三维重建—平台”的SLAM总体技术框架。总体框架如图2所示。

图2 多传感器融合算法框架Fig.2 Multi-sensor fusion algorithm framework

2.1 多传感器数据融合

定位就是基于系统传感器观测与测量数据,通过数据融合来解算系统的运动状态。本节将具体描述所提出的多传感器融合定位与地图重建2个模块,首先阐述基于因子图优化的LiDAR、惯导数据融合方法: 本文引入关键帧和增量式优化的方式求解系统运动状态,通过高效的滑动窗口方法,构建包含固定数量点云帧的局部地图;然后将新的扫描关键帧注册到子关键帧集合中[7],同时固定观测误差项也会被限制在一定范围内;最后对状态估计中的各个状态量进行建模和求解,在接收到新的传感器观测信息后,根据传感器各自的观测方程以及相应的代价函数对状态量进行边缘化,从而实现多传感器的实时数据融合。

此外,由于系统中各个传感器都有自己的时间基准,且采样频率各异,所以需要保证传感器融合的时间信息同步。为了综合运用整体软硬件算法,本系统将系统各功能模块节点化。首先在硬件层面使用开源操作系统(Robot Operating System,ROS)发布—订阅式的通信机制,包括异步话题与同步服务主要通信机制。利用ROS自身提供的时钟信号,通过驱动控制程序和触发传感器响应,将接收到的传感器观测值统一到ROS通信的规范格式下,参照系统时间筛选出邻近时间内的传感器采样数据,从而近似实现不同传感器节点的时间同步[8]。在软件层面,设计算法来同步循环多个传感器所发布的原始数据。主要思想为基于每次更新的LiDAR点云帧序列,将其当前时刻作为插入的时间戳,通过插值计算获得除雷达以外其他传感器的同一时刻等效信息[9]。

2.2 多传感器融合定位里程计实现

当前激光SLAM定位部分存在的主要问题包括点云信息的获取具有较强的环境特征依赖性,定位的效率与精度依赖于系统位姿估计初值的精度以及闭环检测算法的回召率和精确率。为了解决以上问题,并且在具有挑战性的环境下保证系统鲁棒性和计算效率,本文基于因子图优化思想,设计以移动测量系统为中心的LiDAR-惯导定位算法。设计和改进模块包括以下三方面:

① 改进原始点云预处理方式和关键帧选取策略。融合高频IMU来线性插值辅助点云运动畸变校正,并对点云去冗去噪降低配准误差;围绕帧间配准算法时效性和精度问题,改进用于点云配准的关键帧选取策略,相比于传统的点云配准方法,大大增加了计算速度,减少了点云配准所需要的时间。

② 构建基于正态分布变换(NDT)的点云多态配准算法。完成帧间配准,建立雷达相对变换位姿因子,设计基于帧到子地图的局部位姿优化方法,优化定位结果,保持计算效率。

③ 构建因子图模型。将IMU的定位结果,激光雷达扫描配准得到的帧间变换结果以及发生闭环检测时的位姿变换结果,分别传递到因子图优化中,通过联合优化得到的系统定位值,生成系统初步定位轨迹和地图。

本文基于因子图优化框架,根据输入的点云帧序列、惯导测量序列和相机观测序列,估计系统运动状态量,包括关键帧的6-DOF位姿,LiDAR扫描点云值以及IMU的偏置误差。最终估计出移动测量系统的状态及其轨迹。在一般的局部优化过程中,只有滑动窗口中的雷达相对位姿变换因子和IMU预积分因子参与,滑动窗口外的历史状态信息则进行边缘化处理,若发生了位置重访,则闭环检测因子以相对位姿变换因子的形式加入参与联合优化。当新的传感器观测序列进入时,子图和优化窗口按照时间发展同时向前滑动[10]。对此构建出LiDAR-IMU融合定位的无约束优化目标函数为:

(1)

将IMU数据预积分测量值与LiDAR点云数据配准分别得出的帧数据之间的位姿约束,以及在发生位置重访时产生的闭环检测约束,使用基于因子图的优化库(GTSAM)的优化方式融合[11],得到载体更高精度的位置姿态信息。式中包含LiDAR、IMU传感器的残差范数。其中先验因子代价函数提供了当前优化窗口中待优化状态与历史边缘化状态之间的约束为:

(2)

雷达相对位姿变换因子代价函数表示为式(3),描述了滑动优化窗口中相邻LiDAR扫描关键帧之间的位姿运动变换约束,其中闭环检测代价函数则依靠雷达相对位姿变换因子代价函数来实现。

(3)

IMU预积分因子代价函数如式(4)所示,描述了优化窗口中相邻LiDAR扫描关键帧之间,惯导测量状态和随机游走偏移量之间的运动约束。

(4)

关键帧信息包括自身6-DOF位姿、LiDAR测量数据以及观测图像,在完成几何、视觉层面的闭环候选帧选取后,其中基于几何特征点配准方式的激光SLAM闭环检测,将新的点云帧注册到与图中历史节点相关的所有其他帧中,以找到匹配项。同时,视觉通过词袋(BoW)模型比较图像之间的相似度,来判断闭环检测的发生。如果检索到闭环候选帧,则将图像的特征描述符与几何检测出的候选帧配准,以进一步验证闭环候选帧的正确性[12]。

当新的状态节点xk添加到因子图中,通过NDT将子图配准得到的当前帧与候选帧2帧相对位姿,即闭环约束因子[13],融入到当前SLAM系统中。主要流程如下:

① 寻找欧几里德空间上与xk距离相近的关键帧;

② 如图3所示,假设xi为当前闭环候选帧,通过NDT点云配准xk时刻的激光扫描特征Xk到附近的关键帧Fi-m,…,Fi,…,Fi+m,本文定义m=12,得到当前状态xk节点与候选闭环节点即历史状态节点之间的变换关系ΔTi,k;

图3 多传感器融合因子图Fig.3 Multi-sensor fusion factor map

(a)标定环境

(c)标定效果验证

③ 将ΔTi,k根据相对位姿因子构建方法,构建为闭环因子添加到系统因子图中。

以上工作完成了累积误差消除,解决了位置重访问题。 将构建的约束因子放入GTSAM库的因子图中进行优化融合,最终实现了系统在不同环境下的可靠的自我估计定位里程计功能。

2.3 多传感器融合稠密建图

在通过多传感器融合完成系统基本运动定位和点云地图构建后,需要对周围环境进行进一步描述。 本节具体阐述基于多传感器融合的稠密建图方案,主要包括基于因子图优化的轨迹地图更新方式以及LiDAR-相机进一步融合,生成纹理化雷达点云的网格化过程。

首先保证LiDAR和相机的数据同步,对二者进行外参标定,通过视觉与雷达的共有特征进行3D-3D点对匹配,用刚体的平移变换和旋转变换来描述三维激光雷达和相机在空间中的相对位置关系,最终实现融合。原理如下:

(5)

式中:(u,v)表示由相机捕获的图像数据,(x,y,z)表示激光雷达捕获的三维点阵云,其目标是建立一个转化矩阵M,将三维点映射到二维点 。

在已知传感器点的对应关系的情况下,采用Kabsch算法[14]进行点对间的RT矩阵运算。设P和Q是同一个目标在不同坐标系下的2组点,pi和qi是集合内的第i个点。令2个坐标系的旋转为R,平移为T,Kabsch算法求解下述优化问题:

(6)

得出标定后的R、T矩阵后,将激光雷达点云作刚体变换转移至相机坐标系下,再投影至图像坐标系中,最终返回有色点云至雷达坐标系中。

完成全局位姿与地图的更新后,返回当前点云帧集合与闭环检测后得到的变换矩阵。在点云配准环节中,当目标点云为空或目标点云过小时,设置返回空的配准候选集条件,以此判断是否存在配准点云帧,从而确定是否发生了位置重访。当其为空时,说明没有得到点云配准,从而也不存在发生闭环,此时将原始点云转换合并到目标点云中[15],发布更新的点云地图和轨迹。

结合生成的雷达里程计,采用比较传统的方式将里程计和点云信息进行增量式拼接建图,将点云信息合成到TSDF建图,降低地图的内存开销并保证了实时更新。参考Nieβner等[16]在2013年的工作,最终利用实时的雷达里程计和着色点云信息更新周围环境。主要过程如下:

① 将输入的点云按照一定分辨率划分为立方体,又称为体素。由于扫描得到的三维空间内外部部分信息是空白的,在空间划分过程中,为了避免空间资源浪费,提高计算效率,采用八叉树(Octree)非均匀划分,将算法的复杂度降至O(log8N)。根据扫描数据的最大空间尺寸,建立首个立方体作为根节点(即边界区域)包含所有三维空间信息。若立方体内包含空间点,从根节点开始,将包含扫描物体节点均匀划分为8个子节点,递归迭代所有立方体直至满足终止条件;若不包含点或已达到最大深度则不进行划分。

② 构建哈希表进行体素网格的散列存储,每个体素网格大小固定,同时用连续的内存空间储存每一个体素块。转化三维空间中划分构造的体素g到世界坐标系下,得到三维空间点坐标p。通过使用哈希表存储体素网格,提高时间效率,此过程的算法时间复杂度从O(n)降到O(1)。

③ 对每一个雷达线束下的终端体素只执行一次光线投射,这加速了TSDF中地图信息更新[17]的速度,大大提高了算法的时间效率。根据第i帧纹理化点云的当前系统位姿矩阵:

P=[p0p1…pn+k+1]。

(7)

由T(g,i)-1p求得p在LiDAR坐标系的投影点,同时反投影得点云帧中对应像素点x。

④ 通过式(8)计算点p沿投影射线到扫描物体表面的最近距离:

sdfi(p)=‖T(g,i)-1p‖-Ri(x)。

(8)

(9)

3 实验验证与结论

基于上述内容,设计系统整体评估策略。通过研究领域内公开的标准数据集KITTI进行多场景实验,从轨迹精度、算法对比和地图一致性等多个角度,全面分析本文SLAM方案中位姿估计方法的精确性和三维重建的有效性。此外,在移动测量系统上进行了实际应用,对其进行综合分析,验证了系统的可靠性与稳定性。

3.1 公开数据集评估实验

基于标准数据集KITTI的系统轨迹精度评估,该数据集可以很好地观察移动测量平台在各种任务环境下的感知情况。分别在不同类型序列中完成对比实验,展示相对于轨迹真值,改进后的系统前端算法初步定位效果。此外,进一步对比闭环优化后的轨迹效果(带*号),阐述闭环检测和因子图优化对累积误差消除做出的贡献,并对优化前后系统定位算法的鲁棒性展开分析。

00、06为环境特征明显的城镇序列,闭环较多且扫描时间较长。其中00序列检测到的闭环数目较多,06序列则容易发生假性闭环,同时这2个序列容易产生较大的累积误差。

从实验过程中效果分析,以及最终结果图5和图6所示,00、06序列闭环检测准确率较高,在原本发生位置重访的区域上完成了闭环。相较于初始定位的系统运动轨迹,在长时间运动下以及无闭环条件下带来的误差累积,轨迹发生了明显偏移。全局优化后累积误差基本消除,其运动轨迹也更接近于真值,说明后端闭环检测优化达到了预期效果。

图5 00、09序列的系统轨迹精度图与点云地图Fig.5 System trajectory accuracy map and point cloud map of 00 and 09 sequences

图6 06、10序列的系统轨迹精度图与点云地图Fig.6 System trajectory accuracy map and point cloud map of 06 and 10 sequences

由图5可以看出,重建后的环境建筑物轮廓清晰,树木、车辆等物体描述基本准确,局部点云地图如图A、B、C、D所示,证明了所建点云地图的有效性。

09、10为城乡结合环境序列,环境特征较为复杂,环境变换较多,综合了B,C类型场景特点。09序列前期没有闭环,实际的和检测到的闭环均只有1个,位于扫描起始和终止区域,连续且长时间的弯道给系统带来的轨迹漂移,在闭环后显示累积误差大幅度消除并回到初始位置。序列10类似于序列01,受到有限闭环的影响,连续无闭环后的运动轨迹也略微变差,但速度稍慢,整体轨迹精度可观。

为了客观评估系统在定位层面的精度以及算法的鲁棒性,将采用业内通用的相对位姿误差(Relative Pose Error,RPE)评估指标。本文取100 m长度相对平移误差(%)的平均值作为评估指标,与经典激光SLAM方案LOAM以及目前主流的多传感器融合方案LeGO_LOAM[3]、DVL_SLAM[18]和Hdl-graph-slam[19]进行轨迹精度对比评估。目的是验证当前定位算法精度是否能够匹配现有主流算法以及本文算法的鲁棒性。同时为了避免和对比的SLAM方案中的原实验数据相违背,直接引用对应方案论文中的实验结果,并通过开源评估工具来完成实验数据评估[20],具体评估结果如表1所示。

表1 RPE评估结果:相对平移误差

由表1中RPE评估结果可知,LOAM总体具有最高的精度。本文是基于Hdl-graph-slam方案展开的研究,该方案为早期多传感器融合3D SLAM方案,由于在点云采集过程中,需要提取地面点以及进行点云的降采样,通过提取关键帧建立各约束完成位姿估计,因此在高速公路数据集01序列和04无闭环的乡村序列效果相对于其他方案较差,表内评估数据定义为失效。

通过以上分析可以得出,设计调整前端定位和后端建图的策略后,本文定位在大多数序列上精度接近LOAM,且在鲁棒性上略有提高。

对比现有基于LOAM的另外2个SLAM方案,也同样具备较高的轨迹精度,误差维持在米级范围内。其中LeGO-LOAM在01高速公路数据集序列失效,初步分析因为其前端帧配准是基于特征点完成的,且经过2次特征点提取,基于LOAM的点对配准误差思想来估计系统状态,而01序列环境特征点稀疏,不利于该方案的特征提取配准估计。本文仍旧将其01序列的评估数据纳入综合评估,暂认为无效。综上所述,对于本文定位的稳定性和精确度,特别在闭环检测和后端全局优化后,系统整体精度得到可观的提高,证明了闭环优化对系统整体性能的有效性。

由于轨迹精度和建图效果相辅相成,下面进行雷达点云地图和网格(Mesh)地图的一致性分析。图7展示的是发生多次闭环的城镇序列00,从点云图效果来看,在发生位置重访时,其闭环准确率高,所构建环境的点云地图结构无误。根据地图局部效果A、B所示,纹理化后的雷达点云基本具备了识别环境属性的能力。地图局部效果C、D则进一步展示了通过点云网格化后的车辆、墙壁、灌木丛和房屋等环境信息。图7通过坐标系转换,将雷达点云地图和重建Mesh地图统一到世界坐标系下,进行显示分析,可以观察到2种类型点云重建的一致性,标识的A、B、C、D四处稀疏点云和稠密地图基本重合。此外,局部效果图为了具体显示出三维重建效果,将点云显示体积设置为0.01。从车辆、墙壁、灌木丛以及环境整体的三维重建效果来看,提出的方案可基本完成大环境下的稠密地图重建。

图7 07 序列点云与 Mesh 图联合效果Fig.7 Joint effect of 07 sequence point cloud and Mesh map

3.2 本地移动背包系统实验评估

本文设计和构建了低成本高性能的BLS系统,通过本地数据集验证了该移动测量平台的有效性和适用性。最终可知该系统整体对设定高级任务目标,在环境检查、人机交互和城市重建方面有一定的作用和意义。

图8展示了在室外复杂变化环境下的系统状态估计效果,主要围绕电子科技大学众创空间生成轨迹和地图,总长度约400 m。

图8 本地室外状态估计效果定性分析与Mesh地图重建Fig.8 Qualitative analysis of local outdoor state estimation effect and Mesh map reconstruction

如图8(a)卫星实景图所示,环境中包括空旷大道和小树林,地形较为复杂且不平坦。由图8(b)可见,移动测量系统基本构建出环境中有关建筑物、树木和道路的相应特征点云效果,在起始、终止点处轨迹误差达到厘米级。实际运行过程中的系统实时性不差,基本保证人和轨迹同步一致。整体说明系统方案面对较复杂、非结构性强的环境时,状态估计具有较强的鲁棒性。

如图8(c)所示,局部效果图中基本包含环境中树木、建筑物和草地等结构的颜色信息和几何结构信息。室外环境下,由于雷达点的稀疏性和受到单目相机的视野区域限制,可重建基本Mesh地图,若需要丰富信息,需要进一步添加点云地图。

之后,为了验证室内连续不同环境下本地BLS系统的鲁棒性,完成了在室内、走廊和楼梯3种连续不同环境的定位和地图重建实验。其中红色箭头示意Mesh地图下更新的当前系统定位信息,整体效果如图9所示。

(a)楼梯与走廊环境

(c)稠密地图重建效果

室内环境结构主要为楼梯、走廊和室内墙壁,从室内走向楼梯间并完成路径往返,总路程约50 m。由于周围环境纹理色彩信息多为白色,环境结构单一且重复,主要查看环境重建结构。对于楼梯间和走廊重建效果,整体基本没有发生很大的效果偏差,结合状态估计结果和生成的LiDAR点云地图可见,路径返回过程中地图和轨迹发生修正,楼道轮廓直观清晰,整体点云地图中无其他杂乱点,室内和墙壁重建连接在一起,整体基本达到期待的重建效果。

4 结束语

本文基于多传感器融合的方法,研究高性能低成本的移动背包测量系统,并对于当前多传感器SLAM方案中存在的相关问题进行研究与改进,对系统获取各种环境条件下的准确定位结果和丰富的空间信息具有重要意义。总结如下:

① 研究了多传感器融合带来的描述一致性、位置姿态的准确性和建图的有效性等问题,按照“多传感器关联—位姿定位—闭环检测—位姿地图更新—点云网格化—三维地图重建”的算法流程,构建“平台—激光惯导前端定位器—激光视觉后端稠密建图”的多传感融合SLAM方案,集成了激光雷达、单目相机和惯性测量单元等多个传感器。

② 改进多传感器融合SLAM算法,提出了一种基于因子图优化模型的多传感器融合算法。在前端定位模块,构建因子图模型融合雷达惯导信息,设计构建多个约束因子在一个局部滑动窗口中联合优化,得到初步定位值;针对动态物体的干扰,设计位姿再优化,完成雷达相对位姿变换约束因子的构建。

③ 发展一种激光-视觉后端融合方法,在后端闭环检测图优化模块,融合视觉BoW到激光SLAM闭环检测中,将几何-视觉闭环检测有机结合,设计基于几何-视觉的联合闭环检测方法降低系统累积误差。构建闭环约束因子,基于因子图模型,联合约束因子完成全局优化,实现位姿地图的更新。即使在快速运动条件或特征不足的情况下,也能完成高精度定位并保证全局地图一致性。同时,为了获取更丰富的环境信息以及提高系统的泛用拓展性,基于三维激光雷达,进一步融合视觉信息丰富雷达点云的纹理特征,进行点云网格化实现稀疏点云稠密化重建效果。

④ 完成KITTI和本地移动背包2个不同移动测量平台的多场景实验,针对本文融合方案与现有多传感器融合方案进行定性定量的轨迹精度分析,并实现点云地图和三维重建地图的一致性表达,综合分析平台算法效果,最终验证系统算法的鲁棒性和平台的适用性。