一种带有3D运动状态估计的slam系统

A Flexible and Scalable SLAM System with Full 3D Motion Estimation

摘要:

在许多应用场景中,比如城市搜救和搜索(USAR)机器人,需要去获取未知环境的地图。我们提出了一个快速在线学习占用栅格地图、占用较少计算资源的系统。它利用激光雷达系统与基于惯性传感器的3D位姿估计系统进行融合,实现了一种鲁棒的扫描匹配方法。通过地图变化的快速近似和多分辨率栅格地图,在各种有挑战性的环境中实现了可靠的定位与建图。提供了多种数据集以适应嵌入式手持建图系统。我们表明,该系统是足够准确的,在我们考虑的应用场景中,不需要显式闭环检测技术。该软件可作为ROS的开源代码包。

I.介绍

学习环境模型并定位自身是一个真正的机器人在真实世界运行的最重要的能力。在本文中,我们提出了一种灵活的、可升级的系统来解决SLAM问题,已成功的运用在了UGV、USV和一个小型的室内导航系统上。该方法消耗的计算资源较少,故可以应用于低成本、低功耗的嵌入式系统。该方法是在ROS上实现的开源软件。它适应ROS上的API和导航stack,并可以在ROS的生态中替代其他SLAM方法。

本文介绍的系统旨在保证计算力要求低的前提下,实现足够精确的环境感知和自我定位。它可以应用在小尺度的、不必做大的闭环的系统中,并需要使用高更新速率的激光雷达系统。类似的场景包括RoboCup搜救比赛,可能需要在模拟的地震场景中找到受害者,因此需要对车辆在6Dof上进行姿态估计。或者,比地面机器人更灵活的室内飞行器的导航。有关USAR的结果和模型在参考【2】中可以找到。

我们的方法结合了2D SLAM(基于激光雷达的平面地图)和3D导航(基于IMU)融合了2D的SLAM信息作为辅助(FIG.I)。SLAM过程通常是由激光雷达的数据更新来触发的,而整个3D导航解决方案是需要实时计算的,构成车辆控制系统的一部分。

II.相关事宜

近些年已经有大量的研究关于SLAM的问题,例如作为开源软件的gmapping使用的是Rao-Blackwellized粒子过滤器,可以可靠在的典型办公室室内场景使用。然而,这些解决方案的工作最好在平面环境,依赖于现有的,足够精确的航迹以及不利用现代雷达系统提供的高更新率。对于非结构化环境,会导致载体的显著滚转和俯仰运动,或在空中平台上实现这种系统不适用或必须进行显著修改。

一个SLAM的前端和后端系统之间的区别。在大满贯的前端,用于实时在线估计机器人运动,后端用于优化位姿图,和在使用的前端产生的位姿之间的约束。本文提出的方法可以作为一个SLAM的前端和不提供姿势图优化像[ 4 ]和[ 5 ]提出的解决方案。然而,我们表明,在许多情况下,这种优化并不需要在现实中的一些条件,因为这种方法是足够准确的,可用于机器人来执行他们的任务。

基于激光扫描匹配的室内导航系统提出了对旋翼无人机使用[ 6 ]、[ 7 ]、[ 8 ]。在这里,采用两阶段的方法,前端快速扫描的位姿估计,和用于在后台或远程计算机上进行的较慢的后端建图步骤。从雷达扫描对准的位姿估计不直接纳入车辆的控制回路,因此他们只在低速行驶。

在[ 9 ]和[ 10 ]中描述了移动机器人上使用的其他前端系统。在本文的对比,他们没有提供完整的六自由度位姿估计和开源软件。

使用扫描匹配进行定位的工作始于ICP[ 11 ],它起源于注册三维点云的一般方法。许多基于ICP的方法的主要缺点是对点对应的高复杂度搜索,这必须在每次迭代中进行。极坐标扫描匹配(PSM)[ 12 ]避免了利用激光扫描的自然极坐标系统来估计它们之间的匹配的对应搜索。扫描进行预处理,可用于极性扫描匹配。实时相关扫描匹配方法[ 13 ]采用穷举抽样方法进行扫描匹配。通过多种优化,这种方法能够实时应用。基于正态分布变换(NDT)[ 14 ]的扫描匹配将扫描对齐到代表前扫描的正态分布混合。

对于沿海的情况,有研究使用昂贵的多传感器扫描仪[ 15 ],但据笔者的知识,没有单发射器激光雷达为基础的SLAM方法,可在现实世界中的条件下进行测试。

III.系统概述

