论文翻译：Keyframe-based Stereo Visual-inertial SLAM using Nonlinear Optimization

Keyframe-based Stereo Visual-inertial SLAM using Nonlinear Optimization

摘要：

精确度对于自主机器人来说是非常重要的。 在这项工作中，我们提出了一种新颖的视觉惯性SLAM，它结合了立体摄像机和IMU，构建了稀疏地图，并准确地估计了摄像机的姿态。 通过非线性优化将相机和IMU数据紧密耦合。 预积分用于集成旋转、速度和姿势矩阵。 在特征提取、关键帧选择、关键帧选择、循环闭合等方面都采用了一些重要的技术。 此外，该系统还可以在标准计算机上实时运行。 系统定位精度可以达到厘米级，特别是在大范围环境下，系统具有较强的鲁棒性。 我们在公开数据集上提升了系统，以比较其他视觉惯性SLAM方法；我们的系统获得了更好的准确性和鲁棒性。

1.介绍

自主式机器人在学术界、工业界、消费界等交通领域受到广泛关注。 当机器人在环境中自主运动时，精确定位和测绘是其关键技术。 同时定位与测绘(SLAM)[1]是目前智能无人系统领域的一个研究热点，其目标是在未知环境中基于传感器数据实时生成周围地图。 由于其在鲁棒性方面的优势，SLAM在无人驾驶飞行器、移动机器人、自动驾驶汽车以及自主水下机器人等定位和导航领域享有极高的人气。
视觉惯性同时定位与测绘(VI-SLAM)融合相机和IMU数据进行定位和环境感知已成为主流。 视觉惯性系统可以通过相机和IMU来实现，这是最便宜和最小的传感器融合类型。 然而，大多数视觉惯性系统只能提供弹道信息，不能构建地图。 此外，由于缺少闭合回路，即使在再入位置，大多数系统的漂移也会继续。 最后但并非最不重要的一点是，针对大规模环境的准确和健壮的SLAM仍然是一个挑战。 特别是当机器人工作在复杂的环境中时，通过车载传感器获取精确的位姿和比例显得尤为重要。
为此，本文提出了一种新颖的基于立体摄像机和IMU的微型飞行器(MAV)视觉惯性SLAM方法，该方法构造稀疏特征图，精确估计摄像机姿态。 相机和IMU数据紧密耦合，采用非线性优化方法进行状态估计。 通过最小化加权摄像机投影误差和IMU误差来优化当前帧姿态。 整个系统由跟踪、映射和循环闭合三个线程组成。

2.相关工作

目前，相机和IMU的集成SLAM一般分为基于滤波和基于优化的两种方式。在历史上，视觉惯性位姿估计问题一直是通过滤波来解决的。 Mourikis和Roumeliotis[3]提出了一种基于单目摄像机和IMU的EKF滤波器的融合SLAM算法，在大规模场景中进行特征的线性计算和精确的位姿估计。 然而，这项工作缺乏考虑强旋转和弱纹理场景下的定位。 此外，Bloesch等人。 [4]直接使用自适应亮度误差从图像匹配中获得跟踪，并利用FAST角点识别候选特征区域。 如果他们的工作提供了闭合环路，则可以提高精度。
此外，方等人还提出了一些新的结论。 [5]为移动增强和虚拟现实提供了松耦合的视觉-惯性融合，并生成了一个自适应滤波框架来平衡抖动和延迟现象。 但是，由于融合类型的原因，传感器的信息没有得到充分的利用。 最近，Sun et al.。 [6]介绍了一种采用立体摄像机和IMU的视觉惯性里程计。 他们的工作采用FAST角点[7]来提高速度，并通过光流跟踪特征，但在初始化方面存在弱点。

近年来，针对这一问题提出了更多的基于优化方法的研究。Leutenegger等人。 [8]提出了一种基于关键帧的视觉惯性SLAM算法，并将IMU误差项和重投影误差项组合成一个代价函数对系统进行优化。 然而，在复杂的环境下，它的跟踪会失败。 此外，Falquez 等人主张将RGBD相机和IMU集成在一起，以实现密集的视觉里程计。
然而，由于RGBD相机易受大干扰的影响，特别是在大范围的场景和户外环境中，RGBD相机不能用于精确定位。 Ling等人。 [10]介绍了一种基于边缘对齐的视觉惯性方法。 采用基于滑动窗口优化的框架对视觉测量和惯性测量进行融合。 不幸的是，在大规模场景中无法获得相同的定位效果。 此外，Vidal 等人也提出了自己的观点。 [11]在弱光、高动态场景下，采用事件摄像机进行视觉惯性里程计，但系统计算量很大。

