本篇文章主要分析，常規(guī)的ROS機(jī)器人是如何使用Navigation導(dǎo)航包實現(xiàn)實時定位的，定位精度的決定性因素等內(nèi)容，結(jié)構(gòu)上分為詳細(xì)介紹、概括總結(jié)、深入思考三大部分。

一、詳細(xì)介紹

常規(guī)的ROS機(jī)器人一般都會搭載，輪式里程計（編碼器），姿態(tài)傳感器（IMU）、激光雷達(dá)等感知傳感器。

rqt_graph是ROS中進(jìn)行分析的常用工具，下圖是航天三院開發(fā)的輕舟機(jī)器人運(yùn)行時的節(jié)點關(guān)系圖（點擊或拖拽可查看大圖），從下圖可以看出Navigation導(dǎo)航包的“指揮中心“”move_base節(jié)點訂閱了/odom_ekf節(jié)點發(fā)布的/odom_ekf話題，/odom_ekf中的內(nèi)容是機(jī)器人搭載的輪式里程計（編碼器）經(jīng)過推位得到定位信息/odom與姿態(tài)傳感器（IMU）的定位信息經(jīng)過/robot_pose_ekf節(jié)點，即使用擴(kuò)展卡爾曼濾波器（EKF）進(jìn)行融合后的定位信息。

那么我們的ROS機(jī)器人是否就是使用這個定位信息作為機(jī)器人的實時位置進(jìn)行路徑規(guī)劃及其他應(yīng)用呢？答案是否定的，下面給出解釋

move_base節(jié)點中是通過調(diào)用getRobotPose函數(shù)來獲取機(jī)器人當(dāng)前的位姿的

getRobotPose(global_pose, planner_costmap_ros_);

getRobotPose函數(shù)的核心代碼如下，可以看出getRobotPose函數(shù)實際上是通過監(jiān)聽tf樹中的map坐標(biāo)系與base_link坐標(biāo)系的關(guān)系，從而得到map坐標(biāo)系下的base_link的坐標(biāo)，也就是map坐標(biāo)系下機(jī)器人的位姿信息，也就是說機(jī)器人的實時定位信息是通過監(jiān)聽tf樹中map坐標(biāo)系與base_link坐標(biāo)系的變換關(guān)系來計算獲得的，并非使用了訂閱的/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的關(guān)系，那么也就是map下base_link的坐標(biāo)，也就是map下機(jī)器人的坐標(biāo)
tf_.transform(robot_pose,global_pose,costmap->getGlobalFrameID());
  }

tf中的transform函數(shù)的具體代碼如下：（lookupTransform是tf樹的監(jiān)聽函數(shù)）

 //tf中的transform函數(shù)的具體代碼如下：
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;
 }

輕舟機(jī)器人運(yùn)行時的tf樹如下圖所示，可以看出map坐標(biāo)系與base_link坐標(biāo)系之間還存在一個odom坐標(biāo)系，map坐標(biāo)系與odom的坐標(biāo)變換關(guān)系是由/amcl節(jié)點廣播出來的，odom坐標(biāo)系與base_link坐標(biāo)系的坐標(biāo)變換關(guān)系是/robot_pose_ekf節(jié)點廣播出來的，所以，我們可以先大膽的推測，機(jī)器人的實時定位信息跟/amcl節(jié)點與/robot_pose_ekf節(jié)點均有關(guān)，且/amcl節(jié)點給出的定位信息是借助激光雷達(dá)的數(shù)據(jù)，采用粒子濾波算法（PF）估計出來的，而/robot_pose_ekf節(jié)點給出的定位信息是里程計信息和IMU信息經(jīng)過擴(kuò)展卡爾曼濾波（EKF）融合后得到的。

那么它們之間的關(guān)系又是怎樣的呢？下面通過解讀amcl包中廣播odom與map坐標(biāo)系的tf關(guān)系的過程來進(jìn)行解釋。

本部分的源碼如下：

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

以上源碼可提取關(guān)鍵內(nèi)容，總結(jié)如下：

