通俗易懂!视觉slam第十部分——贝叶斯估计

SLAM 过程可以由运动方程和观测方程来描述。那么,假设在 t = 0 到t = N 的时间内,我们有 那么多个位姿,并且有 那么多个路标。按照之前的写法,运动和观测方程为:

每个方程都受噪声影响,所以要把这里的位姿 x 和路标 y 看成服从某种概率分布的随机变量,而不是单独的一个数。因此,我们关心的问题就变成了:当我拥有某些运动数据 u 和观测数据 z 时,如何来确定状态量 x,y 的分布?进而,如果得到了新来时刻的数据之后,那么它们的分布又将发生怎样的变化?在比较常见且合理的情况下,我们假设状态量和噪声项服从高斯分布——意味着在程序中,只需要储存它们的均值和协方差 矩阵即可。均值可看作是对变量最优值的估计,而协方差矩阵则度量了它的不确定性。那
么,问题转变为:当存在一些运动数据和观测数据时,我们如何去估计状态量的高斯分布?

只有运动方程时,相当于我们蒙着眼睛在一个未知的地方走路。尽管我们知道自己每一步走了多远,但是随着时间增长,我们将对自己的位置越来越不确定——内心也就越加不安。这说明在输入数据受噪声影响时,我们对位置方差的估计将越来越大。但是,当我们睁开眼睛时,由于能够不断地观测到外部场景,使得位置估计的不确定性变小了,我们就会越来越自信。如果用椭圆或椭球直观地表达协方差阵,那么这个过程有点像是在手机地图软件中走路的感觉。以下图为例可以想象,当没有观测数据时,这个圆会随着运动越来越大;而如果有正确观测的话,圆就会缩小至一定的大小,保持稳定。

不确定性的直观描述。左侧:只有运动方程时,由于下一个时刻的位姿是在上一个时刻基础上添加了噪声,所以不确定性越来越大。右侧:存在路标点(红色)时,不确定性会明显减小。不过请注意这只是一个直观的示意图,并非实际数据。 

由于位姿和路标点都是待估计的变量,我们改变一下记号,令为 k 时刻的所有未知量。它包含了当前时刻的相机位姿与 m 个路标点。在这种记号的意义下(虽然与之前稍有不同,但含义是清楚的),写成:

同时,把 k 时刻的所有观测记作。于是,运动方程与观测方程的形式可写得更加简洁。这里不会出现 y,但我们心里要明白这时 x 中已经包含了之前的 y 了:

现在考虑第 k 时刻的情况。我们希望用过去 0 到 k 中的数据,来估计现在的状态分布:

下标 0 : k 表示从 0 时刻到 k 时刻的所有数据。请注意来表达所有在 k 时刻的观测数据,注意它可能不止一个,只是这种记法更加方便。下面我们来看如何对状态进行估计。按照 Bayes 法则,把交换位置,有:

第一项称为似然,第二项称为先验。似然由观测方程给定,而先验部分,我们要明白当前状态x_{k}是基于过去所有的状态估计得来的。至少,它会受x_{k-1}影响,于是按照x_{k-1}时刻为条件概率展开:

如果我们考虑更久之前的状态,也可以继续对此式进行展开,但现在我们只关心 k 时刻和 k − 1 时刻的情况。至此,我们给出了贝叶斯估计,虽然上式还没有具体的概率分布形式,所以我还没法实际地操作它。对这一步的后续处理,方法上产生了一些分歧。大体来说,存在若干种选择:其一是假设马尔可夫性,简单的一阶马氏性认为,k 时刻状态只与 k−1 时刻状态有关,而与再之前的无关。如果做出这样的假设,我们就会得到以扩展卡尔曼滤波(EKF)为代表的滤波器方法。在滤波方法中,我们会从某时刻的状态估计,推导到下一个时刻。另外一种方法是依然考虑 k 时刻状态与之前所有状态的关系,此时将得到非线性优化为主体的优化框架。 

发布了369 篇原创文章 · 获赞 189 · 访问量 51万+

猜你喜欢

转载自blog.csdn.net/qq_32146369/article/details/104431493