(完整word版)gmapping完整版.docVIP

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=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);

//weusetherootdirectly

m_particles.back().node=node;

}

geometry_msgs/Twist.msg

#Thisexpressesvelocityinfreespacebrokenintoitslinearandangularparts.

Vector3linear

Vector3angular

//获取激光在里程计坐标系上的坐标

tf::Stampedtf::Transformodom_pose;

try

{

tf_.transformPose(odom_frame_,centered_laser_pose_,odom_pose);

}

处理激光数据

GridSlamProcessor::processScan(constRangeReadingreading,intadaptParticles)

对于第一条数据：

计算激光在每个粒子相应的位姿所扫描到的区域

ScanMatcher::computeActiveArea(ScanMatcherMapmap,constOrientedPointp,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=时，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*△X*-2*△Y*-2*△Y-△X