使用robot_pose_ekf对传感器信息融合

Posted

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了使用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扩展卡尔曼融合包的使用 

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

以上是关于使用robot_pose_ekf对传感器信息融合的主要内容,如果未能解决你的问题,请参考以下文章

服务机器人---多传感器融合

服务机器人---多传感器融合

无线传感器网络数据融合技术

Android 上可靠的传感器融合?

多传感器数据融合算法综述

EKF定位基于传感器信息融合的EKF扩展卡尔曼滤波定位算法matlab仿真