amcl的粒子滤波器
amcl全称自适应的蒙特卡洛定位(Adaptive Monte Carlo Localization)，使用粒子滤波器(Particle Filter, PF)在已知地图的情况下定位机器人位姿。 它的自适应体现在KLD采样的应用上，通过Kullback-Leibler Divergence计算两个概率分布之间的差异，得到一个评价样本质量的统计边界， 并根据这一边界确定粒子数量，详细参见《P.R.》的第八章。
在本系列的粒子滤波器一文中，我们提到典型的PF需要经过"递推样本"、“权重计算”、“重采样”三个阶段。 在分析amcl的总体业务逻辑时， 我们看到"递推样本"的工作是由里程计对象完成的， 激光雷达对象则完成了"权重计算"的工作。 在本文中，我们主要讨论amcl的粒子滤波器框架，以及重采样过程。
1. 数据结构的定义
amcl定义了如下代码所示的结构体pf_t来描述滤波器，我们在注释中说明了各个字段的含义。 值得注意的是，字段pop_err和pop_z是两个可以配置的参数，它们是KLD的体现。其中pop_err约束了是真实分布与估计分布之间的最大误差， pop_z则是一个z分位数限定了估计分布误差小于pop_err的概率。此外，amcl定义了两个样本集合，并使用current_set指示当前激活的集合索引。
参数alpha_slow和alpha_fast的配置，决定了字段w_slow和w_fast两个平均权重过滤值的衰减速率。这些字段用于触发recover机制，增加随机位姿， 用以削弱粒子滤波器的粒子匮乏问题的影响。
最后dist_threshold和converged是用来评定粒子滤波器是否收敛的两个字段。对于定位而言，只有当粒子集合收敛到了一个很小的区域，其不确定度才能保证比较小。
typedef struct _pf_t {
int min_samples, max_samples; // 最少和最多样本数量
double pop_err, pop_z; // 样本集尺寸参数
int current_set; // 当前激活态的样本集索引
pf_sample_set_t sets[2]; // 样本集合,双缓存
double w_slow, w_fast; // running averages
double alpha_slow, alpha_fast; // decay rates for running averages
pf_init_model_fn_t random_pose_fn; // 绘制样本集合的函数指针
void *random_pose_data;
double dist_threshold; // 判定粒子集合发散的阈值
int converged; // 粒子集合收敛的标志
} pf_t;
下面的左侧代码是粒子集合的数据结构定义。其中sample_count和samples分别记录了样本数量和样本，kdtree则用于描述样本的统计直方图。 字段mean和cov分别描述了当前粒子集合的平均位姿和协方差，它们是对当前机器人位姿和不确定度的估计。最后用字段converged来指示粒子集合是否收敛。
在介绍粒子滤波器的时候，我们就提到过，PF所用的蒙特卡洛估计方法，是一种非参数估计方法， 可以描述几乎任何概率分布。针对多峰的分布，这里定义了pf_cluster_t和字段clusters来显式的描述，我们把描述各个峰的子集称为粒子簇。 此外cluster_count和cluster_max_count分别描述了当前粒子簇的数量和最大粒子簇数量。
|
|
上面的右边代码给出了粒子簇和粒子的数据结构。虽然粒子簇也是粒子的集合，它只是描述了簇的粒子数量、概率权重、均值方差等统计信息。而对于每个粒子，我们都有字段pose描述位姿， weight用于描述粒子的概率权重。
2. 粒子滤波器的初始化
回顾amcl的总体业务逻辑，可以看到我们需要调用函数pf_alloc来构建滤波器对象， 然后调用函数pf_init完成初始化。
2.1 pf_alloc
下面是函数pf_alloc的代码片段，我们省略了大量的初值语句，右图则是amcl_node.cpp文件中调用该函数的语句。该函数有6个输入参数，前四个是对滤波器的配置，约束了最少和最多的样本数量，权重过滤值衰减速率。 参数random_pose_fn是一个函数指针，提供了生成随机样本的函数，amcl中使用的是AmclNode::uniformPoseGenerator。对于参数random_pose_data，我的理解是粒子的样本空间， amcl实际使用的是地图对象。
pf_t *pf_alloc(int min_samples, int max_samples, double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void *random_pose_data) {
在函数的一开始调用了C语言的库函数srand48完成随机数种子的初始化。并通过calloc申请滤波器对象的内存。
srand48(time(NULL));
pf = calloc(1, sizeof(pf_t));
接着通过一个for循环完成两个粒子集合的初始化工作。并在7到13行中的为每个粒子集申请内存，并赋予初值。在第14行中构建了三倍于样本集合尺寸的直方图对象kdtree。 最后完成粒子簇的内存申请和均值方差的初始化。
for (j = 0; j < 2; j++) {
set = pf->sets + j;
set->sample_count = max_samples;
set->samples = calloc(max_samples, sizeof(pf_sample_t));
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
sample->pose.v[0] = 0.0; sample->pose.v[1] = 0.0; sample->pose.v[2] = 0.0;
sample->weight = 1.0 / max_samples;
}
set->kdtree = pf_kdtree_alloc(3 * max_samples);
set->cluster_count = 0;
set->cluster_max_count = max_samples;
set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
}
该函数的最后，通过函数pf_init_converged设定滤波器未收敛，并返回滤波器对象。
pf_init_converged(pf);
return pf;
}
2.2 pf_init
下面的代码片段是函数pf_init的实现。该函数有三个参数，其中pf是将要初始化的滤波器对象，mean和cov分别是机器人初始位姿和协方差描述。在函数中，首先通过索引current_set获取当前激活的粒子集合。 并根据输入的均值和方差构建一个高斯分布。
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov) {
pf_sample_set_t *set = pf->sets + pf->current_set;
pf_pdf_gaussian_t *pdf = pf_pdf_gaussian_alloc(mean, cov);
首先清空直方图，并根据刚刚构建的正态分布采样，填充粒子集合更新统计直方图。
pf_kdtree_clear(set->kdtree);
set->sample_count = pf->max_samples;
for (int i = 0; i < set->sample_count; i++) {
pf_sample_t *sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = pf_pdf_gaussian_sample(pdf);
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
pf_cluster_stats(pf, set);
最后释放掉临时申请的高斯分布对象pdf，并标记为未收敛。
pf->w_slow = pf->w_fast = 0.0;
pf_pdf_gaussian_free(pdf);
pf_init_converged(pf);
return;
}
除了函数pf_init之外，amcl还提供了一个称为pf_init_model的函数，其实现如下所示。它完成的功能与pf_init基本是一样的，只是pf_init使用的是高斯模型，pf_init_model可以由用户提供模型。 它有三个参数，其中pf仍然是滤波器对象。init_fn是一个函数指针，用户需要提供一个生成随机样本的函数实现，该参数的作用就相当于pf_init中的高斯分布对象。参数init_data则是生成粒子的样本空间， 我们可以为之赋予地图数据。
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data) {
// ...
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = (*init_fn) (init_data);
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
// ...
}
4. 重采样
根据《P.R.》的介绍， 我们知道如果我们按照粒子滤波器的常规操作，对完成"递推样本"、“权重计算”操作之后的粒子集合进行重采样， 就可以进行全局定位了，但是它还不能够解决机器人绑架的问题。《P.R.》提到了一种注入随机粒子的方法可以解决这一问题，并给出了 amcl所用的Augmented_MCL算法。概算法的核心思想就是， 通过对比短期和长期样本的均值估计来确定注入随机粒子的数量，并在重采样过程中注入新的随机粒子。
4.1 pf_update_sensor
amcl将这一核心思想差分为pf_update_sensor和pf_update_resample两个函数来实现了。下面是pf_update_sensor函数的代码片段，它完成的是短期和长期样本均值的估计， 也就是字段w_slow和w_fast的计算。该函数在测量更新的过程中被调用。 它有三个参数，其中pf是滤波器对象，sensor_fn则是一个函数指针实现了传感器的模型，用于计算各个粒子的概率权重。参数sensor_data则是用于更新的传感器数据。
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data) {
在该函数中，首先获取当前激活的样本集合，并通过函数指针sensor_fn计算各个样本的概率权重。
pf_sample_set_t *set = pf->sets + pf->current_set;
double total = (*sensor_fn) (sensor_data, set);
接下来首先完成样本权重的归一化。
pf_sample_t *sample;
double w_avg=0.0;
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
w_avg += sample->weight;
sample->weight /= total;
}
w_avg /= set->sample_count;
最后按照右边算法伪代码中的第10和11行的公式，计算统计量\(w_{slow}, w_{fast}\)。
if(pf->w_slow == 0.0)
pf->w_slow = w_avg;
else
pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
if(pf->w_fast == 0.0)
pf->w_fast = w_avg;
else
pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
}
实际上，源码中还对上述代码第3行中的total等于0的情况进行了特殊的处理，这里省略之。
4.2 pf_update_resample
函数pf_update_resample具体实现了重采样操作，相比于传统的粒子滤波器的重采样操作，它多了一个根据统计量\(w_{slow}, w_{fast}\)插入随机样本的过程。下面是该函数的代码片段， 它只有一个参数pf用于指示进行重采样更新的滤波器对象。在函数的一开始用两个指针set_a和set_b分别获取了当前激活的粒子集合和待切换的集合。
void pf_update_resample(pf_t *pf) {
pf_sample_set_t *set_a = pf->sets + pf->current_set;
pf_sample_set_t *set_b = pf->sets + (pf->current_set + 1) % 2;
然后对粒子的权重进行积分，获得分布函数，后面用于生成样本。
double *c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
c[0] = 0.0;
for(i=0;isample_count;i++)
c[i+1] = c[i]+set_a->samples[i].weight;
接着计算算法Augmented_MCL中第13行的判据，该判据是用于判定是否需要插入新粒子。
w_diff = 1.0 - pf->w_fast / pf->w_slow;
if(w_diff < 0.0)
w_diff = 0.0;
下面，我们重置更新粒子集合set_b，它将保存重采样后的粒子。
pf_kdtree_clear(set_b->kdtree);
total = 0;
set_b->sample_count = 0;
紧接着，我们就可以完成算法的第12到19行中的重采样过程。这里使用一个while循环进行实现，在循环中先生成一个随机数，并根据算法的第13行的判据插入新粒子。
while(set_b->sample_count < pf->max_samples) {
sample_b = set_b->samples + set_b->sample_count++;
if (drand48() < w_diff)
pf_sample_t *sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
如果判据不生效，那么我们就根据原粒子集合所描述的样本分布进行采样。
else {
int i;
double r = drand48();
for (i = 0; i < set_a->sample_count; i++) {
if((c[i] <= r) && (r < c[i+1]))
break;
}
pf_sample_set_t * sample_a = set_a->samples + i;
sample_b->pose = sample_a->pose;
}
为重采样后的粒子赋予相同的权重，并更新统计直方图。当满足样本限制的时候，就退出循环，结束重采样。
sample_b->weight = 1.0;
total += sample_b->weight;
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
break;
}
在函数的最后，重置统计量\(w_{slow}, w_{fast}\)，归一化样本权重，更新粒子簇，交换当前粒子集，检查粒子集是否收敛。
if(w_diff > 0.0)
pf->w_slow = pf->w_fast = 0.0;
// 归一化样本权重
for (int i = 0; i < set_b->sample_count; i++) {
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
pf_cluster_stats(pf, set_b);
pf->current_set = (pf->current_set + 1) % 2;
pf_update_converged(pf);
free(c);
return;
}
4.3 pf_resample_limit
在本文的一开始，我们就介绍说amcl利用Kullback-Leibler Divergence计算两个概率分布之间的差异，得到一个评价样本质量的统计边界，并根据这一边界确定粒子数量。 这就是《P.R.》中描述的的KLD采样。下面的函数根据直方图统计量k， 和误差边界参数\(\varepsilon\)和\(\sigma\)计算样本集合数量。 结合刚刚的重采样过程就是KLD_Sampling_MCL算法了。
int pf_resample_limit(pf_t *pf, int k) {
if (k <= 1)
return pf->max_samples;
double a = 1;
double b = 2 / (9 * ((double) k - 1));
double c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
double x = a - b + c;
int n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
if (n < pf->min_samples)
return pf->min_samples;
if (n > pf->max_samples)
return pf->max_samples;
return n;
}
5. 完
本文中，我们介绍了amcl的粒子滤波器框架和重采样过程。相比于传统的粒子滤波器，amcl根据粒子集合的统计数据插入一定数量的新粒子，来解决全局定位失效或者说是机器人绑架的问题。 并根据KLD采样自适应的调节粒子数量，可以在定位精度和计算代价之间进行权衡。