里程计航迹推演与IMU预积分

参考:VINS代码注释版

预积分,难以理解的地方主要是,雅可比的计算、怎么通过迭代的方式得到协方差的、使用deltaBias更新预积分是什么鬼?
其实,这与计算轮式里程计在两帧图像之间的协方差过程类似,只不过里程计比较简单,使用航迹推演,再结合协方差传递律,就能得到,所以就很容易明白了,里程计中的航迹推演就对应IMU预积分中的使用中值法计算得到的两帧之间 PVQ 增量(注意,对速度和位移的预积分并没有去掉加速度测量值中的重力g),即预积分要算的东西,预积分说白了就是测量值,测量值都有noise,航迹推演的noise我们知道怎么算得,IMU预积分的noise也一样,只是稍微麻烦一点,用的一套误差运动学方程,最后可得到如下状态方程,
x ˙ = A x + B w , w ∼ N ( 0 , Σ ) \dot x = Ax+Bw, w \sim N(0, \Sigma) x˙=Ax+Bw,wN(0,Σ)

因为 A A A B B B 不是常矩阵,所以这是一个线性时变连续系统,学过控制理论的对这个公式应该很熟悉吧,因为采样周期很小,所以可以进行近似离散化,进而得到,
x ( k + 1 ) = ( I + δ t A ) x ( k ) + δ t B w ( k ) , w ( k ) ∼ N ( 0 , Σ k ) x (k+1)= (I + \delta tA)x(k)+\delta t Bw(k), w(k) \sim N(0, \Sigma_k) x(k+1)=(I+δtA)x(k)+δtBw(k),w(k)N(0,Σk)

注意,这里的 w ( k ) w(k) w(k) 已经变成了离散的噪声,关于IMU的连续噪声如何换成离散噪声用过Kalibr的都比较熟悉吧。为了得到从第i张图片到第i+1张图片之间对IMU预积分的协方差,协方差的传播律大家已经很熟悉了,那预积分的协方差就是对上式进行迭代计算。除此之外,对IMU预积分的时候为什么要计算一个雅可比矩阵,因为后面要在初值处进行泰勒展开(实际上只用到了对bias的雅克比)。每来一帧IMU数据,就递推一次雅可比矩阵和协方差矩阵,最后处理完最后一帧IMU数据后,预积分就完成了,协方差有了,然后就顺理成章开始优化了.
上面这块的实现在 IntegrationBase::midPointIntegration() 函数内

预积分的结果其实就是测量值,类似我们相机观测到的特征点的坐标、里程计的测量值等等,唯一的区别就是,预积分作为测量值本来是不会改变的,但是因为这个测量值里面还含有优化变量bias,这是因为,我们在预积分公式的推到时,将IMU相对世界坐标系(w)的位姿、速度都转换成相对上一帧图像时IMU的坐标系上去了(即,b_k),但是确没有bias做任何处理,所以bias就留下了(其实bias本身变化就很小,两帧图像之间只有微小的变化,不像IMU的位姿,每一时刻都有很大程度的不同),既然这个预积分得到含有优化变量bias,那么当bias改变了,我们自然就要重新计算预积分了,怎么就算了?一个直接的方式就是使用新的bias重新计算一次预积分,这种方法固然可行,但是这和我们当初为什么分离优化变量的初衷相违背(我们正是为了在优化变量更新后不重新计算预积分啊),那该怎么办呢,还记得这个bias的变化很小这个常识吗,很自然就想到使用泰勒展开(进行近似),泰勒展开需要雅可比啊(即,预积分对 k 时刻bias的梯度的转置),这个雅可比我们刚刚在预积分时就算过了啊,所以啊,预积分最后一层神秘的面纱已经揭开了^~^
上面这块的实现在 IntegrationBase::evaluate() 函数内

刚说到要开始优化了,IMU预积分的结果就是测量值,我们如何使用使用测量值进行优化呢?还记得我们如何使用相机对地图点的观测构造残差的的吗,这里也一样,其实我们在推导预积分的时候就已经得到残差了(公式5),只是我们没有写成残差的形式而已,需要进行移项,然后结果一目了然(公式24),然后就是优化的套路,如高斯牛顿法,给一个增量,通过泰勒展开分离,得到这个增量的线性方程,然后就构成了一个线性最小二乘,通过求解这个线性最小二乘,使目标函数最小,也就得到这个增量应该是多少,将这个增量加到上一次优化变量的值上,就得到了本次的优化变量的值,一直循环下去,就得到最终的优化变量的结果,因为进行泰勒展开时,需要使用雅可比,计算雅可比使用的是待优化变量的初值(为了收敛,必须使用初值),只有第一次优化时,计算雅可比使用的是初值,之后使用的上一次优化结果作为线性化点计算雅可比,因为需要反复更新雅可比矩阵,所以耗时是难免,那么有没有什么好的方法避免呢?SVO为了加速,采用inverse compositional formulation方式,只需要计算一次雅可比矩阵
上面这块的实现在 IMUFactor::Evaluate() 函数内


<完>
@leatherwang


你可能感兴趣的:(slam)