【深蓝学院】手写VIO第3章--基于优化的VIO（LSP求解，IMU预积分，Jacobian推导）--笔记

读书健身敲代码

已于 2023-12-28 15:54:35 修改

阅读量781

点赞数 1

文章标签：笔记算法人工智能自动驾驶 linux

于 2023-06-08 22:25:21 首次发布

本文链接：https://blog.csdn.net/qq_37746927/article/details/130714626

版权

0. 内容

在这里插入图片描述

1. 基于BA的VIO融合

优化的方法学会之后，滤波的方法也就会了。
在这里插入图片描述

具体的求解BA问题参考的是SBA的论文，使用的是LM算法（里面有个关于权重μ的计算方法，不同人的实现可能不一样，这些都是实现细节）
在这里插入图片描述
camera两个观测之间的相对位置（pq的变化）可以通过两次观测知道，但是IMU在两个时刻的观测量需要进行积分才能知道，而且IMU数据频率一般比Camera频率高，所以需要逐步地积分才能得到运动情况。

在这里插入图片描述

2. 最小二乘问题的求解

牛顿法，将loss function直接Taylor二阶展开，高斯牛顿法是将原函数f(x)一阶展开，再进行求解。
在这里插入图片描述

解法分为直接解和迭代解：
在这里插入图片描述
马鞍面复习：

迭代下降法本质上就是在不同的点进行(二阶)泰勒展开，忽略高阶项，展开之后进行优化。

2.1 一阶法

下降法（如果方向选择为梯度反方向则就为DL中的梯度下降法，此处叫最速下降法）：
下降法主要是找步长 $\alpha$ 和方向 $d$
在这里插入图片描述

最速下降法：不易收敛；牛顿法：H计算量大。

阻尼法，在牛顿法基础上增加一个 $\Delta x$ 的正则项 $\bm{\frac{1}{2}\mu\Delta x^{T}\Delta x}$ ，使其不会太大（牛顿法和阻尼法实际上是二阶法）：
在这里插入图片描述
阻尼法的阻尼是加在H上的 $\mu$

为方便起见，可将残差stack起来成向量（也可以写成sum of的形式，但向量可供讨论具体小f的一些性质，相应地，雅克比J和海塞H也要相应地写为向量形式）
在这里插入图片描述
F(x)是cost function，最简单的例子： $minF(x)=\frac{1}{2}||f(x)||_2^2$

2.2 二阶法

2.2.1 Gauss-Newton

在这里插入图片描述
损失函数F(x)对 $\Delta x$ 的一阶导数可记作b，也可直接写F(x) $^\prime$

具体的可以看《14讲》中的引入(P131)，有系数矩阵 $D$ 和优化半径 $\mu$ (也就是这里的阻尼因子)，当系数矩阵 $D = I$ 时，才是上述的形式。

2.2.2 LM

2.2.2.1 LM阻尼因子 $\mu$ 初值的选取方法

在这里插入图片描述

阻尼的理解：阻尼大小代表一阶近似的占比程度，也就代表了步长更新方向的占比程度，阻尼越大，步长值越小。 $\rho$ 很大，则说明在下降，要增大步长加快收敛，减小阻尼 $\mu$
$\rho<0$ ，则说明没下降，拒绝本次迭代，减小步长，增大阻尼 $\mu$ 。

在这里插入图片描述

$\tau$ 的设定是根据经验值而来，当前x距离 $x^*$ 较近时，取二阶近似，初值 $\mu_0$ 就设的小一些,乘较小的 $\tau$ ，距离 $x^*$ 较远时，初值就设的应该较大，保证下降的较快，取一阶近似，乘接近于1的 $\tau$

这里高翔补充了一点，：广义的韦达定理保证所有的特征值 $\lambda_i$ 之和跟矩阵的trace是一样的。

所以这里取 $max{J^TJ_{ii}}$ 矩阵的对角线的最大元素是有道理的，即可以有这样的先验： $J^TJ_{ii}$ 的对角线最大元素跟 $J^TJ_{ii}$ 特征值的最大值是在同一个数量级，所以可取对角线最大元素来近似最大特征值。

2.2.2.2 LM阻尼因子 $\mu$ 的更新策略

定性分析，看上面的 $\Delta x$ 的解析形式，
在这里插入图片描述

