第6讲 非线性优化

6.1 状态估计问题

根据前面几章的内容，我们可以根据典型的SLAM模型，得到下面的状态方程和运动方程：

这个式子中，第一个式子描述了运动，也就是当前位置可以通过上一个时刻的位置、传感器的示数以及噪声计算得到，第二个式子则描述了一个状态关系，表示在一个位置上观测一个路标点，在噪声的结果下产生了一个观测结果。对于第二个式子，在第五章中又引入了相机模型的内容，在相机模型中，我们可以具体化这个式子，首先位姿xk可以用变换矩阵或者变换向量来表示，zkj这时作为观测结果，就可以使用像素位置，那么观测方程就可以变成下面的样子：

加上噪声之后必然会产生变化，一般我们用wk和vkj来表示运动方程和观测方程中的噪声，而且一般假设这个噪声都满足均值为零的高斯分布（即标准正态分布），这样引入噪声之后，我们希望利用带有噪声的数据去估计位姿和地图，也就构成了一个状态估计问题。也就是说，如果没有噪声，那么一切都可以标准地计算出来，但是由于噪声的存在，噪声具有不确定性，从而得到的数据也是有不确定性的，进而利用不确定的数据去推测位姿和地图。

这主要是一个概率论的问题，一般都会先将所有待估计的变量存放在一个状态变量中：

也就是说我们要在输入数据u和观测数据z的条件下，去计算状态变量的概率，也就成了一个条件概率，其中u和z就是已知的条件：

这个式子中，最恶心的就是小写字母的含义，这里的小写字母表示的都是向量，这可以看做在产生了现在这个状态的条件下，一开始未知参数是某某状态的概率。
如果忽略时间上的联系，将其看做一堆彼此没有关系的图片，这种问题就转换为了SfM问题，即从许多图像中重建三维空间结构，这种情况下，SLAM可以看作是图像具有先后顺序的，需要实时求解的一个SfM问题，那么利用贝叶斯法则，条件概率可以转换为下面的式子：

左侧一般称为后验概率，右侧的P(Z|X)则称为似然，另一部分P(X)称为先验，直接求后验的分布是困难的，但是求一个状态最优估计，使得在这个状态下，后验概率最大化是可行的，也就是要最大化上面式子中右边的部分。本质上来说，原来是利用观测结果去推测参数，现在换成将参数作为条件去估计结果。

对于贝叶斯公式，由于分母部分与待估计的向量x无关，所以可以直接去掉，所以求解最大后验概率，相当于最大化似然和先验的乘积，进一步来看，这里面的先验P(X)本身表示的是机器人处在某个某个状态的概率，如果不知道这个概率，那么就不知道先验，于是问题就转换为了最大似然估计，即最大化似然：

我们可以这样去理解，既然已经得到了z这个观察真实存在的结果，那么不妨最大化产生这个结果的概率，既然要追求刺激就刺激到底。这就是最大似然估计的直观意义。
这里多插一嘴，上面的两个式子，对应的刚好是最大似然估计和最大后验估计，其实两种估计方法本质上就差一个先验，最大似然估计认为先验都是相等的，从而不去考虑，而最大后验则不这么认为，在一些情况下后验会更加准确一些，二者都是利用结果反推参数：

现在我们确定了，要想得到一个比较准确的预测系数，就需要最大化这个似然函数，根据概率论的知识，我们需要利用负对数去最大化概率密度，对于这个概率分布，本身是一个正态分布，其中均值为观测结果，方差为噪声，也就是下面的式子：

利用概率论的知识展开处理，并通过负对数处理，我们得到最后的优化对象：

对原式的最大化，相当于对负对数的最小化，相当于我们在求一个最小的二次型项，满足这个式子最小化的状态向量就是我们要求的参数：

而这个式子等价于最小化误差的平方，因此对于所有的运动和任意的观测，我们定义数据与估计值的误差：

由此可以得到误差的平方的和：

再从头梳理一遍，我们最根本的目的，是要求一个状态参数，直接求这个参数是困难的，我们转换为求一个最大化的似然函数值，最大化似然函数，在这里具体化为最大化高斯分布，为了方便计算我们转换为最小化负对数，而最小化负对数，等价于最小化误差的平方，也就是优化最后一个式子J(X)。这样就得到了一个总体意义下的最小二乘问题，它的最优解等价于状态的最大似然估计。
直观来讲，由于噪声的存在，当我们把估计的轨迹与地图代入 SLAM 的运动、观测方程中时，它们并不会完美的成立。这时候怎么办呢？我们把状态的估计值进行微调，使得整体的误差下降一些。当然这个下降也有限度，它一般会到达一个极小值。

