【视觉SLAM十四讲学习笔记】第六讲——状态估计问题

发布时间:2024年01月19日

专栏系列文章如下:
【视觉SLAM十四讲学习笔记】第一讲——SLAM介绍
【视觉SLAM十四讲学习笔记】第二讲——初识SLAM
【视觉SLAM十四讲学习笔记】第三讲——旋转矩阵
【视觉SLAM十四讲学习笔记】第三讲——旋转向量和欧拉角
【视觉SLAM十四讲学习笔记】第三讲——四元数
【视觉SLAM十四讲学习笔记】第三讲——Eigen库
【视觉SLAM十四讲学习笔记】第四讲——李群与李代数基础
【视觉SLAM十四讲学习笔记】第四讲——指数映射
【视觉SLAM十四讲学习笔记】第四讲——李代数求导与扰动模型
【视觉SLAM十四讲学习笔记】第五讲——相机模型

我们已经知道,方程中的位姿可以由变换矩阵来描述,然后用李代数进行优化。观测方程由相机成像模型给出,其中内参随相机固定,外参则为相机位姿。于是,我们已经弄清了经典SLAM模型在视觉情况下的表达。
然而由于噪声的存在,运动方程和观测方程的等式必定是不是精确成立的。尽管相机可以非常好地符合针孔模型,但我们得到的数据通常是受各种未知噪声影响的。
所以,预期假设数据必须符合方程,不如讨论如何在有噪声的数据中进行准确的状态估计。解决状态估计问题需要一定程度的最优化背景知识。

状态估计问题

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

经典的SLAM模型由一个运动方程和一个观测方程构成:
在这里插入图片描述

x_k是相机的位姿,可以用T_k∈SE(3)表达。运动方程与输入的具体形式有关。观测方程由真空相机模型给定。假设在x_k处对路标y_j进行了一次观测,对应到图像上的像素位置z_k,j,那么观测方程可以表示成:
在这里插入图片描述
其中K为相机内参,s为像素点的距离,也是(R_k y_j+t_k)的第三个分量。如果使用变换矩阵T_k描述位姿,那么路标点y_j必须以齐次坐标来描述,计算完成后要转换为非齐次坐标。

现在,考虑数据受噪声影响后会发生什么改变。在运动和观测方程中,我们通常假设两个噪声项w_k,v_k,j满足零均值的高斯分布:
在这里插入图片描述
其中N表示高斯分布,0表示零均值,R_k,Q_k,j为协方差矩阵。在这些噪声的影响下,我们希望通过带噪声的数据z和u推断位姿x和地图y(以及它们的概率分布),这构成了一个状态估计问题。

处理这个状态估计问题的方法大致分为两种。

  1. 增量/渐进(incremental),又称滤波器。由于在SLAM过程中,数据随时间逐渐到来,所以我们应该持有一个当前时刻的估计状态,然后用新的数据来更新它。增量方法仅关心当前时刻的状态估计x_k,而对之前的状态则不多考虑。
  2. 批量(batch),把数据攒起来一并处理。相对增量方法,批量方法可以在更大范围达到最优化,被认为优于传统的滤波器,而成为当前视觉SLAM的主流方法。

由于讨论的是批量方法,考虑从1到N的所有时刻,并假设有M个路标点。定义所有时刻的机器人位姿和路标点坐标为

在这里插入图片描述

同样地,用不带下标的u表示所有时刻的输入,z表示所有时刻的观测数据。那么,对机器人状态的估计,从概率学的观点来看,就是已知输入数据u和观测数据z的条件下,求状态x,y的条件概率分布:
在这里插入图片描述
特别地,当我们不知道控制输入,只有一张张图像时,即只考虑观测方程带来的数据时,相当于估计P(x,y|z)的条件概率分布,此问题也称为SfM,即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布,利用贝叶斯法则,有

在这里插入图片描述
贝叶斯法则左侧称为后验概率,右侧的P(z|x)称为似然(Likehood),另一部分P(x)称为先验(Prior)。直接求后验分布是困难的,但是求一个状态最优估计,使得在该状态下后验概率最大化,则是可行的:

在这里插入图片描述

贝叶斯法则的分母部分与待估计的状态x,y无关,因而可以忽略。贝叶斯法则告诉我们,求解最大后验概率等价于最大化似然和先验的乘积

但是,我们不知道机器人位姿或路标大概在什么地方,此时就没有了先验。那么,可以求解最大似然估计(Maximize Likelihood Estimation, MLE):

