GMapping的扫描匹配器
在分析GMapping的粒子滤波框架的时候，我们知道扫描匹配是GMapping进行SLAM建图的一个重要环节。 它存在的作用主要是，在预测机器人位姿的时候，参考激光传感器的扫描数据，以达到改进粒子滤波器中的建议分布的目的。
在GMapping中，专门定义了一个扫描匹配器ScanMatcher。 在ROS的gmapping_demo中，有两个地方用到了这个类型的对象。在建图引擎gsp_中以成员变量存在的匹配器m_matcher， 在ROS封装中更新地图的函数中以局部变量存在的匹配器matcher。本文中研究的是建图引擎中的匹配器， 讨论GMapping的扫描匹配过程，及其改进的建议分布。
1. 建图引擎的scanMatch过程
在GMapping的粒子滤波框架中，每个迭代过程， 我们都需要以当前最新的控制量和测量值\(\langle u_t, z_t \rangle\)作为输入。并对样本集合中的每个粒子，依次完成如下的操作：
- 采样(Sampling): 根据系统的状态方程\(x_t = g_t(u_t, x_{t-1}) + \varepsilon_t\)，在\(t-1\)时刻粒子\(x_{t-1}^{(i)}\)的基础上， 对\(t\)时刻的粒子进行预测得到\(x_t^{\prime(i)}\)。与传统RBPF不同的是，这一预测过程并不是对建议分布\(\pi\)的采样。
- 扫描匹配(Scan-Matching): 以某种寻优的方法来求取\(\hat{x}_t^{(i)} = \mathrm{argmax}_x p\left(x | m_{t-1}^{(i)}, z_t, x_t^{\prime(i)}\right)\)， 同时计算得到一个匹配度，如果匹配度小于一个指定的阈值，认为匹配失败。
- 匹配成功的粒子更新: 如果扫描匹配成功，说明传感器数据可信。在\(\hat{x}_t^{(i)}\)的邻域内计算对\(z_t\)最佳匹配下的建议分布的均值\(\mu_t^{(i)}\)和方差\(\Sigma_t^{(i)}\)， 并采样得到修正后的样本\(x_t{(i)} \sim \mathcal{N}\left(\mu_t^{(i)}, \Sigma_t^{(i)}\right)\)，同时计算样本的权重\(w_t^{(i)}\)。
- 匹配失败的粒子更新: 如果扫描匹配失败，说明传感器数据精度较低。我们仍然采用传统RBPF的方式更新粒子的机器人状态和权重。
- 估计地图(Map Estimation): 对于每个粒子，根据更新的机器人状态\(x_t^{(i)}\)以及观测值\(z_t\)更新地图信息\(m^{(i)}\)。
在代码中，我们能够看到这些操作的影子，但是并不是完全按照上述内容实现。上述内容作为总体思想，对于我们理解代码有帮助。 其中粒子更新过程的第一步已经由GMapping的运动模型搞定了， 剩下的内容都是由扫描匹配器ScanMatcher来完成。 建图引擎的成员函数scanMatch用来整合这些内容。 下面是该函数的代码片段，它以传感器的数据作为输入，在函数体中遍历建图引擎的粒子集合。
inline void GridSlamProcessor::scanMatch(const double* plainReading) {
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++) {
OrientedPoint corrected;
double score, l, s;
匹配器的函数optimize是一种爬山算法，用于寻找\(\hat{x}_t^{(i)} = \mathrm{argmax}_x p\left(x | m_{t-1}^{(i)}, z_t, x_t^{\prime(i)}\right)\)，完成对建议分布的采样。 其中corrected是一个引用方式的传参，该函数最终退出之后，corrected所记录的则是\(\hat{x}_t^{(i)}\)。 此外，该函数返回的是一个匹配度，记录在局部变量score中。建图引擎的成员变量m_minimumScore是一个匹配度阈值，当超过该阈值时，我们认为匹配成功，使用\(\hat{x}_t^{(i)}\)作为更新样本。 否则，我们仍然使用传统的运动模型下的预测状态作为更新样本。
score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
if (score>m_minimumScore)
it->pose=corrected;
完成了对建议分布的采样之后，我们需要计算各个样本的权重。函数likelihoodAndScore计算了更新样本状态的对数似然度，可以通过累加的形式完成样本权重的更新。
m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
it->weight+=l;
it->weightSum+=l;
最后调用函数invalidateActiveArea和函数computeActiveArea，根据机器人的状态和传感器数据计算待更新的地图。函数名中的ActiveArea一词的意义应该就是描述待更新的区域，故称之为Actived。
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
}
}
2. 爬山寻优
扫描匹配器通过函数optimize进行爬山寻优，求取更新样本\(\hat{x}_t^{(i)} = \mathrm{argmax}_x p\left(x | m_{t-1}^{(i)}, z_t, x_t^{\prime(i)}\right)\)。爬山算法是一种局部寻优的算法， 它贪婪的与周围邻接的节点进行比较，并以其中取得最大值的节点作为下一次迭代的起点，直到周围邻接的节点中没有比当前节点更大时结束迭代。整个过程就像爬山一样，一步步向山顶靠近。
2.1 总体寻优思路
在GMapping的粒子滤波框架的式(6)中，使用了一个高斯分布来对建议分布近似。 而高斯分布是一个连续的单峰分布，所以这里可以使用爬山算法来寻找这个唯一的山峰。下面是函数optimize的代码片段，它有四个参数。其中，pnew用于记录寻优的结果\(\hat{x}_t^{(i)}\)， map是粒子维护的地图，init则是经过运动模型采样之后的位姿状态\(x_t^{\prime(i)}\)，readings是激光传感器的扫描数据。该函数的返回值记录了匹配度，该值越小匹配程度越低。
double ScanMatcher::optimize(OrientedPoint& pnew,
const ScanMatcherMap& map,
const OrientedPoint& init,
const double* readings) const {
在函数的一开始定义了一些局部变量，用于爬山算法运行过程中记录一些临时数据，我们在注释中添加了对这些局部变量的解释。
double bestScore = -1; // 搜索过程中的最优匹配度
OrientedPoint currentPose = init; // 爬山算法当前考察节点
double currentScore = score(map, currentPose, readings); // 当前考察节点的匹配度
double adelta = m_optAngularDelta, ldelta = m_optLinearDelta; // 爬山步长
unsigned int refinement = 0; // 优化次数
int c_iterations=0; // 迭代次数
在optimize函数中还定义了一个枚举类型，用于搜索过程中表述爬山方向。
enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
接下来，开始爬山。在爬山过程中，如果周围存在比当前考察节点更优的节点，就更新变量currentPose及其匹配度currentScore。 其迭代过程的终点就是当前匹配度达到最大，或者优化次数超出了指定的迭代次数。完成爬山寻优之后，将局部最优的位姿状态和匹配度返回。
do {
// 爬山过程
} while (currentScore > bestScore || refinement < m_optRecursiveIterations);
pnew = currentPose;
return bestScore;
}
2.2 爬山过程
在每次迭代之初，都对比一次当前的最优匹配度和上次迭代时考察节点的匹配度。如果匹配度得到了提升，说明上次迭代找到了一个更优的解。 我们记录下优化次数，同时调整爬山步长。这里之所以要调整爬山步长，是为了降低解在山峰周围来回反复的情况出现的机率。
if (bestScore>=currentScore) {
refinement++; // 实际上，我严重怀疑这里是一个BUG
adelta*=.5; // 因为，进行迭代的条件之一就是currentScore > bestScore
ldelta*=.5; // 所以，这个分支基本上是不可能执行的
}
接下来准备用于爬山迭代过程的数据，用变量bestScore记录下当前最优的匹配度，并定义局部变量bestLocalPose记录当前最优的节点，localPose则是用于遍历当前节点周围邻接节点的局部变量。 局部变量move记录了爬山的方向。
bestScore = currentScore;
OrientedPoint bestLocalPose = currentPose;
OrientedPoint localPose = currentPose;
Move move = Front;
准备好数据之后，就可以通过一个循环来遍历邻接节点，找到其中最优的节点并更新变量bestLocalPose和currentScore。完成遍历之后，就使用当前最优的节点更新变量currentPose。
do {
// 遍历邻接节点
} while (move != Done);
currentPose = bestLocalPose;
2.3 遍历邻接节点
GMapping使用的是一种有限状态机的机制来遍历邻接节点的。在运行状态机之前，先用临时变量localPose记录下当前考察节点。然后在switch语句中根据move所指的爬山方向，修改localPose， 同时指定下一个遍历循环中将要考察的爬山方向。在该状态机中，依次访问currentPose的前、后、左、右、左转、右转6个邻接节点。
localPose = currentPose;
switch (move) {
case Front: localPose.x += ldelta; move = Back; break;
case Back: localPose.x -= ldelta; move = Left; break;
case Left: localPose.y -= ldelta; move = Right; break;
case Right: localPose.y += ldelta; move = TurnLeft; break;
case TurnLeft: localPose.theta += adelta; move = TurnRight; break;
case TurnRight: localPose.theta -= adelta; move = Done; break;
default:;
}
然后，为每个邻接节点计算一个匹配度。
double odo_gain=1;
// odo_gain = ..., 根据里程计的一些置信度参数计算odo_gain, 默认配置下不会修改，所以代码略
double localScore=odo_gain*score(map, localPose, readings);
最后，如果邻接节点的匹配度大于当前节点的匹配度，就记录下来。并增加迭代计数。
if (localScore > currentScore){
currentScore=localScore;
bestLocalPose=localPose;
}
c_iterations++;
在整个爬山过程中，我们反复用到了一个函数score来求取匹配度。该函数的实现过程与函数likelihoodAndScore基本一样，只是缺少了计算似然度的操作。 下面我们详细解释函数likelihoodAndScore的实现，不再讨论函数score了。
3. 样本似然度和匹配度
函数likelihoodAndScore完成了评价地图、传感器数据、机器人位姿的匹配度，并计算了位姿的对数似然度。它有5个参数，其中参数s和l是两个输出参数，分别用于返回匹配度和对数似然度。 参数map，p，和readings评价的对象，分别描述了待匹配的地图、机器人位姿、激光传感器的数据。
inline unsigned int ScanMatcher::likelihoodAndScore(double& s,
double& l,
const ScanMatcherMap& map,
const OrientedPoint& p,
const double* readings) const {
首先，为输出参数赋予初值0。同时使用指针angle获取传感器各个扫描数据所对应的转角。 扫描匹配器的成员变量m_laserAngles在建图引擎的初始化过程中就已经通过函数setLaserParameters进行了设定。 m_initialBeamsSkip默认情况下为0，必要时也可以使用函数setMatchingParameters进行配置。
l=0; s=0;
const double *angle = m_laserAngles + m_initialBeamsSkip;
然后创建一个临时变量lp将考察位姿p转换到地图坐标系下。
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
此外还需要定义一些临时变量。
double noHit=nullLikelihood/(m_likelihoodSigma); // 没有匹配的似然概率值
unsigned int skip = 0; // 用于对传感器数据筛选的计数器
unsigned int c = 0; // 匹配计数器
double freeDelta = map.getDelta()*m_freeCellRatio; // freeDelta用于确定空闲节点的参数
接下来，我们遍历传感器的读数。有时可能因为传感器的数据比较多，为了提高计算速度，我们需要每个几个数据处理一次。这里用skip作为数据跳跃计数器，当达到跳跃步长m_likelihoodSkip时， 将之清零，并进行一次匹配操作。此外《Probabilistic Robotics》中提到， 在使用超声、激光等传感器测距的时候，一般都会认为传感器的数据是不可能超过一个固定值的，即传感器的量程。所以这里也用成员变量m_usableRange对超出量程的数据进行筛选。
for (const double* r=readings+m_initialBeamsSkip; r < readings+m_laserBeams; r++, angle++){
skip++;
skip = skip > m_likelihoodSkip ? 0 : skip;
if (skip || *r > m_usableRange)
continue;
然后根据扫描数据的距离读数和角度创建一个点phit，及其在地图中的索引点，该点在地图中应当是被占用的。
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint iphit=map.world2map(phit);
接着，我们沿着扫描波束所指的方向构建一个空闲点。我们知道激光传感器测量的是扫描波束所指的最近障碍的距离，那么从波束的起点到测量值所在位置中间的这段区域就应当是空闲的。 由于传感器的读数存在噪声，所以我们需要减去一个合适的值作为空闲区域的判定标准。
Point pfree=lp;
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
下面我们定义两个临时变量。found用于记录在phit的一个邻域内有匹配点。bestMu是一个二维向量用于记录匹配点到考察点之间的距离向量，该向量的长度越小说明两个点越接近，匹配度就越高。
bool found=false;
Point bestMu(0.,0.);
我们在点phit的一个邻域内搜索匹配点。点pr到pf的连线理论上与点iphit到ipfree之间的连线平行。
for (int xx = -m_kernelSize; xx <= m_kernelSize; xx++)
for (int yy = -m_kernelSize; yy <= m_kernelSize; yy++) {
IntPoint pr=iphit+IntPoint(xx,yy);
IntPoint pf=pr+ipfree;
从GMapping的地图结构中获取pr和pf两个点的元素。类PointAccumulator用于用于记录地图坐标的累积更新过程， 可以直接返回对应栅格的占用概率、均值和熵。
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.cell(pf);
阈值m_fullnessThreshold是用于判定栅格被占用的阈值。当我们构建的占用点所对应的cell被占用，空闲点所对应的fcell是空闲的，我们认为匹配了一个点。 然后在变量found留下匹配记录，遍历了整个邻域之后，bestMu将记录下最佳匹配点到地图栅格中心的矢量差，该矢量的长度越小匹配程度就越高。
if (((double)cell) > m_fullnessThreshold && ((double)fcell) < m_fullnessThreshold) {
Point mu=phit-cell.mean();
if (!found){
bestMu=mu;
found=true;
}else
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
}
}
如果考察的扫描点匹配了，我们就累加点的匹配度，当考察了所有的扫描数据点之后，累加变量s就记录了当前激光扫描的匹配度，c则对匹配点进行累加。
if (found) {
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
c++;
}
只要扫描点不被跳过，我们就在一个高斯分布的假设下计算该点的对数似然度，对所有匹配点的累加值l将用于更新粒子的权重。在函数的最后返回匹配点数量。
if (!skip){
double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
l+=(found)?f:noHit;
}
}
return c;
}
4. 地图更新
完成粒子的采样和权值计算之后，我们就需要更新各个粒子所记录的地图。实际上函数computeActiveArea并没有完成对地图的更新，它只是调整了地图的大小，并记录下需要更新的区域。 这个函数的功能很简单，但是源码写的十分冗长，这里不再浪费篇幅展开细讲。
下面只给出伪代码化的computeActiveArea。该函数有三个参数，其中map是需要更新的地图， 在该函数正常退出之后，map将记录下需要更新的区域。p是粒子所记录的机器人状态，readings则记录了激光传感器的扫描值。
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings) {
if (m_activeAreaComputed)
return;
// 计算地图的边界尺寸
Point min(map.map2world(0,0));
Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
UpdateMargin(min, max);
// 如果边界尺寸
if ( !map.isInside(min) || !map.isInside(max)) {
// 添加边界
Boundary(min, max);
map.resize(min.x, min.y, max.x, max.y);
}
// 遍历激光传感器数据
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
angle = m_laserAngles + m_initialBeamsSkip;
for (const double* r = readings + m_initialBeamsSkip; r < readings + m_laserBeams; r++, angle++) {
// 筛选合适传感器数据
if (*r > m_laserMaxRange || *r > m_usableRange || *r==0.0 || isnan(*r))
continue;
// 计算占用栅格
IntPoint p1 = CalOccupancy(*r, *angle, lp);
IntPoint cp = map.storage().patchIndexes(p1);
activeArea.insert(cp);
}
map.storage().setActiveArea(activeArea, true);
m_activeAreaComputed = true;
}
实际更新地图的操作是放在了重采样之后进行的。这是可以理解的，因为重采样过程中， 会删除一些权重较小的粒子，如果我们还对这样的粒子更新地图，就有些浪费计算资源了。我们在计算了激活态的区域之后，就可以调用函数registerScan来将扫描的占用信息实际写到地图中。 下面是函数registerScan的代码片段，它的参数列表与computeActiveArea一样，需要输入待更新的地图、机器人位姿和激光传感器的读数。
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings) {
在函数的一开始，我们先确保已经计算过激活区域，再申请必要的存储空间。
if (!m_activeAreaComputed)
computeActiveArea(map, p, readings);
map.storage().allocActiveArea();
接下来将机器人位姿转换到地图坐标系下，并计算栅格单元索引坐标p0。
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
下面，我们遍历所有扫描数据。在其第14行中，先对扫描数据进行一些筛选，保证更新的扫描数据再量程内并且有意义。在17到20行构建占用栅格坐标，最后在第22行中， 通过地图的接口将占用栅格信息写到地图中。
const double * angle=m_laserAngles+m_initialBeamsSkip;
double esum=0;
for (const double *r = readings + m_initialBeamsSkip; r < readings + m_laserBeams; r++, angle++) {
// 省略源码中分支if (m_generateMap)
if (*r > m_laserMaxRange || *r > m_usableRange || *r == 0.0 || isnan(*r))
continue;
// 构建占用栅格坐标
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
map.cell(p1).update(true,phit);
}
}
return esum;
5. 完
本文中，我们介绍了GMapping的扫描匹配过程。该过程是GMapping不同于传统RBPF的一个重要特点，它在对建议分布进行采样的时候，考虑了激光传感器的读数，可以提高准确度， 减少粒子数量。