ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法

ROS AMCL 算法根据订阅到的地图数据配合激光扫描特征,使用粒子滤波获取最佳定位点,该点称为M(point on map), 它是相对于地图map上的坐标,也就是base_link相对map上的坐标。


odom 的原点是机器人启动时刻的位置,它在map上的位置或转换矩阵是未知的。但是AMCL可以根据最佳粒子的位置推算出 odom->map的tf转换信息并发布到 tf主题上。因为base_link->odom的tf转换信息是每时每刻都在发布的,所以它是已知的。


已知tf转换:

map->base_link

base_link->odom


下面的公式就可以推算:

map->odom = map->base_link - base_link->odom


看一下代码是怎么实现的:

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

hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp

第10行   tmp_tf = 从map原点看base_link的位置

第14行   tmp_tf.inverse()  = 以为Mp为原点,map原点的位置,就是在base_link坐标系下map 原点的位置

第17行   进行 base_link坐标系下的点转换到odom坐标系,也就是把map原点转换到odom坐标系下,等于从odom原点看map原点的位置。放到latest_tf_变量里面。

第37行   发布tf转换关系,latest_tf_.inverse() 得到的是从map原点看odom原点的位置,也就是 odom->map的转换关系


总结

从W的原点看A所在的位置坐标 p,就是A->W转换矩阵,称为WAT.   Ap为A坐标系的点, 它在W上的坐标  WpWAT*Ap

©️2020 CSDN 皮肤主题: 大白 设计师: CSDN官方博客 返回首页
实付0元
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值