IMU预积分(二)

IMU预积分(二)——线性视觉惯导初始化

这篇文章紧跟IMU预积分(一),主要记录2012年那篇经典的预积分论文中的初始化部分。

1、以首帧位姿为参考坐标系

IMU预积分(一)中,我们推导出了一种将若干惯性测量值集成到载体坐标系下的方法,这种方法可在载体初始条件未知的情况下进行积分,得出的一个重要公式如下:
(1.1)pt2n=pt1n+(t2t1)vt1n+12(t2t1)2gn+Cbt1nΔpt2+t1vt2n=vt1n+(t2t1)gn+Cbt1nΔvt2t1ϕt2n=EulerFromDCM(Cbt1nΔCbt2bt1)
然而,当预积分的观测数据融合到导航框架下,这些初始条件仍然是必须的,正如公式(1.1)所描述的,ptanvt1nCbt1n在导航框架下仍是需要估计的。全球导航参考坐标系例如北京54坐标系等常用做惯性导航的导航坐标系,尤其有GPS观测时,在这个坐标系下重力是已知的,但还需要知道初始帧在导航坐标系中的位置、姿态和速度。对于大多数的SLAM应用,以起始帧作为导航坐标系就足够了,当没有绝对位姿观测时,甚至是必须的。下图展示了以首帧为参考坐标系的惯性导航:

在全局参考坐标系下,估计初始条件的主要问题或者难点来自旋转矩阵带来的非线性。若以第一帧为导航的参考坐标系,则载体的初始姿态就是完全确定的了(平移是0,旋转是单位矩阵),这消除了非线性的影响,然而,由于使用了方向任意的参考坐标系,在这个参考坐标系下重力是未知的。这种方法的优点在于对重力的估计是线性的,对初始条件中的旋转估计则不是。

就是说,以全局参考坐标系为导航坐标系时,重力是已知的,但是初始的位置、速度、旋转需要估计,其中对旋转的估计是非线性的;以第一帧为导航坐标系时,初始的位置、旋转是完全确定的,但可以用线性的方法估计初始速度、重力。

2、初始速度估计

重新排列公式(1.1)中的第一个公式,可以看出载体的初始速度是可以观测的:
(1.2)vt1n=pt1npt2n+Cbt1nΔpt2+t1+12(t2t1)2gn(t2t1)
因此,给定导航坐标系下的重力矢量以及另一个传感器(如相机)对两个位姿之间的相对位置估计,则平台的初始速度可以以线性的方式获取。可以看出,初始速度可观测的一个标准是能够估计出导航坐标系下的重力矢量。

3、重力矢量的估计

使用两个连续的位置预积分,一个速度预积分,需要的公式如下:
(1.3)pt2n=pt1n+(t2t1)vt1n+12(t2t1)2gn+Cbt1nΔpt2+t1pt3n=pt2n+(t3t2)vt2n+12(t3t2)2gn+Cbt2nΔpt3+t2vt2n=vt1n+(t2t1)gn+Cbt1nΔvt2t1
将速度方程带入第二个位置方程:
(1.4)pt3n=pt2n+(t3t2)(vt1n+(t2t1)gn+Cbt1nΔvt2t1)+12(t3t2)2gn+Cbt2nΔpt3+t2=pt2n+(t3t2)(vt1n+Cbt1nΔvt2t1)+Cbt2nΔpt3+t2+(t3t2)(12(t3t2)+(t2t1))gn
将公式(1.3)中第一个位置方程中的vt1n分离,带入公式(1.4)并从中分离出重力矢量:
(1.5)gn=pt2npt3n+(t3t2)((pt1npt2n+Cbt1nΔpt2+t1(t2t1))+Cbt1nΔvt2t1)+Cbt2nΔpt3+t2(t3t2)(12(t3t2)+32(t2t1))
这一推导表明,仅在三帧图像的相对位置被估计之后,就可以线性地获得载体的初始速度和重力矢量的估计值。重力矢量可观测带来的一个益处是,利用这个估计,可以提取出载体在导航系下的绝对横摇角和俯仰角。虽然此时我们有了一个重力矢量的估计,但是它并不准确,所以一些VIO在后端优化中也加入了对重力矢量的估计。VINS-mono对重力矢量的refine很巧妙,之后会记录。

听说打赏我的人,最后都找到了真爱。