IMU和轮式里程计的robot_pose_ekf融合

数据

开发包test提供的数据：

Imu发布频率50hz; Odom发布频率30Hz

自己的数据

Imu的frame_id:   imu_world， 发布频率200Hz;

Odom的frame_id: odom, child_frame_id: base_link, 发布频率30Hz

融合

通过发布静态坐标系建立的整体坐标系关系为：

odom_combined->base_footprint->base_link->imu_world

实际操作时报错，且动态坐标系关系odom_combined->base_footprint发布频率仅为0.3Hz左右：

Could not transform imu message from imu_world to base_footprint. Imu will not be activated yet.

根本原因在于这段代码：

    // Transforms imu data to base_footprint frame
    if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_ /*ros::Time::now()*/, ros::Duration(0.5))) //0.5
    {
        // warn when imu was already activated, not when imu is not active yet
        if (imu_active_)
            ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
        else if (my_filter_.isInitialized())
            ROS_WARN("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
        else
            ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
        return;
    }

    StampedTransform base_imu_offset;
    robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_  /*ros::Time::now()*/, base_imu_offset);
    imu_meas_ = imu_meas_ * base_imu_offset;

这段代码说明查找TF树时是基于rosbag录制的系统时间，但TF树目前发布的时间是以系统时间为准，使用以下使用仿真时间的配置无法解决问题。。。

所以办法有很多，我这里有两个：

第一，在源码里增加以bag录制时间为准的坐标系变换关系，即：

    tf::TransformBroadcaster imu_broadcaster;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
    transform.setRotation(tf::Quaternion(0, 0, 0, 1));
    imu_broadcaster.sendTransform(tf::StampedTransform(transform, imu_stamp_, base_footprint_frame_, imu->header.frame_id));

    //raw code
    // Transforms imu data to base_footprint frame
    if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_ /*ros::Time::now()*/, ros::Duration(0.5))) //0.5
    {
        // warn when imu was already activated, not when imu is not active yet
        if (imu_active_)
            ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
        else if (my_filter_.isInitialized())
            ROS_WARN("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
        else
            ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
        return;
    }

    StampedTransform base_imu_offset;
    robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_  /*ros::Time::now()*/, base_imu_offset);
    imu_meas_ = imu_meas_ * base_imu_offset;

第二，不改代码直接将base_footprint_frame改为/imu_world即可，即：

附上完整launch文件

参考

imu frame for Robot_pose_ekf - ROS Answers: Open Source Q&A Forum

robot_pose_ekf with an external sensor - ROS Answers: Open Source Q&A Forum

How do I compute the covariance matrix for an orientation sensor? [closed] - ROS Answers: Open Source Q&A Forum

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

使用 robot_pose_ekf 对imu和odom进行融合_Qm的博客-CSDN博客_robot_pose_ekf融合数据成功的标志

视觉里程计协方差：

​​​​​​combine visual odometry data from ccny_rgbd with encoders/IMU - ROS Answers: Open Source Q&A Forum

A Beginner’s Guide to the ROS robot_pose_ekf Package – Chidambaram Sethu