使用imu_tools对IMU进行滤波可视化
概述:
与 IMU 相关的过滤器和可视化器。堆栈包含:
- imu_filter_madgwick:一种过滤器,可将来自常规 IMU 设备的角速度,加速度和(可选) 磁读数融合到方向中。
- imu_complementary_filter:一种滤波器,该滤波器使用基于互补融合的新颖方法将来自 通用 IMU 设备的角速度,加速度和(可选)磁读数融合到方向四元数中。
- rviz_imu_plugin:rviz 的插件,可显示 sensor_msgs::Imu 消息。
一. 安装imu_tools
下载地址
https://github.com/ccny-ros-pkg/imu_tools.git
选择对应的版本下载压缩包,放到对应的工作空间的src目录下。
使用rosdep安装依赖:
rosdep install imu_tools
然后使用catkin_make
进行编译。
二. 使用
分别介绍两种滤波器的处理方法
1. imu_filter_madgwick
imu_filter_madgwick 包用于过滤器从 IMU 设备的原始数据。它将来自通用 IMU 设备的角速 度,加速度和(可选)磁读数融合到方向四元数中,并在 imu / data 主题上发布融合的数据。
-
订阅的主题
imu / data_raw(sensor_msgs / Imu)
- 包含原始 IMU 数据(包括角速度和线性加速度)的消息。
imu / mag(sensor_msgs / MagneticField)
- [ 可 选 ] 磁 场 矢 量 ; 类 型 为 sensor_msgs / MagneticField 或 geometry_msgs / Vector3Stamped(已弃用),具体取决于参数 use_magnetic_field_msg(请参见下文)
-
发表的话题 imu /data(sensor_msgs / Imu)
- 融合的 Imu 消息,包含方向。
-
重要参数
不可动态重新配置的参数
〜world_frame 指示方向所针对的世界框架,有效值:“ nwu”,“ enu”,“ ned”。
〜use_mag(bool,默认:true) 数据融合中是否使用磁场数据。
〜use_magnetic_field_msg 如果设置为 true,则将/ imu / mag 主题订阅为 sensor_msgs / MagneticField;如果设置为 false(不建议使用),请使用 geometry_msgs / Vector3Stamped。
〜fixed_frame(字符串,默认:odom) 在 publish_tf 中使用的父框架。
〜publish_tf(bool,默认:true) 是否使用在 fixed_frame 中指定的帧作为父帧并在输入 imu 消息中给定的帧作为子帧, 发布表示 IMU 方向的 TF 变换。
〜reverse_tf(bool,默认:false) 如果设置为 true,则发布将从 imu_frame 转换为固定帧,而不是相反。
〜constant_dt(double,默认值:0.0) 使用的 dt;如果为 0.0(默认值),则根据消息头动态计算 dt。
〜publish_debug_topics(bool,默认:false) 如果设置为 true,则发布几个调试主题。
〜stateless(bool,默认:false) 如果设置为 true,则不要发布过滤的方向。取而代之的是,仅根据最新的加速度计(和 可选的磁力计)读数发布定向的无状态估计值。对于调试很有用。
〜remove_gravity_vector(bool,默认:false) 如果设置为 true,则从已发布的 IMU 消息中的加速度字段中减去重力矢量。
最后将话题 imu/data 重映射为 imu_data,这是因为我们数据融合包 robot_pose_ekf 将要订阅该话题。
打开文件:
~/imu_tools_ws/src/imu_tools/imu_filter_madgwick/src/imu_filter_ros.cpp
,有如下代码:
// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
一开始下载下来的源码应该是 /imu/data_raw,根据imu驱动发布的话题进行修改,我设置的发布的话题是/imu,注意,这里再源码里修改不要加 /
因此进行修改,使得topic一致:
// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(
nh_, "imu", queue_size));
打开launch文件进行修改(这里只是一些重要参数):
<param name="fixed_frame" value="base_link" />
<param name="use_mag" value="false" />
<param name="publish_tf" value="false" />
<param name="use_magnetic_field_msg" value="true" />
<param name="world_frame" value="enu" />
<param name="orientation_stddev" value="0.05" />
<param name="angular_scale" value="1.00" />
<remap from="imu/data" to="imu_data"/>
笔者采用的是此滤波器。这里我是加了重映射的,可以看一下robot_pose_ekf订阅的话题:
可见,该融合包订阅的话题是/imu_data,所以最后把滤波器发布的话题重映射为/imu_data。
2. imu_complementary_filter
这里直接讲修改吧
打开文件:
~/imu_tools_ws/src/imu_tools/imu_complementary_filter/src/complementary_filter_ros.cpp
,有如下代码:
// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, "imu", queue_size));
可以看出,imu_tools
订阅的topic为imu/data_raw
,而IMU发布的topic为/imu
,因此需要修改代码,使topic一致:
// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, "imu", queue_size));
打开launch文件:~/imu_tools_ws/src/imu_tools/imu_complementary_filter/launch/complementary_filter.launch
,进行一些修改:
<!-- ComplementaryFilter launch file -->
<launch>
#### Complementary filter
<node pkg="imu_complementary_filter" type="complementary_filter_node"
name="complementary_filter_gain_node" output="screen">
<param name="do_bias_estimation" value="true"/>
<param name="do_adaptive_gain" value="true"/>
<param name="use_mag" value="false"/>
<param name="gain_acc" value="0.01"/>
<param name="gain_mag" value="0.01"/>
<param name="publish_debug_topics" value="false"/>
<param name="publish_tf" value="true"/>
</node>
</launch>
运行imu_tools:
source ~/imu_tools_ws/devel/setup.bash
roslaunch imu_complementary_filter complementary_filter.launch
rviz //打开rviz,在rviz中,点击【Add】,添加【rviz_imu_plugin】
最后看一下我的imu.launch文件中的部分配置,主要是看一下如何使用滤波器这个节点以及参数配置:
如有疑惑,请与我联系。