SLAM因子图构建笔记

因子图简介

  最近在读了Joan Sola所写的Course on SLAM中有关因子图部分的介绍后，发现其中有关于因子图构建的思路觉得很有意思，因此在这里记录一下。

DBN网络

  首先简单地介绍一下如何将一个SLAM问题建立成为一个DBN（Dynamic Bayes Network 动态贝叶斯网络）。DBN是一种统计模型，它可以用一个有向无环图来表示随机变量以及它们之间的依赖关系。它们之间的条件依赖可以用用箭头表示。比如$B⟶A$代表A在B条件下的概率，无环则表示不会出现诸如A依赖于B，B依赖于C，C又依赖于A的情况。在SLAM问题中，我们有四种类型的随机变量：

其中运动模型表示为：
$$x_i=f_i(x_{i-1},u_i)+w_i$$用子图表示$x_i$在给定$x_{i-1}$以及$u_i$时的条件概率即为：

同样，测量模型可以表示为：
$$z_k=h_k(x_i,m)+v_k$$用子图表示为：

m表示传感器对周围环境的测量值，在SLAM问题中经常表示为路标点的位置(landmark)，因此在传感器姿态为$x_i$时测量得到的路标点$l_j$的测量模型为：$$z_k=h_k(x_i,l_j)+v_k$$此时子图可以表示为：

将所有的子图聚合在一起可以得到完成的DBN网络：

此时所有随机变量的联合概率写为：
$$P(X,L,U,Z)\propto P(X_0)\prod _{i=1}^{M}P(x_i|x_{i-1},u_i)\prod _{k=1}^{K}P(z_k|x_{i_k},l_{j_k})$$其中$P(x_0)$表示初始时的先验概率，我们最终要优化的目标函数和状态量是：
{ X ∗ , L ∗ } = arg ⁡ max ⁡ X , L P ( X 0 ) ∏ i = 1 M P ( x i ∣ x i − 1 , u i ) ∏ k = 1 K P ( z k ∣ x i k , l j k ) \{X^*,L^*\}=\arg\max_{X,L} P(X_0)\prod_{i=1}^{M}P(x_i|x_{i-1},u_i)\prod_{k=1}^{K}P(z_k|x_{i_k},l_{j_k}) {X∗,L∗}=argX,Lmax​P(X0​)i=1∏M​P(xi​∣xi−1​,ui​)k=1∏K​P(zk​∣xik​​,ljk​​)

SLAM转化为因子图

  联合概率问题可以转化表示成$M+K$个因子组成的图，每一个因子由测量值（可以是控制量$U$也可以是观察量$Z$）产生，并且连接在不同的节点（位姿或者路标点或者两者都有）两端。所以一个因子图通常由两个节点组成：变量节点（variable）该节点由我们需要求解的变量组成，比如位姿或者路标点的位置，以及因子（factor），表示两个节点之间的约束关系。

已知运动和测量模型为：

因此每一个因子的条件概率写为：

我们可以将上边的条件概率（i.e factor）表示为更加紧凑的形式，令：
$$e_k(x_{i_k-1},x_{i_k})=f_{i_k}(x_{i_k-1},u_{i_k})-x_{i_k}\text{\hspace{1em}(1)}$$
$$e_k(x_{i_k},l_{j_k})=h_k(x_{i_k},l_{j_k})-z_k\text{\hspace{1em}(2)}$$
此时所有的因子可以写成一个统一的形式：
$${\varphi }_k=\exp (-\frac{1}{2}{e}_k^{\top }{\Omega }_k{e}_k)$$
这表明，只要我们可以从第k个测量值与状态量$i_k$和$j_k$里边计算得到误差项$e_k$
$$e_k(x_{i_k},x_{j_k},z_k)$$
那么该测量值具体是控制变量$U$还是传感器的测量值$L$就不重要了，换句话说，控制变量$U$和测量值$L$都可以作为测量值$Z$构建因子。 有$K$个测量值就有$K$个因子。（注：这里的$K$和上边表示路标点数量的$K$我猜测应该不是一个$K$，这里的$K$表示控制变量和路标点数量的总和，而上边的K仅表示路标点的数量）
  现在我们的待估计量为：
