ROS源代码阅读（8）——定位

2021SC@SDUSC
ROS源代码阅读（8）

“SLAM定位： 机器人定位的方法可以分为非自主定位与自主定位两大类。 非自主定位是在定位的过程中机器人需要借助机器人本身以外的装置如：全球定位系统(GPS)、全局视觉系统等进行定位； 自主定位是机器人仅依靠机器人本身携带的传感器进行定位。由于在室内环境中，不能使用GPS，而安装其它的辅助定位系统比较麻烦。 因此机器人一般采用自主定位的方法。 按照初始位姿是否已知，可把机器人自主定位分为初始位姿已知的位姿跟踪(Pose tracking)和初始位姿未知的全局定位(Global localization)。 位姿跟踪是在已知机器人的初始位姿的条件下，在机器人的运动过程中通过将观测到的特征与地图中的特征进行匹配，求取它们之间的差别，进而更新机器人的位姿的机器人定位方法。位姿跟踪通常采用扩展卡尔曼滤波器(Extended Kalman Filter，EKF)来实现。该方法采用高斯分布来近似地表示机器人位姿的后验概率分布，其计算过程主要包括三步：首先是根据机器人的运模型预测机器人的位姿，然后将观测信息与地图进行匹配，最后根据预测后的机器人位姿以及匹配的特征计算机器人应该观测到的信息，并利用应该观测到的信息与实际观测到的信息之间的差距来更新机器人的位姿。 全局定位是在机器人的初始位姿不确定的条件下，利用局部的、不完全的观测信息估计机器人的当前位姿。能否解决最典型而又最富挑战性的“绑架恢复”问题在一定程度上反应了机器人全局定位方法的鲁棒性与可靠性。 实际情况是机器人在移动，周围物体（比如墙，各种路标等）是静止的。但是相对机器人而言，是墙在移动。我们通过特征点的匹配，得出墙移动后的位置（x2,y2,z2,alpha2,beta2,gama2)，相机中两帧间的位置差（x2-x1,y2-y1,z2-z1,…)。于是，我们就可以得出机器人的位移，从而实现机器人的定位。 SLAM建图： 我们所谓的地图，即所有路标点的集合。一旦我们确定了路标点的位置，那就可以说我们完成了建图。 稠密建图： 单个图像中的像素，只能提供物体与相机成像平面的角度以及物体采集到的亮度，而无法提供物体的距离（Range）。而在稠密重建，我们需要知道每一个像素点（或大部分像素点）的距离，大致上有以下几种解决方案： 1.使用单目相机，利用移动相机之后进行三角化，测量像素的距离。 2.使用双目相机，利用左右目的视差计算像素的距离（多目原理相同）。 3.使用 RGB-D 相机直接获得像素距离。 使用 RGB-D 进行稠密重建往往是更常见的选择。而单目双目的好处，是在目前 RGB-D 还无法很好应用的室外、大场景场合中，仍能通过立体视觉估计深度信息。 点云地图（Point_Cloud Map） 1.在生成每帧点云时，去掉深度值太大或无效的点。 2.利用统计滤波器方法去除孤立点。 3.该滤波器统计每个点与它最近 N 个点的距离值的分布，去除距离均值过大的点。这样，我们保留了那些“粘在一起”的点，去掉了孤立的噪声点。 最后，利用体素滤波器（Voxel Filter）进行降采样。由于多个视角存在视野重叠，在重叠区域会存在大量的位置十分相近的点。这会无益地占用许多内存空间。体素滤波保证在某个一定大小的立方体（或称体素）内仅有一个点，相当于对三维空间进行了降采样，从而节省了很多存储空间。 建图和定位一样，过程中会产生误差，并且误差会逐渐积累。有许多技术能补偿这些误差，比如那些能再现某些特征过去的值的方法（也就是说，图像匹配法或者环路闭合检测法），或者对现有的地图进行处理——以融合该特征在不同时间的不同值。此外还有一些用于SLAM统计学的技术可起到作用，包括卡尔曼滤波、粒子滤波（实际上是一种蒙特卡罗方法）以及扫描匹配的数据范围。”

amcl就是2D的概率定位系统，输入激光雷达数据、里程计数据，输出机器人在地图中的位姿。用的是自适应蒙特卡洛定位方法，这个方法是在已知地图中使用粒子滤波方法得到位姿的。如果里程计没有误差，完美的情况下，我们可以直接使用里程计信息推算出机器人（base_frame）相对里程计坐标系的位置。但现实情况，里程计存在漂移以及无法忽略的累计误差，所以AMCL采用先根据里程计信息初步定位base_frame，然后通过测量模型得到base_frame相对于map_frame（全局地图坐标系）的偏移，也就知道了机器人在地图中的位姿。（注意，这里虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为里程计的漂移。）
AMCL定位算法直接影响机器人的导航精度，因此对源码的研究和分析非常必要。

看CMakeLists。我们可以看到，这个包会生成
三个库：
amcl_pf
amcl_map
amcl_sensors

一个节点：
amcl

其中amcl的订阅与发布：
发布话题：
amcl_pose： geometry_msgs::PoseWithCovarianceStamped，后验位姿+一个6*6的协方差矩阵（xyz+三个转角）
particlecloud：geometry_msgs::PoseArray，粒子位姿的数组
一个15秒的定时器：AmclNode::checkLaserReceived，检查上一次收到激光雷达数据至今是否超过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();我这里也没有订阅，因为只使用了一个固定的地图。
主要看amcl.cpp
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的类对象，最关键的中断函数配置都在该类的构造函数中实现。

AmclNode

AmclNode::AmclNode()
{
  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
  std::string tmp_model_type;
  private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
  
  private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
//从参数服务器中获取初始位姿及初始分布
  updatePoseFromServer();
  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
}

requestMap

AmclNode::requestMap()
{
  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 );
}

一直请求服务static_map直到成功，该服务在map_server这个包的map_server节点中进行定义

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();
}

这里请求服务static_server提供map，然后调用handleMapMessage处理地图信息。这里的地图类型是nav_msgs::OccupancyGrid。

laserReceived

感觉这里是支持多个激光雷达的，找到当前响应的激光雷达之前存储的信息，如相对于base的转换，是否更新等，使用map结构直接通过id来找到对应序号即可,如果之前没有备案则在map结构中备案，然后存到frame_to_laser_及lasers_中下次备用

AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{ 
  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，
    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_))
    {
      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)
  {
 
  }
}

其中变量pose是base相对于odom的位姿；pf_odom_pose_则是上一时刻base相对于odom的位姿，用于后续得到机器人的相对运动程度。

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，即用旋转、直线运动和另一个旋转来表示相对运动
    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);
    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;
      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));
      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;
}