SLAM初学-后端(一)
date
Jun 13, 2022
Last edited time
Mar 27, 2023 08:50 AM
status
Published
slug
SLAM初学-后端(一)
tags
SLAM
summary
type
Post
Field
Plat
后端要做什么？
前端能够根据相邻的两幅图像判断出此时此刻的位姿，是暂时的；那么后端需要对前端测量以及计算的结果进行矫正，不仅用过去的信息，也用未来的信息更新自己，希望能够得到一个长时间的正确状态。
前面有讲过，在运动方程和观测方程中，如果把位姿和路标看成随机变量，就可以把问题变为已知观测数据和运动数据情况下，如何确定状态量的分布问题，是一个状态估计问题。假设噪声和状态量服从高斯分布，只需要估计它的均值和方差，即可确定状态量的分布。
完成后端优化可以使用滤波器，也可以使用非线性优化，本讲对两种方法都做了推导。
概述
状态估计的概率解释
SLAM过程可以由运动方程与观测方程来描述, 那么假设在 到 的时间段内, 有位姿 到 , 并且有路标 , 那么运动方程与观测方程写为:
其中, 为运动数据, 为观测数据. 注意以下几点:
- 观测方程中, 只有 能看到 才有观测数据, 否则就没有. 一般情况下, 由于视觉SLAM的特征点数量 众多, 实际的观测方程数量会远远大于运动方程.
- 由于可能没有能够测量运动方程的设备, 因此没有运动方程. 这时候, 我们会假设相机保持静止或者相机保持匀速直线运动.
- 当只有运动方程的时候, 误差是逐渐累积的, 我们对位置方差的估计将会越来越大. 而如果有观测方程, 那么位置估计的不确定性会变小.
我们可以将位姿和路标写在一起, 将 记为 , 其为 时刻的所有未知量, 包含了当前时刻的相机位姿与 个路标点. 将 时刻的所有观测记为 . 那么, 原方程可以写为:
在已知 时刻的状态、 时刻的观测下，估计 现在的状态分布 , 则可以得到
证明
记 .
则
假设 之间相互独立, 则 , 带入得:
另外也可以得到 .
即
其中, 第一项为由观测方程确定得似然, 第二项为先验部分.
先验概率中 时刻的状态 受到前面 时刻状态的影响，对先验概率进行以 时刻为条件展开，可以写为
- 后续的方法
- 马尔可夫方法
- 非线性优化
现在没有具体的概率分布形式, 后续有不同的不同的方式进行处理.
假设马尔可夫性（k时刻状态只与k-1时刻状态有关，与之前状态无关）。
依然考虑 时刻状态与之前所有状态之间的联系, 此时将得到以非线性优化为主题的优化框架.
线性系统与KF(卡尔曼滤波)
由于假设了马尔可夫性, 则当前时刻的状态只与上一个时刻的状态有关，式二第一个概率化简为 , 第二个概率化简为 .
- 假设
- 状态量服从高斯分布
- 可以使用线性方程描述运动方程和观测方程
假设状态量服从高斯分布, 那么我们只需要维护状态的均值以及方差即可. 在线性高斯系统中，用线性方程描述运动方程和观测方程，写为
假设这里的噪声服从高斯分布: . 假设我们已知 时刻的状态估计 与协方差 . 记 为后验分布, 为先验分布.
后验的估计
那么, 先验的分布为:
这一步称为预测, 它表示了如何从上一个时刻的状态, 根据有噪声的输入信息推断当前时刻的状态分布. 记为 . 则
后验的更新
后验的分布为:
那么, 根据式二, 我们假设 , 可以得到:
由于等式两侧都是高斯分布, 因此只需要比较等式两边的指数部分.
比较 的二次系数有:
则
令
得到后验方差表达式:
另外, 还可以仅使用先验概率来表示 如下:
另外, 比较 一次系数可得:
带入式(6), 得到均值方差表达式:
总结
上面的步骤可以归纳为预测以及更新两个步骤.
- 预测
- 更新: 先计算 ,它又称为卡尔曼增益
然后计算后验概率分布:
在第一步预测中，使用 时刻的后验分布估计 时刻的先验分布，这一步不确定性变大，第二步使用 时刻的先验分布估计 时刻的后验分布，对于结果进行修正，缩小不确定性。
非线性系统和EKF(扩展卡尔曼滤波)
由于在SLAM中的运动方程以及观测方程一般都是非线性函数, 那么一个符合高斯分布的状态经过非线性变换之后, 往往就不再满足高斯分布. 因此, 我们将卡尔曼滤波器的结果拓展到非线性系统, 通常会在某个点处对方程进行展开，保留一阶项，按照线性系统进行推导。
线性化
对于 时刻的运动方程与观测方程, 我们在 处进行线性化(即一阶泰勒展开).
记 , 上式可以写为:
预测
我们使用 时刻的后验 来得到 时刻的先验 , 因此将 带入得到:
对于方差 只与 的方差 和噪声方差 有关, 则
这里认为 为一个确定的常数, 那么 也是一个常数.
则预测部分为:
其中, 为 的方差.
更新
卡尔曼增益为:
然后计算后验概率分布:
这一步与 KF 一致
EKF的局限
- 滤波器方法在一定程度上假设了马尔可夫性，也就是 时刻的状态只与 时刻相关，而与 之前的状态和观测都无关.
- 由于 EKF 仅仅做了一阶泰勒展开, 因此存在非线性误差.
- EKF 需要存储状态量的均值和方差，并对它们进行维护和更新, 因此不可适用于大型场景.
- EKF 等滤波器由于没有异常检测机制, 因此导致系统在存在异常值的时候容易发散.
BA 与图优化
Bundle Adjustment，是指从视觉重建中提炼出最优的3D模型和相机参数（内参数和外参数）。
优化目标
回顾整个投影过程，假设已知相机的外参数 和世界坐标系的 点坐标，求解 点的像素坐标过程。
- 世界坐标转化为相机坐标：
- 相机坐标投影到归一化平面：
- 去畸变（径向畸变）：
- 计算像素坐标：
整个过程中，用到了相机的位姿 和路标 的世界坐标，得到了路标的像素坐标，对应到观测方程 中，观测 就是像素坐标 ，相机位姿 用李代数表示 ，路标 用三维点 的坐标。那么通过降低观测到的像素坐标 和估计到的像素坐标 之间的误差，就可以得到最优的 和 的解。用最小二乘表示整体的代价函数为：
采用非线性优化更新增量的方式求解，令 ， 。
引入增量后，目标函数变为：
其中, 表示 对于相机位姿的偏导数， 表示 对于路标点位置的偏导数。
接下来就可以使用非线性优化方法进行求解.