【泡泡图灵智库】基于通用优化的多传感器全局位姿估计框架（arXiv）

会员服务 ·

【泡泡图灵智库】基于通用优化的多传感器全局位姿估计框架（arXiv）

2019 年 4 月 17 日 泡泡机器人SLAM

泡泡图灵智库，带你精读机器人顶级会议文章

标题：A General Optimization-based Framework for GlobalPose Estimation with Multiple Sensors

作者：Tong Qin, Shaozu Cao, Jie Pan, and Shaojie Shen

来源：arXiv 2019

编译： 夏友杰

审核：李鑫

欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

摘要

大家好，今天为大家带来的文章是——A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors，该文章发表于arXiv 2019。

准确的状态估计对自主机器人而言是基本问题。为了达到局部精确和全局无偏状态估计，通常多个具有互补特性的传感器会被结合使用。局部传感器（如相机，IMU，激光雷达等）可以在小范围内提供精准的位姿，而全局传感器（如GPS，磁力计，气压计等）可在大场景下提供有噪声的但全局无偏的定位。本文提出了一个传感器融合框架，通过融合局部状态和全局传感器来达到局部准确且全局无偏的姿态估计。局部估计由现有VO/VIO方案得到，并在姿态图优化中与全局传感器信息融合。在图优化中，局部估计会被对齐到全局坐标系中，同时累积误差会被消除。该系统性能通过公开数据集和实际环境实验进行评估，结果并与当前最先进的算法进行比较。特别强调的是该系统是一个通用框架，它可以比较容易地在一个统一的姿态图优化中融合不同的全局传感器。本文实现已经开源，参见 https://github.com/HKUST-Aerial-Robotics/VINS-Fusion。

主要贡献

1. 一个融合多种全局传感器和局部估计的通用框架，它可以达到局部准确和全局无偏定位；

2. 在公开数据集和现实实验中验证所提出的系统；

3. 面向社区开源代码。

算法流程

图1 本文提出框架结构示意。全局估计器通过融合局部估计与多种全局传感器以达到局部准确和全局无偏位姿估计。

1. 系统概览

根据测量值参考系的不同，可将传感器分为局部和全局两类。

1.1 局部传感器

相机，激光雷达，IMU（加速度计和陀螺仪）等。该类传感器并非提供全局参考值，通常需要一个参考系。总体来说，机器人的第一个位姿会被设为原点来启动传感器。而机器人位姿的估计则是出发点增量式地得到。因此，累计误差会随着与出发点的距离增长而增加。

1.2 全局传感器

GPS，磁力计，气压计等。这类传感器提供全局参考值。它们总是在一个固定的全局参考系中工作，即地球参考系。参考系的原点是固定的且可以提前知晓。该类传感器提供全局测量值，但伴随着噪声，且误差与运动距离是独立的。如GPS，它测量相对于地球的绝对经纬度和海拔。经纬度和海拔可以转化为xyz坐标。如磁力计，它测量磁场方向和强度，进而得到方向信息。如气压计，它可以测量气压，进而得到高度信息。

框架结构如图1所示。局部传感器（相机和IMU）被用于局部估计，采用当前已有的视觉里程计/视觉惯性里程计（VO/VIO）算法来得到局部位姿。局部结果和全局传感器将会作为全局位姿图的输入，它们将会被转化为统一的因子来构建优化问题。全局估计器将会得到局部准确和全局无偏的六自由度（6-DOF）位姿结果。

2. 算法流程

2.1 局部位姿估计

这里采用现有VO/VIO算法，VINS-Mono，来得到局部位姿估计，只要它们能够得到六自由度位姿即可。这部分并不是本文的主要贡献，因此在此简述。

VIO算法估计滑动窗口内的几帧IMU的位姿和特征点的深度，待估计状态定义如下：

其中，第k帧IMU状态x_k由IMU中心相对于局部参考系l的位置p，速度v，方向q，以及加速度计偏移b_a和陀螺仪偏移b_g。此处选取第一帧IMU位姿为参考帧。特征点由它们在第一个被观测到的图像帧中的逆深度\lambda参数化。估计问题可以表示为如下非线性最小二乘问题，

其中三项分别为来自上一次边缘化后的先验信息，惯性残差项和视觉残差项。VIO算法可以得到局部参考系下准确实时的六自由度位姿。

2.2 全局位姿图结构

全局位姿图结构如图2所示。每个节点表示世界坐标系下的一个位姿，包括位置和方向。节点的密度由最低频率的传感器决定。两个相邻节点间的边是一个局部约束，由局部估计(VO/VIO)得到。其他边表示全局约束，由全局传感器得到。

