amcl_laser 似然域--概率计算
1、蒙特卡罗定位主要流程:
1)评分的基础方法是机器人感知模型----测距仪似然域模型。这里的模型采用四类测量误差,小的测量噪声hit、意外对象引起的误差short,以及由于未检测到对象引起的误差max和随机意外噪声rand。
2)通过按照不同权重进行组合上面四类重点测量误差,得到一个混合分布模型
3)通过该方法计算每个粒子,将平面栅格地图转化为似然地图
4)伪代码
伪代码程序用似然域计算测量概率的算法。外循环将各个p (z|x, m) 的值相乘,并假定不同传感器波束的噪声是相互独立的。
第 4 行检查传感器读数是否为最大距离读数,如果是则被舍弃。第 5-8 行处理有趣的情况:会计算x-y 空间中与最近障碍物的距离(第 7 行)。在第 8 行通过将一个正态分布和一个均匀分布混合得到似然结果。(附代码和解析)
amcl_laser.cpp
//混合分布 Z_hit,Z_rand,Z_max
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
// Compute the sample weights
//遍历每个粒子
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// sample->pose表示每个粒坐标
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
//转换坐标系,与激光雷达数据匹配
//将传感器局部坐标经过三角变换映射到全局坐标系下,
//self->laser_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;//无法解释的随机测量的概率分布
//data->range_count 一帧激光点云数值
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//计算结果 不是个 数,
//它用于表示一个本来要返回数值的操作数未返回数值的情况。
//NaN与任何值都不相等,
if(obs_range != obs_range)
continue;
pz = 0.0;
// Compute the endpoint of the beam//hit高斯
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);//x坐标
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);//y坐标
// 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)
pz += self->z_hit * exp(-(z * z) / z_hit_denom);//在障碍物点周围用正态分布和均匀分布计算似然域??????????????????
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
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;//将每个激光点计算的似然域累加起来(多大范围)
}
sample->weight *= p;//概率等于权重
total_weight += sample->weight;
}
return(total_weight);
}