Graph-based Optimization

1 综述

基于图优化的slam主要分为以下三个部分：

• 前端：基于传感器数据建图，匹配相邻帧，添加节点和边（raw graph），节点表示机器人的位姿，边表示节点之间的位姿联系。位姿信息可以来自里程计计算，可以来自ICP激光点云匹配，也可以来自闭环检测反馈。
• 后端：优化图，基于历史信息的约束，调整新加入的机器人位姿顶点使其尽量满足边的约束（optimized graph）。
• 宏观的闭环检测：根据闭环信息优化矫正整个拓扑图。

这里面涉及到了两个优化，一个是后端局部优化，一个是全局闭环优化，两者计算的思路是一样的。

2 优化

2.1 全局闭环优化，用于矫正整个拓扑图

前端后端完成的事情是探索并创建新的节点，获得新的测量值，添加新的位姿关系方程：

$$\begin{eqnarray} \begin{split} & x_0 + z_{01} = x_1\\ & x_1 + z_{12} = x_2\\ & ...\\ & x_{k-1} + z_{k-1, k} = x_{k}\\ \end{split} \end{eqnarray}$$

而全局闭环检测添加已知节点之间的位姿约束关系：

$$\begin{equation} x_i + z_{i j} = x_j, \ \ \ \ i,j\in [0, k] \end{equation}$$

再添加一个初始条件（不是必须的，但是后面实验表明固定一个顶点比不固定效果要好——相当于有一个明确可信的基准）：

$$x_0 = 0$$

• 以上线性方程组中，闭环检测部分的方程中的两个结点都在前面出现过，因此不增加矩阵的秩，因此最终要求解包含$k$个方程$k+1$个未知数的线性方程组。
• 闭环的关键性：如果没有闭环条件，方程组$Ax=b$左右两边秩是相等的——有唯一解，添加了闭环条件以后，相当于方程组左侧$A$的秩不变，但是右侧$b$的秩则增加了，$rank(A) < rank(A, b)$——没有解析解，只有最优。
• 实际上状态$\textbf x$是一个包含夹角$\theta$的向量$[x, y, \theta]$，实际相对位姿的计算并非简单的线性叠加：

$$\textbf x \oplus \Delta \textbf x = \begin{pmatrix} x + \Delta x cos\theta - \Delta y sin \theta \\ y + \Delta x sin\theta + \Delta y cos \theta \\ normAngle(\theta + \Delta \theta) \end{pmatrix}$$

举个栗子：

机器人从起始位置$x_0=0$处出发，里程计测得它向前移动了1m，到达$x_1$，接着测得它向后移动了0.8m，到达$x_2$，这时通过闭环检测，发现他回到了起始位置。

首先根据给出信息构建图：

$$x_0 + 1 = x_1\\ x_1 - 0.8 = x_2$$

然后根据闭环条件添加约束：

$$x_2 = x_0$$

补充初始条件：

$$x_0 = 0$$

使用最小二乘法求上述方程组的最优解，首先构建残差平方和函数：

$$\begin{eqnarray} \begin{split} & f_1 = x_0 = 0\\ & f_2 = x_1 - x_0 - 1 = 0\\ & f_3 = x_2 - x_1 + 0.8 = 0\\ & f_4 = x_2 - x_0 = 0 \end{split} \end{eqnarray}$$ $$c = \Sigma_{i=1}^4 f_i^2 = x_0^2 + (x_1-x_0-1)^2 + (x_2-x_1+0.8)^2 + (x_2-x_0)^2$$

然后对每个参数求偏导：

$$\frac{\partial c}{\partial x_1} = -x_0 + 2x_1-x_2 -1.8=0\\ \frac{\partial c}{\partial x_2} = -x_0 - x_1 +2x_2 + 0.8 = 0$$

解得$x_1 = 0.93, x_2 = 0.07$，可以看到闭环矫正了所有节点的位姿，优化了整个拓扑图。

2.2 后端局部优化，用于矫正局部地图

再举个栗子：

机器人从起始位置$x_0=0$处出发，并观测到其正前方2m处有一个路标$l_0$，里程计测得它向前移动了1m，到达$x_1$，这时观测到路标在其正前方0.8m处。

首先根据前端信息建图 raw graph（这样建图明显是存在累积误差的）：

$$x_0 + 1 = x_1$$

然后添加闭环约束：

$$x_1 + 0.8 = l_0\\ x_0 + 2 = l_0$$

初始条件：

$$x_0 = 0$$

构建残差平方和：

$$c = \Sigma_{i=1}^4 f_i^2 = x_0^2 + (x_1-x_0-1)^2 +(l_0-x_0-2)^2 + (l_0-x_1-0.8)^2$$