图2 全局位姿图结构示意

位姿图优化的本质是一个极大似然估计（Maximum Likelihood Estimation，MLE）问题。MLE由机器人一段时间内位姿的联合概率分布构成。假设所有测量概率是独立的，问题一般可以表示如下

其中，S为测量值集合，包括局部测量（VO/VIO）和全局观测（GPS，磁力计，气压计等）。假设观测的不确定性服从高斯分布，可以进一步得到

至此，状态估计问题已经被转化为一个非线性最小二乘问题，也被称为集束调整（Bundle Ajustment，BA）。

2.3 传感器因子

1) 局部因子

由于局部估计（VO/VIO）在较小范围内是准确的，可以利用两帧之间的相对位姿来构建局部因子如下：

其中，(q^l_{t-1}, p^l_{t-1})和(q^l_t, p^l_t)是VO/VIO算法得到的局部坐标系下t-1和t时刻位姿。

2) GPS因子

GPS原始测量量是经度，纬度和海拔，并不是xyz轴下坐标值。总体而言，可以将经纬度和海拔转化为ENU（local East North up）坐标，p^{GPS}_t，则GPS因子构建如下

3）磁力计因子

磁力计可以测量磁场强度，表示为一个向量。该向量的位置可以帮助确定世界坐标系下的方向。首先通过查表得到ENU坐标系下局部地区的磁场强度z^w，并假设该强度在当地是常数。测量值定义为z^m_t，而磁力计与ENU坐标系匹配后，z^m_t的方向应当与z^w的方向匹配。由此可得磁力计因子如下：

其中，q^m_b是机器人中心到磁力计中心的变换，可以通过离线标定得到。

4）气压计因子

气压计测量当地的气压。假设某高度的气压在一定时间内是常数，因此气压可以线性地转化为高度。定义高度的测量值为z^m_t，得到一个因子表示高度估计的残差，如下

5）其他因子

尽管本文只详细讨论了GPS，磁力计和气压计，其他全局传感器，比如运动捕捉系统，WiFi和蓝牙指纹均可以加入现有系统。关键点在于将这些测量建模为残差因子并置于同一全局坐标系下。

2.4 位姿图优化

一旦因子图构建完成，优化过程等价于找到节点的配置来尽可能多的匹配所有边的约束。本文采用Ceres Solver来求解非线性问题，它采用高斯牛顿法和LM法来迭代求解。

位姿图优化运行频率极低（1Hz），但每次优化完成后，都可以得到局部坐标系和全局坐标系之间的变换。因此，可以通过该变换将连续的高频局部位姿（VO/VIO，200Hz）转换为实时高频的全局位姿。

主要结果

本文提出的系统经过公开数据集和实际环境中的试验验证和评估。

所采用数据集为KITTI数据集，比较算法为ORB-SLAM，MSF。ORB-SLAM是一个基于优化的算法，它有强大的重定位功能。MSF是一种基于EKF框架的算法，融合了视觉里程计，惯性测量和GPS。

1. 数据集测试

图3 10_03_drive_0042数据序列上的旋转误差和平移误差。

图4 KITTI数据序列（ 09_30_drive_0033）上由ORB-SLAM和本文算法恢复得到的轨迹。

表1 KITTI数据集上的RMSE（单位：米）

2. 实地试验

图5 小型户外试验的相对位姿误差

表2 户外试验的RMSE（单位：米）

图6 大规模户外试验轨迹，分别由VIO和所提算法（VIO+GPS+磁力计+气压计）恢复。

Abstract

Accurate state estimation is a fundamental problem for autonomous robots. To achieve locally accurate and globally drift-free state estimation, multiple sensors with complementary properties are usually fused together. Local sensors (camera, IMU, LiDAR, etc) provide precise pose within a small region, while global sensors (GPS, magnetometer, barometer, etc) supply noisy but globally drift-free localization in a large-scale environment. In this paper, we propose a sensor fusion framework to fuse local states with global sensors, which achieves locally accurate and globally drift-free pose estimation. Local estimations, produced by existing VO/VIO approaches, are fused with global sensors in a pose graph optimization. Within the graph optimization, local estimations are aligned into a global coordinate. Meanwhile, the accumulated drifts are eliminated. We evaluate the performance of our system on public datasets and with real-world experiments. Results are compared against other state-of-the-art algorithms. We highlight that our system is a general framework, which can easily fuse various global sensors in a unified pose graph optimization. Our implementations are open source.

如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。

点击阅读原文，即可获取本文下载链接。

欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。

有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！

泡泡网站：www.paopaorobot.org

泡泡论坛：http://paopaorobot.org/bbs/