使用imu_tools对IMU进行滤波可视化

使用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 主题上发布融合的数据。

1. 订阅的主题

   imu / data_raw（sensor_msgs / Imu）

   • 包含原始 IMU 数据（包括角速度和线性加速度）的消息。

   imu / mag（sensor_msgs / MagneticField）

   • [ 可 选 ] 磁 场 矢 量 ； 类 型 为 sensor_msgs / MagneticField 或 geometry_msgs / Vector3Stamped（已弃用），具体取决于参数 use_magnetic_field_msg（请参见下文）

2. 发表的话题 imu /data（sensor_msgs / Imu）

   • 融合的 Imu 消息，包含方向。

3. 重要参数

   不可动态重新配置的参数

   〜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文件中的部分配置，主要是看一下如何使用滤波器这个节点以及参数配置：

如有疑惑，请与我联系。