亚博科技和幻尔科技的十轴IMU在Ros2 Humble下驱动后数值无限趋于0的解决方案

在做机器人导航以及建模的时候，考虑到多传感器融合可能会带来更好的效果，于是决定使用幻尔科技的十轴IMU（其实亚博科技与幻尔科技这块IMU的内部完全一致，驱动代码都完全一致）驱动后使用以下命令输出传来的四元数等数据。

$ ros2 launch wit_ros2_imu rviz_and_imu.launch.py

起初我以为是我数据传输问题，经过检查波特率等发现没有问题，经过使劲晃动IMU数据依然很小，带有e-05次方等，无限趋向于0。查阅网上的文章发现也有人遇到了这个问题，但是并非Humble版本，他给予了那个版本的解决方案，我迁移到Humble版本，并且直接在驱动得到的数据做了一个卡尔曼滤波（其实是因为如果要对IMU的ROS2数据加滤波又要使用另外一个包IMU_Tools）。

这里直接贴代码了，Ros2 Humble版本的驱动，要注意端口Port以及波特率与上位机一致，不然得不到数据。

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(gnss_imu_sim)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(sensor_msgs REQUIRED)

# 手动设置serial库路径

set(SERIAL_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/../serial/include)
set(SERIAL_LIBRARY ${CMAKE_SOURCE_DIR}/../serial/build/libserial.a)

add_executable(imu_driver_exe src/imu.cpp)
target_include_directories(imu_driver_exe PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
  ${SERIAL_INCLUDE_DIR}) # 添加serial库头文件路径
target_link_libraries(imu_driver_exe ${SERIAL_LIBRARY}) # 链接serial库文件
target_compile_features(imu_driver_exe PUBLIC c_std_99 cxx_std_17)

ament_target_dependencies(
  imu_driver_exe
  "rclcpp"
  "std_msgs"
  "sensor_msgs"
  "tf2"
)

install(TARGETS imu_driver_exe DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>gnss_imu_sim</name>
  <version>0.0.0</version>
  <description>A ROS 2 package for GNSS and IMU simulation.</description>
  <maintainer email="zard@todo.todo">zard</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>tf2</depend>
  <depend>sensor_msgs</depend>

  <depend>serial</depend> <!-- Assuming the 'serial' library is required -->

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

src/imu.cpp

#include <iostream>
#include <chrono>
#include <cmath>
#include <vector>
#include <thread>
#include <atomic>
#include <serial/serial.h>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include "rclcpp/rclcpp.hpp"

#define BAUDRATE 9600

std::atomic_bool imu_thread_running;
std::atomic_bool imu_data_ready;
std::vector<uint8_t> buff;
std::vector<int16_t> acceleration(4, 0);
std::vector<int16_t> angularVelocity(4, 0);
std::vector<int16_t> magnetometer(4, 0);
std::vector<int16_t> angle_degree(4, 0);

// 卡尔曼滤波器类
class KalmanFilter
{
public:
    KalmanFilter(double process_noise, double measurement_noise, double estimation_error)
        : process_noise_(process_noise), measurement_noise_(measurement_noise), estimation_error_(estimation_error), last_estimation_(0)
    {
    }

    double update(double measurement)
    {
        double kalman_gain = estimation_error_ / (estimation_error_ + measurement_noise_);
        double current_estimation = last_estimation_ + kalman_gain * (measurement - last_estimation_);
        estimation_error_ = (1 - kalman_gain) * estimation_error_ + fabs(last_estimation_ - current_estimation) * process_noise_;
        last_estimation_ = current_estimation;
        return current_estimation;
    }

private:
    double process_noise_;
    double measurement_noise_;
    double estimation_error_;
    double last_estimation_;
};

// 每种传感器数据的卡尔曼滤波器实例
std::vector<KalmanFilter> acc_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));
std::vector<KalmanFilter> gyro_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));
std::vector<KalmanFilter> mag_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));
std::vector<KalmanFilter> angle_filters(4, KalmanFilter(1e-2, 1e-1, 1.0));