（1）獲取base_link在世界坐標(biāo)系map的坐標(biāo)變換，即base_link在map下的坐標(biāo)，存放在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()表示為世界坐標(biāo)系map到base_link的坐標(biāo)變換，即map在base_link下的坐標(biāo)，存放在tmp_tf_stamped.pose中

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

（3）使用transform變換獲取map到odom的變換，即map原點在odom坐標(biāo)系下的坐標(biāo)，存放在odom_to_map中，并進(jìn)行了格式轉(zhuǎn)換存放在latest_tf_中。

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

這里的具體實現(xiàn)過程如下：tmp_tf_stamped中存放的是世界坐標(biāo)系map到base_link的坐標(biāo)變換，根據(jù)此處傳入的參數(shù)可知transform函數(shù)中監(jiān)聽了base_link到odom坐標(biāo)系的坐標(biāo)變換，因此，可以看成將世界坐標(biāo)系map到base_link的坐標(biāo)變換再進(jìn)行了一次從base_link到odom的變換，進(jìn)而得到了map到odom的坐標(biāo)變換，即map原點在odom坐標(biāo)系下的坐標(biāo)，存放在odom_to_map中

（4）最后，對latest_tf_求逆，得到odom—>map的變換，即odom在map坐標(biāo)系下的坐標(biāo)。

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

（5）廣播odom—>map的坐標(biāo)變換關(guān)系，即可實現(xiàn)對EKF的修正。

二、概括總結(jié)

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

總的來說，/robot_pose_ekf節(jié)點會高頻的廣播給出使用里程計信息和IMU信息經(jīng)過擴(kuò)展卡爾曼濾波（EKF）融合后得到的定位信息，這個定位信息就是odom坐標(biāo)系下的機(jī)器人位姿，如果這個定位信息是準(zhǔn)確的，odom坐標(biāo)系將與map坐標(biāo)系近似于重合，此時，定位信息可以看成全局坐標(biāo)系map下的機(jī)器人位姿信息。

然而里程計和IMU會有累計誤差，且該誤差會隨著時間的推移不斷增大，尤其是車輪打滑的情況下，這個偏移會很大，即odom坐標(biāo)系會逐漸偏離map坐標(biāo)系。此時，將ekf輸出的定位信息作為機(jī)器人在全局坐標(biāo)系下的位姿信息是不合適的，這也是為什么還需要AMCL通過粒子濾波輸出定位信息的原因。

AMCL功能包借助激光雷達(dá)的感知信息，通過粒子濾波低頻的廣播出odom坐標(biāo)系與map坐標(biāo)系的偏差，在這個過程中，會將粒子濾波估計出的全局坐標(biāo)系map下的機(jī)器人的位姿信息當(dāng)做“真值”去使用，即認(rèn)為粒子濾波估計出的定位信息是接近于真值的。用這個map→base_link（機(jī)器人）的坐標(biāo)關(guān)系減去ekf輸出的base_link→odom的坐標(biāo)關(guān)系，就是AMCL廣播的odom與map坐標(biāo)系的偏差，用這個偏差疊加上ekf的輸出來對efk估計的定位信息進(jìn)行糾正，并作為機(jī)器人實時的定位信息。

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

三、深入思考

1、odom坐標(biāo)系與map坐標(biāo)系的偏差所代表的意義是什么？

odom坐標(biāo)系與map坐標(biāo)系的偏差實際上是使用激光雷達(dá)感知信息的粒子濾波估計出定位信息與使用里程計和IMU信息通過擴(kuò)展卡爾曼濾波估計出的定位信息的差值。

在將AMCL使用粒子濾波估計出的機(jī)器人在全局坐標(biāo)系下的位姿信息，作為機(jī)器人真實的位姿信息的情況下，odom坐標(biāo)系與map坐標(biāo)系的偏差可以看成，ekf估計出的機(jī)器人的位姿信息與真實值之間的偏差，即odom與map坐標(biāo)系差的越大，ekf當(dāng)前的估計值與真實值的差距也就越大，越不準(zhǔn)確。

2、粒子濾波與擴(kuò)展卡爾曼濾波輸出的定位信息那個更準(zhǔn)確？

