gmapping

Gmapping(fastSlam)1、获取激光数据并处理如果是第一个激光数据，则初始化粒子，每个粒子维护一幅地图map，机器人的位姿pose，上个时刻的位姿previousPose，权重weight，权重总和weightSum。地图包括的信息有：m_center：地图中心点（x,y）、m_worldSizeX：地图长度、m_worldSizeY：地图宽度、m_delta：网格边长代表的长度。单位[m/cell]m_xmin、m_xmax、m_ymin、m_ymax，分别对应地图横坐标最小值和最大值、纵坐标最小值和最大值。粒子初始化，初始信息都相同TNode*node=newTNode(initialPose,0,0,0);ScanMatcherMaplmap(Point(xmin+xmax,ymin+ymax)*.5,xmax-xmin,ymax-ymin,delta);for(unsignedinti=0;isize;i++){m_particles.push_back(Particle(lmap));m_particles.back().pose=initialPose;m_particles.back().previousPose=initialPose;m_particles.back().setWeight(0);m_particles.back().previousIndex=0;//thisisnotneeded//m_particles.back().node=newTNode(initialPose,0,node,0);//weusetherootdirectlym_particles.back().node=node;}geometry_msgs/Twist.msg#Thisexpressesvelocityinfreespacebrokenintoitslinearandangularparts.Vector3linearVector3angular//获取激光在里程计坐标系上的坐标tf::Stampedtf::Transformodom_pose;try{tf_.transformPose(odom_frame_,centered_laser_pose_,odom_pose);}处理激光数据GridSlamProcessor::processScan(constRangeReading&reading,intadaptParticles)对于第一条数据：1、计算激光在每个粒子相应的位姿所扫描到的区域ScanMatcher::computeActiveArea(ScanMatcherMap&map,constOrientedPoint&p,constdouble*readings)对每一束激光束，设start为该激光束起点，end为激光束端点（障碍物位置），使用Bresenham划线算法确定激光束经过的网格，算法原理如下：设start、end所在直线方程为y=kx+b，首先通过直线的斜率确定了在x方向进行单位步进还是y方向进行单位步进：当斜率k的绝对值|k|1时，在x方向进行单位步进；当斜率k的绝对值|k|1时，在y方向进行单位步进。下面以|k|1时推导Bresenham算法的数学依据：如上图，已知有一直线y=kx+b，|k|1。我们通过斜率确定了x方向为单位步进。当x=xm时，y=ym。那么当x执行一个单位步进时（即x=xm+1时）,y等于ym还是等于ym+1更符合这个直线方程呢？单凭肉眼我们很难得出结论，最好的办法当然是比较ym和ym+1和真实的方程的y值的差是多少（即Yreal=k*(xm+1)+b），看看哪一个更靠近真实的方程的y值。我们设Dupper=ym+1-Yreal=ym+1-k*(xm+1)+b）;表示ym+1和方程真实值的差Ddown=Yreal-ym=k*(xm+1)+b）-ym;表示ym和方程真实值的差那就是我们要比较Dupper和Ddown的大小。假设Diff=Dupper-Ddown=（ym+1-k*(xm+1)+b））-（k*(xm+1)+b）-ym）令△X为线段x方向的间距（即|end.x-start.x|），△Y为线段y方向的间距（|end.y-start.y|）。pm=△X*Diff=2*△X*ym-2*△Y*xm-2*△Y-△X*（2b-1）；那么pm1=pm+2*△X*（ym1-ym）-2*△Y；=pm+2*△X-2*△Y（pm0）或者=pm-2*△Y（pm0）其中ym1-ym取0还是1，取决于pm的符号,pm0取0，算法循环判断pm，如果小于0，则下一个点的y坐标加1。根据等式Diff=Dupper-Ddown=（ym+1-（k*(xm+1)+b））-（k*(xm+1)+b）-ym）以及k=△Y/△X，我们可以得出起始像素（x0,y0）的参数p0的值：P0=2*△X*y0-2*△Y*x0-2*△Y-△X*（2b-1）=△X-2*△Y；voidGridLineTraversal::gridLineCore(IntPointstart,IntPointend,GridLineTraversalLine*line){//d相当于pmintdx,dy,incr1,incr2,d,x,y,xend,yend,xdirflag,ydirflag;intcnt=0;dx=abs(end.x-start.x);dy=abs(end.y-start.y);if(dy=dx){d=dx-2*dy;incr1=-2*dy;incr2=2*(dx-dy);if(start.xend.x){x=end.x;y=end.y;ydirflag=(-1);xend=start.x;}else{x=start.x;y=start.y;ydirflag=1;xend=end.x;}line-points[cnt].x=x;line-points[cnt].y=y;cnt++;if(((end.y-start.y)*ydirflag)0){while(xxend){x++;if(d0){d+=incr1;}else{y++;d+=incr2;}line-points[cnt].x=x;line-points[cnt].y=y;cnt++;}}else{while(xxend){x++;if(d0){d+=incr1;}else{y--;d+=incr2;}line-points[cnt].x=x;line-points[cnt].y=y;cnt++;}}}else{d=dy-2*dx;incr1=-2*dx;incr2=2*(dy-dx);if(start.yend.y){y=end.y;x=end.x;yend=start.y;xdirflag=(-1);}else{y=start.y;x=start.x;yend=end.y;xdirflag=1;}line-points[cnt].x=x;line-points[cnt].y=y;cnt++;if(((end.x-start.x)*xdirflag)0){while(yyend){y++;if(d0){d+=incr1;}else{x++;d+=incr2;}line-points[cnt].x=x;line-points[cnt].y=y;cnt++;}}else{while(yyend){y++;if(d0){d+=incr1;}else{x--;d+=incr2;}line-points[cnt].x=x;line-points[cnt].y=y;cnt++;}}}line-num_points=cnt;}2、更新单元格被扫描过的总次数visits和被标记为障碍物的次数n，ScanMatcher::registerScan(ScanMatcherMap&map,constOrientedPoint&p,constdouble*readings)//匹配扫描根据Bresenham算法确定激光束扫描过的单元格，所以扫描过的单元格被扫描过的次数加1，激光束末端对应的单元格障碍物标记次数加1，并累加障碍物坐标acc.x和acc.y，则障碍物的。最后单元格是障碍物的概率p=n/visits。后续激光处理后续的激光数据要与有地形进行匹配，修正每个粒子的位置，对应的函数为GridSlamProcessor::scanMatch(constdouble*plainReading)首先计算当前位置initPose的一个分数score：计算每束激光对应的障碍物坐标phit，再计算phit对应的网格坐标iphit，激光束上与障碍物相邻的非障碍物网格为pfree，pfree的坐标由phit移动一个网格得到；然后在iphit以及周围的8个网格搜索最有可能是障碍物的网格。最有可能的判断方法为：该网格是障碍物的概率大于一个阈值，其对应的pfree是障碍物的概率小于一个阈值，并且该网格对应的障碍物坐标1.0/n*Point(acc.x,acc.y)与phit的距离d最小；最后score=score+exp(-1.0/sigma*d*d)。累加计算score，可参考NDT(normaldistributionstransform)算法。因为距离越大，score应越小，score较大值应集中在距离最小值处，这符合正态分布模型，所以使用exp来计算每束激光的score。接着对initPose进行微调，即分别轻微调整initPose的坐标和角度，计算其分数，最后选择分数最大的对应的位姿作为修正后的位姿。AMCL(自适应蒙特卡洛定位)测距仪测量模型：一、光束模型（beammodels）。1、该模型包含四种类型的误差（噪声）：较小的测量噪声、动态物体带来的误差、未探测到物体带来的误差（没有探测到物体时将使用测距仪的最大测程作为测量数据，因此也有可能不正确）、随机误差。（1）测量噪声：由测距仪的精度、大气对测量信号的影响等造成，其概率模型一般是以理想测量距离为均值的高斯模型，表示为：是从在地图上从位置沿激光束方向到障碍物的实际距离，是传感器测量距离。是归一化常数：（2）动态物体误差：由未预测到的物体（Unexpectedobjects）带来的误差。环境是动态的，而保存的地图是静态的，即不变的。没有包含在地图里的物体的出现会产生一个令人意外的比较小的测量数据。典型的动态物体就是行人。处理这些误差的一种方法就是把它当成传感器噪声。测量到的距离越大，检测到动态物体的概率越小，且小距离对应的测量到的是动态物体的概率应远大于大距离的概率，随意概率随距离的增大呈指数下降趋势，所以其概率模型为指数分布，表示为：（3）测量失败：有时候，传感器探测不到障碍物，比如试图探测一些吸收光线的物体，此时的探测数据将失效。典型的探测结果就是传感器返回自身的最大探测距离。概率模型表示为：（应该就是两点分布，即失败与没有失败）这里的是一个指示函数，当参数为真时取1，为假时取0。严格来说，这里的并不是概率密度函数，因为它是一个离散分布。但我们可以把它当成在以为中心的很小范围的均匀分布，所以也可以当作概率密度函数处理。（4）随机误差：测距仪偶尔会产生一些无法解释的测量结果。因为这种误差没有明显的特征，所以就用均匀分布来表示其概率模型。2、最终的加权概率密度模型表示如下：3、调整模型参数。