本文的其余部分组织如下。 第3节描述了符号和定义。 第四节介绍了系统概况，第五节给出了实验结果，第六节给出了结论和下一步工作。

3.符号和细节

本文的符号和细节如下。 (·)c是摄像机坐标系。 (·)b是与IMU对齐的主体坐标系。 (·)w为世界坐标系，重力方向与z轴方向一致。 采用经典针孔相机模型[12]将相机框架下的3D空间点XC∈R3变换为图像平面下的2D点XC∈Ω⊂R2，构造投影函数π：R3∈Ω：

$\pi\left(\mathrm{X}^{c}\right)=\left[\begin{array}{c} f_{u} \frac{X^{c}}{Z^{c}}+c_{u} \\ f_{v} \frac{Y^{c}}{Z^{c}}+c_{v} \end{array}\right]$π(Xc)=[fu​ZcXc​+cu​fv​ZcYc​+cv​​]

其中 $X^{c}=\left[X^{c} Y^{c} Z\right],\left[f_{u} f_{v}\right]^{T}$Xc=[XcYcZ],[fu​fv​]T 是相机的焦距 ， $\left[c_{u} c_{v}\right]^{T}$[cu​cv​]T 是相机的投影中心.

IMU包含一个三轴加速度计和一个三轴陀螺仪，其中a(T)代表IMU加速度，w(T)代表IMU角速度，ba(T)和bg(T)分别代表加速度计偏置和陀螺偏置。
加速度计和陀螺仪测量可以计算为：

$_{m e a} w^{b}(t)=w^{b}(t)+b_{g}(t)+\eta_{g}(t)$mea​wb(t)=wb(t)+bg​(t)+ηg​(t)

$_{mea} a(t)=\mathrm{R}^{\mathrm{T}}(t)(a(t)-g)+b_{a}(t)+\eta_{a}(t)$mea​a(t)=RT(t)(a(t)−g)+ba​(t)+ηa​(t)

其中 $_{m e a} w^{b}(t)$mea​wb(t) 和 $_{m a a} a^{b}(t)$maa​ab(t) 分别表示加速度计和陀螺仪的测量值. $w^{b}(t)$wb(t) 和 $a^{b}(t)$ab(t)分别是IMU加速度计和陀螺仪的实际值。 IMU测量由偏差和噪声发出，但可以在IMU校准期间获得。

$\dot{b}_{g}(t)=\eta_{b_{8}}(t) \quad \dot{b}_{a}(t)=\eta_{a g}(t)$b˙g​(t)=ηb8​​(t)b˙a​(t)=ηag​(t)

其中 $\eta_{8}(t), \eta_{a}(t), \eta_{b g}(t),$η8​(t),ηa​(t),ηbg​(t), 和 $\eta_{a g}(t)$ηag​(t) 服从零均值高斯分布.值得注意的是，相机和 IMU 之间的坐标变换可以表示为 $T_{c b}=\left[\mathrm{R}_{c b} | \mathrm{p}_{c b}\right],$Tcb​=[Rcb​∣pcb​],它可以通过多传感器校准系统[13，14]预先获得。

4.系统概述

本部分设计了立体视觉惯性系统，实现了立体摄像机与IMU的紧密耦合。 这项工作贡献有系统优化、特征提取、关键帧选择和环路闭合。

4.1紧耦合

本工作建立了基于ORB-SLAM2的视觉惯性SLAM系统[15]。 ORB-SLAM2是一个开源的视觉SLAM系统，实时摄像机定位具有明显的旋转不变性，丢失的跟踪可以恢复。 受此方法启发，本文利用EPnP[16]对摄像机轨迹进行估计。 初始化时采用奇异值分解(SVD)[17]求解庞大矩阵。 此外，为了提高计算速度和性能，采用了多线程方式。 本文采用五点RANSAC优化方法进行翻译估算。
相机和IMU数据紧密耦合以估计姿态。 本文从机器人运动学的角度出发，计算了IMU的转动、平移和速度，以及IMU偏差的误差项。 系统的15维状态变量表示为：

$\mu_{i}=\left[R_{i}, p_{i}, v_{i}, b_{a i}, b_{g i}\right] \in \mathrm{R}^{15}$μi​=[Ri​,pi​,vi​,bai​,bgi​]∈R15

