视觉slam十四讲ch6 非线性优化

想做梦的泽熙

已于 2024-02-07 23:34:36 修改

阅读量1.3k

点赞数 19

分类专栏： SLAM 文章标签：人工智能算法

于 2024-02-06 00:23:20 首次发布

本文链接：https://blog.csdn.net/m0_74625021/article/details/136050038

版权

SLAM 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

视觉slam十四讲ch6 非线性变换

一：状态估计问题

1：批量状态估计与最大后验估计

(1)经典SLAM模型的运动方程和观测方程

运动方程: $X_k=f（X_k-1~,U_k）+W_k$
$X$ 表示相机的位姿，每个时刻位姿记为X1，X2……X(k)构成了运动的位姿, $X_k$ 可以由 $T_k$ 变换矩阵 $SE (3)$ 给出
$U_k$ 是传感器的输入
$W_k$ 是运动噪声， $W_k$ ~N(0, $R_k$ )
运动方程解释了从k-1时刻到k时刻，机器人的位姿是如何变化的。SLAM的关键问题就是解决估计X的问题
观测方程: $Z_k=h（Y_j，X_k）+V_k，_j$

$Y_j$ 是对应的路标

$Z_k,_j$ 是对应图像的像素位置

$V_k,_j$ 是观测噪声， $V_k$ ~N(0, $Q_k,_j$ )

(2)视觉SLAM的观测方程（由针孔模型给定）：

$s*Z_k,_j=K(R_k,*Y_j+t_k)$

其中：

K为相机内参
s为像素点距离，也是( $R_k*t_k$ )的第三个分量

若使用变换矩阵 $T_k$ 描述位姿，则 $Y_j$ 必须用齐次坐标来描述，计算完成后要转换为非齐次坐标（见第五讲）

（3）视觉SLAM的问题

通过带噪声的数据 $Z$ 和 $U$ 推测位姿 $X$ 和地图 $Y$ (以及他们的概率分布（即状态估计问题）

解决这种状态估计问题方法：

滤波器（增量/渐进）法：持有一个当前时刻的估计状态，然后用新的数据来更新它
批量法：把数据“攒”起来一并处理（例：把0-k时刻的所有输入和观测数据放在一起，在这样的输入和观测下，如何估计整个0-k时刻的轨迹与地图）------SfM(Structure from Motion) 具有不实时性，不符合SLAM的应用场景
滑动窗口估计法:固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化

（4）批量方法

考虑从1到N的所有时刻，并假设有M个路标点，定义所有时刻的机器人位姿及路标点坐标

$X=({X_1，X_2,......,X_N}), Y=({Y_1,Y_2,......,Y_M})$

不带下标的U代表所有时刻的输入
Z代表所有时刻的观测数据

从概率学的角度，视觉SLAM的问题就是：已知输入数据U和观测数据Z的条件下，求状态X,Y的概率分布

当我们不知道控制输入U时候，只有一张张图片时候，即只考虑观测方程带来的数据时，相当于估计 $P (X, Y ∣ Z)$ 的概率分布，此问题也称SfM,即：如何从一张张图像中重构三维空间结构

由贝叶斯法则：P(A|B)= $\frac{P(B|A)*P(A)}{P(B)}$

$P(X,Y|Z)=\frac{P(Z|X,Y)*P(X,Y)}{P(Z)}\propto P(Z|X,Y)*P(X,Y)$

其中：P(X,Y|Z)称为后验概率，P(Z|X,Y)称为似然，P(X|Y)称为先验
直接求解后验估计是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大是可行的

$X,Y)^*MAP=arg max P(X,Y|Z)=argmaxP(Z|X,Y)*P(X,Y)$

由于P(X,Y|Z)的分母部分与待估计的状态X,Y无关，故可以忽略
贝叶斯法则可以得知求解最大后验概率等价于最大似然和先验的乘积

