AMCL似然域模型原理解析
在学习AMCL定位时,似然域模型,思考好久,终于想明白一个困扰很久的问题.
为什么激光扫描点能够计算粒子的权重?看了好多同志们的的解注,都没消除疑惑.
主要是这一段:
// 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]);
//当该点位于边界时，看做最大障碍物距离，否则取最小障碍物距离，当最小距离小于设定的波束跳跃距离时，可用点+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;//当前激光点与最近障碍物栅格之间的距离
针对采集的同组激光点,粒子越远离激光原点,与采集的该组激光点的距离越大,最终计算的粒子权重越小.因为采集的激光点相对激光原点的位置已定,当粒子位置偏离激光原点越远,越多激光点跑到有效地图外(有效地图区域是粒子位置确定的),跑到有效地图外,则该激光取最大距离,距离越大,概率越小.
因此这样就可以计算粒子权重的大小了.(当时阅读教材的时候,关于距离这块就没想明白.)
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;//遍历每个粒子，这是粒子对应的位姿，是经运动模型更新后 先验位姿
//ROS_INFO("pose LikelihoodFieldModel: %f, %f, %f",pose.v[0],pose.v[1],pose.v[3]);
// Take account of the laser pose relative to the robot
//这个应该是通过机器人与全局坐标系的位姿（每个粒子的位姿），计算激光雷达相对于全局坐标系的位姿。
//方便后续将激光雷达扫描的终点转换到全局坐标系
//点进去pf_vector_coord_add这个函数，
//b对应的就是《概率机器人》P128,6.4第一个公式中的机器人在t时刻的位姿，a代表“与机器人固连的传感器局部坐标系位置”
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 = (data->range_count - 1) / (self->max_beams - 1);//计算步长 只取一组数据中的max_beams个点
// 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]; //观测到的距离 Z(k,t） //距离
obs_bearing = data->ranges[i][1]; //角度
// This model ignores max range readings
//如果obs_range > data->range_max 则结束跳过以下更新（即：如果测距传感器输出了最大值z(k,t)= Z_max，则这些坐标在物理世界没有任何意义
//似然域测量模型简单地将大于最大距离的读数丢弃）
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]);
//当该点位于边界时，看做最大障碍物距离，否则取最小障碍物距离，当最小距离小于设定的波束跳跃距离时，可用点+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;
//z 越大，pz 越小，粒子权重越小
}//每个粒子的所有激光雷达点循环
sample->weight *= p;
total_weight += sample->weight;
}//粒子循环
return(total_weight);
}