SLAM学习——非线性优化

1.状态估计问题

对于SLAM经典模型，我们知道是由一个运动方程和一个观测方程构成，如下方程：

{xk=f(xk−1,uk)+wkzk,j=f(yj,xk)+vk,j { x k = f ( x k − 1 , u k ) + w k z k , j = f ( y j , x k ) + v k , j

其中 xk x k 为相机的位姿， u u 为输入数据，即为采集到的数据。

假如我们在 xk x k 处观测到路标 yj y j ，对应到图像上的像素位置 zk,j z k , j ,那么我们的观测方程可以表示为：

szk,i=K∗exp(ε∧)∗yi s z k , i = K ∗ e x p ( ε ∧ ) ∗ y i

其中 s s 为像素距离，即摄像机坐标系上的一点到像素坐标系的距离， K K 为内参， exp(ε∧) e x p ( ε ∧ ) 为李代数 se(3) s e ( 3 ) 。接着，在运动方程和观测方程中，我们假设噪声都满足于均值为0的高斯分布：

wk∼N(0,Rk),vk∼N(0,Qk,j) w k ∼ N ( 0 , R k ) , v k ∼ N ( 0 , Q k , j )

在这些噪声的影响下，我们希望通过带噪声的数据 z z 和 u u 来推断位姿 x x 和地图 y y (以及他们的概率分布)，这构成了一个状态问题，其中有优化方法有：1.扩展卡尔曼滤波器（ EKF E K F ）求解，关心的是当前时刻的状态估计，而对直接的状态没有多加考虑。2.非线性优化，使用所有时刻采集到的数据进行状态估计，被认为优于传统滤波器。

从概率学角度来看，把所有需要估计的变量都放到一个“状态变量中”，即 x={x1,⋯,xN,y1,⋯,yM} x = { x 1 , ⋯ , x N , y 1 , ⋯ , y M } ,在已知输入数据 u u 和观测数据 z z 的条件下,求计算 x x 的条件概率分布： P(x|z,u) P ( x | z , u ) 。

当然，如果在运动过程中没有采集数据，且只有一张张图片时，概率分布又可以转换为 P(x|z) P ( x | z ) 。如果忽略图像在时间上的联系，看做一堆彼此没有关系的图片，该问题为SfM,即如何从许多图像中重建三维空间结构。当我们把SLAM看做图像具有时间先后顺序，需要实时求解一个SfM问题。利用贝叶斯法则，有：

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

其中，贝叶斯法则左侧通常成为后验概率，右侧的 P(z|x) P ( z | x ) 成为似然，另一部分P(x)称为先验。直接求后验是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化，则是可行的。即最大后验概率为最大似然与先验的乘积。

如果当不知道机器人在哪里，那我们则没有了先验，可以求解 x 的最大似然估计( MLE M L E ):

x∗MLE=argmaxP(z|x) x M L E ∗ = a r g m a x P ( z | x )

上述公式可表达为：在什么样的状态下，最可能产生现在观测到的数据。

为转换为最小二乘问题，我们可以得到最大似然估计等价于最小化噪声项的平方（在范数意义下），我们定义数据与估计值之间的误差：

{ev,k=xk−f(xk−1,uk)ey,j,k=zk,j−h(xk,yj) { e v , k = x k − f ( x k − 1 , u k ) e y , j , k = z k , j − h ( x k , y j )

并求该误差的平方和：

J(x)=∑keTv,kR−1kev,k+∑k∑jeTy,k,jQ−1k,jey,k,j J ( x ) = ∑ k e v , k T R k − 1 e v , k + ∑ k ∑ j e y , k , j T Q k , j − 1 e y , k , j

以上公式为整体意义上的最小二乘法，它的最优解等价于状态的最大似然估计。当我们把估计的轨迹与地图代入SLAM的运动、观测方程时，并不会很完美。这时候我们对状态进行微调，使得整体的误差下降，这是一个非线性优化问题。

SLAM中的最小二乘问题具有一些特定的结构：
1.整个问题的目标函数由许多个误差的（加权的）平方和组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一俩个状态变量有关，运动误差只与 xk−1,xk x k − 1 , x k 有关，观测误差只与 xk,yj x k , y j 有关。
2.如果使用李代数表示，那么该问题转换成无约束最小二乘问题。
3.使用二范数度量误差，相当于欧式空间中距离的平方。

2.非线性最小二乘

举个最简单的最小二乘问题: xmin12∥f(x)∥22 x m i n 1 2 ‖ f ( x ) ‖ 2 2 （这里应该是二范数），为求其最小值，则需要求其导数，然后得到其求解 x x 的最优解。对于不方便求解的最小二乘问题。

非线性最小二乘：最小二乘法（又称最小平方法）通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。

例如SLAM问题，我们具体步骤有：

1.给定某个初始值。
2.对于第k次迭代，寻找增量 Δxk Δ x k , 使得 ∥f(xk+Δxk)∥22 ‖ f ( x k + Δ x k ) ‖ 2 2 （这里应该是二范数）达到最小。
3.若 Δxk Δ x k 足够小，则停止。
4.否则，令 xk+1=xk+Δxk x k + 1 = x k + Δ x k ,返回第2步。

但是，如何求这个增量 Δxk Δ x k 呢 ????

求解增量最直观的方式是将目标函数在 x 附近进行泰勒展开，得到：

∥f(x+Δx)∥22≈∥f(x)∥22+J(x)Δx+12ΔxTHΔx ‖ f ( x + Δ x ) ‖ 2 2 ≈ ‖ f ( x ) ‖ 2 2 + J ( x ) Δ x + 1 2 Δ x T H Δ x （这里应该是二范数的平方）

我们可以保留泰勒展开一届或者二阶项，对应的求解方法为一阶梯度或二阶梯度法。

如果保留一阶梯度，那么增量的解就为 −JT(x) − J T ( x ) , 则沿着反方向梯度方向前进即可。称为最速下降法，但是由于过于贪心，容易走锯齿路线，增加迭代次数。如果保留二阶梯度，即为牛顿法，那么增量的解为 HΔx=−JT H Δ x = − J T ，但是需要计算 H H 矩阵(海塞矩阵)，尽量避免。

补充一些范数的知识：

1-范数： ||x||1=∑Ni=1|xi| | | x | | 1 = ∑ i = 1 N | x i | ,即向量元素绝对值之和。

2-范数： ||x||2=∑Ni=1x2i−−−−−−−√ | | x | | 2 = ∑ i = 1 N x i 2 ，代表向量元素绝对值的平方和再开方。

p-范数： ||x||p=(∑Ni=1|xi|p)1/p | | x | | p = ( ∑ i = 1 N | x i | p ) 1 / p ,向量元素绝对值的p次方和的p次幂。

2.1高斯牛顿法

高斯牛顿法，它的思想是将f(x)进行泰勒展开（目标函数不是f(x)）：

f(x+Δx)≈f(x)+J(x)Δx f ( x + Δ x ) ≈ f ( x ) + J ( x ) Δ x

其中 J(x) J ( x ) 为 f(x) f ( x ) 关于x的导数，根据前面的框架，需要求得下降矢量 Δx Δ x ，使得 ||f(x+Δx)||2 | | f ( x + Δ x ) | | 2 达到最小。其中，将 Δx Δ x 看做变量，那么我们会有一个线性的最小二乘问题：

在这里，我们将目标函数对 Δx Δ x 进行求导，最后得到线性方程有： J(x)TJ(x)Δx=−J(x)Tf(x) J ( x ) T J ( x ) Δ x = − J ( x ) T f ( x )

这个方程称之为增量方程，也称之为高斯牛顿方程，将左边的系数设为 H H 右边的系数设为 g g ，则有 H H Δx Δ x = g g 。求解增量方程是整个优化问题的核心所在。

高斯牛顿法求解的算法步骤可写成：

1.给定初始值 x0 x 0
2.对于第k次迭代，求出当前的雅克比矩阵 J(xk) J ( x k ) 和误差 f(xk) f ( x k ) 。
3.求解增量方程： HΔxk=g H Δ x k = g
4.若 Δxk Δ x k 足够小，则停止。否则，令 xk+1=xk+Δxk x k + 1 = x k + Δ x k 返回第二步

高斯牛顿法的缺点：

1.要求 H H 是可逆的，而且是正定的，如果出现 H H 矩阵奇异或者病态，此时增量的稳定性较差，导致算法不收敛
2.步长问题，若求出来的 Δxk Δ x k 太长，则可能出现局部近似不够准确，无法保证迭代收敛

矩阵的小知识：

1.行列式=0，为奇异矩阵，反之为非奇异矩阵
2.可逆矩阵为非奇异矩阵（满秩），非奇异矩阵也为可逆矩阵
3. AX=0 A X = 0 ，若A为奇异阵，有无穷解； A A 为非奇异阵， AX=0 A X = 0 有唯一解且 AX=b A X = b 有唯一解
4.M为实对称矩阵,若有 XTMX>0 X T M X > 0 ,矩阵正定，若 XTMX>=0 X T M X >= 0 ，矩阵半正定

有改进的方法：一维搜索方法，增加一个标量 a a ，寻求 Δxk Δ x k ，然后使得 ||f(x+αΔx)||2 | | f ( x + α Δ x ) | | 2 最小。

2.2列文伯格-马夸尔特方法（阻尼牛顿法)

在高斯牛顿法的基础上给 Δx Δ x 添加一个信赖区域，如果在这个信赖区域里面，那么认为近似是有效的,该信赖区域为：

p=f(x+Δx)−f(x)J(x)Δx p = f ( x + Δ x ) − f ( x ) J ( x ) Δ x （分母为方向乘步长）

p p 的分子是实际函数下降的值，分母是近似模型下降的值，若 p=1 p = 1 ，那么最好，若p太小，则实际减少值远少于近似减小的值，缩小近似范围。若 p p 太大，则实际减少值远大于近似减小的值，放大近似范围。

列文伯格-马夸尔特方法的算法步骤可写成：

1.给定初始值 x0 x 0 ，以及优化半径 u u
2.对于第k次迭代，求解：

Δx∗=argminΔx12||f(x)+J(x)Δx||2 Δ x ∗ = a r g m i n Δ x 1 2 | | f ( x ) + J ( x ) Δ x | | 2 ，s.t. ||DΔxk||2≤u | | D Δ x k | | 2 ≤ u

这里u是信赖区域的半径，D在后面讲
3.计算 p p
4.若 p>3/4 p > 3 / 4 ,则 u=2u u = 2 u
5.若 p<1/4 p < 1 / 4 ,则 u=0.5u u = 0.5 u
6.若 p p 大于某阈值，则认为可行。令 xk+1=xk+Δxk x k + 1 = x k + Δ x k 。
7.判断是否收敛，如不收敛返回第2步，否则结束。

这里近似范围扩大的倍数和阈值都是经验值，可以替换成其他数值。上述约束中相当于把增量限定在半径为u的球里面，认为在球内的才有效。带上 D D 后成为椭圆，至于 D D 的形式，可为单位阵也可为对角非负数对角阵。

在上述求解中，由于是有约束优化，可以利用拉格朗日乘子将其转化为一个无约束优化问题：

minΔxk12||f(xk)+J(xkΔxk)||2+λ2||DΔx||2 m i n Δ x k 1 2 | | f ( x k ) + J ( x k Δ x k ) | | 2 + λ 2 | | D Δ x | | 2 (其中 λ λ 为拉格朗日乘子)

对无约束问题展开之后，得到：

(H+λDTD)Δx=g ( H + λ D T D ) Δ x = g

当参数 λ λ 较小时，H占主导地位，说明二次近似模型在该范围内是比较好的，该方法接近于高斯牛顿法。当参数 λ λ 较大时， λ λ 所在项接近于一阶梯度下降法。该方法可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题。

最后，对于常见的非线性优化，可以通过 ceres 和 g2o 库进行求解。