基于二维激光的机器人室内构图

MachinePerceptionandInteractionGroup(MPIG)gwt@

基于二维激光的机器人室内构图宫婉婷MPIGOpenSeminar

0095GMapping算法理论研究GMapping代码介绍GMapping算法原理分析Libpointmatcher代替ICP运行结果对比内容概要粒子滤波粒子滤波算法源于Montecarlo思想，即以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率如P(x)的地方，一概对变量x采样，以大量采样及其相应的权值来近似表示P(x)。而Karman则只能处理线性高斯分布的概率问题论文理论研究

机器人图模型通过RBPF滤波器构图

SIR滤波流程弊端：在新观测到临时，每次评估抓取粒子的权重。当随时间增长轨迹长度增加时，这些流程的精确性将会失效。引入：Doucetetal.使用递归的方程计算权重，限制提议分布满足给定假设：许多存在的粒子滤波严格依赖与公式（6），然而通用的算法指定用于计算地图的具体框架，并不确定提议分布怎样计算与何时执行重采样。，Gmapping算法描述了怎样计算精确的提议分布，且进行适应性的重采样。算法介绍：GMapping是一种改进的Rao-Blackwellized粒子滤波法，采样的每个粒子都包含环境地图信息。关键问题是怎样降低粒子数。在ROS中是一个第三方包，包含openSLAM的Gmapping的一个ROS封装。Gmapping的ROS节点slam_gmapping提供基于激光的SLAM，依靠移动机器人的激光和位姿数据，使用slam_gmapping创建2D网格地图。贡献：1.采用adaptivetechniques降低RBPF的粒子数，执行选择性重采样，限制粒子耗尽问题。2.提出方法计算精确的建议式分布，同时考虑机器人运动和最近观测信息GMapping算法精确的提议分布提议分布π：一种对于目标分布的近似模拟目标分布：理想的粒子分布形态作用：作用是预测阶段从提议分布π抽样获取下一批粒子，同时用来计算权重，用来理论上代替目标分布。直观上，提议分布越近似目标分布，滤波器性能越好，理想情况下不需要重采样步骤，（目标分布与提议分布差距太大的情况会引入重采样）在SLAM中闭合后验是不能得到的。模拟提议分布的多种方法里程运动模型，权重由观测模型计算优点：运动模型易于计算，可以根据观测模型计算权重，缺点：当传感器信息精度远高于基于里程计的机器人运动估测的情况下是次优的。由于仅有一部分样本覆盖了观测模型下具有高可能性的状态空间，所以每次独立采样的权重差异很大，需要相当多的样本来充分覆盖具有高观测概率的区域。图1示出了其中传感器观察似然的有意义的区域显著小于运动模型的有意义的区域的情况。所以需要大量粒子来充分覆盖具有高观察可能性的区域

有效计算改进的提议分布

这样，获得一个最优分布的近似闭合形式，使用这个分布可以计算权重：AdaptiveResamplingGMapping采用Neff决定是否执行采样步骤，每当采样粒子数下降到N/2的阈值以下时，进行重新采样，其中N是粒子数。大量实验表明，这种方法大大降低了好粒子被替换的风险，因而降低重采样操作次数，仅在需要时进行。算法流程代码分析运动模型更新t时刻的粒子群（模型中添加了高斯噪声）OrientedPointMotionModel::drawFromMotion(constOrientedPoint&p,constOrientedPoint&pnew,constOrientedPoint&pold)const{doublesxy=0.3*srr;OrientedPointdelta=absoluteDifference(pnew,pold);OrientedPointnoisypoint(delta);noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));noisypoint.theta=fmod(noisypoint.theta,2*M_PI);if(noisypoint.theta>M_PI)noisypoint.theta-=2*M_PI;returnabsoluteSum(p,noisypoint);//叠加噪音}扫描匹配/**Justscanmatcheverysingleparticle.Ifthescanmatchingfails,theparticlegetsadefaultlikelihood.*/inlinevoidGridSlamProcessor::scanMatch(constdouble*plainReading){//sampleanewposefromeachscaninthereferencedoublesumScore=0;for(ParticleVector::iteratorit=m_particles.begin();it!=m_particles.end();it++){OrientedPointcorrected;doublescore,l,s;score=m_matcher.optimize(corrected,it->map,it->pose,plainReading);//it->pose=corrected;if(score>m_minimumScore){it->pose=corrected;}else{if(m_infoStream){m_infoStream<<"ScanMatchingFailed,usingodometry.Likelihood="<<l<<std::endl;m_infoStream<<"lp:"<<m_lastPartPose.x<<""<<m_lastPartPose.y<<""<<m_lastPartPose.theta<<std::endl;m_infoStream<<"op:"<<m_odoPose.x<<""<<m_odoPose.y<<""<<m_odoPose.theta<<std::endl;}}m_matcher.likelihoodAndScore(s,l,it->map,it->pose,plainReading);sumScore+=score;it->weight+=l;it->weightSum+=l;//setuptheselectivecopyoftheactivearea//bydetachingtheareasthatwillbeupdatedm_matcher.invalidateActiveArea();m_puteActiveArea(it->map,it->pose,plainReading);}if(m_infoStream)m_infoStream<<"AverageScanMatchingScore="<<sumScore/m_particles.size()<<std::endl;}GridSlamProcessor::scanMatchPropasalDistributionRBPF-gmapping使用的是运动模型作为提议分布。GMapping中混合了运动模型和观测模型的提议分布，将扫描观测值整合到了提议分布中。因此均值和方差的计算与单纯使用运动模型作为提议分布的有所区别。提议分布的作用是获得最优的粒子点，同时用来计算权重，这个体现在ScanMatcher::likelihoodAndScore()和ScanMatcher::score()方法中，score方法中采用的是服从0均值的高斯分布近似提议分布,由于L(i)区域小的原理，所以采用混合的提议分布。权重更新voidGridSlamProcessor::updateTreeWeights(boolweightsAlreadyNormalized){if(!weightsAlreadyNormalized){normalize();//归一化权重计算（20）}resetTree();//初始化粒子的树节点，权重，访问次数，父节点等propagateWeights();//为了迭代计算，计算上一次粒子的权重（19）}重采样