6.2 非线性最小二乘

前面的一节我们将求参数转换为了求解一个最小二乘问题，这一节主要是看如何解决一个最小二乘问题，首先看一个简单的最小二乘问题：

这个最小二乘问题要求寻找一个向量x，让一个非线性函数的值最小。如果函数f的形式很简单，那么可以直接用解析形式来求，也就是求导然后让导数等于零，直接解出来向量x的最优值，这就和求解一个二元函数的极值是一样的。这样直接解出来的点，可能是极值或者鞍点的值，最后挨个比较就可以得到最值。但是，方程是否容易求解，取决于导函数的形式，在SLAM中，由于我们使用李代数，所以并不能得到一个很容易求解的导函数，也就是说上面这种直接让导函数等于零然后算出极值的情况在SLAM中是不成立的。
对于不方便直接求解的最小二乘问题，我们一般使用迭代的方法，也就是从一个初始值出发，不断更新，使目标函数下降：

所以找极值的过程，就转换为了寻找梯度并下降的过程，其实说白了就是梯度下降法。前面看了那么久的CS231N，里面梯度下降法和梯度下降法的各种优化这里终于派上用场。

在求解最小二乘问题的过程中，关键在于求解增量，求解增量，最直观的方法是用泰勒展开，也就是将目标函数在x附近进行泰勒展开，从而将非线性函数转换为好计算的线性函数：

对于上面的泰勒展开，由于在SLAM中待优化函数是矩阵形式的，所以一阶导数和二阶导数必须都是矩阵形式，所以这里式子中展开成了雅可比矩阵和海森矩阵。我们可以选择保留泰勒展开的一阶项或者二阶项，对应的求解方法则为一阶梯度法或者二阶梯度法。
保留一阶梯度的情况，增量的方向为：

在这个方向上取一个步长，就可以得到一个最快的下降方向。这其实就是梯度下降法，只不过换成了矩阵形式，一些写法发生了变化。这种方法称为最速下降法。

另一方面，保留二阶梯度，那么增量方程就变为：

求右侧关于△x的导数并令其等于零，就可以得到增量的解：

这其实就是牛顿法，利用二阶导去优化，找到一阶导等于零的点，从而找到最小值。

上面的两种方法，各有各的缺点，梯度下降法过于贪心，容易走出锯齿路线，从而增加了迭代次数，对于牛顿法，需要计算海森矩阵，运算过程很麻烦。一般来说使用更加实用的两种方法：高斯牛顿法和列文伯格——马夸尔特方法。

高斯牛顿法是优化算法里最简单的方法，它的思想是将待优化函数进行一阶泰勒展开，我们要寻找下降矢量，也就是解一个线性的最小二乘问题：

对△x求导并令其等于零，得到方程组：

这是一个线性方程组，只有一个变量△x，我们称这个线性方程组为增量方程，将左边的系数定义为H，右边定义为g，上面的式子变为：

也就是说，高斯牛顿法是使用JTJ作为海森矩阵的近似，从而忽略了计算H的过程。求解增量方程是整个优化问题的核心所在，如果可以解出这个方程，那么高斯牛顿法就可以转换为：

从步骤里也可以看出，增量方程的求解是最关键的一步。个人的理解就是找方向的过程中，用一阶导精度不够，用二阶导计算海森矩阵太麻烦，于是高斯牛顿法用一个量，近似海森矩阵，从而在展开到二阶导的情况下计算量又不那么大，从而一定程度上也保证了精度。
对于这个方法，原则上要求近似的矩阵H是可逆的，但实际中H一般是半正定的，也就是说增量会不稳定，导致算法不收敛。严重的情况甚至可能导致目标函数变得更大。

列文伯格——马夸尔特方法则使用了另一种思路。在高斯牛顿法中，我们使用近似二阶泰勒展开，只能在近似点附近才能有很好的效果，所以我们可以考虑加一个信赖区域，不让它太大而使得近似不准确。这种方法也被称为信赖区域法，区域内近似有效，出了这个区域，近似就无效了。
对于这个区域，我们可以使用下面的式子去衡量泰勒近似的效果：

ρ的分子是实际函数下降的值，分母是近似模型下降的值。如果ρ接近于1，则近似是好的。如果ρ太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果ρ比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
于是框架可以优化为下面的流程：

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。

标签：似然,函数,求解,非线性,近似,优化,二乘,式子
来源： https://blog.csdn.net/weixin_43849505/article/details/121730482