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 s l o w w_{slow} wslow:长期似然平均估计
w f a s t w_{fast} wfast:短期似然平均估计
α s l o w \alpha_{slow} αslow:长期指数滤波器衰减率
α f a s t \alpha_{fast} αfast:短期指数滤波器衰减率
0 < α s l o w < α f a s t 0<\alpha_{slow}<\alpha_{fast} 0<αslow<αfast
失效恢复的核心思想是:当正确粒子偶然被舍弃,通过重新随机注入粒子的方法,可以重新恢复正确的估计。
实现思路:测量似然的一个突然衰减(短期似然劣于长期似然)象征着粒子质量的下降,这将引起随机采样数目的增加。
w a v g w_{avg} wavg粒子的平均权重,当粒子质量下降时,平均权重随之下降, w s l o w 、 w f a s t w_{slow}、w_{fast} wslow、wfast也会随之下降,但是显然 w f a s t w_{fast} wfast下降的速度要快于 w s l o w w_{slow} wslow——这由衰减率决定,因此随机概率 p = 1 − w f a s t w s l o w p = 1 - \frac{w_{fast}}{w_{slow}} p=1−wslowwfast会增大,随机粒子数目增加。而当粒子质量提高时,粒子短期权重要好于长期,随机概率小于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 handler
signal(SIGINT, sigintHandler);
// Make our node available to sigintHandler
amcl_node_ptr.reset(new AmclNode());
if (argc == 1)
{
// run using ROS input
ros::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 nicely
amcl_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 pose
updatePoseFromServer();
cloud_pub_interval.fromSec(1.0);
//tfb is the tf Broadcaster
tfb_.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 pfs
global_loc_srv_ = nh_.advertiseService("global_localization",
&AmclNode::globalLocalizationCallback,
this);
//create the service of "request_nomotion_update,"nomotionUpdateCallback force samples updates without motion reveived
nomotion_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_frame
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_);
//when received scan msg, laserReceived be called
laser_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 globalLocalizationCallback
initial_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 inited
if(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 wait
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
... ...
map_ = convertMap(msg);
#if NEW_UNIFORM_SAMPLING
// Index of free space
free_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 filter
pf_ = pf_alloc(min_particles_, max_particles_,
alpha_slow_, alpha_fast_,
(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
(void *)map_);
// Instantiate the sensor objects
// Odometry
delete odom_;
odom_ = new AMCLOdom();
ROS_ASSERT(odom_);
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
// Laser
delete 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 string
std::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 base
geometry_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 frame
pf_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 filter
bool 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 occurs
update = update || m_force_update;
... ...
// Use the action data to update the filter
//利用运动学模型更新粒子位姿
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
... ...
// If the robot has moved, update the filter
if(lasers_update_[laser_index])
... ...
// UpdateSensor use the observe mode to compute the sample's weight
//使用观测模型来更新粒子权重
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
... ...
// Resample the particles
if(!(++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 sample
pf_vector_t pose;
// Weight for this pose
double weight;
} pf_sample_t;
//粒子簇结构,一个位姿节点的粒子簇
// Information for a cluster of samples
typedef struct
{
// Number of samples
int count;
// Total weight of samples in this cluster
double weight;
// Cluster statistics
pf_vector_t mean;
pf_matrix_t cov
// Workspace
double m[4], c[2][2];
} pf_cluster_t;
// 粒子集,kdtree存储,包含多个粒子簇,多个位姿节点粒子簇
// Information for a set of samples
typedef struct _pf_sample_set_t
{
// The samples
int sample_count;
pf_sample_t *samples;
// A kdtree encoding the histogram
pf_kdtree_t *kdtree;
// Clusters
int cluster_count, cluster_max_count;
pf_cluster_t *clusters;
// Filter statistics
pf_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 samples
int min_samples, max_samples;
// Population size parameters
double 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 likelihood
double w_slow, w_fast;
// Decay rates for running averages
double alpha_slow, alpha_fast;
// Function used to draw random pose samples
pf_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 converged
int converged;
// boolean parameter to enamble/diable selective resampling
int selective_resampling;
} pf_t;
参考
《Probabilistic_Robotics》
http://wiki.ros.org/amcl
amcl介绍