$\mu$ 处于分母上，增大阻尼 $\mu$ 会减小步长（有负号不一定代表就是负值，只能整体当做正值来理解才说得通），也符合“阻尼”的直观感受：当某次 $\Delta x$ 使得cost function增大，则应该增大阻尼使 $\Delta x$ 变小，减小步长；反之，如果cost function在下降则应该减小阻尼 $\mu$ ，增大步长，加快收敛速度。
在这里插入图片描述

上述推导说明分母一定是>0的，因为寻找的 $\Delta x$ 会使L每次都使得L下降，但是cost function F不一定会下降（因为L是对F的二次近似，理论保证是L每次都会下降，但是实际F不一定下降），所以当 $\rho<0$ 时表示F上升了。
当 $\rho$ >1时代表此时方向是正确的，则应减小 $\mu$ 加快下降速度；如果0< $\rho$ <1则代表则代表实际下降比较小，可能已经到最优值附近，应增大 $\mu$ 减慢下降速度。

$\Delta x$ 越小说明近似的越好，越接近最优值。

在这里插入图片描述
发现了14讲关于这个 $\mu$ 更新策略的一个错误， $\rho>\frac{3}{4}$ 时应该减小阻尼 $\mu$ ， $\rho<\frac{1}{4}$ 时应该增大阻尼 $\mu$

在这里插入图片描述

Nielsen采用了新的 $\rho$ 的计算策略，分母直接为L，判断 $\rho$ 的符号，当F下降时逐渐减小 $\rho$ ，当增大时， $\rho$ 下降的也很快。
在这里插入图片描述

2.3 鲁棒核函数（或叫M-Estimation）

outlier会使得F变得很大，为了处理outlier，在 $f^2$ 外面增加一层 $\rho$ 对f进行处理 $\rho(f^2)$ ，也有 $\rho(f)^2$
在这里插入图片描述
给x微小扰动 $\Delta x$ 计算 $\Delta s$ ，带入(14)可得加上鲁棒核函数之后的展开式

鲁棒核函数本质上是对每项误差的重加权（Reweighted），State Estimation中有对Iterated Reweighted Least Square（IRLS）和鲁棒核函数等价的证明。

在这里插入图片描述
95%有效原则，带上outlier和不带时的协方差之比接近95%，原文较接近数理统计方面的东西。

对正态分布，若鲁棒核是Huber，则控制参数设为1.345可满足95%原则，Cauchy应为2.3849
若非正态分布，则需统计残差协方差，在进行归一化处理（SVO,DVO,PTAM都是如此）变成正态分布，然后在使用正态分布的参数处理方法。
（但是在实际可能这个参数不会影响很大，可能直接设置为1，大多数情况Cauchy更好，可以证明Cauchy核函数等价于IRLS，但是Huber是线性的，更简单，Cauchy对每个残差要多算个log）

在这里插入图片描述

g2o里面残差的一二阶导数应该可以和前面的对应起来了
在这里插入图片描述

2.4 小结

在这里插入图片描述

3. VIO残差函数的构建

3.1 IMU预积分

从归一化残差的角度来考虑信息矩阵 $\Sigma^{-1}$ ，为了把不同传感器的误差进行归一化，然后叠加使用，中间用信息矩阵进行了重加权。
从优化的角度来理解信息矩阵，残差f(x)小，协方差小，逆就大，所以应该占的权重更大，更关注这些准确的点。

基于滑动窗口的VIO残差的构建，这里使用协方差来进行归一化（还是不是很理解）
在这里插入图片描述
上述
第一项prior代表之前时刻的先验，
第二项代表当前窗口内数据的残差，因为之前窗口外时刻的观测数据可能还有用，所以当做先验加入优化项中进行优化，
第三项是我们熟悉的相机的误差（如重投影误差）

关于IMU需要优化的状态量：
在这里插入图片描述
其中 $\lambda$ 是逆深度，用于对landmark进行参数化，具体的：
$x=zu\\ y=zv\\ z=z$
所以令 $\lambda=\frac{1}{z}$ ，上式变为
$x=\frac{u}{\lambda}\\ y=\frac{v}{\lambda}\\ z=\lambda$
由于u，v是已知的观测，使用逆深度 $\lambda$ 而非 $x,y,z]^T$ 来参数化landmark，有3个原因：

