视觉SLAM（三）：非线性优化

1. SLAM基本问题

1.1. 问题阐述

根据前文视觉SLAM（二）：相机与图像描述可知，不考虑畸变情形下空间中一个点到像素位置的映射为

$$P^{\prime \prime }=\left[\begin{array}{lcr}1& 0& 0\\ 0& 1& 0\end{array}\right]\cdot K\cdot \frac{R\cdot (P_w-O_{c_w})}{(001)\cdot R\cdot (P_w-O_{c_w})}\text{\hspace{1em}(重要)}$$

注意到该公式中的参数其实是相机位姿（偏航角、俯仰角、滚转角和位置）和路标点$P$的位置 的函数，而且还是精确表达式，实际情况中会存在噪声干扰。下面将该公式抽象化

● 观测方程
$x_k$：$k$时刻的相机位姿

$y_j$：编号为$j$的路标点世界坐标

$z_{k,j}$：路标点$j$ 在$k$时刻 相机的像素坐标

$v_{k,j}\sim N(0,Q_{k,j})$：观测噪声

综上可以得到观测方程

$$z_{k,j}=h(y_j,x_k)+v_{k,j}\text{\hspace{1em}(重要)}$$

● 运动方程
这里面相机位姿是随着时间变化的，因此得到运动方程如下

$u_k$：$k$时刻对机器人的控制量

$w_k\sim N(0,R_k)$：运动噪声

$$x_k=f(x_{k-1},u_k)+w_k\text{\hspace{1em}(重要)}$$

下面介绍SLAM基本问题，SLAM（Simultaneous Localization And Mapping, SLAM）同时定位与地图构建，即机器人在完全陌生的环境下对环境进行建图，同时估计自身的状态。
因此SLAM基本问题可以描述为：对于运动方程 和 观测方程，已知观测数据 $z$ 和控制输入$u$，估计位姿状态$x$和路标$y$的过程。

1.2. 基于最大似然估计的批量估计法

那该如何对$x$和$y$进行估计呢？此处将SLAM的估计问题具体化

1.2.1. 批量估计

批量估计指通过以往$1,...,N$时刻看到$M$个路标点的批量数据$u=[u_1,...u_N{]}^T$和$z=\left[\begin{array}{lcr}{z}_{1,1}& ...& z_{1,M}\\ ...& ...& ...\\ z_{N,1}& ...& z_{N,M}\end{array}\right]$来估计以往的批量状态$x=[x_1,...x_N{]}^T$和地图$y=[y_1,...y_M{]}^T$

1.2.2. 最大似然估计（Maximize Likelihood Estimation, MLE）

通过样本对系统参数估计的一种方法，思路是 参数估计值=使样本值出现的概率达到最大的系统参数。

若概率为$P(x|\theta )$其中$\theta$为待估计参数，已知一堆独立重复试验得到的样本$x_1,...x_n$，则这些样本出现概率的表达式为$P(x_1|\theta ),...,P(x_n|\theta )$，则MLE的求法是

$$\hat{\theta }=\underset{\theta }{\mathrm{argmax}}P(\theta |x_1,...,x_n)=\underset{\theta }{\mathrm{argmax}}\frac{P(x_1,...,x_n|\theta )P(\theta )}{P(x_1,...,x_n)}=\underset{\theta }{\mathrm{argmax}}\prod _{i=1}^{n}P(x_i|\theta )$$

一般使用最小化负对数处理，将简化运算，则有

$$\hat{\theta }=\underset{\theta }{\mathrm{argmin}}\sum _{i=1}^n\ln P(x_i|\theta )$$

1.2.3. SLAM的MLE模型

用MLE解决SLAM估计问题的思路是将$x$和$y$看作待估计参数，将$z$和$u$看作已知样本，即

$$(\hat{x},\hat{y})=\underset{(x,y)}{\mathrm{argmax}}P(z,u|x,y)$$

因此需要先计算$P(z,u|x,y)$的表达式，此处假设 观测量与控制量 相互独立，不同时刻或不同路标点的观测量相互独立，各时刻的控制量相互独立，因此有

$$P(z,u|x,y)=\prod _{k=1}^N\prod _{j=1}^{M}P(z_{k,j}|x_k,y_j)\prod _{k=1}^{N}P(u_k|x_k,x_{k-1})$$

注意到高斯噪声的存在可以推得观测量的分布：

$$P(z_{k,j}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})$$

而控制量写成可反函数的形式，即$u_k=q(x_k,x_{k-1})+w_k$

$$P(u_k|x_k,x_{k-1})=N(q(x_k,x_{k-1}),R_k)$$

之后根据1.2.2.节描述，写出高斯分布并作负对数处理，最终得到一个具有最小二乘形式的问题：

( x ^ , y ^ ) = arg min ⁡ ( x , y ) { ∑ k = 1 N ∑ j = 1 M [ z k , j − h ( y j , x k ) ] T Q k , j − 1 [ z k , j − h ( y j , x k ) ] + ∑ k = 1 N [ x k − f ( x k − 1 , u k ) ] T R k − 1 [ x k − f ( x k − 1 , u k ) ] } (重要) (\hat{x},\hat{y})= \argmin_{(x,y)}\{\sum_{k=1}^{N} \sum_{j=1}^{M} {[z_{k,j}-h(y_j,x_k)]^TQ^{-1}_{k,j}[z_{k,j}-h(y_j,x_k)]}+ \sum_{k=1}^{N}{[x_k-f(x_{k-1},u_k)]^TR^{-1}_{k}[x_k-f(x_{k-1},u_k)]}\} \tag{重要} (x^,y^​)=(x,y)argmin​{k=1∑N​j=1∑M​[zk,j​−h(yj​,xk​)]TQk,j−1​[zk,j​−h(yj​,xk​)]+k=1∑N​[xk​−f(xk−1​,uk​)]TRk−1​[xk​−f(xk−1​,uk​)]}(重要)

但是注意上述运动方程和观测方程可能为非线性量，所以可能需要去解决一个非线性最小二乘问题，下面讲。

1.3. 非线性最小二乘

即求解

$$x^{\ast }=\underset{x}{\mathrm{argmin}}F(x)=\underset{x}{\mathrm{argmin}}\frac{1}{2}||f(x)|{|}_2^2$$

其中$x$为自变量，$f$为任意变量非线性函数。

的确可以直接$\frac{dF(x)}{dx}=0$求极值判断，但是非线性函数可能求导比较复杂，此处讨论比较通用的方法。

1.3.1. 高斯牛顿法（Gauss-Newton method, G-N）

1.3.2. 列文伯格-马夸尔特法（Levenberg-Marquardt method, L-M）