文章目錄

• • • • AMCL介紹
      • 代碼結構解析
      • • AMCL接口定義介紹鏈接
        • 參考代碼地址
      • 參考

AMCL介紹

AMCL( Adaptive Mentcarto Localization)，自適應蒙特卡洛定位，基于粒子濾波器的定位算法，與gmapping的定位方法類似，但主要實現定位功能。

MCL：蒙特卡洛定位介紹
MCL算法流程：

隨機生成粒子，粒子權重相等。更常用是位置初始化可以設為(0,0,0)。

根據運動模型估計新時刻位姿

根據觀測模型及觀測值，更新該時候粒子攜帶位姿的權重

重采樣，調整粒子分布，收緊粒子(去掉可信度低的粒子，增加新的)

計算粒子權重的均值和方差，得到最佳位置估計。

回到第2步，進行下一幀的掃描匹配。

MCL問題：

• 權值退化：如果任由粒子權值增長，只有少數粒子的權值較大，其余粒子的權值可以忽略不計，變成無效粒子，因此需要引入重采樣。需要衡量粒子權值的退化程度。
• 粒子多樣性喪失：通常我們會舍棄權值較小的粒子，代之以權值較大的粒子。這樣會導致權值小的粒子逐漸絕種，粒子群多樣性減弱，從而不足以近似表征后驗密度。從而出現正確粒子被去掉的可能性，而后出現粒子的奇異性喪失，總體表現就是粒子的權重均值偏低。

改進算法：AMCL
AMCL 結合自適應（Augmented_MCL）和庫爾貝克-萊不勒散度采樣（KLD_Sampling_MCL）算法

• Augmented_MCL：
  在機器人遭到綁架的時候，它會在發現粒子們的平均分數突然降低了，這意味著正確的粒子在某次迭代中被拋棄了，此時會隨機的全局注入粒子（injection of random particles）。
  
  算法流程上看，augmented_MCL算法最顯著的區別就是引入了四個參數用于失效恢復：
  $w_{slow}$：長期似然平均估計
  $w_{fast}$：短期似然平均估計
  ${\alpha }_{slow}$：長期指數濾波器衰減率
  ${\alpha }_{fast}$：短期指數濾波器衰減率
  $0<{\alpha }_{slow}<{\alpha }_{fast}$
  失效恢復的核心思想是：當正確粒子偶然被舍棄，通過重新隨機注入粒子的方法，可以重新恢復正確的估計。
  實現思路：測量似然的一個突然衰減（短期似然劣于長期似然）象征著粒子質量的下降，這將引起隨機采樣數目的增加。
  $w_{avg}$粒子的平均權重，當粒子質量下降時，平均權重隨之下降，$w_{slow}、w_{fast}$也會隨之下降，但是顯然$w_{fast}$下降的速度要快于$w_{slow}$——這由衰減率決定，因此隨機概率$p=1?\frac{w_{fast}}{w_{slow}}$會增大，隨機粒子數目增加。而當粒子質量提高時，粒子短期權重要好于長期，隨機概率小于0，不生成隨機粒子。

• KLD_Sampling_MCL：
  動態調整粒子數，當機器人定位精確時(粒子都集中在一塊)，減少粒子數，當定位不精確時，增加粒子數。
  在柵格地圖中，看粒子占了多少柵格。占得多，說明粒子很分散，在每次迭代重采樣的時候，允許粒子數量的上限高一些。占得少，說明粒子都已經集中了，那就將上限設低。bin應該是指柵格直方。一個bin對應一個柵格，用來統計有多少粒子在這個柵格中。k則是粒子占有的網格數，根據k值調整粒子數。
  

代碼結構解析

AMCL接口定義介紹鏈接

http://docs.ros.org/lunar/api/amcl/html/annotated.html

參考代碼地址

https://github.com/ros-planning/navigation.git

代碼解析：
amcl的文件結構：

主要實現文件是：amcl_node.cpp，包含整體的流程。

pf下文件是關于粒子濾波器的結構定義等，比如粒子的class結構，粒子的存儲kdtree等
map下是關于地圖結構的定義等，amcl是接收指定的map信息，來實現定位，接收到map message后存儲及地圖相關的處理在這個文件夾下。
sensor下則是 里程計的結構，雷達掃描數據結構等

