ROS开源包之粒子滤波定位(AMCL)
虽然现在直接使用amcl定位的很少了，但是它是粒子滤波的典型应用，还是可以了解一下。查阅了很多解读，自己也总结一下，最后在gazebo仿真平台上看看粒子定位跑起来的效果。
0、源码介绍
源码地址：
git clone [email protected]:ros-planning/navigation.gitamcl实现了在二维平面的概率定位，核心当然就是粒子滤波器pf，其源码目录主要3类文件：
map：内部维护一个障碍物距离表；
pf：粒子滤波器；
sensors：传感器运动、观测模型（里程计、激光）；
amcl_node.cpp：最后外加一个ros的node文件去启动和管理，核心函数laserReceived；
发布的话题：
1）/amcl_pose：类型 geometry_msgs::PoseWithCovarianceStamped，后验定位数据， base_frame_id到global_frame_id的定位；
2）/particlecloud：类型geometry_msgs::PoseArray，粒子群中每个的位姿；
订阅的话题：1）scan_topic：2d激光数据 2）initialpose设定的初始位姿 3）获取map地图；
发布的tf：在接收到第一次激光时，amcl查找激光帧和基本帧 (~base_frame_id) 之间的转换，并永远锁定它，amcl估计（~base_frame_id）相对于全局框架（~global_frame_id）的变换，但它只发布全局框架和里程计框架（~odom_frame_id）之间的变换，即map->odom；
常用的服务：1）/set_map加载地图 2）global_localization重置粒子分布，均匀分布在地图上；
AMCL算法流程（laserReceived函数）：获取当前odom_cur坐标 -> 计算odom_last与odom_cur的差值 -> 运动模型去传播粒子的位置 -> 利用激光数据去更新粒子权重 -> 重采样 -> 根据权重发布最大的粒子族，以粒子族的平均位置作为当前最佳位置估计；
二、运动\观测模型
1、里程计---amcl_odom.h
amcl内部实现了三轮全向行进模型（omni）、两轮差分模型（diff）；amcl_sensor.h定义了虚基类AMCLSensor，odom与laser继承于它。(实际内部有4种选项,官网修复bug后的模型)
差速模型的实现注释：
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) {
......
case ODOM_MODEL_DIFF: // 2-差速
{
// Implement sample_motion_odometry (Prob Rob p 136) //内部模型实现参考(Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2; // 差速模型将运动分解为：2次旋转和1次平移
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
// angle_diff函数：得到两个角度的差值，使用夹角小的
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0; // 原地旋转
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
// Sample pose differences
// 为分解的3部分运动（2次旋转+1次位移），添加噪声
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans));
delta_trans_hat = delta_trans -
pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise);
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans));
// Apply sampled update to particle pose
// 将计算出的位移应用与每个粒子
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
}
2、laser观测模型---amcl_laser.h
也实现了两种模型：(实际内部有三种选项)
内部实现：
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data){
......
//根据模型选择，对应的粒子权重计算函数
pf_update_sensor(pf, (pf_sensor_model_fn_t) xxx, data);
}
// 似然场模型
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set){
......
// Compute the sample weights
// 遍历所有激光雷达数据点
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose; //遍历每个粒子，这是粒子对应的位姿，是经运动模型更新后先验位姿
// Take account of the laser pose relative to the robot
// 计算激光雷达相对于全局坐标系的位姿
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit; //测量噪声的方差
double z_rand_mult = 1.0/data->range_max;
// 利用step，选一些点进行，进行似然计算的
step = (data->range_count - 1) / (self->max_beams - 1); // 步进
// Step size must be at least 1
if(step < 1)
step = 1;
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max)
continue;
// Check for NaN
if(obs_range != obs_range)
continue;
pz = 0.0;
// Compute the endpoint of the beam
// 得到激光雷达点最远端的世界坐标
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
//转换到栅格坐标
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
// 点不能越过地图
if(!MAP_VALID(self->map, mi, mj))
z = self->map->max_occ_dist;
else
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist; // 查表获得最近障碍物的距离
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
// 将距离z带入 0均值的标准高斯分布求概率密度pz
pz += self->z_hit * exp(-(z * z) / z_hit_denom); // 概率密度pz
// Part 2: random measurements
pz += self->z_rand * z_rand_mult; // 测量误差
// TODO: outlier rejection for short readings
assert(pz <= 1.0); // 概率密度0-1
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz; // 不太懂？？？？？
}//每个粒子的所有激光雷达点循环
......
}三、map部分
map.h定义了map_t结构体，用于描述栅格地图：
1）每个栅格元素离它附近的、最远的障碍物距离（最重要！！！）；
2）地图原点、大小、以及分辨率；
map_cspare.cpp中函数map_update_cspare()进行计算上述1），生成一张距离表，在利用雷达的观测计算粒子权重时会使用到这张表。
四、pf粒子滤波
主要实现粒子的重采样和计算位置最大后验估计，代码实现的基本原理参考概率机器人那本书，大概流程注释如下：
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data){
......
// 计算粒子权重
total = (*sensor_fn) (sensor_data, set);
......
// Normalize weights
// 计算所有粒子的权重平均值
double w_avg=0.0;
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
w_avg += sample->weight;
sample->weight /= total;
set->n_effective += sample->weight*sample->weight;
}
// Update running averages of likelihood of samples (Prob Rob p258)
w_avg /= set->sample_count;
// 根据权重均值和衰减系数计算生成随机粒子的可能性（这也是amcl优于mcl的地方）
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);
......
}
// 重采样
void pf_update_resample(pf_t *pf) {
......
w_diff = 1.0 - pf->w_fast / pf->w_slow; // [0-1] 生成随机粒子的可能性
if(w_diff < 0.0)
w_diff = 0.0;
//printf("w_diff: %9.6f\n", w_diff);
while(set_b->sample_count < pf->max_samples)
{
sample_b = set_b->samples + set_b->sample_count++;
if(drand48() < w_diff) // 确定是生成随机粒子 还是 从之前的粒子群选粒子
sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); // 生成高斯随机粒子
else
{
// Naive discrete event sampler
// 从之前的粒子群选粒子
double r;
r = drand48();
for(i=0;isample_count;i++)
{
if((c[i] <= r) && (r < c[i+1]))
break;
}
assert(isample_count);
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Add sample to list
sample_b->pose = sample_a->pose;
}
sample_b->weight = 1.0;
total += sample_b->weight;
// Add sample to histogram
// 新采样的粒子加入到kdtree
// 如果粒子已经存在，就增大它的权重
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
// See if we have enough samples yet
// 自适应粒子数目的大小
// kd树叶子数目越多，那么粒子分布的越散开，就需要增加粒子跟踪机器人位置
// 反之，只需要少量粒子就可以进行位置跟踪了
if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
break;
}
// Reset averages, to avoid spiraling off into complete randomness.
if(w_diff > 0.0)
pf->w_slow = pf->w_fast = 0.0;
//fprintf(stderr, "\n\n");
// Normalize weights
for (i = 0; i < set_b->sample_count; i++)
{
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
// Re-compute cluster statistics
// 降采样后的粒子聚类，计算每个类的粒子位置和权重
// 选出权重最大的粒子群，作为机器人位姿估计
pf_cluster_stats(pf, set_b);
......
} 五、参数配置
参考官网的详细注释:
#使用直接服务形式加载地图文件
#可视化发布频率
详细参数去官网：
amcl - ROS Wikihttp://wiki.ros.org/amcl
六、gazebo仿真效果
由于yq不能返校没办法，网上找了一个工程做一下仿真，简单测试一下，后面可能会做一下实际定位效果，评估一下误差。
1.仿真的 tf_tree:
2.初始时候的粒子分布:
3.调用服务后重置粒子分布:
rosservice call /global_localization "{}" 
4.运动一段距离后,粒子收敛情况:
5.几乎收敛:
仿真的测试代码上传到了git.
七、速腾激光雷达3D点云转2D点云
PointCloud2转换为LaserScan，参考：[email protected]:qqngwzlb/pointcloud_to_laserscan.git