因为去年读过一次AMCL的源码，那时候读的时候，并不能完全理解某些模块，如map，求粒子簇的这块内容，所以再看的时候，是以地图为切入点看的，然后就是pf。这两部分弄差不多了，再重头开始整理下流程。
1、从main函数中的new AmclNode()开始
AmclNode::AmclNode() : sent_first_transform_(false), latest_tf_valid_(false), map_(NULL), pf_(NULL), resample_count_(0), odom_(NULL), laser_(NULL), private_nh_("~"), initial_pose_hyp_(NULL), first_map_received_(false), first_reconfigure_call_(true) { boost::recursive_mutex::scoped_lock l(configuration_mutex_); /***参数省略**/ odom_frame_id_ = stripSlash(odom_frame_id_); base_frame_id_ = stripSlash(base_frame_id_); global_frame_id_ = stripSlash(global_frame_id_); updatePoseFromServer();//从参数服务器中获取初始位姿 cloud_pub_interval.fromSec(1.0); tfb_.reset(new tf2_ros::TransformBroadcaster()); tf_.reset(new tf2_ros::Buffer()); tfl_.reset(new tf2_ros::TransformListener(*tf_)); //后验位姿+一个6*6的协方差矩阵（xyz+三个转角） pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true); //粒子位姿的数组 particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true); //发布服务，获得没有给定初始位姿的情况下在全局范围内初始化粒子位姿；里面有多层函数调用 global_loc_srv_ = nh_.advertiseService("global_localization", &AmclNode::globalLocalizationCallback, this);// 该Callback调用pf_init_model // 然后调用AmclNode::uniformPoseGenerator // 在地图的free点随机生成pf->max_samples个粒子 //发布服务，没运动模型更新的情况下也暂时更新粒子群 nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this); //发布服务，回调函数中包含handleMapMessage()和handleInitialPoseMessage(req.initial_pose); //分别处理地图转换和初始位姿信息（这里用高斯来初始化），详细见函数内部说明 set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this); //订阅服务，根据激光扫描数据更新粒子，laserReceived是粒子滤波主要过程 laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100); laser_scan_filter_ = new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, *tf_, odom_frame_id_, 100, nh_); laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, this, _1)); //订阅服务，根据rviz中给的初始化位姿 initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this); //根据参数，选择是订阅地图话题还是调用静态地图 if(use_map_topic_) { map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this); ROS_INFO("Subscribed to map topic."); } else { requestMap(); //请求静态地图，调用map_server节点 } m_force_update = false; //发布服务，动态参数配置器 dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~")); dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); // 15s timer to warn on lack of receipt of laser scans, #5209 //检查上一次收到激光雷达数据至今是否超过15秒，如超过则报错 laser_check_interval_ = ros::Duration(15.0); check_laser_timer_ = nh_.createTimer(laser_check_interval_, boost::bind(&AmclNode::checkLaserReceived, this, _1)); diagnosic_updater_.setHardwareID("None"); diagnosic_updater_.add("Standard deviation", this, &AmclNode::standardDeviationDiagnostics); }
按照AmclNode的构造函数，依次执行参数初始化，节点的订阅发布，数据订阅时处理各种函数。如下一一分析。
1、globalLocalizationCallback函数：用到pf_init_model在前面的粒子滤波器初始化解析过，相较pf_init，这里是均匀分布初始化一个滤波器，其他的都一样。
bool AmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { if( map_ == NULL ) { return true; } boost::recursive_mutex::scoped_lock gl(configuration_mutex_); ROS_INFO("Initializing with uniform distribution"); pf_init_model(pf_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, (void *)map_);//粒子滤波器初始化的时候调用的是均匀分布的位姿模型 ROS_INFO("Global initialisation done!"); pf_init_ = false; return true; }
这个函数在一个发布服务里，不明白这么做的意义，后面好好学习下ROS的服务机制，再思考这个问题。
2、nomotionUpdateCallback函数:没有运动时，强制更新滤波器，这里函数只是对update的标识参数赋值true。具体的更新操作放在更新函数里。
// force nomotion updates (amcl updating without requiring motion) bool AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { m_force_update = true; //ROS_INFO("Requesting no-motion update"); return true; }
3、setMapCallback函数
bool AmclNode::setMapCallback(nav_msgs::SetMap::Request& req, nav_msgs::SetMap::Response& res) { handleMapMessage(req.map);//地图转换，记录free space，以及初始化pf_t 结构体,实例化运动模型(odom)和观测模型(laser)； handleInitialPoseMessage(req.initial_pose);// 根据接收的初始位姿消息,在该位姿附近高斯采样重新生成粒子集 res.success = true; return true; }
其中handleMapMessage在前面的文章已经分析过了，就不再说。
4、laserReceived函数：不同激光雷达的数据标记、滤波器更新参数初始化、运动更新、观测更新、重采样、发布位姿数据。由于代码较长，只列出运动更新、观测更新、重采样的代码。这部分似然域模型作为测量模型，及重采样的实现原理，参阅《概率机器人》。
运动更新:delta是上一时刻pose和pf_odom_pose的差值
// Apply the action model bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) { AMCLOdomData *ndata; ndata = (AMCLOdomData*) data; // Compute the new sample poses pf_sample_set_t *set; set = pf->sets + pf->current_set; // pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta); for (int i = 0; i < set->sample_count; i++) { pf_sample_t *sample = set->samples + i; sample->pose.v[0] += ndata->delta.v[0]; sample->pose.v[1] += ndata->delta.v[1]; sample->pose.v[2] += ndata->delta.v[2]; } return true; }
观测更新：
// Update the filter with some new sensor observation void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data) { int i; pf_sample_set_t *set; pf_sample_t *sample; double total; set = pf->sets + pf->current_set; // Compute the sample weights total = (*sensor_fn) (sensor_data, set); if (total > 0.0) { // 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; //权重求平均，因为最后算出的total_weight是超过1的 } // Update running averages of likelihood of samples (Prob Rob p196) w_avg /= set->sample_count; 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); } else { // Handle zero total for (i = 0; i < set->sample_count; i++) { sample = set->samples + i; sample->weight = 1.0 / set->sample_count; } } return; }
重采样：pf_update_resample函数
if(!(++resample_count_ % resample_interval_)) { pf_update_resample(pf_); resampled = true; }
// Resample the distribution 重采样分布 void pf_update_resample(pf_t *pf) { int i; double total; pf_sample_set_t *set_a, *set_b; //set_a为上周期采样的粒子，set_b为本周期将要采样的粒子 pf_sample_t *sample_a, *sample_b; //double r,c,U; //int m; //double count_inv; double* c; double w_diff; set_a = pf->sets + pf->current_set; set_b = pf->sets + (pf->current_set + 1) % 2; //这里是粒子集指针的切换 // Build up cumulative probability table for resampling. 累积概率以进行重采样 // TODO: Replace this with a more efficient procedure 这里应该有更高效的方法 // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html) c = (double*)malloc(sizeof(double)*(set_a->sample_count+1)); c[0] = 0.0; for(i=0;i<set_a->sample_count;i++) c[i+1] = c[i]+set_a->samples[i].weight;//作用在后面 // Create the kd tree for adaptive sampling pf_kdtree_clear(set_b->kdtree); // Draw samples from set a to create set b. total = 0; set_b->sample_count = 0; w_diff = 1.0 - pf->w_fast / pf->w_slow; //随着粒子的更新，如果大权重集中在少数的粒子上， // 此时和初始的分布差异较大，即w_idff变大，需要重采样；如果是大部分粒子有较大的权重，差异不大，不进行重采样 if(w_diff < 0.0) w_diff = 0.0; while(set_b->sample_count < pf->max_samples) { sample_b = set_b->samples + set_b->sample_count++;//set_b的粒子指针指向set_b的采样数之后的位置，用于添加重采样的粒子 if(drand48() < w_diff)//随机生成一个粒子， sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); else { // Naive discrete event sampler double r; r = drand48();//0-1的值作为概率 for(i=0;i<set_a->sample_count;i++) //通过遍历，找到该随机数对应权重是哪个粒子，r代表的是权重， { if((c[i] <= r) && (r < c[i+1])) //相当于概率直方图的bin，以权重作bin的范围，可以参考c[i+1] = c[i]+set_a->samples[i].weight;表示区间范围,r在区间范围内的权重，找到i break; } assert(i<set_a->sample_count); sample_a = set_a->samples + i;//在原始采样集set_a中找到随机产生的权重所在的粒子i处，取这个地方的指针 assert(sample_a->weight > 0); // Add sample to list sample_b->pose = sample_a->pose;//将i处的粒子添加到sample_b中 } sample_b->weight = 1.0; total += sample_b->weight;//相当于计数 // Add sample to histogram pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);//加入kdtree中 // See if we have enough samples yet 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, " "); // Normalize weights for (i = 0; i < set_b->sample_count; i++) { sample_b = set_b->samples + i; sample_b->weight /= total;//重采样后的权重都是1/n } // Re-compute cluster statistics pf_cluster_stats(pf, set_b); // Use the newly created sample set pf->current_set = (pf->current_set + 1) % 2; //此时set_b成为当前粒子集 pf_update_converged(pf); free(c); return; }
重采样粒子数的限制：pf_resample_limit函数
// Compute the required number of samples, given that there are k bins // with samples in them. This is taken directly from Fox et al. int pf_resample_limit(pf_t *pf, int k)//这里是KLD自适应采样的过程，流程参照prob robot P201， // k是kdtree的叶子节点个数，表示有几个粒子在一个cluster里面 // n是书上的M_x { double a, b, c, x; int n; if (k <= 1)//初始时，k较小，采样粒子数是最大采样数 return pf->max_samples; a = 1; b = 2 / (9 * ((double) k - 1)); c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z; x = a - b + c; 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; }
AMCL的整体代码基本上就分析完了。总结：
1、地图信息的用途：初始化滤波器，可以均匀或者高斯分布的在地图空白区域撒粒子;似然域模型计算z变量时，取z的最近的障碍物点的距离值。（这点需要看原理）
2、滤波器分运动和观测更新两部分，观测用的是似然域模型，重采样后粒子权重一样。
3、kdtree的生成，是为了方便计算粒子簇，找到权重高的粒子簇位姿的平均值作为机器人的位姿估计。