ROS机器人如何使用Navigation导航包实现实时定位

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

一、详细介绍

常规的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::getIdentity(), global_pose.pose);
geometry_msgs::PoseStampedrobot_pose;
tf2::getIdentity(), robot_pose.pose);
robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp=ros::Time();//latestavailable
ros::Time current_time = ros::now(); // save time for checking tf delay later


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

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

 //tf中的transform函数的具体代码如下：
template
  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::Quaternionq;
q.setRPY(0,0,hyps[max_weight_hyp].pf_pose_mean.v[2]);
tf2::Transformtmp_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::PoseStampedtmp_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("Failedtosubtractbasetoodomtransform");
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::Timetransform_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的修正。

二、概括总结

☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆

总的来说，/robot_pose_ekf节点会高频的广播给出使用里程计信息和IMU信息经过扩展卡尔曼滤波（EKF）融合后得到的定位信息，这个定位信息就是odom坐标系下的机器人位姿，如果这个定位信息是准确的，odom坐标系将与map坐标系近似于重合，此时，定位信息可以看成全局坐标系map下的机器人位姿信息。

然而里程计和IMU会有累计误差，且该误差会随着时间的推移不断增大，尤其是车轮打滑的情况下，这个偏移会很大，即odom坐标系会逐渐偏离map坐标系。此时，将ekf输出的定位信息作为机器人在全局坐标系下的位姿信息是不合适的，这也是为什么还需要AMCL通过粒子滤波输出定位信息的原因。

AMCL功能包借助激光雷达的感知信息，通过粒子滤波低频的广播出odom坐标系与map坐标系的偏差，在这个过程中，会将粒子滤波估计出的全局坐标系map下的机器人的位姿信息当做“真值”去使用，即认为粒子滤波估计出的定位信息是接近于真值的。用这个map→base_link（机器人）的坐标关系减去ekf输出的base_link→odom的坐标关系，就是AMCL广播的odom与map坐标系的偏差，用这个偏差叠加上ekf的输出来对efk估计的定位信息进行纠正，并作为机器人实时的定位信息。

☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆

三、深入思考

1、odom坐标系与map坐标系的偏差所代表的意义是什么？

odom坐标系与map坐标系的偏差实际上是使用激光雷达感知信息的粒子滤波估计出定位信息与使用里程计和IMU信息通过扩展卡尔曼滤波估计出的定位信息的差值。

在将AMCL使用粒子滤波估计出的机器人在全局坐标系下的位姿信息，作为机器人真实的位姿信息的情况下，odom坐标系与map坐标系的偏差可以看成，ekf估计出的机器人的位姿信息与真实值之间的偏差，即odom与map坐标系差的越大，ekf当前的估计值与真实值的差距也就越大，越不准确。

2、粒子滤波与扩展卡尔曼滤波输出的定位信息那个更准确？

使用激光雷达感知信息的粒子滤波估计出定位信息比使用里程计和IMU信息通过扩展卡尔曼滤波估计出的定位信息要准确，尤其是在机器人的运动存在打滑现象时，下图中的给出了机器人运行一段时间回到起点后，ekf和amcl输出的定位信息的偏差，可见在ekf在x轴上的偏差高达1.49m，而amcl的偏差仅在0.08m，运行多圈以后ekf的偏差甚至达到了5.3m，而此时amcl的偏差仅为0.2m，可以看出经过实验测试，在机器人存在打滑现象时，amcl输出的定位信息的精度要远高于ekf输出的定位信息。这也是为什么可以将amcl输出的定位信息近似当真值使用来对ekf进行修正的原因。

3、既然AMCL输出的定位信息比ekf输出的定位信息要准确，为什么不直接使用AMCL输出的定位信息作为机器人的定位信息使用，而是使用AMCL对ekf的输出进行修正？为什么AMCL不直接广播map到base_link的坐标变换关系？

因为，AMCL的定位信息准确，但计算量较大，只能输出一个低频的定位信息，如10hz，而ekf的定位信息误差较大，但可以高频的输出定位信息，如100hz。采用AMCL的对ekf进行修正的模式，即可以较好的保证定位信息的实时性，又能较好的保证定位信息的准确性。

4、ROS机器人是如何使用Navigation导航包实现定位的精度的决定性因素是什么？

ROS机器人是如何使用Navigation导航包实现定位的精度的决定性因素是 AMCL中粒子滤波的估计精度。假设AMCL输出的定位信息的频率是10HZ，ekf输出的定位信息的频率是100hz。则在0.1 _ n时刻机器人使用的定位信息就是AMCL输出的定位信息，在0.1 _ n ~ 0.1 *（n+1） 的时间段内，比如0.16时刻输出的定位信息 是0.1时刻AMCL输出的定位信息与0.1时刻ekf输出的定位信息的差值，加上0.16时刻ekf输出的定位信息。即0.16时刻的定位使用的是0.1时刻的AMCL对ekf的修正（0.1时刻的map→base_link）加上0.16时刻的ekf输出（0.16时刻的base_link → odom）。其中n取任意整数

所以，在0.1 _ n时刻的机器人定位信息的估计精度就是AMCl中粒子滤波的估计精度，在0.1 _ n ~ 0.1 *（n+1）的时间段内，ekf的估计精度越高只能保证，在该时间段内的估计偏差越小，但是最终取决定性作用的还是AMCL的估计精度，举个简单的例子，在0.1时刻，AMCl估计的定位信息是 机器人处于x轴的5m处（认为机器人0.1时刻真实位置也在5m处），0.1时刻ekf估计的定位信息是机器人处于x轴的5.2m处，此时0.1时刻amcl的修正信息是0.2m ，在0.16时刻ekf的估计位置是5.8m处，然而机器人的真实位置在5.4m处，0.16时刻AMCl的修正量依然使用的是0.1时刻的修正量，最终输出的定位信息是在5.6m处，与真实信息差了0.2m，然而在0.2时刻，此时机器人位于6.01m处，AMCl估计的机器人也6m处，ekf估计的机器人位于6.6m处，此时ekf的估计偏差是0.61，AMCl对ekf的纠正是0.6，此时输出的定位信息即6m，与真实位置偏差是0.01，也就是AMCL的估计精度。

所以，总的来说，ROS机器人是如何使用Navigation导航包实现定位的精度的决定性因素是 AMCL中粒子滤波的估计精度，即使是在0.1 _ n ~ 0.1 _（n+1）的时间段内ekf的偏移特别大，那在 0.1 *（n+1）也会被修正为AMCL估计的位姿，与真实位置的定位偏差也会变为AMCL的估计偏差。

审核编辑：汤梓红