相对许多其他基于网格的2D-SLAM来说,本文提供了一种可用的,具有完整的6自由度运动的平台。我们的系统可以预测6自由度的平移和旋转状态。为了实现这一点,我们的系统由两个主要部分组成,导航滤波器融合了来自于惯性传感器和其他可用的传感器到一个可用的3D数据,而2D SLAM则提供平面的位姿信息。这两部分的更新都是单独的,是松耦合系统,他们会定时保持同步。

我们定义导航坐标系统是一个右手系统,它的原点在平台的起点上,Z轴指向上方,X轴在启动时指向平台的偏航方向。完整的3D状态表示为:,其中是欧拉角的roll、pitch和yaw,是位置和速度在导航坐标系中的表示。

惯性的测量构成了输入矢量,其中角速度矢量以及加速度矢量。由非线性微分方程描述刚体运动:

 (1)

(2)

(3)

是映射自身坐标系到导航坐标系的方向余弦矩阵,是欧拉角到角速度的导数,g是固定重力向量[16]。当使用低成本传感器时,通常会忽略由于地球自转而产生的伪力的影响。

由于传感器噪声,速度和位置表现出明显的漂移。因此,必须进一步融合传感器信息。在本文中,这个信息是通过扫描匹配,非常适合用在室内场景。其他可能的来源是用于航向信息的磁场传感器或用于海拔高度估计的气压传感器。如果可用,odom航迹信息可以用来提供速度的测量。在室外场景中,卫星导航系统通常被用作辅助系统以防止惯性导航解决方案漂移[ 17 ]。

根据不同的平台,系统方程可以引入更多的约束,从而化简系统状态空间。

IV.2D SLAM

能够代表任意环境的栅格地图的使用,这是在现实世界环境中,利用激光雷达的移动机器人定位的一种行之有效的方法[18]。由于雷达平台可能具有6自由度运动,扫描已被转化成一个估计位姿在局部稳定的坐标系。使用估计的方位和联合值,扫描被转换成扫描端点的点云。

根据具体情况,这个点云可以进行预处理,例如对点数进行降采样或移除无效点。对于所提出的方法,只使用基于端点z坐标的滤波,以便在扫描匹配过程中仅使用预期扫描平面的阈值内的端点。

A.Map Access

FIG.2.  (a) 占用栅格地图的双线性滤波。点是其值应插值的点。(b) 占用栅格地图和空间导数。

占用栅格地图的离散性限制了可以实现的精度,也不允许直接计算插值值或导数。出于这个原因,一个插值方案允许通过双线性滤波子网格单元的精度估计占用概率和衍生工具。直观地说,网格映射单元格值可以看作是底层连续概率分布的样本。

给定一个连续的地图坐标,占用值以及梯度可以像FIG.2(a)一样用四个临近点近似,沿着x轴和y轴进行线性插值,然后得到:

(4)

导数可以近似为:

(5)

(6)

需要注意的是,地图的采样点、栅格单元位于单位距离为1的规则网格上,从而简化了梯度近似方程。

B.Scan Matching

扫描匹配是将激光雷达扫描与现有地图对齐的过程。现代激光扫描仪具有低测量噪声和高扫描速度。由于这种原因,注册扫描的方法可能会产生非常精确的结果。多机器人系统的激光扫描仪的精度和精度比里程计数据高很多,如果在所有。第VI部分显示的例子,对为什么不可用odom里程计数据提出了评价。

迄今为止,我们的方法是基于带有地图记忆的端点对齐的优化方法。根据计算机视觉工作带来的启发,我们的基本思路是使用高斯-牛顿法。使用这种方法,不需要在激光端点之间进行数据关联搜索或穷举搜索。当扫描与现有地图对齐时,匹配将通过所有前面的扫描隐式进行。

我们寻求一个刚性变换的函数最小值:

(7)

也就是说,我们需要找到一个转换,使得激光扫描与地图有最佳的对齐。在这里,是扫描端点的世界坐标。有一个函数,可以表示机器人在世界坐标系中的坐标:

(8)

函数返回给出的地图坐标值。鉴于一些的的初始估计,我们要估计,它根据以下方程去优化测量误差:

(9)

我们对做一阶泰勒展开得:

(10)

通过将的偏导数设置为0而使以下方程最小化:

(11)

得到高斯-牛顿方程的最小化问题:

(12)

其中:

(13)

对地图梯度的近似是在第IV-A提到。用方程(8)我们得到:

(14)

可以解出高斯-牛顿方程(12),得到一个接近最小值。重要的是要注意,该算法适用于非光滑线性逼近的地图梯度,这意味着局部二次收敛到最小不能保证。然而,该算法在实践中具有足够的精度。

