没有合适的资源?快使用搜索试试~ 我知道了~
移动机器人FastSLAM算法
5星 · 超过95%的资源 需积分: 49 87 下载量 14 浏览量
2017-11-28
20:00:43
上传
评论 4
收藏 1016KB DOC 举报
温馨提示
试读
18页
移动机器人FastSLAM算法。公式推导;FastSLAM的粒子表示形式;机器人模型;算法步骤和代码
资源推荐
资源详情
资源评论
移动机器人 FastSLAM 算法
1. 基于粒子滤波器的 SLAM 算法
1.1 粒子滤波器定位的基本原理
粒子滤波(PF)定位方法用粒子集描述概率分布,而不用概率分布函数本身表示
[1]
。
卡尔曼滤波器(KF)和扩展卡尔曼滤波器(EKF)利用参数化模型(一个高维的高斯函
数)来表示概率密度分布;粒子滤波器采用一系列有限的采样粒子代表概率密度分布,
高概率区域含有高概率的粒子,反之亦然。粒子滤波以样本集合的方法逼近概率分布,
如果粒子数趋于无穷大,那么这种非参数化的粒子分布能够逼近任何复杂的概率分布。
因此,粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布。它采用一
组带有权值的随机样本来描述概率分布。在概率高的区域,粒子的密度就大,相反,
在概率低的区域,粒子的密度就小。
粒子滤波定位方法优点很突出:能够处理多峰分布;能够很大地降低内存耗费;
能够更精确地定位,且便于实现;不受限于高斯分布,在很多定位问题中,如位姿跟
踪、全局定位甚至绑架问题,都取得较好的应用。粒子滤波方法作为一种基于贝叶斯
估计的非线性滤波算法,在处理非高斯非线性时变系统的参数估计和状态滤波问题方
面具有独特的优势。
EKF 只适用于高斯噪声模型,对于非线性的模型表现非常差。而粒子滤波则适用
于任意噪声模型。利用 EKF 只能在滤波误差和预测误差很小的情况下通过对系统的非
线性进行近似以获得令人满意的估计精度,否则滤波初期估计协方差下降太快会导致
滤波不稳定甚至发散。此外 EKF 还要求噪声必须服从高斯分布,大大限制它的应用范
围。而 PF 在测量的基础上,通过调节个粒子的权值大小和样本的位置,来近似实际的
概率分布,并以样本的均值作为系统的估计值。因此,PF 不受线性化误差和高斯噪声
假定的限制,适用于任何状态转换和观测模型,有效克服了 EKF 的缺点。
PF 也有缺点:相对于 EKF,粒子滤波的计算了较大,特别是在采用非常多的粒子
数目时。随着计算机处理能力的不断增强,以及各种改进的实时粒子滤波算法不断涌
现,PF 算法计算了大等障碍正在逐渐消失;在采用 PF 算法时,在粒子滤波中,普遍
存在的问题可能是存在的退化现象
[2]
。因为 PF 抽取 N 个具有权值的粒子后,系统如接
收到新的观测数据,则实时更新每个粒子的权值。重要性权值的分布会变得越来越倾
斜,这时有可能出现粒子匮乏现象。即经过几次迭代后,少数样本会占去总体样本的
绝大部分权重,除了一个粒子以外,其余粒子均只有微小的权值,可忽略不计,这样
1
使得样本集不能体现所要表达的后验概率密度,同样使得大量计算浪费在几乎不起任
何作用的粒子的更新上。
1.2 扩展卡尔曼滤波器算法
卡尔曼滤波器是由Kalman在20世纪60年代提出的一种递推滤波器算法,它不需要
保留使用过的测量数据,当系统检测到新的测量数据后,可以利用递推公式获得新的
系统状态估计。扩展卡尔曼滤波器是在卡尔曼滤波器的基础上针对非线性系统输出估
计提出的一种改进方法。在扩展卡尔曼滤波器中,对非线性系统首先采用一级泰勒展
开进行线性化近似,然后利用最小均方误差准则对非线性系统状态方程和测量方程的
输出进行估计。在基于扩展卡尔曼滤波器的机器人同步定位与地图重建(Extended
Kalman Filter SLAM,EKF-SLAM)算法中,系统噪声均被默认为符合高斯分布,因此
利用系统状态的均值和协方差进行系统状态输出的描述。在 EKF-SLAM中,系统状态
中包括机器人的位姿和已检测到所有路标的位置,因此,协方差矩阵的大小随着路标
数量成平方比而增大。在EKF-SLAM算法中,根据机器人的运动状态方程,并结合机
器人当前位姿和运动控制输入利用扩展卡尔曼滤波器对机器人下一时刻的位姿进行预
测估计。当机器人传感器检测到外部路标时,根据传感器测量方程,结合机器人当前
位姿、路标位置以及传感器的测量信息采用扩展卡尔曼滤波器进行系统状态的更新。
传统的EKF-SLAM算法存在如下两个典型问题,一是计算复杂度,由于在EKF-
SLAM中维护着系统状态的协方差,并且在每次接受到传感器测量数据时需要进行系统
状态协方差的更新,因此EKF-SLAM算法的计算复杂度为 (其中 为已检测到
的路标数目)。二是系统状态估计的一致性,由于EKF-SLAM算法中利用扩展卡尔曼
滤波器对机器人运动方程和传感器测量方程的输出进行估计,然而在扩展卡尔曼滤波
器中,对系统输出采用的是一级泰勒公式展开近似,因此当系统的非线性非常严重时,
采用扩展卡尔曼滤波器对系统输出估计的精确性会严重降低,甚至导致输出估计的发
散。针对EKF-SLAM算法计算复杂度大的问题,Newman等人提出了一种基于局部子地
图序列的SLAM算法,它的核心是重建一系列相互独立而且具有固定大小的局部地图,
然后再利用扩展卡尔曼滤波器将各个局部地图合并为一个完整的全局地图,从而保证
SLAM过程中的耗时不会随着检测到路标数目的增大而无限增大。Lina等人从机器人位
姿估计一致性最大化角度对局部地图的大小取值进行了分析。Chong等人同样提出利用
子地图进行环境地图描述的方法,他的做法是对子地图之间的相对位置进行单独估计,
当机器人回到先前的子地图区域时,采用搜索算法对机器人进行重新定位。子地图分
割法可以有效降低计算的复杂度,但它带来的最大问题是数据关联的确认上有一定的
困难。
1.3 粒子重采样
通过增加采样点数可以有效地克服退化现象,采样点数越小退化现象越严重,然
而采样点数大时又影响计算的实时性。可见通过增加采样点数来解决退化问题是不现
2
实的。
目前消除退化问题的两个关键技术分别是:选取适当的重要密度函数和进行再采
样。粒子滤波的收敛精度和收敛速度直接关系到它能否实现应用及应用的范围,是另
一个值得重点研究的关键内容。为了避免粒子匮乏,Gorden 等人提出了重采样方案,
其主要思想史除去那些权值小的粒子,并复制权值大的粒子,重采样通过过滤样本集
中权值较低的样本。保留高权重样本,使权重集中在权重大的样本上,来达到解决退
化问题,一般根据权重对后验概率密度进行离散近似处理。
重采样原理: 时刻的先验概率可由带有权值的粒子近似表示。经过系统观测
并重新计算权值,那些权值大的粒子可以分裂出若干新的粒子,而权值很小的粒子将
被丢弃,如此即可得到一组新的粒子。这些新的粒子加入随机量后即可预测在 t 时刻的
状态,即系统状态转移过程。最后再次进入系统观测过程,并预测下一时刻的状态。
重采样阶段会造成样本有效性和多样性的损失,导致样本贫化现象。如何保持粒子的
有效性和多样性,克服样本贫化也是应该考虑的问题之一。
2. SLAM 的通用框架和理论模型
编码器
测量和位置预
测
用确定的地图
估计
细调特征参数
增加可信度
加入新的特征
扩充地图
消除讨厌的特
征减小可信度
地图
匹配
非期望
的观测
在机传感器观
测
细调特征参数
增加可信度
加入新的特征
扩充地图
消除讨厌的特
征减小可信度
不
可
预
测
的
观
测
否
非期望的观测
是
否
匹配的
预测和观测
是
原始传感器数据
感
知
预
测
的
特
征
观
测
图 1 SLAM 通用框架
3
SLAM 问题是把定位和地图构建结合在一起的,下图为 SLAM 的理论模型:
移动机器人
路标
x
t 1
u
t
x
t
u
t 1
x
t 1
u
t 2
x
t 2
z
jt ,1
m
j
z
kt ,
m
k
图 2 SLAM 的理论模型
3. 移动机器人 SLAM 问题描述
3.1 SLAM 计算复杂度
SLAM解法在实际应用中需要满足实时性要求,即传感器的数据采集周期应大于
SLAM算法处理一帧传感器数据的时间。全状态SLAM算法如EKF-SLAM的计算复杂度
与状态向量的维数成指数关系,在路标数目较大时全状态SLAM算法不能满足实时性要
求 , 因 而 不 适 合 大 范 围 环 境 中 的 SLAM 问 题 , Guivant 等 人 提 出 了 CEKF-
SLAM(Compressed EKF)算法,采用近似的EKF全方差计算方法,将大范围环境分成
若干局部子区域,在大部分时间仅用EKF更新状态向量中的局部估计,在机器人从一
个局部区域转向另一个局部区域时对状态向量的均值和方差进行全局更新,从而大大
提高了计算效率。此外,Leonard等将整个地图分解为一系列子地图,然后对每个子地
图进行滤波估计,降低了状态向量方差更新过程中的计算复杂度。扩展信息滤波器
(Extended Information Filter,EIF)是EKF的对称形式,它保持了EKF数学严谨、估计
准确的特点,并且,其信息矩阵几乎稀疏,特别适合SLAM问题,Eustice等从稀疏信息
矩阵的角度提出了SEIF-SLAM算法,估计精度也得到了保证。 Montemerlo等人采用
1
2
4
剩余17页未读,继续阅读
资源评论
- motekxj2017-12-08很不错的经验总结哦,谢谢分享!
口袋里のInit
- 粉丝: 1065
- 资源: 82
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功