VINS-MONO翻译以及理论推导系列(一)：IV测量预处理

最新推荐文章于 2021-11-17 01:49:53 发布

稻壳特筑

最新推荐文章于 2021-11-17 01:49:53 发布

阅读量589

点赞数 2

分类专栏： SLAM vins-mono

本文链接：https://blog.csdn.net/neptune4751/article/details/107147452

版权

SLAM 同时被 2 个专栏收录

82 篇文章 14 订阅

订阅专栏

vins-mono

12 篇文章 4 订阅

订阅专栏

IV：测量预处理

该部分描述惯性以及单目视觉测量预处理步骤。对于视觉测量，我们追踪连续帧间的特征点以及在最近的帧中检测新的特征点。对于IMU测量，对两连续帧进行预计分。注意到我们使用的廉价的IMU，其测量值会受到偏置和噪声的双重影响。因此我们在IMU预计分的过程中特别考虑偏置。

A.视觉处理前端

对每一帧新的图像，KLT稀疏光流算法[29]对现有的特征点进行跟踪.同时，检测新的角点特征[30]以维持每帧图像特征最低数目（100-300）。检测器通过设置两相邻特征点的最小像素间隔来保证统一的特征点分布。首先对2D特征点去畸，然后在去除外点之后将其投影到单位球面上。使用基础矩阵模型[31]的RANSAC去除外点.

关键帧也在该步骤中选取。我们有两个选取关键帧的标准。一种是平均视差与前一帧差距较大。如果最近关键帧与当前帧之间被追踪的特征点的平均视差大于某一阈值，我们认为当前帧为新的关键帧。旋转和平移都可以引起视差。然而，由于特征点在纯旋转的运动下没有办法被三角化。为避免这种情况，我们在计算视差时使用陀螺仪测量值的短期积分来补偿旋转。注意，这种旋转补偿只用在关键帧选取，不会用在VINS框架中的旋转计算。那么即使陀螺仪包含很大的噪声或者是偏置，只会导致次优的关键帧选择结果，不会直接影响估计的质量。另外一个标准是追踪的质量。如果追踪的特征点低于某个阈值，我们会把该帧作为新的关键帧。这种准则避免完全丢失特征追踪。

B.IMU预积分

IMU预计分在文献[22]首次提出, 它将欧拉角的旋转误差参数化。在我们之前的工作中[7]，我们提出了一种流形上的IMU预计分旋转公式。该工作使用连续时间IMU误差状态动力学推导协方差传递函数。但是忽略了IMU的偏置。这种预计分原理在文献[23]中通过添加后验IMU偏置矫正进行改进。本文中，通过包含IMU偏置矫正对之前工作[7]中提到的IMU预计分进行拓展。原始的陀螺仪和加速度计测量值 $\hat{w}$ 和 $\hat{a}$ 如下：
　　　　　　　　　　　

在这里插入图片描述

IMU测量值是在本体坐标系中测量的，它是平衡重力和平台动力的合力，并受到加速度偏置 $b_a$ 陀螺仪偏置 $b_w$ 和附加噪声的影响。我们假设加速度、陀螺仪测量的噪声是高斯白噪声， $n_a\sim N\left ( 0,\sigma^2_a\right )$ , $n_\omega\sim N\left ( 0,\sigma^2_\omega\right )$ 。加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯白噪声， $n_{b_a}\sim N\left ( 0,\sigma{^2_{b_a}}\right )$ , $n_{b_\omega}\sim N\left ( 0,\sigma{^2_{b_\omega}}\right )$ ：
　　　　　　　　　　　

在这里插入图片描述

给定对应于体坐标系 $b_k$ 和 $b_{k+1}$ 的两个时刻，位置、速度和方向状态可以在时间间隔 $[t_k,t_{k+1}]$ 间，在世界坐标系下中通过惯性测量值传递：
　　　　　　　　　　　

$\Delta t_k$ 是时间间隔 $[t_k,t_{k+1}]$ 之间的持续时间。

公式（3）中对旋转的推导：

可见，IMU状态传递需要坐标系 $b_k$ 的旋转、位置和速度。当这些起始状态改变时，我们需要重新传递IMU测量值。特别是在基于优化的算法中，每次调整位姿时，都需要在它们之间重新传递IMU测量值。这种传递策略在计算上要求很高。为了避免重新传递，我们采用了预积分算法。

将参考坐标系从世界坐标系转变为局部坐标系 $b_k$ 后，我们可以只对线性加速度 $\hat{a}$ 和角速度 $\hat {\omega}$ 相关的部分进行预积分，如下所示：

在这里插入图片描述

