SLAM14讲学习笔记(三)非线性优化基础

76
0
2020年11月11日 09时12分

这部分的内容,第一次看觉得很难看懂,不知所云;最近第二次看,可以看明白了。本着“先赶理论,后赶代码,借着代码复习C++”的原则,现在先不去学习代码的内容。

 

这一章的内容,应该先从宏观上看。第一次看的时候,就是陷入了细节,忘记了每时每刻主要是在做什么,因此看完之后不能融会贯通。说明学习的时候,把握整体的脉络比追究细节更重要(毕竟不是考试,平时不用闭卷)。后者可以临时抱佛脚,现用现查,而前者不行。我在这篇文章的最后,放上了链接,是这章内容的具体用途。如果前面的内容已经熟知了,但仍然不太理解这个非线性优化,希望打开我最后的总结链接看看,融会贯通。


主要有两个方程:

状态方程:

 

1

 

其中,w,v都是噪声。非线性优化的主要目的就是在有噪声的情况下,找到它的本质规律。

 

2

 

假设两个噪声满足上述的形式。

P(x|z)∝P(z|x)P(x)

即后验正比与似然乘以先验。当然先验P(x)也有可能是完全不知道的。

 

我的理解是,想知道在已知观测数据z(比如在图像中的位置)的情况下,估计系统的状态(比如它的位姿),这就是后验概率,想根据z直接求位姿是比较困难的,但我们可以用似然的思路来解决,用一个非常不严谨但是很形象的说法,就是比如不停的试各种状态,看看是在什么样的状态下,最有可能产生现在观测到的数据。找到这个状态了,那就是我要求的位姿了。为了找到一个x和y(位姿和地标),使得利用它们产生的观测数据z2最接近目前直接观测到的z,那这样的x和y不就是我们需要的吗?

 

3

 

如上,实际的观测数据的似然概率,满足这样一个形式,

 

所以要最大化这个似然概率!(这块和之后的卡尔曼滤波器内容很完美的结合了起来,我在里面有对这部分内容展开更详细的叙述:SLAM14讲学习笔记(六)后端(最难一章:卡尔曼滤波器推导、理解以及扩展),如果没有学到卡尔曼滤波器,就先不用看了)

 

显然优化的思路应该是最小化什么东西,所以采取用负对数的形式,最小化它的负对数,其实就是最大化它的似然概率。

对于矩阵的正态分布,长这个样子:

 

4

 

求负对数,是下面这个样子:

 

5

 

右边第一项,和x无关,人也不可能影响它,就是一个客观的自然规律,因此跳过,求第二项的最小值,也就算出最大的后验概率了。

我们把slam的两个方程套进去,先套观测方程:

 

x就是观测值了,也就是zkj;μ作为一个均值,应该是没有误差的,因此就是h(y,x),不带最后那个噪声v。∑作为方差,也就是噪声的方差Q,因此其实就是要求下面这个x:

 

6

 

如果把误差用e表示,那么一个观测方程,一个状态方程结合起来的优化函数就可以写成这样:

 

7

 

这个R可不是位姿啊,事实上是状态方程里的那个噪声所服从高斯分布的方差。高博书里用了这个很误导人的符号,这点应该改下。

顺便说一下,如果要融合IMU,也是这种形式,再加一项误差。这个公式很让人害怕,但是这么拆开一看,其实也就是那回事。

接下来的目标,就是怎么求这个优化函数的最小值。

 


接下来的这块就是这章最难的地方了,第一次看的时候,不知所云,这次看我觉得有所理解了,所以现在从宏观的地方总结一下:

非线性最小二乘这块,就是为了算极值,目的是优化上面的那个联合优化函数,算它的最小值,这样就能求出我想要的位姿了。

两种思路:第一种,固定搜索方向,沿着该方向寻找步长,也就是line search方式。第二种方式是固定搜索区域,再从区域里找最优点,也就是Trust Region方式。


Line search的思路是这样的:

 

8

 

Line search主要有两种解法:梯度下降法和高斯牛顿法。

梯度下降法:

又可以分为两种,一阶梯度法和二阶梯度法。这两种都是把函数在x附近泰勒展开:

 

9

 

H是二阶导数,海塞矩阵。J是雅克比矩阵,表示fx关于x的导数。这种方式的优点是比较直观,解线性方程组就行了。

如果是一阶梯度法,那就不要第二项,让它最小就完事了。这又叫最速下降法,缺陷是太贪心了,容易走锯齿路线。

如果是二阶梯度法,那么第二项还是要的,求右侧关于△x的导数,并令其为0。这种方法又叫牛顿法,缺陷是它得算一个H矩阵,不好算。

两者结果的区别,都是-JT,后者是H△x=-JT,前者直接△x就是-JT(T是转置符号)

这里用的2范数,理论上是可以换成别的范数的。

总的来说这两种方式不太实用,下面介绍比较实用的方式:

高斯牛顿法:

这的思想是,把fx一阶展开(不是fx的平方了)

 

