一文看尽4种SLAM中零空间的维护方法

作者丨wuRDmemory@知乎

来源丨https://zhuanlan.zhihu.com/p/341322063

编辑丨3D视觉工坊

写在前面

笔者最近在总结整个MSCKF的相关知识的时候，算是比较仔细的推导了与零空间相关的部分，放在之前笔者会认为这部分知识确实是比较难理解的部分，但是花了很多时间总结发现其实都还是有很强的相关性的，所以这里笔者算是总结一下这部分的内容，算是对近来知识的一个总结。

本文主要会涉及4种零空间的维护方法，分别是：

1.First-Estimate-Jacobian，该方法也是用途最广的维护能观性的方法，可以适用于EKF-base和Graph-base的SLAM系统中；

2.Observability-Constrainted 方法，该方法用途可能不多，但其有自身的优势在，且是开源的S-MSCKF中使用的方法；

3.DSO中关于零空间维护的方法，其中不仅使用了FEJ技术，同时对增量方程和求解的增量都做了正交化去除零空间变量的影响；

4.VINS中关于零空间维护的方法，该方法主要是数值上的维护；

Reference

1.https://zhuanlan.zhihu.com/p/304889273笔者关于FEJ 的相关总结；

2.https://zhuanlan.zhihu.com/p/328940891笔者关于OC-KF的相关总结；3.https://blog.csdn.net/wubaobao1993/article/details/105106301笔者关于DSO对于零空间维护的相关总结，该总结现下看来是不太对的；4.https://blog.csdn.net/wubaobao1993/article/details/108354488笔者关于VINS对于零空间维护的相关总结；

5.Consistency of EKF-Based Visual-Inertial Odometry. MSCKF能观性的分析论文；

EKF-Base方法对零空间的维护

从前有两位年轻人都致力于EKF-Base（本文更多的是针对MSCKF方法）方法中能观性的分析，但是都遇到了很大的问题。于是两人约定一起到山上去请教一位德高望重的老师。

老师：你二人既然都是研究能观性的分析，那么一定知道能观性分析的评价标准是能观性矩阵吧？

两位年轻人：自然是知道的，对于线性系统而言，系统的能观性主要由能观性矩阵反应。

老师：嗯，很好，既然如此，我就给二位一人三个锦囊，希望你二人能在各自的研究方向上都能悟道。

两位年轻人接过锦囊，开心的下山了。

First-Estimate-Jacobian方法

第一位年轻人下山之后便回到了家，继续开始对于能观性的分析，他迫不及待的打开第一个锦囊，上面写着一行字和如下的公式：

锦囊1. 不依赖IMU的运动方程，从头推导理想情况下的状态传递方程

年轻人反复看了多遍老师的建议，于是经过一段时日，年轻人终于按照提示推出了新的状态传递方程：

其中

1.所有的变量都是理想情况下的值，也就是并不涉及到时刻的概念（公式中的 t 和 t+1 更多的表示的递进关系，可以理解为当前的位姿经过IMU激励得到下一个位姿），更进一步的讲，某个变量在预测阶段被预测出来，那么由于预测阶段都是理想的，所以该预测值也是相当理想的，即真值；

2.

；

3.

；

年轻人觉得很开心，觉得新的状态传递方程一定可以帮助他解开心中的迷惑。但是研究良久，年轻人还是一无所获，于是他又拿出第二个锦囊，想看看老师又给了他怎样的启示，只见上面写道：

锦囊2. 使用理想的观测矩阵，从前到后推导能观性矩阵中的某一行，总结出零空间应该是怎么样的

年轻人很聪明，他马上把注意力集中在理想情况下的过程，首先他写出了理想情况下的观测矩阵：

将公式（4）中的两部分和在一起可得：

于是年轻人按照老师的建议，取出其中一行进行推导，如下：

将公式（3）和公式（5）带入到公式（6）中可得：

公式（7）中因为该部分仅涉及到IMU状态部分的分析，因此省去了观测的部分，同时省去了观测矩阵前面的公共部分。

在推导过程中，年轻人发现按照这样乘法下去，在每一次乘法之后都能得到一个很相似的形式，以前面两次乘积为例可以得到：

年轻人很快便总结出了规律，于是在 t0 时刻，整个乘积变作（这里把特征部分的元素给添加回来了）：

所以对应的零空间其实就是：

除此之外，年轻人也在推导过程中总结出了一个很重要的规律：之所以乘积最后能满足公式（10）所示的零空间，其比较重要的原因在于在乘积过程中，所有的位姿和速度都用的是一个值（在理想情况下，都使用的是理想值），这保证了相同项之间可以互相抵消。