在许多应用中,高斯近似的匹配不确定性是可取的。实例参数过滤器的更新以及使用图优化SLAM后端构成约束。一种方法是使用基于抽样的协方差估计,取样不同的位姿估计接近扫描匹配的位姿,并从这些构造协方差。这类似于无迹卡尔曼滤波的想法[ 20 ]。第二种方法是使用近似Hessian矩阵得到协方差估计。这里,协方差矩阵近似于:

 (15)

其中是一个比例因子,取决于激光雷达的性能。全面的推导可在[ 21 ]中获得。

C.地图的多重分辨率表示

任何爬山法或基于梯度的方法有其陷入局部极小的风险。由于所提出的方法是基于梯度上升,它也很容易陷入局部极小。使用计算机视觉中常用的类似于图像金字塔方法在多分辨率地图中表示会得到缓解。在我们的方法中,我们可以选择使用多个占用栅格地图,每个粗糙的地图具有前一个分辨率的一半。然而,多个地图的等级并不是从一个单一的高分辨率地图生成(采用高斯滤波和降采样等在图像处理中常见的做法)。相反,不同的映射被保存在内存中,同时使用对齐过程产生的位姿估计进行同步更新。这种生成方法确保地图是一致的尺度同时避免昂贵的下采样操作。扫描定位过程是在粗糙的地图的等级开始,由此估计的姿态获得作为下一级开始估计,类似于在[22]。带来的一个好处是粗粒度地图的即时可用性,例如可以用于路径规划。

Fig.3. 多分辨率地图:(a)20cm网格(b)10cm网格(c)5cm网格

V.3D状态估计

本节包括第III节的全三维状态向量估计和2D SLAM。导航滤波器以100赫兹的恒定速率实时运行,并在扫描匹配位姿和其他传感器信息一到达时就同步更新。其他传感器的融合除了扫描匹配位姿以外都不在本文的范围内。具体实现细节可在[23]中找到。

A.导航滤波器

为了估计平台的6D位姿,我们采用了带有通用模型(方程(1)-(3))的扩展卡尔曼滤波器(EKF)。此外,随着加速度随时间的变化,陀螺仪和加速度计的偏差增大了状态向量,并对结果产生显著影响。请注意,由于矩阵中的欧拉角项,系统方程是非线性的,因此必须使用非线性滤波器。惯性测量被认为是已知的系统输入。

速度和位置更新是测得的加速度的一个纯积分,如果没有通过测量更新的额外反馈,系统将是不稳定的。防止状态估计无限增长的通常对策是在方差达到一定阈值时,伪零速度更新,否则不能保证稳定性。在本文中,在平面的二维位姿定位是通过扫描匹配更新,而全3D的位姿估计需要额外的高度气压计和距离传感器。

B.SLAM整合

为了获得最佳性能,2D SLAM解决方案和3D EKF估计之间的信息必须在两个方向上进行交换。系统不同步,EKF通常以较高的更新速率运行。为了提高扫描匹配过程的性能,EKF的位姿估计投影在XY平面,用于扫描匹配优化过程的估计初始化。另外,估计的速度和角速率也可以被整合以提供扫描匹配的估计初始化。

在相反的方向,协方差交叉(CI)是用来融合SLAM位姿与信念状态[24]。一个简单的卡尔曼测量更新会导致过于自信的估计,因为它假设统计独立的测量误差。

我们将扫描时的卡尔曼估计表示为,协方差为P。还有,方程(7)和(15)中的SLAM位姿为。融合结果有:

(16)

以及:

(17)

利用观测矩阵将整个状态空间投影到SLAM的3维子空间中。参数用来调整SLAM的更新效果。

类似于卡尔曼滤波和信息滤波之间的二元性,协方差交叉也可以用它的协方差形式来写:

(18)

以及:

(19)

其中:

(20)

根据(16)和(17)的全状态协方差的推理在计算上的开销是很大的,这是最合适的方法。

VI.结果

我们展示了三种不同用例的通用性和健壮性的例子。应该指出的是,用于基准测试的SLAM方法缺乏6自由度运动,考虑到我们的方法以及激光雷达的高更新率,它利用标准数据集。为此,我们提供ROSBAG格式的数据集进行测试。

Fig.4.平台:(a)RoboCup搜救大赛中的Hector UGV(b)整合的建图系统,可以手持建图(c)UGV建图系统装在一个USV平台上

A.USAR场景