以下為amcl_node.cpp 的main函數：

main主要任務：
node創建及初始化;
參數解析，判斷是不是基于數據集bag文件的運行。
amcl的功能實現是在AmclNode()的初始化中。

int main(int argc, char** argv) {ros::init(argc, argv, "amcl");ros::NodeHandle nh;// Override default sigint handlersignal(SIGINT, sigintHandler);// Make our node available to sigintHandleramcl_node_ptr.reset(new AmclNode());if (argc == 1){// run using ROS inputros::spin();}else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag")){if (argc == 3){amcl_node_ptr->runFromBag(argv[2]);}else if ((argc == 4) && (std::string(argv[3]) == "--global-localization")){amcl_node_ptr->runFromBag(argv[2], true);}}// Without this, our boost locks are not shut down nicelyamcl_node_ptr.reset();// To quote Morgan, Hooray!return(0); }

AmclNode()的初始化

AmclNode主要任務：
完成參數param的解析
發布pub topic：“tf”、 “amcl_pose”、“particlecloud”
訂閱sub topic：“tf”、scan_topic_、“map”/“static map”
提供服務service：global_localization、request_nomotion_update、set_map

AmclNode::AmclNode() : ... ...private_nh_.param("laser_min_range", laser_min_range_, -1.0);private_nh_.param("laser_max_range", laser_max_range_, -1.0);private_nh_.param("laser_max_beams", max_beams_, 30);private_nh_.param("min_particles", min_particles_, 100);private_nh_.param("max_particles", max_particles_, 5000); ... ...base_frame_id_ = stripSlash(base_frame_id_);global_frame_id_ = stripSlash(global_frame_id_);//init the poseupdatePoseFromServer();cloud_pub_interval.fromSec(1.0);//tfb is the tf Broadcastertfb_.reset(new tf2_ros::TransformBroadcaster());// tf_ use to buffer the scan msg tf_.reset(new tf2_ros::Buffer());//tfl is the tf Listener tfl_.reset(new tf2_ros::TransformListener(*tf_));//publish the topic "amcl_pose"pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);//publish the topic "particlecloud"particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);//create the service of "global_localization". globalLocalizationCallback is init the pfsglobal_loc_srv_ = nh_.advertiseService("global_localization",&AmclNode::globalLocalizationCallback,this);//create the service of "request_nomotion_update,"nomotionUpdateCallback force samples updates without motion reveivednomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);//subscribe scan topic,MessageFilter used to filter and buffer the scan message util scan could be tramform to odom_framelaser_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_);//when received scan msg, laserReceived be calledlaser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,this, _1));//subscibe topic "initialpose",initialPoseReceived used to init pose that service received,if this not be call, then use map origin to init pose by globalLocalizationCallbackinitial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);//subscribe topic map, get the map, mapReceived will be called//handleMapMessage is used to deal with map message//when map msg received, the pf, odom and laser be created and initedif(use_map_topic_) {map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);ROS_INFO("Subscribed to map topic.");} else {... ...}

tf2_ros::MessageFilter的使用是將msg緩存，直到能夠轉換到指定的坐標系，如上是將scan msg緩存在tf_指定區域，直至msg可以被轉換到odom_frame_id_。此時laserReceived被調用。

當接收到全局map信息后，mapReceived被調用，處理map msg及完成pf(濾波器)、odom(里程計)、laser等結構的創建及初始化

void AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg) {if( first_map_only_ && first_map_received_ ) {return;}handleMapMessage( *msg );first_map_received_ = true; } //free_space_indices store the free cell index //alloc the pfs //new the AMCL odom and set the model of robot //new the AMCL_laser, this stores map ptr,max num of particles likelyhood type void AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg) {// if the config is being writen, stock and waitboost::recursive_mutex::scoped_lock cfl(configuration_mutex_);... ...map_ = convertMap(msg); #if NEW_UNIFORM_SAMPLING// Index of free spacefree_space_indices.resize(0);for(int i = 0; i < map_->size_x; i++)for(int j = 0; j < map_->size_y; j++)if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)free_space_indices.push_back(std::make_pair(i,j)); #endif// Create the particle filterpf_ = pf_alloc(min_particles_, max_particles_,alpha_slow_, alpha_fast_,(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,(void *)map_);// Instantiate the sensor objects// Odometrydelete odom_;odom_ = new AMCLOdom();ROS_ASSERT(odom_);odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );// Laserdelete laser_;laser_ = new AMCLLaser(max_beams_, map_);ROS_ASSERT(laser_);if(laser_model_type_ == LASER_MODEL_BEAM)laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,sigma_hit_, lambda_short_, 0.0);else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){... ...} }