求偏导求解：$x_1 = 1.07, l_0 = 1.93$，可以看到后端是对前端新添加进来的节点位姿做了矫正，消除部分测量误差。

这里面涉及到两种传感器信息——里程计和激光雷达，两种传感器的精度是有差别的，我们对其的信任程度也应该不同，反映到公式中就是要为不同传感器信息赋予不同的权重。假设编码器信息更准确，那么：

$$c = \Sigma_{i=1}^4 f_i^2 = x_0^2 + \textbf{10}(x_1-x_0-1)^2 +(l_0-x_0-2)^2 + (l_0-x_1-0.8)^2$$

调整权重之后解得：$x_1 = 1.01, l_0 = 1.9$，可以看到计算结果会向着更信任的传感器的测量结果靠近。

2.3 严格推导

2.3.1 信息矩阵（误差权重矩阵)

图优化问题转化为最小二乘问题，首先是带权重的残差平方和函数的一般形式：

$$F(x) = \Sigma_{i,j} e(x_i, x_j, z_{i,j})^T\Omega_{i,j}e(x_i, x_j, z_{i,j})$$ $$x^{*} = argminF(x)$$

其中的$\Omega_{i,j}$项就是上文提到的误差权重矩阵，它的正式名字叫信息矩阵。

传感器的测量值，可以看作是以真值为中心的多元高斯分布：

$$f_x(x_1, x_2, ..., x_k) = \frac{1}{\sqrt{}(2\pi)^k|\Sigma|}exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))$$

协方差矩阵$\Sigma$对角线上的值表示每一维对应的方差，该方差值越大表示该维度不确定性越大，对应的信息权重应该越小。实际上拓扑图上每条边对应的信息矩阵就是对应测量协方差矩阵的逆。

2.3.2 非线性

上文已经提到，位姿变化非线性——非线性最小二乘问题，要采用迭代法求解。迭代法需要有一个好的初始假设值，然后在这个值附近增量式迭代寻找最优解。

$$f(x) = \Sigma e^T\Omega e\\ 最小二乘问题目标函数：min \frac{1}{2}||f(x)||_2^2$$

首先要将非线性函数转化成关于增量$\Delta x$的线性函数——泰勒展开，根据具体的展开形式又分为：

• 一阶、二阶梯度法

  直接对目标函数在$x$附近进行泰勒展开：

  $$||f(x+\Delta x)||_2^2 \approx ||f(x)||_2^2 +J(x) \Delta x = \frac{1}{2} \Delta x^T H \Delta x$$

  一阶梯度法（最速下降法）：只保留一阶梯度，并引入步长$\lambda$：

  $$\Delta x^* = -\lambda J^T(x)$$

  二阶梯度法（牛顿法）：保留一阶和二阶梯度信息

  $$J^T(x)+H\Delta x^*=0$$

  最速下降法过于贪心，容易走出锯齿路线，增加迭代次数。牛顿法需要计算目标函数的二阶导数（Hessian矩阵），计算困难。

• 高斯牛顿法

  对$f(x)$而不是目标函数$f(x)^2$在$x$附近进行一阶泰勒展开：

  $$f(x+\Delta x) \approx f(x) + J(x) \Delta x$$

  对应每一个误差函数$e_{ij}$：

  $$\begin{split} & e_{ij}(x_i+\Delta x_i, x_j+\Delta x_j) = e_{i,j}(x+\Delta x) \\ &\approx e_{ij} +\frac{\partial e_{ij}}{\partial x}\Delta x = e_{ij} + J_{ij}\Delta x \end{split}$$

  ​ 其中$J_{ij}$为初始值附近的雅可比矩阵（定义见卡尔曼滤波）。

  带入目标函数得到近似二阶展开：

  $$\begin{split} & F_{ij}(x+\Delta x) = e_{ij}(x+\Delta x)^T \Omega_{ij}e_{ij}(x+\Delta x)\\ & \approx (e_{ij} + J_{ij}\Delta x)^T \Omega_{ij}(e_{ij} + J_{ij}\Delta x)\\ & = \underbrace{e_{ij}^T\Omega_{ij}e_{ij}}_{c_{ij}} + 2\underbrace{e_{ij}^T\Omega_{ij}J_{ij}}_{b_{ij}^T}\Delta x + \Delta x^T \underbrace{J_{ij}^T\Omega_{ij}J_{ij}}_{H_{ij}}\Delta x\\ & = c_{ij} + 2b_{ij}^T\Delta x + \Delta x^T H_{ij}\Delta x \end{split}$$

  求解增量$\Delta x$：

  $$2b + 2H\Delta x^* = 0\\ 高斯牛顿方程：H\Delta x^* = -b\\$$

  对比牛顿法可见，高斯牛顿法用$J^TJ$作为二阶Hessian矩阵的近似，简化了计算。

  上述算法要求近似$H$矩阵是正定且可逆的，实际数据很难满足，因而在使用高斯牛顿算法时可能出现$H$为奇异矩阵或病态的情况，增量稳定性较差，导致算法不收敛。

  图形上来思考，就是近似后的梯度方向不再是梯度变化最快的方向，可能引起不稳定。

