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的估計偏差。

審核編輯：湯梓紅