AMCL源码架构讲解与详细分析

AMCL算法简介

AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位 ，源于MCL算法的一种增强，本章简要介绍AMCL的算法原理并着重讲解源码包（详细算法原理请见ROS及SLAM进阶教程（二）AMCL算法原理讲解）
AMCL是2D的概率定位系统，这个方法是在已知地图中使用粒子滤波方法得到位姿的。输入激光雷达数据、里程计数据，输出机器人在地图中的位姿。如果里程计没有误差，完美的情况下，我们可以直接使用里程计信息推算出机器人（base_frame）相对里程计坐标系的位置。但现实情况，里程计存在漂移以及无法忽略的累计误差，所以AMCL采用先根据里程计信息初步定位base_frame，然后通过测量模型得到base_frame相对于map_frame（全局地图坐标系）的偏移，也就知道了机器人在地图中的位姿。（注意，这里虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为里程计的漂移。）
与MCL不同的地方是AMCL算法在机器人遭到绑架的时候，会随机注入粒子（injection of random particles），增加粒子的方法引起两个问题，一是每次算法迭代中应该增加多少粒子，二是从哪种分布产生这些粒子。
解决第一个问题可通过监控传感器测量的概率来评估增加粒子，即式$(1.1)$
$$p(z_t|z_{1:t-1},u_{1:t},m)$$
并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式$(2.2)$
$$\frac{1}{M}\sum _{m=1}^M{w}_t^{[m]}\approx p(z_t|z_{1:t-1},u_{1:t},m)$$
这个接近式$(1.1)$中的期望概率。
解决第二个问题可以根据均匀分布在位姿空间产生粒子，用当前观测值加权得到这些粒子。如下给出增加随机粒子的蒙特卡洛定位算法自适应变种（AMCL）：

算法第5行使用运动模型采样，以当前置信度为起点使用粒子，第6行使用测量模型以确定粒子的重要性权值。这个算法跟踪式(1.1)的似然值的短期与长期均值，在第8行中给出了经验测量似然，并在第10、11行维持短期和长期似然平均，算法要求$0\le {\alpha }_{slow}\le {\alpha }_{fast}$ ，参数α_slow和α_fast分别估计长期和短期平均的指数滤波器的衰减率。算法的关键在第13行，重采样过程中，随机采样以以下式$(1.3)$概率增加
$$max\{0.0,1-\frac{w_{fast}}{w_{slow}}\}$$
否则重采样以MCL相同的方式进行，即根据式$(1.3)$如果短期似然优于长期似然，则算法将判断不增加随机采样，否则的话则按两者之比的比例增加随机采样，以这种方式可抵消瞬时传感器噪声带来的定位误差。
AMCL定位算法直接影响机器人的导航精度，因此对源码的研究和分析非常必要，之后章节便是AMCL源码的详细分析。

文章一部分内容参考博客ROS Navigation之amcl源码解析

AMCL包结构与通信

本章将从整体讲解AMCL包的架构和各文件之间的联系，首先可以发现该源码包中包含cfg配置文件、examples演示示例、src源码（包含map, pf, sensors三部分内容以及其节点文件amcl_node.cpp）、test参数包、package.xml文件以及外部的CmakeLists.txt文件，首先我们从CmakeLists.txt来研究这个包文件。

CmakeLists研究

makefile是在Linux编译c或者c++代码的时候的一种脚本文件，但是每一个功能都要写一个makefile文件，这样如果这个工程很大，而且相关性比较强的话，makefile的书写就会变得相对繁琐。cmake的出现就是为了解决这样的问题，cmake的入门相当容易，而且管理也特别方便简单，cmake的所有语句都写在一个CMakeLists.txt的文件中，CMakeLists.txt文件确定后，直接使用cmake命令进行运行。因此要研究一个包文件的架构首先需要研究的就是CmakeLists.txt文件。
根据AMCL中CmakeLists.txt文件的说明，AMCL包含三个库：

1.	add_library(amcl_pf  
2.	                    src/amcl/pf/pf.c  
3.	                    src/amcl/pf/pf_kdtree.c  
4.	                    src/amcl/pf/pf_pdf.c  
5.	                    src/amcl/pf/pf_vector.c  
6.	                    src/amcl/pf/eig3.c  
7.	                    src/amcl/pf/pf_draw.c)  
8.	  
9.	add_library(amcl_map  
10.	                    src/amcl/map/map.c  
11.	                    src/amcl/map/map_cspace.cpp  
12.	                    src/amcl/map/map_range.c  
13.	                    src/amcl/map/map_store.c  
14.	                    src/amcl/map/map_draw.c)  
15.	  
16.	add_library(amcl_sensors  
17.	                    src/amcl/sensors/amcl_sensor.cpp  
18.	                    src/amcl/sensors/amcl_odom.cpp  
19.	                    src/amcl/sensors/amcl_laser.cpp)  
20.	target_link_libraries(amcl_sensors amcl_map amcl_pf)  

