SLAM中的数学基础四-基于因子图的状态估计

贝叶斯网络下表示下的完全SLAM系统能很方便地转换成因子图表示，这样贝叶斯网络中的最大后验估计就等效为因子图中的最小二乘估计，接下来求解最小二乘问题（通常为非线性最小二乘问题）。求解非线性最小二乘问题的方法大致上有两种：

• 一是先对该非线性问题进行线性化近似处理，然后直接求解线性方程得到待估计量。
• 二是不直接求解，通过迭代策略让目标函数快速下降到最值处，对应的估计量就求出了。

7.5.1 非线性最小二乘估计

式7-50描述了SLAM问题的最小二乘形式，式中包含了2种类型的约束，运动约束和观测约束。将这两种约束用统一的方式来表述，式7-50就可以改写成式7-123所示的形式，由于式7-123通常是非线性函数，故这是一个非线性最小二乘。
$$
min_x\sum_i||y_i-f(x_i)||_{\Sigma_i^{-1}}^2\tag{7-123}
$$

7.5.2 直接求解方法

​ 求解式7-123，可以将函数$f$进行线性化近似，从而将非线性最小二乘转换成线性最小二乘，然后直接求解线性方程就能得到线性化最小二乘的解。

​ 可以使用泰勒展开进行线性化，即非线性函数$f(x)$在$x_0$附近的局部区域取值可以用泰勒级数来近似。在对精度没有要求时，可以用最简单的一阶泰勒展开对非线性函数$f(x)$进行线性化。

​ 线性化后，线性最小二乘的直接求解方法，就是解$Ax=b$这个线性方程。该线性方程由于矩阵A通常为不可逆矩阵，所以不能直接求逆运算。该线性方程通常采用数值方法求解，例如$Cholesky$分解和$QR$分解。

7.5.3 优化方法

实际SLAM问题的非线性最小二乘问题中，难以找到合适的线性化方法，初值也比较难以确定。

求解非线性最小二乘问题常用的方法是优化方法，优化方法通过迭代的方式，按照一定的策略不断地调整自变量的取值使得代价函数逐渐变小，当代价函数不再下降或者下降幅度很小时，迭代就完成了。

5中主流的迭代策略：

• 梯度下降算法
• 最速下降算法
• 高斯牛顿算法
• 列文伯格*马夸尔特算法
• 狗腿算法

1. 梯度下降算法

将式7-123中的误差累计运算用函数$\psi(x)$表示，7-130所示，其中$x=[x_1,x_2,\cdots,x_i,\cdots]^T$是一个多维向量，每一个元素$x_i$都是一个待估计的状态量。
$$
\psi(x)=\sum_i||y_i-f(x_i)||_{\Sigma_i^{-1}}^2\tag{7-130}
$$
非线性问题就是求解函数$\psi(x)$最小值的问题。梯度下降，其实就是自变量$x$沿梯度反方向进行调整，对应的函数$\psi(x)$取值就会下降，这样不断地按照梯度方向调整自变量x，使函数$\psi(x)$的取值下降到最小为止，如式7-131。

其中，$x^{(k)}$表示第k步迭代中自变量x的取值，$\alpha$表示自变量x的调整步长，$\triangledown\psi(x^k)$表示函数在$x^k$处的梯度。
$$
do\lbrace x^{(k+1)}=x^{(k)}-\alpha\cdot\triangledown\psi(x^k)\
\rbrace while(\psi(x*{k+1})<\psi(x^{(k)}))\tag{7-131}
$$

在SLAM中，函数$\psi(x)$的自变量$x=[x_1,x_2,\cdots,x_i,\cdots]$的每个元素$x_i$表示机器人或者路标的空间位姿$pose$，而位姿由三维坐标和空间姿态两部分组成，其中空间姿态角通常由四元数表示。式7-131进行梯度下降时，涉及对该pose变量求导数，并对pose进行加法迭代运算，而pose内部存在额外的约束导致求导数和求和运算不能直接进行，所以需要将pose转换到李代数上求导数和求和。

