基于概率的移动机器人SLAM算法框架

—31—基于概率的移动机器人SLAM算法框架石杏喜1,2，赵春霞2(1.南京理工大学理学院，南京210094；2.南京理工大学计算机学院，南京210094)摘要：在移动机器人同时定位与地图创建(SLAM)过程中，机器人本身位置不确定，其所处环境也不可预知，针对这些不确定性因素，应用贝叶斯规则作为理论基础，建立移动机器人SLAM算法的概率 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 示模型，通过扩展卡尔曼滤波器实现SLAM算法，并介绍一种激光雷达数据与特征地图的数据关联 方法 快递客服问题件处理详细方法山木方法pdf计算方法pdf华与华方法下载八字理论方法下载 。实验结果表明，该方法为实现SLAM算法提供了一种有效可靠的途径。关键词：机器人；概率论；同时定位与地图创建；扩展卡尔曼滤波器SLAMAlgorithmFrameworkofMobileRobotBasedonProbabilitySHIXing-xi1,2,ZHAOChun-xia2(1.CollegeofSciences,NanjingUniversityofScienceandTechnology,Nanjing210094;2.CollegeofComputer,NanjingUniversityofScienceandTechnology,Nanjing210094)【Abstract】DuringthemobilerobotSimultaneousLocalizationAndMapping(SLAM),thelocationisunknownandtheenvironmentroundisalsounpredictable.Aimingattheseuncertainfactors,theBayesruleisasatheoryfoundation,theprobabilitymodelofthemobilerobotSLAMisfounded,therealizationprocessoftheSLAMbyExtendedKalmanFilter(EKF)isdiscussed.Adataassociationmethodbetweenthelaserradarandthefeaturemapisintroduced.ExperimentalresultsshowthismethodiseffectiveandreliabletorealizeSLAM.【Keywords】robot;probabilitytheory;SimultaneousLocalizationAndMapping(SLAM);ExtendedKalmanFilter(EKF)计算机工程ComputerEngineering第36卷第2期Vol.36No.22010年1月January2010·博士 论文 政研论文下载论文大学下载论文大学下载关于长拳的论文浙大论文封面下载 ·文章编号：1000—3428(2010)02—0031—02文献标识码：A中图分类号：TP241概述移动机器人需要在自身位置不确定的条件下，在未知环境中创建地图，同时利用地图进行自主定位和导航，即移动机器人的同时定位和地图创建(SimultaneousLocalizationAndMapping,SLAM)，SLAM可以描述为一个“鸡生蛋，蛋生鸡”的问 题 快递公司问题件快递公司问题件货款处理关于圆的周长面积重点题型关于解方程组的题及答案关于南海问题 ，一方面为实现精确定位，机器人需要利用位置相对确定的环境地图不断校正自身的位置，而另一方面为了确定环境地图的位置，机器人需要获取其自身所在的位置，2个方面互为前提[1]。由于在SLAM过程中，存在很多不确定性因素。首先由于各传感器噪声数据使得机器人本身具有不确定性，其次机器人所处环境也是不可预知的。这些不确定性因素使SLAM变得更加困难，因此，近年来，越来越多的研究者把概率理论应用到移动机器人的SLAM技术中[2]。2移动机器人SLAM的概率描述方法机器人的SLAM问题可以描述为：机器人和其工作环境组成的动态系统，设T[,,]rrrrkkkkxy=∂X表示机器人的状态；T12[,,,]mknmmm="X表示点特征地图；T1:12[,,,]kkzzz="z为观测数据系列；T1:12[,,,]kkuuu="u为控制命令系列，设机器人初始状态概率分布为0()rpX，根据贝叶斯滤波原理得到后验概率密度。这样，通过观测数据1:kz和控制输入1:ku来递推计算状态rkX和mkX取不同值时的置信度，由此获得状态的最优估计，其基本步骤分为预测(prediction)和更新(updating)[3]。(1)预测阶段，由于地图特征是静态的，因此有1mmkk−=XX，为了表示方便，这里用M来表示。预测阶段由系统的状态转移模型，在未获得k时刻的观测值时，实现后验概率11:11:10(,/,,)rrkkkpM−−−XuzX到先验概率1:1:10(,/,,)rrkkkp−XMuzX的推导：1:1:10111:11:101(,/,,)(/,)(,/,,)drrkkkrrrrkkkkkkkpppx−−−−−−=×∫XMuzXXuXXMuzX(1)1(/,)rrkkkp−XuX是机器人的运动模型，是在已知k时刻机器人控制输入ku和1k−时刻位姿1rk−X的条件下，求取k时刻机器人位姿rkX的条件概率。(2)更新阶段，即由系统的观测模型，在获得k时刻的观测值kz后实现先验概率1:1:11(,/,,)rrkkkp−XMuzX到后验概率1:1:0(,/,,)rrkkkpXMuzX的推导，即1:1:01:11:0(,/,,)(/,)(,/,,)rrrrrkkkkkkkkpppη−=XMzuXzXMXMzuX(2)其中，(/,)rkkpzXM为系统的观测模型，如果已知机器人的运动模型和观测模型，就可以通过式(1)、式(2)进行迭代求解SLAM问题[4]。其中最基本的方法是基于EKF的SLAM算法。3机器人的状态模型3.1机器人的运动模型设机器人的联合状态向量为T[,]rmkkk=XXX，特征点一般基金项目：国家自然科学基金资助项目(60705020)；国家部委基金资助项目作者简介：石杏喜(1975－)，男，讲师、博士，主研方向：模式识别，机器人自主导航，GPS卫星定位理论及其组合导航；赵春霞，教授、博士、博士生导师收稿日期：2009-06-10E-mail：xingxishi@163.com—32—是静止的，即k时刻与1k−时刻的位置不发生变化。设机器人的控制输入为T[,]rrkkkvθ=u，则机器人的运动模型1(/,)rrkkkp−XXu可以描述为111111cos()(,)sin()sin()rrrrkkkkrrrrrrkkkkkkkrrrkkkxTvfyTvTvLθθθ−−−−−−⎡⎤⎢⎥+∂+⎢⎥==+∂+⎢⎥⎢⎥⎢⎥∂+⎢⎥⎣⎦XXu(3)其中，T为采样周期；L为机器人前后轮间距。3.2机器人的观测模型通过激光雷达测量得到特征点到机器人的距离ρ和机器人前进方向的夹角β，设第i个特征点的位置坐标为(,)iixy，则机器人观测模型可以表示为22()()()arctanrrkikikrkkrikkkrikxxyyhyyxxρβ⎡⎤−+−⎢⎥⎡⎤===⎢⎥⎢⎥−−∂⎣⎦⎢⎥−⎣⎦zX(4)4EKF-SLAM算法EKF是SLAM算法基础。其过程可以归纳为预测过程、更新过程和状态增广过程[5]。(1)预测过程假设在1k−时刻状态向量及其协方差矩阵分别为1k−X和1k−P，将1k−P写成分块矩阵形式为11111()rrrmkkkrmTmmkkPPPP−−−−−⎡⎤=⎢⎥⎣⎦P(5)其中，1rrkP−,1rmkP−和1mmkP−分别为1k−时刻机器人位姿的协方差估计，特征点位置的协方差估计和机器人位姿和地图特征点位置的互协方差估计。根据机器人运动模型得到状态向量的预测值和方差为1/11(,)rkkkkmkfXuX−−−⎡⎤=⎢⎥⎣⎦X(6)/1/1/1/1/1()rrrmkkkkkkrmTmmkkkkPPPP−−−−−⎡⎤=⎢⎥⎣⎦P(7)(2)更新过程在k时刻得到观测值kz，便可对其预测进行更新，因此，可以得到：/1kkkkk−=+XXwv(8)T/1kkkkkk−=−PPwsw(9)其中，kv为新息；ks为新息方差；kw为卡尔曼滤波增益。(3)状态向量增广过程状态向量增广过程为对于新观测到的特征点必须加入到状态向量中，并进行初始化：rkkmaknewnew⎡⎤⎡⎤⎢⎥==⎢⎥⎢⎥⎣⎦⎢⎥⎣⎦XXXXzz(10)0()000rrrmkkrmTmmakkPPPPR⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦P(11)cos()(,)sin()rrrknewnewknewknewrrknewnewkxrgyrββ⎡⎤++∂==⎢⎥++∂⎣⎦mXz(12)ˆ()(,)rkmaakrknewfg⎡⎤⎢⎥==⎢⎥⎢⎥⎣⎦XXXXXz(13)TˆaaaXaX=∇∇PfPf(14)5数据关联在移动机器人SLAM中，数据关联用来建立观测与地图中已有特征的关系，错误的数据关联不仅会影响机器人的定位，还会改变建立的地图，直接导致SLAM算法的发散。在SLAM中使用的数据关联算法也多种多样，本文采用近邻算法，设在k时刻，激光雷达得到m个环境特征im的观测值ikz(1,2,,im=")，地图中已经存在n个特征jm(1,2,,jn=")，利用观测方程可预测k时刻的地图特征点jm的预测观测值：/1/1/1()kkjkkkkkhρβ−−−⎡⎤==⎢⎥⎣⎦zX(15)根据新息ijkv和新息协方差ijks计算马氏距离(MahalanobisDistance)：2T1()()()ijijijijkkkkD−=vsv(16)2()ijkD满足2x分布，当给出一定的置信水平1α−，一般为95%，以2,1nxα−作为关联门，满足下式的地图特征点，可以认为是与im相容的地图特征点，选择马氏距离最短的特征点作为最佳匹配[5]：22,1()ijknDxα−<(17)6实验结果采用EKF算法，对SLAM进行了仿真实验分析，环境区域大小为100m×100m，共有21个地图特征和4个路口，图1为机器人沿4个路口探索时的仿真环境地图。设定其仿真条件和 参数 转速和进给参数表a氧化沟运行参数高温蒸汽处理医疗废物pid参数自整定算法口腔医院集中消毒供应 为：机器人初始状态值T[30,30,0]r=−−X，运行线速度1m/srv=，角速度20/180rad/srθ=×π，速度误差0.3m/srvσ=，舵角误差0.3/180rad/srθσ=×π，激光雷达的测距误差1maρσ=和测角误差1/180radβσ=×π。-50-40-30-20-100102030405050-50-40-30-20-1001020304050X/meterY/meterW2W3W4W1W2W4W3W1505040403020100-10-20-30-40-5020X/meter-50-40-30-20-1001020304050Y/meter图1机器人轨迹及特征位置图2~图4分别是机器人X轴方向、Y轴方向和方位角的估计误差随时间变化的曲线，可以看出，误差随运行时间，逐渐积累，并且经过EKF滤波使其精度明显提高。05001000150020002500-2-1.5-1-0.500.5Time/secondXEstimatedError/meter滤波前滤波后图2X轴方向的估计误差(下转第41页)