本文永久更新地址:https://blog.csdn.net/u013834525/article/details/80166552
0. 写在最前面
这篇文章记录下自己在阅读amcl源码过程中的一些理解,如有不妥,欢迎评论或私信。
本文中所有代码因为篇幅等问题,都只给出主要部分,详细的自己下载下来对照着看。
作者是在校研究生,会长期跟新自己学习ROS以及SLAM过程中的一些理解,喜欢的话欢迎点个关注。
1. amcl是干什么的
Amcl is a probabilistic localization system for a robot moving in 2D. It implements the adaptive (or KLD-sampling) Monte Carlo localization approach (as described by Dieter Fox), which uses a particle filter to track the pose of a robot against a known map.
以上是官网的介绍,说白了就是2D的概率定位系统,输入激光雷达数据、里程计数据,输出机器人在地图中的位姿。用的是自适应蒙特卡洛定位方法,这个方法是在已知地图中使用粒子滤波方法得到位姿的。
如下图所示,如果里程计没有误差,完美的情况下,我们可以直接使用里程计信息(上半图)推算出机器人(base_frame)相对里程计坐标系的位置。但现实情况,里程计存在漂移以及无法忽略的累计误差,所以AMCL采用下半图的方法,即先根据里程计信息初步定位base_frame,然后通过测量模型得到base_frame相对于map_frame(全局地图坐标系),也就知道了机器人在地图中的位姿。(注意,这里虽然估计的是base到map的转换,但最后发布的是map到odom的转换,可以理解为里程计的漂移。)
2. 总体情况
2.1 CMakeLists
研究一个ROS包,肯定要先看CMakeLists。我们可以看到,这个包会生成
三个库:
- amcl_pf
- amcl_map
- amcl_sensors
- amcl
2.2 其中amcl的订阅与发布:
发布话题:- amcl_pose: geometry_msgs::PoseWithCovarianceStamped,后验位姿+一个6*6的协方差矩阵(xyz+三个转角)
- particlecloud:geometry_msgs::PoseArray,粒子位姿的数组
- 一个15秒的定时器:AmclNode::checkLaserReceived,检查 15上一次收到激光雷达数据至今是否超过15秒,如超过则报错。
发布服务:
- global_localization:&AmclNode::globalLocalizationCallback,这里是没有给定初始位姿的情况下在全局范围内初始化粒子位姿,该Callback调用pf_init_model,然后调用AmclNode::uniformPoseGenerator在地图的free点随机生成pf->max_samples个粒子
- request_nomotion_update:&AmclNode::nomotionUpdateCallback没运动模型更新的情况下也暂时更新粒子群
- set_map:&AmclNode::setMapCallback
- dynamic_reconfigure::Server动态参数配置器。
订阅话题:
- scan_topic_:sensor_msgs::LaserScan,AmclNode::laserReceived,这里是tf订阅,转换到odom_frame_id_
- initialpose:AmclNode::initialPoseReceived,这个应该就是订阅rviz中给的初始化位姿,调用AmclNode::handleInitialPoseMessage,只接受global_frame_id_(一般为map)的坐标,并重新生成粒子
- map:AmclNode::mapReceived这个在use_map_topic_的时候才订阅,否则requestMap();我这里也没有订阅,因为只使用了一个固定的地图。
3.amcl_node.cpp
这个文件实现了上述的amcl节点功能。
3.1 main
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(); } }
没啥好说的,主要就是定义了amcl节点,初始化了一个AmclNode的类对象,最关键的中断函数配置都在该类的构造函数中实现。
3.2 AmclNode
AmclNode::AmclNode() { boost::recursive_mutex::scoped_lock l(configuration_mutex_); //测量模型选择,具体参考《概率机器人》第6章内容,这里默认的likelihood_field是6.4节的似然域模型 std::string tmp_model_type; private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field")); //机器人模型选择,具体参考《概率机器人》5.4节里程计运动模型内容,这里默认的diff应该是差分型(两轮驱动)的机器人?,另外一个是全向机器人? private_nh_.param("odom_model_type", tmp_model_type, std::string("diff")); //从参数服务器中获取初始位姿及初始分布 updatePoseFromServer(); //定义话题及订阅等,具体在本文第2章有讲 pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true); }
3.3 requestMap
AmclNode::requestMap() { //一直请求服务static_map直到成功,该服务在map_server这个包的map_server节点中进行定义 while(!ros::service::call("static_map", req, resp)) { ROS_WARN("Request for map failed; trying again..."); ros::Duration d(0.5); d.sleep(); } handleMapMessage( resp.map ); }
AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg) { //free相应的指针 freeMapDependentMemory(); //转换成标准地图,0->-1(不是障碍);100->+1(障碍);else->0(不明) 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_); // 从参数服务器获取初始位姿及方差放到pf中 updatePoseFromServer(); // 定义里程计与激光雷达并初始化数据 // 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_); // In case the initial pose message arrived before the first map, // try to apply the initial pose now that the map has arrived. applyInitialPose(); }
3.4 laserReceived
其中变量pose是base相对于odom的位姿;pf_odom_pose_则是上一时刻base相对于odom的位姿,用于后续得到机器人的相对运动程度。
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) { // Do we have the base->base_laser Tx yet?感觉这里是支持多个激光雷达的,找到当前响应的激光雷达之前存储的信息, //如相对于base的转换,是否更新等,使用map结构直接通过id来找到对应序号即可 //如果之前没有备案则在map结构中备案,然后存到frame_to_laser_及lasers_中下次备用 if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end()) { 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]; } // Where was the robot when this scan was taken?获得base在激光雷达扫描时候相对于odom的相对位姿 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; } // If the robot has moved, update the filter else if(pf_init_ && lasers_update_[laser_index])//如果已经初始化并需要更新则更新运动模型 { //这是amcl_odom.cpp中最重要的一个函数,实现了用运动模型来更新现有的每一个粒子的位姿(这里得到的只是当前时刻的先验位姿) // Use the action data to update the filter odom_->UpdateAction(pf_, (AMCLSensorData*)&odata); } bool resampled = false; // If the robot has moved, update the filter if(lasers_update_[laser_index]) {//接下来一大片都是对原始激光雷达数据进行处理,转换到AMCLLaserData。包括角度最小值、增量到base_frame的转换、测距距离最大值、最小值。 //具体看代码,篇幅限制不详细列了 try { tf_->transformQuaternion(base_frame_id_, min_q, min_q); tf_->transformQuaternion(base_frame_id_, inc_q, inc_q); } ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_); range_min = std::max(laser_scan->range_min, (float)laser_min_range_); ldata.ranges = new double[ldata.range_count][2]; 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)//这里将最小值当最大值处理,因为在类似likelihood_field模型中,会直接将最大值丢弃 ldata.ranges[i][0] = ldata.range_max; } //注意这里是amcl_laser.cpp的UpdateSensor,不是amcl_sensor.cpp的。通过判断前面设置的测量模型调用pf_update_sensor, //该函数需要传入相应模型的处理函数,这里所有的测量模型在《概率机器人》的第六章有详细讲,具体自己看,后面只针对自己使用的likelihood_field模型讲一下 //pf_update_sensor实现对所有粒子更新权重,并归一化、计算出《概率机器人》8.3.5的失效恢复的长期似然和短期似然 lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata); lasers_update_[laser_index] = false; pf_odom_pose_ = pose; //多少次激光雷达回调之后进行重采样呗,我这里resample_interval_=0.5,只有一个激光雷达,每次都更新。 // Resample the particles if(!(++resample_count_ % resample_interval_)) { //按照一定的规则重采样粒子,包括前面说的失效恢复、粒子权重等,然后放入到kdtree,暂时先理解成关于位姿的二叉树, //然后进行聚类,得到均值和方差等信息,个人理解就是将相近的一堆粒子融合成一个粒子了,没必要维持太多相近的@TODO pf_update_resample(pf_); resampled = true; } // Publish the resulting cloud // TODO: set maximum rate for publishing if (!m_force_update) { //将新粒子发布到全局坐标系下,一般是map particlecloud_pub_.publish(cloud_msg); } } if(resampled || force_publication) { //遍历所有粒子簇,找出权重均值最大的簇,其平均位姿就是我们要求的机器人后验位姿,到此一次循环已经所有完成 for(int hyp_count = 0; hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++) { if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) { 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) { 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++) { p.pose.covariance[6*i+j] = set->cov.m[i][j]; } } p.pose.covariance[6*5+5] = set->cov.m[2][2]; pose_pub_.publish(p); last_published_pose = p; //这里就是所说的,map~base减去odom~base得到map~odom,最后发布的是map~odom。 // 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(resampled || force_publication) { } }
3.5 AMCLOdom::UpdateAction
// Apply the action model bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) { 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; //计算里程计得到的ut,即用旋转、直线运动和另一个旋转来表示相对运动 // 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] + //如果原地旋转的情况,定义第一个旋转为0,认为所有旋转都是第二个旋转做的 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); //这里作者比书本多考虑了一种情况,就是机器人旋转小角度,然后往后走的情况,比如旋转-20°,然后往后走,计算出来可能是160°, //但实际只转了20°。按书本可能是比较大的方差(160°),不太准确,但这里的话还是按照比较小的方差来采样。 //但我表示怀疑,万一真的转了160°再前向运动的话怎么办?还是用比较小的方差来采样,可能会使得采样的准确率下降。 //实际中,编码器采样率很高,且设定了一个很小的变化角度就进行运动模型更新,所以我说的这种情况几乎不会发生 // 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; // 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; } } } return true; }
3.6 LikelihoodFieldModel
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set) { // 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 //这个应该是通过机器人与全局坐标系的位姿(每个粒子的位姿),计算激光雷达相对于全局坐标系的位姿。方便后续将激光雷达扫描的终点转换到全局坐标系 //点进去pf_vector_coord_add这个函数,b对应的就是《概率机器人》P128,6.4第一个公式中的机器人在t时刻的位姿,a代表“与机器人固连的传感器局部坐标系位置” pose = pf_vector_coord_add(self->laser_pose, pose);//这里的laser_pose其实是laserReceived函数一开始初始化每个激光雷达时得到的激光雷达在base中的位姿 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;//无法解释的随机测量的分母 //因为时间问题,并不是全部点都进行似然计算的,这里只是间隔地选点,我这里设置的是一共选择60个点~~~ step = (data->range_count - 1) / (self->max_beams - 1); for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; obs_bearing = data->ranges[i][1]; //继续上面《概率机器人》P128的后半段公式,得到激光雷达点最远端的世界坐标 // 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_cspace.cpp的map_update_cspace中实现遍历计算,该函数是被AMCLLaser::SetModelLikelihoodField调用 //而这个函数则只在AmclNode::handleMapMessage中调用???而这个handle在多个地方调用,暂时未清楚@TODO // 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;//这里有点不太懂。。。理论上应该是上面注释掉的公式。这里应该是为了防止噪声,比如某个出现错误的0,其余都不管用了 }//每个粒子的所有激光雷达点循环 sample->weight *= p; total_weight += sample->weight; }//粒子循环 return(total_weight); }