本讲开始进入后端模块。后端优化目前分为基于滤波器的方法和基于非线性优化的方法,本讲讲述以EKF为代表的滤波器法,以及稀疏性。
一、 后端优化概述
1. 后端模块的作用
前端估计得到:
- kkk时刻相机的位姿,xkx_kxk
- 地图中路标jjj的世界坐标,yjy_jyj
前端估计只基于kkk的前一时刻或前几时刻。后端优化所做的,则是根据到目前为止观测到的所有信息,优化之前的估计结果。
后端优化中,把位姿xxx和路标yyy都看做服从某种概率分布的随机变量。已知相机运动数据uuu,观测数据zzz,问题转化为确定其概率分布,进行最大似然估计。
后端优化主要有两种方法:
- 假设马尔科夫性,kkk时刻状态只与k−1k-1k−1时刻有关。即本讲的EKF法。
- 假设kkk时刻状态与之前所有状态有关,为基于非线性优化的方法。将在下一讲详细阐释。
2. 问题描述
变量:
xkx_kxk:kkk时刻下所有未知量,包含相机和此时刻m个可见路标。
已知:
u1:ku_{1:k}u1:k:前kkk帧的运动数据。
z1:kz_{1:k}z1:k:前kkk帧的观测数据。
估计:
P(xk∣x0,u1:k,z1:k)P(x_k|x_0,u_{1:k},z_{1:k})P(xk∣x0,u1:k,z1:k):根据所有已知,估计现在的位姿和路标的状态分布。
根据贝叶斯法则:
P(xk∣x0,u1:k,z1:k)∝P(zk∣xk)P(xk∣x0,u1:k,z1:k−1)P(x_k|x_0,u_{1:k},z_{1:k})∝P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1})P(xk∣x0,u1:k,z1:k)∝P(zk∣xk)P(xk∣x0,u1:k,z1:k−1)
即:后验正比于似然乘先验。
其中,先验指前k−1k-1k−1帧的观测。根据马尔科夫假设,对先验展开得:
P(xk∣x0,u1:k,z1:k−1)=∫P(xk∣xk−1,x0,u1:k,z1:k−1)P(xk−1∣x0,u1:k,z1:k−1)dxk−1P(x_k|x_0,u_{1:k},z_{1:k-1})=\int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})dx_{k-1}P(xk∣x0,u1:k,z1:k−1)=∫P(xk∣xk−1,x0,u1:k,z1:k−1)P(xk−1∣x0,u1:k,z1:k−1)dxk−1
即:xkx_kxk受xk−1x_{k-1}xk−1影响,考虑所有k−1k-1k−1时刻的xxx可能,对每一种可能,其概率是xk−1x_{k-1}xk−1下xkx_{k}xk的概率,乘以xk−1x_{k-1}xk−1的概率。
问题的最终目标是:求状态变量的最大后验概率形式。
二、 卡尔曼滤波器
1. 线性系统和卡尔曼滤波器(KF)
根据马尔科夫性,当前时刻状态只和上一时刻有关。我们来化简先验概率部分,把这个公式挪下来:
P(xk∣x0,u1:k,z1:k−1)=∫P(xk∣xk−1,x0,u1:k,z1:k−1)P(xk−1∣x0,u1:k,z1:k−1)dxk−1P(x_k|x_0,u_{1:k},z_{1:k-1})=\int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})dx_{k-1}P(xk∣x0,u1:k,z1:k−1)=∫P(xk∣xk−1,x0,u1:k,z1:k−1)P(xk−1∣x0,u1:k,z1:k−1)dxk−1
等式右边第一项可以化简为:
P(xk∣xk−1,x0,u1:k,z1:k−1)=P(xk∣xk−1,uk)P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})=P(x_k|x_{k-1},u_{k})P(xk∣xk−1,x0,u1:k,z1:k−1)=