所提出的方法是使用无人地面车辆进行自主探索和城市搜索和救援(搜救)受害者检测场景RoboCup救援机器人联赛。Hector UGV系统如Fig.4(a)所示。激光雷达稳定在滚转和俯仰轴周围,以保持激光与地面一致,最大限度地提高平面激光雷达扫描的信息增益。在Fig.5(a)和Fig.5(b),我们的方法能够学习高度准确和一致的地图,尽管在UGV系统自由度状态的显著变化。视频显示在机器人世界杯2010竞技场救援现场的在线地图以及本节中的其他实验可在线查阅。

B.沿岸场景

测试在沿海水域由于植被茂密,GPS中断频繁,Hector UGV的定位系统安装在一个控制系统(Fig.4(c))。没有对USV系统改造的必要性,完整的SLAM系统安装和运行了不到30分钟。在维吉尼亚的克莱特湖,部分日志文件记录和用于学习的环境地图没有GPS数据,还是没有具体的运动模型的车辆,这都将需要更紧密的集成和较长的准备时间。如Fig.5所示,我们的方法可以在GPS中断的情况下为导航提供足够精确的地图。因为它不使用提取的特征线,它在沿海植被提供了高度的非结构化环境中工作。应该指出的是,用银行utm-30lx雷达不返回有效的距离测量,当电子束击中水,这是优势,利于预防错误的测量影响的SLAM系统。一旦GPS购回,一个基于图形的SLAM方法可用于优化提供的姿势图扫描匹配通过添加GPS构成GPS停电前和额外的绝对位置约束后取得。这是今后工作的主题。需要指出的是,使用hokuyo UTM-30LX激光雷达,有时激光束照射在水面上不返回有效测量时,是有利于防止错误测量对SLAM的影响的。一旦GPS信号恢复,一个基于图形的SLAM方法可以用于优化扫描匹配(通过GPS中断之前和之后的GPS位姿作为额外的位置约束)提供的位姿图。这将是今后的工作方向。

C.嵌入式建图系统

FIg.4(b)中展示了嵌入式建图系统。它由一个hokuyo UTM-30LX激光雷达系统,基于英特尔Atom Z530 CPU板以及一个小型低成本MEMS IMU组成。如此,该系统为所提出的SLAM方法的应用提供了所有必要的传感器。它可以很容易地安装在无人驾驶的车辆上,也可以用手携带来学习环境地图。Fig.6(a)显示了通过在地面实况地图上通过机器人在RoboCup2011救援竞技场行走的地图。Fig.6(b)显示了在德国Dagstuhl castle的一个新建筑中的建图。从路径和映射中可以看出,该系统足够精确地关闭在小规模场景中通常遇到的闭环,而不使用显式循环闭包的方法,保持计算要求低,并防止运行期间对估计地图的更改。视频和ROS记录文件已经上线。记录文件可以在atom z530上以3倍速度运行而没有质量损失。因此,以SLAM使用的设置,消耗不到嵌入式建图系统的全部计算资源的一半。

Fig.5车辆实验:(a)RoboCup2010上的地图(b)0.25m精度的栅格地图覆盖在卫星照片上

Fig.6真实环境的手持建图(a)RoboCup2011搜救环境(多个小闭环)(b)Schloss Dagstuhl的一个新建筑(大闭环)

VII.结论

本文提出了一种灵活的、可伸缩的SLAM方法,可用于多种场景。我们展示的适用性不同的情况下如模拟城市搜索和救援,在USV以及手持嵌入式系统沿海建图。我们表明,该系统正确估计位姿,并整合所使用的激光雷达系统的三维运动,同时消耗低计算资源。该系统现在作为开源软件提供。

据作者所知,高更新率,同时在二维绘图和低功耗平台运行的六自由度位姿估计的组合是不可用的开放源码软件。

VIII.感谢

德国研究基金会(DFG)RTG1362“混合模式环境中的协作、自适应和响应监视”。我们感谢弗吉尼亚理工大学电气与计算机工程系的Dan Stilwell和Aditya Gadre,布拉德利,给我们机会来执行克莱特湖USV实验。感谢Christian Lindig博士提供Dagstuhl floor图。

