简单理解VIO(三)--基于优化的IMU与视觉信息融合

文章目录

• • 一、视觉SLAM中的Bundle Adjustment问题
  • 二、最小二乘问题的求解
  • • 1、迭代下降法
    • • 1.1、最速下降法
      • 1.2、牛顿法
      • 1.3、阻尼法
      • 1.4、高斯牛顿
      • 1.5、LM
    • 2、鲁棒核函数
  • 三、VIO残差函数的构建
  • • 1、系统需要优化的状态量
    • 2、VIO中基于逆深度的重投影误差
    • 3、IMU测量值积分
    • 4、IMU预积分
  • 四、残差Jacobian的推导
  • 五、结论

一、视觉SLAM中的Bundle Adjustment问题

已知：

• 状态初始值，根据里程计大致位置，可以计算出机器人的位置与特征的位置。如： q = [0.96, 0, 0, 0.25], p = [1, 2, 0], f = [2, 4, 0].
• 观测量：图像观测到的特征的像素坐标 z = [100, 0];

目标函数：

最小化所有观测像素的残差：

a r g m i n Σ ∣ ∣ z i − π ( q , p , f ) ∣ ∣ arg min \Sigma ||z_i - \pi (q, p, f)|| argminΣ∣∣zi​−π(q,p,f)∣∣

这里的 π ( q , p , f ) \pi(q, p, f) π(q,p,f)是两个步骤，首先将特征点转换到局部坐标，然后把局部坐标转换到像素坐标，第二步的过程为：

x p i x = f x x z x_{pix} = f_x \frac{x}{z} xpix​=fx​zx​

相机局部坐标系为：前z，右x，下y，在二维情况中，y一直为0.

二、最小二乘问题的求解

关于泰勒展开式的理解： 泰勒展开通过一定阶数的表达式来代表（近似）一个复杂的函数。一般来说，求取泰勒展开式有两种：

• 已知复杂表达式，且复杂表达式可导，那直接通过一阶导数和二阶导数来求取。
• 当不知道复杂表达式，或者不可导时，可以通过在其上采样，通过采样点来求取泰勒展开。

在自动驾驶中，一般采用三次曲线来描述车道线，它其实就是现实生活中，复杂车道线的一个三次泰勒展开式。车身坐标系其实就是展开点。

一种比较简单的记忆泰勒展开式的方法：一次项使用直线，二次项使用抛物线，分别在0点处的情况来说明。

关于线性问题，如果是线性问题，其泰勒展开式，就可以描述其在整个可行解区间的近似，直接使用最小二乘解一次即可。

Hessian H的性质：当一阶导数为0时：

1. 如果H正定，特征值都大于0，则一阶导数单增，此处为最小值。
2. 如果H负定，特征值都小于0，则一阶导数单减，此处为最大值。
3. 如果H不定，特征值有正有负，此处不确定，为鞍点。

1、迭代下降法

下降法的初衷，就是找到一个方向，然后确定步长。正如下楼梯，如果简化为一个1维的问题，那就是超楼下指的方向就下降方向(-x), 然后确定使用线搜索确定步长。

1.1、最速下降法

梯度的负方向下降速度最快。也就是沿着切线方向。在最后容易震荡。适合一开始

用 y = sin ⁡ ( x ) y = \sin(x) y=sin(x)来理解。

1.2、牛顿法

求二阶最优，最稳定，但是速度慢，适合最后几步。

牛顿法，本质就是初中时学的，二次方程的根：对于方程 a x 2 + b x + c = 0 ax^2 + bx +c = 0 ax2+bx+c=0, 其最值在 x = − b 2 a x = -\frac{b}{2a} x=−2ab​, 其中 a = 1 / 2 ∗ H , b = J a = 1/2*H, b = J a=1/2∗H,b=J, 显然，这种解法不一定能找到得最小值，只适用于快要接近于最优值了。

1.3、阻尼法

增加一个阻尼因子，在二次项上增加，调节a的大小。

1.4、高斯牛顿

高斯牛顿法，本质就是F函数被表示为了一个二次型，即 F = f ∗ f F = f*f F=f∗f. 然后把牛顿法用上，就成了高斯牛顿，真简单。

1.5、LM

将阻尼因子动态调节，就成了列文伯格马夸尔特算法。

2、鲁棒核函数

三、VIO残差函数的构建

1、系统需要优化的状态量