算法结构分析硬件需求：使用slam_gmapping,需要移动机器人提供里程计数据，且水平安装固定激光测距仪。Slam_gmapping节点作用：扫描输入数据数据Odom里程计Tf坐标系Tf坐标转换需要涉及激光坐标系，基坐标系和里程坐标系从激光扫描创建地图Gmapping订阅及发布话题slam_gmapping采用sensor_msg/LaserScan消息，并且构建地图nv_msgs/OccupancyGrid)。地图可以通过topic或service检索。Slam_gmapping节点订阅发布话题TFScanMap_metadatamap~entropy从这个话题获得地图数据，此话题锁存，并定期更新机器人位姿分布熵表示服务Dynamic_map调用这个服务来获取地图数据参数Max_Urangeparticles

Max_interationsmap_frameOdom_frameminimumSorellsanplerangesrrMap_updatelskip需要的tf变换：<the

frame

attached

to

incoming

scans>

→

base_link通常是一个固定值，

由robot_state_publisher或

tf

static_transform_publisher.定期广播。base_link

→

odom通常有里程计系统提供(例如，移动机器人的驱动)提供的tf变换：map

→

odom在地图坐标系中机器人位姿的当前估计TF介绍：tf是一个包，可以使用户随着时间的推移跟踪多个坐标系间的转换，tf保持在时间缓冲区内的树结构，可以使用户在任意时间转换两个坐标系之间的点与矢量等。Tf的功能：机器人系统有许多3D坐标系，随时间变换，比如worldframe,base_frame,这些tf可以随时间跟踪这些坐标系，可以给出确定时间点两个坐标系之间的相对运动关系，以及基于某坐标系下物体的位姿，以及在map坐标系下base_frame坐标系的当前位姿。利用libpointmatcher替换ICP匹配经典的ICP算法Libpontmatcher介绍Libpontmatcher的ICP算法Libpointmatcher的编译Libpointmatcher怎样链接到外部工程经典ICP算法

ICP算法流程Libpontmatcher介绍Libpointmatcher是一个实现ICP算法来配准点云的库，一种模块化icp链，模块化和快速性，l为配准算法提供了理想的解决方案。支持点到点和点到面的ICP配准。原始ICP算法的弊端：icp算法的本质是局部收敛近似算法，结果取决于初始姿态，需要提供初始假设，初始假设通常由惯性测量，里程计或启发式运动模型提供，这些都有有限精度和观测中随时间增加的不确定度。另外激光扫描匹配的一个难点是大量的离群点，有大量的不能反映扫描特征的点。为了弱化这些因素对ICP算法性能的影响，引入Libpointmatcher替代原始ICPlibpointmatcher中,每一个模块都是一个类,每一个类会定义一些自己的控制参数,所以使用一个yaml文件来输入控制参数.ICP的输入为两个点云,输出是两个点云之间的旋转量和平移变换量的估计值.一般称第一个点云叫做参考点云(reference),第二叫reading(怎么读取点云).ICP链首先会对两幅点云做滤波处理(PointMatcher::DataPointsFilters),然后对两幅点云进行匹配(PointMatcher::Matcher),去掉outlier点(PointMatcher::OutlierFilters),计算使配准误差最小(PointMatcher::ErrorMinimizer)的变换参数(PointMatcher::TransformationParameters).编译方法：http://libpointmatcher.readthedocs.io/en/latest/Compilation/Libpintmatcher的ICPLibpointmatcher怎样链接到外部工程由于libpointmatche