在实际现实中，我们也无法大概知道机器人的位姿或路标，此时就不知道了先验

那么就可以求解最大似然估计（Maximize Likelihood Estimation,MLE）

$X,Y）^*MLE=arg max P(Z|X,Y)$

似然：在当前的位姿和路标下，可能产生什么样的观测数据

由于观测到的数据是已知的，最大似然估计就可以理解为：“在什么样的·状态下，最可能产生现在观测到的数据”

2：最小二乘的引出

怎么求最大似然估计？

在高斯分布的前提假设下，最大似然能够有较为简单的形式：

对于某一次观测：

$Z_k,_j=h(Y_j,X_k)+V_k,_j$

由于噪声 $V_k$ ~N(0, $Q_k,_j$ ),故观测数据的条件概率为

$P(Z_j,_k|X_k,Y_j)=N((h(Y_j,X_k),Q_k,_j)$

它仍然是一个高斯分布，高斯分布的概率密度函数:

$P(X)=\frac{1}{\sqrt{(2\pi)^Ndet(cov)}}*e^{-\frac{1}{2}(X-u)^Tcov^{-1}(X-u)}$

其中：

N为分布的维度
cov为对应的协方差矩阵，det为对应的行列式
u是均值

考虑单次观测的最大似然估计，可以用最小负对数将高斯分布概率密度函数化简，通过求解负对数的最小值得到原函数的最大值

取负对数得：

$-ln(X)=\frac{1}{2}ln(2\pi)^Ndet(cov)+\frac{1}{2}(X-u)^Tcov^{-1}(X-u)$

负对数的右侧第一项与X无关，可以略去
最小化右侧的二次项，即可得到对状态的最大似然估计

$X_k,Y_k)^*=arg max N(h(Y_j,X_k),Q_k,_j)=arg min((Z_k,_j-h(X_k,Y_j)^TQ_{k,j}^{-1}(Z_k,_j-h(X_k,Y_j))$

其中：

$Z_k,_j$ 对应上述的自变量X
$h(X_k.Y_j)$ 对应均值u
$Q_{k,j}$ 对应协方差cov

该式等价于：最小化误差项（即误差）的一个二次型，这个二次型称为“马哈拉诺比斯距离(Mahalanobis distance)”,又称“马氏距离”，可以看成由 $Q_{k,j}^{-1}$ 加权后的欧式距离， $Q_{k,j}^{-1}$ 也叫做信息矩阵（高斯分布协方差矩阵之逆）

现在我们考虑批量时刻的数据，通常假设：各个时刻的输入和观测是相互独立的，这意味着：各个输入之间是相互独立的，各个观测之间是也是相互独立的

依据以下结论：

在概率论中：若事件A和事件B是相互独立的，则：P(AB)=P(A)P(B)
经典SLAM运动模型：

$X_k=f（X_k-1~,U_k）+W_k$

$Z_{k,j}=h(Y_j,X_k)+V_{k,j}$

推出：

$P(Z,U|X,Y)=\prod_{k}{P(U_k|X_{k-1,k})\prod_{k,j}P(Z_{k,j}|X_k,Y_j)}$

定义：各次输入和观测数据与模型之间的误差

$e_{u,k}=X_k-f(X_{k-1},U_k)$

$e_{Z,j,k}=Z_{j,k}-h(X_k,Y_j)$

最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计

利用负对数，把乘积变成求和：

$J(X,Y)=\sum{e_{u,k}^{T}R_k^{-1}e_{u,k}}+\sum_k{\sum_j{e_{Z,k,j}^{T}Q_{k,j}^{-1}e_{Z,k,j}}}$

k是对应时刻
j是对应某个路标Y

这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。

直观上看，由于噪声的存在，当我们将估计的轨迹与地图代入SLAM的运动，观测方程中时，它们并不会完美地成立。

这时候我们需要把对状态的估计值进行微调，使得整体的误差下降一些，使其达到一个极小值，这个就是典型的非线性优化的过程

仔细观察上式，可以发现SLAM中的最小二乘问题具有一些特定结构：

整个问题的目标函数由许多个误差的（加权的）二次型组成。虽然总体变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。

例如：运动误差只与 $X_{k-1},X_k$ 有关，观测误差只与 $X_k,Y_K$ 有关。这种关系会让整个问题具有一种稀疏的形式
若用李代数表示增量，则该问题是无约束的最小二乘问题。若用旋转/变换矩阵描述位姿，则会引入旋转矩阵自身的约束( $RR^T=I$ ,det®=1),额外的约束会使优化变得困难，这体现了李代数的优势
使用二次型度量误差。误差的分布将会影响此项在整个问题中的权重

例如：若某次的观测非常准确，那么协方差矩阵将会“小”，而信息矩阵将会“大”

3：例子：批量状态估计

考虑一个非常简单的离散时间系统：

$X_k=X_{k-1}+U_k+W_k$ $W_k$ ~N(0, $Q_k$ )

$Z_k=X_k+n_k$ $n_k$ ~N(0, $R_k$ )

这个可以表达一辆沿X轴前进或后退的汽车

第一个方程为运动方程： $U_k$ 为输入， $W_k$ 为噪声
第二个方程为观测方程： $Z_k$ 是对汽车位置的测量，取时间k=1,2,…3

现在希望根据已有的 $V, Y$ 进行状态估计。设初始状态 $X_0$ 已知，下面推导批量状态估计的最大似然估计

首先令批量状态变量为 $X=[x_0,x_1,x_2,x_3]^T$ ,令批量观测为 $Z=[z_1,z_2,z_3]^T$ , $U=[u_1,u_2,u_3]^T$

依照之前的推导，最大似然估计：

$X^*_{map}=arg max P(X|U,Z)=argmax P(U,Z|X)=\prod^{3}_{k=1}P(U_k|X_{k-1},X_{k})\prod^{3}_{k=1}P(Z_k|X_k)$

对于每一项：

运动方程： $P(U_k|X_{k-1},X_k)=N(X_k-X_{k-1},Q_k)$
观测方程： $P(Z_k|X_k)=N(X_k,R_k)$

构造误差变量：
$e_{u,k}=X_k-X_{k-1}-U_k$

$e_{z,k}=Z_k-X_k$

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

$min\sum^3_{k=1}e_{u,k}^TQ_k^{-1}e_{u,k}+\sum_{k=1}^3e_{z,k}^TR_{k}^{-1}e_{z,k}$

此外这个系统是线性系统，我们可以很容易地将它写成向量形式。定义向量 $y=[u,z]^T$ ,可以写出矩阵 $H$ ,使得 $y - H x = e$ ~N(0, $\sum$ )

$H=\begin{bmatrix} 1&-1&0&0 \\ 0&1&-1&0\\ 0&0&1&-1 \\ 0&1&0&0 \\ 0&0&1&0\\0&0&0&1 \end{bmatrix}$

且： $\sum=diag(Q_1,Q_2,Q_3,R_1,R_2,R_3)$

整个问题可以写成：

$x^*_{map}=arg min(e^T{\sum}^{-1}e)$

这个问题的唯一解：

$x^*_{map}=(H^T{\sum}^{-1}H)^{-1}H^T{\sum}^{-1}y)$

二：非线性最小二乘

先考虑一个简单的最小二乘问题：
$min_{x}F(x)=\frac{1}{2}||f(x)||^2_2$

其中：

自变量 $x{\in}R^n$
f是任意标量非线性函数 $f(x):R^n{\to}R$
系数 $\frac{1}{2}$ 是无关紧要的

如何求解这样一个最小值问题？

若f是个数学形式上很简单的函数：可以直接利用求导得到能够取到极小值的点

$\frac{dF}{dx}=0$

这个方程是否容易求解也是个问题。

若f的导函数是一个简单的线性函数，那么这个问题就是一个简单的线性最小二乘问题
若f的导函数比较复杂，我们可以采用迭代的方式==，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降==

具体步骤如下：

1:给定某个初始值X_0
2:对于第k次迭代，寻找一个增量delta(X_k),使得||f(X_k+delta(X_k))||达到最小值
3:若delat(X_k)足够小，则停止
4:否则，令X_{k+1}=X_k+delta(X_k)，返回第二步

这让求解导函数为零的问题变成了一个不断寻找下降增量 $\delta{X_k}$ 的问题

当函数下降到增量非常小的时候，就认为算法收敛，目标函数达到了一个极小值（这个是一个局部的问题，我们只关心f在迭代处的局部性质而非全局性质）

接下来我们将考察如何寻找这个增量 $\delta{X_k}$

1：一阶和二阶梯度法

现在考虑第k次迭代，假设在 $X_k$ 处，想要找到增量 ${\triangle}X_k$ ,最直观的方式是将目标函数在 $X_k$ 附近进行泰勒展开：

$F(X_k+{\triangle}X_k){\approx}F(X_k)+J(X_k)^T{\triangle}X_k+{\frac{1}{2}}{\triangle}X_k^TH(X_k){\triangle}X_k$

其中：

$J(X_k)$ 是 $F (X)$ 关于 $X$ 的一阶导数（也叫梯度，雅可比矩阵）
H则是二阶导数（海塞矩阵）

（1）一阶梯度法

若保留一阶梯度，那么取增量为反向的梯度，即可保证函数下降

${\triangle}X^*=-J(X_k)$

这个只是一个方向，我们还要指定一个步长 $\lambda$ 。这个步长可以根据一定的条件来计算，这种方法称为“最速下降法”–只要我们沿着反向梯度的方向前进，在一阶线性近似下，目标函数必然会下降。

（2）二阶梯度法

注意以上讨论都是在第k次迭代时进行的，并不涉及其他的迭代信息，所以为了简化符号，省略下标k，并认为这些讨论对任意一次迭代都成立

保留二阶梯度信息，此时增量方程为：

${\triangle}X^*=arg min(F(X)+J(X)^T{\triangle}X+\frac{1}{2}{\triangle}X^TH{\triangle}X)$

右侧只含 ${\triangle}X$ 的零次，一次，二次项。求右侧等式关于 ${\triangle}X$ 的导数并令它等于零，得到：

$J+H{\triangle}X=0 {\to} H{\triangle}X=-J$

求解这个方程就得到了增量，该方法又称为==”牛顿法“ ==

(3)最速下降法和牛顿法的缺点

最速下降法：

过于贪心，容易走出锯齿路线，反而增加了迭代次数
牛顿法

需要计算目标函数的H矩阵，在问题规模比较大时，非常困难，尽量避免H的计算。

对于一般的问题，一些拟牛顿法可以得到较好的结果

2：高斯牛顿法

高斯牛顿法是最优化算法中最简单的方法之一。

它的思想：将 $f (x)$ 进行一阶项的泰勒展开 (注意这里不是目标函数 $F (x)$ ,而是 $f (x)$ ,否则就是牛顿法了)

$f(x+{\triangle}x){\approx}f(x)+J(x)^T{\triangle}x \tag{1}$

$J(x)^T$ 为 $f (x)$ 关于 $x$ 的导数，为nx1的向量

根据前面的框架，当前的目标是寻找 ${\triangle}x$ ,使得 $||f(x+{\triangle}x)||^2$ 达到最小

为了求 ${\triangle}x$ ,需要一个线性最小二乘问题：
${\triangle}x^*=argmin\frac{1}{2}||f(x)+J(x)^T{\triangle}x||$

这个方程与之前的有什么不同吗？先展开目标函数的平方项：
$\frac{1}{2}||f(x)+J(x)^T{\triangle}x||^2=\frac{1}{2}(f(x)+J(x)^T{\triangle}x)^T(f(x)+J(x)^T{\triangle}x)=\frac{1}{2}(||f(x)||^2_2+2f(x)J(x)^T{\triangle}x+{\triangle}x^TJ(x)J(x)^T{\triangle}x)$

求上式关于 ${\triangle}x$ 的导数，并令其等于零：

$J(x)f(x)+J(x)J(x)^T{\triangle}x=0$

令 $J(x)J(x)^T=H(x)$ , $- J (x) f (x) = g (x ）$ ,则上式可以改写为：

$H{\triangle}x=g$

这个方程式关于变量 ${\triangle}x$ 的线性方程，我们称之为增量方程，也可称为高斯牛顿方程或正规方程

高斯牛顿法将 $JJ^T$ 作为牛顿法中二阶矩阵H的近似，从而省略了计算H的过程

高斯牛顿的算法步骤：

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

从算法步骤中可以看出，增量方程的求解占据着主要地位。只要我们能够正确解出增量，就能保证目标函数能够正确的下降

为了求解增量方程，我们需要求解 $H^{-1}$ ，这需要 $H$ 矩阵可逆，但实际的矩阵 $JJ^T$ 只有半正定性。

即使用高斯牛顿法时候，可能 $JJ^T$ 为奇异矩阵或者病态矩阵的情况，此时增量的稳定性较差，导致算法不收敛。直观来说，原函数在这个点的1局部近似不像一个二次函数。
更严重的是，假设 $H$ 非奇异也非病态，如果我们求解出来的步长 ${\triangle}x$ 太大，也会导致我们采用的局部近似式($f(x+{\triangle}x){\approx}f(x)+J(x)^T{\triangle}x $)不够准确，这样一来我们甚至无法保证它的迭代收敛，甚至有可能让目标函数更大都是有可能的

列文伯格–马夸尔特方法在一定程度上修正了这些问题。一般认为它比高斯牛顿法更健壮，但它的收敛速度可能比高斯牛顿法更慢，被称为“阻尼牛顿法”

3：列文伯格–马夸尔特方法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自信想到应该给 ${\triangle}x$ 添加一个范围，称为信赖区域。在这个范围内，二阶近似是有效的，离开这个区域，近似可能会出问题，这类方法也称为“信赖区域方法”

如何确认这个信赖区域的范围呢？

如果近似模型与实际函数之间的差异越小，近似效果越好，应该扩大近似的范围；反之差异越大，近似效果越差，应该缩小近似的范围

定义一个指标来刻画近似的好坏程度：

$p=\frac{f(x+{\triangle}x)-f(x)}{J(x)^T{\triangle}x} \tag{6.34}$

分母 $J(x)^T{\triangle}x$ 是实际下降的值
分子 $f(x+{\triangle}x)-f(x)$ 是近似模型下降的值

根据 $p$ 的值

$p$ 接近1,则近似是好的
$p$ 太小，说明实际减小的值远小于近似减小的值，则认为近似比较差，需要缩小近似范围
$p$ 太大，说明实际下降的值远大于近似减小的值，则需要扩大近似范围

构建一个改良版高斯牛顿法框架：

1:给定初始值x_0,以及初始优化半径u
2:对于第k次迭代，在高斯牛顿法的基础上加上信赖区域，求解min(1/2)*||f(x_k)+J(x_k)^T*delta(x)||^2,限制条件为：||D*delta(x)||^2<=u
  其中u是信赖区域的半径，D是系数矩阵
3:根据式6.34计算p
4:若p>3/4,则设置u=2u
  若p<1/4,则设置u=0.5u
5:如果p大于某阈值，则认为近似可行。令x_k+1=x_k+delta(x_k)
6:判断算法是否收敛。如果不收敛则返回第二步，否则结束