LR Tightly-coupled Fusion of GPS in Optimization-based VIO

Abstract

提出一种融合GPS和视觉, 惯性测量在一个非线性优化的估计器.

系统状态的一个滑窗的最近状态会被估计, 通过最小化重投影误差, 相对惯性误差和全局位置误差.

我们用IMU预积分来形成惯性误差.

我们的方法持续的比松耦合的融合方法好. 相对位置误差少50%.

1. Introduction

更准确的GPS系统, 比如差分GPS, 是可能的但是他们需要基站.

[3, 4] 里用pose-graph优化融合了GPS. 但是, 这样的系统是松耦合的, 说明相对位姿估计是用VIO算法估计的, 而只是用pose graph优化align到全局坐标下.

全局位置观测用来定义新的因子. 我们像[5]一样定义一个基于关键帧的滑窗优化, 主要的区别是加入了全局位置的因子, 状态量是不会有变化的.

因为全局误差项的框架, 计算量没啥变化, 比起VIO的情况.

Filtering Methods:

滤波的方法有高效的估计, 通过只估计最近的状态. 很多基于滤波的方法, 融合了视觉和惯性测量的, 都是被[8]inspired, 提出了EKF.

[10]在线标定了IMU-GPS的外参和时延.

Smoothing Methods:

被分类为了 full 或者是 fixed-lag smoothers.

2. Problem Formulation

A. Notation

我们假设重力方向和(z^w) 是一致的.

全局位置估计是用 (p_{p_k}^w)的, 这里(P) 是跟(B)的一个刚体, 用 (p_p^b). 距离, 点 (P) 可以表示接收器天线的位置. 这里 (p_p^b) 可以通过标定获得.

滑窗优化的变量是: (mathcal{X} ={mathcal{L}, mathcal{X}_B}).

B. Optimization-based Visual, Inertial, Global Information Fusion

cost function:

[$J_{V I}(mathcal{X})=sum_{k=0}^{K-1} sum_{j in mathcal{J}_{k}}left|mathbf{e}_{mathbf{v}}^{j, k} ight|_{mathbf{W}^{j, k}}^{2}+sum_{k=0}^{K-1}left|mathbf{e}_{mathbf{i}}^{k} ight|_{mathbf{W}_{i}^{k}}^{2}+left|mathbf{e}_{mathbf{p}} ight|^{2}$ ]

(e_v) 是视觉残差, (e_i) 是惯性残差, (e_p) 是边缘化残差?.

全局位置残差的推导是IMU预积分算法inspired.

误差项 (e_p) 表示从边缘化获得的prior information. 我们用了[5]里的边缘化策略. 即: 当一个新的帧插入了滑窗, 我们区分两种case.

滑窗中的最老帧不是关键帧, 它会被边缘化, 它的landmark会被扔了.
是的话, landmark也会被边缘化.

全局位置残差:

[$J(mathcal{X})=J_{V I}(mathcal{X})+sum_{k=0}^{K-1} sum_{j in mathcal{G}_{k}}left|mathbf{e}_{mathbf{g}}^{j, k} ight|_{mathbf{W}_{mathbf{g}}^{k}}^{2}$ ]

3. Derivation of Global Position Residuals

A. IMU Preintegration

我们用[17]提出的IMU预积分推导, 是基于持续时间的基于四元数的[18]和[7]中IMU偏置的操作.

IMU残差是用来约束连续状态,

用加速度计: (hat{mathbf{a}}_{t}=mathbf{a}_{t}+mathbf{b}_{a_{t}}+mathbf{R}_{w}^{t} mathbf{g}^{w}+mathbf{n}_{a})

用角速度计: (hat{mathbf{w}}_{t}=mathbf{w}_{t}+mathbf{b}_{w_{t}}+mathbf{n}_{w})

加速度计和陀螺仪的噪声是额外的高斯噪声:

加速度计: (mathbf{n}_{a} sim mathcal{N}left(mathbf{0}, sigma_{a}^{2} cdot mathbf{I} ight))

角速度计: (mathbf{n}_{w} sim mathcal{N}left(mathbf{0}, sigma_{w}^{2} cdot mathbf{I} ight))

偏置被model成随机游走:

加速度计: (dot{mathbf{b}}_{a_{t}}=eta_{b_{a}}); (oldsymbol{eta}_{b_{a}} sim mathcal{N}left(mathbf{0}, sigma_{b_{a}}^{2} cdot mathbf{I} ight))

角速度计: (dot{mathbf{b}}_{w_{t}}=eta_{b_{w}}); (oldsymbol{eta}_{b_{w}} sim mathcal{N}left(mathbf{0}, sigma_{b_{w}}^{2} cdot mathbf{I} ight))

给定时间段 ([t_k, t_{k+1}]) , (p, v, q) 会在这个时间段上用加速度计, 角速度计传播.

