视觉SLAM⑥---非线性优化

原创

已于 2022-05-15 21:55:01 修改 · 4.1k 阅读

31 ·

CC 4.0 BY-SA版权

文章标签：

#机器学习 #计算机视觉 #算法

于 2022-05-11 11:51:33 首次发布

本文讲解了SLAM中的状态估计问题，重点介绍了最小二乘法、高斯牛顿法、列文伯格—马夸尔特方法，以及如何通过Ceres和g2o库进行曲线拟合的实际应用。通过实例展示了批量状态估计和优化技术在视觉SLAM中的作用。

6.0 主要目标

主要目标：
1.理解最小二乘法的含义和处理方式。
2.理解高斯牛顿法(Gauss-Newton's method)、列文伯格—马夸尔特方法(Levenburg-
-Marquadt's method）等下降策略。
3.学习Ceres库和g2o库的基本使用方法。

        在前面几讲，我们介绍了经典SLAM模型的运动方程和观测方程。现在我们已经知道，方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。于是，我们已经弄清了经典SLAM模型在视觉情况下的具体表达。
        然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。尽管相机可以非常好地符合针孔模型，但遗憾的是，我们得到的数据通常是受各种未知噪声影响的
即使我们有高精度的相机，运动方程和观测方程也只能近似成立。所以，与其假设数据必须符合方程，不如讨论如何在有噪声的数据中进行准确的状态估计。
        解决状态估计问题需要一定程度的最优化背景知识。本节将介绍基本的无约束非线性优化方法，同时介绍优化库g2o和Ceres的使用方式。

6.1 SLAM问题的数学表达

        假设小萝卜正携带着某种传感器在未知环境里运动，怎么用数学语言描述这件事呢？

        首先，由于相机通常是在某些时刻采集数据的，所以我们也只关心这些时刻的位置和地图。这就把一串连续时间的运动变成了离散时刻 $t=1,2,3...K$ 当中发生的事情。在这些时刻，用 $x$ 表示小萝卜自身的位置。于是各时刻的位置就记为 $x_{1},x_{2}...x_{K }$ ，它们构成了小萝卜的轨迹。

        地图方面，我们假设地图是由许多个路标组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有 $N$ 个，用 $y_{1},y_{2},y_{3}....y_{N}$ 表示它们。
        在这样的设定中，“小萝卜携带着传感器在环境中运动”，由如下两件事情描述：
①什么是运动？        我们要考察从 $k- 1$ 时刻到 $k$ 时刻，小萝卜的位置 $x$ 是如何变化的。
②什么是观测？        假设小萝卜在 $k$ 时刻于 $x_{k}$ 处探测到了某一个路标 $y_{j}$ ，我们要考察如何用
数学语言来描述这件事情。
        先来看运动。通常，机器人会携带一个测量自身运动的传感器。这个传感器可以测量有关运动的读数，但不一定直接就是位置之差，还可能是加速度角速度等信息。有时我们也给小萝卜发送指令，例如“前进1米”“左转90°”，或者“油门踩到底”“刹车”等。无论是何种情况，我们都能使用一个通用的、抽象的数学模型来说明此事：

                                                         $x_{k} = f(x_{k-1},u_{k},w_{k})$

        这里， $u_{k}$ 是运动传感器的读数或者输入， $w_{k}$ 为该过程中加入的噪声。注意，我们用一个一般函数于来描述这个过程，而不指明 $f$ 具体的作用方式。这使得整个函数可以指代任意的运动传感器/输入，成为一个通用的方程，而不必限定于某个特殊的传感器上。我们把它称为运动方程。
        噪声的存在使得这个模型变成了随机模型。换句话说，即使我们下达“前进1米”的命令,也不代表小萝卜真的前进了1米。如果所有指令都是准确的，也就没必要估计了。事实上，小萝卜可能某次只前进了0.9米，另一次前进了1.1米，再一次可能由于轮胎打滑，干脆没有前进。于是，每次运动过程中的噪声是随机的。如果我们不理会这个噪声，那么只根据指令来确定的位置可能与实际位置相差十万八千里。
        与运动方程相对应，还有一个观测方程。观测方程描述的是，当小萝卜在 $x_{K}$ 位置上看到某个路标点 $y_{j}$ 时，产生了一个观测数据 $z_{k,j}$ 。同样，用一个抽象的函数 $h$ 来描述这个关系：

                                                         $z_{k,j} = h(y_{j},x_{k},v_{k,j})$

        这里， $v_{k,j}$ 是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据 $z$ 及观测方程 $h$ 也有许多不同的形式。
        我们用的函数 $f,h$ ，似乎并没有具体地说明运动和观测是怎么回事？同时，这里的 $x,y,z$ 又是什么呢？事实上，根据小萝卜的真实运动和传感器的种类，存在着若干种参数化(Parameterization)方式。

        什么叫参数化呢？举例来说，假设小萝卜在平面中运动，那么，它的位姿 $R$ 由两个位置和一个转角来描述，即 $x_{k} = [x_{1},x_{2},\theta ]_{k}^{T}$ ，其中 $x_{1},x_{2}$ 是两个轴上的位置而 $\theta$ 为转角。同时，输入的指令是两个时间间隔位置和转角的变化量 $u_{k}=[\Delta x_{1},\Delta x_{2} ,\Delta \theta ]_{k}^{T}$ ，于是，此时运动方程就可以具体化为：

                                                  $\begin{bmatrix} x_{1}\\ x_{2} \\ \theta \end{bmatrix}_{k}=\begin{bmatrix} x_{1}\\ x_{2} \\ \theta \end{bmatrix}_{k-1}+ \begin{bmatrix} \Delta x_{1}\\ \Delta x_{2} \\ \Delta \theta \end{bmatrix}_{k} +w_{k}$

        这是简单的线性关系。不过，并不是所有的输人指令都是位移和角度的变化量,例如“油门”或者“控制杆”的输入就是速度或加速度量，所以也存在着其他形式更加复杂的运动方程，那时我们可能需要进行动力学分析。
        关于观测方程，以小萝卜携带着的一个二维激光传感器为例。我们知道激光传感器观测一个2D路标点时，能够测到两个量:路标点与小萝卜本体之间的距离 $r$ 和夹角 $\varphi$ 。记路标点为 $y_{j} = [y_{1},y_{2}]_{j}^{T}$ ，位姿为 $x_{k} = [x_{1},x_{2}]_{k}^{T}$ ，观测数据为 $z_{k,j} =[ r_{k,j},\varphi _{k,j}]^{T}$ ，那么观测方程就写为：

                                         $\begin{bmatrix} r_{k,j}\\ \phi r_{k,j} \end{bmatrix} =\begin{bmatrix} \sqrt{(y_{1,j}-x_{1,k})^{2}+(y_{2,j}-x_{2,k})^{2}}\\ ( arctan\frac{y_{2,j}-x_{2,k}}{y_{1,j}-x_{1,k}} ) \end{bmatrix} +v$

        考虑视觉SLAM时，传感器是相机，那么观测方程就是“对路标点拍摄后，得到图像中的像素”的过程。这个过程牵涉相机模型的描述，在第5讲中详细介绍。
        可见，针对不同的传感器，这两个方程有不同的参数化形式。如果我们保持通用性，把它们取成通用的抽象形式，那么SLAM过程可总结为两个基本方程：

     $x_{k} = f(x_{k-1},u_{k},w_{k})\rightarrow k=1,2.....K$

$z_{k,j} = h(y_{j},x_{k},v_{k,j})\rightarrow (k,j)\epsilon O$

        其中 $O$ 是一个集合，记录着在哪个时刻观察到了哪个路标（通常不是每个路标在每个时刻都能看到的——我们在单个时刻很可能只看到一小部分)。

        这两个方程描述了最基本的SLAM问题：当知道运动测量的读数 $u$ ，以及传感器的读数 $z$ 时，如何求解定位问题（估计 $x$ ）和建图问题（估计 $y$ ）?这时，我们就把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量?

        状态估计问题的求解，与两个方程的具体形式，以及噪声服从哪种分布有关。按照运动和观测方程是否为线性、噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

        其中线性高斯系统(Linear Gaussian，LG系统)是最简单的，它的无偏的最优估计可以由卡尔曼滤波器(Kalman Filter，KF)给出。

        而在复杂的非线性高斯系统 (Non-Linear Non-Gaussian，NLNG系统) 中，我们会使用以扩展卡尔曼滤波器(Extended Kalman Filter，EKF)和非线性优化两大类方法去求解。直至21世纪早期，以EKF为主的滤波器方法在SLAM中占据了主导地位。我们会在工作点处把系统线性化，并以预测—更新两大步骤进行求解(见第10讲)。最早的实时视觉SLAM系统就是基于EKF2开发的。随后，为了克服 EKF的缺点(例如线性化误差和噪声高斯分布假设)，人们开始使用粒子滤波器(Particle Filter)等其他滤波器，乃至使用非线性优化的方法。时至今日，主流视觉SLAM使用以图优化(Graph Optimization)为代表的优化技术进行状态估计。我们认为优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法（见第10讲和第11讲)。

