FishBot里程计和IMU传感器数据融合

小鱼

FishBot里程计和IMU传感器数据融合

需求描述

大家好，我是小鱼。今天我将向大家介绍如何在ROS 2的humble版本上，为fishbot实现里程计和IMU数据的融合，以生成更稳定的里程计数据。

实现步骤

为了实现里程计和IMU数据的融合，我们可以使用一个名为robot_localization的开源库，它提供了ROS 2的humble版本的功能包，非常适用于我们的fishbot机器人平台。你可以在这里找到它的代码库：https://github.com/cra-ros-pkg/robot_localization。

1. 安装robot_localization

首先，我们需要在ROS 2的humble版本上安装robot_localization库。我们可以使用apt命令进行安装，确保安装了适用于ROS 2 humble版本的robot_localization包。让我们一起运行以下命令：

sudo apt install ros-humble-robot-localization -y

2. 发布静态TF变换

在开始之前，我们需要发布一个静态TF变换，将base_link和IMU之间的坐标系关系告知robot_localization节点。我们可以使用以下命令行发布该静态TF变换：

ros2 run tf2_ros static_transform_publisher 0.08 0.0 0.03 0 0 0 1 base_link imu

其中，0.08、0.0和0.03分别表示x、y和z轴上的平移量。请根据实际情况进行调整。

3. 创建和编写参数文件

在进行数据融合之前，我们需要创建一个参数文件，用于配置robot_localization节点的参数。为了满足fishbot的需求，我会给出一个示例参数文件供参考。你可以使用任何文本编辑器创建一个.yaml文件，并根据自己的需求进行配置。让我们来看一下示例参数文件的内容：

### ekf config file ###
ekf_filter_node:
    ros__parameters:
        debug: true
        frequency: 30.0
        two_d_mode: false
        publish_acceleration: false
        publish_tf: true
        map_frame: map              # 如果未指定，默认为"map"
        odom_frame: odom            # 如果未指定，默认为"odom"
        base_link_frame: base_link  # 如果未指定，默认为"base_link"
        world_frame: odom           # 如果未指定，默认为odom_frame的值

        odom0: /odom
        odom0_config: [true,  true,  true,
                       false, false, false,
                       false, false, false,
                       false, false, true,
                       false, false, false]

        imu0: /imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      false, false, false,
                      false, false, false]

在这

个示例参数文件中，你可以根据自己的需求进行配置。例如，我们可以将debug参数设置为true以启用调试模式，将frequency参数设置为30.0以设置节点的频率。另外，我们需要指定里程计和IMU数据的话题，并相应地配置参数。

4. 运行融合节点

现在，我们可以通过命令行来启动robot_localization节点并加载参数文件。请打开终端，并运行以下命令：

ros2 run robot_localization ekf_node --ros-args --params-file /path/to/your/params/file.yaml

请将/path/to/your/params/file.yaml替换为你自己的参数文件的实际路径。

运行上述命令后，robot_localization节点将会启动，并根据参数文件中的配置来执行里程计和IMU数据的融合。

5. 使用launch文件配置和运行（可选）

除了通过命令行启动节点，我们也可以使用launch文件来更方便地配置和运行robot_localization节点。我已经为你准备了一个示例的launch文件。你可以将以下内容保存为launch文件（例如robot_localization.launch.py）：

import os
import launch
import launch_ros

def generate_launch_description():
    package_name = 'robot_localization'
    ld = launch.LaunchDescription()
    pkg_share = launch_ros.substitutions.FindPackageShare(package=package_name).find(package_name) 

    robot_localization_node = launch_ros.actions.Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node',
        output='screen',
        parameters=[os.path.join(pkg_share, 'params/ekf_example.yaml'), {'use_sim_time': launch.substitutions.LaunchConfiguration('use_sim_time')}]
    )

    ld.add_action(launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='False',
                                                description='Flag to enable use_sim_time'))
    ld.add_action(robot_localization_node) 

    return ld

在这个示例的launch文件中，我们使用了launch_ros.actions.Node来创建一个节点，并设置了参数文件的路径。请根据实际情况修改参数文件的路径。

要运行launch文件，请在终端中运行以下命令：

ros2 launch your_package_name robot_localization.launch.py

请将your_package_name替换为你自己的包的名称。

通过使用launch文件，我们可以更加方便地配置和运行robot_localization节点。

总结

在本文中，我向大家介绍了如何在ROS 2的humble版本上使用robot_localization库，为fishbot机器人平台实现里程计和IMU数据的融合。通过这种融合方式，我们可以生成更稳定准确的里程计数据，从而提高机器人的导航性能。同时，我还展示了如何发布base_link到IMU的静态TF变换，以供robot_localization节点使用。最后，我介绍了使用launch文件来更方便地配置

测试视频展示：

2543298600

@小鱼我融合odom和IMU时，机器人在rviz中晃动，这是为什么？已经用complementary_filter_node过滤IMU数据了，并且rostopic type /robot/odometry/filtered_odom ：nav_msgs/Odometry，实在不知道哪里导致机器人晃动

小鱼

@2543298600 仔细检查数据源问题，各轴方向问题

Tom

@小鱼在 FishBot里程计和IMU传感器数据融合中说：

ros2 run robot_localization ekf_node --ros-args --params-file /path/to/your/params/file.yaml

这边按照您的步骤执行发现没有执行成功，是什么原因吗

改成launch文件执行也没运行起来
list一下好像也没有这个节点

然后用命令执行：
ros2 run robot_localization ekf_node --ros-args --params-file /opt/ros/humble/share/robot_localization/params/ekf_example.yaml

也是啥都没有请问是什么情况吗