amcl_laser 似然域--概率计算

1、蒙特卡罗定位主要流程：amcl_laser.cpp

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); }