崔岩的笔记——FastSLAM(1)序章

符号对照表

S_t t时刻机器人位姿 P_t 线性运动噪声
S^t {S_1,S_2,...,S_t} g(S_t,\theta,n_t) 测量模型
\theta_n 第n各地标的位置 R_t 线性测量噪声
\theta 地图。包括n个地标的位置 \hat Z_{n_t} 第n_t个地标t时刻的期望观测
Z_t t时刻传感器观测 Z_t-\hat Z_{n_t} 量测更新
Z^t {Z_1,Z_2,...Z_t} Z_t 更新协矩阵
u_t t时刻控制量 G_\theta 地标位姿的雅可比测量模型
u^t {u_1,u_2,...,u_t} G_{S_t} 机器人位姿的雅可比测量模型
n_t t时刻所有的观测关联数据 S_t t时刻的FastSLAM粒子
n^t {n_1,n_2,...,n_t} S_t^{[m]} t时刻第m个FastSLAM粒子
h(S_{t-1},u_t) 运动模型

联合估计

SLAM,Simultaneous Localization and Mapping,实时定位与构图,既没有准确的机器人路径,也没有先验地图信息。

因为运动噪声,导致机器人的位姿是不准确的。

又因为测量噪声和机器人位姿的不确定性,导致与外界环境的相对位置不准,即构图不准确。

因此机器人位姿和地图应当同时进行估计。

后验估计

通过控制信息对运动噪声进行预测,即通过控制信息进行概率约束得到正确位姿。

通过观测信息对环境特征噪声进行测量,即通过观测信息进行概率约束得到机器人与环境的关系。

当获得当前时刻的观测信息时,不仅要更新当前的地图特征,还应当修正之前的地图特征。

后验概率的优点:

①增强噪声环境下实时定位与构图的鲁棒性

②可以比较不同环节之间的不确定性,即比较不同环节之间的后验概率分布

SLAM中的贝叶斯滤波器:

P(S_t,\theta|Z^t,u^t,n^t) = \eta P(Z_t|S_t,\theta,n_t)\int P(S_t|S_{t-1},u_t)P(S_{t-1},\theta|Z^{t-1},u^{t-1},n^{t-1})dS_{t-1}

其中的联合后验概率为:

P(S_t,\theta|Z^t,u^t)

但实际上我们无法获得观测数据与地标之间的关联数据,即该观测数据是相对于哪一个地标的观测数据(地标一致性问题)。因此在上式中写作:

P(S_t,\theta|Z^t,u^t,n^t)

扩展卡尔曼滤波

EKF除了给出状态量的方差还需要给出各状态量之间的关系(用协方差矩阵表示)

EKF的两个缺点:

①复杂度

需要大量的计算量和存储空间

任何传感器信息的输入都需要对所有状态量进行更新

②单假定数据关联

我们给定的协方差阵和状态量之间的关联都是不准确的

EKF没有关于数据关联不确定性方面的机制,EKF所给出的数据关联是单一且确定的,因此上述误差不可逆转

我们可以通过同时考虑多个数据关联来提高EKF中数据关联的准确性,但需要付出一定的计算代价

SLAM的结构性及稀疏性

对于一个观测量来说,与其关联较大的状态量较少,因此可以根据这个特征进行稀疏矩阵的分解计算。

FastSLAM

崔岩的笔记——FastSLAM(1)序章_第1张图片

 

SLAM就如同一个动态贝叶斯网络

若已知机器人的路径,则各个地标点的位置估计是相互独立的(原理参考d—分离)

则:

P(S_t,\theta|Z^t,u^t,n^t) = P(S_t|Z^t,u^t,n^t) \prod_{n=1}^NP(\theta_n|S^t,Z^t,u^t,n^t)

P(S_t|Z^t,u^t,n^t)为路径后验

\prod_{n=1}^NP(\theta_n|S^t,Z^t,u^t,n^t)为地标估计

粒子滤波的递归更新

①采样新的机器人路径,给定新的控制

②根据观测对地标进行更新(EKF)

③为每个粒子分配权重

④粒子重采样

我们根据机器人路径进行后验估计,那么估计的位数应当随路径增加而增加

多假设数据关联

使用粒子滤波可以让每个粒子用不用的数据关联模型,铜鼓哦粒子滤波控制地标的增加与减少

计算权重时应考虑每个粒子间地标的不同,剔除个别粒子

你可能感兴趣的:(FastSLAM,slam,概率论)