视觉SLAM笔记(49) 后端状态估计


1. 误差累积

前端视觉里程计能给出一个短时间内的轨迹和地图
但由于不可避免的误差累积,这个地图在长时间内是不准确的
所以,在视觉里程计的基础上
还希望构建一个尺度、规模更大的优化问题,以考虑长时间内的最优轨迹和地图
不过,考虑到精度与性能的平衡,实际当中存在着许多不同的做法


2. 状态估计的概率解释

觉里程计只有短暂的记忆,而希望整个运动轨迹在较长时间内都能保持最优的状态
可能会用最新的知识,更新较久远之前的状态——站在“久远的状态”的角度上看,仿佛是未来的信息告诉它“你应该在哪里”
所以,在后端优化中,通常考虑一个更长时间内(或所有时间内)的状态估计问题
而且不仅使用过去的信息更新自己的状态,也会用未来的信息来更新自己
这种处理方式不妨称为“批量的”(Batch

否则,如果当前的状态只由过去的时刻决定,甚至只由前一个时刻决定
那不妨称为“渐进的”(Incremental

已经知道 SLAM 过程可以由运动方程和观测方程来描述
那么,假设在 t = 0 到 t = N 的时间内
有 x0 到 xN 那么多个位姿,并且有 y1,…, yM 那么多个路标
按照之前的写法,运动和观测方程为:

在这里插入图片描述

注意以下几点:

  1. 观测方程中,只有当 xk 看到了 yj 时,才会产生观测数据,否则就没有
    事实上,在一个位置通常只能看到一小部分路标
    而且,由于视觉 SLAM 特征点数量众多
    所以实际当中观测方程数量会远远大于运动方程的数量
  2. 可能没有测量运动的装置,所以也可能没有运动方程
    在这个情况下,有若干种处理方式:认为确实没有运动方程,或假设相机不动,或假设相机匀速运动
    这几种方式都是可行的
    在没有运动方程的情况下,整个优化问题就只由许多个观测方程组成
    这就非常类似于 SfM(Structure from Motion)问题,相当于通过一组图像来恢复运动和结构
    与 SfM 中不同的是, SLAM 中的图像有时间上的先后顺序,而SfM 中允许使用完全无关的图像

每个方程都受噪声影响
所以要把这里的位姿 x 和路标 y 看成服从某种概率分布的随机变量,而不是单独的一个数


3. 所关心的问题

因此,关心的问题就变成了:
当拥有某些运动数据 u 和观测数据 z 时,如何来确定状态量 x,y 的分布?

进而,如果得到了新来的数据之后,那么它们的分布又将发生怎样的变化?
在比较常见且合理的情况下,假设状态量和噪声项 服从高斯分布
意味着在程序中,只需要储存它们的均值和协方差矩阵即可
均值可看作是对变量最优值的估计,而协方差矩阵则度量了它的不确定性

那么,问题转变为:
当存在一些运动数据和观测数据时,如何去估计状态量的高斯分布?
依然设身处地地扮演一下小萝卜
只有运动方程时,相当于蒙着眼睛在一个未知的地方走路
尽管知道自己每一步走了多远,但是随着时间增长,将对自己的位置越来越不确定
这说明在输入数据受噪声影响时,对位置方差的估计将越来越大
但是,当睁开眼睛时,由于能够不断地观测到外部场景,使得位置估计的不确定性变小了
如果用椭圆或椭球直观地表达协方差阵,那么这个过程有点像是在手机地图软件中走路的感觉
以下图为例,可以想象,当没有观测数据时,这个圆会随着运动越来越大
而如果有正确观测的话,圆就会缩小至一定的大小,保持稳定

在这里插入图片描述


4. 最大似然估计

上面的过程以比喻的形式解释了状态估计中的问题,下面要以定量的方式来看待它
视觉SLAM笔记(26) 状态估计问题,介绍了最大似然估计,提到把状态估计转换为最小二乘的做法
现在更仔细地讨论这个问题
首先,由于位姿和路标点都是待估计的变量,改变一下记号,令 xk 为 k 时刻的所有未知量
它包含了当前时刻的相机位姿与 m 个路标点
在这种记号的意义下(虽然与之前稍有不同,但含义是清楚的),写成:
在这里插入图片描述
同时,把 k 时刻的所有观测记作 zk
于是,运动方程与观测方程的形式可写得更加简洁
这里不会出现 y,但这时 x 中已经包含了之前的 y 了:
在这里插入图片描述
现在考虑第 k 时刻的情况
希望用过去 0 到 k 中的数据,来估计现在的状态分布:
在这里插入图片描述

下标 0 : k 表示从 0 时刻到 k 时刻的所有数据
请注意 zk 来表达所有在 k 时刻的观测数据,注意它可能不止一个,只是这种记法更加方便

下面来看如何对状态进行估计:
按照 贝叶斯(Bayes)法则,把 zk 与 xk 交换位置,有:
在这里插入图片描述
这里 第一项称为似然,第二项称为先验

似然由观测方程给定,而先验部分,要明白当前状态 xk 是基于过去所有的状态估计得来的
至少,它会受 xk−1 影响,于是按照 xk−1 时刻为条件概率展开:

在这里插入图片描述
如果考虑更久之前的状态,也可以继续对此式进行展开
但现在只关心 k 时刻和 k − 1 时刻的情况
至此,给出了贝叶斯估计,但上式还没有具体的概率分布形式
所以还没法实际地操作它
对这一步的后续处理,方法上产生了一些分歧

大体来说,存在若干种选择:
其一是假设马尔可夫性,简单的一阶马氏性认为, k 时刻状态只与 k −1 时刻状态有关,而与再之前的无关
如果做出这样的假设,就会得到以扩展卡尔曼滤波(EKF)为代表的滤波器方法
在滤波方法中,会从某时刻的状态估计,推导到下一个时刻
另外一种方法是依然考虑 k 时刻状态与之前所有状态的关系,此时将得到非线性优化为主体的优化框架

目前视觉SLAM 主流为非线性优化方法


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(48) 局部地图
视觉SLAM笔记(47) 优化 PnP 的结果
视觉SLAM笔记(46) 基本的 VO
视觉SLAM笔记(45) 搭建 VO 框架
视觉SLAM笔记(44) RGB-D 的直接法


发布了217 篇原创文章 · 获赞 292 · 访问量 289万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览