关于VIO零速更新(ZUPT)与控制三种约束的工程实践

今天这篇是深度稍微高一些的，尽量写细，但是具体实践各家都有不同的方式与工程习惯，就不多赘述了。

小组工作比较忙，代码还没来得及整理，总体更新一下基础知识。

VIO系统后端核心的三种约束是：先验约束，视觉约束与IMU预积分。

实验对象是VINS-MONO，首先因为我们主要的工程是基于VINS-MONO，另外VINS-MONO也是当前所有VIO系统里整个骨干脉络和原理逻辑比较清晰的一版，在其基础上出的工程问题较容易被处理与定位。

VIO，视觉IMU里程计，相信未来是比较实用和高性价比的位置姿态估计方式，但当前主要存在以下的一系列问题：

1.FEJ，等一线性化点雅克比，VIO系统在SWF滑动窗口优化的过程中，信息矩阵H会分为两个部分H1和H2，其中一部分线性化点在变化，另一部分线性化点被固定。针对性的策略是使不同残差对同一状态估计求Jacobian时，线性化点必须一致，以避免矩阵中的Null space退化而使不可观变量变为可观。VINS-MONO未使用FEJ，原因未知，实际产生的结果并未有太多影响。

2.YAW角与3自由度不可观问题，这4个自由度不可观的好处就在于系统可以更好的估计最优解，坏处在于如果发生异常现象(不要过于相信数据集，在实际情况下异常现象是很容易出现的)时，轨迹会产生突刺，并迅速因尺度不确定性而导致系统不可用，即使回环也无法处理。

2.视觉完全丢失的情况，举几个很常见的例子(1)突然进入无纹理区转向 (2)突然遭受撞击在极短时间从东南向西北 (3)使用卷帘快门高速进入了急剧晃动和颠簸的状态。这多种情况都会导致系统视觉跟踪丢失，如其使用的LK法中，蓝色点表示新跟踪点，到了一段时间后转红色，这一类情况都会导致红色丢失产生一堆新的蓝色。产生的直接结果就是系统不再有视觉约束，如采用的硬件比较弱的话，系统会快速输入错误的先验约束，并逐步被IMU预积分约束联合拉飞。

3.相机行进间视觉剧烈扰动的情况，相机前出现新的无法被辨识的动态障碍物，如人员，并大量破坏了特征点。在这种情况下VIO会不断更新错误的关键帧，并塞满滑窗SWF，在IMU实际并没有什么太大实际运动的情况下，视觉约束的权重矩阵Σ会占据主导。

4.零速下IMU漂移的问题，这也是很普遍的现象之一了。当系统快速进入零速(缓慢进入问题不大)，系统Margin new frame的策略是不再生成新的关键帧KF，并丢弃视觉测量，继续向下传递预积分。在此种情况下VIO会很容易进入累积预积分误差进入尺度飘飞。

5.零速下视觉扰动的问题，如系统顺利进入零速，轨迹稳定后相机前出现了一系列的扰动(和第3点基本一致，略有不同)，系统也会较容易进入尺度飘飞。

6.长时间无yaw输入向上产生大pitch角长期静止观察产生尺度飘飞。这个问题还没有定位，但实际是有这类现象的，row角同理。

对应的解决工程及手段：

这块应该是充满干货的知识了，也不知道会不会有人看到~

从1-6的一系列问题，大家会了解，使VIO正确和理想的工作本来就是一件充满困难的事情。首先这类系统要完全掌握和学懂原理，就需要花大量的时间，即使学懂了，上面那一系列的问题也是难以避免的，那这一系列的问题根源是什么呢？ 其实就是在你使用的位置姿态估计组件与方法中的约束！以VIO为例，其中最关键的就是先验约束，视觉约束和IMU约束，我们的目标函数最终实际是针对这3种约束的非线性最小二乘解，但这每个约束里面的Σ及其对应的权重控制策略，决定了系统的鲁棒性和可用性。而实际是否会有比较完美的策略呢？当然不会有，这里就需要使用对应的工程进行深化处理。

1.零速更新ZUPT是一种工程策略，如针对问题5和6，可以直接在后端优化前加入限定条件，我们使用的策略是针对IMU输入的6自由度，判断在一段时间内IMU对应的欧式距离平均值，我们的IMU是200HZ，即5ms输出一个数据。通过40-100ms即8至20个IMU数据计算欧式距离，即可判断系统是否进入静止/零速状态。在状态被激活时，可以破坏原Σ的权重策略(如直接降低视觉权重至极小值)，使视觉滑窗SWF停止更新。对应的IMU策略有多种 (1)如不断使用最新的一组最新的IMU数据进行预积分，相当于把VIO变成了IO，工程实操会比较困难和更细节一些。(2)或者直接在零速更新判断生效时，把VIO变成VO，使IMU的预积分停止，但是第二种情况仍然会出现障碍物扰动导致的轨迹飘飞。在工程处理的过程中，一定要注意不要破坏滑窗和后端优化本身！

2.针对问题2有一种更简单直接的暴力策略，实际效果是很好的，即通过IMU的高速运动输入(如上提到的欧式距离策略)，(1)一种策略是使系统直接从VIO变为IO，这样在处理剧烈颠簸和急剧晃动时的视觉丢失效果是比较好的。前提仍然是一定注意不要破坏滑窗和后端优化本身！(2)另一种策略是在后端优化中写回调函数，从前端来取红色/蓝色LK法的特征点数量进行直接判断。 一种是视觉的方法，一种是IMU的方法。本质都是针对极端情况建立新的三约束Σ调整策略。

3.问题1的话可以直接尝试手动加入FEJ，我们试了一下效果没什么变化。问题2其实算是问题3-6的成因之一，就不赘述了。最后讲一下问题3，问题3是最难处理的，比如系统在运动，同时一直有个人员在前方随动的情况。这种我们目前也没有好的方法，其实从思路上来说就是让系统进入纯IO优化，但是这个判断条件难以被建立，如果主系统带NPU的话就好办，可以用机器学习的方法建立人员判断条件进入IO，那就很简单了。

4.加入这些工程策略后，尺度飘飞得到了很好的处理。大家也可以使用自己的策略去约束极端情况。综上所述，最容易出现问题的场景如下：(1)静转动，动转静 (2)视觉高速晃动丢失，无纹理丢失，大Pitch/Row视觉丢失 (3)活动障碍物

常规的处理手段是内参/外参调整，使用硬件同步对时确保td的准确性，使用全局快门。很多同学认为如果能保障这些都处理得很好的情况时，建立一个统一的Σ策略即可解决问题，实际也是不可能的~今天介绍的重点是如何从系统后端优化与三约束的整体层面去策略性地提升鲁棒性Robust。