• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 yuanguobin01 3164次浏览 0个评论

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激光雷达似然域测量模型

 


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明深入AMCL(三):AMCL手动初始化后如何自动定位
喜欢 (0)

您必须 登录 才能发表评论!

加载中……