接收到scan的消息，laserReceived處理并判斷是否需要及完成更新及重采樣。

scan處理函數完成任務：
計算雷達相對于機器人車體base坐標系的安裝laser pose
獲取里程計的估計信息，判斷是否需要更新
更新完成后，完成重采樣

// if the laserpose in base frame is not inited,then get the laser pose in base frame // get the odom frame // determine whether update pf or not // resamping after update void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) {// laser_scan_frame_id is the name of lase_scan_frame in string// get the lase_scan_frame name in stringstd::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);... ...//laser_pose is the laser pose in base frame,this is a constant//laser_pose is the transform form base_laser to basegeometry_msgs::PoseStamped laser_pose;try{this->tf_->transform(ident, laser_pose, base_frame_id_);}... ...// Where was the robot when this scan was taken?// get the robot pose in odom framepf_vector_t pose;if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],laser_scan->header.stamp, base_frame_id_))... ...// See if we should update the filterbool update = fabs(delta.v[0]) > d_thresh_ ||fabs(delta.v[1]) > d_thresh_ ||fabs(delta.v[2]) > a_thresh_;//m_force_update used to temporarily let amcl update samples even when no motion occursupdate = update || m_force_update;... ...// Use the action data to update the filter//利用運動學模型更新粒子位姿odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);... ...// If the robot has moved, update the filterif(lasers_update_[laser_index])... ...// UpdateSensor use the observe mode to compute the sample's weight//使用觀測模型來更新粒子權重lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);... ...// Resample the particlesif(!(++resample_count_ % resample_interval_)){pf_update_resample(pf_);resampled = true;}pf_sample_set_t* set = pf_->sets + pf_->current_set;

重要的結構：
粒子結構定義amcl/include/pf/pf.h

//單個粒子：代表位姿，權重 // Information for a single sample typedef struct {// Pose represented by this samplepf_vector_t pose;// Weight for this posedouble weight; } pf_sample_t; //粒子簇結構，一個位姿節點的粒子簇 // Information for a cluster of samples typedef struct {// Number of samplesint count;// Total weight of samples in this clusterdouble weight;// Cluster statisticspf_vector_t mean;pf_matrix_t cov// Workspacedouble m[4], c[2][2]; } pf_cluster_t; // 粒子集，kdtree存儲，包含多個粒子簇，多個位姿節點粒子簇 // Information for a set of samples typedef struct _pf_sample_set_t {// The samplesint sample_count;pf_sample_t *samples;// A kdtree encoding the histogrampf_kdtree_t *kdtree;// Clustersint cluster_count, cluster_max_count;pf_cluster_t *clusters;// Filter statisticspf_vector_t mean;pf_matrix_t cov;int converged;double n_effective; } pf_sample_set_t; // 粒子濾波器結構體，包含了配置信息 typedef struct _pf_t {// This min and max number of samplesint min_samples, max_samples;// Population size parametersdouble pop_err, pop_z;// The sample sets. We keep two sets and use [current_set]// to identify the active set.int current_set;pf_sample_set_t sets[2];// Running averages, slow and fast, of likelihooddouble w_slow, w_fast;// Decay rates for running averagesdouble alpha_slow, alpha_fast;// Function used to draw random pose samplespf_init_model_fn_t random_pose_fn;void *random_pose_data;double dist_threshold; //distance threshold in each axis over which the pf is considered to not be convergedint converged;// boolean parameter to enamble/diable selective resamplingint selective_resampling; } pf_t;

參考

《Probabilistic_Robotics》
http://wiki.ros.org/amcl
amcl介紹

總結

以上是生活随笔為你收集整理的AMCL介绍的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。