因为一些观测到的特征点深度值可能会非常大，难以进行优化；
可以减少实际优化的参数变量（ $x,y,z]^T$ -> $\lambda$ ）
逆深度更加服从高斯分布（见SLAM14讲建图的P326）。

优化出的 $\lambda$ 可以求出归一化系下的3D坐标，忽略内参。

补一个对于camera成像平面的总结：
参考博客
在这里插入图片描述

在这里插入图片描述

逆深度实际上是另一种参数化方式，类似于(x,y,z)，xyz具有相关性，体现在协方差矩阵上，非对角线元素不为0，而u，v，d，可以近似独立，所以协方差矩阵可为对角阵。实际上d不为正态分布，但是1/d可近似为正态分布。（具体描述见SLAM14讲建图的P326）
可以看下图，3D坐标由uv和 $\lambda$ 就能表示了。
在这里插入图片描述

误差的构建：
基于逆深度的rpj误差
在这里插入图片描述

在这里插入图片描述

这里旋转的更新再次复习：实际上是 $q_{wb_t}$ 四元数乘一个更新量 $\Delta q$ ，其中 $\delta \theta$ 用 $\omega^{b_t}$ 表示。
在这里插入图片描述

加速度积分，速度积分，旋转四元数积分：
在这里插入图片描述

每次前面的数据如果变化的话，后面的所有的值都要跟着积分变化，计算量很大。
把100个积分变成一个就好了，于是出现了IMU预积分：
预积分思想是：把相对于世界坐标系的部分单拎出来，IMU各个时刻之间的相对位置不变（这部分就叫做IMU预积分）。
在这里插入图片描述
3个预计分量分别是：i~j时刻的IMU的相对（即在body系下的）位移，速度，姿态变化量

在这里插入图片描述
将（32）式中的右边移项得到 $\alpha(位移)，\beta(速度)$ 的预测值，和观测值作差即得IMU预积分的误差，bias直接两时刻的测量相减即可，四元数旋转需要按照下面的推导来计算，其中旋转部分左乘四元数的逆即可。（误差是15维：pvq分别都是3维，两个bias也都是3维，共15维）
在这里插入图片描述
结合第一讲中关于四元数的求导

四元数的误差是计算观测和预测的逆，理想情况下是单位四元数 $1,0]^T)_{(4,1)}$ ，虚部表示的是这两个四元数表示的姿态之间的角度差异，当角度很小时，代表这两个姿态角度差异很小，所以取虚部表示姿态差异。
虚部取出来 $\frac{\theta}{2}$ ，乘以2就得到了旋转的角度 $\theta$
自己的推导：
在这里插入图片描述

3.2 预积分的离散形式

这里计算 $\omega,a$ 时，假设k和k+1时刻测量的 $\omega,a$ 的bias是不变的，而且这里没有考虑加计和陀螺的白噪声，理论来说是应该考虑的。而最后两行真正计算K+1时刻的bias时，就要考虑噪声noise了。

在这里插入图片描述

3.3 预积分的方差传递（有线性函数协方差传播结论）

使用预积分需要对其误差进行推导，进而需要推导其协方差，而因为预积分是一个个时刻的数据积分而来，而每个时刻数据的协方差都不一样，所以预积分的协方差存在一个传递的关系。

一个IMU数据的协方差和不确定度可以标定出来，但是如何传递呢？
协方差阵的定义：
选自这里本博客3.3.1节有摘选。
搞清楚协方差阵和互协方差阵的区别，协方差描述的是一组观测中的各个观测值之间的相关性，是方阵；而互协方差阵指两组观测之间的相关性，不一定是方阵，因为两组观测量的数量不一定相同。

协方差阵的计算：
在这里插入图片描述

可用结论：

在这里插入图片描述

在这里插入图片描述
但是关于四元数旋转的部分却不是线性的，所以要把非线性的四元素变化转化为线性的变化。

k时刻的状态误差来自于k-1时刻的状态误差和测量噪声传递而来，用前面的协方差传递结论传递结论：

在这里插入图片描述

3.3.1方差传递补充（不想看可直接跳到3.4）

在这里插入图片描述

3.4 状态误差线性递推公式的推导