例如在

和

，其中当

使用同样的值时才可以抵消，不至于引入其他项。

于是年轻人回想自己的推导过程，因为在自己的系统中，整个系统维护了一个滑动窗口，该窗口内记录了一段时间内的位姿数据，该数据会因为新观测而被更新，导致后续在构建观测矩阵时一直使用的是最新的值，也就是同样的位姿和速度，使用了两个时刻的值，这样的方法使得推导过程中增加好很多因为使用不同时刻值的扰动项！例如在时刻

，则对于第

个节点（假设该节点的预测时刻也为

时刻）来说形式如下：

其中：

1.

的形式应与公式（9）中的理想情况相似：

2.

是由于不同时刻线性化造成的扰动项，形式如下：

其中形如

就是在 s 时刻第 s 个节点的值，可以看到其中有较多的同一节点在不同时刻的值，这些值导致了扰动项的产生，具体形式可以参考参考【5】中的公式。

3.

矩阵是一些公共项，这里不多做分析；

锦囊3. 对同一变量使用第一次预测出的值作为观测方程的线性化点——FEJ

年轻人从推导理想情况的步骤中得到启发，于是他尝试将同一变量使用同一时刻的值进行实际情况的推导，选择怎样的时刻呢？年轻人有三个选择，以 t 时刻产生的节点为例：

1.该变量被 t-1 时刻的更新值预测出来的值——

；

2.该变量在 t 时刻更新之后的值——

；

3.该变量在之后时刻被更新之后的值——

；

最后年轻人选择了第一个，因为在能观性矩阵中涉及到状态转移矩阵

，此时对于

节点，仅有刚刚预测出来的值。

于是年轻人对公式做了如下的改动：

1.将所有的状态转移矩阵做如下变换：

2.里面的

和

中的相应项也要变为FEJ的形式；

3.将所有的观测矩阵做如下变换：

将上述的改动带入到能观性矩阵中之后，果然消掉了所有的扰动项，得到了形如公式（10）的零空间：

年轻人很开心，他连忙打开老师给予的第三个锦囊，果然老师的提示与自己的想法是一致的。

以上的详细推导可以看参考【1】

Observability-Constraint 方法

时间回到第二个年轻人那里，他下了山只有，并没有直接回家，而是去参加了一个朋友主办的研讨会，会议上他了解到如何求解带约束的最优化问题，这个研讨会在后面帮助了他很多。第二个年轻人也从老师的建议中获取了很多；

锦囊1. 根据IMU运动方程，推导误差状态转移方程的闭式解，并用该理想情况下的转移矩阵传递初始的零空间

根据年轻人之前的研究，根据IMU的运动方程推导得到的误差状态微分方程为：

其中：

这里误差驱动矩阵

不对零空间的分析有贡献，所以暂时就不分析了；

那么根据微分方程，可以得到状态转移方程为：

其中：

根据公式（17）可以列出如下公式：

求解上述的微分方程，易得：

其中：

需要特别注意的是上述的推导是建立在初始时刻为 t0 的前提下，且认为初始时刻的转移矩阵为单位阵。

在初始的 t0 时刻，以特征点

为例的系统能观性矩阵为：

因此得到初始的零空间如下：

和公式（14）是一样的，于是按照老师的提示，年轻人使用理想情况的状态转移矩阵对初始的零空间进行传递，那么在 t 时刻得到：

上述公式中因为假设是在理想情况下，因此去掉了时间的影响。

锦囊2. 根据状态转移矩阵，求解相邻时刻间零空间的传递情况

根据公式（19），年轻人很快的求得了相邻时刻的状态转矩矩阵为：

于是根据锦囊1中的分析，在 t1 时刻，理想的零空间为：

那么 t2 时刻的理想状态下零空间应为：

可见在理想的情况下，零空间会跟着状态传递一起传递到当前时刻，且零空间的状态量满足一个通用的形式，该形式的物理意义为三个绝对位置和以重力方向为旋转轴的旋转。

那么公式（25）是否依旧是能观性矩阵的零空间呢？此时还需要用观测矩阵进行验证，引入公式（5）所表示的理想观测矩阵可以发现：

年轻人也很快就悟得了老师所想要表达的意思——在理想情况下，零空间会随着状态转移一起向前传递，且保持一个通用的形式。

锦囊3. 修改状态传递矩阵和观测矩阵，使之满足理想情况的约束

