位姿估计Robot_pose_efk的配置和使用
发布日期:2021-10-03 22:59:08 浏览次数:60 分类:技术文章

本文共 4909 字,大约阅读时间需要 16 分钟。

Robot_pose_efk 用于融合里程计,惯性测量单元和视觉里程计的传感器输出,从而减少测量中的总体误差。

了解ROS的robot_pose_ekf软件包中扩展卡尔曼滤波器的用法:

robot_pose_ekf软件包用于基于来自不同来源的(部分)位姿测量值来估计机器人的3D姿势。它使用带有6D模型(3D位置和3D方向)的扩展卡尔曼滤波器,将来自车轮里程计,IMU传感器和视觉里程计的测量结果结合起来。基本思想是提供与不同传感器的松散耦合集成,其中传感器信号以ROS消息的形式接收。

主页:http://ros.org/wiki/robot_pose_ekf

Github:https://github.com/ros-planning/robot_pose_ekf

robot_pose_ekf软件包可用于融合里程计,惯性测量单元和视觉里程计的传感器输出,从而减少测量中的总体误差。

1 如何使用

1.1 配置
可以在robot_pose_ekf软件包目录中找到EKF节点的默认启动文件(launch)。启动文件包含许多可配置的参数:

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

sensor_timeout:当传感器停止向滤波器发送信息时,滤波器在没有传感器的情况下等待多长时间才重新开始工作
odom_used, imu_used, vo_used:确认是否输入
可以在启动文件中修改配置,如下所示:

 <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>
1
2
3
4
5
6
7
8
9
10
11
12
1.2 运行
(1)编译

 $ rosdep install robot_pose_ekf

 $ roscd robot_pose_ekf
 $ rosmake
1
2
3
(2)运行

 $ roslaunch robot_pose_ekf.launch

1
2 节点
2.1 订阅的话题
odom (nav_msgs/Odometry)
2D pose (used by wheel odometry) :该2D pose包含了机器人在地面的位置(position)和方位(orientation)信息以及该位姿的协方差(covariance)。用来发送该2D位姿的消息实际上表示了一个3D位姿,只不过把z,pitch和roll分量简单忽略了。

imu_data (sensor_msgs/Imu)

3D orientation (used by the IMU):3D方位提供机器人底座相对于世界坐标系的Roll,Pitch和Yaw信息。 Roll和Pitch角是绝对角度(因为IMU具有重力参考),而Yaw角是相对角度。 协方差矩阵用来指定方位测量的不确定度。当仅仅收到这个话题消息时, robot_pose_ekf不会启动,因为它还需要来自话题vo或者odom的消息。

vo (nav_msgs/Odometry)

3D pose (used by Visual Odometry):3D位姿可以完整表示机器人的位置和方位,以及该位姿的协方差。当传感器只测量3D位姿的一部分(e.g. the wheel odometry only measures a 2D pose)时, 可以给3D位姿没有实际测量的部分指定一个较大的协方差。

robot_pose_ekf节点不需要所有三个传感器源始终都可用。每个源给出一个位姿估计和一个协方差。这些源以不同的速率和不同的延迟运行。源会随时间出现或消失,节点将自动检测并使用可用的传感器。要添加自己的传感器输入,请查看教程:添加GPS传感器。

总结:

Odometry message contains the 2D pose, ie x, y and yaw angle

IMU message contains the 3D orientation, ie. roll, pitch and yaw angles.
VO message contains the full 3D pose, ie. x,y,z and roll, pitch, yaw angles.
2.2 发布的话题
robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
滤波器的输出(估计的机器人3D位姿)。

注意:/odom和/robot_pose_ekf/odom_combined消息类型不同,前者是nav_msgs/Odometry,后者是geometry_msgs/PoseWithCovarianceStamped 。两者的区别:后者的内容是前者的一部分。

将geometry_msgs/PoseWithCovarianceStamped转换为nav_msgs/Odometry:

#!/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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
2.3 提供的tf转换
odom_combined → base_footprint

3 如何工作

3.1 基本流程
从每个传感器获取数据
检查它们是否有效
如果它们有效,则将它们相对于参考基准坐标系进行转换
当获得传感器信息时,它将被存储,直到所有传感器的信息可用为止。收到的每个数据都有自己的时间戳
一旦所有数据都可用,则在所有传感器数据均可用时,针对每个可用传感器数据更新扩展卡尔曼滤波器(在Orocos-BFL库中定义)。即:如果来自里程计的数据在时间t_0(> 0)可用,则来自imu的数据在时间t_1(> t_0)处获得,而来自视觉里程计的数据在时间t_2(> t_1)处获得,则在时间t_1对所有三组数据进行滤波。
该融合的传感器数据被转换为里程计消息,并在话题/odom_combined上发布
3.2 位姿解释
给滤波器节点提供信息的所有传感器源都有自己的参考坐标系,并且随着时间推移都可能出现漂移现象。因此,每个传感器发出来的绝对位姿不能直接对比。 因此该节点使用每个传感器的相对位姿差异来更新扩展卡尔曼滤波器。