可以看出预积分项(6)能通过将 $b_k$ 视为参考帧的IMU测量值单独得到。 $\alpha^{b_k}_{b_{k+1}}$ 、 $\beta^{b_k}_{b_{k+1}}$ 、 $\gamma^{b_k}_{b_{k+1}}$ 只与 $b_k$ 和 $b_{k+1}$ 中的IMU偏置有关，与其他状态无关。当偏置估计发生变化时，若偏置变化很小，我们将 $\alpha^{b_k}_{b_{k+1}}$ 、 $\beta^{b_k}_{b_{k+1}}$ 、 $\gamma^{b_k}_{b_{k+1}}$ 按其对偏置的一阶近似来调整，否则就进行重新传递。这种策略为基于优化的算法节省了大量的计算资源，因为我们不需要重复传递IMU测量值。

对于离散时间的实现，可以采用不同的数值积分方法，如欧拉积分、中点积分、RK4积分等。这里选择了欧拉积分来演示易于理解的过程(我们在代码中使用了中点积分)。

在开始时， $\alpha^{b_k}_{b_k}$ 、 $\beta^{b_k}_{b_k}$ 是0， $\gamma^{b_k}_{b_k}$ 是单位四元数。 $\alpha$ ，β，γ在(6)中的平均值是如下逐步传递的。注意，增加的噪声项 $n_a$ ， $n_{\omega}$ 是未知的，在实现中被视为零。这得到了预积分的估计值，标记为 $\hat \left ( \cdot\right )$ ：

在这里插入图片描述

$i$ 是在 $[t_k,t_{k+1}]$ 中IMU测量值对应的离散时刻， $\delta t$ 是IMU测量值 $i$ 和 $i+1$ 之间的时间间隔。

然后讨论协方差传递问题。由于四维旋转四元数 $\gamma _t^{b_k}$ 被过参数化，我们将其误差项定义为围绕其平均值的扰动：

在这里插入图片描述

其中 $\delta\theta_t^{b_k}$ 是三维小扰动。

我们可以导出误差项的连续时间线性化方程(6)：

在这里插入图片描述

$p_{b_{k+1}}^{b_k}$ 可以通过初始协方差 $p_{b_k}^{b_k}=0$ 的一阶离散时间协方差更新递归计算：

在这里插入图片描述

其中 $Q$ 是噪声的对角线协方差矩阵 $\left ( \sigma_a^2, \sigma_\omega^2,\sigma_{b_a}^2,\sigma_{b_\omega}^2\right )$ .

同时， $\delta z^{b_k}_{b_{k+1}}$ 的一阶雅可比矩阵 $J_{b_{k+1}}$ 相对于 $\delta z^{b_k}_{b_k}$ 也可以用初始雅可比矩阵 $J_{b_k}=I$ 递归计算。

在这里插入图片描述

利用这个递推公式，得到协方差矩阵 $P_{b_{k+1}}^{b_k}$ 和雅可比矩阵 $J_{b_{k+1}}$ 。关于偏置的一阶近似 $\alpha^{b_k}_{b_{k+1}}$ 、 $\beta^{b_k}_{b_{k+1}}$ 、 $\gamma^{b_k}_{b_{k+1}}$ 可以写为：

在这里插入图片描述

其中 $J_{b_a}^\alpha$ 是 $J_{b_{k+1}}$ 中的子块矩阵，其位置对应于 $\frac {\delta\alpha_{b_{k+1}}^{b_k}}{\delta b{a_k}}$ . $J_{b_\omega}^\alpha,J_{b_a}^\beta,J_{b_\omega}^\beta,J_{b_\omega}^\gamma$ .也使用同样的含义。当偏置估计发生轻微变化时，我们使用(12)近似校正预积分结果，而不重新传递。

现在我们可以写下IMU测量模型所对应的协方差 $P_{b_{k+1}}^{b_k}$ ：

在这里插入图片描述

参考：

1.https://blog.csdn.net/pancheng1/article/details/81008081的论文翻译。

2.崔华坤的《VINS代码推导以及代码解析》（注：该文档已经在本博主的上传资料中上传，希望对大家有帮助）

稻壳特筑

关注

2
点赞
踩
2

收藏

觉得还不错? 一键收藏
打赏
5
评论
VINS-MONO翻译以及理论推导系列(一)：IV测量预处理

IV：测量值预处理该部分描述惯性以及单目视觉测量的预处理步骤。对于视觉测量，我们追踪连续帧间的特征点以及在最近的帧中检测新的特征点。对于IMU测量，对两连续帧进行预计分。注意到我们使用的廉价的IMU，其测量值会受到偏置和噪声的双重影响。因此我们特意将偏置考虑在IMU预计分的过程中。A.视觉处理前端对每一帧新的图像，现有的特征点由KLT稀疏光流算法[29]进行跟踪.同时检测新的角点特征以维持每帧图像中最低（100-300）特征点数。检测器通过设置两相邻特征点的最小像素间隔来保证统一的特征点分布。首
复制链接

扫一扫