陀螺仪+轮速计+单目视觉融合

陀螺仪+轮速计+单目视觉融合

参考：Tightly-coupled Monocular Visual-odometricSLAM using Wheels and a MEMS Gyroscope。仅备忘

摘要

1、提出一种预积分理论，将轮编码器和陀螺仪测量结果整合到局部框架里。预积分解决了旋转群SO(3)的流形结构。并讨论了不确定性传播和偏差校正。
2、里程计的误差项由预积分模型设计，紧融合在视觉优化框架
3、三种情况下鲁棒性：两种测量都可用；视觉测量失效；车轮编码器滑移

单目SLAM相关工作：

• 第一个实时单目视觉SLAM ，MonoSLAM是基于EKF的方法，在滤波中计算雅可比矩阵方法导致系统具有不正确的可观性，因此系统不一致且精度低；为了解决这个问题，黄国权等人提出first-estimates Jacobian 方法，他利用第一个可用的估计而不是不同的线性化点来计算雅可比，来保证系统的正确可观性，从而提高系统的一致性和准确性。J. A Hesch等人提出可观测性约束的EKF，明确强制实施系统的不可观测方向，从而提高系统的一致性和准确性。
• 另一方面，基于非线性优化的方法精度更高，因为它能够在每次迭代时重新线性化测量模型，以更好的处理非线性化，但是计算成本高。第一个基于实时优化的单目视觉SLAM系统是由Klein和Murray提出的PTAM。PTAM分两个线程，一个线程中，系统对选定的关键帧和构造的地图点进行BA，另一个线程通过最小化特征点与匹配地图点的重投影误差来优化相机位姿。
• 基于PTAM改进为ORB-SLAM2。以上是特征点法。估计准确，但计算耗资源，且对弱纹理环境低鲁棒性。
• 直接法是根据光强，在低纹理环境中以及在相机散焦和运动模糊的情况下优于特征点法。比如DTAM、SVO、LSD-SLAM是单目直接法。
• 基于滤波最好的VIO是MSCKF，它利用视觉测量提供的所有可用的几何信息，计算复杂度仅在特征点数量上呈线性，通过剔除在状态向量中的点特征实现。
• OKVIS基于图优化框架紧融合VI测量，由于在该系统下，当线性化点改变时，重复计算IMU积分，为了消除这种计算。Forster等人提出IMU预积分理论，以全概率的方法将IMU预积分因子和视觉因子紧耦合在一起。
• 后来提出的ORB-VISLAM有闭环，鲁棒性和精度更高。后来还有VINS_mono、VINS_Fuision
• 后来有人提出VINS系统融合里程计，提供系统可观测的尺度。

算法详解前基本说明：