3.3 协方差解释

随着机器人的移动,其位姿的不确定性越来越大。随着时间的流逝,协方差将无限增长。因此,在位姿本身上发布协方差是没有用的,而是传感器源发布协方差如何随时间变化,即速度的协方差。请注意,使用对世界的观测(例如,测量到已知墙壁的距离)将减少机器人位姿的不确定性;但是,这是定位而不是里程计。

3.4 Timing

假定机器人上次更新位姿滤波器在t_0时刻, 该节点只有在收到每个传感器测量值(时间戳>t_0)之后才会进行下一次的滤波器更新。 例如,在odom话题收到一条消息时间戳(t_1 > t_0), 且在imu_data话题上也收到一条消息( 其时间戳t_2 > t_1 > t_0), 滤波器将被更新到所有传感器信息可用的最新时刻,这个时刻是t_1。 在t_1时刻odom位姿直接给出了,但是imu位姿需要通过在t_0和t_2两时刻之间进行线性插值求得。在t_0到 t_1时间段,机器人位姿滤波器使用odom和IMU相对位姿进行更新。

上图显示了PR2机器人从给定的初始位置(绿点)启动,四处行驶并返回初始位置时的实验结果。完美的里程计x-y图应显示出精确的回路闭合。蓝线显示来自车轮里程计的输入,蓝点表示估计的终点位置。红线显示robot_pose_ekf的输出,该输出结合了车轮里程计和imu的信息,红点表示估计的最终位置。

4 源码解读

https://blog.csdn.net/zhxue_11/article/details/83828877
https://blog.csdn.net/weixin_42048023/article/details/84550630
https://blog.csdn.net/shoufei403/article/details/102655696
5 总结
首先表明,通过测试,这个包可以用,而且效果还不错。但是,有些注意事项和踩坑环节,过不去就容易放弃,甚至说它无用。而且,这个高度依赖ROS和TF,虽然方便,但是如果要自己整合或者剥离ROS,也需要下一番功夫。
————————————————
版权声明:本文为CSDN博主「W_Tortoise」的原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接及本声明。
原文链接:https://blog.csdn.net/learning_tortosie/article/details/103179488

转载地址:https://blog.csdn.net/lclfans1983/article/details/105499366 如侵犯您的版权,请留言回复原文章的地址,我们会给您删除此文章,给您带来不便请您谅解!

上一篇:不定积分的概念与性质
下一篇:ROS使用yocs_smoother_velocity做速度平滑处理

发表评论

最新留言

哈哈,博客排版真的漂亮呢~
[***.90.31.176]2024年03月22日 07时26分27秒

关于作者

    喝酒易醉,品茶养心,人生如梦,品茶悟道,何以解忧?唯有杜康!
-- 愿君每日到此一游!

推荐文章

java 远程 yarn jar_再论Yarn Client和Yarn cluster 2019-04-21
java单元测试断言_单元测试+断言 2019-04-21
java 创建压缩包_用Java创建ZIP压缩文件 2019-04-21
java typedarray_TintTypedArray.java 2019-04-21
java字符字面量_java – 字符串字面量的行为是令人困惑的 2019-04-21
php判断数组的值是否为空,PHP判断数组是否为空的常用方法(五种方法) 2019-04-21
php 读数据库,PHP数据库 2019-04-21
PHP能不能下载报表,PHP生成Excel报表的方法 2019-04-21
php mht2html,PHP 处理 mht 文件 2019-04-21
rt2tr matlab,MATLAB机器人工具箱参考 2019-04-21
MATLAB中GUI界面弹出菜单的使用,Matlab GUIDE使用说明(Matlab GUI界面) 2019-04-21
win iis对比apache php,服务器Apache与IIS的区别 2019-04-21
怎样用xampp测试php环境变量,使用xampp配置php运行环境的方法 2019-04-21
qq互联php教程,thinkphp5怎么整合qq互联登录教程 2019-04-21
Java怎么比较4数字大小,怎么判断四个数不成比例-判断4个数值相等-数学-古残夷同学... 2019-04-21
mysql建立索引 性能测试_MySQL分区和索引性能测试 2019-04-21
数据结构java实验 刘小晶_数据结构实例解析与实验指导:Java语言描述 2019-04-21
java实现 k nn算法_java-C中的k-NN示例问题(OpenCV) 2019-04-21
java接口的理解_Java接口的理解 - rabbit_mom的个人空间 - OSCHINA - 中文开源技术交流社区... 2019-04-21
java重用名快捷键_Eclipse 最常用的 10 组快捷键,个个牛逼! 2019-04-21