从上面的分析可以知道理想情况下，整个系统应满足如下性质：

性质一：系统的零空间可以通过状态转移矩阵传播到当前时刻 t，且形式与初始零空间相似，由 t 时刻状态的预测值有关；

性质二：相邻时刻的零空间可以通过状态转移矩阵进行传播，且零空间的形式也满足通用形式；

性质三：通过状态传递矩阵传播到当前时刻 t 的零空间与观测矩阵正交，亦即优化问题的优化方向与零空间正交，因此优化量不会破坏系统的零空间；

但是实际情况下，状态传递矩阵和观测矩阵并不能很好的满足上面的三个性质，但是根据老师的锦囊，年轻人很快便有了破解的方法：

1.对于 t 时刻，假设之前的状态都已经是理想状态了，那么该时刻所需要维护的零空间为：

其中各个变量为 t-1 时刻的预测值，之所以为预测值，有两个原因：

由公式（22）可以看到理想情况下的初始零空间由纯状态传递过程传递过来，所以在 t-1 时刻的零空间由 t-1 时刻的预测值组成；

由性质三可知，理想情况下，观测矩阵与零空间正交，所以经过更新阶段不应该影响 t-1 预测阶段传递过来的零空间；

1.经过 t 时刻的预测阶段，理想的零空间应该变为：

其中

为算法期望的状态转移矩阵；

2.在更新阶段，理想的观测矩阵应该与预测阶段之后的零空间正交：

其中

为算法期望的观测矩阵；

对于上面公式（28）（29），年轻人并没有直接对方程进行求解，而是结合当前系统的状态转移矩阵

和观测矩阵

共同求解一个最优化问题，问题形式如下：

对优化问题（30）运用拉格朗日乘子法以及KKT条件求解对偶问题可以得到最优解为：

该部分详细推导见参考【2】

小结

可以看到，FEJ和OC-KF的方法都可以理论上较好的维护零空间，相对而言，FEJ有如下优缺点：

1.状态传递矩阵和观测矩阵数值上直接维护了零空间，并没有像OC-KF一样额外构建最优化问题并修改系统方程的一些东西；

2.可以扩展到了Graph-base的方法上；

3.但是比较依赖初始的线性化点，如果线性化点不好，那么后续的系统可能向不好的方向发展；

而OC-KF方法的优缺点如下：

1.对于线性化点的依赖性不是很强，后续的优化方向虽然和FEJ方法的零空间一样正交，但是结合了当前的状态值；

2.可以扩展到Graph-base的方法上；

3.缺点笔者个人觉得就是比较理想，维护的都是假设理想情况的零空间；

DSO对于零空间的维护

如果各个SLAM或者VO的开源系统也有相应的星座的话，那笔者认为DSO当属其中的处女座了。

整个DSO中对于理论应用用料相当的足，同时各种细节也处理的相当到位，其中就不乏本文的主题——零空间。

DSO对于零空间的维护主要用了三个技巧：FEJ、增量方程的正交化、增量值的矫正，诚意算是相当足了。FEJ就不多介绍了，和上面的EKF-base中相同，这里的FEJ也是第一次得到状态变量时，把当时的值作为后续的线性化点；下面的重点放在对后面两个部分的介绍上。

DSO为什么使用了FEJ还是用别的技术维护零空间？

从上面的分析不难知道，FEJ技术是在EKF-base的系统中推导出来的，那么对于Graph-base的系统，其实这个方法稍微显得“水土不服”，笔者认为的本质原因在于：在优化的时候，对于同一个landmark，不可能保证所有观测到它的位姿都已经被固定在了初始线性化点上（实际上对于DSO而言，当特征点不再被观测到的时候才设定FEJ的线性化点），所以导致同一个优化问题使用两个不同的线性化点，引入虚假信息。

笔者认为FEJ的引入作用如下：

1.对于固定化住的位姿，其优化问题在求解之后的增量时的协方差矩阵被固定了，就没有所谓的实际状态没有变化，但是因为线性化点的变化导致整个增量的协方差矩阵变化的现象；且如果一个优化问题不再进来新的观测和关键帧，那么其相对于初始线性化点的增量之后都会保持一样，不会变化（想象一个机器人停止在原地，那么他的所有估计变量以及协方差应该保持不变，不应该随着线性化点的变化而变化）；

2.边缘化阶段也不用担心因为不同线性化点引起的不一致性；

稍微记录一下增量的协方差矩阵如何求得：

1.原始问题为对数最大似然问题，即：

2.GN求解问题为：

GN问题求得的最优解为：