class IMUDriverNode : public rclcpp::Node
{
public:
    IMUDriverNode(const std::string& nodeName) : Node(nodeName)
    {
        // 获取串口
        _port_name = this->declare_parameter<std::string>("port_name", "/dev/imu_usb");
        // 发布IMU数据
        publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu/data_raw", 10);
        // 发布磁力计数据
        mag_publisher_ = this->create_publisher<sensor_msgs::msg::MagneticField>("/imu/mag", 10);
        // IMU驱动线程
        imu_thread_ = std::thread(&IMUDriverNode::imuThread, this, _port_name);
    }

    void joinIMUThread()
    {
        imu_thread_.join();
    }

private:
    void imuThread(const std::string &port_name)
    {
        serial::Serial imu_serial;
        try
        {
            imu_serial.setPort(port_name);
            imu_serial.setBaudrate(BAUDRATE);
            serial::Timeout timeout = serial::Timeout::simpleTimeout(500);
            imu_serial.setTimeout(timeout);
            imu_serial.open();
            RCLCPP_INFO(this->get_logger(), "\033[32mSerial port opened successfully...\033[0m");
        }
        catch (const serial::IOException &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open the IMU serial port.");
            return;
        }

        imu_serial.flush();

        while (rclcpp::ok() && imu_thread_running.load())
        {
            if (imu_serial.available())
            {
                uint8_t data;
                imu_serial.read(&data, 1);
                buff.push_back(data);

                if (buff.size() >= 11 && buff[0] == 0x55)
                {
                    std::vector<uint8_t> data_buff(buff.begin(), buff.begin() + 11);
                    std::vector<uint8_t> data(buff.begin() + 2, buff.begin() + 10);
                    bool angle_flag = false;

                    if (data_buff[1] == 0x51)
                    {
                        if (checkSum(data_buff))
                        {
                            acceleration = hexToShort(data);
                            RCLCPP_DEBUG(this->get_logger(), "Acceleration data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x51 Check failure.");
                        }
                    }
                    else if (data_buff[1] == 0x52)
                    {
                        if (checkSum(data_buff))
                        {
                            angularVelocity = hexToShort(data);
                            RCLCPP_DEBUG(this->get_logger(), "Angular velocity data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x52 Check failure.");
                        }
                    }
                    else if (data_buff[1] == 0x53)
                    {
                        if (checkSum(data_buff))
                        {
                            angle_degree = hexToShort(data);
                            angle_flag = true;
                            RCLCPP_DEBUG(this->get_logger(), "Angle data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x53 Check failure.");
                        }
                    }
                    else if (data_buff[1] == 0x54)
                    {
                        if (checkSum(data_buff))
                        {
                            magnetometer = hexToShort(data);
                            RCLCPP_DEBUG(this->get_logger(), "Magnetometer data updated.");
                        }
                        else
                        {
                            RCLCPP_WARN(this->get_logger(), "0x54 Check failure.");
                        }
                    }

                    buff.clear();

                    if (angle_flag)
                    {
                        imu_data_ready.store(true);
                        RCLCPP_DEBUG(this->get_logger(), "IMU data ready for publishing.");
                    }
                }
                else if (buff[0] != 0x55)
                {
                    buff.clear();
                }
            }

            if (imu_data_ready.load())
            {
                // 更新滤波数据
                std::vector<double> acc_filtered(4, 0);
                std::vector<double> gyro_filtered(4, 0);
                std::vector<double> mag_filtered(4, 0);
                std::vector<double> angle_filtered(4, 0);

                for (size_t i = 0; i < 4; i++)
                {
                    acc_filtered[i] = acc_filters[i].update(acceleration[i]);
                    gyro_filtered[i] = gyro_filters[i].update(angularVelocity[i]);
                    mag_filtered[i] = mag_filters[i].update(magnetometer[i]);
                    angle_filtered[i] = angle_filters[i].update(angle_degree[i]);
                }

                // 发布IMU数据
                sensor_msgs::msg::Imu imu_msg;
                imu_msg.header.stamp = this->now();
                imu_msg.header.frame_id = "base_link";

                imu_msg.linear_acceleration.x = static_cast<double>(acc_filtered[0]) / 32768.0 * 16 * 9.8;
                imu_msg.linear_acceleration.y = static_cast<double>(acc_filtered[1]) / 32768.0 * 16 * 9.8;
                imu_msg.linear_acceleration.z = static_cast<double>(acc_filtered[2]) / 32768.0 * 16 * 9.8;

                imu_msg.angular_velocity.x = static_cast<double>(gyro_filtered[0]) / 32768.0 * 2000 * M_PI / 180;
                imu_msg.angular_velocity.y = static_cast<double>(gyro_filtered[1]) / 32768.0 * 2000 * M_PI / 180;
                imu_msg.angular_velocity.z = static_cast<double>(gyro_filtered[2]) / 32768.0 * 2000 * M_PI / 180;

                double roll = static_cast<double>(angle_filtered[0]) / 32768.0 * M_PI;
                double pitch = static_cast<double>(angle_filtered[1]) / 32768.0 * M_PI;
                double yaw = static_cast<double>(angle_filtered[2]) / 32768.0 * M_PI;
                tf2::Quaternion quat;
                quat.setRPY(roll, pitch, yaw);

                imu_msg.orientation.x = quat.getX();
                imu_msg.orientation.y = quat.getY();
                imu_msg.orientation.z = quat.getZ();
                imu_msg.orientation.w = quat.getW();

                publisher_->publish(imu_msg);
                RCLCPP_INFO(this->get_logger(), "IMU data published.");

                // 发布磁力计数据
                sensor_msgs::msg::MagneticField mag_msg;
                mag_msg.header.stamp = this->now();
                mag_msg.header.frame_id = "base_link";
                mag_msg.magnetic_field.x = static_cast<double>(mag_filtered[0]);
                mag_msg.magnetic_field.y = static_cast<double>(mag_filtered[1]);
                mag_msg.magnetic_field.z = static_cast<double>(mag_filtered[2]);

                mag_publisher_->publish(mag_msg);
                RCLCPP_INFO(this->get_logger(), "Magnetometer data published.");

                imu_data_ready.store(false);
            }
        }
        imu_serial.close();
    }

