视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题

专栏系列文章如下：

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客_global shutter

第六讲：非线性优化

经典SLAM模型的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。得到的数据通常是受各种未知噪声影响的。即使有高精度的相机，运动方程和观测方程也只能近似成立。所以问题是如何在有噪声的数据中进行准确的状态估计，这需要一定程度的最优化背景知识。

6.1 状态估计问题

6.1.1 批量状态估计与最大后验估计

经典 SLAM 模型由一个运动方程和一个观测方程构成：

xk是相机的位姿变量，可以由Tk∈SE(3)表达。运动方程与输入的具体形式有关，在视觉SLAM中没有特殊性（和普通的机器人、车辆的情况一样）。观测方程则由针孔模型给定。假设在xk处对路标yj进行了一次观测，对应到图像上的像素位置zk,j，那么，观测方程可以表示成

其中K为相机内参，s为像素点的距离，也是(Rkyj +tk)的第三个分量。如果使用变换矩阵Tk描述位姿，那么路标点yj必须以齐次坐标来描述，计算完成后要转换为非齐次坐标。

在运动和观测方程中，通常假设两个噪声项wk,vk,j满足零均值的高斯分布：

其中N表示高斯分布，0表示零均值，Rk,Qk,j为协方差矩阵。在这些噪声的影响下，希望通过带噪声的数据z和u推断位姿x和地图y（以及它们的概率分布），这构成了一个状态估计问题。

处理这个状态估计问题的方法分成两种。

第一种：由于在SLAM过程中，这些数据是随时间逐渐过来的，所以在直观上，应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量（incremental）的方法，或者叫滤波器。在SLAM的早期研究，主要使用扩展卡尔曼滤波器（EKF）及其衍生方法来求解。

第二种：是把数据累加起来一并处理，这种方式称为批量（batch）的方法。例如，可以把0到k时刻所有的输入和观测数据都放在一起，求在这样的输入和观测下，如何估计整个0到k时刻的轨迹与地图？

增量方法仅关心当前时刻的状态估计xk，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，成为当前视觉SLAM的主流方法。极端情况下，可以让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，这也正是SfM（Structure from Motion）的主流做法。这种极端情况不实时，不符合SLAM的运用场景。所以在SLAM中，实用的方法通常是一些折中的手段。比如，固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，这就是滑动窗口估计法。还有因子图增量平滑优化的方法，能够增量增加优化问题并进行动态调整，能够达到有滤波器的速度和图优化的精度。

先讨论批量方法，考虑从1到N的所有时刻，并假设有M个路标点。定义所有时刻的机器人位姿和路标点坐标为：

用不带下标的u表示所有时刻的输入，z表示所有时刻的观测数据。对机器人状态的估计，从概率学的观点来看，就是已知输入数据u和观测数据z的条件下，求状态x,y的条件概率分布：

特别地，当不知道控制输入，只有一张张图像时，即只考虑观测方程带来的数据时，相当于估计P(x,y|z)的条件概率分布，此问题也称为Structure from Motion（SfM），即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布，利用贝叶斯法则，有：

贝叶斯法则左侧称为后验概率，右侧的 P(z|x) 称为似然（Likehood），另一部分 P(x) 称为先验（Prior）。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化（Maximize a Posterior，MAP），则是可行的：

贝叶斯法则的分母部分与待估计的状态x,y无关，因而可以忽略。贝叶斯法则说明，求解最大后验概率等价于最大化似然和先验的乘积。进一步，如果不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计（Maximize Likelihood Estimation，MLE）：

似然是指“在现在的位姿下，可能产生怎样的观测数据”。但是由于知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

6.1.2 最小二乘的引出

如何求最大似然估计呢？在高斯分布的假设下，最大似然能够有较简单的形式。回顾观测模型，对于某一次观测：

假设噪声项vk ∼ N (0,Qk,j)，观测数据的条件概率为：

它依然是一个高斯分布。考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。

高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布x ∼ N(µ,Σ)，它的概率密度函数展开形式为：

对其取负对数，则变为：

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的x时，第一项与x无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入SLAM的观测模型，相当于在求：

该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离（Mahalanobis distance），又叫马氏距离。它也可以看成是由(Qk,j)-1 加权之后的欧氏距离（二范数），这里(Qk,j)-1也叫做信息矩阵，即高斯分布协方差矩阵之逆。

现在考虑批量时刻的数据。通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测也是独立的。于是可以对联合分布进行因式分解：

这说明可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

那么，最小化所有时刻估计值与真实值之间的马氏距离，等价于求最大似然估计。负对数允许把乘积变成求和：

这样就得到了一个最小二乘问题（Least Square Problem），它的解等价于状态的最大似然估计。直观上看，由于噪声的存在，当我们把估计的轨迹与地图代入 SLAM 的运动、观测方程中时，它们并不会完美地成立。这时对状态的估计值进行微调，使得整体的误差下降一些。当然这个下降也有限度，它一般会到达一个极小值。这就是一个典型非线性优化的过程。

SLAM 中的最小二乘问题具有一些特定的结构：

• 整个问题的目标函数由许多个误差的（加权的）二次型组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如，运动误差只与xk−1,xk 有关，观测误差只与xk,yj有关。这种关系会让整个问题有一种稀疏的矩阵形式，计算量大大减少。

• 如果使用李代数表示增量，则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束，即需在问题中加入RTR = I且det(R) =1的条件。额外的约束会使优化变得更困难。这体现了李代数的优势。

• 使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如，某次的观测非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”，所以这个误差项会在整个问题中占有较高的权重。

6.1.3 例子：批量状态估计

考虑一个离散时间系统：

这可以表达一辆沿 x 轴前进或后退的汽车。第一个公式为运动方程，uk 为输入，wk 为噪声；第二个公式为观测方程，zk 为对汽车位置的测量。取时间 k = 1,...,3，现希望根据已有的 v,y 进行状态估计。设初始状态 x0 已知，来推导批量（batch）状态的最大似然估计。

首先，令批量状态变量为x = [x0,x1,x2,x3]T，令批量观测为z = [z1,z2,z3]T，定义u = [u1,u2,u3]T。最大似然估计为：

运动方程：

观测方程：

构建误差变量：

于是最小二乘的目标函数为

这个系统是线性系统，将它写成向量形式。定义向量y = [u,z]T，那么可以写出矩阵H，使得：

那么：

整个问题可以写成：

这个问题有唯一的解：