SLAM中的零空间问题

最新推荐文章于 2023-12-14 09:37:34 发布

古路

最新推荐文章于 2023-12-14 09:37:34 发布

阅读量3.9k

点赞数 10

分类专栏： # VIO 文章标签： VINS零空间 SLAM不可观状态量 SLAM零空间问题

本文链接：https://blog.csdn.net/fb_941219/article/details/100279527

版权

VIO 专栏收录该内容

10 篇文章 2 订阅

订阅专栏

SLAM中的零空间问题

0. 引言

首先回顾一下张宇讲的。对 $A x = 0$ 、矩阵 $A$ .

线性相关，有非0解，不是满秩，行列式为0，有多余向量。
线性无关，只有0解，满秩，行列式不为0，没有多余向量。

1.矩阵零空间

对于矩阵 $A$ ，所有满足 $A x = 0$ ,的向量 $x$ 组成的集合 $N$ ，可以证明 $N$ 包含零向量，且对线性运算封闭，因此 $N$ 是一个向量子空间，这个子空间叫做矩阵A的零空间。

求矩阵的零空间，就是求方程组 $A x = 0$ 的解空间。

矩阵可以看做一组列向量 $C_1,C_2,...,C_n$ ，那么如果这组向量是线性无关的，那么 $A X = 0$ 的解空间只包含一个向量：零向量。反之，如果零空间包含非零向量，说明矩阵的列向量线性相关。

2.一个解题的例子

解线性方程组：
$\left\{\begin{array}{c}{x_{1}+2 x_{2}+3 x_{3}+x_{4}=5} \\ {2 x_{1}+4 x_{2} \quad-x_{4}=-3} \\ {-x_{1}-2 x_{2}+3 x_{3}+2 x_{4}=8} \\ {x_{1}+2 x_{2}-9 x_{3}-5 x_{4}=-21}\end{array}\right.$
解：对方程组的增广矩阵 $\overline{\mathbf{A}}$ 作初等行变换有：

$\overline{A}=\left(\begin{array}{ccccc}{1} & {2} & {3} & {1} & {5} \\ {2} & {4} & {0} & {-1} & {-3} \\ {-1} & {-2} & {3} & {2} & {8} \\ {1} & {2} & {-9} & {-5} & {-21}\end{array}\right)$ $\rightarrow\left(\begin{array}{ccccc}{1} & {2} & {0} & {-\frac{1}{2}} & {-\frac{3}{2}} \\ {0} & {0} & {1} & {\frac{1}{2}} & {\frac{13}{6}} \\ {0} & {0} & {0} & {0} & {0} \\ {0} & {0} & {0} & {0} & {0}\end{array}\right)$

通过画阶梯线可以知道，该方程组的自由变量为 $x_2$ 、 $x_4$ 两个自由变量不可控（SLAM中的自由度，不可观状态量），故其解也不唯一，只能求解通解。后面的就不求解了，讲到这儿就和SLAM系统联系上了！！更进一步，线性代数中的自由变量个数= $n - R a n k (A)$ ,其中 $n$ 为 $x$ 变量的个数。

3.SLAM系统中的可观性定义

对于测量系统 $\mathbf{z}=h(\boldsymbol{\theta})+\varepsilon$ ，其中 $\mathbf{z} \in \mathbb{R}^{n}$ 为测量值， $\boldsymbol{\theta} \in \mathbb{R}^{d}$ 为系统状态量, $\varepsilon$ 为测量噪声向量。 $h(\cdot)$ 是个非线性函数，将状态量映射成测量。对于理想数据，如果以下条件成立，则系统状态量 $\boldsymbol{\theta}$ 可观：
$\forall \theta, \forall \theta^{\prime} \in \mathbb{R}^{d},\left\{\theta \neq \theta^{\prime}\right\} \Rightarrow\left\{h(\theta) \neq h\left(\theta^{\prime}\right)\right\}$ （简单的讲就是状态量变，观测值就变）“ $\neq$ “这个是不等于符号，显示出来有问题！

对于SLAM系统而言（如单目VO），当我们改变状态量时，测量不变意味着损失函数不会改变，更意味着求解最小二乘时对应的信息矩阵 $\Lambda$ 存在着零空间。

4.SLAM中的零空间问题

只做逻辑分析，不做理论推导。证明麻烦的是，旋转参数化比较难。因为虽然说旋转只有3个自由度，但是参数化形式并没有三自由度的表达形式(四元数不是矩阵形式了，另外rpy的形式不知道能不能用)，这样就很难直接从矩阵的形式直接说明自由度的问题。故只动嘴皮子，理论推导希望大佬指点。

单目SLAM系统有7自由度不可观：6自由度姿态+尺度。
单目+IMU系统有4自由度不可观：yaw角+3自由度位置不可观。roll和pitch由于重力的存在而可观，尺度因子由于加速度计的存在而可观。

在求解增量方程中， $\delta x$ 就是我们需要求解的状态量，从结果逆推，若是VO系统状态量中我们已经知道结果为7个不可观自由度（即是二小节中的自由变量），那么逆推就可以知道Hessian矩阵存在一个7维的零空间，Hessian矩阵一定不满秩。而在滑动窗口的Margin过程中会导致Hessian矩阵的零空间变化，会造成求解出的 $\delta x$ 产生漂移，导致系统漂移！

比如: 求解单目 SLAM 进行 Bundle Adjustment 优化时，问题对应的信息矩阵 $\Lambda$ 不满秩，对应的零空间为 $\mathbf{N}$ , 用高斯牛顿求解时有：
$\mathbf{\Lambda }\delta \mathbf{x}=\mathbf{b}$ $\mathbf{\Lambda }\delta \mathbf{x}+\mathbf{N}\delta \mathbf{x}\text{ }=\mathbf{b}$
增量 $\delta x$ 在零空间维度下变化，并不会改变我们的残差。这意味着系统可以有多个满足最小化损失函数的解 x。而认为的加以干预很容易导致漂移！

5.解决

First Estimate Jacobian(FEJ)算法！

6.update

@larry_dongy 评论指正：
　　 “5. 解决。FEJ算法”，我个人觉得有误导性。（1）FEJ确实解决的是零空间的某些问题，但具体而言是在线性化时通过固定线性化点避免零空间维度的减少，从而避免引入错误信息而造成误差；（2）而零空间内的漂移，不是由于零空间塌缩导致的，而是由于优化求解过程中导致的（这部分可以参考这篇文章，里面的推导，感觉不错）。避免零空间漂移的方法，正交投影保留垂直部分增量。（3）按照我的理解，JEF和零空间漂移貌似是两个问题，博主可以再考虑一下？也避免其他看到这里的朋友产生误会。