• 列文伯格—马夸尔特法

  为$\Delta x$添加一个信赖区域，不让它因为过大而使得近似$f(x+\Delta x) = f(x) + J(x)\Delta x$不准确。

  $$\rho = \frac{f(x+\Delta x) - f(x)}{J(x) \Delta x}$$

  可以看到如果$\rho$接近1，说明近似比较好。如果$\rho$比较大，说明实际减小的值远大于估计减小的值，需要放大近似范围，反之你懂的。

  $$||D\Delta x_k||_2^2 \leq \mu$$

  将每次迭代得到的$\Delta x$限定在一个半径为信赖区域的椭球中，根据$\rho$的大小修改信赖区域。于是问题转化成为了带不等式约束的优化问题：

  $$min_\Delta x \frac{1}{2}||f(x) + J(x)\Delta x||^2_2, \ \ s.t. ||D \Delta x ||^2 = \mu$$

  用拉格朗日乘子转化成无约束问题：

  $$min_\Delta x \frac{1}{2}||f(x) + J(x)\Delta x||^2_2 + \frac{\lambda}{2}||D \Delta x ||^2$$

  展开后得到如下形式：

  $$(H + \lambda D^TD)\Delta x^* = -b$$

  通常把$D$取值为单位阵$I$，得到更简化形式：

  $$(H + \lambda I)\Delta x^* = -b$$

  当$\lambda$较小时，$H$占主要地位，说明二次近似模型较好，LM算法更接近高斯牛顿法。当$\lambda$较大时，$\lambda I$占主要地位，LM算法更接近一阶梯度法。修正了线性方程组矩阵的病态问题，比高斯牛顿法更加健壮，但是收敛速度也更慢。

  图形上思考，LM算法修正了高斯牛顿法得到的梯度，以此固定一个搜索区域，在区域内寻找最优。

2.3.3 稀疏矩阵

对于误差函数$e_{ij}$，它只和$e_i$和$e_j$有关，因此它的雅可比矩阵有如下结构（行数是$x$的维度，列数是拓扑图中节点映射关系的数目）：

$$J_{ij} = \begin{bmatrix} 0 & ... & 0 & \underbrace{\frac{\partial e_{i}}{\partial x_i}}_{A_{ij}} & 0 & ... & \underbrace{\frac{\partial e_{j}}{\partial x_j}}_{B_{ij}} & 0 & ... & 0 \end{bmatrix}$$

相应地$b_{ij}$是一个包含很多0的列向量：

$$\begin{split} b_{ij}^T& = e_{ij}^T \Omega_{ij} J_{ij}\\ &= e_{ij}^T\Omega_{ij} (0 ... A_{ij}...B_{ij}...0)\\ &=(0...e_{ij}^T\Omega_{ij}A_{ij}...e_{ij}^T\Omega_{ij}B_{ij}...0) \end{split}$$

$b = \Sigma_{ij} b_{ij}$：

$H_{ij}$是一个包含很多0的对称阵：

$$\begin{split} H_{ij}& = J_{ij}^T \Omega_{ij}J_{ij} \\ & = \begin{pmatrix} ...\\ A_{ij}^T\\ ...\\ B_{ij}^T\\ ... \end{pmatrix} \Omega_{ij} \begin{pmatrix} ... & A_{ij} & ... & B_{ij} & ... \end{pmatrix}\\ & = \begin{pmatrix} & \\ & A_{ij}^T\Omega{ij}A_{ij} & A_{ij}^T\Omega_{ij}B_{ij} & \\ & \\ & B{ij}^T\Omega_{ij}A_{ij} & B_{ij}^T\Omega_{ij}B_{ij}& \\ & \end{pmatrix} \end{split}$$

$H=\Sigma_{ij}H_{ij}$：

梳理一下计算流程：$e_{ij} \to J_{ij} \to A_{ij}, B_{ij} \to b_{ij}, H_{ij} \to b, H \to \Delta x^* \to x$

2.3.4 误差函数

