欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识！
您现在的位置是： 首页

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

程序员文章站 2022-04-15 22:21:23
...

ROS导航功能包：ros-planning/navigation

中国大学mooc：Navigation 工具包说明

目录

ROS导航功能包：ros-planning/navigation

中国大学mooc：Navigation 工具包说明

一、csdn：ROS下robot_pose_ekf扩展卡尔曼融合包的使用

二、ROS wiki：带外部传感器的robot_pose_ekf

三、创客智造：ROS与navigation教程-robot_pose_ekf

四、csdn：22.IMU和里程计融合

五、csdn：使用 robot_pose_ekf 对imu和odom进行融合

一、csdn：ROS下robot_pose_ekf扩展卡尔曼融合包的使用

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

有一个imu，而且校准的好（博主使用的是razor 9dof的imu，通过ros包pub一个Imu的data类型）

原先的tf_tree中可以看到这个单单使用里程计的tf图片：（由arduion转换的，底层通信的node里发布的）

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

由于ekf包会为我们处理好这部分tf，所以不需要我们发布变换了，所以需要将上面这部分发布的代码注释掉！

之后我们需要加一个imu的link（由于我们的比赛中已经有了这个link，所以这一步就不做处理：静态发布imu的tf）

现在开始进行使用ekf包来融合：

  • 开启底盘的launch发布odom的topic
  • 开启imu的launch发布imu的topic
  • 开启运行robot_pose_ekf的launch

为了查看ekf包是否正常工作，使用以下代码查看：

rosservice call /robot_pose_efk/get_status

查看Odometry sensor和IMU sensor 是否is used ，is active来确保ekf包的是否正确使用

如果成功，那么tf图是这样的，odom–base_footprint的转换就是由/robot_pose_ekf发布的

至于融合后的效果，就要看imu的校准度了！！！看效果的话可以用rviz或者导出rosbag包用matlab的plot函数绘出来，matlab可能更直观点，可以看这个：使用Matlab与ROS端通信以及绘制Odom里程计信息

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

二、ROS wiki：带外部传感器的robot_pose_ekf

带外部传感器的robot_pose_ekf

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

三、创客智造：ROS与navigation教程-robot_pose_ekf

ROS与navigation教程-robot_pose_ekf

  • Robot Pose EKF 包用于评估机器人的3D位姿，基于来自不同来源的位姿测量信息。它使用带有6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自轮式里程计，IMU传感器和视觉里程计的数据信息。基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。

如何使用：EKF节点的默认启动文件位于robot_pose_ekf包目录中​​​​​​​

<launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <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="true"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
</launch>

参数：

  • freq: 滤波器更新和发布频率。注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度。
  • sensor_timeout: 当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。
  • odom_used, imu_used, vo_used: 确认是否输入

编译运行：

rosdep install robot_pose_ekf
roscd robot_pose_ekf
rosmake

roslaunch robot_pose_ekf.launch

节点：

订阅的话题：

odom ([nav_msgs/Odometry][2])
2D pose (用于轮式里程计)：其包含机器人在地面中的位置（position）和方位（orientation）以及该位姿的协方差。 发送此2D位姿的消息实际上表示3D位姿，但z，pitch和roll分量被简单忽略了。
imu_data ([sensor_msgs/Imu][3]) 
3D orientation (用于IMU):3D方位提供机器人基座相对于地图坐标系的Roll, Pitch and Yaw偏角。Roll and Pitch角是绝对角度（因为IMU使用了重力参考），而YAW角是相对角度。协方差矩阵指定的方位测量的不确定度。当仅仅收到这个主题消息时，机器人位姿ekf还不会启动，因为它还需要来自主题'vo'或者'odom'的消息。
vo ([nav_msgs/Odometry][4]) 
3D pose (用于视觉里程计):3D位置表示机器人的完整位置和方位以及该位姿的协方差。当用传感器只测量部分3D位姿(e.g. the wheel odometry only measures a 2D pose)时候, 可以给还未真正开始测量的部分3D位姿先简单指定一个大的协方差。

发布的话题：

robot_pose_ekf/odom_combined ([geometry_msgs/PoseWithCovarianceStamped][6]) 
滤波器输出 (评估的3D机器人位姿)。

tf转换：odom_combined → base_footprint​​​​​​​

四、csdn：22.IMU和里程计融合

22.IMU和里程计融合

实际使用中会出现*打滑和累计误差的情况，这里单单使用编码器得到里程计会出现一定的偏差，虽然激光雷达会纠正，但一个准确的里程对这个系统还是较为重要。

IMU即为 惯性测量单元，一般包含了三个单轴的加速度计和三个单轴的陀螺仪，简单理解通过加速度二次积分就可以得到位移信息、通过角速度积分就可以得到三个角度

查看话题：/robot_pose_ekf/odom_combined

rostopic info /robot_pose_ekf/odom_combined

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

类型需要注意下是PoseWithCovarianceStamped并非Odometry,后面会用到这个作为显示，所以还需要一个转换

查看该topic信息可以看到odom_ekf订阅了该topic,再次查看该节点信息可以看到

他会发出一个Odometry的topic,发出一个tf

查看订阅滤波器输出话题的节点：/odom_ekf

rosnode info /odom_ekf

 网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

在robot_pose_ekf配置时，做了些映射处理，这样可以保证导航层在使用和不用imu的时候无需修改就可以工作,有无imu输出的tf都是odom → base_footprint​​​​​​​，发出的里程都是odom，所以将带有imu的里程topic映射为wheel_odom

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

五、csdn：使用 robot_pose_ekf 对imu和odom进行融合

使用 robot_pose_ekf 对imu和odom进行融合

注意发布消息时 topic 的名称要对应,否则会起不到滤波作用！！！

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>
  • output_frame, base_footprint_frame:用于指定输出 tf 变换中坐标系的名字,默认为 odom_combined 和 base_footprint。

注意使用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/odom_combined 消息：

在 movebase 需要订阅/odom 的话题,想让这个消息被 movebase使用

这里主要是看 rbx_bringup 包里提供的一个节点:odom_ekf.py，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

很简单, /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 使用了!

<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>

这个节点就订阅/robot_pose_ekf /odom_combined 下的位置估计信息然后发布/odom_ekf

/odom_ekf 和/odom 的信息类型相同

网上收集关于robot_pose_ekf扩展卡尔曼融合包的使用

回复：

<node pkg="move_base" type="move_base" name="move_base" output="screen"> 
  <remap from="odom" to="odom_ekf" />
 </node> 
 
在move_base将odom改为odom_ekf，主要是在源码中回调函数的消息类型是nav::Odometry,消息类型需要一致，如果没有用odom_ekf.py，应该会报错

 

 

 

 

 

相关标签： ROS

上一篇： js实现展示pdf的多种方法

下一篇： Hibernate中的Criteria查询

推荐阅读