gmapping.doc

Gmapping(fastSlam) 获取激光数据并处理 如果是第一个激光数据，则初始化粒子，每个粒子维护一幅地图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 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().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 directly m_particles.back().node = node; } geometry_msgs/Twist.msg # This expresses velocity in free space broken into its linear and angular parts. Vector3 linear Vector3 angular // 获取激光在里程计坐标系上的坐标 tf::Stampedtf::Transform odom_pose; try { tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose); } 处理激光数据 GridSlamProcessor::processScan(const RangeReading reading, int adaptParticles) 对于第一条数据： 计算激光在每个粒子相应的位姿所扫描到的区域 ScanMatcher::computeActiveArea(ScanMatcherMap map, const OrientedPoint p, const double* 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 = 时，y = 。那么当x 执行一个单位步进时（即x = +1时）,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.x - start.x|），△Y 为线段y方向的间距（|end.y - start.y|）。= △X* Diff = 2*△