$$\mathbf{x}=[{\mathbf{x}}_1,...,{\mathbf{x}}_N{]}^{\top }$$
x i = { X i , L i } , i ∈ 1 , 2 , . . . , N \mathbf{x}_i=\{X_i,L_i\},i\in1,2,...,N xi​={Xi​,Li​},i∈1,2,...,N
测量值为：
$$\mathbf{z}=[{\mathbf{z}}_1,...,{\mathbf{z}}_K{]}^{\top }$$
z j = { U j , Z j } , j ∈ 1 , 2 , . . . , K \mathbf{z}_j=\{U_j,Z_j\},j\in1,2,...,K zj​={Uj​,Zj​},j∈1,2,...,K
此时的联合概率为：
$$P(\mathbf{x},\mathbf{z})\propto \prod _{i=1}^K{\varphi }_k\propto \prod _{i=1}^K\exp (-\frac{1}{2}{\mathbf{e}}_k^{\top }{\mathbf{\Omega }}_k{\mathbf{e}}_k)$$
对该联合概率取负对数可以将$\exp (\cdot )$消掉，得到：
$${\mathbf{x}}_{opt}=\underset{\mathbf{x}}{\mathrm{argmax}}\sum _{k=1}^K{\mathbf{e}}_k({\mathbf{x}}_i,{\mathbf{x}}_j{)}^{\top }{\mathbf{\Omega }}_k{\mathbf{e}}_k({\mathbf{x}}_i,{\mathbf{x}}_j)$$

有关于运动误差因子的构建技巧

从上边的推导中，当给定我们一个运动向量${\mathbf{u}}_i$，我们可以利用运动模型构建运动误差为：
$$\mathbf{e}=f({\mathbf{x}}_{i-1},{\mathbf{u}}_i)-{\mathbf{x}}_i$$但是这样构建误差是有一个前提假设的，那就是高斯噪声${\mathbf{w}}_i$在函数$f(\cdot )$之外。即：
$${\mathbf{x}}_i=f_x({\mathbf{x}}_{i-1},{\mathbf{u}}_i)+{\mathbf{w}}_i,{\mathbf{w}}_i\sim \mathcal{N}(0,\mathbf{\Omega })\text{\hspace{1em}(3)}$$
但是显示情况中，我们更常见的却是如下的运动模型：
$${\mathbf{x}}_i=f_x({\mathbf{x}}_{i-1},{\mathbf{u}}_i-{\mathbf{w}}_i),{\mathbf{w}}_i\sim \mathcal{N}(0,\mathbf{\Omega })\text{\hspace{1em}(4)}$$在这种情况下，我们常常做的是通过$f(\cdot )$关于${\mathbf{w}}_i$的$Jacobian$将原函数线性化，即：
$${\mathbf{x}}_i\approx f_x({\mathbf{x}}_{i-1},{\mathbf{u}}_i)+\frac{\partial f}{\partial \mathbf{w}}{\mathbf{w}}_i\text{\hspace{1em}(5)}$$
但是这样做非常容易发生由于$Jacobian$不满秩而导致的协方差矩阵$\mathbf{\Omega }$为奇异矩阵的错误（无法求逆），为了解决这个问题，我们通常避免$Jacobian$的计算，即可以将误差项表示为如下的形式：
$${\mathbf{u}}_i=f^{-1}({\mathbf{x}}_i,{\mathbf{x}}_{i-1})+{\mathbf{w}}_i$$
$$\mathbf{e}=f^{-1}({\mathbf{x}}_i,{\mathbf{x}}_{i-1})-{\mathbf{u}}_i$$
该形式和之前测量值的误差表达式形式一致：
$$\mathbf{e}=h({\mathbf{x}}_i,{\mathbf{l}}_j)-\mathbf{z}$$
但是在一些特殊的传感器上（比如IMU），$f^{-1}(\cdot )$可能不那么容易求得，此时我们就需要将测量值通过一个函数变形为：$\mathbf{z}=z(\mathbf{u})$，此时由控制变量得到的测量值写为：
$${\mathbf{z}}_i=g({\mathbf{x}}_i,{\mathbf{x}}_{i-1})+{\mathbf{w}}_i$$
误差项变为：
$$\mathbf{e}=g({\mathbf{x}}_i,{\mathbf{x}}_{i-1})-{\mathbf{z}}_i$$
由于IMU的测量值有6个自由度，因此导致$f(\cdot )$的非线性效应十分严重，逆函数不容易求出，一个解决方法就是利用观测的差值，即先将IMU的读数进行数次预积分，以降低$f(\cdot )$的维度，这样$f(\cdot )$的逆函数就比较容易求得了。