6.2 状态估计问题

6.2.1 批量状态估计与最大后验估计

1.没有噪声时的运动与观测模型

由6.1可知，经典SLAM模型由一个运动方程和一个观测方程构成：

$\left\{\begin{matrix} x_{k} = f(x_{k-1},u_{k},w_{k})\\ z_{k,j}=h(y_{j},x_{k},v_{k,j}) \end{matrix}\right.$
式6-1 SLAM问题的数学表达

        通过第4讲， $x_{k}$ 是相机的位姿，可以用 $SE(3)$ 来描述。

        观测方程，第5讲已经说明，即针孔相机模型。

        为了对它们有更深的印象，我们不妨讨论其具体参数化形式。

        首先，位姿变量 $x_{k}$ 可以由 $T_{k}\epsilon SE(3)$ 表达。其次，运动方程与输入的具体形式有关，但在视觉SLAM中没有特殊性(和普通的机器人、车辆的情况一样)，我们暂且不谈。

        观测方程则由针孔模型给定。假设在 $x_{k}$ 处对路标 $y_{j}$ 进行了一次观测，对应到图像上的像素位置 $z_{k,j}$ ，那么，观测方程可以表示成：



$sz_{k,j}=K(R_{k}y_{j}+t_{k})$
式6-2 观测方程的参数化表示

         其中 $K$ 为相机内参， $s$ 为像素点的距离，也是 $R_{k}y_{j}+t_{k}$ 的第三个分量。如果使用变换矩阵 $T_{k}$ 描述位姿，那么路标点 $y_{j}$ 必须以齐次坐标来描述，计算完成后要转换为非齐次坐标。

图6-1 像素点的距离s指什么

2.加入噪声后出现的状态估计问题

         现在，考虑数据受噪声影响后会发生什么改变。在运动和观测方程中，我们通常假设两个噪声项 $w_{k},v_{k,j}$ i满足零均值的高斯分布，像这样：

                                                 $w_{k}\sim N(0,R_{k}) ,v_{k}\sim N(0,Q_{k,j})$

        其中 $N$ 表示高斯分布，0表示零均值， $R_{k},Q_{k,j}$ 为协方差矩阵。在这些噪声的影响下，我们希望通过带噪声的数据 $z$ 和 $u$ 推断位姿 $x$ 和地图 $y$ (以及它们的概率分布)，这构成了一个状态估计问题。（即根据像素点位置和终端输入推断机器人的位置以及路标点的位置）

3.两种处理状态估计的办法(如何根据像素点估计出图像原来的位置)

        处理这个状态估计问题的方法大致分成两种。由于在SLAM过程中，这些数据是随时间逐渐到来的，所以，我们应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量/渐进(incremental)的方法，或者叫滤波器。在历史上很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器及其衍生方法求解它。

        另一种方式，则是把数据“攒”起来一并处理，这种方式称为批量(batch)的方法。例如，我们可以把0到k时刻所有的输入和观测数据都放在一起。问？在这样的输入和观测下，如何估计整个0到k时刻的轨迹与地图呢？

(给出所有的 $z_{k,j}$ or 部分 $z_{k,j}$ )

        这两种不同的处理方式引出了很多不同的估计手段。

        大体来说，增量方法仅关心当前时刻的状态估计 $\mathbf{x_{k}}$ ，而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，而成为当前视觉SLAM的主流方法。

        极端情况下，我们可以让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，这也正是SfM (Structure from Motion)的主流做法。

        当然，这种极端情况显然是不实时的，不符合SLAM的运用场景。所以在SLAM中，实用的方法通常是一些折衷的手段。例如，我们固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，这是后面要讲到的滑动窗口估计法。