VSLAM：预积分公式推导(一)

VSLAM：预积分公式推导(一)

一、基本预备知识

1.1 预备知识：

 传统的递推算法是根据上一时刻的IMU状态量，利用当前时刻测量得到的加速度与角速度，进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中，各个状态量都是估计值，并且会不断调整，每次调整都会重新进行积分，传递IMU测量值。预积分的目的是将相对测量量与据对位姿解耦合，避免优化时重复进行积分。四元数的表示方法有两种：一种是Hamilton(右手系)表示，另一种是JPL(左手系)表示。读者对公式推导时一定注意。

1.2 IMU模型

 我们将以前文章中的IMU公式拿过来：

表示在body坐标系下(IMU坐标系)，随机游走及噪声不再进行解释。我们将图像帧记作

及

，body坐标系下记作

和

，我们将位置，速度和旋转在时间

到

内进行积分，其世界坐标系下的公式可以写为：

 注意这里的四元数虚部在前，实部在后，其中：

 旋转矩阵的推导：我们设

是单位四元数，

是由四元数确定的角速度，则单位四元数的导数可以用左乘及右乘来表示：

 所以：

1.3 离散表示

 我们以中值积分给出离散表示L:

二、IMU预积分

2.1 连续形式

 基本思想就是将参考坐标系从

转到第

帧的body坐标系下，相当于两边同时乘

，我们直接用论文中的公式来表示：

 其中：

 上述可以理解为

对

的相对运动量，其中

状态的改变并不会对其产生影响，可以将其作为非线性优化变量，避免重复计算。实际到这里，只要求解出积分，我们就完成了预积分的计算，我们的目标也就是在此。实际当中随机游走也是发生改变的，所以我们将上述变量再次进行一阶近似，我们再次使用论文中的公式进行表示：

2.2 离散形式

 我们同样以中值积分的形式给出：

 其中位置、速度初始值为0，旋转为单位四元数，噪声视为0。