使用 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 Odometryclass OdomEKF():def __init__(self):# Give the node a namerospy.init_node('odom_ekf', anonymous=False)# Publisher of type nav_msgs/Odometryself.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)# Wait for the /odom_combined topic to become availablerospy.wait_for_message('input', PoseWithCovarianceStamped)# Subscribe to the /odom_combined topicrospy.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.headerodom.header.frame_id = '/odom'odom.child_frame_id = 'base_footprint'odom.pose = msg.poseself.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
使用 robot_pose_ekf 对imu和odom进行融合相关推荐
- [学习SLAM]基于EKF使用 robot_pose_ekf 对imu和odom进行融合定位(robot_pose_ekf) 篇章 (2)
使用 robot_pose_ekf 对imu和odom进行融合 robot_pose_ekf 是 ROS Navigation stack 中的一个包,通过扩展卡尔曼滤波器对 imu.里程计 odom ...
- ROS小车记录系列(二)IMU采集、过滤,与odom数据融合,发布新的odom话题
(二)IMU采集.过滤,利用EKF将IMU与odom融合,发布新的odom话题 A.ROS采集节点 B.imu_tools过滤imu数据 C.使用 robot_pose_ekf 对imu和odom进行 ...
- 2. IMU原理及姿态融合算法详解
文章目录 2. IMU原理及姿态融合算法详解 一.组合 二. 原理 a) 陀螺仪 b) 加速度计 c) 磁力计 三. 旋转的表达 a) 欧拉角 b) 旋转矩阵 c) 四元数 d) 李群 SO(3)\t ...
- 结合imu与odom运行robot_pose_ekf
EKF融合运行记录 泰裤辣!!! 虚拟机空间不够,在复制bag包时直接崩溃,再也打不开,只好说拜拜,从头开始吧 step1 克隆robot_pose_ekf包 mkdir -p ekf_ws/src ...
- 激光+视觉+IMU+GPS如何做融合?
应用背景介绍 自主导航是机器人与自动驾驶的核心功能,而SLAM技术是实现自主导航的前提与关键.现有的机器人与自动驾驶车辆往往会安装激光雷达,相机,IMU,GPS等多种模态的传感器,而且已有许多优秀的激 ...
- 动手学无人驾驶(6):基于IMU和GPS数据融合的自车定位
在上一篇博文<动手学无人驾驶(5):多传感器数据融合>介绍了如何使用Radar和LiDAR数据对自行车进行追踪,这是对汽车外界运动物体进行定位. 对于自动驾驶的汽车来说,有时也需要对自身进 ...
- 无人车传感器 IMU与GPS数据融合进行定位机制
前言 上一次的分享里,我介绍了GPS的原理(三角定位)及特性(精度.频率),同时也从无人车控制的角度,讨论了为什么仅有GPS无法满足无人车的定位要求. 为了能让无人驾驶系统更高频率地获取定位信息,就必 ...
- IMU校正以及姿态融合
本文为博主"声时刻"原创文章,未经博主允许不得转载. 联系方式:shenshikexmu@163.com 缘起 有缘在简极科技兼职两年.接触了IMU,我去的时候那家公司还是一个要把 ...
- odomimu融合用于Cartographer建图
任务动机:使用odom&imu融合用于Cartographer建图,提高Cartographer建图时的匹配精度,同时降低运算消耗. 任务描述:使用odom&imu融合用于Cartog ...
最新文章
- Best Time to Buy and Sell Stock II
- hle机器人_《英雄联盟》Deft加入HLE打野CaD续约Arthur加入
- 【Paper】11_[25]A distributed control scheme for multiple robotic vehicles to make group formations
- 虚拟机使用的是此版本 VMware Workstation 不支持的硬件版本。 模块“Upgrade”启动失败。 未能启动虚拟机(修改.vmx文件virtualHW.version = “xx“字段)
- 使用apache模块rewrite_module
- api报错 javaee maven_JavaEE关于Maven的配置与学习
- 配置Windows 2008 R2 防火墙允许远程访问SQL Server 2008 R2 更改端口 连接字符串 IP+逗号+端口号...
- 【OpenCV 例程200篇】97. 反谐波平均滤波器
- C#数据结构-广义表和递归
- Java讲课笔记07:计数循环与嵌套循环
- kubernetes 查看日志方法
- drupal.behavior 和 document.ready 没有直接的关系
- 最详细的联发科刷机教程MediaTek SP Flash Tool教程MTK通用驱动Windows 10 MTK VCOM USB Preloader Drivers
- 恭喜腾佑当选“河南省企业上云推进联盟”理事单位!
- 读取yaml文件出现UnicodeDeco: ‘gbk‘ codec can‘t decode byte 0xaf in position 68: illegal multibyte sequen报错
- vscode怎么运行代码HTML 怎么在vscode编写HTML代码
- 2022.6.14日新selenium写法
- 【bzoj3926】[Zjoi20150]诸神眷顾的幻想乡 后缀自动机+trie
- java编写机器人_用JAVA编写自己的机器人一起“厮杀”
- 【Netty之旅四】你一定看得懂的Netty客户端启动源码分析!