深入AMCL(三):AMCL手动初始化后如何自动定位

AMCL首次启动总有个烦恼的问题，就是手动初始化位姿很难给出准确的，也不会自动更新，像下面这样

像这样做成产品当然不行啦。
解决上面的问题主要有两个思想
（1）用户给个大概初始位姿，算法自动更新寻找真实值
（2）不需要用户给初始位姿，采用其它辅助方法

----------------------------------------分割线-----------------------------------------------
下面主要讨论第一种实现方式。
思想：让AMCL算法中在里程计没更新下自动更新粒子
1.不知道大家有没有注意AMCL算法中提供了ROS服务

nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);

所以你只要在终端敲入

rosservice call /amcl/request_nomotion_update

粒子就会逐渐更新啦。
进一步查看AMCL中这个服务回调函数干了啥

bool AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req,
                                      std_srvs::Empty::Response& res)
{
	m_force_update = true;
	//ROS_INFO("Requesting no-motion update");
	return true;
}

很明显只让一个变量为True，由此我们直接开机自动就把这个变量置True，设置个条件退出（就是这样简单了），所以直接在AmclNode::laserReceived函数中的这段后面添加

  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;

需要添加的。

    pf_sample_set_t *tmp = pf_->sets + pf_->current_set;
    if (tmp->sample_count < (min_particles_ + 200) && m_force_update == true)   //add by yuanguobin
    {
      //if (!autoReInitialFlag)
      //{
         //AutoReInitialPose(last_published_pose);
      //}
      //else
      //{
        m_force_update=false;
      //}
     //autoReInitialFlag = true;
    }

还有一个函数需要修改void AmclNode::handleInitialPoseMessage()

  applyInitialPose();

  m_force_update = true; //add by yuanguobin
  //autoReInitialFlag = false; //add by yuanguobin
}

以上简单修改两处，就可以试试效果


真烦CSDN不能上传视频。。
当然上面还是可能会遇到最后并没有完全找到真值，总是差一点。但是多加一个函数就可以了，假如需要使用这段代码，需要把上面修改的代码中被注释的打开。

/*add by yuanguobin  */
void AmclNode::AutoReInitialPose(geometry_msgs::PoseWithCovarianceStamped & last_pose)
{
  tf2::Transform pose_new;
  tf2::convert(last_pose.pose.pose, pose_new);
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
  pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  // Copy in the covariance, converting from 6-D to 3-D
 
  pf_init_pose_cov.m[0][0] = 0.25;
  pf_init_pose_cov.m[1][1] = 0.25;
  pf_init_pose_cov.m[2][2] = 4.0/180.0*M_PI;

  delete initial_pose_hyp_;
  initial_pose_hyp_ = new amcl_hyp_t();
  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
  applyInitialPose();
  pf_init_=true;
}

一个小细节提示下，假如你想给的初始较差，记得去AMCL中把pf_init_pose_cov这个值改大点。

下一期讲讲如何实现不需要人的。

欢迎各位爱好者留言和加微信讨论（15112119047）

深入AMCL(一):AMCL中轮式里程计误差模型参数
深入AMCL(二):2d激光雷达似然域测量模型