通过最小化加权特征投影误差和IMU误差来优化当前帧的姿态。 姿态估计问题进一步转化为联合优化问题来估计状态：

$\mu=\underset{\mu}{\arg \min }\left(\sum r_{u}^{I}(\xi)+\mathrm{E}_{\mathrm{IMU}}(i, j)\right)$μ=μargmin​(∑ruI​(ξ)+EIMU​(i,j))

其中$\Sigma r_{u}^{1}(\xi)$Σru1​(ξ)和 $\mathrm{E}_{\mathrm{IMU}}(i, j)$EIMU​(i,j)分别是相机和IMU的误差方程。 采用高斯-牛顿算法对整个方程进行优化。 $\Sigma r_{u}^{\mathrm{I}}(\xi)$ΣruI​(ξ) 是受到ORB-SLAM2的启发。 值得注意的是，以左摄像机帧号为标准，在第一帧之前的MU数据会被删除。

这项工作使用预积分来积分旋转、速度和姿态矩阵。 IMU数据预积分结果表示为

$\begin{array}{c} R(t+\Delta t)=\operatorname{Exp}\left(\left(_{\text {mea}} w(t)-b_{g}(t)-\eta_{g d}(t)\right) \Delta t\right) \\ v(t+\Delta t)=v(t)+R(t)\left(_{\text {mea}} a(t)-b_{a}(t)-\eta_{a d}(t)\right) \Delta t \\ p(t+\Delta t)=p(t)+v(t) \Delta t+\frac{1}{2} g \Delta t^{2}+\frac{1}{2} R(t)\left(_{\text {mea}} a(t)-b_{a}(t)-\eta_{a d}(t)\right) \Delta t^{2} \end{array}$R(t+Δt)=Exp((mea​w(t)−bg​(t)−ηgd​(t))Δt)v(t+Δt)=v(t)+R(t)(mea​a(t)−ba​(t)−ηad​(t))Δtp(t+Δt)=p(t)+v(t)Δt+21​gΔt2+21​R(t)(mea​a(t)−ba​(t)−ηad​(t))Δt2​

两个关键帧之间的预积分表示为：

$R_{j}=R_{i} \prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(_{m e a} w_{k}-b_{g_{k}}-\eta_{g d_{k}}\right) \Delta t\right)$Rj​=Ri​k=i∏j−1​Exp((mea​wk​−bgk​​−ηgdk​​)Δt)$\begin{array}{c} \left.v_{j}=v_{i}+\mathrm{g} \Delta t_{i j}+\sum_{k=i}^{j-1} R_{k}\left(_{m e a} a_{k}-b_{g_{k}}-\eta_{g d_{k}}\right) \Delta t\right) \\ p_{j}=p_{j}+\sum_{k=i}^{j-1}\left[v_{k} \Delta t+\frac{1}{2} \mathrm{g} \Delta t^{2}+\frac{1}{2} R_{k}\left(_{m e a} a_{k}-b_{a_{k}}-\eta_{a d_{k}}\right) \Delta t^{2}\right) \end{array}$vj​=vi​+gΔtij​+∑k=ij−1​Rk​(mea​ak​−bgk​​−ηgdk​​)Δt)pj​=pj​+∑k=ij−1​[vk​Δt+21​gΔt2+21​Rk​(mea​ak​−bak​​−ηadk​​)Δt2)​

IMU的误差表示为：

