3、IMU传感器接入：IMU数据模型、ROS2 IMU驱动实现、imu_filter_madgwick滤波配置

IMU，也就是惯性测量单元，在导航里是个老熟人了。它不像激光雷达那样直接给你地图，也不像摄像头那样能看到纹理。它只告诉你一件事：我现在的加速度是多少？角速度是多少？ 说白了，就是靠“猜”来推算位置和姿态。

我个人习惯把IMU比作“盲人走路”。你闭着眼睛，靠身体的感觉判断自己走了多远、拐了几个弯。IMU就是机器人的“身体感觉”。但问题是，它会有误差，而且误差会累积。这就是为什么我们需要好好处理IMU数据。

3.1 IMU数据模型：你拿到的到底是什么？

先搞清楚IMU输出的原始数据长什么样。一个标准的IMU，比如MPU6050或BMI088，会输出三组数据：

• 三轴加速度：单位是 m/s²，测量的是重力加速度和运动加速度的合成。
• 三轴角速度：单位是 rad/s，测量的是绕X、Y、Z轴的旋转速度。
• 三轴磁场强度（可选）：单位是 μT，用于辅助航向校正。

但注意，这些数据不是完美的。我在项目中遇到过一个问题：IMU静止时，加速度计读出来的值不是 [0, 0, 9.8]，而是 [0.2, -0.1, 9.6]。为什么？因为存在零偏和噪声。

IMU的误差模型可以简单表示为：

测量值 = 真实值 + 零偏 + 噪声 + 尺度因子误差

嗯，这里要注意：零偏是缓慢变化的，噪声是高频的。如果你直接拿原始数据去积分算位置，那误差会像滚雪球一样越来越大。我刚开始做导航时，就吃过这个亏——机器人明明没动，积分出来的位置却跑出去好几米。

3.2 ROS2 IMU驱动实现：从硬件到话题

在ROS2里，IMU驱动的作用就是把硬件数据转换成标准的 sensor_msgs/Imu 话题。这个标准消息包含：

字段	含义
orientation	四元数表示的姿态
angular_velocity	角速度 (rad/s)
linear_acceleration	线加速度 (m/s²)
orientation_covariance	姿态协方差矩阵
angular_velocity_covariance	角速度协方差矩阵
linear_acceleration_covariance	线加速度协方差矩阵

写驱动时，我建议你重点关注两个点：

1. 坐标系对齐：IMU的坐标系必须和机器人坐标系对齐。如果IMU是斜着装的，那数据就要做旋转矩阵变换。
2. 时间戳：ROS2里时间戳非常重要。IMU数据必须带上硬件时间戳，否则融合时会出问题。

下面是一个简单的ROS2 IMU驱动代码片段，我用的是C++：

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>

class ImuDriver : public rclcpp::Node
{
public:
  ImuDriver() : Node("imu_driver")
  {
    publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 10);
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(10), std::bind(&ImuDriver::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto msg = sensor_msgs::msg::Imu();
    msg.header.stamp = this->now();
    msg.header.frame_id = "imu_link";

    // 这里读取硬件数据
    msg.linear_acceleration.x = read_accel_x();
    msg.linear_acceleration.y = read_accel_y();
    msg.linear_acceleration.z = read_accel_z();

    msg.angular_velocity.x = read_gyro_x();
    msg.angular_velocity.y = read_gyro_y();
    msg.angular_velocity.z = read_gyro_z();

    // 协方差矩阵先填默认值
    msg.linear_acceleration_covariance[0] = 0.01;
    msg.angular_velocity_covariance[0] = 0.01;

    publisher_->publish(msg);
  }

  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

3.3 imu_filter_madgwick滤波配置：让数据更靠谱

拿到原始IMU数据后，下一步就是滤波。我个人比较喜欢Madgwick滤波器，因为它计算量小、收敛快，适合嵌入式平台。

imu_filter_madgwick 是ROS2里一个现成的滤波包。它会融合加速度计和陀螺仪的数据，输出稳定的四元数姿态。配置起来很简单，但有几个参数需要调：

• gain：滤波器增益，默认0.1。值越大，对加速度计信任越多；值越小，对陀螺仪信任越多。
• zeta：陀螺仪漂移补偿，默认0.0。如果你的陀螺仪零偏严重，可以适当调大。
• remove_gravity_vector：是否去除重力分量。一般设为true。

我曾经在一个四足机器人项目里，因为gain设得太小，导致姿态响应特别慢，机器人走路时姿态估计滞后了半秒。后来把gain调到0.5，效果就好了很多。

配置文件的示例：

imu_filter_madgwick:
  ros__parameters:
    gain: 0.1
    zeta: 0.0
    remove_gravity_vector: true
    fixed_frame: "odom"
    use_mag: false
    publish_tf: true
    world_frame: "enu"

3.4 知识体系总览

为了让你更直观地理解IMU接入的整个流程，我画了一张图：

从这张图你可以看到，整个流程其实就三步：硬件读取 → 驱动封装 → 滤波输出。每一步都有坑，但只要你理解了数据模型和参数含义，就能轻松搞定。

好了，IMU这部分就聊到这。记住一句话：IMU是导航的“内功”，练好了，后面的融合就稳了。