AMCL的关键--Cluster

转自知乎: https://zhuanlan.zhihu.com/p/59411695

基本原理：
首先关注的是粒子滤波器的基本原理。对于每个粒子滤波器对象都有两个粒子集合，该粒子集合是为后续Resampling时，能将其中一个粒子集合作为缓存集合使用。但是由于每个粒子集合中包含的粒子数量很大（比如5000个粒子，可自定义），因此每个粒子的权重即使在多次Resampling之后都还是很小，并且这也将导致位置上相邻的粒子的权重差值不大，从而使粒子滤波器在最后输出预测位姿时不准确。

为了解决上面提到的问题，在Ros的代码中便引入了一个新的结构体（Cluster）。Cluster是相邻粒子的一个集合，其具有权重与位姿等属性，与粒子相似。而其位姿是通过该Cluster中包含的粒子的位姿加权平均得到。由于初始化时粒子在地图上的位置是通过均匀分布得到，所以此时Cluster的数量与粒子数量相等，即每一个Cluster中只有一个粒子。 而当粒子在地图上产生聚合后，Cluster的数量就会急剧下降，此时选取权重最大的Cluster作为粒子滤波器的输出结果。

image.png

为了能高效存储粒子信息，并且能将相邻的粒子进行聚类分析，因此需要引入KD-Tree数据结构。

首先是构建KD-Tree时先将粒子的三个位姿信息进行放大，将放大后的位姿信息当作KD-Tree中节点的key值。 在程序中放大系数为key = [图片上传失败...(image-44b35e-1575537946796)]

（见函数pf_kdtree_alloc）。

接着将插入的第一个节点作为根节点（root），而将之后插入的第二个节点的key值与根节点的key值做差，选取差的绝对值中最大的那个key，并将这个key的维度定义为Pivot dimension, 并且将这两个节点在Pivot dimension上的key值的平均值设为这一层根节点的Pivot Value，当第二个节点在Pivot Dimension 的 key值小于Pivot Value，则将该节点插入该树第一层根节点的左边，而根节点将被插入该树第二层的右边，如果大于则相反。之后插入新的粒子时便循环执行上述步骤，直到所有粒子插入完毕。此时kd-tree中叶节点的个数与粒子的个数相等且为n，而整个kd-tree中的节点个数约为2n。

当kd-tree生成之后便可以对Resampling之后的粒子进行聚类。 而聚类算法是通过pf_kdtree_cluster_node函数来实现的（代码见下面）。 该函数在粒子初始化或者每次Resampling之后都会调用，以确保粒子的聚合状态得以更新。

以下着重解释一下pf_kdtree_cluster_node函数如何将相邻的粒子进行聚类分析。首先node作为基准节点，通过for循环将node的三个key值分别加是上-1，0，1。

nkey[0] = node->key[0] +[-1, 0, 1];
nkey[1] = node->key[1] + [-1, 0, 1];
nkey[2] = node->key[2] + [-1, 0, 1];

也就是共有27种可能（每个key有三种维度, 即x, y, yaw。 每个维度又要加上三种可能，即加上（-1）， 0， 或1。 因此共333有种可能）。

接下来将这27种新的key值（即nkey）代入函数pf_kdtree_find_node（返回NULL指针则证明没有该节点）中查找现有kd-tree中是否已经有节点的key值与该新的key（即nkey）值相等，如果有则将找到的节点即（nnode）的cluster与基准节点（node）的进行统一。接着将nnode作为基准节点再次寻找nnode周围是否还存在粒子。最后相邻的粒子的cluster都是一样的，从而实现了将粒子聚类。

void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth)
{
 int i;
 int nkey[3];
 pf_kdtree_node_t *nnode;

 for (i = 0; i < 3 * 3 * 3; i++)
  {
    nkey[0] = node->key[0] + (i / 9) - 1;
    nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
    nkey[2] = node->key[2] + ((i % 9) % 3) - 1;

    nnode = pf_kdtree_find_node(self, self->root, nkey);
 if (nnode == NULL)
    {
 continue;
    }
 // This node already has a label; skip it.  The label should be
 // consistent, however.
 if (nnode->cluster >= 0)
    {
 continue;
    }

 // Label this node and recurse
    nnode->cluster = node->cluster;

 pf_kdtree_cluster_node(self, nnode, depth + 1);
  }
 return;
}