深入分析常规ROS机器人如何使用Navigation导航包实现实时定位

   本篇文章主要分析，常规的ROS机器人是如何使用Navigation导航包实现实时定位的，定位精度的决定性因素等内容，结构上分为详细介绍、概括总结、深入思考三大部分。

---

   注：本文首发于古月居，原文链接如下：

–

   https://www.guyuehome.com/45110

–

   本篇文章我首发在古月居，因版权原因，在CSDN不能放全文，只能放一小部分，欢迎大家前往古月居查看完整文章！！！，链接如上↑↑↑

–

---

   一、详细介绍

   常规的ROS机器人一般都会搭载，轮式里程计（编码器），姿态传感器（IMU）、激光雷达等感知传感器。

   rqt_graph是ROS中进行分析的常用工具，下图是航天三院开发的轻舟机器人运行时的节点关系图（点击或拖拽可查看大图），从下图可以看出Navigation导航包的“指挥中心“”move_base节点订阅了/odom_ekf节点发布的/odom_ekf话题，/odom_ekf中的内容是机器人搭载的轮式里程计（编码器）经过推位得到定位信息/odom与姿态传感器（IMU）的定位信息经过/robot_pose_ekf节点，即使用扩展卡尔曼滤波器（EKF）进行融合后的定位信息。

   那么我们的ROS机器人是否就是使用这个定位信息作为机器人的实时位置进行路径规划及其他应用呢？答案是否定的，下面给出解释

   move_base节点中是通过调用getRobotPose函数来获取机器人当前的位姿的

getRobotPose(global_pose, planner_costmap_ros_);

   getRobotPose函数的核心代码如下，可以看出getRobotPose函数实际上是通过监听tf树中的map坐标系与base_link坐标系的关系，从而得到map坐标系下的base_link的坐标，也就是map坐标系下机器人的位姿信息，也就是说机器人的实时定位信息是通过监听tf树中map坐标系与base_link坐标系的变换关系来计算获得的，并非使用了订阅的/odom_ekf话题中的消息。

    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = robot_base_frame_;
    robot_pose.header.stamp = ros::Time(); // latest available
    ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

    // get robot pose on the given costmap frame
    try
    {
       //通过tf获取map到base_link的关系，那么也就是map下base_link的坐标，也就是map下机器人的坐标
      tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
    }

   tf中的transform函数的具体代码如下：（lookupTransform是tf树的监听函数）

  //tf中的transform函数的具体代码如下：

 template <class T>
    T& transform(const T& in, T& out, 
		 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
  {
    // do the transform
    tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
    return out;
  }

   轻舟机器人运行时的tf树如下图所示，可以看出map坐标系与base_link坐标系之间还存在一个odom坐标系，map坐标系与odom的坐标变换关系是由/amcl节点广播出来的，odom坐标系与base_link坐标系的坐标变换关系是/robot_pose_ekf节点广播出来的，所以，我们可以先大胆的推测，机器人的实时定位信息跟/amcl节点与/robot_pose_ekf节点均有关，且/amcl节点给出的定位信息是借助激光雷达的数据，采用粒子滤波算法（PF）估计出来的，而/robot_pose_ekf节点给出的定位信息是里程计信息和IMU信息经过扩展卡尔曼滤波（EKF）融合后得到的。

   那么它们之间的关系又是怎样的呢？下面通过解读amcl包中广播odom与map坐标系的tf关系的过程来进行解释。

   本部分的源码如下：

   geometry_msgs::PoseStamped odom_to_map;
      try
      {
        tf2::Quaternion q;
        q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
        tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],        
                                              hyps[max_weight_hyp].pf_pose_mean.v[1],
                                              0.0));

        geometry_msgs::PoseStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = base_frame_id_;
        tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
        tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);

        this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
      }
      catch(const tf2::TransformException&)
      {
        ROS_DEBUG("Failed to subtract base to odom transform");
        return;
      }

      tf2::convert(odom_to_map.pose, latest_tf_);
      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_);
        geometry_msgs::TransformStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = global_frame_id_;
        tmp_tf_stamped.header.stamp = transform_expiration;
        tmp_tf_stamped.child_frame_id = odom_frame_id_;
        tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);

        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }

   以上源码可提取关键内容，总结如下：

   （1）获取base_link在世界坐标系map的坐标变换，即base_link在map下的坐标，存放在tmp_tf 中

tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], hyps[max_weight_hyp].pf_pose_mean.v[1]0.0));

   （2）将tmp_tf通过求逆变换inverse()表示为世界坐标系map到base_link的坐标变换，即map在base_link下的坐标，存放在tmp_tf_stamped.pose中

tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);

   （3）使用transform变换获取map到odom的变换，即map原点在odom坐标系下的坐标，存放在odom_to_map中，并进行了格式转换存放在latest_tf_中。

this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);

   这里的具体实现过程如下：tmp_tf_stamped中存放的是世界坐标系map到base_link的坐标变换，根据此处传入的参数可知transform函数中监听了base_link到odom坐标系的坐标变换，因此，可以看成将世界坐标系map到base_link的坐标变换再进行了一次从base_link到odom的变换，进而得到了map到odom的坐标变换，即map原点在odom坐标系下的坐标，存放在odom_to_map中

---

   （4）最后，对latest_tf_求逆，得到odom–>map的变换，即odom在map坐标系下的坐标。

tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);

   （5）广播odom–>map的坐标变换关系，即可实现对EKF的修正。

---

   二、概括总结

   本部分内容欢迎前往古月居查看，链接如下：

---

   三、深入思考

   本部分内容欢迎前往古月居查看，链接如下：

---

–

   https://www.guyuehome.com/45110

–

   本篇文章我首发在古月居，因版权原因，在CSDN不能放全文，只能放一小部分，欢迎大家前往古月居查看完整文章！！！，链接如上↑↑↑

---