基于粒子滤波器的SLAM的仿真研究
基于滤波slam算法总结
基于滤波slam算法总结滤波SLAM（Simultaneous Localization and Mapping）算法是移动机器人领域的一项关键技术，它能够在未知环境中实现定位与地图构建。
本文将基于滤波SLAM算法进行总结，梳理其发展历程、主要算法及其优缺点，以期为相关领域的研究者和工程师提供参考。
一、滤波SLAM算法概述滤波SLAM算法是一种基于概率论的方法，主要用于解决移动机器人在未知环境中的定位与地图构建问题。
滤波SLAM算法的核心思想是通过观测模型和运动模型对机器人的状态进行估计，从而实现定位与地图构建。
根据所使用的滤波方法不同，滤波SLAM算法可以分为以下几类：扩展卡尔曼滤波（EKF-SLAM）、粒子滤波（PF-SLAM）和优化滤波（Optimization-based SLAM）。
二、主要滤波SLAM算法及其优缺点1.扩展卡尔曼滤波（EKF-SLAM）优点：EKF-SLAM算法计算量较小，适用于线性化程度较高的系统。
缺点：当系统非线性程度较高时，EKF-SLAM算法的线性化近似可能导致性能下降；同时，EKF-SLAM算法难以处理大规模地图。
2.粒子滤波（PF-SLAM）优点：粒子滤波能够较好地处理非线性、非高斯系统，适用于复杂环境下的SLAM问题。
缺点：粒子滤波算法计算量较大，实时性较差；此外，当粒子数量较少时，可能导致滤波发散。
3.优化滤波（Optimization-based SLAM）优点：优化滤波算法具有较高的准确性和鲁棒性，能够处理大规模地图和复杂环境。
缺点：优化滤波算法计算量较大，实时性相对较差。
三、滤波SLAM算法发展趋势1.算法优化：针对不同场景和需求，对滤波SLAM算法进行优化，提高其准确性和实时性。
2.数据融合：结合多种传感器数据，如激光雷达、摄像头、IMU等，提高SLAM系统的鲁棒性和准确性。
3.深度学习：利用深度学习技术，如卷积神经网络（CNN）和循环神经网络（RNN），实现端到端的SLAM算法。
基于混合信息滤波的粒子滤波SLAM算法
基于混合信息滤波的粒子滤波SLAM算法王晓华;杨幸芳【期刊名称】《计算机应用研究》【年(卷),期】2013(30)7【摘要】为解决粒子滤波SLAM中存在的计算效率高及粒子退化造成的估计精度低等问题,结合精确稀疏滞后状态信息滤波估计精度高以及精确稀疏扩展信息滤波计算效率高的优点,将两者混合应用于粒子滤波SLAM算法中,不但在保证计算效率的条件下提高了状态估计精度,并且还克服了机器人转动状态以及环境特征疏密带来的应用缺陷.实验结果表明了该方法的有效性与可行性.%In order to solve the problems of much computation and low estimation precision caused by particle degradation.The advantage of ESDF is high estimation accuracy and ESEIF's advantage is high efficiency.This paper used a mixture of them in the particle filter SLAM,which not only used the historical information maintained by information matrix of the relationship between the robot pose and characteristics fully and improved the accuracy of the estimate,but also overcame the defects in the application made by robot rotational state and characteristics density.Experimental results show that the proposed algorithm is valid and feasible.【总页数】4页(P1988-1990,1994)【作者】王晓华;杨幸芳【作者单位】西安工程大学电子信息学院,西安 710048;西安工程大学电子信息学院,西安 710048【正文语种】中文【中图分类】TP242【相关文献】1.基于精确稀疏扩展信息滤波的粒子滤波SLAM算法研究 [J], 朱代先;王晓华2.基于稀疏扩展信息滤波和粒子滤波的SLAM算法 [J], 朱代先;王晓华3.基于粒子滤波的未知环境通信的多机器人SLAM算法研究 [J], 曾凌烽;高易年;王欣4.基于改进粒子滤波的SLAM算法研究 [J], 孙昊;周阳;李丽娜5.基于高斯-粒子滤波的SLAM算法提取果实特征 [J], 王丹丹;石峰;杜雪;袁赣南因版权原因，仅展示原文概要，查看原文内容请购买。
无线传感器网络环境下基于粒子滤波的移动机器人SLAM算法
摘
丹, 李
勇, 张
辉, 李
迅
( 国防科技大学 机电工程与自动化学院 , 湖南 长沙 410073) 要 : 定位问题是移 动机 器 人研 究 领域 中 最基 本 的 问题 , 在 Bayes 的 框 架下 研 究了 机 器人 与 无 线传 感 器网 络
( W SN ) 组成系统中的同时建图与定位问题 ( SLAM ). 针对该系统中只存在距离测量信息可用的情况提出了一 种基于 粒子滤波的 SLAM 算法 . 该方法将机器人状态和节点位置估计设置为 一组全局估 计粒子 , 通过对 粒子及其权 重的更 新来计算整个系统的状态 . 算法将 W SN 节点的位置 估计在 机器人 的路径上 分解为 相互独 立的估 计 , 从而将全 局粒 子的计算转化为使用一个 机器人状态滤波器和对应 于每个 机器人 粒子的节 点位置 滤波器 进行计 算 . 针对观测 信息 低维的特点 , 设计了处理低维观测信息的方法 , 使得 观测信 息可以 在滤波阶 段得到 合理利 用 . 并且详 细介绍了 提出 的 SLAM 算法原理和计算过程 , 并通过仿真实验证明了算法的有效性和实用性 . 关键词 : 无线传感器网络 ; 移动机器人 ; 同时定位与建图 ; 粒子滤波 中图分类号 : TP24 文献标识码 : A 文章编号 : 1673 4785( 2010) 05 0425 07
[ 7]
示为 Y1!t = ( Y1, Y2, ∀, Yt ).
图 1 机器人、 无线传感器 网络系统 F ig. 1 Robot and W SN sy stem
.
机器人装备有里程计, 根据里程计的数据可以 获得相邻时间序列的运动输出 . 将机器人从 t - 1 到 t时刻的运动输出计为 ut . 机器人 r 和节点 j 之间可 以通过无线通信设备进行测距 , 使用 Z t 表示系统在 t时刻 ( 在 [ t- 1 , t] 时段 ) 获得的测量集合 . 其中 Z t = { z t }, z t 代表机 器人与节点 ct 之间 的测量, ct#
基于粒子滤波的室内巡逻机器人SLAM技术研究
TECHNOLOGY AND INFORMATION68 科学与信息化2023年1月下基于粒子滤波的室内巡逻机器人SLAM技术研究*梁明亮 张凯 王云飞郑州铁路职业技术学院 河南 郑州 451460摘 要 在室内巡逻机器人工作中，同步定位和地图构建（SLAM）技术是其关键。
集中于机器人系统硬件和算法两个层次，怎样精准定位，改善地图构建精度成为研究重点。
本文对Rao-Blackwellized粒子滤波器、RBPF-SLAM 进行改进，并采用马尔可夫链－再取样法（MCMC）技术，解决该算法中粒子劣化问题，加入激光雷达观察模式，以改善建议分布的准确性。
关键词 机器人；室内定位；同步定位与地图构建；粒子滤波；全向移动Research on SLAM Technology of Indoor Patrol Robot Based on Particle Filtering Liang Ming-liang, Zhang Kai, Wang Yun-feiZhengzhou Railway V ocational and Technical College, Zhengzhou 451460, Henan Province, ChinaAbstract In the work of indoor patrol robots, simultaneous localization and mapping (SLAM) technology is crucial. Focusing on the two levels of robot system hardware and algorithm, how to accurately locate and improve the accuracy of mapping has become the focus of research. In this paper, the Rao-Blackwelled particle filter and RBPF-SLAM are improved, and the Markov Chain Monte Carlo (MCMC) technique is used to solve the particle deterioration problem in the algorithm, and the laser radar observation mode is added to improve the accuracy of the proposed distribution.Key words robot; indoor localization; simultaneous localization and mapping; particle filtering; omnidirectional movement引言本论文对巡逻机器人的全向运动底盘进行设计，有效解决在现场测试中容易遇到的转向困难、碰撞等问题。
基于粒子滤波的SLAM算法并行优化与实现
基于粒子滤波的SLAM算法并行优化与实现朱福利;曾碧;曹军【摘要】Simultaneous localization and mapping is a new type of mobile robot localization method, which can obtain data through the mobile robot's own sensors and simultaneous localization and map building in a completely unknown environment. Based on PF-SLAM algorithm, the probability distribution of the location pose is expressed by the particle set, and the calculated amount is proportional to the size of the particle set, and also the number of particles determines the algorithm's location accuracy and anti-jamming capability. At the same time, increasing the number of particles will increase the computing time, which will lead to the positioning delay and the positioning error of the mobile robot. A method is presented to improve the algorithm by using GPU parallel computing, which can reduce the calculation time, thereby to reduce the positioning error caused by positioning delay. Experimental results show that the improved algorithm of GPU parallel computing has a significant effect.%基于粒子滤波的即时定位与地图构建(simultaneous localization and mapping,SLAM)算法,可在完全未知的环境进行即时的定位和地图构建.该算法使用粒子集表示定位位姿的概率分布情况,计算量与粒子集的规模成正比,在一定范围内,粒子的数量越多,算法的定位准确度和抗干扰能力越好,但在增加粒子数量的同时,将增加计算时间,从而导致定位延迟,造成移动机器人的定位误差.提出一种结合粒子滤波和SLAM算法特点的GPU并行优化的方法进行加速,从而减少计算带来的定位延迟和定位误差.通过实验,证明使用GPU并行计算的算法改进有明显效果.【期刊名称】《广东工业大学学报》【年(卷),期】2017(034)002【总页数】5页(P92-96)【关键词】即时定位与地图构建;粒子滤波;GPU并行计算【作者】朱福利;曾碧;曹军【作者单位】广东工业大学计算机学院,广东广州 210000;广东工业大学计算机学院,广东广州 210000;广东工业大学计算机学院,广东广州 210000【正文语种】中文【中图分类】TP301移动机器人的自主定位是机器人智能化的一种重要体现，已成为移动机器人领域中的热门研究方向. 即时定位与地图构建[1-3](simultaneous localization and mapping，SLAM)解决的问题是指移动机器人在一个完全未知的环境中，仅使用自带的传感器，在移动过程中根据从传感器中获得的数据进行即时定位，并增量式地构建环境地图. 目前SLAM算法已成为机器人研究的热点，被认为是实现完全自主智能的移动机器人的关键.粒子滤波(Particle Filter，PF)[4-5]算法是贝叶斯滤波的一种实现，适用于非线性、非高斯时变系统，基于粒子滤波的SLAM算法是SLAM定位问题的主流解决方法之一.机器人的SLAM定位结果常用于机器人的实时避障和路径规划，这要求SLAM定位具有较高的精度和实时性. 基于粒子滤波的SLAM算法计算时间复杂度大，而SLAM定位结果又需要实时性，计算时间过长则增加定位延迟，这将导致较大的定位误差.目前大部分对基于粒子滤波SLAM算法的改进研究都集中于改进算法的定位精度或通过控制粒子数量来降低计算时间，如文献[6]利用人工鱼群算法优化粒子分布来提高SLAM算法的定位精度，这种方法在增加定位精度的同时增加了算法复杂度和计算时间；文献[7]通过KLD(Kullback-Leibler Distance)算法控制粒子数量，在数据不够收敛的时候仍需要大量的粒子，不能根本解决算法计算时间长的问题.针对此问题，本文提出一种使用GPU并行计算[8]的方法对基于粒子滤波的SLAM 算法进行改进优化，并对改进效果进行分析.SLAM利用传感器获得运动和环境测距信息进行实时定位，并同时根据定位结果进行实时的地图构建. SLAM可描述为式(1)[9]：其中，k表示时刻；为k时刻定位的先验概率分布；xk表示机器人的定位后验概率分布；z为激光雷达等传感器获得的观测数据；u为移动数据.为系统运动模型，为观测模型.粒子滤波使用粒子的分布表示概率分布，是一种非线性滤波器，主要有预测、更新、重采样等过程[10-12]，是基于粒子滤波SLAM算法的核心. 算法利用粒子集表示移动机器人的定位结果的概率分布，粒子集中的每个粒子表示机器人的可能位姿，图1为算法实现基本流程.(1) 初始化.初始化样本集，从先验分布P(x0)采样得到N个粒子,i=1,···,N ，每个粒子初始权值为(2) 预测.根据运动模型，由上一时刻粒子集和机器人移动数据，计算获得机器人定位粒子集的先验分布. 由于从机器人内部传感器获得的移动数据存在误差，需要根据实际移动平台设置运动模型的噪声参数.(3) 观测.粒子集中每个粒子的观测似然率是观测阶段将获得的先验分布与传感器获得的测距数据结合，再利用观测模型计算出来的. 然后根据每个粒子的观测似然率对每个粒子进行加权，得到后验分布. 观测模型的基本思想是将观测数据转换到每个粒子的位姿上，通过将观测数据与已知地图进行对比，计算得此粒子的观测似然率. (4) 是否需要重采样.若有效粒子数Neff小于粒子数的一半N/2，则进行重采样，否则进入下一步. (5) 重采样.根据粒子的权值对粒子集进行采样，产生新的粒子集. 通过除去权值较低的粒子，保留权值高的粒子[13]，让粒子分布逐渐收敛接近最优值.基于粒子滤波的SLAM算法使用粒子集表示定位的概率分布，粒子数量决定算法的抗干扰能力、鲁棒性和定位准确度等. 在越复杂、宽广的环境下进行SLAM定位，对粒子数量的要求越高. 粒子数量直接影响算法计算量及速度. 目前对基于粒子滤波的SLAM算法的改进大多数都会增加算法复杂度，即在提高定位精度同时会降低算法执行速度. 而移动机器人的定位结果具有时效性，所以SLAM的计算速度又将影响定位准确度.本文提出一种结合粒子滤波和SLAM算法的，GPU并行计算优化的方式进行加速，从而减少计算带来的定位延迟.2.1 并行优化分析基于粒子滤波的SLAM算法主要有预测、观测和重采样3个阶段. 其中预测阶段使用运动模型对移动数据加入随机噪声，将上一时刻粒子集加入移动数据生成此时刻的粒子集先验分布. 此过程中计算较为简单，每个粒子只执行一次对移动数据加操作.重采样的实现过程主要是根据粒子的权值，决定粒子的去留. 其中最容易的实现方法是把权值低的粒子去除，把权值高的粒子复制[14]. 而其他一些较复杂方法需要在所有粒子中进行衡量，各个粒子计算的关联程度大，不适于并行计算.算法的观测过程，主要利用观测模型计算每个粒子的观测似然率，而在粒子观测似然率的计算中，是以从测距传感器获得的测量数据和粒子作为输入，与已知地图进行比较计算得到观测似然率，再根据观测似然率对每个粒子进行加权. 在计算过程中每个粒子分开计算，具有较高的计算独立性.在实际环境使用中计算粒子的观测似然率是一个复杂的计算过程，基于激光雷达和栅格地图的观测模型将可能面临4种噪声数据：测量固有误差phit、预料外障碍物造成的误差pshort、测量失效误差pmax、随机误差prand. 观测模型的主要任务是在存在多种噪声数据的情况下，使用先验分布和观测值估计出后验分布. 其中观测模型可用式(2)表示：式(2)中，为k时刻第i个粒子获得的观测值. zhit、zshort、zmax、zrand为观测模型固有参数，与实际使用的测距传感器的特性等有关.测量固有噪声在有效范围内呈正态分布，可用式(3)表示：式(3)中，σhit是硬件固有噪声参数，为k时刻第i个粒子在已有地图上获得的理想的正确观测数据.式(4)中，λshort为观测模型的固有参数，需要根据实际环境设定.如式(5)，zmax为扫描的有效范围，在观测距离不超过有效范围时，将不存在测量失效误差.式(6)表示在有效测量范围内存在的随机误差.计算粒子观测似然率过程中，将从测距传感器获得的一系列测量点，并根据粒子表示位姿，分别转换为已知全局地图上的点，再分别对这些点周围n×n区域进行一次遍历，根据选择方法不同对此n×n区域进行相应计算，其中n为观测模型的有效计算范围.如使用式(2)的计算方法，将在全局地图中遍历测量障碍物点周围n×n区域，并寻找地图上一个与此测量障碍物点最近的已知障碍物点作为式(3)中的，对所有的测量障碍物点重复此计算. 一般使用360度激光雷达可一次获取约300个扫描点. 由于这些扫描点间有一定的相关性，在计算过程中可以在扫描数据中间隔取点作为障碍物点进行上述处理，减少冗余计算，提高计算速度.在此假设每次可从传感器获得M个扫描点，间隔S个点取一个扫描点作为计算观测似然率的过程中的测量障碍物点，则每次计算观测似然率时需要进行M/S次坐标转换，并对n×n×M/S的栅格点进行遍历和计算. 在实际应用中可能使用成千上万个粒子，如果按传统的流水线方式执行，则将在更新过程的计算耗费大量时间. 2.2 并行优化设计计算粒子观测似然率是算法中计算量最多的一个过程，而且粒子间的计算较为独立，适合进行GPU并行加速优化.由上述分析，本文设计将计算粒子观测似然率部分移入GPU中进行大规模并行计算，加速前判断当前粒子数是否大于阈值(θ=868，加速比为1时粒子数)，若大于则进行GPU并行加速，否则执行串行更新过程. 并行优化后的算法执行框架如图2所示.将算法中更新阶段的计算粒子观测似然率部分改用GPU并行执行. 由于CPU与GPU不使用同一寻址空间，所以使用GPU的代价是增加了两个CPU内存和GPU 内存间的数据复制过程. 其中CPU-GPU数据复制包括已知地图、粒子集数据和观测数据. GPUCPU的数据复制，只需要将每个粒子对应的观测似然率从GPU复制到CPU即可. 最后再根据粒子的观测似然率对粒子进行加权和权值规范化，即完成观测阶段.2.3 并行优化实现根据上节分析，使用CUDA进行GPU并行计算的开发，实现算法的并行优化，并对基于粒子滤波的SLAM的GPU并行优化验证.使用原算法的预测和重采样，将更新阶段利用CUDA进行重新编程，使用GPU并行计算粒子的观测似然率. 在实现对算法的GPU并行优化后，与原算的执行时间进行对比，分析优化结果.分别使用100、1 000、5 000、15 000、40 000、50 000、100 000个粒子进行实验，对比其加速部分的平均执行时间和总体的平均执行时间. 实验数据如图3–图5所示.图3中对比了并行优化前后加速部分平均执行时间，即算法的更新部分平均执行时间. 并行更新执行时间包括以下操作：将CPU内存中的粒子集数据、扫描数据和一些相关计算参数复制到GPU内存；粒子观测似然率由GPU计算；再将GPU 计算出的观测似然率数据复制到CPU内存空间[15-16]；根据粒子集观测似然率对粒子进行加权等.由图3可见，在粒子数低于1 000时，使用GPU并行执行时间比使用CPU流水线执行更新操作相近或要更长，这是由于CPU主频高于GPU约3倍，处理相同任务的速度远远快于单个GPU. 其次使用GPU进行并行计算，需要将粒子集数据、扫描数据和一些相关计算参数复制到GPU内存中，这需要耗费一定的时间. 所以在使用粒子数量较少的时候，不能发挥GPU加速的优势，在粒子集中粒子数量达到15 000时，使用GPU并行计算的算法加速效果明显，算法更新阶段的执行速度达到原来的3.5倍. 而在粒子集中粒子数量到达40 000后，GPU对算法的并行加速稳定在4.5倍左右，效果明显，大大降低算法中更新阶段的计算时间.图4与图3的结果相近. 图4比较了使用GPU并行优化前后算法的平均执行时间. 在粒子数较多时，GPU并行优化的加速效果明显.如图5所示，优化前算法中更新阶段平均执行时间均占算法平均执行时间的75%，说明本文设计过程中对并行优化部分的分析无误. 更新阶段是在算法过程中计算量最大的部分，在此部分进行并行加速是合理的设计.图6为GPU并行加速前后机器人绕室内一圈的运动轨迹图(如图中蓝色所示). 图6(a)中由于机器人运动速度过快，而CPU处理速度较慢，从而导致观测数据之间关联较小，与真实运动轨迹偏差较大. 图6(b)运用GPU并行加速，提高算法更新阶段的执行效率，解决了轨迹偏差较大的问题. 实验证明，GPU并行加速不仅提高了算法执行效率，而且明显减小了由计算时间过长导致的定位误差.基于粒子滤波的SLAM算法由于其实用性和创新性，已成为移动机器人领域中的热门研究. 但算法计算量大，与机器人CPU计算资源紧张是一对明显的矛盾. SLAM定位结果具有时效性，算法计算时间过长将增加定位误差. 本文针对此问题提出一种基于粒子滤波的SLAM算法的GPU并行优化方法，通过实验验证，算法的GPU并行优化效果明显，可减少由算法计算时间带来的定位误差，为移动机器人的SLAM算法提供一种全新的优化思路.[ 1 ]DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping (SLAM): Part I The essential algorithms [J]. Robotics and Automation Magazine, 2006, 13(2): 99-110.[ 2 ]SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Autonomous Robot Vehicles. 1990, 4: 167-193. [ 3 ]SHOWDONG H, GAMINI D. Convergence and consistency analysis for extended Kalman filter based SLAM[J]. IEEE Transactions on Robotics, 2007, 23(5): 1036-1049.[ 4 ]宋宇, 李庆玲, 康轶非, 等. 平方根容积Rao-Blackwillised粒子滤波SLAM算法[J]. 自动化学报, 2014, 40(2): 357-367. SONG Y, LI Q L, KANG Y F, et al. SLAM with squareroot cubature rao-blackwillisedparticle filter[J]. Acta Automatica Sinica, 2014, 40(2): 367-367.[ 5 ]王丁. 基于改进粒子滤波的检测前跟踪算法[D]. 成都: 电子科技大学电子工程学院, 2012.[ 6 ]朱磊, 樊继壮, 赵杰, 等. 未知环境下的移动机器人SLAM方法[J]. 华中科技大学学报（自然科学版）, 2011, 39(7): 9-13. ZHU L, FAN J Z, ZHAO J. SLAM method for mobile robot in unknown environment[J]. Huazhong University of Technology (Natural Science Edition), 2011, 39(7): 9-13.[ 7 ]DIETER FOX. Adapting the sample size in particle filters through KLD-sampling[J]. TheInternational Journal of Robotics Research, 2003, 22: 985-1003.[ 8 ]卢风顺, 宋君强, 银福康, 等. CPU/GPU协同并行计算研究综述[J]. 计算机科学, 2011, 28(3): 5-9. LU F S, SONH J Q, YIN F K, et al. Survey of CPU/GPU synergetic parallel computing[J]. Computer Science, 2011, 28(3): 5-9.[ 9 ]徐李超, 张祺. 一种仿人机器人步态优化的新方法研究[J]. 广东工业大学学报, 2012, 29(1): 50-54. XU L C, ZHANG Q. Gait Optimizing of humanoid robots using a new method[J]. Journal of Guangdong University of Technology, 2012, 29(1): 50-54.【相关文献】[10]王法胜, 鲁明羽, 赵清杰, 等. 粒子滤波算法[J]. 计算机学报, 2014, 37(8): 1679-1694. WANG F S, LU M Y, ZHAO Q J, et al. Particle filtering algorithm[J]. Chinese Journal of Computer, 2014, 37(8): 1679-1694.[11]于金霞, 汤永利, 许景. 一种改进的自适应优化粒子滤波算法研究[J]. 小型微型计算机系统, 2013, 34(6): 1446-1450. YU J X, TANG Y L, XU J. Research on an improved particle filter algorithm based on adaptive optimization mechanism[J]. Journal of Chinese Computer Systems, 2013, 34(6): 1446-1450.[12]左军毅, 张怡哲, 梁彦. 自适应不完全重采样粒子滤波器[J]. 自动化学报, 2012, 38(4): 647-652. ZUO J D, ZHANG Y Z, LIANG Y, Particle filter based on adaptive part resampling[J]. Acta Automatica Sinica, 2012, 38(4): 647-652.[13]冯驰, 王萌. 粒子滤波器重采样算法的分析与比较[J]. 系统仿真学报, 2009, 21(4): 1101-1110. FENG C, WANG M. Analysis and comparison of resampling algorithms in particle filter[J]. Journal of Systems Simulation, 2009, 21(4): 1101-1110.[14]刘洞波. 移动机器人粒子滤波定位与地图创建方法研究[D]. 长沙: 湖南大学电气与信息工程学院, 2013.[15]郭睿冉, 宋建新. 图像压缩感知的基于GPU的并行重构算法研究[J]. 电视技术, 2014, 38(11): 15-19. GUO R R, SONG J X. Research and implementation of parallel signal reconstruction about image compressed sensing based on GPU[J]. Digital Video, 2014, 38(11): 15-19. [16]刘燕龙, 原玲, 姜文超, 基于Calculix的船舶疲劳强度并行计算方法研究与应用[J]. 广东工业大学学报, 2015, 32(4): 77-82. LIU Y L, YUAN L, JIANG W C. Parallel mechanism research and application of calculix in ship fatigue analysis environment[J]. Journal of Guangdong University of Technology, 2015, 32(4): 77-82.。
无线传感器网络环境下基于粒子滤波的移动机器人SLAM算法
无线传感器网络环境下基于粒子滤波的移动机器人SLAM算法海丹;李勇;张辉;李迅【期刊名称】《智能系统学报》【年(卷),期】2010(5)5【摘要】定位问题是移动机器人研究领域中最基本的问题,在Bayes的框架下研究了机器人与无线传感器网络(WSN)组成系统中的同时建图与定位问题(SLAM).针对该系统中只存在距离测量信息可用的情况提出了一种基于粒子滤波的SLAM算法.该方法将机器人状态和节点位置估计设置为一组全局估计粒子,通过对粒子及其权重的更新来计算整个系统的状态.算法将WSN节点的位置估计在机器人的路径上分解为相互独立的估计,从而将全局粒子的计算转化为使用一个机器人状态滤波器和对应于每个机器人粒子的节点位置滤波器进行计算.针对观测信息低维的特点,设计了处理低维观测信息的方法,使得观测信息可以在滤波阶段得到合理利用.并且详细介绍了提出的SLAM算法原理和计算过程,并通过仿真实验证明了算法的有效性和实用性.【总页数】7页(P425-431)【作者】海丹;李勇;张辉;李迅【作者单位】国防科技大学,机电工程与自动化学院,湖南,长沙,410073;国防科技大学,机电工程与自动化学院,湖南,长沙,410073;国防科技大学,机电工程与自动化学院,湖南,长沙,410073;国防科技大学,机电工程与自动化学院,湖南,长沙,410073【正文语种】中文【中图分类】TP24【相关文献】1.基于粒子滤波的移动机器人SLAM改进算法 [J], 郭利进;王化祥;孟庆浩;邱亚男2.基于固定滞后Gibbs采样粒子滤波的移动机器人SLAM [J], 张恒;刘艳丽;樊晓平;瞿志华3.基于粒子滤波的移动机器人SLAM算法 [J], 涂刚毅;金世俊;祝雪芬;宋爱国4.基于区间分析无迹粒子滤波的移动机器人SLAM方法 [J], 刘洞波;刘国荣;王迎旭;肖鹏5.基于Rao-Blackwellized粒子滤波器移动机器人SLAM研究 [J], 黄辉;邹安安;胡鹏;邹媛媛;蔡庆荣因版权原因，仅展示原文概要，查看原文内容请购买。
基于稀疏扩展信息滤波和粒子滤波的SLAM算法
摘
要 ： 对传统粒子滤 波算 法单 次迭代过 程 中仅应 用到 ３前的信 息 ， 小权值粒 子代表 的信 息在 重采样 中被 针 － ＂ 且
ｂｃｕｅｏ ｓ ａ ｅ ｈ ｐｒ ｃ ｍ ｖｄｉ ｓｍ ｌ ａｄｓ ｇ ｅａ ｏ ． ｉ ｕｔ ｅｕ ｏａｉ ｔ ｎＡ ｄＭ ｐ ｉｇ（Ｌ Ｍ） ｅａｓ ｆ ｍ ｌｗ ｉ ｔ ａｔｌ ｒ ｏｅ ｒ ａ ｐｅ ｎ ｉ ｌ ｉ ｒｔ ｎ Ａ Ｓｍ ｌ ｎｏ ｓ ｃｌａｉ ｎ ａ ｐ ｌ ｇ ｉｅｅ ｎｅ ｎ ｅｔ ｉ ａ Ｌ ｚ ｏ ｎ ＳＡ
ａｇ ｒ ｈ ｂ ｓ ｄ ｎ ｐ ｒ ｅ ｘ ｅ ｄ ｄ ｎ ｏ ａ ｉｎ ｉ ｔｒ ｏ ｉ ｇ ｐ ｒ ｃｅ ｉ ｔｒ ｗａ ｂ ｏ ｇ ｔ ｏ ｗ ｒ ． Ｉ ｏ ａｉｎ ｌｏ ｉ ｍ ａ ｅ ｏ ｓ ａ ｓ ｅ ｔｎ ｅ ｉｆ ｒ ｔ ｆ ｅ ｃ ｍｂ ｎ ａ ｔ ｌ ｆ ｅ ｓ ｒ ｕ ｈ ｆ ｒ ａ ｄ ｔ ｍ ｏ ｌ ｉ ｌ ｆ ｍ ｎ ｒ ｔ ｍａｒ ｏ ｔｘ ｉ
ｍａ ｎａ ｎ ｄ ｉｔｒｃ ｌ ｎｏ ｍａ ｉｎ， ｐ ｒｉｌｓ ｏ ｔ ｉｉ ｇ ｉ ｔｉ ｅ ｈｓｏ ａ ｉ ｆｒ ｔｏ ｉ ａ ｔｃｅ ｃ ｎ ａｎ ｎ ｈｉｔｒｃ ｌ ｎｏ ｍａｉ ｎ ｂａ ｎ ｄ ｙ ｓｏ ａ ｉ ｆｒ ｔｏ ｏ ｔｉ ｅ ｂ Ｇｉｂｓ ａ ｌｎｇ ｐａ ｔ ｌ ｓ ｅ ｉ ｂ ｓ ｍｐ ｉ ， ｒｉ ｅ ｓ ｔ ｃ
Ｃａｌ ｉ ｕ ａｉｎ ｒ ｃ ｒｉｄ ｕｔｔ ｈ ｗ ｈ ｐ ｓｔｏ ｎｇ ａ ｃ ｒ ｃ ｏ ｈｉ ｍｅｈ ｄ ｓ ｂ ｕ ０％ ｒｏ ｓｍ ｌｔｏ ｓ ｗｅ ｅ ａ ｒｅ ｏ ｏ ｓ ｏ ｔ ｅ ｏ ｉｉｎｉ ｃ ｕ ａ ｙ ｆｔ ｓ ｔｏ ｉ ａ ｏ ｔ８ ｌ ｏ ｈ Ｓ ａ ｇ ｒｔ ｍ’． ｉ ｍｏ ｅ ｈａ ｓＳ ｒ ｔ ｎ Ｆａ ｔ ＬＡＭ ２．０
dynaslam原理
DynaSLAM原理详解一、简介DynaSLAM是一种基于粒子滤波的实时动态视觉同时定位与地图构建（Simultaneous Localization and Mapping，简称SLAM）系统。
DynaSLAM算法通过在粒子滤波框架下融合IMU（惯性测量单元）信息和视觉信息，实现了实时、高精度的定位和地图构建。
本文将对DynaSLAM的原理进行详细阐述。
二、基本原理2.1 粒子滤波粒子滤波是一种基于蒙特卡洛方法的非线性滤波技术，用于处理非线性非高斯分布问题。
粒子滤波通过构建一组粒子来表示系统的状态分布，然后根据观测数据对粒子进行权重更新，最后通过重采样操作得到新的粒子集合。
粒子滤波具有全局最优性和并行计算优势，适用于非线性、非高斯系统的估计问题。
2.2 IMU信息融合IMU是一种基于惯性加速度计和陀螺仪的测量设备，可以提供物体的加速度和角速度信息。
IMU信息对于视觉SLAM系统的定位和建图具有重要意义，可以提高系统的精度和鲁棒性。
然而，由于IMU的测量噪声较大，直接使用IMU信息会导致系统的定位误差累积。
因此，需要将IMU信息与视觉信息进行融合，以提高系统的性能。
2.3 视觉信息融合视觉SLAM系统通常采用特征点匹配方法进行建图和定位。
视觉信息融合的目的是将IMU信息与视觉特征点进行关联，以减小定位误差。
常用的视觉信息融合方法有扩展卡尔曼滤波（Extended Kalman Filter，EKF）和粒子滤波。
DynaSLAM 采用了粒子滤波框架下的视觉信息融合方法。
三、DynaSLAM算法流程3.1 状态定义与初始化DynaSLAM的状态定义包括相机位姿、IMU姿态、地图点位置和特征点观测值。
初始状态下，相机位姿和IMU姿态通过预积分方法得到，地图点位置和特征点观测值通过初始扫描得到。
3.2 IMU预积分IMU预积分是根据IMU的测量值（加速度和角速度）预测相机位姿的过程。
预积分方法可以采用基于欧拉方程的方法或基于贝叶斯优化的方法。
基于粒子群优化的Rao-Blackwellized粒子滤波SLAM算法
收稿日期：2014-03-05；修回日期：2014-04-14。
作者简介：姚二亮（1991-），男，安徽凤阳人，硕士研究生，主要研究方向：移动机器人SLAM 技术； 张国良（1970-），男，四川金堂人，教授，博士，主要研究方向：先进控制、智能机器人； 汤文俊（1986-），男，安徽黄山人，博士研究生，主要研究内容：多机器人SLAM ； 徐君（1986-），男，重庆人，博士研究生，主要研究方向：多机器人系统。
文章编号：1001-9081（2014）S2-0037-04基于粒子群优化的Rao-Blackwellized 粒子滤波SLAM 算法姚二亮*，张国良，汤文俊，徐 君（第二炮兵工程大学三系，西安710025）(*通信作者电子邮箱familyyao915@)摘 要：为了实现在高相似度环境中移动机器人精确高效的自定位与建图，提出了一种基于粒子群优化（PSO ）的Rao-Blackwellized 粒子滤波同步定位与地图构建（SLAM ）算法。
利用激光扫描数据校正里程计信息，得到多模态的似然函数，克服相似环境对机器人定位的影响；利用粒子群优化算法提高常规粒子滤波器的估计性能，使得高似然采样集向各个后验概率密度分布取值极大的区域运动，同时保持低似然粒子多样性，从而在一定程度上克服粒子贫乏问题，并且显著地降低精确定位所需的粒子数。
对所提算法与Gmapping 算法在MIT 数据集上进行仿真对比实验，结果表明了该算法的可行性和有效性。
关键词：移动机器人；同步定位与地图构建；粒子滤波器；建议分布；粒子群优化中图分类号：TP242 文献标志码：ARao-Blackwellized particle filter simultaneous localization and mappingalgorithm based on particle swarm optimizationYAO Erliang *,ZHANG Guoliang,TANG Wenjun,XU Jun(Department No.3,The Second Artillery Engineering University,Xi'an Shaanxi 710025,China )Abstract:In order to realize a precious and efficient Simultaneous Localization and Mapping (SLAM)of a mobile robot in the environments with high similarities,a Rao-Blackwellized particle filter SLAM algorithm based on Particle Swarm Optimization (PSO)was proposed.By correcting odometer information with the laser scanning data and acquiring a multimode likehood function,the influences of the similar environment to the robot localization were overcome.The performance of the generic particle filter was improved by PSO.Particles in high likelihood sampling set moved to the region with posterior probability distribution maximum value,meanwhile the algorithm maintained the multiplicity of the low likehood particles.The improverishment of the particle filter was overcome partly and the number of required particles was reduced observably.The simulation experiments were performed for the comparison of the proposed algorithm with Gmapping algorithm on public MIT datasets,and the feasibility and effectiveness of the algorithm are validated.Key words:mobile robot;Simultaneous Localization and Mapping (SLAM );particle filter;proposal distribution;Particle Swarm Optimization (PSO)0 引言同时定位与地图构建（Simultaneous Localization andMapping ，SLAM ）是指移动机器人在未知环境中运动时逐步构建环境地图，同时利用该地图计算机器人位姿的过程［1］。
基于粒子滤波的单目视觉SLAM算法_陈伟
第37卷第3期 2008年5月机器人 R O B O TV o l .30,N o .3 M a y ,2008文章编号:1002-0446(2008)03-0242-06基于粒子滤波的单目视觉S L A M 算法陈伟,吴涛,李政,贺汉根(国防科学技术大学机电工程与自动化学院,湖南长沙 410073)摘 要:针对携带有单目摄像机和码盘的微小机器人的定位与建图问题,提出了基于粒子滤波的S L A M (同时定位与建图)算法.从摄像机中提取图像特征点,并在图像序列中加以匹配,根据相应时刻的摄像机位姿计算得到对应的环境标志点坐标;机器人的大致位姿估计由码盘运动模型获得.在机器人移动过程中,环境标志点的观测信息和码盘信息通过粒子滤波相融合,从而提高了机器人定位的精度,同时也得到了更为准确的环境标志点坐标.仿真实验结果表明本算法有效、可靠.关键词:S L A M ;微小机器人;码盘;单目视觉中图分类号: T P 24 文献标识码: BAMo n o c u l a r V i s i o n S L A M A l g o r i t h m B a s e do n P a r t i c l e F i l t e rC H E NW e i ,W UT a o ,L I Z h e n g ,H EH a n -g e n(C o l l e g e o f M e c h a t r o n i c s a n dA u t o m a t i o n ,N a t i o n a l U n i v e r s i t y o f D e f e n s e T e c h n o l o g y ,C h a n g s h a 410073,C h i n a ) A b s t r a c t :T o d e a l w i t ht h e l o c a l i z a t i o n a n dm a p p i n g p r o b l e mo f m i n i a t u r e r o b o t e q u i p p e dw i t h m o n o c u l a r c a m e r a a n d e n -c ode r s ,a nS L A M(s i m u l t a n e o u s l o c a l i z a t i o n a n d m a p p i n g )a p p r o a c h b a s e d o n p a r t i c l ef i l t e r i ng i s p r e s e n t e d .Th ei m a g e f e a -t u r e s a r e d e t e c t e df r o m c a m e r a a n d m a t c h e d a m o n g s u c c e s s i v e f r a m e s ,a n d t h e n c o o r d i n a t e s o f t h e c o r r e s p o n d i n g f e a t u r e s i n e n v i r o n m e n t s a r e c o m p u t e d a c c o r d i n g t o t h e c a m e r a p o s e s .T h e r o u g h e s t i m a t e s o f r o b o t p o s e s a r e o b t a i n e d f r o mt h e e n c o d e r m o t i o nm o d e l .D u r i n g r o b o t m o t i o n ,t h e i n f o r m a t i o nf r o mf e a t u r e o b s e r v a t i o n s i s f u s e d w i t h t h a t f r o mt h e e n c o d e r s b y p a r t i c l e f i l t e r ,s o l o c a t i o n p r e c i s i o n o f t h e r o b o t i s i m p r o v e d a n d a l s o m o r e e x a c t c o o r d i n a t e s o f t h e f e a t u r e s a r e g o t t e n .T h e r e l i a b i l i t y a n d e f f i c i e n c y o f t h e m e t h o da r e p r o v e d b ys i m u l a t i o ne x p e r i m e n t s . K e y w o r d s :s i m u l t a n e o u s l o c a l i z a t i o na n dm a p p i n g(S L A M );m i n i a t u r e r o b o t ;e n c o d e r ;m o n o c u l a r v i s i o n 收稿日期:2007-08-131 引言(I n t r o d u c t i o n )机器人的定位与建图是自主移动机器人的热点研究领域.以往是将定位和建图作为两个独立的领域分别进行研究———或者在精确定位的前提下进行环境建模,或者在已有环境地图的条件下实现定位.但是随着机器人技术的发展,其应用的范围越来越广,从室内拓展到了室外,甚至某些未知环境.此时,机器人既不能提前获取环境地图,也无法借助外界手段提供精确的定位.因而在未知环境中,机器人如何创建地图并同时进行自主定位和导航成为当今机器人研究领域的热点问题之一———移动机器人的同时定位与建图S L A M[1](s i m u l t a n e o u s l o c a l i z a t i o na n dm a p -p i n g ).S L A M 也称为C M L [2](c o n c u r r e n t m a p p i n g a n d l o c a l i z a t i o n ),最先是由S m i t h 和C h e e s e m a n 在相关的论文中提出的[3,4],它具有重要的理论价值与应用前景,被认为是实现移动机器人完全自主化的关键技术之一[5,6].S L A M 方法可以简单地描述为:在未知环境中,机器人从某一位置开始移动,根据控制信息和传感器观测数据同时进行位置估计与地图构建.定位与地图构建融为一体,而不再是独立的两个阶段.视觉传感器包含了丰富的环境信息,可以用于目标识别跟踪、环境地图构建、障碍检测等.对于微小型机器人而言,装备体积较大而又昂贵的传感器是不现实的,因而利用尽可能少而且廉价的传感器获得尽可能多的信息是对微小机器人的一个基本要求.机器人装备的传感器要尽可能兼顾多种任务,而摄像机无疑是一种理想的选择.摄像机以其获取信息丰富、价格低廉的特点,在机器人定位与建图中引起了越来越多的关注.有很多学者提出了不同的定位方法,这些定第30卷第3期陈伟等: 基于粒子滤波的单目视觉S L A M算法243 位方法大体可分为以下三类:第一类是基于立体视觉的方法[7],这类方法的突出优点是能获取周围环境的深度信息,从而能够实现较为准确的定位,但存在需要对摄像机进行标定等问题;第二类是基于全方位视觉传感器的定位方法[8],它无需转动就能获得360°的环境信息,其缺点是感知到的环境信息会产生很大的畸变;第三类是基于单目视觉的机器人定位算法[9],这类方法具有简单易用、适用范围广等特点,它还可以与里程仪等传感器相结合实现运动立体视觉定位,实现对环境特征的三维测量,完成环境建图,因而单目视觉使用较为灵活,也不会像全方位视觉传感器那样产生很大的畸变.鉴于微小机器人的客观条件所限,本文阐述了以单目视觉与码盘里程仪相结合的S L A M算法.单纯依靠码盘,通过对车轮转动的记录来粗略地确定位置和姿态,可以实现简单的定位.但是由于车轮与地面存在滑移现象,定位误差会随着路程的增加而增大,如果误差长时间得不到修正,最终必将导致定位失败.视觉图像包含了丰富的信息,利用它进行误差修正是一种合理的选择.此方法既达到了提高定位精度的目的,又不会增加移动机器人的硬件负担,这对于微小型机器人是非常重要的.另外,对于未知环境中的建图问题,机器人无法通过某一时刻的单目图像获取环境深度信息,但依靠不同时刻两帧图像及码盘提供的机器人位姿变化信息,模仿立体视觉就可以实现对环境特征的三维测量,我们称之为运动立体视觉,这就是本文建图方法的基本思想.本文所提出的方法在摄像机图像序列中提取特征点,并进行特征匹配和三维计算.将初始状态的车体坐标系作为世界坐标系,最初两个时刻利用码盘实现较为准确的定位,由摄像机得到从机器人到标志点的向量,结合机器人两个位置坐标以直线求交点的方式获得标志点的世界坐标.机器人在移动中通过码盘不断计算出新的位姿信息,摄像机也不断地获取旧标志点的新观测信息,也可能得到新的标志点.利用这些标志点观测信息的变化,不断修正机器人的位姿,并反过来对环境标志点坐标加以修正.这个过程中会有旧的标志点消失、新的标志点被获取,这是一个不断更新的过程.2 算法使用模型的描述(D e s c r i p t i o no f m o d-e l s u s e di nt h e a l g o r i t h m)本文所用数据来自码盘和单目摄像机.首先,码盘差速定位模型对机器人各个时刻的位置坐标进行初步估计,然后,根据摄像机观测到的不同时刻标志点的位置变化,对机器人控制中的位置估计结果进行进一步修正,从而提高定位和环境模型的精度.2.1 码盘差速定位模型如果轮式机器人所运动的环境可被假定为二维平面,那么它的位姿可以用两侧车轮的轮速推算得到.车在n时刻的位置可用x R n=βnx ny n表示,βn为机器人的方向角,x n、y n为两码盘轴线中点的坐标.非全轮转向的轮式机器人的运动是绕着一个中心做圆周运动(当两侧车轮速度相同时,圆的中心在无穷远处,即机器人做直线运动),设两轮轮距为L,码盘的线数为P(轮子转一圈码盘输出的脉冲数),轮径为D.通过左右码盘的脉冲频率f L和f R可以算得轮子的线速度为:s R=f RPπD+γRs L=f LPπD+γL(1) 其中,由于地面轮胎变形以及打滑等原因,两轮的实际速度与通过码盘计算得到的数据存在误差.这个误差显然是随机的,和路面情况、行驶速度、驱动力大小等都有关系.所以,这里必须存在一个噪声干扰γ,统计显示这个噪声大致服从一个非零均值的高斯分布,γR～N(μR,σR2)、γL～N(μL,σL2)相互独立.机器人的角速度为:ω=s R-s LL(2)运动学方程为:βn+1=βn+s R n-s L nLΔtx n+1=x n+L(s R n+s L n)2(s R n-s L n)(s i nβn+1-s i nβn) y n+1=y n-L(s R n+s L n)2(s R n-s L n)(c o sβn+1-c o sβn)(3)2.2 摄像机成像模型摄像机模型延用针孔模型,忽略摄像机畸变,则有如下关系式:=R t0T31 M4)244 机 器 人2008年5月即为由世界坐标系到摄像机坐标系的变换方程.uv1=1z cf u-f u c o tθu00f v/s i nθv0001 Kf000f0001x cy cz c(5)式(5)是摄像机坐标系中特征点坐标到像素坐标系的投影方程.其中(u0,v0)为O在像素坐标系(u,v)中的坐标,像素在轴上的尺寸分别为Δx、Δy、f u=1Δx、f v=1Δy.K即为摄像机的内参数矩阵,M为外参数矩阵.内参数矩阵K由摄像机标定得到,外参数矩阵M按照坐标系之间的相互转换关系得到.图1 摄像机坐标系、车体坐标系和世界坐标系关系图F i g.1 T h e r e l a t i o n s h i pa m o n g c a m e r a,v e h i c l ea n d w o r l d r e f e r e n c ef r a m e s摄像机的安装如图1所示,摄像机轴线Z c与车体坐标系的X r轴在同一平面内,两轴有夹角χ,也就是相当于摄像机坐标系是在车辆坐标系基础上向上平移了h、并且绕Y r轴顺时针旋转了角χ(夹角为正值).我们假设机器人初始时刻的车体坐标系为世界坐标系,这样我们就可以得到外参数矩阵M.R=s i nβ-c o sβ0-s i nχc o sβ-s i nχs i nβ-c o sχc o sχc o sβc o sχs i nβ-s i nχ,t=-Rx ny nh2.3 环境标志点的计算在计算标志点之前必须先确定世界坐标系,我们将机器人初始时刻的车体坐标系定义为世界坐标系,如果在不同时刻的图像上分别得到同一个标志点的对应特征,并且成功匹配,那么结合机器人在相应时刻的位姿信息就能够计算出标志点坐标.首先,当机器人在n成像点的像素坐标为u n i=u n iv n i,这是一个经过投影变换和离散化得到的坐标,可以认为真实的投影点是在这个像素附近范围内的一个均匀分布,即x i u n=x u n iy v n i=Δx 00 Δyu i nv i n+ΔuΔv(6)其中Δu～u-Δx2,Δx2,Δu～u-Δy2,Δy2.通过式(5)得到的像素坐标可以转化为摄像机坐标系中从摄像机中心到标志点的归一化向量,归一化后为Vnc i=p n c iq n c ir n c i,再由式(4)变换到世界坐标系,其向量为V n w i=p n w iq n w ir n w i,如果在下一时刻n+1还能观测到该标志点,同样可以得到对应的向量V w i n+1=p n+1w iq n+1w ir n+1w i.这两个时刻摄像机的相应位置分别为x ny nh、x n+1y n+1h,因此,在世界坐标系中就得到两条直线方程,即q n w i(x i-x n)=p n w i(y i-y n)r n w i(x i-x n)=p n w i(z i-h)q n+1w i(x i-x n+1)=p n+1w i(y i-y n+1)r n+1w i(x i-x n+1)=p n+1w i(z i-h)这两条直线的交点即为标志点坐标.但是在三维坐标系中,两直线不一定相交,所以这里我们用最小二乘解得到标志点的坐标估计.3 粒子滤波的S L A M算法按上述方法计算得到环境标志点的坐标估计,此标志点在机器人的后继运动过程中一般可以保持在一段时间内被观测到,当标志点离开观测范围或者没法准确匹配时就退出计算,与此同时机器人又可以不断获得新的标志点加入计算.这样就可以利用这些观测信息不断地修正机器人的位置估计和环境标志点坐标,这就是本文同时定位与建图的核心思想.鉴于实际系统误差的非高斯性,具体S L A M算法采用粒子滤波方法来实现.第30卷第3期陈伟等: 基于粒子滤波的单目视觉S L A M 算法245图2 环境特征计算示意图F i g .2 S k e t c ho f f e a t u r e c o o r d i n a t e s c a l c u l a t i o n粒子滤波方法通过非参数化的蒙特卡洛模拟方法来实现贝叶斯滤波,用样本形式———而不是函数形式———对先验信息和后验信息进行描述.序贯重要性采样(s e q u e n t i a l i m p o r t a n c e s a m p l i n g ,S I S )粒子滤波算法是一种按照蒙特卡洛仿真实现贝叶斯滤波的技术,其关键思想是根据一组带有相应权值的随机样本来表示后验概率密度函数,根据这些样本和权值来计算估计值[10].设概率密度函数p (x )∝π(x ),而π(x )也是一个概率密度函数,难以对其进行采样,但能够对其进行计算.假设有一个容易采样的概率密度函数q (·),根据q (·)进行采样而得到的样本x ⌒(i )～q (x ), i =1,2,…,N(7)称为采样粒子(s a m p l i n g p a r t i c l e ),而q (·)称为重要性密度函数(i m p o r t a n c e d e n s i t y f u n c t i o n ).那么概率密度函数p (·)的加权近似就可以表示为:p (x )≈∑Ni =1λ(i )δ(x -x ⌒(i ))(8)其中,λ(i )∝π(x ⌒(i ))q (x ⌒(i ))(i =1,2,…,N),是第i 个粒子的正则权值,满足∑ni =1λ(i )=1.设{x ⌒(i )0:n }Ni =1=d e f{x (i )0,x (i )1,…,x (i )n },i =1,2,…,N {λ(i )n }Ni =1=d e f{λ(1)n ,λ(2)n ,…,λ(N )n},表示n 时刻对所有状态采样且容量为N 的样本,以及相应的权值.如果样本{x ⌒(i )0:n }Ni =1是由重要性密度函数q (X n Z n )抽取得到的,则其权值按下式确定:λ(i )n∝p (x ⌒(i )0:n Z n )q (x ⌒(i )0:n Z n )(9) 在时刻n 假定已经有了用样本对p (X n -1 Z n -1)的近似重构,而获取新的观测量z n 之后,需要用一组新的样本对p (X n Z n )进行近似重构.如果这个重要性函数为一阶马尔可夫过程,且可表示为递推形式q (X n Z n )=q (x n X n -1,Z n )q (X n -1 Z n -1),那么就能利用已有的样本x ⌒(i )0:n -1～q (X n -1 Z n -1)以及新的状态采样x ⌒(i )n ～q (x n X n -1,Z n )得到样本x ⌒(i )0:n ～q (X n Z n ).进而,如果q (x n X n -1,Z n )=q (x n x n -1,z n ),那么重要性密度函数仅仅依赖于x n -1和z n ,这对在每一步只要求得到滤波估计p (x n Z n )的一般情况非常有用.在此情况下只需要存储x ⌒(i )n .利用贝叶斯公式得到p (X n Z n )=p (z n X n ,Z n -1)p (X n Z n -1)p (z n Z n -1)=p (z n x n )p (x n x n -1)p (z n Z n -1)p (X n -1 Z n -1)∝p (z n x n )p (x n x n -1)p (X n -1 Z n -1)由上式可得λ(i )n∝p (x ⌒(i )0:n Z n )q (x ⌒(i )0:n Z n)=p (z n x ⌒(i )n )p (x ⌒(i )n x ⌒(i )n -1)p (x ⌒(i )n -1 Z n -1)q (x ⌒(i )n x ⌒(i )n -1,z n )q (x ⌒(i )n -1 Z n -1)修正的权值就是λ(i )n∝λ(i )n -1p (z n x ⌒(i )n )p (x ⌒(i )n x ⌒(i )n -1)q (x ⌒(i )n x ⌒(i )n -1,z n)(10)归一化后的权值为λ(i )n=λ(i )n -1∑N i =1λ(i )n -1(11)以上(7)～(11)式组成了S I S 粒子滤波的基本算法.S I S 粒子滤波的一个普遍问题就是退化现象,即经过几次迭代之后,差不多所有的粒子都有极小的权值.重要性权值的方差随着时间的递增而增加,所以要消除退化现象是不可能的.这种退化意味着大量的计算都用来更新粒子,但这些粒子对于逼近概率密度函数的贡献几乎为零.对样本进行重新采样是限制重要性权值退化现象的一个有效办法.其主要思想是在重要性采样的基础上加入重采样,淘汰权值低的粒子,而集中于权值高的粒子,从而限制退化现象.重采样方法对每个粒子x=⌒(i )n 按其权值生成N i 个副样本,并使得∑N i=N,若N i=0则该粒子被淘汰.粒子滤波中的一个主要问题就是高维数、大计246 机 器 人2008年5月算量,在本文算法中如果按照传统的方法,每增加一个环境标志点就要将状态向量增加三维,在保持相同采样精度的条件下,采样粒子数随着特征点的增加成几何级数增长,在标志点较多的情况下会因为计算量太大使算法无法实现.针对本文实际情况,状态向量定义为x n=[(x R n)T (x1)T…(x i)T…(x m)T]T,其中x i=[x i y i z i]T,m为标志点数目,1<i<m.如果每一个标志点采样次数为N,则增加一个标志点计算量就为原来的N倍.而为了尽可能逼近状态的真实分布,采样数量又不能太小,标志点过少相当于减少了观测次数,也不利于定位精度的提高.所以对滤波方法作如下改进:每次只用一个标志点和机器人位姿坐标x i n=[(x R n)T (x i)T]T进行采样,这样增加一个标志点计算量只增加了1/m,也就是说机器人在一个位置观测到m个标志点就进行m次滤波计算.具体算法步骤:①当n=0时,机器人在初始位置,其位姿为x R n=0,观测到a个标志点,并得到对应向量.②当n=j时,观测到b个标志点,同样得到对应向量,其中有m个标志点(m并非固定值)与j-1时刻匹配成功.i.如果标志点为第二次观测到的坐标点,也就是没有计算过坐标,则由2.3节所述的方法计算其坐标.由式(6)可知,这个坐标值存在误差.假设误差服从均匀分布,根据重要密度函数q(x i(t)j x i(t)j-1,u i,z j)=p(x i(t)j x i(t)j-1,u i)作N次随机采样并计算得到状态粒子x i(t)j.p函数的值由式(1)和摄像机观测范围的标志点的均匀分布计算得到.具体来说,这个粒子由机器人的位姿和一个环境特征坐标构成,机器人的位姿可以由左右实际轮速唯一确定,两者相互独立,其分布如式(1)所述;另外,标志点的图像坐标在像素上服从均匀分布.因此,我们可以得到:p(x i(t)jx i(t)j-1,u i)=p(v R j-1)p(v L j-1)p(x u j-1i)p(y v j-1i)p(x u j i)p(y v j i).归一化得到对应x i(t)j 的权值λi(t)j.i i.如果标志点已经计算过坐标并且得到采样粒子,则同样根据重要密度函数q(x i(t)j x i(t)j-1,u i,z j)=p(x i(t)j x i(t)j-1,u i)抽取粒子x i(t)j,按下式更新权值,并归一化.λi(t) j ∝λi(t)j-1p(z j x i(t)j)p(x i(t)jx i(t)j-1,u i)q(x i(t)jx i(t)j-1,z j,u i)(12) 在q(x i(t)j xi(t)j-1,z j,u i)=p(x i(t)jx i(t)j-1,i的情况下λi(t)j∝λi(t)j-1p(z j x i(t)j).i i i.对所有观测到的标志点完成滤波之后将得到的(x R n)T取均值作为这一步的定位结果,最后一次观测到的标志点滤波后得到的坐标值当成其实际坐标.③计算N e f f=1∑Nt=1λi(t)j,如果N e f f≤N3,则使用重采样法防止权值退化.4 实验结果(E x p e r i m e n t a l r e s u l t)对此算法进行仿真验证,仿真条件为:以均值为10%机器人轮速控制量的高斯噪声作为差速定位模型的噪声;通过模拟一个像素为320×240、观测角度为60°的摄像机得到观测量,观测噪声由标志点在像素坐标上的一个均匀分布确定.实验结果如表1和图3所示.图3(a)中显示的三条轨迹分别为真实的轨迹、码盘得到的轨迹和使用该S L A M算法得到的轨迹,结合图3(d)可以看出使用S L A M方法的定位精度明显得到提高,而且随着时间的推移提高的程度越发明显,这也显现出了码盘误差的累积效应,S L A M算法能有效地降低累积误差.图3(b)为机器人实际移动过程中所构建的环境标志点模型,因为视觉测量的距离越远误差就越大,所以只选取观测到的较近范围内的标志点,而且为了使用高精度匹配的标志点,其数量较少.结合图3(c),该S L A M算法同样提高了环境标志点的精度,但是由图3(c)也可以看到该S L A M算法并没有提高个别标志点的精度,这是因为标志点计算中用到了两个时刻机器人的方向角和位移,这两个误差在计算过程中相互抵消,反而使标志点计算较为准确,而S L A M算法的观测量并非完全独立于码盘定位,也就是说它也要受到累积误差的影响,标志点计算还是会存在误差.表1的统计数据显示,相对于码盘差速模型,使用S L A M算法,定位误差由11.6%减小到3.6%,环境标志点估计误差也从0.365m减小到0.106m,定位和环境建模误差都降低到前者的1/3左右,定位和环境模型精度明显提高.表1 误差统计T a b l e1 E r r o r s t a t i s t i c s平均值误差距离(m)百分比(%)环境标志点平均误差码盘定位算法0.33211.60.365第30卷第3期陈伟等: 基于粒子滤波的单目视觉S L A M 算法247图3 实验结果图F i g .3 S k e t c h o f e x p e r i m e n t r e s u l t s5 结论(C o n c l u s i o n s )本文针对微小移动机器人自身特点提出了一种结合码盘和单目视觉的粒子滤波S L A M 方法.该S L A M 方法无需大量环境标志点,但是为了提高环境模型的精细程度,可以对一般的标志点单纯地进行环境建模而不参与定位计算,这样既减少了计算量,又不会影响地图对实际环境的表达.此算法依赖码盘定位差速模型,因此仍然摆脱不了误差累积的影响,机器人长距离的移动导致的误差累积会严重影响定位和环境建模的精度.实验表明:该S L A M 方法虽然未能彻底消除误差的累积,但能较好地对其进行抑制,有效地提高了系统定位精度.参考文献 (R e f e r e n c e s )[1]D u r r a n t -Wh y t e H ,M a j u m d e r S ,T h r u nS ,e t a l .A B a y e s i a na l g o -r i t h m f o r s i m u l t a n e o u s l o c a l i z a t i o na n dm a pb u i l d i n g [A ].P r o c e e d -i n g s o f t h e 10t h I n t e r n a t i o n a l S y m p o s i u m o f R o b o t i c s R e s e a r c h [C ].B e r l i n ,G e r m a n y :S p r i n g e r ,2003.49-66.[2]T h r u n S ,B u r g a r d W,F o x D .Ap r o b a b i l i s t i c a p p r o a c h t o c o n c u r r e n tm a p p i n g a n dl o c a l i z a t i o nf o r m o b i l er o b o t s [J ].M a c h i n eL e a r n i n g ,1998,31(1-3):29-53.[3]S m i t h R ,S e l f M ,C h e s s e m a nP .E s t i m a t i n g u n c e r t a i ns p a t i a l r e l a -t i o n s h i p s i nr o b o t i c s [A ].P r o c e e d i n g so f t h eI E E E I n t e r n a t i o n a l C o n f e r e n c eo nR o b o t i c sa n dA u t o m a t i o n [C ].P i s c a t a w a y ,N J ,U S A :I E E E ,1987.850.[4]S m i t h RC ,C h e e s e m a nP .O nt h er e p r e s e n t a t i o na n de s t i m a t i o no f s p a t i a l u n c e r t a i n t y [J ].T h eI n t e r n a t i o n a l J o u r n a l o f R o b o t i c s R e -s e a r c h ,1986,5(4):56-68.[5]C s o r b aM .S i m u l t a n e o u s L o c a l i z a t i o na n dM a pB u i l d i n g [D ].O x -f o r d :U n i v e r s i t y o f O x f o r d ,1997.[6]D i s s a n a y a k eMW M G ,N e w m a nP ,C l a r kS ,e t a l .As o l u t i o nt o t h e s i m u l t a n e o u sl o c a l i z a t i o na n dm a pb u i l d i n g(S L A M )p r o b l e m [J ].I E E E T r a n s a c t i o n so nR o b o t i c sa n dA u t o m a t i o n ,2001,17(3):229-241.[7]G e h r i g SK ,S t e i n FJ .D e a dr e c k o n i n g a n dc a r t o g r a p h y u s i n g s t e r e o v i s i o nf o r a n a u t o n o m o u s c a r [A ].P r o c e e d i n g s o f t h e I E E EI n t e r n a -t i o n a l C o n f e r e n c eo nI n t e l l i g e n t R o b o t s a n dS y s t e m s [C ].P i s c a t -a w a y ,N J ,U S A :I E E E ,1999.1507-1512.[8]Y a g i Y ,N i s h i z a w a Y ,Y a c h i d a M .M a p -b a s e d n a v i g a t i o nf o r a m o -b i l er o b o tw i t ho m n i d i r e c t i o n a l i m a g es e n s o rC O P I S [J ].I E E E T r a n s a c t i o n s o nR o b o t i c sa n dA u t o m a t i o n ,1995,11(5):634-648.(下转第253页)第30卷第3期李明富等: 基于双目视差和主动轮廓的机器人手眼协调控制技术研究253要包括两个方面:一方面,通过对物体轮廓几何特征、颜色特征和运动特征等进行综合考虑,建立相应的综合概率密度模型,这样就可以避免出现轮廓变形的情况;另一方面,利用分类学习方法建立各种物体的轮廓概念模型,使机器人能够对抽象的形状概念进行理解并辅助决策和操作.参考文献 (R e f e r e n c e s)[1]M a r c h a n dE,C h a u m e t t eF.F e a t u r et r a c k i n gf o r v i s u a l s e r v o i n gp u r p o s e s[J].R o b o t i c s a n dA u t o n o m o u s S y s t e m s,2005,52(1):53-70.[2]H a g e r G D,T o y a m a K.X V i s i o n:Ap o r t a b l es u b s t r a t ef o r r e a l-t i m ev i s i o na p p l i c a t i o n s[J].C o m p u t e r V i s i o na n dI m a g eU n d e r-s t a n d i n g,1998,69(1):23-37.[3]M a r c h a n dE.V i S P:As o f t w a r e e n v i r o n m e n t f o r e y e-i n-h a n d v i s u a ls e r v o i n g[A].P r o c e e d i n g s o f t h e I E E EI n t e r n a t i o n a l C o n f e r e n c e o nR o b o t i c sa n d A u t o m a t i o n[C].P i s c a t a w a y,N J,U S A:I E E E,1999.3224-3229.[4]S u n d a r e s w a r a nV,B e h r i n g e r R.V i s u a l s e r v o i n g-b a s e da u g m e n t e dr e a l i t y[A].P r o c e e d i n g so f t h eF i r s t I n t e r n a t i o n a l Wo r k s h o po nA u g m e n t e dR e a l i t y[C].N a t i c k,M A,U S A:AK P e t e r s,1998.193-200.[5]S u l l i v a nMJ,P a p a n i k o l o p o u l o s NP.U s i n g a c t i v e d e f o r m a b l e m o d-e l s t ot r a c kd ef o r m a b l eo b j e c t si nr o b o t i cv i s u a ls e r v o i n ge x p e r i-m e n t s[A].P r o c e e d i n g so f t h e I E E EI n t e r n a t i o n a l C o n f e r e n c eo nR o b o t i c sa n d A u t o m a t i o n[C].P i s c a t a w a y,N J,U S A:I E E E,1996.2929-2934.[6]P o r t aJ M,V e r b e e kJ J,K r o s e BJ A.A c t i v e a p p e a r a n c e-b a s e dr o-b o t l oc a l i z a t i o n u s i n g s t e r e o v i s i o n[J].A u t o n o m o u s R o b o t s,2005,18(1):59-80.[7]夏利民,谷士文,罗大庸,等.基于活动轮廓的机器人视觉伺服控制[J].国防科技大学学报,2000,22(1):60-64.[8]X i a LM,G uS W,L u o DY,e t a l.R o b o t i c v i s u a l s e r v o i n g b a s e do ns n a k e s[A].P r o c e e d i n g s o f t h e3r dWo r l dC o n g r e s s o nI n t e l l i-g e n t C o n t r o l a n dA u t o m a t i o n[C].P i s c a t a w a y,N J,U S A:I E E E,2000.1317-1320.[9]P r e s s i g o u t M,M a r c h a n dE.R e a l t i m ep l a n a r s t r u c t u r e t r a c k i n g f o rv i s u a l s e r v o i n g:Ac o n t o u r a n dt e x t u r ea p p r o a c h[A].P r o c e e d i n g s o f t h e I E E E/R S J I n t e r n a t i o n a l C o n f e r e n c e o nI n t e l l i g e n t R o b o t s a n dS y s t e m s[C].P i s c a t a w a y,N J,U S A:I E E E,2005.251-256.[10]I s a r dM,B l a k e A.C O N D E N S A T I O N-C o n d i t i o n a l d e n s i t y p r o p a-g a t i o n f o r v i s u a l t r a c k i n g[J].I n t e r n a t i o n a l J o u r n a l o f C o m p u t e r V i-s i o n,1998,29(1):5-28.[11]F e r r e M,A r a c i l R,N a v a s M.S t e r e o s c o p i c v i d e o i m a g e s f o r t e l e r o-b o t ic a p p l i c a t i o n s[J].J o u r n a l o f R o b o t i c S y s t e m s,2005,22(3):131-146.[12]H a g e r GD,C h a n g W C,M o r s e AS.R o b o t h a n d-e y e c o o r d i n a t i o nb a s e do n s t e r e o v i s i o n[J].I E E EC o n t r o l S y s t e m s M a g a z i n e,1995,15(1):30-39.作者简介: 李明富(1979-),男,博士生.研究领域:机器人视觉,机器人遥操作,智能控制. 付 艳(1977-),女,博士,讲师.研究领域:人工智能,视觉认知学,人因工程. 李世其(1965-),男,博士,教授,博士生导师.研究领域:机器人遥操作,虚拟现实,力学仿真.(上接第247页)[9]Z h o n g ZG,Y i J Q,Z h a o DB,e t a l.N o v e l a p p r o a c h f o r m o b i l e r o-b o t l oc a l i z a t i o nu s i n gm o n o c u l a r v i s i o n[A].P r o c e ed i n g so f S P I E(v o l.5286)[C].B e l l i n g h a m,W A,U S A:S P I E,2003.159-162.[10]A r u l a m p a l a m M S,M a s k e l l S,G o r d o n N,e t a l.At u t o r i a l o np a r t i-c l ef i l t e r s f o r o n l i n e n o n l i n e a r/n o n-G a u s s i a nB a y e s i a nt r a c k i n g[J].I E E ET r a n s a c t i o n s o nS i g n a l P r o c e s s i n g,2002,50(2):174-188.作者简介: 陈 伟(1976-),男,博士生.研究领域:机器人视觉,机器人导航与定位等. 吴 涛(1975-),男,博士,副教授.研究领域:统计学习,模式识别,机器人视觉等. 李 政(1972-),男,博士生.研究领域:统计学习,模式识别,图像处理等.。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
引 言1
机器人真正具备自主能力的先决条件是能否自己定位 的同时精确地做出环境地图[1]。这个问题也就是Simultaneous Localization and Map Building (SLAM)。SLAM问题是智能移 动机器人所要研究的基本问题，近来引起学者的广泛关注， 文[2]对该问题的研究进展做了很好的综述。
第 19 卷第 16 期 2007 年 8 月
系 统 仿 真 学 报© Journal of System Simulation
Vol. 19 No. 16 Aug., 2007
基于粒子滤波器的 SLAM 的仿真研究
鞠纯纯，何 波，刘保龙，王永清
(中国海洋大学, 山东 青岛 266071)
摘 要：机器人同时定位与精确地图创建能力是自主移动机器人的先决条件。SLAM 的很多实现方
概率 p( xt | zt , ut ) ，可以表示为如下贝叶斯概率模型： p( xt | zt , ut )
∫ = η p(zt | xt ) p( xt | ut , xt−1 ) p( xt−1 | zt−1, ut−1 )dxt−1 (2)
η 是个常数，传感器观测值遵守的概率分布 p(zt | xt ) 分布通 常称为观测模型，而 p(xt | ut , xt−1) 通常称为运动模型。
应用卡尔曼滤波器必须假定系统的输入噪声和观测噪 声服从高斯分布，这需要环境特征必须是唯一确定的。当无 法满足这一点时，就要采取其他方法例如粒子滤波器来实现 SLAM。粒子滤波器也是一种蒙特卡洛方法，基本思想是利 用新的观测数据来更新机器人的位置分布[5]。粒子滤波器在
收稿日期：2006-06-23
修回日期：2007-01-11
= p(m | st , zt , ut , nt ) p(st | zt , ut , nt )
N
∏ = p(st | zt , ut , nt ) p(mi | st , zt ,ut , nt )
(4)
i =1
这样，SLAM 分解为机器人路径估计和环境特征位置估 计两个部分。也就是将 SLAM 分解成 K+1 个估计问题：一 个是估计机器人路径 st ，在此基础上，用 K 个估计器来分 别估计 K 个环境特征位置的问题。这样分解完全考虑到估计 机器人位姿和估计环境特征位置之间的独立性，是正确并且
(3)Leabharlann nt ∈{1,...k} 表示t时刻观测到的环境特征。为了计算方便，假 设环境坐标是唯一确定的，并且环境坐标的数量K也是已知 的。通常计算(3)一般是比较难的。用Rao-Black-wellised
粒子滤波器可以提供比较有效的解决方法。
这个方法的思想是用分解贝叶斯滤波器：
p(st , m | zt ,ut , nt )
关键词：SLAM；移动机器人；粒子滤波器；卡尔曼滤波器
中图分类号：TP 242
文献标识码：A
文章编号：1004-731X (2007) 16-3715-04
Simulation Research on Simultaneous Robot Localization and Mapping Based on Particle Filter
“临时”集合。如果集合 St−1 根据 p(st−1 | zt−1,ut−1, nt−1 ) 分布， 临时集合的分布就服从 p(st | zt−1,ut , nt−1) 。然后通过临时集
合取样得到新集合
St
。每个粒子
st,[i ]
的选取用权值
w[ i ] t −1
来衡
量，权值计算如下[13]：
∫ p(nt | zt , ut )α = p(nt | st , zt ,ut ) p(st | zt ,ut )dst
JU Chun-chun, HE Bo, LIU Bao-long, WANG Yong-qing
(Ocean University of China, Qingdao 266071, China)
Abstract: The ability to simultaneous localization and mapping is a predetermination of antomomous robots. Now, few approaches can manage the environment with masses of landmarks. The posterior distribution over robot pose and landmark locations was estimated with paticle filter and Kalman Filter respectively. The key idea of the algorithm is factorating of the Bayes filter into an estimation of robot path estimation and an estimation of landmarks. To avoid the depletion problem, the particle population was injected during the update phase with a small number of particles created directly from the sensor data as well as using the weights of the particles to decide which ones were going to be progated forward. Simulation results demonstrate 5000 landmarks with 100 particles can be handled successfully. Key words: SLAM; Mobile Robots; Particle Filter; Kalman Filter
器估计与该位置相关的K个环境特征，这样系统中共有MK 个卡尔曼滤波器，每个卡尔曼滤波器只对单个环境特征处 理。计算复杂度可以降低到O(MlogK)。
对于粒子滤波器，如果迭代过程中粒子数目减少到一定 数值，正确的状态附近缺乏粒子，就会导致衰竭问题[12]。这 要求粒子的数量随着系统的状态增加也能相应地增加，并且 重采样过程需要采取适当的措施。本文除了用权值来选取粒 子，还通过传感器数据生成少量的粒子，这些粒子与选取的 粒子无关。在更新阶段注入这些粒子来补充粒子数的不足。
本文的研究方法是用Rao-Black-wellised粒子滤波器估 计机器人路径的后验概率，每个粒子与一个完整的地图相关 联。对于环境特征的分布，在每个位置使用K个卡尔曼滤波
• 3715 •
第 19 卷第 16 期 2007 年 8 月
系统仿真学报
Vol. 19 No. 16 Aug., 2007
1 基本的 SLAM 问题
SLAM中，用一个状态矢量 xt 表示机器人的位姿(位置和 朝向)和环境特征的位置。机器人依靠传感器得到的信息和自
身的运动信息来求状态变量。这两个信息用如下的集合表示：
zt = {z1,...zt }
ut = {u1,...ut }
(1)
zt表示传感器在t时刻的测量值；ut代表机器人在[t-1，t]期间 运动的控制量。SLAM的目的是估计t时刻状态变量x的后验
法无法解决有大量环境特征的环境。应用粒子滤波器和卡尔曼滤波器分别估计机器人位姿和环境特征
的后验概率分布。这个算法的基础是把后验概率分解成路径的后验概率和环境特征的后验概率分布。
为避免衰竭问题，在粒子滤波器的重采样阶段，除了用权值选取粒子，还在更新阶段直接注入从传
感器数据生成的少量粒子。仿真结果显示这个算法可以用 100 个粒子处理 5000 个环境特征的优越性。
SLAM主要方法首先在1986年由Smith 和Cheeseman在 学术论文中提出[3-4]。论文中提出用扩展卡尔曼滤波器来估 计机器人的位姿和环境特征的位置的后验概率分布。在过去 的二十年里，这个方法在机器人领域广泛地被应用。最近的 研究中，这个方法的热点是用于处理更多环境特征的环境以 及数据关联问题。
可行的[12]。机器人路径后验概率 p(st | zt ,ut , nt ) 由粒子集合
表示，这些粒子从空间中有效取样，即使对非线性运动模型， 也可以提供较好的估计。环境特征估计 p(mi | st , zt ,ut , nt ) 由 卡尔曼滤波器实现，每个卡尔曼滤波器代表不同的环境特 征。环境特征的估计是建立在路径估计的基础上，每个粒子
2 基本的粒子滤波器问题
粒子滤波器的基本思想是用一些样本或粒子 {xt[i]} 来估
计
p( xt
|
zt
, ut
)
。
x[i] t
是M个样本中第i个样本，M是粒子滤波
器的大小。粒子滤波器算法：
1、随机初始化M个粒子。
2、求每个粒子的运动模型。
3、对于每个粒子预测观测值，再根据观测值计算权值。
4、重采样：根据权值重新选择那些最好地解释观测量
机器人定位领域较早取得的成功体现在1999年Dellaert[5]的 论文中。近来，粒子滤波器已经成为解决更高维数的机器人 问 题 ( 例 如 SLAM) 的 核 心 方 法 。 Murphy 等 人 采 用 RaoBlack-wellised粒子滤波器来有效地估计机器人的路径和相 关的地图[6]。此后，很多研究对其进行改进。Thrun[7]等人提 出一种转置协方差矩阵的方法。这种方法，可以有效的融合 观测值。Paskin[8]等人提出用节点树来解决slam问题。减少 节点能够提供线性时间滤波器的操作，这相比EKF减少了复 杂度的计算。Estrada[9]等人提出一种分级地图的处理方法。 用排列在图表结构中的局部地图的集合来描述全局地图。算 法致加强了全局一致性。Guivant[10]等人提出结合环境特征 和栅格地图的方法，该方法在建立在HYMM的基础上。