《gmapping》由会员分享，可在线阅读，更多相关《gmapping（8页珍藏版）》请在金锄头文库上搜索。
1、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 = new TNo
2、de(initialPose, 0, 0, 0);ScanMatcherMap lmap(Point(xmin + xmax, ymin + ymax) * .5, xmax - xmin,ymax - ymin, delta);for (unsigned int i = 0; i size; i+) m_particles.push_back(Particle(lmap);m_particles.back().pose = initialPose;m_particles.back().previousPose = initialPose;m_particles.back().setWeigh
3、t(0);m_particles.back().previousIndex = 0;/ this is not needed/ m_particles.back().node=new TNode(initialPose, 0, node, 0);/ we use the root directlym_particles.back().node = node;geometry_msgs/Twist.msg# This expresses velocity in free space broken into its linear and angular parts.Vector3 linearVe
4、ctor3 angular/ 获取激光在里程计坐标系上的坐标tf:Stamped odom_pose; try tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose); 处理激光数据GridSlamProcessor:processScan(const RangeReading & reading, int adaptParticles)对于第一条数据：1、 计算激光在每个粒子相应的位姿所扫描到的区域ScanMatcher:computeActiveArea(ScanMatcherMap& map, const Orien
5、tedPoint& p, const double* readings)对每一束激光束，设start为该激光束起点，end为激光束端点（障碍物位置），使用Bresenham划线算法确定激光束经过的网格，算法原理如下：设start、end所在直线方程为y = kx + b，首先通过直线的斜率确定了在x方向进行单位步进还是y方向进行单位步进：当斜率k的绝对值|k|1时，在y方向进行单位步进。 下面以|k|1时推导Bresenham算法的数学依据： 如上图，已知有一直线y = kx+b，|k|1。我们通过斜率确定了x方向为单位步进。当x = 时，y = 。那么当x 执行一个单位步进时（即x = +1
6、时）,y等于还是等于+1更符合这个直线方程呢？单凭肉眼我们很难得出结论，最好的办法当然是比较和+1和真实的方程的y值的差是多少（即Yreal = k*(+1)+b），看看哪一个更靠近真实的方程的y值。 我们设 Dupper = + 1 - Yreal = +1 - k*(+ 1)+b）; 表示+1和方程真实值的差 Ddown = Yreal - = k*(+1)+b）- ; 表示和方程真实值的差 那就是我们要比较Dupper和Ddown的大小。假设Diff = Dupper - Ddown = （+1 - k*(+1)+b） - （k*(+1)+b）- ） 令X 为线段x方向的间距（即|end
7、.x - start.x|），Y 为线段y方向的间距（|end.y - start.y|）。= X* Diff = 2*X* -2*Y* -2*Y-X*（2b-1）；那么 = +2*X*（- ）-2*Y； = + 2*X - 2*Y （ 0）其中 - 取0还是1，取决于的符号, 0取0，算法循环判断，如果小于0，则下一个点的y坐标加1。 根据等式Diff = Dupper - Ddown = （+1 - （k*(+1)+b） - （k*(+1)+b）- ）以及k = Y/X，我们可以得出起始像素（x0,y0）的参数p0的值：P0 =2*X* -2*Y* -2*Y-X*（2b-1）= X-2*Y
8、； void GridLineTraversal:gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )/ d相当于 int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag; int cnt = 0; dx = abs(end.x-start.x); dy = abs(end.y-start.y); if (dy end.x) x = end.x; y = end.y; ydirflag = (-1); xend = start
9、.x; else x = start.x; y = start.y; ydirflag = 1; xend = end.x; line-pointscnt.x=x; line-pointscnt.y=y; cnt+; if (end.y - start.y) * ydirflag) 0) while (x 0) d+=incr1; else y+; d+=incr2;line-pointscnt.x=x;line-pointscnt.y=y;cnt+; else while (x 0) d+=incr1; else y-; d+=incr2;line-pointscnt.x=x;line-po
10、intscnt.y=y;cnt+; else d = dy - 2*dx; incr1 = -2*dx; incr2 = 2 * (dy - dx); if (start.y end.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-pointscnt.x=x; line-pointscnt.y=y; cnt+; if (end.x - start.x) * xdirflag) 0) while (y
11、0) d+=incr1; else x+; d+=incr2;line-pointscnt.x=x;line-pointscnt.y=y;cnt+; else while (y 0) d+=incr1; else x-; d+=incr2;line-pointscnt.x=x;line-pointscnt.y=y;cnt+; line-num_points = cnt;2、 更新单元格被扫描过的总次数visits和被标记为障碍物的次数n，ScanMatcher:registerScan(ScanMatcherMap& map, const OrientedPoint& p, const dou
12、ble* readings)/ 匹配扫描根据Bresenham算法确定激光束扫描过的单元格，所以扫描过的单元格被扫描过的次数加1，激光束末端对应的单元格障碍物标记次数加1，并累加障碍物坐标acc.x和acc.y，则障碍物的。最后单元格是障碍物的概率p = n / visits。后续激光处理后续的激光数据要与有地形进行匹配，修正每个粒子的位置，对应的函数为GridSlamProcessor:scanMatch(const double* plainReading) 首先计算当前位置initPose的一个分数score：计算每束激光对应的障碍物坐标phit，再计算phit对应的网格坐标iphit，
13、激光束上与障碍物相邻的非障碍物网格为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(normal distributions transform)算法。因为距离越大，score应越小，score较大值应集中在距离最小值处，这符合正态分布模型，所以使用exp来计算每束激光的score。接着对initPose进行微调，即分别轻微调整initPose的坐标和角度，计算其分数，最后选择分数最大的对应的位姿作为修正后的位姿。A
