gmapping(fast_slam)

gmapping获取激光数据及处理

如果是第一束激光数据，则初始化粒子。
Particle的信息有：
每个粒子维护一幅地图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，分别对应地图横坐标最小值和最大值、纵坐标最小值和最大值。

Point m_center;
double m_worldSizeX, m_worldSizeY, m_delta;
Storage m_storage;
int m_mapSizeX, m_mapSizeY;
int m_sizeX2, m_sizeY2;

粒子初始化，初始信息都相同

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;

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

tf::Stamped<tf::Transform> odom_pose;
  try
  {
    tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
  }

处理激光数据
GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)

对于每束激光的处理

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

void 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方向进行单位步进。

至于具体的算法内容请百度。

void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )

上面API功能为已知start、end,求解激光束所经过的所有栅格，并存储在line中，line定义如下：

typedef struct {
  int     num_points;
  IntPoint*  points;
} GridLineTraversalLine;

2.更新单元格被扫描过的总次数visits和被标记为障碍物的次数n

double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings)

根据Bresenham算法确定激光束扫描过的单元格，所以扫描过的单元格被扫描过的次数加1，激光束末端对应的单元格障碍物标记次数加1，并累加障碍物坐标acc.x和acc.y，最后单元格是障碍物的概率p = n / visits。这个函数还涉及到了栅格障碍物的信息熵，下面贴出

1948年，香农Claude E. Shannon引入信息（熵），将其定义为离散随机事件的出现概率。一个系统越是有序，信息熵就越低；反之，一个系统越是混乱，信息熵就越高。所以说，信息熵可以被认为是系统有序化程度的一个度量。
这里的话一个栅格越倾向于占用或者未被占用的状态，她的信息熵越低。

double PointAccumulator::entropy() const{
	if (!visits)
		return -log(.5);
	if (n==visits || n==0)
		return 0;
	double x=(double)n*SIGHT_INC/(double)visits;
	return -( x*log(x)+ (1-x)*log(1-x) ); 
}

3.后续激光处理
后续的激光数据要与有地形进行匹配，修正每个粒子的位置，对应的函数为

GridSlamProcessor::scanMatch(const double* plainReading)

scanMatch的部分这里大致介绍下，下一篇博客会详细介绍。
首先计算当前位置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(normal distributions transform)算法。因为距离越大，score应越小，score较大值应集中在距离最小值处，这符合正态分布模型，所以使用exp来计算每束激光的score。
接着对initPose进行微调，即分别轻微调整initPose的坐标和角度，计算其分数，最后选择分数最大的对应的位姿作为修正后的位姿。

来源：CSDN

作者：黄大大大康

链接：https://blog.csdn.net/weixin_43933310/article/details/103894295