本篇文章主要分析,常規(guī)的ROS機(jī)器人是如何使用Navigation導(dǎo)航包實(shí)現(xiàn)實(shí)時(shí)定位的,定位精度的決定性因素等內(nèi)容,結(jié)構(gòu)上分為詳細(xì)介紹、概括總結(jié)、深入思考三大部分。
一、詳細(xì)介紹
常規(guī)的ROS機(jī)器人一般都會(huì)搭載,輪式里程計(jì)(編碼器),姿態(tài)傳感器(IMU)、激光雷達(dá)等感知傳感器。
rqt_graph是ROS中進(jìn)行分析的常用工具,下圖是航天三院開發(fā)的輕舟機(jī)器人運(yùn)行時(shí)的節(jié)點(diǎn)關(guān)系圖(點(diǎn)擊或拖拽可查看大圖),從下圖可以看出Navigation導(dǎo)航包的“指揮中心“”move_base節(jié)點(diǎn)訂閱了/odom_ekf節(jié)點(diǎn)發(fā)布的/odom_ekf話題,/odom_ekf中的內(nèi)容是機(jī)器人搭載的輪式里程計(jì)(編碼器)經(jīng)過推位得到定位信息/odom與姿態(tài)傳感器(IMU)的定位信息經(jīng)過/robot_pose_ekf節(jié)點(diǎn),即使用擴(kuò)展卡爾曼濾波器(EKF)進(jìn)行融合后的定位信息。
那么我們的ROS機(jī)器人是否就是使用這個(gè)定位信息作為機(jī)器人的實(shí)時(shí)位置進(jìn)行路徑規(guī)劃及其他應(yīng)用呢?答案是否定的,下面給出解釋
move_base節(jié)點(diǎn)中是通過調(diào)用getRobotPose函數(shù)來(lái)獲取機(jī)器人當(dāng)前的位姿的
getRobotPose(global_pose, planner_costmap_ros_);
getRobotPose函數(shù)的核心代碼如下,可以看出getRobotPose函數(shù)實(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ī)器人的實(shí)時(shí)定位信息是通過監(jiān)聽tf樹中map坐標(biāo)系與base_link坐標(biāo)系的變換關(guān)系來(lái)計(jì)算獲得的,并非使用了訂閱的/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)行時(shí)的tf樹如下圖所示,可以看出map坐標(biāo)系與base_link坐標(biāo)系之間還存在一個(gè)odom坐標(biāo)系,map坐標(biāo)系與odom的坐標(biāo)變換關(guān)系是由/amcl節(jié)點(diǎn)廣播出來(lái)的,odom坐標(biāo)系與base_link坐標(biāo)系的坐標(biāo)變換關(guān)系是/robot_pose_ekf節(jié)點(diǎn)廣播出來(lái)的,所以,我們可以先大膽的推測(cè),機(jī)器人的實(shí)時(shí)定位信息跟/amcl節(jié)點(diǎn)與/robot_pose_ekf節(jié)點(diǎn)均有關(guān),且/amcl節(jié)點(diǎn)給出的定位信息是借助激光雷達(dá)的數(shù)據(jù),采用粒子濾波算法(PF)估計(jì)出來(lái)的,而/robot_pose_ekf節(jié)點(diǎn)給出的定位信息是里程計(jì)信息和IMU信息經(jīng)過擴(kuò)展卡爾曼濾波(EKF)融合后得到的。
那么它們之間的關(guān)系又是怎樣的呢?下面通過解讀amcl包中廣播odom與map坐標(biāo)系的tf關(guān)系的過程來(lái)進(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原點(diǎn)在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_);
這里的具體實(shí)現(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原點(diǎn)在odom坐標(biāo)系下的坐標(biāo),存放在odom_to_map中
(4)最后,對(duì)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)系,即可實(shí)現(xiàn)對(duì)EKF的修正。
二、概括總結(jié)
☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆
總的來(lái)說,/robot_pose_ekf節(jié)點(diǎn)會(huì)高頻的廣播給出使用里程計(jì)信息和IMU信息經(jīng)過擴(kuò)展卡爾曼濾波(EKF)融合后得到的定位信息,這個(gè)定位信息就是odom坐標(biāo)系下的機(jī)器人位姿,如果這個(gè)定位信息是準(zhǔn)確的,odom坐標(biāo)系將與map坐標(biāo)系近似于重合,此時(shí),定位信息可以看成全局坐標(biāo)系map下的機(jī)器人位姿信息。
然而里程計(jì)和IMU會(huì)有累計(jì)誤差,且該誤差會(huì)隨著時(shí)間的推移不斷增大,尤其是車輪打滑的情況下,這個(gè)偏移會(huì)很大,即odom坐標(biāo)系會(huì)逐漸偏離map坐標(biāo)系。此時(shí),將ekf輸出的定位信息作為機(jī)器人在全局坐標(biāo)系下的位姿信息是不合適的,這也是為什么還需要AMCL通過粒子濾波輸出定位信息的原因。
AMCL功能包借助激光雷達(dá)的感知信息,通過粒子濾波低頻的廣播出odom坐標(biāo)系與map坐標(biāo)系的偏差,在這個(gè)過程中,會(huì)將粒子濾波估計(jì)出的全局坐標(biāo)系map下的機(jī)器人的位姿信息當(dāng)做“真值”去使用,即認(rèn)為粒子濾波估計(jì)出的定位信息是接近于真值的。用這個(gè)map→base_link(機(jī)器人)的坐標(biāo)關(guān)系減去ekf輸出的base_link→odom的坐標(biāo)關(guān)系,就是AMCL廣播的odom與map坐標(biāo)系的偏差,用這個(gè)偏差疊加上ekf的輸出來(lái)對(duì)efk估計(jì)的定位信息進(jìn)行糾正,并作為機(jī)器人實(shí)時(shí)的定位信息。
☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆ ☆☆☆
三、深入思考
1、odom坐標(biāo)系與map坐標(biāo)系的偏差所代表的意義是什么?
odom坐標(biāo)系與map坐標(biāo)系的偏差實(shí)際上是使用激光雷達(dá)感知信息的粒子濾波估計(jì)出定位信息與使用里程計(jì)和IMU信息通過擴(kuò)展卡爾曼濾波估計(jì)出的定位信息的差值。
在將AMCL使用粒子濾波估計(jì)出的機(jī)器人在全局坐標(biāo)系下的位姿信息,作為機(jī)器人真實(shí)的位姿信息的情況下,odom坐標(biāo)系與map坐標(biāo)系的偏差可以看成,ekf估計(jì)出的機(jī)器人的位姿信息與真實(shí)值之間的偏差,即odom與map坐標(biāo)系差的越大,ekf當(dāng)前的估計(jì)值與真實(shí)值的差距也就越大,越不準(zhǔn)確。
2、粒子濾波與擴(kuò)展卡爾曼濾波輸出的定位信息那個(gè)更準(zhǔn)確?
使用激光雷達(dá)感知信息的粒子濾波估計(jì)出定位信息比使用里程計(jì)和IMU信息通過擴(kuò)展卡爾曼濾波估計(jì)出的定位信息要準(zhǔn)確,尤其是在機(jī)器人的運(yùn)動(dòng)存在打滑現(xiàn)象時(shí),下圖中的給出了機(jī)器人運(yùn)行一段時(shí)間回到起點(diǎn)后,ekf和amcl輸出的定位信息的偏差,可見在ekf在x軸上的偏差高達(dá)1.49m,而amcl的偏差僅在0.08m,運(yùn)行多圈以后ekf的偏差甚至達(dá)到了5.3m,而此時(shí)amcl的偏差僅為0.2m,可以看出經(jīng)過實(shí)驗(yàn)測(cè)試,在機(jī)器人存在打滑現(xiàn)象時(shí),amcl輸出的定位信息的精度要遠(yuǎn)高于ekf輸出的定位信息。這也是為什么可以將amcl輸出的定位信息近似當(dāng)真值使用來(lái)對(duì)ekf進(jìn)行修正的原因。
3、既然AMCL輸出的定位信息比ekf輸出的定位信息要準(zhǔn)確,為什么不直接使用AMCL輸出的定位信息作為機(jī)器人的定位信息使用,而是使用AMCL對(duì)ekf的輸出進(jìn)行修正?為什么AMCL不直接廣播map到base_link的坐標(biāo)變換關(guān)系?
因?yàn)?,AMCL的定位信息準(zhǔn)確,但計(jì)算量較大,只能輸出一個(gè)低頻的定位信息,如10hz,而ekf的定位信息誤差較大,但可以高頻的輸出定位信息,如100hz。采用AMCL的對(duì)ekf進(jìn)行修正的模式,即可以較好的保證定位信息的實(shí)時(shí)性,又能較好的保證定位信息的準(zhǔn)確性。
4、ROS機(jī)器人是如何使用Navigation導(dǎo)航包實(shí)現(xiàn)定位的精度的決定性因素是什么?
ROS機(jī)器人是如何使用Navigation導(dǎo)航包實(shí)現(xiàn)定位的精度的決定性因素是 AMCL中粒子濾波的估計(jì)精度。假設(shè)AMCL輸出的定位信息的頻率是10HZ,ekf輸出的定位信息的頻率是100hz。則在0.1 _ n時(shí)刻機(jī)器人使用的定位信息就是AMCL輸出的定位信息,在0.1 _ n ~ 0.1 *(n+1) 的時(shí)間段內(nèi),比如0.16時(shí)刻輸出的定位信息 是0.1時(shí)刻AMCL輸出的定位信息與0.1時(shí)刻ekf輸出的定位信息的差值,加上0.16時(shí)刻ekf輸出的定位信息。即0.16時(shí)刻的定位使用的是0.1時(shí)刻的AMCL對(duì)ekf的修正(0.1時(shí)刻的map→base_link)加上0.16時(shí)刻的ekf輸出(0.16時(shí)刻的base_link → odom)。其中n取任意整數(shù)
所以,在0.1 _ n時(shí)刻的機(jī)器人定位信息的估計(jì)精度就是AMCl中粒子濾波的估計(jì)精度,在0.1 _ n ~ 0.1 *(n+1)的時(shí)間段內(nèi),ekf的估計(jì)精度越高只能保證,在該時(shí)間段內(nèi)的估計(jì)偏差越小,但是最終取決定性作用的還是AMCL的估計(jì)精度,舉個(gè)簡(jiǎn)單的例子,在0.1時(shí)刻,AMCl估計(jì)的定位信息是 機(jī)器人處于x軸的5m處(認(rèn)為機(jī)器人0.1時(shí)刻真實(shí)位置也在5m處),0.1時(shí)刻ekf估計(jì)的定位信息是機(jī)器人處于x軸的5.2m處,此時(shí)0.1時(shí)刻amcl的修正信息是0.2m ,在0.16時(shí)刻ekf的估計(jì)位置是5.8m處,然而機(jī)器人的真實(shí)位置在5.4m處,0.16時(shí)刻AMCl的修正量依然使用的是0.1時(shí)刻的修正量,最終輸出的定位信息是在5.6m處,與真實(shí)信息差了0.2m,然而在0.2時(shí)刻,此時(shí)機(jī)器人位于6.01m處,AMCl估計(jì)的機(jī)器人也6m處,ekf估計(jì)的機(jī)器人位于6.6m處,此時(shí)ekf的估計(jì)偏差是0.61,AMCl對(duì)ekf的糾正是0.6,此時(shí)輸出的定位信息即6m,與真實(shí)位置偏差是0.01,也就是AMCL的估計(jì)精度。
所以,總的來(lái)說,ROS機(jī)器人是如何使用Navigation導(dǎo)航包實(shí)現(xiàn)定位的精度的決定性因素是 AMCL中粒子濾波的估計(jì)精度,即使是在0.1 _ n ~ 0.1 _(n+1)的時(shí)間段內(nèi)ekf的偏移特別大,那在 0.1 *(n+1)也會(huì)被修正為AMCL估計(jì)的位姿,與真實(shí)位置的定位偏差也會(huì)變?yōu)锳MCL的估計(jì)偏差。
審核編輯:湯梓紅
-
傳感器
+關(guān)注
關(guān)注
2552文章
51366瀏覽量
755730 -
機(jī)器人
+關(guān)注
關(guān)注
211文章
28627瀏覽量
207954 -
定位
+關(guān)注
關(guān)注
5文章
1347瀏覽量
35449 -
激光雷達(dá)
+關(guān)注
關(guān)注
968文章
4022瀏覽量
190234 -
ROS
+關(guān)注
關(guān)注
1文章
279瀏覽量
17050
原文標(biāo)題:ROS機(jī)器人如何使用Navigation導(dǎo)航包實(shí)現(xiàn)實(shí)時(shí)定位
文章出處:【微信號(hào):vision263com,微信公眾號(hào):新機(jī)器視覺】歡迎添加關(guān)注!文章轉(zhuǎn)載請(qǐng)注明出處。
發(fā)布評(píng)論請(qǐng)先 登錄
相關(guān)推薦
評(píng)論