说回正题，为了估计预计分量在propogate过程中的误差，需要对估计预计分量的协方差（这里误差即是协方差，不确定度(这里还不确定，需要继续研究)）。基于线性函数协方差传播结论，我们需要对预积分量中的非线性部分（四元数）进行线性化，然后使用此结论递推所有预计分量的误差。常用的线性化方法有一阶泰勒展开和使用一阶导数，于是有了如下两种方法：
从状态量和使用误差的导数来更新误差的方式来求误差的递推

将系统的状态方程进行一阶泰勒展开（主要讲）
求出误差的一阶导数，进行传递

KF假设马尔科夫性，k时刻的数据只和k-1有关，KF只能用于线性系统，若想拓展到非线性系统，则可以将运动和观测方程在某点一阶泰勒展开，只考虑线性部分，就得到了EKF（见SLAM14讲后端1部分）。下面状态量=真值+误差。公式(38)在 $(\hat{x}_{k-1},\hat{u}_{k-1})$ 一阶泰勒展开，两边消去真值就得到了状态误差的传递，那么问题就变成了如何求出状态量 $x_k$ 对 $x_{k-1}$ 的雅可比(F)和对噪声 $n_{k-1}$ 的雅可比(G)不同时刻的雅可比stack起来就变成了两个雅可比矩阵。

在这里插入图片描述
因为已知速度和状态量之间的关系（速度导数是world系下加速度+g），且状态量可以直接用，也就是说可以由已有的状态量去推导出未知的导数，所以求其误差之间的关系也是可以的，所以会有第二种基于误差的一阶导数的方法，常见于MSCKF这类优化的论文中。

而基于优化的方法（VINS-MONO，预积分等）和 EKF 中大多数使用的是第一种基于一阶泰勒展开的方法。

3.5 预积分误差的雅克比推导（重难点）

总体思想是：对状态量的递推式进行一些泰勒展开，一次只分析一个状态/输入量，两边分别对某个状态量的误差( $\delta \alpha, \delta \theta, \delta \beta, \delta b_{k}^{a}, \delta b_k^{g}$ )或输入量( $n_k^a, n_k^g, n_{k+1}^a, n_{k+1}^g, n_{b_k^a}, n_{b_k^g}$ )求偏导，真值导数为0，误差导数就是要求的Jacobian(F或者G)。

具体都要转到i时刻的body系下，分别是角速度，旋转四元数，加速度，预计分量： $\alpha,\beta$ ，以及加计和陀螺的bias
在这里插入图片描述

在这里插入图片描述

对各状态量求导：

3.5.1 对速度预计分量的雅克比（反对称矩阵的伴随性质）

在这里插入图片描述
上面的(49)是(48)两边 $\beta$ 一阶泰勒展开，然后右边对 $\beta_{b_k}$ 求偏导，即得对上一时刻速度预计分量( $\beta_{b_k}$ )的雅克比。

3.5.2 对角度预计分量的雅克比

在这里插入图片描述
k时刻旋转会受到一个微小的旋转扰动（可写成se(3)形式），

对exp进行一阶泰勒展开，再利用反对称矩阵的性质: $a^{\wedge}b=-b^{\wedge}a$ 即可约掉分母，得part1的雅克比。

在这里插入图片描述
所以可得对角度预积分量的雅克比 $f_{32}$

part2这部分化简用到了反对称矩阵的伴随性质：
在这里插入图片描述
参考以下博客：
反对称矩阵的性质

看到伴随之后想到之前SLAM14讲的时候做过一个证明题：