滑动窗口中，待优化的变量包含：body的PVQ(位置速度姿态)，特征点的位置，角速度和角速度的偏置量。

2、VIO中基于逆深度的重投影误差

因为前面已经假设了二维的场景，相机坐标系下的feature与图像坐标系下的观测之间的关系为： u = f ∗ x z u = f*\frac{x}{z} u=f∗zx​, 因为是二维场景，所以相机的观测都在一条水平线上 v = f ∗ y z = 0 v = f*\frac{y}{z} = 0 v=f∗zy​=0. 实例检验：当x = 1m, z = 1m, y = 0m, f = 500, u = 500, v = 0;

残差的定义则为

r c = [ u − f ∗ x z v − f ∗ y z ] = [ u − f ∗ x z 0 ] r_c = [\begin{matrix}u - f*\frac{x}{z} \\ v - f*\frac{y}{z}\end{matrix}] =[\begin{matrix}u - f*\frac{x}{z} \\ 0\end{matrix}] rc​=[u−f∗zx​v−f∗zy​​]=[u−f∗zx​0​]

3、IMU测量值积分

我们的状态量，是怎么与IMU测量值发生关系的？前一时刻的PVQ，通过对IMU测量值积分，得到下一个时刻的PVQ。具体来说：P-通过假设匀加速直线运动模型；V-通过匀加速直线运动模型，Q-通过匀角速度运动模型。实例检验：角速度1°/s, 加速度1m/s^2, 速度1m/s, 时间t=0-1s; 初始状态： P 0 = [ 0 , 0 ] , V 0 = [ 0 , 0 ] , Q 0 = 0 ° P_0 = [0, 0], V_0 = [0, 0], Q_0 = 0° P0​=[0,0],V0​=[0,0],Q0​=0°。 经过一秒后， P 1 = [ 0.5 , 0.5 ] , V 1 = [ 1 , 1 ] , Q 1 = 1 ° P_1 = [0.5, 0.5], V_1 = [1, 1], Q_1 = 1° P1​=[0.5,0.5],V1​=[1,1],Q1​=1°$

写成通用形式：

q j = ∫ q t ⊗ [ 0 , 1 / 2 w ] δ t q_j = \int q_t \otimes [0, 1/2w] \delta t qj​=∫qt​⊗[0,1/2w]δt

v j = v i + ∫ q t a δ t v_j = v_i + \int q_t a \delta t vj​=vi​+∫qt​aδt

p j = p i + ∫ v t δ t + 1 / 2 ∫ ∫ q t a δ t p_j = p_i + \int v_t \delta t + 1/2\int\int q_ta \delta t pj​=pi​+∫vt​δt+1/2∫∫qt​aδt

这里的观测值a与w都是局部坐标系下的观测值。

缺点： 我们可以看到，积分里面的量包含了观测量，也包含了待优化的姿态，那么，我们每一步迭代都需要重新计算积分里面的量。而积分是一个计算量比较大的操作。比如两个相邻的姿态之间，有是个加速度值，那么就是计算10*3次，如果迭代10次，那么就需要对积分量进行300次计算，如果有100个姿态，则计算量高达3万次。

4、IMU预积分

将上面的三个式子变成：

q j = q i ∫ q i t ⊗ [ 0 , 1 / 2 w ] δ t q_j = q_i \int q_{it} \otimes [0, 1/2w] \delta t qj​=qi​∫qit​⊗[0,1/2w]δt

v j = v i + q i ∫ q i t a δ t v_j = v_i + q_i\int q_{it} a \delta t vj​=vi​+qi​∫qit​aδt

p j = p i + ∫ v t δ t + 1 / 2 q i ∫ ∫ q i t a δ t p_j = p_i + \int v_t \delta t + 1/2q_i\int\int q_{it}a \delta t pj​=pi​+∫vt​δt+1/2qi​∫∫qit​aδt

这样变换之后，与积分相关的内容，其实是一个常数了，只需要计算一次，也就是说，上面的三万次计算量，变成了30*100 = 3千次，计算量缩小了十倍。

预计分的本质： 将状态量变成了观测量。

四、残差Jacobian的推导

（@todo）

五、结论

1. 通过预计分，计算量优化了n倍，n是迭代的次数。这个倍数很重要，比如以前的1hz, 可以提高到10hz, 就能衍生出更多的应用场景了。