ROS的amcl定位中 amcl_pose频率太低的问题

        笔者在用amcl定位来获取小车的当前位姿时,发现该程序更新的amcl_pose频率太低了,根本没有办法来做实时控制。仔细阅读完源码之后发现,该话题的发布需要经过重采样,而重采样的触发条件是里程计显示小车平移了0.2m(参数update_min_d)或者旋转了30度(参数update_min_a),也就是说小车在这期间是不发出位姿坐标的,那么如何解决这个问题呢?

        一开始笔者认为调小这些参数不就好了吗,结果发现自己太天真,如果把这两个参数调的特别小的话会发现重采样后粒子收敛的特别快,很快就会汇聚到一点了,这样是不利于定位的,很容易掉地图。后来在阅读参考了ROS下move_base的代码之后才了解到获取小车当前位姿的正确方法:通过tf变换!!!

        因为我们获取的是小车在地图上的位置坐标,所以我们只要得到了map到车体坐标系(base_link)的tf变换就行了,这个在程序里是一直更新的而且频率也符合要求。至于为什么在没有经过粒子滤波的时候也有值,笔者猜测是因为在重采样期间该值由里程计提供,而重采样起到的是把里程计拉回正确位置的效果,所以坐标可能在重采样时发生一点小小的跳变。

        至于如何获取不同坐标系之间的tf变换我的方法如下:

        tf::StampedTransform transform;
        try{
        //得到坐标odom和坐标base_link之间的关系
            //listener.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("map", "base_link",
            ros::Time(0), transform);
           }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
           }

这样一来可以通过

        tx=transform.getOrigin().x();
        ty=transform.getOrigin().y();
        tw=tf::getYaw(transform.getRotation());

来得到小车的x,y,yaw的信息了。亲测有效。如果我的理解有什么不正确的地方欢迎指正。

 

©️2020 CSDN 皮肤主题: 数字20 设计师:CSDN官方博客 返回首页