• 符号说明：
  • $(.{)}^W$表示世界坐标参考帧，$(.{)}^{O_k}$,$(.{)}^{C_k}$和$(.{)}^{B_k}$表示第k帧下的轮速计、相机、惯导。${\mathbf{R}}_{{\mathcal{F}}_2}^{{\mathcal{F}}_1}\in \mathbf{S}\mathbf{O}(3)$表示从{${\mathcal{F}}_2$}到{${\mathcal{F}}_1$}的旋转，${\mathbf{p}}_{{\mathcal{F}}_2}^{{\mathcal{F}}_1}\in {\mathbb{R}}^3$描述帧{${\mathcal{F}}_2$}相对于帧{${\mathcal{F}}_1$}的3D位置。
  • 轮式编码器与相机外参为${\mathbf{R}}_O^C\in \mathbf{S}\mathbf{O}(3)$、${\mathbf{p}}_O^C\in {\mathbb{R}}^3$，${\mathbf{R}}_B^O\in \mathbf{S}\mathbf{O}(3)$表示从惯性系到轮式编码器系的旋转。
  • 第k帧位姿用刚体变化${\mathbf{T}}_W^{O_k}=Rp0I的矩阵块$ 表示， ${\mathbf{f}}_j^W\in {\mathbb{R}}^3$和${\mathbf{f}}_j^{C_k}\in {\mathbb{R}}^3$表示第j个地图点的在全局系{$W$}和相机系{$C_k$}下的3D位置。
  • $\xi \in {\mathbb{R}}^3$表示过度参数化的旋转矩阵$\mathbf{R}$。log映射为：
    $${\xi }^{\wedge }=log(\mathbf{R})\text{\hspace{1em}(1)}$$
    其中$(.{)}^{\wedge }$表示反对称矩阵，故：
    $$\xi =Log(\mathbf{R})=log(\mathbf{R}{)}^{\vee }\text{\hspace{1em}(2)}$$
    相反，指数映射将李代数$\mathfrak{s}\mathfrak{o}(3)$与旋转矩阵R相关联：
    $$\mathbf{R}=Exp(\xi )=exp({\xi }^{\wedge })\text{\hspace{1em}(3)}$$
  • 视觉测量为一系列特征点，是通过相机投影模型$\pi :{\mathbb{R}}^3\to {\mathbb{R}}^2$，投影第$l$个在当前帧${\mathbf{f}}_l^{C_k}=(x_c,y_c,z_c{)}^T\in {\mathbb{R}}^3$地图点到图像坐标系${\mathbf{z}}_{kl}=(u,v{)}^T\in \Omega$:
    $$\begin{gathered} {\tilde{\mathbf{z}}}_{kl}={\mathbf{z}}_{kl}+{\mathbf{\sigma }}_{kl} \\ =\pi ({\mathbf{f}}_l^{C_k})+{\mathbf{\sigma }}_{kl}\text{\hspace{1em}(4)} \end{gathered}$$
    其中 ${\tilde{\mathbf{z}}}_{kl}$为相应的特征测量，${\mathbf{\sigma }}_{kl}$是2x1测量噪声，其协方差为${\mathbf{\Sigma }}_{C_{kl}}$。
  • 陀螺仪在k时刻的测量值角速度${\tilde{\mathbf{\omega }}}_k$：
    $${\tilde{\mathbf{\omega }}}_k={\mathbf{\omega }}_k+{\mathbf{b}}_{g_k}+{\mathbf{\eta }}_{gd}\text{\hspace{1em}(5)}$$
    其中${\mathbf{b}}_{g_k}、{\mathbf{\eta }}_{gd}$为bias和零均值高斯白噪声。
  • 轮式编码器测量为：
    $$\begin{gathered} \widetilde{D{l}_k}=D{l}_k+{\eta }_{ed} \\ \widetilde{D{r}_k}=D{r}_k+{\eta }_{ed}\text{\hspace{1em}(6)} \end{gathered}$$
    其中$\widetilde{D{l}_k}、{\eta }_{ed}$为测量值和高斯白噪声。
    因此，由{$O_{k-1}$}到{$O_k$}测量的3D位置为：
    $$$\begin{gathered} \widetilde{{\mathbf{\psi }}_{O_k}^{O_{k-1}}}={\mathbf{\psi }}_{O_k}^{O_{k-1}}+{\mathbf{\eta }}_{\psi d} \\ \left[\begin{array}{c}1/2(\widetilde{D{l}_k}+\widetilde{D{r}_k})\\ 0\\ 0\end{array}\right]=-{\mathbf{R}}_W^{O_{k-1}}{\mathbf{R}}_W^{{O_k}^T}{\mathbf{p}}_W^{O_k}+{\mathbf{p}}_W^{O_{k-1}}+{\mathbf{\eta }}_{\psi d}({\eta }_{ed})\text{\hspace{1em}(7)} \end{gathered}$$
    • 地面机器人相对于空间机器人只有3自由度，平面约束。

紧耦合非线性优化

中间一大堆计算雅可比和残差公式

• A、Map初始化：
  • 初始化由视觉和里程计构建地图点的初始集。1、在当前帧k检测ORB特征点并且与参考帧r进行特征点匹配，如果有足够的匹配点则执行下一步，否则将当前帧设置为参考帧；2、检查视差，并且选出一组具有足够视差的特征匹配，当大于阈值是，使用里程计测量值计算两帧之间的相对变换，并对匹配的特征点进行三角测量；3、如果成功创建的地图点的大小大于threadshold，则对重投影误差、里程计误差和平面误差执行全局BA来优化初始地图。
