学完了高博的《SLAM十四讲》,基本了解了SLAM的框架结构,然后看了一些ORB-SLAM的代码,跑了几个模型,看了几篇论文,感觉还是有很多关于状态估计的问题不是很清楚,然后在知乎看到了关于slam推荐书籍,有一本状态估计和概率机器人,但是状态估计目前只有英文版,发现概率机器人有中文版,所以就买了一本概率机器人。虽然翻译质量可能不是太好,但是还是能读,有些地方还是要看英文才行!感谢翻译概率机器人的作者,节省了时间!最近一口气把整个概率机器人的第一部分看完了,自己做了一点笔记,还是贴出来吧!做笔记只是为了加深自己理解,可能有些地方理解不到位,欢迎各位批评指正!

整本书分为4部分,分别是基础知识、定位、地图构建和规划与控制。基础知识一共6章。第一章绪论就不说了,其余五章分别是递归状态估计(贝叶斯滤波)、高斯滤波、非参数滤波、机器人运动(状态方程)、机器人感知(观测方程)。接下来我会把这六章的笔记都贴出来,加深自己理解!

接下来就开始贴第二章的笔记吧!

本章主要介绍的是机器人中贝叶斯滤波算法的基本思想。本章为机器人及其环境的交互建立了一个耦合的动态系统模型,也就是建立了两个方程,观测方程和控制方程。机器人通过控制方程来影响环境,观测方程仅仅观测环境变化,而不会对其产生影响。

在概率机器人中,机器人和环境通过上述两个方程联系在一起,这两个方程也可以看做是两个分布,即状态转移分布和观测分布。状态转移分布描述了状态如何随时间的变化而变化的特征(控制方程)。观测分布则是描述测量状态的特征(由控制方程引起的状态改变)。因为这两个都是以概率形式来描述的,所以就形成了概率机器人的基石。

机器人置信度是对给定所有过去传感器测量和所有过去控制的环境状态的一个后验分布。贝叶斯滤波是计算机器人置信度的基本算法,它是一个递归的,t时刻的置信度由t-1时刻的置信度来计算。置信度反应的是机器人有关环境状态的内部信息,比如机器人位姿,一般而言机器人是不知道自己的位姿的,它必须通过外部观测数据来推测出自己的位姿,也就是从内部置信度识别出自己的真实状态。在概率机器人当中是通过条件概率分布来表示置信度。对于真实的状态置信度分布可以为每一个可能的假设分配一个概率。置信度分布是以已经获得的观测数据为条件的关于状态变量的一个后验概率。

在还没有进行t时时刻的测量之前,仅仅基于t-1时刻之前的测量和t时刻之前的控制,我们可以得到一个预测概率,即:

它是一个基于之前状态的后验,在综合时刻t的测量之前,预测了t时刻的状态。由该预测计算 bel(xt)的过程 被称为测量更新。

贝叶斯滤波有一个非常重要的假设,就是马尔科夫假设,它假设当前状态是所有过去状态的总结,也就是说,下一个状态仅仅由当前状态决定,与之前的状态都无关。

贝叶斯滤波算法

贝叶斯算法有两个基本步骤组成:

1. 计算预测,也就是根据上一个状态和当前的控制预测当前的状态。

2. 测量更新,根据当前的测量结果去修正上一步的预测结果。这一步主要是根据贝叶斯法则来实现的,也就是条件概率。

整个算法如下:

第三章高斯滤波(正态分布)

高斯滤波也就是在将贝叶斯滤波里面的所有置信度都用正态分布来表示。高斯分布具有单峰,这是符合机器人学中很多的追踪问题都是单峰的,并且后验都是以小的不确定性聚集在真实状态的周围。本章讨论了两种参数的绿滤波,基于矩参数的滤波和基于正则参数的滤波。矩参数是指均值和方差,因为均值和方差是概率分布的一阶矩和二阶矩。正则参数由信息矩阵和信息向量组成。通过矩阵求逆,可以从一个参数得到另外的一个参数,这两种方法对偶的。

对于这两种滤波方式都必须要满足三个基本要求:1.初始置信度必须满足高斯分布。2.状态转移方程是一个包含独立高斯噪声,且是与参数呈线性关系的函数。3.测量方程也是一个包含独立高斯噪声,且与参数呈线性关系的函数。

两种滤波的方式都可以扩展到非线性系统。可以通过泰勒展开用近似替代的方法得到一个近似线性的函数,这样的方法叫做扩展卡尔曼滤波和扩展信息滤波。

卡尔曼滤波还可以采取不同的线性近似方法得到无迹卡尔曼滤波,通过将函数选定点进行无迹变换得到线性化。扩展卡尔曼滤波核无迹卡尔曼滤波都是通过将非线性函数近似线性化来处理得到的。泰勒级数展开和无迹变换的精度性取决于两个因素:系统的非线性程度和不确定程度。如果能够较为准确的知道系统的状态,那么由线性化所造成的误差就相对较小。

高斯向多峰后验的扩展被称为多假设卡尔曼滤波,它用高斯混合分布表示后验,高斯混合分布就是多个高斯分布的加权和,在更新该滤波的时候需要分裂和融合或者修剪单个高斯分布。