使用激光雷達(dá)感知信息的粒子濾波估計出定位信息比使用里程計和IMU信息通過擴(kuò)展卡爾曼濾波估計出的定位信息要準(zhǔn)確，尤其是在機(jī)器人的運(yùn)動存在打滑現(xiàn)象時，下圖中的給出了機(jī)器人運(yùn)行一段時間回到起點后，ekf和amcl輸出的定位信息的偏差，可見在ekf在x軸上的偏差高達(dá)1.49m，而amcl的偏差僅在0.08m，運(yùn)行多圈以后ekf的偏差甚至達(dá)到了5.3m，而此時amcl的偏差僅為0.2m，可以看出經(jīng)過實驗測試，在機(jī)器人存在打滑現(xiàn)象時，amcl輸出的定位信息的精度要遠(yuǎn)高于ekf輸出的定位信息。這也是為什么可以將amcl輸出的定位信息近似當(dāng)真值使用來對ekf進(jìn)行修正的原因。

3、既然AMCL輸出的定位信息比ekf輸出的定位信息要準(zhǔn)確，為什么不直接使用AMCL輸出的定位信息作為機(jī)器人的定位信息使用，而是使用AMCL對ekf的輸出進(jìn)行修正？為什么AMCL不直接廣播map到base_link的坐標(biāo)變換關(guān)系？

因為，AMCL的定位信息準(zhǔn)確，但計算量較大，只能輸出一個低頻的定位信息，如10hz，而ekf的定位信息誤差較大，但可以高頻的輸出定位信息，如100hz。采用AMCL的對ekf進(jìn)行修正的模式，即可以較好的保證定位信息的實時性，又能較好的保證定位信息的準(zhǔn)確性。

4、ROS機(jī)器人是如何使用Navigation導(dǎo)航包實現(xiàn)定位的精度的決定性因素是什么？

ROS機(jī)器人是如何使用Navigation導(dǎo)航包實現(xiàn)定位的精度的決定性因素是 AMCL中粒子濾波的估計精度。假設(shè)AMCL輸出的定位信息的頻率是10HZ，ekf輸出的定位信息的頻率是100hz。則在0.1 _ n時刻機(jī)器人使用的定位信息就是AMCL輸出的定位信息，在0.1 _ n ~ 0.1 *（n+1） 的時間段內(nèi)，比如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取任意整數(shù)

所以，在0.1 _ n時刻的機(jī)器人定位信息的估計精度就是AMCl中粒子濾波的估計精度，在0.1 _ n ~ 0.1 *（n+1）的時間段內(nèi)，ekf的估計精度越高只能保證，在該時間段內(nèi)的估計偏差越小，但是最終取決定性作用的還是AMCL的估計精度，舉個簡單的例子，在0.1時刻，AMCl估計的定位信息是 機(jī)器人處于x軸的5m處（認(rèn)為機(jī)器人0.1時刻真實位置也在5m處），0.1時刻ekf估計的定位信息是機(jī)器人處于x軸的5.2m處，此時0.1時刻amcl的修正信息是0.2m ，在0.16時刻ekf的估計位置是5.8m處，然而機(jī)器人的真實位置在5.4m處，0.16時刻AMCl的修正量依然使用的是0.1時刻的修正量，最終輸出的定位信息是在5.6m處，與真實信息差了0.2m，然而在0.2時刻，此時機(jī)器人位于6.01m處，AMCl估計的機(jī)器人也6m處，ekf估計的機(jī)器人位于6.6m處，此時ekf的估計偏差是0.61，AMCl對ekf的糾正是0.6，此時輸出的定位信息即6m，與真實位置偏差是0.01，也就是AMCL的估計精度。

所以，總的來說，ROS機(jī)器人是如何使用Navigation導(dǎo)航包實現(xiàn)定位的精度的決定性因素是 AMCL中粒子濾波的估計精度，即使是在0.1 _ n ~ 0.1 _（n+1）的時間段內(nèi)ekf的偏移特別大，那在 0.1 *（n+1）也會被修正為AMCL估計的位姿，與真實位置的定位偏差也會變?yōu)锳MCL的估計偏差。

審核編輯：湯梓紅