11

 

这个也是要求极值,这里的f都是之前上个部分求出的联合优化函数。求极值的目的是让它最小。

因此构建线性最小二乘问题:

 

12

 

把上面的这个给展开,写成自身转置和自身的乘积,然后把转置加进括号里,再交叉相乘去掉所有的括号,变成多项式的形式。

下一步求△x的导数,令其为0。

 

最后求得的结果是JT *J *△x=-JT*fx  (这里的fx是误差),近似写成H△x=g。

说白了就是用J和自身转置的乘积来代替牛顿法里的H矩阵的计算过程。这是它的优点。

 

这种方法的限制在于:JT和J构成的近似H矩阵应该是可逆的,正定的(特征值为正,各阶顺序主子式为正)。但实际算出来的是半正定的。如果出现它是奇异矩阵或者是病态矩阵,那算法就不会收敛。

 

对于这个的改进,有的方法是在找到△x以后,再找一个α,让||f(x+α△x)||2最小,不是简单的让α等于1。


Trust Region的思路是这样的:

列文伯格-马尔夸特方法:

 

13

 

这里的ρ如下定义:

 

14

 

分子表明实际函数下降的值,分母表明用雅克比矩阵的近似估计值。可以根据ρ来调整近似范围,太小说明分母较大,说明估计的太大,应该缩小范围;太大说明分母较小,说明的估计的小,可以放大近似范围。

上面算法里的阈值3/4和1/4,还有半径的放缩系数2和0.5都是经验值。

 

列文伯格-马尔夸特方法又可以叫做阻尼牛顿法。对于上面的D,我认为这个是控制区域的形状。如果区域是球,那就取单位阵E,它也可以把球弄成一个椭球。

注意上面步骤里面,是有个st不等式约束的,可以用拉格朗日乘子把它转化成一个无约束优化问题。(对于这点暂时我不是很明白原理,为了不偏离主题,我目前当它这样能解决问题就完事了)

 

15

 

算出来以后,是这样:

 

16

 

λ较小,H占主要地位,接近上面的高斯牛顿法。

λ较大,更接近一阶梯度下降法。DT*D可以简化为单位矩阵E。

列文伯格-马尔夸特方法优点就是可以避免非奇异与病态问题,缺点是收敛速度会很慢。

 


最后总结一下这一章的思路:想通过最大似然估计来求位姿,那最大似然就等同于最小化负对数;把观测方程和状态方程代入进去,这个负对数函数就是要优化的函数,要最小的函数,就和神经网络里的损失函数似的。要最小化它,就是求它的极小点,求的方式就是line search或者trust region方式,line search有梯度下降法(包括一阶的最速下降法和二阶的牛顿法,前者直观但是太贪心,后者直观但是得算H矩阵)和高斯牛顿法(用J和自身转置的乘积来代替H矩阵)。trust region主要是列温伯格马尔夸特方法,避免非奇异性问题,但是收敛较慢。


今天再补充一下,这章的内容由于没有套用后面的知识,所以有些抽象,不能理解意义。看完后面的,这里很多东西也就能明白了。在非线性优化的过程中,目标函数作为想要优化的结果,这个函数的自变量x,其实就是位姿Rt或者位置p

 

因此,上面式子中的x,就可以当做位姿,我们可以这么理解:在这个位姿下,对它进行△x的修正,使得目标函数也就是误差最小了,然后把位姿更新成x+△x。正常来说。y=f(x),我们想知道y的极小值,求的应该是直接去求关于x的导数,在导数为0处是f(x)极小值,目标函数也就是误差值最小,对吧?,那这求△x是干什么呢?

 

这恰恰就是这章独特、精髓之处。实际的数据是有误差的,是想要根据很多有误差和噪声的数据,最后求得一个最合适的位姿,让平均的误差最小。用f(x)直接对x求导,是非常不方便的!因此采用f(x+△x)的形式,不断沿着梯度下降的方向走,迭代更新x,从而计算使得目标函数最小的x值。

 

这章的缺陷在于,读者不知道这是在干什么,实际上这是后面大部分内容的基础,建议配合光度误差和重投影误差的内容去阅读:SLAM14讲学习笔记(五)视觉里程计(光流法和直接法)和对于雅克比矩阵的理解 在这个的最后,我总结了怎么通过误差函数算雅克比矩阵,这样算出雅克比矩阵才能套在上面的非线性优化的地方求出△x,进一步实现x+△x的更新。看完这个,才能真正明白这节的非线性优化内容是干什么的,否则肯定是看了就忘,不理解含义。另外,在后端,这块也会被反复提及和使用,我认为这是实现SLAM理论的精髓部分,后端内容在这里,有点基础的可以进去一看:SLAM14讲学习笔记(七)后端(BA与图优化,Pose Graph优化的理论与公式详解、因子图优化)

其余内容,详见我的SLAM专栏学习笔记,欢迎留言交流。

发表评论

后才能评论