SLAM学习笔记(八)

后端

Posted by QIY on September 6, 2020

高翔老师SLAM14讲学习笔记

后端

  • 后端(Backend)(状态估计问题)
    • 从带噪声的数据估计内在状态——状态估计问题
    • Estimated the inner state from noisy data
  • 渐进式(Incremental)
    • 保持当前状态的估计,在加入新信息时,更新已有的估计(滤波)
  • 批量式(Batch)
    • 给定一定规模的数据,计算该数据下的最优估计(优化)

      1 状态估计问题

  • 数学描述
    • 符号定义:
    • t = 0 … N 时间内,机器人的位姿为: (相当于一个R、t),同时有路标 (三D点)
    • 运动和观测方程记为: f:非线性函数,Uk:运动输入量,wk:噪声量; 第二个公式呢:在k时刻对yj这个路标做一次观测,拿到了zk,j(2d点 u、v),(对3D观测得到一个2D像素,就是这样一件事情),h也是一个非线性函数。 如果是纯视觉SLAM,没有对相机运行估计,没有相机运动的输入量、观测量,那只有下面这个式子(观测方程)。 整个过程可以理解为,当你拿有噪声的数据时,任意时候只拿到了z 和 u,通过z和u去估计x0-xn和y0-yn服从怎样的一种分布。一般是拿到x、y的联合分布。
  • 用随机变量表示状态,估计其概率分布
  • k 时刻所有待估计的量组成k时刻状态: 符号稍微有点冲突
  • k 时刻观测统一记成: 方程简化为:
  • 根据过去0时刻到k时刻的数据
  • 估计当前的状态:
  • Bayes展开: (提取zk)
  • 先验部分按条件概率展开:

    k时刻受先前状态的影响 k-1时刻的状态估计

  • 分歧:
    • 可以假设k时刻状态只和k-1时刻有关(假设了一阶马尔可夫性)
    • 或假设k时刻状态与先前所有时刻均有关(不假设马尔可夫性) 假设一阶马尔可夫性的情况: (运行方程) 第二项: 状态观测方程。
  • 于是,该公式指出了如何从k-1时刻的状态分布递推至k时刻的分布
    • 只是现在我们还没有代入具体的分布形式 在线性模型、高斯状态分布下,我们将得到卡尔曼滤波器 在非线性模型、高斯状态分布下,可以在工作点附近线性展开,得到扩展卡尔曼滤波器

      2 卡尔曼滤波器(KF)

      目的:求

  • 卡尔曼滤波器的推导
    • 线性模型和高斯噪声: (高斯白噪声) 状态的高斯分布(区别先后验) 后验(仅通过运行方程推的话称为后验,上尖尖表示,假设k-1时刻是知道了,它满足一个高斯分布,均值是 ,协方差是 先验(上杠杠)。 忽略计算,得 卡尔曼滤波的更新式: 总结: 经典卡尔曼滤波器的五个公式,给出了线性高斯系统的最优无偏估计 10.24:使用运行方程,k-1的后验推k时刻的先验。 10.25-10.26:k时刻的先验推k时刻的后验。

      3 扩展卡尔曼滤波器(EKF)

      -假设在工作点附近是一个线性系统,同样使用卡尔曼推导有: -

  • EKF优点
    • 推导简单清楚,适用各种传感器形式
    • 易于做多传感器融合
  • EKF缺点
    • 一阶马尔可夫性过于简单
    • 可能会发散(要求数据不能有outlier,如 特征匹配错误)
    • 线性化误差(仅有一次迭代的线性优化问题)
    • 需要存储所有状态量的均值和方差,平方增长

      4 BA与图优化

      目的:BA与图优化结合,更好更快的优化BA。

  • Bundle Adjustment已经在之前介绍过了
    • 事实上BA属于批量式的优化方法
    • 给定很多个相机位姿与观测数据,计算最优的状态估计
    • 定义每个运动/观测方程的误差,并从初始估计开始寻找梯度下降
  • BA问题与图结构的关系
    • BA虽然是个纯优化问题,但亦可以用图模型清晰地表述出来
    • 顶点为优化变量,边为运动/观测约束
    • 本身还有一些特殊的结构
  • 考虑在位姿 i 处对路标 j 的一次观测 zij: 特点:
  • 每个观测只关系两个变量,其中一个是相机,一个是路标
  • 纯视觉Ba中,不存在相机与相机/路标与路标之间的关联
  • 整个误差函数由许多个这样小的项组成 如果对变量做好排序,例如所有相机位姿在前,路标在后,那么 H 有一定的特殊结构: 图模型与H矩阵存在对应关系:图模型中存在边=>H相应地方出现非零块
  • 实际当中的H,路标数量远大于位姿数量(箭头形矩阵或镐子形矩阵)
  • 利用H的特殊结构,可以大幅加速Hx=-b线性方程的求解
  • 加速手段又称边缘化(Marginalization)[意义有很多种]
  • Hx=-b的结构(以下目的为了消元): B:小,对角块;C,大,对角块 E和E转置:与图模型对应,可稠密
  • 因为 C 是对角块,所以可以用C对E进行消元(高斯消元),得到: 从而:
  • 该方程组分为两步来求:
    1. 求解上半部分,规模较小,得到
    2. 将结果代入下半部分,得到
  • 这个做法称为Marginalization或Schur消元
    1. 从消元角度来讲,亦可使用Cholesky等其他消元方式解此稀疏方程
    2. 从Marginalization角度来讲,是我们把所有的路标信息边缘化到了相机的信息中(这样说 边缘化就是“消元”)