分别是amcl_pf, amcl_map和amcl_sensors，并且包含一个节点：

1.	add_executable(amcl  
2.	                       src/amcl_node.cpp)  

即amcl_node.cpp节点文件，并通过该文件生成可执行文件，以运行AMCL。同时在节点运行的时候需要订阅和发布话题与服务才能实现与库文件的通信，接下来将说明库与节点之间的通信与联系。

体系结构与研究

AMCL源码包有非常多的代码文件，但是他们彼此之间是有联系的，而关键的节点就是amcl_node.cpp文件，它起到连接外部参数文件与库文件并将计算结果发布出去的作用，其整体架构如下图$2.1$所示：

amcl_node.cpp文件作为关键节点，从三个库文件分别订阅相关话题信息，从参数文件中得到参数并进行动态配置，通过其功能函数对订阅的信息进行处理，并发布机器人的后验位姿、反馈给库文件相应的服务使其实现更新或实现其他功能，以此实现AMCL的功能。
其中第三章将着重讲解节点文件中功能函数的实现，首先讲解了图$2.1$中订阅和发布话题与服务的相关函数，然后解释了其他相关功能函数；第四章将简要介绍三个库文件sensors, pf和map的函数组成结构和功能实现。

节点文件函数讲解

上一节从整体讲解了AMCL包的架构以及各文件之间的联系，本节将详细讲解节点文件中各函数的功能实现。
订阅(advertise)与发布(publish)是ROS节点实现通信的必要手段，同时还有service实现ROS节点的点对点通信。

service通信是封装性质的，service服务详解可参考rosservice: ROS服务

AMCL包中包含粒子滤波、地图、传感器三个库，实现他们之间的通信相当复杂，下面将从节点文件的订阅话题、发布话题与发布服务出发介绍节点文件的主要函数。

订阅话题函数

根据图$2.1$所示，AMCL一定需要获取雷达的信息，并将其通过TF变换转换到odom_frame_id_，需要订阅rviz中给的初始化位姿，并将其变换到global_frame_id_即地图中的坐标，并重新生成粒子。除此之外，AMCL节点文件中判断当使用use_map_topic_的时候订阅地图，一般不需要使用，因为使用了一个固定的地图。其订阅服务详解如下。

scan_topic

订阅传感器信息代码及注释如下所示

1.	laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);  //位于Amcl_Node初始化函数中 
2.	laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,  
3.	                                                   this, _1));  
4.	//调用laserReceived函数 
5.	void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
6.	{
7.		//代码部分实现将雷达信息的tf变换
8.	} 

initial_pose

订阅初始化位姿代码及注释如下所示

1.	initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);  //位于Amcl_Node初始化函数中 
2.	//订阅初始位姿，并调用initialPoseReceived函数  
3.	void AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)  
4.	{  
5.	  handleInitialPoseMessage(*msg);//调用处理函数  
6.	}  
7.	  
8.	void AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg)  
9.	{  
10.	  ...//代码部分实现将坐标变换到map对应的坐标中的功能  
11.	} 

map_sub_

在有初始位姿和传感器信息后，需要将信息转换到在map对应的global_frame_id中进行处理，因此还需要订阅map信息

1.	if(use_map_topic_) {//位于AmclNode初始化函数中  
2.	    map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);  
3.	    //调用mapReceived  
4.	    ROS_INFO("Subscribed to map topic.");  
5.	  } else { requestMap();}//调用请求地图函数  
6.	  
7.	void AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg)  
8.	{  
9.	  if( first_map_only_ && first_map_received_ ) {  
10.	    return;  
11.	  }  
12.	  handleMapMessage( *msg );//调用处理函数  
13.	  first_map_received_ = true;  
14.	}  
15.	
16.	void AmclNode::requestMap()  
17.	{  
18.	  boost::recursive_mutex::scoped_lock ml(configuration_mutex_);  
19.	  
20.	  // get map via RPC  
21.	  nav_msgs::GetMap::Request  req;  
22.	  nav_msgs::GetMap::Response resp;  
23.	  ROS_INFO("Requesting the map...");  
24.	  while(!ros::service::call("static_map", req, resp))  
25.	  {  
26.	    ROS_WARN("Request for map failed; trying again...");  
27.	    ros::Duration d(0.5);  
28.	    d.sleep();  
29.	  }  
30.	  handleMapMessage( resp.map );//重新请求地图成功后调用处理函数  
31.	}  
32.	  
33.	void AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)  
34.	{  
35.	    ...
36.	    //这个函数是主要的模型处理函数，其主体有三个部分  
37.	    //第一部分处理地图信息，并给free space添加index，其中msg必须是基于global_frame_id_的 
38.	    //第二部分创建粒子滤波器并初始化这个滤波器  
39.	    //第三部分初始化里程计模型和测距仪（雷达）模型  
40.	}