卡尔曼滤波算法

卡尔曼滤波实现了连续状态的置信度计算,但是它不适用与离散或者混合的状态空间。它用矩参数表示置信度:在t时刻,置信度用均值和方差表示。如果它满足三个条件和马尔科夫性那么它的后验就是高斯的。三个条件在上面已经提到了,状态转移方程的具体形式如下:

其中 xt  xt-1 分别表示两个时刻的状态, ut 为t时刻的控制向量。 At  Bt 分别是 nXn 和的 nXm 矩阵,其中m是控制向量的维数,n是状态的维数。 εt 是一个满足高斯分布的噪声,均值为0,方差为 Rt 

测量方程的具体形式如下:

Ct kXn 的矩阵,k的测量向量的维数,n是状态的维数。 σt 是满足高斯分布的测量噪声,均值为0,方差为 Qt 

初始置信度满足正态分布,初始置信度的均值为 μ0 ,方差为 Σ0 

整个算法如下:

前两行分别计算预测的均值和方差,这两个表示了在测量更新前的预测置信度,这只是仅仅考虑了控制所引起的改变。第三行是计算的卡尔曼增益,具体实现过程可以在网上百度,有很多介绍。第四行和第五行是考虑了测量之后的对预测的置信度进行的更新。卡尔曼滤波由于是贝叶斯滤波的一种,所以它也包含贝叶斯滤波的两个步骤,预测和更新。

扩展卡尔曼滤波

由于卡尔曼滤波算法有三个要求,其中观测方程和测量方程都必须满足线性关系,但是很多实际情况下这两个方程可能都是非线性的,所以卡尔曼滤波不能很好的满足这个需求。但是有很多办法可以将非线性的近似为线性处理,扩展卡尔曼滤波通过泰勒展开的办法来将非线性函数近似线性化。

状态转移方程和观测方程如下:

g(ut,xt-1)  h(xt)都是非线性函数。 εt 是一个满足高斯分布的噪声,均值为0,方差为 Rt  δt 是满足高斯分布的测量噪声,均值为0,方差为 Qt 

将两个非线性函数泰勒展开:

既然是近似,那么肯定要选取一个认为最有可能的值来近似,这样近似的效果才会好,所以这里用 ut-1 去近似 xt-1 ,用t是课的预测均值去近似真实的状态值。那么状态转移置信度的均值就变成了],观测置信度的均值变为了,两个的方差都没有发生改变。这样我们就可以得到扩展卡尔曼滤波算法了。

整个算法如下:

无迹卡尔曼滤波算法

扩展卡尔曼滤波是采用的泰勒级数展开得到的,但是为了得到更好的结果,我们还可以采用另外一种方法,通过加权统计线性回归过程实现随机线性化,这种方法通常能够得到更好的近似,被称为无迹卡尔曼滤波算法。

这些点的具体选取过程如下:通过选取位于均值处的点和对称分布于主轴协方差处的两个点(每一维),然后将其进行非线性变换。具体的选取规则如下:

参数 β 是考虑了附加的高斯噪声所产生的影响。如果分布是精确的高斯模型,通常取值为2.

选取的点经过非线性变换得到对应的点:

经过变换后的点服从的高斯分布均值和方差计算如下:

整个算法如下:

信息滤波

卡尔曼滤波的对偶算法就是信息滤波,他们两个有着相同的假设,都假设置信度用高斯分布表示。两者主要的不同在于对高斯置信度的表示不同,卡尔曼滤波是用他们的矩(一阶矩和二阶矩),而信息滤波是用他们的正则参数来表示的。由于参数的不同也导致了不同的更新方程,但是思想都是一样的。具体而言就是对于卡尔曼滤波而言可能参数计算复杂,那么信息滤波可能会简单点,但是信息滤波计算复杂的地方,卡尔曼滤波计算又要简单一些。

正则参数由矩阵和向量组成。他们可以由一阶矩和二阶矩推导出来。

这两个参数是通过高斯变换简单的处理而得到的。

把只含有的项提取出来,其余都是常量,得到:

把矩阵 Ω 和向量 ξ 带入可以得到由正则参数表示的高斯分布

信息滤波算法:

对于信息滤波而言,同样需要满足三个要求,状态转移方程和观测方程都是线性的,且都有一个均值为0的高斯噪声,初始状态满足高斯分布,观测方程和状态转移方程都如卡尔曼滤波一样。

整个算法如下:

信息滤波如同卡尔曼滤波一样,都是分为两个步骤,先预测然后更新。可以看到信息滤波再预测的时候需要求一个 nXn 矩阵的逆矩阵,这将加大运算难度。而在卡尔曼滤波中就不会存在这个问题。但是在预测的时候卡尔曼滤波需要求卡尔曼增益,这个卡尔曼增益会有一个求逆矩阵,所以这两个是对偶的。

扩展信息滤波

同样的,信息滤波也可以近似非线性的情况,称为扩展信息滤波算法,状态转移方程和观测方程都和扩展卡尔曼滤波是一样的,符号意义也相同。

整个算法如下:

这就是概率机器人第二章和第三章的内容了。现在先打算把笔记做完,然后过段时间尽量把每个算法的代码都写一下!欢迎各位批评正!