$\begin{array}{c} \mathrm{E}_{\mathrm{IMU}}(i, j)=\rho\left(\left[\mathrm{e}_{R}^{T} \mathrm{e}_{v}^{T} \mathrm{e}_{p}^{T}\right] \sum_{I}\left[\mathrm{e}_{R}^{T} \mathrm{e}_{v}^{T} \mathrm{e}_{p}^{T}\right]^{T}\right)+\rho\left(\mathrm{e}_{b}^{T} \Sigma_{R} \mathrm{e}_{b}\right) \\ \mathrm{e}_{R}=\log \left(\left(\Delta R_{i j} \operatorname{Exp}\left(\mathrm{J}_{\Delta R}^{g} b_{g j}\right)\right)^{T} R_{i}^{b w} R_{j}^{b w}\right) \\ \mathrm{e}_{v}=R_{i}^{b w}\left(_{\mathrm{w}} v_{j}^{b}-_{\mathrm{w}} v_{i}^{b}-\mathrm{g}^{w} \Delta t_{i j}\right)-\left(\Delta v_{i j}+\mathrm{J}_{g}^{\Delta v} b_{g}^{j}+\mathrm{J}_{a}^{\Delta v} b_{a}^{j}\right) \\ \mathrm{e}_{p}=R_{\mathrm{BW}}^{i}\left(_{\mathrm{w}} p_{\mathrm{B}}^{j}-_{\mathrm{w}} p_{\mathrm{B}}^{i}-_{\mathrm{w}} v_{\mathrm{B}}^{i} \Delta t_{i j}-\frac{1}{2} \mathrm{g}_{\mathrm{w}} \Delta t_{i j}^{2}\right)-\left(\Delta p_{i j}+\mathrm{J}_{\Delta p}^{\mathrm{g}} b_{g}^{j}+\mathrm{J}_{\Delta p}^{a} b_{a}^{j}\right) \\ \mathrm{e}_{b}=b^{j}-b^{i} \end{array}$EIMU​(i,j)=ρ([eRT​evT​epT​]∑I​[eRT​evT​epT​]T)+ρ(ebT​ΣR​eb​)eR​=log((ΔRij​Exp(JΔRg​bgj​))TRibw​Rjbw​)ev​=Ribw​(w​vjb​−w​vib​−gwΔtij​)−(Δvij​+JgΔv​bgj​+JaΔv​baj​)ep​=RBWi​(w​pBj​−w​pBi​−w​vBi​Δtij​−21​gw​Δtij2​)−(Δpij​+JΔpg​bgj​+JΔpa​baj​)eb​=bj−bi​

其中ΣI是通过[18]的技术获得的信息矩阵的预积分形式。 ΣR是随机游走偏差，Ja(·)和Jg(·)是未显式计算预积分时偏置变化影响的一阶近似。 ρ是一个胡伯权函数。

4.2特征提取

特征检测是发现特征并确定其在图像中的位置。 SLAM系统具有多种特点。 Harris[19]、FAST[20]、ORB[21]和SIFT[22]主要用于VI-SLAM。 在兼顾性能和计算量的基础上，从两幅图像中提取ORB特征，使每幅左图像都能与右图像进行匹配。 这些特征被定义为(clu，clv，cru)，即左图像上的坐标(clu，clv)和右图像中的水平坐标。 在对图像进行直方图均衡化预处理后，提取ORB特征。 除非存在现有的2D-3D对应点，否则将使用更高的权重深度过滤器过滤掉异常点。 然后将要素添加到本地地图中。 图像特征提取和稀疏特征图如图1所示。此外，稀疏特征还可用于导航。

4.3关键帧选择

需要注意的是，视觉里程计系统在每一帧都运行，而地图构建和环路闭合检测程序仅在插入新关键帧时启动。当很长一段时间没有关键帧插入时，本地地图将会闲置，跟踪将变得脆弱。 在这项工作中，关键帧被压缩为特征描述符，占用较少的内存。

此外，当前帧和最后一个关键帧之间的间隔在固定阈值(例如0.5s)内，以保证系统精度。 当前帧相对于前一个关键帧的旋转在固定阈值内(例如，10°)。 当生成新的关键帧时，循环闭合线程提供词袋模型来查找可能重新访问地图的候选关键帧

4.4环路封闭

近年来，视觉惯性里程计技术取得了很好的效果，但这些方法缺乏检测环路的能力，即使机器人不断地重复访问同一位置，轨迹估计也会累积漂移。 本文提出了一种新颖的环路闭合方法，如图2所示。在环路闭合模块中加入了摄像机和IMU数据。 在连续环闭合后进行位置识别，然后利用SIM3对整个轨迹进行校正。 该系统嵌入了基于FBOW[23]的位置识别模块，用于重新定位、在跟踪失败的情况下或用于在已经映射的场景中进行初始化以及用于闭合循环。

相对简约的建筑风格，使形象在现实环境中更具重合性。 系统设置了合适的阈值，避免了误报，提高了系统的准确性。 例如，当前帧和关键帧之间的相似性是最新关键帧的三倍。 此外，本文还提出了一种循环闭合缓存机制。 已经确定多个帧的环路闭合是正确的，以消除由单个图片引起的失配。

5.实验

实验表明，该系统只有在GPU支持下才能实时运行，并且在摄像头频率为10 Hz时获得了较好的精度。 这些实验基于EURC数据集[24]，这是目前比较不同方法的最佳选择。
数据集安装在装有视觉惯性(VI)传感器的微型飞行器(MAV)上[25]。 VI传感器和MAV如图3所示。