至此，node函数中所有订阅话题相关的代码已经讲解完成，与订阅相对应，AMCL会发布一些话题，分别是amcl_pose（后验位姿）、particlecloud粒子位姿数组和一个检查激光雷达数据的定时器。

发布话题函数

节点文件发布的话题包括输出后验位姿、粒子位姿和检查雷达的定时器，前者是AMCL的输出结果，而后面两个话题的作用是使库文件实现更新。

amcl_pose

AMCL输出的结果是机器人在地图中的位姿，输出通过发布话题实现。其中发布的位置信息是后验位姿。

关于后验位姿的讲解参考博客SLAM14讲学习笔记（后验=似然*先验）

1.	ros::Publisher pose_pub_;  
2.	  //定义新的发布变量 
3.	pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);  //声明
4.	//位于AmclNode初始化函数中  
5.	  
6.	void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)  
7.	{  
8.	   ... 
9.	   //函数中的其他部分计算得到机器人后验位姿xyz和表示三个转角信息的一个6*6的协方差矩阵，再进行publish
10.	   pose_pub_.publish(p);  
11.		 ... 
12.	}  

particlecloud

与发布位姿相对应，AMCL也发布粒子位姿的数组，以使粒子滤波实现更新，其代码如下所示

1.	ros::Publisher particlecloud_pub_;  
2.	//定义新的发布变量  
3.	particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);  
4.	//位于Amcl_Node初始化函数中  
5.	void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)  
6.	{  
7.	    ...  
8.	    // Publish the resulting cloud  
9.	    // TODO: set maximum rate for publishing  
10.	    if (!m_force_update)  
11.	    {  
12.	     ... 
13.	      //计算得到粒子云的位姿数组
14.	      particlecloud_pub_.publish(cloud_msg);  //发布粒子位姿的数组
15.	    }  
16.	    ...  
17.	}  

check_laser_time

前述两个发布的话题是AMCL所输出的结果，另外还有一个话题是一个15秒的定时器，用于反馈给激光雷达确认是否收到数据，如果超过15秒未收到则会报错，如下所示

1.	// 15s timer to warn on lack of receipt of laser scans, #5209  
2.	  laser_check_interval_ = ros::Duration(15.0);  
3.	  check_laser_timer_ = nh_.createTimer(laser_check_interval_,   
4.	                     boost::bind(&AmclNode::checkLaserReceived, this, _1));  // 位于Amcl_Node初始化函数中，调用checkLaserReceived函数
5.	  
6.	void AmclNode::checkLaserReceived(const ros::TimerEvent& event)  
7.	{  // 检查是否超时未收到数据，如果超时则会报错
8.	  ros::Duration d = ros::Time::now() - last_laser_received_ts_;  
9.	  if(d > laser_check_interval_)  
10.	  {  
11.	    ROS_WARN("No laser scan received (and thus no pose updates have been published) for %f seconds.  Verify that data is being published on the %s topic.",  
12.	             d.toSec(),  
13.	             ros::names::resolve(scan_topic_).c_str());  
14.	  }  
15.	}  

发布服务函数

除了订阅和发布的话题之外，AMCL有发布四个服务以实现函数功能。

global_localization

global_localization是没有给定初始位姿的情况下在全局范围内初始化粒子位姿，其部分源码及注释如下

1.	ros::ServiceServer global_loc_srv_; //定义服务变量  
2.	global_loc_srv_ = nh_.advertiseService("global_localization", &AmclNode::globalLocalizationCallback, this);  
3.	//调用globalLocalizationCallback函数  
4.	  
5.	boolAmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req,  
6.	                                     std_srvs::Empty::Response& res)  
7.	{  
8.	  if( map_ == NULL ) {  
9.	    return true;  
10.	  }  
11.	  boost::recursive_mutex::scoped_lock gl(configuration_mutex_);  
12.	  ROS_INFO("Initializing with uniform distribution");  
13.	  pf_init_model(pf_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,  
14.	                (void *)map_);//调用uniformPoseGenerator函数初始化全局范围内粒子位姿  
15.	  ROS_INFO("Global initialisation done!");  
16.	  pf_init_ = false;  
17.	  return true;  
18.	}  

request_nomotion_update

request_nomotion_update是没有运动模型更新的情况下也暂时更新粒子群，判断没有运动时也将粒子群更新设置为true，代码非常简单

1.	ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion  
2.	//定义服务变量  
3.	nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);  
4.	//调用nomotionUpdateCallback函数  
5.	  
6.	// 强迫无运动情况下的更新 (amcl updating without requiring motion)  
7.	bool AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req,  
8.	                                     std_srvs::Empty::Response& res)  
9.	{  
10.	    m_force_update = true;  // 该参数决定强行更新粒子群
11.	    //ROS_INFO("Requesting no-motion update");  
12.	    return true;  
13.	} 

