(完整word版)gmapping

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

2、 TNode(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()

3、.setWeight(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

4、 linearVector3 angular/获取激光在里程计坐标系上的坐标tf:Stamped<tf:Transform> odom_pose;trytf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);处理激光数据GridSlamProcessor:processScan(const RangeReading & reading, int adaptParticles)对于第一条数据1、计算激光在每个粒子相应的位姿所扫描到的区域ScanMatcher二computeActiveArea(ScanM

5、atcherMap&map, const OrientedPoint& p, constdouble* readings)对每一束激光束，设start为该激光束起点，end为激光束端点(障碍物位置)，使用Bresenham 划线算法确定激光束经过的网格，算法原理如下：设start、end所在直线方程为y = kx + b,首先通过直线的余率确定了在x方向进行单位步进还是y方向进行单位步进：当斜率 k的绝对值|k|<1时，在x方向进行单位步进；当斜率 k 的绝对值|k|>1时，在y方向进行单位步进。下面以|k|<1时推导Bresenham算法的数学依据：如上图，

6、已知有一直线y = kx+b, |k|<1。我们通过斜率确定了 x方向为单位步进。当x = xm 时，y= ym。那么当x执行一个单位步进时(即x = xm+1时)，y等于ym还是等于ym+1 更符合这个直线方程呢？单凭肉眼我们很难得出结论，最好的办法当然是比较 y 和y +1m m和真实的方程的 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)+

7、b) - ym;表示 ym和方程真实值的差那就是我们要比较 Dupper和Ddown的大小。假设Diff = Dupper - Ddown = ( ym+1 - k*(xm+D+b) - (k*(xm+D+b)- ym)令 X为线段x方向的间距(即|end.x - start.x| ), Y为线段y方向的间距(|end.y - start.y| )。 pm= AX* Diff = 2* AX* ym-2*AY* xm -2* Y-A X* (2b-1);那么 Pmi = Pm+2*F*（ 丫刀中-?2 3；=Pm + 2*AX - 2*AY （ Pm< 0）或者=Pm - 2*AY（ P

8、m> 0）其中ym1 - ym取0还是1,取决于pm的符号，pm>0取0,算法循环判断pm,如果 小于0,则下一个点的y坐标加1。根据等式 Diff = Dupper- Ddown = ( ym+1 - (k*( xm+1)+b) ) - (k*( xm+1)+b) - ym) 以及k = AY/AX,我们可以得出起始像素(x0,y0)的参数p0的值：P0 =2* AX* y0 -2* AY* x0 -2* Y-A X* ( 2b-1) = AX-2* AY;void GridLineTraversal:gridLineCore ( IntPoint start, IntPoint

9、 end, GridLineTraversalLine *line ) d相当于pmint 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 <= dx) d = dx - 2*dy; incr1 = - 2 * dy; incr2 = 2 * (dx - dy); if (start. x > end. x) x = end. x; y = end. y;x;x;

10、 y = start.x;cnt. x=x;cnt. y=y;y;ydirflag = (-1); xend = start. else x = start. ydirflag = 1; xend = end.line->pointsline->pointscnt+;if (end. y- start.y) * ydirflag) > 0) while (x < xend) x+;if (d > 0) d+=incr1; else y+; d+=incr2;line-> points cnt.x=x;line-> points cnt.y=y;cnt+

11、; else while (x < xend) x+;if (d > 0) d+=incr1; else y-; d+=incr2;line-> points cnt.x=x;line-> points cnt.y=y;cnt+; else d = dy - 2*dx;incrl = -2*dx; incr2 = 2 * (dy - dx); if (start. y > end. y) y = end. y; x = end. x;yend = start. xdirflag = (-1); else y = start. yend = end. xdirfla

12、g = 1;line->pointsline->pointscnt+;y;y; x = start. x; y;cnt. x=x;if (end. x-start.x) * xdirflag) >0) cnt. y=y;while (y < yend) y+;if (d > 0) d+=incr1; else x+; d+=incr2;line-> points cnt.x=x;line-> points cnt.y=y;cnt+; else while (y < yend) y+;if (d > 0) d+=incr1; else x-;

13、 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, const double * readings) / 匹配扫描 根据Bresenham算法确定激光束扫描过的单元格，所以扫描过的单元格被扫描 过的次数加1,激光束末端对应的单元格障碍

14、物标记次数加1,并累加障碍物坐标acc.x和acc.y ,则障碍物的。最后单元格是障碍物的概率p = n / visits 。后续激光处理后续的激光数据要与有地形进行匹配，修正每个粒子的位置，对应的函数为 GridSlamProcessor二 scanMatch( const double * plainReading)首先计算当前位置initPose的一个分数score :计算每束激光对应的障碍物 坐标phit ,再计算phit对应的网格坐标iphit ,激光束上与障碍物相邻的非障 碍物网格为pfree , pfree的坐标由phit移动一个网格得到；然后在iphit以及 周围的8个网格搜索

15、最有可能是障碍物的网格。最有可能的判断方法为：该网格是障碍物的概率大于一个阈值，其对应的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 o接着对initPo

16、se进行微调，即分别轻微调整initPose的坐标和角度，计算 其分数，最后选择分数最大的对应的位姿作为修正后的位姿AMCL自适应蒙特卡洛定位)测距仪测量模型：一、光束模型(beam models)。1、该模型包含四种类型的误差(噪声)：较小的测量噪声、动态物体带来的误 差、未探测到物体带来的误差(没有探测到物体时将使用测距仪的最大测程作为 测量数据，因此也有可能不正确)、随机误差。(1)测量噪声：由测距仪的精度、大气对测量信号的影响等造成，其概率模型 一般是以理想测量距离为均值的高斯模型，表示为：Phit（z，| x（,m）=f n "（士 琢*，咯t） if。W 4 W Wmax

17、1 0otherwisezt是从在地图了门上从位置If沿激光束方向到障碍物的实际距离，zt是传感器测量距离。其 it）=n是归一化常数:(2)动态物体误差：由未预测到的物体(Unexpected objects)带来的误差。环 境是动态的，而保存的地图是静态的，即不变的。没有包含在地图里的物体的出 现会产生一个令人意外的比较小的测量数据。典型的动态物体就是行人。处理这些误差的一种方法就是把它当成传感器噪声。测量到的距离越大，检测到动态物 体的概率越小，且小距离对应的测量到的是动态物体的概率应远大于大距离的概 率，随意概率随距离的增大呈指数下降趋势，所以其概率模型为指数分布，表示为：Pshcrt

18、.(i' I1? Ashortfid if 0 << 若"0otherwiseAs in the previous case, we need a normalizer q since our exponential is liinited to the interval 0;艺f . Because the cumulative probability in this interval is given asAshort eAahortif dZt = 一旧-入M国* + 旧-h3to Jo一e一Amhortthe value of rj can be deri

19、ved as：_ 1(3)测量失败：有时候，传感器探测不到障碍物，比如试图探测一些吸收光线 的物体，此时的探测数据将失效。典型的探测结果就是传感器返回自身的最大探 测距离。概率模型表示为：(应该就是两点分布，即失败与没有失败)We will model this case with a point-mass distribution centered atzGD = «z=z = J k蒸max，为中心的很小这里的是一个指示函数，当参数为真时取 1,为假时取0。严格来说，这里的 PmaK并不是概率密度函数，因为它是一个离散分布。但我们可以把它当成在以 范围的均匀分布，所以也可以当作概率密度函数处理。(4)随机误差：测距仪偶尔会产生一些无法解释的测量结果。Random measurements. Finally, range finders occasionally produce entirely unexplamable measurements. For example, sonars oft