在这里插入图片描述
直观地讲,似然是指“在现在的位姿下,可能产生怎样的观测数据”。由于我们知道观测数据,所以最大似然估计可以理解成:“在什么样的状态下,最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

最小二乘的引出

如何求最大似然估计呢?在高斯分布的假设下,最大似然能够有较简单的形式。回顾观测模型,对于某一次观测:

在这里插入图片描述
由于我们假设了噪声项
在这里插入图片描述

所以观测数据的条件概率为:

在这里插入图片描述

它依然是一个高斯分布。考虑单词观测的最大似然估计,可以使用最小化负对数来求一个高斯分布的最大似然。

我们知道高斯分布在负对数下有较好的数学形式。考虑任意高位高斯分布x~N(μ, Σ),它的概率密度函数展开形式为

在这里插入图片描述
对其去负对数,则变为

在这里插入图片描述

因为对数函数是单调递增的,所以对原函数求最大化相当于对负对数求最小化。在最小化上式的x时,第一项与x无关,可以略去。于是,只要最小化右侧的二次型项,就得到了对状态的最大似然估计。代入SLAM的观测模型,相当于在求:

在这里插入图片描述

我们发现,该式等价于最小化噪声项的一个二次型,称为马哈拉诺比斯距离,又叫马氏距离。它也可以看成由Q_k,j-1 加权之后的欧氏距离(二范数),这里Q_k,j-1 也叫做信息矩阵,即高斯分布协方差矩阵之逆。

现在我们考虑批量时刻的数据。通常假设各个时刻的输入和观测是相对独立的,这意味着各个输入之间是独立的,各个观测之间是独立的,并且输入和观测也是独立的。于是我们可以对联合分布进行因式分解:

在这里插入图片描述

这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差:

在这里插入图片描述

那么,最小化所有时刻估计值与真实读数之间的马氏距离,等价于求最大似然估计。负对数允许我们把乘积变成求和:

在这里插入图片描述
这样就得到了一个最小二乘问题(Least Square Problem),它的解等价于状态的最大似然估计。直观上看,由于噪声的存在,当我们把估计的轨迹与地图代入SLAM的运动、观测方程中时,它们并不会完美地成立。对于这种情况,我们对状态的估计值进行微调,使得整体的误差下降一些。当然,这个下降也有限度,它一般会到达一个极小值。这就是一个典型的非线性优化的过程。

仔细观察上式,我们发现SLAM中的最小二乘问题具有一些特定的结构:

  • 首先,整个问题的目标函数由许多个误差的(加权的)二次型组成。虽然总体的状态变量维数很高,但每个误差项都是简单的,仅与一两个状态变量有关。例如,运动误差只与x_k-1,x_k有关,观测误差只与x_k,y_j有关。这种关系会让整个问题有一种稀疏的形式。
  • 其次,如果使用李代数表示增量,则该问题是无约束的最小二乘问题。如果用旋转矩阵/变换矩阵描述位姿,则会引入旋转矩阵自身的约束,即需在问题中加入s.t.RT R=I且det(R)=1这样令人头大的条件。额外的约束会使优化变得更加困难。这体现了李代数的优势。
  • 最后,我们使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。例如,某次的观测非常准确,那么协方差矩阵就会“小”,而信息矩阵就会“大”,所以这个误差会在整个问题中占有较高的权重。

批量状态估计

现在介绍如何求解这个最小二乘问题,这需要一些非线性优化的基本知识

考虑一个非常简单的离散时间系统:
在这里插入图片描述
这可以表达一辆沿x轴前进或后退的汽车。第一个公式为运动方程,u_k为输入,w_k为噪声;第二个公式为观测方程,z_k为对汽车位置的测量。取时间k=1,…,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 。按照先前的推导,我们知道最大似然估计为

在这里插入图片描述

对于具体的每一项,比如运动方程,我们知道:

在这里插入图片描述
观测方程:

在这里插入图片描述

根据这些方法,我们就能够实际地解决上面的批量状态估计问题。根据之前的叙述,可以构建误差变量:

在这里插入图片描述

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

在这里插入图片描述

此外,这个系统是线性系统,我们可以很容易地将它写成向量形式。定义向量y=[u,z]T ,那么可以写出矩阵H,使得

在这里插入图片描述
那么:

在这里插入图片描述

在这里插入图片描述

整个问题可以写成

在这里插入图片描述

之后我们将看到,这个问题有唯一解:

在这里插入图片描述

文章来源:https://blog.csdn.net/PuddleRubbish/article/details/135689114
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。