zoukankan      html  css  js  c++  java
  • 使用robot_pose_ekf对传感器信息融合

       robot_pose_ekf是ROS Navigation stack中的一个包,通过扩展卡尔曼滤波器对imu、里程计odom、视觉里程计vo的数据进行融合,来估计平面移动机器人的真实位置姿态,输出odom_combined消息。robot_pose_ekf只适用于平面上的轮式移动机器人,因此odom信息中的z,pitch和roll分量可以被忽略。IMU可以提供车体坐标系相对于世界坐标系的姿态(RPY角),其中Roll和Pitch是绝对角度,因为有重力方向作为参考,而偏航角Yaw则是一个相对角度(如果IMU中没有集成电子罗盘测量地球磁场角作为参考)。IMU姿态的协方差矩阵代表了姿态测量的不确定度。

      robot_pose_ekf默认监听的topic为:imu_dataodomvo,因此要注意发布消息时topic的名称要对应,否则会起不到滤波作用(The robot_pose_ekf node does not require all three sensor sources to be available all the time. A source can appear and disappear over time, and the node will automatically detect and use the available sensors)。不想使用默认名称的话可以用remap元素进行名称重映射。robot_pose_ekf.launch文件如下:

     <launch>
          <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
            <param name="output_frame" value="odom_combined"/>
            <param name="base_footprint_frame" value="base_footprint"/>
            <param name="freq" value="30.0"/>
            <param name="sensor_timeout" value="1.0"/>
            <param name="odom_used" value="true"/>
            <param name="imu_used" value="true"/>
            <param name="vo_used" value="false"/>
            <param name="debug" value="false"/>
            <param name="self_diagnose" value="false"/>
        </node>
     </launch>

      参数说明如下:

    • freq:滤波器更新和发布频率。注意频率高仅仅意味着一段时间可以获得更多机器人位姿信息,但是并不表示可以提高位姿估计的精度。

    • sensor_timeout:当传感器停止向滤波器发送信息时,滤波器在没有传感器的情况下等待多长时间才重新开始工作。

    • odom_used, imu_used, vo_used:确认是否输入。

    • output_frame, base_footprint_frame:用于指定输出tf变换中坐标系的名字,默认为odom_combined和base_footprint。

    Published Topics

      rostopic list命令可以查看ros中的topic,下图中白色的/robot_pose_ekf/odom_combined话题就是robot_pose_ekf节点发布的:

    Provided tf Transforms

    • odom_combined → base_footprint

      robot_pose_ekf在输出odom_combined信息同时还会发布相关的坐标变换,输入下面指令查看tf变换关系:

    rosrun rqt_tf_tree rqt_tf_tree

      可以看出robot_pose_ekf节点会发布base_footprint坐标系相对于odom_combined坐标系的变换:

      参数output_frame和base_footprint_frame默认为odom_combined和base_footprint,也可以根据需要在launch文件中进行更改,比如分别改为odom和base_link,再次用rqt_tf_tree命令查看,如下图所示:

      robot_pose_ekf节点默认会从odom、imu_data、vo这三个topic上订阅消息,可以使用remap将其映射到新名称的topic上。重映射是基于替换的思想,每个重映射包含一个原始名称和一个新名称。每当节点使用重映射中的原始名称时,ROS客户端库就会将它默默地替换成其对应的新名称。

     <launch>
        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
            <param name="output_frame" value="odom"/>
            <param name="base_footprint_frame" value="base_link"/>
            <param name="freq" value="30.0"/>
            <param name="sensor_timeout" value="1.0"/>
            <param name="odom_used" value="true"/>
            <param name="imu_used" value="true"/>
            <param name="vo_used" value="false"/>
            <param name="debug" value="false"/>
            <param name="self_diagnose" value="false"/>
    
            <remap from="imu_data" to="imu" />      <!-- 将节点订阅的imu_data话题改名为imu -->
        </node>
     </launch>

      下面在VREP中进行仿真测试:将移动机器人目录中的lumibot模型拖入场景中,并将加速度计和陀螺仪安装在机器人上,并在lumibot_body上添加一个脚本,发布里程计和imu信息(可以在发布的信息上添加一些噪声)。注意使用robot_pose_ekf进行滤波时传感器的协方差矩阵信息不能空着,否则可能会出现错误,因此要设置合理的值。

       imu数据的协方差矩阵设置可以参考:https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py

    self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
    self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]

      底盘运动时odom的协方差矩阵如下,参考:https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/covariances.py 

    ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                            0, 1e-3, 0, 0, 0, 0,
                            0, 0, 1e6, 0, 0, 0,
                            0, 0, 0, 1e6, 0, 0,
                            0, 0, 0, 0, 1e6, 0,
                            0, 0, 0, 0, 0, 1e3]
    
    ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                             0, 1e-3, 0, 0, 0, 0,
                             0, 0, 1e6, 0, 0, 0,
                             0, 0, 0, 1e6, 0, 0,
                             0, 0, 0, 0, 1e6, 0,
                             0, 0, 0, 0, 0, 1e3]

      注意imu信息的协方差矩阵中代表机器人航向角的分量方差为1e-6,而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e3,两个值相差很大。在进行EKF融合时,会更“相信”imu提供的姿态信息,因为其方差更小。比如机器人在转动过程中轮子发生了打滑,用编码器推算出的姿态一直在旋转,而实际姿态(主要由IMU测量得到)却没发生太大变化,这种情况就需要使用信息融合方法来减小误差。协方差矩阵中的参数设置非常重要,要根据传感器手册或者实际使用测量来确定。

      VREP脚本中可以订阅odom_combined消息,在回调函数中设置odomCombined相对于odom的位置,然后通过Graph记录下来,绘制出轨迹曲线。

    function subscriber_callback(msg)
        -- This is the subscriber callback function
        simSetObjectPosition(odomCombinedHandle,odomHandle,{msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z})
    end
    
    
    if (sim_call_type==sim_childscriptcall_initialization) then
       
      subscriber
    = simExtRosInterface_subscribe('/robot_pose_ekf/odom_combined','geometry_msgs/PoseWithCovarianceStamped','subscriber_callback') simExtRosInterface_subscriberTreatUInt8ArrayAsString(subscriber) end

      运行robot_pose_ekf.launch文件,然后开始VREP仿真。下图中蓝色的曲线是使用robot_pose_ekf融合后的机器人运动轨,红色为原始的带噪声的轨迹曲线(这只是一个例子,实际效果怎么样还要调整各种参数):

      另外需要注意的是,robot_pose_ekf会发布base_link到odom的tf变换,因此我们自己的程序中就不用发布了,否则会出现冲突(在tf树中是不能构成回路的,只能有一个父坐标系,但是可以有很多子坐标系 )。下图是仿真过程中rivz显示的原始odom(黄色箭头)和融合后的odom_combined(红色箭头)信息,以及base_link坐标系和odom坐标系间的变换关系。

    参考:

    robot_pose_ekf

    robot_pose_ekf 使用说明

    V-rep中的加速度计与陀螺仪

    ROS中测试机器人里程计信息

    robot_pose_ekf with an external sensor

    数据融合-机器人定位教程1-传感数据准备

    ROS与navigation教程-robot_pose_ekf介绍

    ROS下robot_pose_ekf扩展卡尔曼融合包的使用 

    航向姿态参考系统与惯性测量单元的联系与区别

  • 相关阅读:
    HDU 1009 FatMouse' Trade
    HDU 2602 (简单的01背包) Bone Collector
    LA 3902 Network
    HDU 4513 吉哥系列故事——完美队形II
    LA 4794 Sharing Chocolate
    POJ (Manacher) Palindrome
    HDU 3294 (Manacher) Girls' research
    HDU 3068 (Manacher) 最长回文
    Tyvj 1085 派对
    Tyvj 1030 乳草的入侵
  • 原文地址:https://www.cnblogs.com/21207-iHome/p/8117069.html
Copyright © 2011-2022 走看看