• B、跟踪之前成功的视觉跟踪
  • 一旦里程计预测当前帧的初始位姿，则将局部地图中的地图点投影到当前帧并与从当前帧提取的关键点进行匹配。然后通过最小化能量函数来优化当前帧的位姿。判断后端中的地图是否被更新，位姿预测和优化方法不同。并且提供了一种解决轮子滑移的情况，
    • 1、当地图更新时的Tracking：在后端地图更新之后立即执行跟踪时，首先计算当前帧和之前关键帧m之前的里程计预积分值。然后，计算出相对变化${\mathbf{T}}_{O_m}^{O_k}$与优化的last关键帧结合（怎么结合？）以预测当前帧的初始位姿。这么做的是因为在后端执行局部或全局BA后，上一关键帧的位姿足够精确。最后，通过最小化下列能量函数来优化当前帧的状态：
      
    • 2、当地图没有更新时的Tracking：当后端中的地图未发生变化时，只计算当前帧k与上一帧k-1之间的预积分测量值，并通过将相对变换${\mathbf{T}}_{O_{k-1}}^{O_k}$整合（怎么整合？）到上一帧的位姿来预测当前帧的初始位姿。然后，通过最小化以下目标函数来优化当前帧的位姿：
      
      其中残差项${\mathbf{r}}_{0_{k-1}}=${${\mathbf{r}}_{{\mathbf{R}}_{k-1}}^T,{\mathbf{r}}_{{\mathbf{p}}_{k-1}}^T,{\mathbf{r}}_{{\mathbf{b}}_{k-1}}^T$}$\in {\mathbb{R}}^9$是上一帧的先验误差：
      
    • 3、 检测并解决车轮滑移：如果上述优化使一半的原始匹配点为异常值，则认为当前帧k经历一次滑移。一旦检测到车轮打滑，将滑点标志设置为当前帧并重置当前帧初始位姿，即上一帧k-1的位姿。然后在局部地图中重新投影地图点，并与当前帧的特征点重新匹配。最后，仅通过那些匹配的特征点来优化当前帧的状态：
      优化之后，当前帧的结果估计和Hessian矩阵被用作下一次估计的先验。
• C、当视觉跟踪丢失：
  • 假设之前视觉跟踪丢失，则当前帧会发生以下三种情况:1)机器人访问已重建的区域 ；2)机器人访问新的可以建立足够地图点的环境；3)机器人依然无法获得视觉特征。对于情况1)，执行全局重定位，使用DBoW和PnP来计算当前帧位姿。对于2)，首先使用里程计来预测当前帧的初始位姿，然后将上一帧看到的地图点投影到当前帧并使用这些匹配的特诊点优化当前帧位姿。对于情况3)，使用里程计来计算当前帧的位姿。
  • 当视觉跟踪丢失后从当前帧提取足够的特征时，首先认为机器人可能返回已经重建的环境，因此执行全局重定位（情况1）。然后，如果重定位连续失败，直到选择具有足够特征的第二个关键帧来重建地图，则认为机器人进入新环境。当足够匹配的特征点支持相机位姿是，则认为视觉信息可用于当前帧的跟踪（情况2）。
• D、关键帧的策略：如果视觉跟踪成功，两个关键帧选择标准，1)当前帧跟踪的特征点比上一关键帧少50%； 2)局部BA完成后加入关键帧。满足以上条件尽可能多的插入关键帧；如果视觉跟踪丢失，满足下列条件之一时将关键帧插入后端，1)距离上一关键帧的traveled 距离大于某一阈值，2)距离上一关键帧的相对旋转角度超过阈值，3)局部BA在局部mapping线程中完成。
• E、后端：
  • 包括局部mapping和局部回环线程。局部mapping线程旨在构建环境的新地图点，并且优化局部地图。如果新的关键帧k的视觉跟踪丢失，我们通过为关键帧k添加一个新节点和与上一关键帧连接的边缘来更新图模型，以确保能够构建新的map。此外，视觉-里程计局部BA优化之前N个关键帧（局部window）和这些关键帧所看到的所有点，通过最小化窗口中的损失函数(12)实现。注意，链接到上一关键帧的里程计约束仅为那些没有滑点标志的关键帧。回环线程负责回环时消除累计漂移。

Meixiang Quan, Songhao Piao, Minglang Tan, Shi-Sheng Huang, 1Tightly-coupled Monocular Visual-odometricSLAM using Wheels and a MEMS Gyroscope.2018