    bool checkSum(const std::vector<uint8_t> &data_buff)
    {
        uint8_t sum = 0;
        for (size_t i = 0; i < data_buff.size() - 1; i++)
        {
            sum += data_buff[i];
        }

        return sum == data_buff[data_buff.size() - 1];
    }

    std::vector<int16_t> hexToShort(const std::vector<uint8_t> &hex_data)
    {
        std::vector<int16_t> short_data(4, 0);
        for (size_t i = 0; i < hex_data.size(); i += 2)
        {
            int16_t high = static_cast<int16_t>(hex_data[i + 1]);
            short_data[i / 2] = static_cast<int16_t>((high << 8) | hex_data[i]);
        }

        return short_data;
    }

    std::string _port_name;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
    rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr mag_publisher_;
    std::thread imu_thread_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto imu_node = std::make_shared<IMUDriverNode>("imu_driver_node");
    imu_thread_running.store(true);

    rclcpp::spin(imu_node);
    imu_thread_running.store(false);

    imu_node->joinIMUThread();

    rclcpp::shutdown();
    return 0;
}

launch/imu_driver_launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='gnss_imu_sim',
            executable='imu_driver_exe',
            name='imu_driver_exe',
            parameters=[{"port_name": "/dev/imu_usb"}],  # Removed the tilde
            output='both'
        )
    ])

注意，要安装serial包Davesh98/系列： ROS2_HUMBLE (github.com)

最好把它放在imu_driver_exe，也就是src的上级目录，这样Cmake里的相对位置不会报错，省事了很多。修改了驱动文件可以看到数据的单位正常了，趋势也正常了。