在世界系下传播需要知道初始位姿, 这也暗含每次初始状态改变, 比如在优化的时候更新, 需要重新传播.

IMU预积分的主要好处就是每次更新后不需要重新传播.

传播是在local frame (B_k) 上的, 而不是世界系:

[egin{aligned} mathbf{R}_{w}^{b_{k}} mathbf{p}_{b_{k+1}}^{w} &=mathbf{R}_{w}^{b_{w}}left(mathbf{p}_{b_{k}}^{w}+mathbf{v}_{b_{k}}^{w} Delta t_{k}-frac{1}{2} mathbf{g}^{w} Delta t_{k}^{2} ight)+oldsymbol{alpha}_{b_{k+1}}^{b_{k}} \ mathbf{R}_{w}^{b_{k}} mathbf{v}_{b_{k+1}}^{w} &=mathbf{R}_{w}^{b_{k}}left(mathbf{v}_{b_{k}}^{w}-mathbf{g}^{w} Delta t_{k} ight)+oldsymbol{eta}_{b_{k+1}}^{b_{k}} \ mathbf{q}_{w}^{b_{k}} otimes mathbf{q}_{b_{k+1}}^{w} &=gamma_{b_{k+1}}^{b_{k}} end{aligned} ]

B. Global Position Residuals

全局位姿观测由 ({ p_{p_j}^w }) 给出, 我们model测量的不确定性是额外的高斯噪声.

[hat{mathbf{p}}_{p_{j}}^{w}=mathbf{p}_{p_{j}}^{w}+mathbf{n}_{p} ]

给当前滑窗的一个状态 (x_k), 一个观测 (hat{p}_{p_j}^w) 在时刻 (t_j in [ t_k, t_{k+1})); 这个残差被定义为:

[mathbf{e}_{mathbf{g}}^{j, k}=mathbf{R}_{w}^{b_{k}}left(hat{mathbf{p}}_{b_{j}}^{w}-mathbf{p}_{b_{k}}^{w}-mathbf{v}_{b_{k}}^{w} Delta t_{k}+frac{1}{2} mathbf{g}^{w} Delta t_{k}^{2} ight)-hat{oldsymbol{alpha}}_{b_{j}}^{b_{k}} ]

这里测量 (hat{p}_{p_j}^w) 转换到 (hat{p}_{b_j}^w) :

[hat{mathbf{p}}_{b_{j}}^{w}=hat{mathbf{p}}_{p_{j}}^{w}-mathbf{R}_{b_{j}}^{w} mathbf{p}_{p}^{b} ]

with (mathbf{R}_{b_{j}}^{w}=mathbf{R}_{b_{k}}^{w} hat{gamma}_{j}^{k})

为了定义全局残差, 状态位置会有惯性测量在时间段 ([t_k, t_j]) 传播.

为了推导残差的权重 (mathbf{W}^k_g), 我们重写(7)作为:

[egin{aligned} mathbf{e}_{mathbf{g}}^{j, k}=& mathbf{R}_{w}^{b_{k}}left(-mathbf{p}_{b_{k}}^{w}-mathbf{v}_{b_{k}}^{w} Delta t_{k}+frac{1}{2} mathbf{g}^{w} Delta t_{k}^{2} ight) \ &-hat{oldsymbol{alpha}}_{b_{j}}^{b_{k}}+mathbf{R}_{w}^{b_{k}} hat{mathbf{p}}_{p_{j}}^{w}-mathbf{R}left(hat{gamma}_{j}^{k} ight) mathbf{p}_{p}^{b} end{aligned} ]

在上式中, (hat{oldsymbol{alpha}}_{b_{j}}^{b_{k}}, hat{mathbf{p}}_{p_{j}}^{w} ext { and } hat{oldsymbol{gamma}}_{j}^{k}) 都是带噪声的测量. 协方差 (hat{gamma}_j^k) 是基于gyro的噪声和偏置. 因为gyro的噪声在 (hat{alpha}_{b_j}^{b_k}) 已经考虑了, 而且一般比加速度计噪声小很多, 所以在(mathbf{W}_g^k) 的推导中忽视 (hat{gamma}_j^k) .

最终, 残差权重是基于 (hat{oldsymbol{alpha}}_{b_{j}}^{b_{k}}, hat{mathbf{p}}_{p_{j}}^{w})的

[mathbf{W}_{mathbf{g}}^{k}=hat{oldsymbol{alpha}} mathbf{P}_{b_{j}}^{b_{k}}+mathbf{R}_{w}^{b_{k}}left(sigma_{p}^{2} cdot mathbf{I} ight)left(mathbf{R}_{w}^{b_{k}} ight)^{t} ]

4. Experiment

A. EuRoC Dataset

立体图像是20Hz, IMU测量是200Hz.

Comparison to loosely-coupled:

5. Conclusion

没啥.