set_map

set_map是设置地图信息，代码与订阅的话题相关

1.	ros::ServiceServer set_map_srv_; //定义服务变量  
2.	set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);  
3.	//调用setMap回调函数  
4.	  
5.	bool AmclNode::setMapCallback(nav_msgs::SetMap::Request& req,  
6.	                         nav_msgs::SetMap::Response& res)  
7.	{  
8.	  handleMapMessage(req.map);  
9.	  handleInitialPoseMessage(req.initial_pose);  
10.	//这两个函数在前面订阅话题部分都有讲解  
11.	  res.success = true;  
12.	  return true;  
13.	}  

dynamic_configure

dynamic_reconfigure是动态参数配置器，配置影响模型的参数信息，函数功能实现测距仪模型、里程计模型和AMCL所有参数的配置

1.	dynamic_reconfigure::Server<amcl::AMCLConfig> *dsrv_;//定义服务变量  
2.	  
3.	dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));  
4.	dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);  
5.	//调用配置函数reconfigureCB  
6.	void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)  
7.	{  
8.	    //连接配置文件，将参数传递至函数中  
9.	}

本小节讲解了amcl_node.cpp文件的结构以及其订阅与发布的通信机制，结合了相关的源码讲解了其大致的结构，下面将对amcl_node.cpp文件中的main函数及其涉及的其他功能函数进行解读。

main函数

main函数中判断初始化下argc为1时正常运行ROS的输入，当argc为3或者指定使用bag数据运行时会启用bag文件中的数据进行运行，其代码及相关注释如下所示

1.	int main(int argc, char** argv)  
2.	{  
3.	  ros::init(argc, argv, "amcl");  
4.	  ros::NodeHandle nh;  
5.	  
6.	  // 遇到shutdown时恢复默认的数据，其中sigintHandler函数调用了savePoseToServer函数保存了shutdown前的最新pose信息  
7.	  signal(SIGINT, sigintHandler);  
8.	  // Make our node available to sigintHandler  
9.	  amcl_node_ptr.reset(new AmclNode());  
10.	  
11.	  if (argc == 1)  
12.	  {  
13.	    // run using ROS input  
14.	    ros::spin();  
15.	  }  
16.	  else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))  
17.	  {  
18.	    amcl_node_ptr->runFromBag(argv[2]);  
19.	    //使用bag文件中的TF和雷达信息来作为替代驱动AMCL  
20.	  }  
21.	  // Without this, our boost locks are not shut down nicely  
22.	  amcl_node_ptr.reset();  
23.	  // To quote Morgan, Hooray!  
24.	  return(0);  
25.	}  

至此，AMCL节点文件相关函数及其功能大致讲解完成，只要理解了第二章中图$2.1$所标明的文件结构和联系，根据订阅和发布、main函数的主线来理解节点文件源码就变得非常容易。接下来将讲解最后一部分——三个库文件中的函数架构和实现功能。

库函数功能简介

根据第一章原理的介绍，AMCL算法的实现是接收里程计模型信息、传感器模型信息，在map中通过粒子滤波的方式实现的机器人定位，源码包的三个库文件sensors, map和pf分别对应其功能的实现，下面将介绍这三个库文件的具体实现功能。

sensors

sensors库中包含amcl_laser, amcl_odom, amcl_sensors三个函数文件和相对应的三个头文件，他们之间彼此独立，其实现的功能如下图所示：

map

map库中包含map, map_cspace, map_draw, map_range, map_store五个函数文件，对应不同功能的实现，他们都引用同一个头文件共享地图信息，如下图所示

pf

pf粒子滤波部分是实现机器人定位的关键算法，也是库文件中函数最复杂、代码量最大的部分，是之后改进AMCL定位效果着重要尝试修改的部分。其库中包含eig3, pf, pf_draw, pf_kdtree, pf_pdf, pf_vector六个函数文件，除了pf_draw其他文件都有对应的头文件，与sensors库函数互不相关不同的是六个函数文件中pf.c是关键文件，负责输出粒子滤波的结果，几个文件实现功能的介绍如下所示

各文件之间的关系如下图所示

以上是本节对AMCL三个库函数功能的介绍，未给出源码示例，因为代码部分比较简单，需要深究的是pf相关代码，博主还在进一步研究中。

博主有两年多ROS的使用经验，目前仍在不停研究中。本系列ROS及SLAM进阶教程将涵盖ROS的进阶功能使用、机器人SLAM及导航的设计及研究等领域，持续不断更新中。如果大家有相关问题或发现作者漏洞欢迎私戳，同时欢迎关注收藏。
同时欢迎关注博主Git：
https://github.com/redglassli