前面定义过位姿的非线性叠加，显然位姿误差也不是简单的线性加减关系：

$$e_{ij}(x_i, x_j) = t2v(Z_{ij}^{-1}(X_i^{-1}.X_j))$$

其中的$Z_{ij}$、$X_i$、$X_j$都是矩阵形式。$X_i^{-1}X_j$表示节点j到节点i之间的位姿差异$\hat Z_{ij}$，假设这个转移矩阵形式如下：

$$\hat Z_{ij} = \begin{bmatrix} R_{2*2} & t_{2*1} \\ 0 & 1 \end{bmatrix}$$

假设测量值$Z_{ij}$形式如下：

$$Z_{ij} = \begin{bmatrix} R^{'} & t^{'}\\ 0 & 1 \end{bmatrix}$$

分块矩阵的求逆过程如下：

$$\begin{split} \begin{bmatrix} R & t\\ 0 & 1 \end{bmatrix}^{-1}& = \begin{bmatrix} \begin{bmatrix} I & t\\ 0 & 1 \end{bmatrix} \begin{bmatrix} R & 0\\ 0 & 1 \end{bmatrix} \end{bmatrix}^{-1}= \begin{bmatrix} R & 0\\ 0 & 1 \end{bmatrix}^{-1} \begin{bmatrix} I & t\\ 0 & 1 \end{bmatrix}^{-1}\\ & =\begin{bmatrix} R^T & 0\\ 0 & 1 \end{bmatrix} \begin{bmatrix} I & -t\\ 0 & 1 \end{bmatrix}= \begin{bmatrix} R^T & -R^Tt\\ 0 & 1 \end{bmatrix} \end{split}$$

所以误差$e_{ij}$计算如下：

$$E_{ij} = Z_{ij}^{-1}\hat Z_{ij} = \begin{bmatrix} R^{'} & t^{'}\\ 0 & 1 \end{bmatrix}^{-1} \begin{bmatrix} R & t\\ 0 & 1 \end{bmatrix}= \begin{bmatrix} R^{'T} & -R^{'T}t^{'}\\ 0 & 1 \end{bmatrix} \begin{bmatrix} R & t\\ 0 & 1 \end{bmatrix}= \begin{bmatrix} R^{'T}R & R^{'T}(t-t^{'})\\ 0 & 1 \end{bmatrix}$$ $$e_{ij} = t2v(E_{ij})= \begin{bmatrix} R_z(t_{\hat z} - t_z)_{2*1}\\ \theta_\hat z - \theta_z \end{bmatrix}_{3*1}= \begin{bmatrix} R_z(x_{\hat z} - x_z)\\ R_z(y_{\hat z} - y_z)\\ \theta_\hat z - \theta_z \end{bmatrix}= \begin{bmatrix} R_z[R_i(x_{j} - x_i) - x_z]\\ R_z[R_i(y_{j} - y_{i}) - y_z]\\ \theta_j - \theta_i - \theta_z \end{bmatrix}$$

求解雅可比矩阵$J_{ij}$：

$$A_{ij} = \begin{bmatrix} \frac{\partial e_1}{\partial x_i} & \frac{\partial e_1}{\partial y_i} & \frac{\partial e_1}{\partial \theta_i}\\ \frac{\partial e_2}{\partial x_i} & \frac{\partial e_2}{\partial y_i} & \frac{\partial e_2}{\partial \theta_i}\\ \frac{\partial e_3}{\partial x_i} & \frac{\partial e_3}{\partial y_i} & \frac{\partial e_3}{\partial \theta_i}\\ \end{bmatrix}= \begin{bmatrix} -R_z^TR_i^T & 0 & R_z^T\frac{\partial R_I^T}{\partial \theta_i}(x_j-x_i)\\ 0 & -R_z^TR_i^T & R_z^T\frac{\partial R_I^T}{\partial \theta_i}(y_j-y_i)\\ 0 & 0 & -1 \end{bmatrix}$$ $$B_{ij} = \begin{bmatrix} R_z^TR_i^T & 0\\ 0 & 1 \end{bmatrix}$$

累加$b$和$H$矩阵：

$$b_{[i]} += A_{ij}\Omega_{ij}e_{ij}\\ b_{[j]} += B_{ij}^T\Omega_{ij}e_{ij}\\ H_{[ii]} += A_{ij}^T\Omega_{ij}A_{ij}\\ H_{[ij]} += A_{ij}^T\Omega_{ij}B_{ij}\\ H_{[ji]} += B_{ij}^T\Omega_{ij}A_{ij}\\ H_{[jj]} += B_{ij}^T\Omega_{ij}B_{ij}$$