Hector SLAM解读(1)原文翻译的更多相关文章

  1. rplidar & hector slam without odometry

    接上一篇:1.rplidar测试 方式一:测试使用rplidar A2跑一下手持的hector slam,参考文章:用hector mapping构建地图 但是roslaunch exbotxi_br ...

  2. 【ros】rplidar Hector Slam

    想用rplidar跑一下hector slam,在网上发现了几个教程写的都不错,但是亲测发现都有点不足,综合了一下,进行补充. 1. 安装ros 和 创建工作空间 http://blog.csdn.n ...

  3. ASP.NET 生命周期(原文翻译)

    在网上看到这篇文章,老外写的,里面很多图片挺精致,顺带翻译过来给大家分享下,英语太次好多地方都翻不过来 ASP.NET application and page life cycle Download ...

  4. 【特征匹配】BRISK原文翻译

    原文:Stefan Leutenegger, Margarita Chli et al.<BRISK: Binary Robust Invariant Scalable Keypoints> ...

  5. 《The java.util.concurrent Synchronizer Framework》 JUC同步器框架(AQS框架)原文翻译

    一.论文简介 闲来无事,看看源码,发现了一篇JDK作者的论文<The java.util.concurrent Synchronizer Framework>主要描述了作者对Abstrac ...

  6. Spark Streaming + Kafka Integration Guide原文翻译及解析

    前面写了关于kafka和spark streaming的结合使用(https://www.cnblogs.com/qfxydtk/p/11662591.html),其具体使用用法其实来自于原文:htt ...

  7. 【Hector slam】A Flexible and Scalable SLAM System with Full 3D Motion Estimation

    作者总结了SLAM前端和后端的区别 While SLAM frontends are used to estimate robot movement online in real-time, the ...

  8. SLAM for dummies中文翻译

    1.简介 本文的主要目的是简单介绍移动机器人领域中广泛应用的技术SLAM(同步定位与地图绘制)的理论基础以及应用细节.虽然目前存在很多关于SLAM技术的方方面面的论文,但是对于一个新手来说,仍然需要花 ...

  9. BootStrap 4正式版发布(原文翻译)

    原文链接:http://blog.getbootstrap.com/2018/01/18/bootstrap-4/ 关于Bootstrap 什么是Bootstrap Bootstrap,来自 Twit ...

随机推荐

  1. H.265视频编码与技术全析(下)

    H.265视频编码与技术全析(下) 四.帧内预测模式 共35个(h264有9个),包括Planar,DC,33个方向模式: 除了Intra_Angular预测外,HEVC还和H.264/MPEG-4 ...

  2. 开放式神经网络交换-ONNX(上)

    目的 本文档包含ONNX语义的规范性规范. "onnx"文件夹下的.proto和.proto3文件构成了用协议缓冲区定义语言编写的语法规范..proto和.proto3文件中的注释 ...

  3. Kafka源码分析(三) - Server端 - 消息存储

    系列文章目录 https://zhuanlan.zhihu.com/p/367683572 目录 系列文章目录 一. 业务模型 1.1 概念梳理 1.2 文件分析 1.2.1 数据目录 1.2.2 . ...

  4. pytest 给用例打标签

    1.给用例添加自定义标签命令:@pytest.mark.tagname     #tagname是自定义的标签名 import pytest class TestClass(): @pytest.ma ...

  5. 三、部署被监控主机-Zabbix Agent

    三.部署被监控主机-Zabbix Agent 1) 源码安装Zabbix agent软件 在2.100和2.200做相同操作(以zabbixclient web1为例). [root@zabbixcl ...

  6. 《Learning to warm up cold Item Embeddings for Cold-start Recommendation with Meta Scaling and Shifting Networks》论文阅读

    <Learning to warm up cold Item Embeddings for Cold-start Recommendation with Meta Scaling and Shi ...

  7. Redis不是只有get set那么简单

    我以前还没接触Redis的时候,听到大数据组的小伙伴在讨论Redis,觉得这东西好高端,要是哪天我们组也可以使用下Redis就好了,好长一段时间后,我们项目中终于引入了Redis这个技术,我用了几下, ...

  8. 【题解】Luogu P3110 [USACO14DEC]驮运Piggy Back

    [题解]Luogu P3110 [USACO14DEC]驮运Piggy Back 题目描述 Bessie and her sister Elsie graze in different fields ...

  9. NOIP模拟测试29「爬山·学数数·七十和十七」

    爬山题解不想写了 学数数 离散化然后找到以每一个值为最大值的连续子段有多少个,然后开个桶维护 那么怎么找以每一个值为最大值的连续子段个数 方法1(我的极笨的方法) 考试时我的丑陋思路, 定义极左值为左 ...

  10. ORA-09817:Write to audit file failed

    ERROR: ORA-09817: Write to audit file failed. --这种错误基本是磁盘空间满了 问题描述:sqlplus / as sysdba登不上,报上述ora-098 ...