因為去年讀過一次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, "\n\n"); // 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的生成，是為了方便計算粒子簇，找到權重高的粒子簇位姿的平均值作為機器人的位姿估計。