amcl（自适应蒙特卡罗）定位方法是目前比较流行的定位算法，它利用粒子表示置信度，通过把合适的概率运动模型与感知模型代入到粒子滤波算法模型中得到估算出的机器人状态。而根据tf树就可以看到，它实际上是估算了base_link到map的tf变换。
和local_planner相似，这个包的文件也很多，先在CmakeList看看它们的依赖关系，可以看到它生成了amcl_sensors，amcl_map，amcl_pf三个库，以及amcl_node.cpp生成了一个可执行node。这一点我们很清楚，这三个库就是感知模型、似然域模型与粒子滤波模型。那就先进入amcl_node.cpp看看：
上来就是这个struct，其实它就是一个粒子，包含了机器人的位姿信息，另外粒子拥有一个重要性权重，会随迭代次数而变化。
// Pose hypothesis
typedef struct
{
// Total weight (weights sum to 1)
double weight;
// Mean of pose esimate
pf_vector_t pf_pose_mean;
// Covariance of pose estimate
pf_matrix_t pf_pose_cov;
} amcl_hyp_t;
接下来就是类的声明了，这是真的拥挤。
一．构造函数先是声明了激光、里程计、粒子滤波模型的参数，再声明了一些其他参数（例如小车是全向轮还是差分模型，似然域模型的选择等），接下来是发布的话题与服务：
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);
nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);
set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);
laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ =
new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
*tf_,
odom_frame_id_,
100);
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
this, _1));
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();
}从这一段看到amcl发布的话题有最终估算的位姿amcl_pose，粒子群particlecloud（可以看到粒子群从一开始在地图上杂乱无章分布直到汇聚到真实位置的过程），另外接收的话题有initialpose和scan，以及request map这么个服务。
接下来是AmclNode::savePoseToServer这个函数，它将最终一刻估算的位姿信息保存到参数服务器中，那么紧接着的AmclNode::updatePoseFromServer就是从参数服务器中加载上次的位姿信息。
二．AmclNode::handleMapMessage这个函数是一个比较重要的部分，它根据地图信息创造了地图空闲区域、粒子滤波器、里程计运动模型与激光感知的似然域模型。
map_ = convertMap(msg);
//把地图中不是障碍的点保存下来，由于地图信息为-1的栅格不是障碍物，因此将空闲区域保存下来
#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_);
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;
//这是从参数服务器获取初始位姿信息
// Initialize the filter
updatePoseFromServer();
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;
//定义里程计与激光雷达并初始化
// 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){
ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
laser_likelihood_max_dist_,
do_beamskip_, beam_skip_distance_,
beam_skip_threshold_, beam_skip_error_threshold_);
ROS_INFO("Done initializing likelihood field model.");
}
//激光数据与似然域模型
else
{
ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
laser_likelihood_max_dist_);
ROS_INFO("Done initializing likelihood field model.");
}
// In case the initial pose message arrived before the first map,
// try to apply the initial pose now that the map has arrived.
applyInitialPose();在这里我们可以看到，在创造粒子滤波器的过程中调用了AmclNode::uniformPoseGenerator，转到它看看具体实现：
１．drand48返回服从均匀分布的·[0.0, 1.0) 之间的 double 型随机数。
２．在循环中直到产生一个位置与方向都合法的粒子，退出循环。也就是说，每次调用该函数产生一个随机位姿的粒子。
三．AmclNode::laserReceived是接下来的重点，长篇大论之后会得到我们最终需要的位姿，也是这个类中的最终实现。
//这一段先是从frame_to_laser_这个map里中找寻scan，如果没找到就新建一个，但是用map来储存，应该是可以允许多个雷达同时工作的
// Do we have the base->base_laser Tx yet?
if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
{
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
//更新激光感知模型
lasers_.push_back(new AMCLLaser(*laser_));
lasers_update_.push_back(true);
laser_index = frame_to_laser_.size();
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)),
ros::Time(), laser_scan->header.frame_id);
tf::Stamped<tf::Pose> laser_pose;
try
{
this->tf_->transformPose(base_frame_id_, ident, laser_pose);
}
catch(tf::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
laser_scan->header.frame_id.c_str(),
base_frame_id_.c_str());
return;
}
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.getOrigin().x();
laser_pose_v.v[1] = laser_pose.getOrigin().y();
// laser mounting angle gets computed later -> set to 0 here!
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose(laser_pose_v);
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
laser_pose_v.v[0],
laser_pose_v.v[1],
laser_pose_v.v[2]);
frame_to_laser_[laser_scan->header.frame_id] = laser_index;
} else {
// we have the laser pose, retrieve laser index
laser_index = frame_to_laser_[laser_scan->header.frame_id];
}
//这里是获取此时激光感知时刻的odom到base_link的变换
// Where was the robot when this scan was taken?
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_))
{
ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
return;
}
pf_vector_t delta = pf_vector_zero();
//如果不是第一帧，看运动幅度是否超过设定值需要更新，即更新了地图信息或者位姿
if(pf_init_)
{
// Compute change in pose
//delta = pf_vector_coord_sub(pose, pf_odom_pose_);
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
// 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_;
update = update || m_force_update;
m_force_update=false;
// Set the laser update flags
if(update)
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
}
bool force_publication = false;
if(!pf_init_)
{
// Pose at last filter update
pf_odom_pose_ = pose;
// Filter is now initialized
pf_init_ = true;
// Should update sensor data
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
force_publication = true;
resample_count_ = 0;
}
//这里是正式开始更新滤波器了，将里程计的先验位姿更新为后验
// If the robot has moved, update the filter
else if(pf_init_ && lasers_update_[laser_index])
{
//printf("pose\n");
//pf_vector_fprintf(pose, stdout, "%.3f");
AMCLOdomData odata;
odata.pose = pose;
// HACK
// Modify the delta in the action data so the filter gets
// updated correctly
odata.delta = delta;
//这里是调用的关键，我们随后分析
// Use the action data to update the filter
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
// Pose at last filter update
//this->pf_odom_pose = pose;
}
bool resampled = false;
//将激光数据处理为AMCLLaserData格式
// If the robot has moved, update the filter
if(lasers_update_[laser_index])
{
AMCLLaserData ldata;
ldata.sensor = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
//激光扫描的最小值与角度增量
tf::Quaternion q;
q.setRPY(0.0, 0.0, laser_scan->angle_min);
tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
try
{
tf_->transformQuaternion(base_frame_id_, min_q, min_q);
tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
}
double angle_min = tf::getYaw(min_q);
double angle_increment = tf::getYaw(inc_q) - angle_min;
// wrapping angle to [-pi .. pi]
//fmod函数是用来取余数
angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
// Apply range min/max thresholds, if the user supplied them
if(laser_max_range_ > 0.0)
ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
else
ldata.range_max = laser_scan->range_max;
double range_min;
if(laser_min_range_ > 0.0)
range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
else
range_min = laser_scan->range_min;
// The AMCLLaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
ROS_ASSERT(ldata.ranges);
for(int i=0;i<ldata.range_count;i++)
{
// amcl doesn't (yet) have a concept of min range. So we'll map short
// readings to max range.
if(laser_scan->ranges[i] <= range_min)
ldata.ranges[i][0] = ldata.range_max;
else
ldata.ranges[i][0] = laser_scan->ranges[i];
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment);
}
//对每个激光雷达数据进行更新，并计算出粒子的重要性权重并归一化
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
//对粒子进行重采样，进行聚类与淘汰
// Resample the particles
if(!(++resample_count_ % resample_interval_))
{
pf_update_resample(pf_);
resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);
//这里就是将一次更新之后的所有粒子发布成PoseArray，即小箭头们
// Publish the resulting cloud
// TODO: set maximum rate for publishing
if (!m_force_update) {
geometry_msgs::PoseArray cloud_msg;
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id = global_frame_id_;
cloud_msg.poses.resize(set->sample_count);
for(int i=0;i<set->sample_count;i++)
{
tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
tf::Vector3(set->samples[i].pose.v[0],
set->samples[i].pose.v[1], 0)),
cloud_msg.poses[i]);
}
particlecloud_pub_.publish(cloud_msg);
}
}
if(resampled || force_publication)
{
if (!resampled)
{
// re-compute the cluster statistics
pf_cluster_stats(pf_, pf_->sets);
}
// Read out the current hypotheses
double max_weight = 0.0;
int max_weight_hyp = -1;
std::vector<amcl_hyp_t> hyps;
hyps.resize(pf_->sets[pf_->current_set].cluster_count);
//遍历聚类后的粒子集，选择权重最大的粒子集，该粒子集的平均位姿就是本次更新后的机器人估计位姿，以下的代码目的都是为了计算出权值最大的估计位姿并发送话题与TF变换
for(int hyp_count = 0;
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
{
double weight;
pf_vector_t pose_mean;
pf_matrix_t pose_cov;
if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
{
ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
break;
}
hyps[hyp_count].weight = weight;
hyps[hyp_count].pf_pose_mean = pose_mean;
hyps[hyp_count].pf_pose_cov = pose_cov;
if(hyps[hyp_count].weight > max_weight)
{
max_weight = hyps[hyp_count].weight;
max_weight_hyp = hyp_count;
}
}
if(max_weight > 0.0)
{
ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
/*
puts("");
pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
puts("");
*/
geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = global_frame_id_;
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
p.pose.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t* set = pf_->sets + pf_->current_set;
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
p.pose.covariance[6*i+j] = set->cov.m[i][j];
}
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
p.pose.covariance[6*5+5] = set->cov.m[2][2];
/*
printf("cov:\n");
for(int i=0; i<6; i++)
{
for(int j=0; j<6; j++)
printf("%6.3f ", p.covariance[6*i+j]);
puts("");
}
*/
pose_pub_.publish(p);
last_published_pose = p;
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;
if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
}
else
{
ROS_ERROR("No pose!");
}
}
else if(latest_tf_valid_)
{
if (tf_broadcast_ == true)
{
// Nothing changed, so we'll just republish the last transform, to keep
// everybody happy.
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
}
// Is it time to save our last pose to the param server
ros::Time now = ros::Time::now();
if((save_pose_period.toSec() > 0.0) &&
(now - save_pose_last_time) >= save_pose_period)
{
this->savePoseToServer();
save_pose_last_time = now;
}
}
上述就是AmclNode类的主要实现，在上段代码中它调用了 AMCLOdom::UpdateAction与UpdateSensor，先转到AMCLOdom文件下。
四．AMCLOdom::UpdateAction函数，这个函数是为了得到从t-1时刻到t时刻机器人运动模型的概率分布模型。
//这个AMCLOdomData其实只有位姿与本时刻的变化量两部分组成
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);
switch( this->model_type )
{
//我们选择差分轮的模型来看看
case ODOM_MODEL_DIFF:
{
//运动模型在单个运动时刻的近似
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
//由于测距模型将机器人在时刻内的运动近似成旋转、平移、旋转的三个步骤，这里是在原地旋转时直接将第一次旋转略去，纯粹当做第二次旋转
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
//这是对里程计读数的相对运动参数进行计算
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
//这一段是机器人里程计模型的采样算法，计算对给定位姿Xt-1与Xt之间的相对运动参数
//加入高斯噪声模型是误差分布
// Sample pose differences
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans));
delta_trans_hat = delta_trans -
pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise);
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans));
//这一段是为了计算运动参数各自的误差概率，并叠加在先验参数上
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
break;
}
return true;五．AMCLLaser::UpdateSensor是通过特定的感知模型进行粒子滤波器的更新：
if (this->max_beams < 2)
return false;
// Apply the laser sensor model
if(this->model_type == LASER_MODEL_BEAM)
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);
else
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
return true;因此先转到AMCLLaser::LikelihoodFieldModel看看似然域测量模型的具体实现：
//遍历整个粒子集合，重新计算权重
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
//这是在里程计运动模型更新后的位姿
pose = sample->pose;
//这个函数是将传感器局部坐标经过三角变换映射到全局坐标系下
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
//预计算似然域，离散栅格化
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
step = (data->range_count - 1) / (self->max_beams - 1);
// Step size must be at least 1
if(step < 1)
step = 1;
//开始通过利用与最近物体的欧氏距离计算激光模型似然的算法，对所有特征（激光数据）进行遍历
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
//似然域测量模型简单地将最大距离读数丢弃
// This model ignores max range readings
if(obs_range >= data->range_max)
continue;
// Check for NaN
if(obs_range != obs_range)
continue;
pz = 0.0;
//计算波束的最终距离并将其转换到全局坐标系下
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
//这一块的参数在map_update_cspace函数中计算，具体来说是将整个地图代入并计算障碍物位置，并将最近距离预先计算好，再把当前的栅格代入即可
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj))
z = self->map->max_occ_dist;
else
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
//将正态分布与均匀分布混合后得到的似然结果
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
//得到单个粒子的权重并加入到粒子集中
sample->weight *= p;
total_weight += sample->weight;
}
return(total_weight);// Update the cspace distance values
void map_update_cspace(map_t *map, double max_occ_dist)
{
unsigned char* marked;
std::priority_queue<CellData> Q;
marked = new unsigned char[map->size_x*map->size_y];
memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
map->max_occ_dist = max_occ_dist;
//形成一个类似对数表的映射，相当于一次性计算完毕与最近占用栅格的距离
CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
// Enqueue all the obstacle cells
CellData cell;
cell.map_ = map;
for(int i=0; i<map->size_x; i++)
{
cell.src_i_ = cell.i_ = i;
for(int j=0; j<map->size_y; j++)
{
if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
{
map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
cell.src_j_ = cell.j_ = j;
marked[MAP_INDEX(map, i, j)] = 1;
// cout<<"cell.i_ = "<<cell.i_<<" cell.j_ = "<<cell.j_\
// <<" cell.src_i_ = "<<cell.src_i_<<" cell.src_j_ = "<<cell.src_j_<<endl;
Q.push(cell);
}
else
map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
}
}
//它不仅是单纯处理障碍物栅格，而是从近到远依次处理所有栅格与最近障碍物距离
while(!Q.empty())
{
CellData current_cell = Q.top();
// cout<<"current_cell.i_-1 = "<<current_cell.i_-1<<" current_cell.j_ = "<<current_cell.j_\
// <<" current_cell.src_i_ = "<<current_cell.src_i_<<" current_cell.src_j_ = "<<current_cell.src_j_<<endl;
//将距离障碍物最近的某个栅格的上下左右的栅格加入
if(current_cell.i_ > 0)
enqueue(map, current_cell.i_-1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if(current_cell.j_ > 0)
enqueue(map, current_cell.i_, current_cell.j_-1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.i_ < map->size_x - 1)
enqueue(map, current_cell.i_+1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.j_ < map->size_y - 1)
enqueue(map, current_cell.i_, current_cell.j_+1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
//这里弹出的是加入上下左右后重新排序之后的最小数据的栅格
Q.pop();
}
delete[] marked;
}再转到pf.c看看pf_update_sensor函数的实现：
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;
}
//更新短期似然平均与长期似然平均
// Update running averages of likelihood of samples (Prob Rob p258)
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);
//printf("w_avg: %e slow: %e fast: %e\n",
//w_avg, pf->w_slow, 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;那么，在各粒子集的权值更新后便可进行重采样，重采样中淘汰一些权值较低的粒子，并加入一些粒子。这些新加入的粒子可以是随机加入的，也可以是根据当前观测值加权后的粒子。这样便可以将新的小箭头们发布了，也可以发布新估算出的权值最大的粒子集的平均位姿。
307
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
