ROS 主動蒙特卡羅粒子濾波定位演算法 AMCL 解析-- map與odom座標轉換的方法
阿新 • • 發佈:2019-02-01
ROS AMCL 演算法根據訂閱到的地圖資料配合鐳射掃描特徵,使用粒子濾波獲取最佳定位點,該點稱為Mp (point on map), 它是相對於地圖map上的座標,也就是base_link相對map上的座標。
odom 的原點是機器人啟動時刻的位置,它在map上的位置或轉換矩陣是未知的。但是AMCL可以根據最佳粒子的位置推算出 odom->map的tf轉換資訊併發布到 tf主題上。因為base_link->odom的tf轉換資訊是每時每刻都在釋出的,所以它是已知的。
已知tf轉換:
map->base_link
base_link->odom
下面的公式就可以推算:
map->odom = map->base_link - base_link->odom
看一下程式碼是怎麼實現的:
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f", hyps[max_weight_hyp].pf_pose_mean.v[0], hyps[max_weight_hyp].pf_pose_mean.v[1], hyps[max_weight_hyp].pf_pose_mean.v[2]); // subtracting base to odom from map to base and send map to odom instead tf::Stamped<tf::Pose> odom_to_map; try { tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], hyps[max_weight_hyp].pf_pose_mean.v[1], 0.0)); tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(), laser_scan->header.stamp, base_frame_id_); this->tf_->transformPose(odom_frame_id_, tmp_tf_stamped, odom_to_map); } catch(tf::TransformException) { ROS_DEBUG("Failed to subtract base to odom transform"); return; } latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()), tf::Point(odom_to_map.getOrigin())); 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_); tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), transform_expiration, global_frame_id_, odom_frame_id_); this->tfb_->sendTransform(tmp_tf_stamped); sent_first_transform_ = true; }
hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp
第10行 tmp_tf = 從map原點看base_link的位置
第14行 tmp_tf.inverse() = 以為Mp為原點,map原點的位置,就是在base_link座標系下map 原點的位置
第17行 進行 base_link座標系下的點轉換到odom座標系,也就是把map原點轉換到odom座標系下,等於從odom原點看map原點的位置。放到latest_tf_變數裡面。
第37行 釋出tf轉換關係,latest_tf_.inverse() 得到的是從map原點看odom原點的位置,也就是 odom->map的轉換關係
總結
從W的原點看A所在的位置座標 p,就是A->W轉換矩陣,稱為WAT. Ap為A座標系的點, 它在W上的座標 Wp = WAT*Ap