ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法
来源:互联网 发布:免费mysql数据库服务器 编辑:程序博客网 时间:2024/05/16 18:38
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
- ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法
- ROS里基本坐标系的理解:map,odom,base_link,base_laser
- amcl算法(定位)
- 蒙特卡洛方法与粒子滤波
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS学习系列---RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS探索总结(十五)——amcl(导航与定位)
- ROS探索总结(十五)——amcl(导航与定位)
- amcl ROS
- AMCL定位
- 机器人定位(amcl)与导航
- 粒子滤波跟踪算法
- 粒子滤波跟踪方法
- 粒子滤波跟踪方法
- 粒子滤波跟踪方法
- c++显示转换static_cast, dynamic_cast, const_cast
- 基于3D卷积神经网络的人体行为理解(论文笔记)
- Unable to instantiate activity ComponentInfo
- 请勿滥用 2PC prepared transaction
- vim常用命令
- ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法
- Ajax缓存问题
- /usr/bin/ld: cannot find -lc
- Deep Learning论文笔记之(一)K-means特征学习
- 微信公众平台昵称乱码emoji表情过滤
- java-性能
- Java并发之CyclicBarrier示例
- 数据库里面各种键
- WIN10升级后无线网卡被禁用解决办法