2.最速下降算法

梯度下降算法中的步长$\alpha$的选取至关重要，最速下降法就是梯度下降法的一种具体形式。

最速下降法中，每次迭代都选取一个合适的步长$\alpha$，使得函数沿当前梯度反方向下降。如式7-132，按照这个策略选取步长$\alpha$：
$$
\alpha_k=arg min_{\alpha \geq0} \psi(x_k-\alpha\cdot\triangledown\psi(x^k))\tag{7-132}
$$

3. 高斯*牛顿算法

最速下降算法使用梯度来指导迭代的方向，高斯牛顿算法将从另外一个角度来指导迭代的方向。

(1) 梯度、雅可比矩阵和海森矩阵

讨论自变量$x=[x_1,x_2,\cdots,x_n]^T$为多维变量，因变量$f(x)$为一维或多维的情况，如式7-133和式7-134。
$$
f(x)=f(x_1,x_2,\cdots,x_n)\tag{7-133}
$$

$$
f(x)=\left[
\begin{matrix}
f_1(x_1,x_2,\cdots,x_n)\
f_2(x_1,x_2,\cdots,x_n)\
\cdots\
fm(x_1,x_2,\cdots,x_n)
\end{matrix}
\right]\tag{7-134}
$$

自变量多维，因变量一维的情况，函数的一阶导数就是梯度，函数的二阶导数就是海森矩阵。

自变量多维，因变量多维的情况，函数的一阶导数就是雅克比矩阵，函数的二阶导数就是由多个海森矩阵组成的一个大矩阵。

(2)牛顿算法

牛顿算法是采用二阶泰勒展开对$\psi(x)$进行近似，然后在近似局部域中找到局部最小值，并作为下一个迭代点。

(3)高斯-牛顿算法

见《机器人SLAM导航-核心技术与实战》P213

4.列文伯格-马夸尔特算法

高斯牛顿算法迭代过程收敛很快，但是只有当展开点$x^k$在$\psi(x)$目标点附近时，迭代效果才会比较好。

LM算法的关键是用策略去确定每一步迭代过程$\mu_k$的取值。

5.狗腿算法

首先计算代价函数$\psi(x)$的梯度，然后利用式7-132确定步长$\alpha_k$。

各种优化方法对比

• 梯度下降法，其实就是一种暴力算法，下降的效率时好时坏。
• 最速下降法是梯度下降法的一种更具体的形式，其虽然比梯度下降法快，但需要在寻找合适的步长$\alpha_k$上付出额外计算代价。
• 高斯-牛顿法将代价函数$\psi(x)$在迭代点$x^k$上做展开近似，当展开点附近区域的线性度好时，高斯牛顿法下降比较最速下降法要快。
• LM算法是高斯-牛顿法的一种改进算法，解决了展开点附近区域的线性度差时，高斯-牛顿法不一定下降的问题。其思路是利用线性度$\gamma_k$来调节高斯牛顿的阻尼系数$\mu_k$。
• 狗腿算法是高斯牛顿法的另一种改进，解决了LM算法中存在的拒绝更新问题。其思路是利用线性度$\gamma_k$来调节置信域$d_k$，然后用置信域$d_k$指导高斯牛顿法和梯度下降法的混合过程。

每一种方法都是前一种的改进，下降速度越来越好，但是计算代价也越来越高，因此实际中应该权衡利弊。

常用优化工具

以上的优化算法都有大量的现场代码实现库。

例如：

• Ceres-Solver
• g2o
• GTSAM
• iSAM

典型的SLAM算法

针对在线SLAM系统，以扩展卡尔曼滤波为代表的滤波方法是求解该状态估计问题最典型的方法。

对完全SLAM系统，也可以使用滤波方法求解，如FAST-SLAM，但是贝叶斯网络下的完全SLAM系统能够很方便地转换为因子图表示，利用因子图表示完全SLAM问题后，使用非线性最小二乘估计进行求解会更方便。