使用 robot_pose_ekf 对imu和odom进行融合

  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_data 、 odom 和 vo,因此要注意发布消息时 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

• robot_pose_ekf/odom_combined(geometry_msgs/PoseWithCovarianceStamped): The output of the filter (the estimated 3D robot pose)
  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,如果 imu 节点发布的话题是 imu_data 就不用修改 -->
	</node>
</launch>

 在移动机器人平台测试:将拥有加速度计和陀螺仪的 imu 安装在机器人上,机器人发布里程计和 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 测量得到)却没发生太大变化,这种情况就需要使用信息融合方法来减小误差。协方差矩阵中的参数设置非常重要,要根据传感器手册或者实际使用测量来确定。
  运行 robot_pose_ekf.launch 文件,然后开始移动。下图中蓝色的曲线是使用 robot_pose_ekf 融合后的机器人运动轨,红色为原始的带噪声的轨迹曲线(这只是一个例子,实际效果怎么样还要调整各种参数):
  1.另外需要注意的是,robot_pose_ekf 会发布 base_link 到 odom 的 tf 变换,因此我们自己的程序中就不用发布了,否则会出现冲突(在 tf 树中是不能构成回路的,只能有一个父坐标系,但是可以有很多子坐标系 )。下图是仿真过程中 rivz 显示的原始/odom(黄色箭头)和融合后的/robot_pose_ekf/ odom_combined(红色箭头)信息,以及 base_link 坐标系和 odom坐标系间的变换关系。
  2、注意/odom和/robot_pose_ekf/odom_combined消息类型不同,前者是nav_msgs/Odometry,后者是geometry_msgs/PoseWithCovarianceStamped ,两者的区别:后者的内容是前者的一部分。

以上标红色的注意事项:

1、robot_pose_ekf 会发布 base_link 到 odom 的 tf 变换,因此我们自己的程序中就不用发布了,否则会出现冲突(在 tf树中是不能构成回路的,只能有一个父坐标系,但是可以有很多子坐标系 )

没有用robot_pose_ekf融合imu和odom的tf, 查看tf,维护好自己的tf 对我们来说至关重要,稍有不慎就会出现很多麻烦。
  从上面的图片可以看到 odom–base_footprint 是通过 arduino 转换的,这一部分一般都是在我们与底盘通信的 node 里发布的。
  这部分代码就是 arduino 实现上面 tf 转换的源代码,为什么要把这部分提出来呢,因为 robot_pose_ekf 的包会处理好这部分 tf,不改参数默认发布 odom_combined–base_footprint 的 tf 转换(我们把 odom_combined 改为 odom),所以不需要我们发布变换了,所以我们使用这个包第一步就是要将我们上面提到的那一段代码注释掉,之后我们需要加一个 imu 的 link,直接使用静态tf 发布吧。

<node name="base_imu_link" pkg="tf" type="static_transform_publisher" args="0 0 0 0 3.1415926 0 /base_link /base_imu_link 50"/>

  可以看到这里的 odom – base_footprint 的转换就是由/robot_pose_ekf 发布的了,至于融合后的效果就是要看 imu 的校准度了,看效果的话可以用 rviz 或者导出 rosbag 包用 matlab 的 plot 函数绘出来。

2、注意/odom和/robot_pose_ekf/odom_combined消息类型不同,前者是nav_msgs/Odometry,后者是geometry_msgs/PoseWithCovarianceStamped ,两者的区别:后者的内容是前者的一部分。
现在我们得到了/robot_pose_ekf/odom_combined 消息。在 movebase 需要订阅/odom 的话题,想让这个消息被 movebase使用。这里主要是看 rbx_bringup 包里提供的一个节点:odom_ekf.py,很简单, /odom和/robot_pose_ekf/odom_combined它们消息类型是不同的,前者是nav_msgs/Odometry,后者是 geometry_msgs/PoseWithCovarianceStamped ,两者的区别:后者的内容是前者的一部分。odom_ekf.py 就是将它们转化用的。转化后还要在发布融合信息的 launch 文件里将/robot_pose_ekf/odom_combined 话题 remap 成/odom_ekf.就可以给 movebase 使用了!

在 ros by exampe 一书中 7.8 节 Out and Back Using Odometry 中

$ roslaunch rbx1_bringup odom_ekf.launch

这个 launch 就是调用了 robot_pose_ekf 对机器人的位置进行估计,打开 launch 文件

<launch>
   <node pkg="rbx1_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
   	<remap from="input" to="/robot_pose_ekf /odom_combined"/>
   	<remap from="output" to="/odom_ekf"/>
   </node>
</launch>

接着打开 odom_ekf.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry

class OdomEKF():
   def __init__(self):
       # Give the node a name
       rospy.init_node('odom_ekf', anonymous=False)

       # Publisher of type nav_msgs/Odometry
       self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
       
       # Wait for the /odom_combined topic to become available
       rospy.wait_for_message('input', PoseWithCovarianceStamped)
       
       # Subscribe to the /odom_combined topic
       rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
       
       rospy.loginfo("Publishing combined odometry on /odom_ekf")
       
   def pub_ekf_odom(self, msg):
       odom = Odometry()
       odom.header = msg.header
       odom.header.frame_id = '/odom'
       odom.child_frame_id = 'base_footprint'
       odom.pose = msg.pose
       
       self.ekf_pub.publish(odom)
       
if __name__ == '__main__':
   try:
       OdomEKF()
       rospy.spin()
   except:
       pass

这个节点就订阅/robot_pose_ekf /odom_combined 下的位置估计信息然后发布/odom_ekf,/odom_ekf 和/odom 的信息类型相同
robot_pose_ekf 该包的输出 :
发布一个 topic, 类型需要注意下是 PoseWithCovarianceStamped 并非 Odometry
后面会用到这个作为显示,所以还需要一个转换
查看该 topic 信息可以看到 odom_ekf 订阅了该 topic
再次查看该节点信息可以看到

参考文献

[1] 使用robot_pose_ekf对传感器信息融合 - XXX已失联 - 博客园
https://www.cnblogs.com/21207-iHome/p/8117069.html
[2] IMU和里程计融合 - PIBOT导航机器人 - CSDN博客
https://blog.csdn.net/baimei4833953/article/details/80768762
[3] Turtlebot学习指导第四篇＿使用robot_pose_ekf包,EKF(扩展卡尔曼滤波器)对机器人位置进行校正 - 天空中的太阳和月 - CSDN博客
https://blog.csdn.net/u010918541/article/details/51209666
[4] robot_pose_ekf使用遇到的问题 - ethan_guo的博客 - CSDN博客
https://blog.csdn.net/ethan_guo/article/details/79635575
[5] ROS下robot_pose_ekf扩展卡尔曼融合包的使用 - shenghuaijing3314的博客 - CSDN博客
https://blog.csdn.net/shenghuaijing3314/article/details/78220151
[6] ROS中测试机器人里程计信息 - XXX已失联 - 博客园
https://www.cnblogs.com/21207-iHome/p/8066135.html
[7] ros ekf融合odom imu ov信息 - 天才樱木 - CSDN博客
https://blog.csdn.net/datase/article/details/83095458
[8] 使用EKF融合odometry及imu数据 - xiekaikaibing的博客 - CSDN博客
https://blog.csdn.net/xiekaikaibing/article/details/80402113