这里的exp ([ωδt]×就是一个旋转矩阵 $R$ ，exp ([−ωδt]×)是 $R^{-1}$ (之前问GPT-4的时候它告诉我的，后来一想，旋转矩阵正交，所以 $R^{-1}=R^{T}$ 这不就对上了嘛)，所以就有了part2最后的化简部分。

3.5.3 速度预计分量对角速度bias的雅克比

在这里插入图片描述

在这里插入图片描述
前面的一项 $I$ 跟分母 $\partial \delta b_{k}^g$ 没关系，可直接扔掉。

使用李代数的右扰动的推导：
在这里插入图片描述

$f_{35}$ 手推，还使用了exp的展开和 $a^{\wedge}b=-b^{\wedge}a$ ：

在这里插入图片描述

3.5.4 (旋转)角度预计分量对角度的雅可比

在这里插入图片描述
只考虑旋转误差，不考虑角速度测量的bias等误差，而且四元数有如下性质：
3组四元数相乘等于对应的左乘四元数，右乘四元数的逆，即点乘换成四元数乘：
$q*p=q\otimes p\otimes q^{*}$
其中 $q^{*}$ 为四元数q的逆
在这里插入图片描述

在这里插入图片描述

注意，预积分量 $q$ 和 $\theta$ 的关系要缕清，预积分 $q$ 本身是旋转，而其误差为角度 $\theta$ ，对 $q$ 进行一阶泰勒展开才能获得 $\theta$ ，对于 $q_{b_ib_{k+1}}$ 展开式的左边和右边是一样的， $q_{b_ib_{k}}$ 展开都是下图(38)的形式：

在这里插入图片描述

如针对 $q_{b_ib_k}$ ，其一阶泰勒展开方式就是 $\hat{q}_{b_ib_k}\otimes\begin{bmatrix} 1\\ \frac{1}{2}\delta \theta_{ b_k} \end{bmatrix}$

其中 $\hat{q}_{b_ib_k}$ 是真值， $\begin{bmatrix} 1\\ \frac{1}{2}\delta \theta_{ b_k} \end{bmatrix}$ 就是 $\delta q_{b_ib_k}$

这里把 $q_{b_ib_{k+1}}$ 左乘个逆移到右边，计算旋转的测量值的误差传递，倒数第二行就是运用上面的四元数的公式，R就是 $\begin{bmatrix} 1 \\ -\frac{1}{2} \omega\delta t \\ \end{bmatrix}$ 对应的旋转矩阵，于是就可以看出，从k到k+1时刻的姿态角的不确定度是通过R矩阵传递的。
以上是旋转预计分量的递推形式，接下来推导k+1对k时刻的雅可比 $f_{22}$ ：
在这里插入图片描述

3.5.5 $f_{15}，g_{12}$

剩下的 $f_{15}，g_{12}$ 在作业的博客中有推导 $f_{14}$ 推完 $f_{15}$ 后很简单。

3.6 小结

IMU预积分就是body下各个时刻的IMU状态量的预先积分，不用每次都从头开始
预积分的离散形式通过mid_point进行计算
IMU的误差传递
误差由k时刻的状态误差（系数F）和测量噪声（系数G）传递到k+1时刻：
雅可比F，G的推导，VINS-MONO里面使用了对状态量进行一阶泰勒展开的方法，核心思想是式(38)：

在这里插入图片描述

如针对 $q_{b_ib_k}$ ，其一阶泰勒展开方式就是 $\hat{q}_{b_ib_k}\otimes\begin{bmatrix} 1\\ \frac{1}{2}\delta \theta_{ b_k} \end{bmatrix}$

其中 $\hat{q}_{b_ib_k}$ 是真值， $\begin{bmatrix} 1\\ \frac{1}{2}\delta \theta_{ b_k} \end{bmatrix}$ 就是 $\delta q_{b_ib_k}$

要明白这里推导的IMU预积分的Jocabian是为整个系统构建Jocabian做准备的，整个系统Jocabian包括视觉和IMU的部分。后面再详细学习。

4. 系统Jacobian的推导(有R的就右乘一个exp然后再换出来，剩下的逆深度等就是链式求导法则)

在这里插入图片描述

预计分量对bias求导较为麻烦，预积分假设bias不动，但是实际上bias是动的，所以求他的bias就是比较麻烦的（我理解就是因为预积分有bias不动的先验，然后再去推对bias的导数就有些冲突），所以这里就使用了一阶泰勒展开来近似，丢掉高阶项，

在这里插入图片描述

4.1 举个例子

姿态对角度求导，IMU预积分得一个 $q_{ji}$ ，world系下得两个时刻的q，之间可以求出一个 $q_{ji}$ ，求这两个 $q_{ji}$ 四元数之间的四元数乘法可得相对位姿，取虚部对角度求导即得Jacobian。方法是对 $q_wb_i$ 取一个小扰动，使用公式变换到右边去，跟分母消掉（下面*都代表取逆），下面还有关于四元数左乘和右乘的算子，看Joan sola的四元数手册。
（高翔说还能用李代数？四元数连乘会麻烦一些，用李代数会简单点？）