3.所以对于

变量而言，其协方差为：

于是，引入了FEJ之后，因为jacobian的固定导致

的协方差就被固定了；

DSO如何维护零空间

因为DSO的论文中也并没有这个地方的参考，所以以下部分均是笔者结合了多篇博客以及自己的一些思考所得到的，如有问题，欢迎拍砖讨论。

DSO对于零空间的建模

DSO对于零空间的扰动笔者个人觉得还是比较形象化的，比如在分析EKF-base的方法时，我们通常都在讨论零空间是如何的；而DSO把零空间的扰动具象化了，即作者求得了扰动矩阵

，该矩阵表示零空间中的量如何传递影响线性化点的，即：

这里以位姿节点

为例，给出了零空间对于线性化点的转移矩阵（DSO这里仅对迭代开始时刻的线性化点求解了转移矩阵，迭代内部没有），如果零空间的扰动为

，那么对于节点

的增量为

。这部分具体的代码可以参见DSO的工程中的HessianBlock.cpp中的setStateZero函数。

增量方程对于零空间的正交化

这部分主要用到了上述OC-KF中对于观测矩阵的修改技术。

对于Graph-base的SLAM而言，如果使用的是基于GN方法的话，其能观性矩阵主要依赖于增量方程中的

矩阵，而根据

，所以整个优化问题的能观性矩阵的秩主要由观测矩阵

决定。于是按照EKF-base的分析而言，这里也主要希望理想的观测矩阵可以与零空间正交。

在DSO中，笔者认为作者也对理想的观测矩阵

做了一个如公式（30）的优化问题，这里说一下约束：

对于GN方法求解而言，我们希望

也就是希望在理想的观测矩阵下，零空间的扰动与观测矩阵正交，上述公式中的线性化点笔者认为是很复杂的，有些是固定线性化点的，有些是没有固定线性化点的。

不过这里的约束为

，不过这里因为零空间的扰动

，所以优化问题为：

所以最优的观测矩阵

，将其代入到增量方程的公式中可得：

这个部分对于 H 矩阵的推论和代码部分对不上，但是笔者实在没有找到参考或者其他的理论得到代码中的推论，笔者尝试修改代码中的部分为公式（38），最终的效果没有看出来差距，这个地方还有待研究。

增量对于零空间扰动的正交化

这部分与上部分的正交化同理，作者也是构建了一个优化问题求解理想的增量：

所以最优的增量值为

，注意这里的

放在了前面，主要是为了维度上的对齐。

关于正交化的一点点说明和思考

上面的两个部分的约束条件均是对零空间的映射矩阵

进行的正交化，可能有些小伙伴认为应该是对于零空间的具体扰动

进行正交化更好，但是在计算的过程中，算法并不知道在零空间上的扰动

是多少，所以对于

进行正交化更合理；

VINS对于零空间的维护

相比而言，VINS对于零空间的维护就比较简单，仅仅是保证滑动窗口中的最老关键帧的yaw轴在数值上的一致性。

因为旋转是可逆的，所以如果滑动窗口中最老的关键帧的姿态发生了变化，其实可以认为是机器本身的姿态没有变化，但是世界坐标系由原来的

变为了

下面，算法要做的其实就是把所有的位姿再转回到原来的世界坐标系

下；

1.优化前yaw轴为old_yaw，此时的yaw轴在原来的世界坐标系下

；

2.优化后yaw轴为new_yaw，此时的yaw轴在新的世界坐标系下

；

3.所以将yaw轴转回到原来的坐标系下需要

，因为算法仅仅矫正yaw轴就可以了，所以该矫正位姿仅仅为yaw=old_yaw-new_yaw的旋转矩阵或者四元数就可以了；

总结

本文详细总结了几种对于零空间维护的方法，其中用处最广的是两种方法：

1.Firsr-Estimate-Jacobian，在EKF-base的系统中，该方法使用预测时刻的变量作为后续的线性化点，保证同一变量使用相同时刻的值来消除因为更新带来的扰动项，保证零空间的一致性；

2.Observability-Constraint，该方法构建优化问题以修改观测矩阵和状态传递矩阵，从而保证零空间的传递以及优化方向的正交；

3.DSO同时使用了两个技术，极其“残忍”的维护了VO系统的零空间；

4.VINS在数值上维护了yaw轴的一致性，但是没有用其他的技术保证零空间（甚至没有保证边缘化之后的线性化点），但是其效果可以看到对于有闭环的SLAM系统来说，可能零空间带来的影响一个闭环就制得死死的了；

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。