Skip to content

Wiki

Inertial Measurement Unit (IMU)

About 9 min read

1.Introduction

Concept

IMU is the abbreviation of Inertial Measurement Unit; it is a navigation sensor that integrates multiple inertial sensors such as accelerometers, gyroscopes, magnetometers, etc. An IMU can measure and record in real time information such as the acceleration, angular velocity, and orientation of the object in which it is located, and through signal processing and mathematical models calculate the object's attitude, position, and motion state, etc.

An IMU (Inertial Measurement Unit) is mainly composed of the following types:

  1. Accelerometer: Used to measure the acceleration of an object, typically providing acceleration data along three axes (X, Y, Z). An accelerometer can help determine an object's linear motion and changes in motion posture.

  2. Gyroscope: Used to measure an object's angular velocity or rate of change of angle. A gyroscope can provide information about an object's rotation, determining its angle, orientation, and rotational motion.

  3. Magnetometer: Used to measure the strength and direction of the magnetic field around an object (that is, to measure field strength). A magnetometer can help determine an object's orientation and navigation, and plays an important role especially in geomagnetic positioning and heading control.

These sensors are usually integrated together to form an IMU, and their outputs are combined through data fusion algorithms to provide comprehensive attitude, position, and motion information.

Function

An IMU is an important sensor component that plays a significant role in attitude perception, driving state monitoring, and non-GPS positioning.

Features

Inertial measurement units (IMUs), as a common type of sensor, also have many applications in the field of robotics. Below are some advantages and disadvantages of IMUs in the field of robotics:

(1) Advantages:

  • High real-time capability: The IMU can acquire attitude and acceleration data at a very high sampling frequency, providing real-time motion information.

  • High accuracy: The IMU can measure the acceleration and angular velocity of an object, and after fusion and filtering, it can provide relatively high-accuracy attitude estimation and motion tracking.

  • Not limited by lighting conditions: The IMU does not depend on lighting conditions and is suitable for various environments, such as indoor, outdoor, and low-light conditions.

  • Compact size: IMUs usually have a small, lightweight, and compact design, making them suitable for embedded systems and robot platforms with high space constraints.

  • Low power consumption: The power consumption of an IMU is relatively low, making it suitable for applications with limited resources or requiring long-term operation.

(2) Disadvantages:

  • Cumulative error: Because errors in integral calculations accumulate over time, IMU attitude estimation may drift as time passes, requiring fusion with other sensors or the use of calibration methods for compensation.

  • Susceptible to external interference: IMU measurement results are easily affected by external interference such as vibration, shock, and temperature changes, which may lead to unstable or inaccurate attitude estimation.

  • Not suitable for absolute positioning: Using an IMU alone cannot provide the absolute position information of an object, and it needs to be combined with other sensors (such as GNSS) to achieve absolute positioning.

  • Cannot perceive environmental information: An IMU can only provide its own acceleration and angular velocity data, and cannot directly perceive the environment or detect surrounding objects.

In summary, IMUs in the field of robotics offer advantages such as high real-time performance, relatively high accuracy, insensitivity to lighting conditions, compact size, and low power consumption. However, they also have disadvantages, including cumulative errors, susceptibility to external interference, unsuitability for absolute localization, and inability to perceive environmental information. Therefore, when designing robotic systems, it is necessary to comprehensively consider application requirements and fuse IMUs with other sensors (such as cameras, LiDAR, etc.) to enhance the robot's perception, localization, and control capabilities.

2.Message API

In ROS2, the IMU message is encapsulated as ansensor_msgs/msg/Imuinterface. You can view the specific data structure of this interface using the commandros2 interface show sensor_msgs/msg/ImuThe result is as follows:


# 这是一个从惯性测量单元（Inertial Measurement Unit，简称IMU）获取数据的消息。
#

# 加速度应该以米/秒^2为单位（不是以g为单位），旋转速度应该以弧度/秒为单位。
#

# 如果测量的协方差已知，则应该填写相应值。全零的协方差矩阵将被解释为“协方差未知”，必须从其他源获取协方差才能使用协方差数据。
#

# 如果没有其中一个数据元素的估计值（例如，你的IMU不会产生方向估计），请将相关协方差矩阵的第 0 个元素设置为 -1。

# 如果你正在解析此消息，请检查每个协方差矩阵的第一个元素中是否有 -1 的值，并忽略相关的估计值。

std_msgs/Header header # 标头
    builtin_interfaces/Time stamp # 时间戳
        int32 sec
        uint32 nanosec
    string frame_id # imu坐标系

geometry_msgs/Quaternion orientation # 四元数姿态
    float64 x 0
    float64 y 0
    float64 z 0
    float64 w 1
float64[9] orientation_covariance # 姿态协方差矩阵

geometry_msgs/Vector3 angular_velocity # 三轴角速度
    float64 x
    float64 y
    float64 z
float64[9] angular_velocity_covariance # 角速度协方差矩阵

geometry_msgs/Vector3 linear_acceleration # 三轴线性加速度
    float64 x
    float64 y
    float64 z
float64[9] linear_acceleration_covariance # 线性加速度协方差矩阵

3.Package creation

First, create a functionality package.

ros2 pkg create imu_plumb --build-type ament_cmake --dependencies rclcpp sensor_msgs --node-name imu_node

Let's compile it first.

colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

4.Main logic of the node

#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include "cyber_imu/WitStandardProtocol.h"
#include "sensor_msgs/msg/imu.hpp"
#include <cstdint>
#include <rclcpp/logging.hpp>
#include <sensor_msgs/msg/detail/imu__struct.hpp>
#include <tf2/LinearMath/Quaternion.h>

class ImuNode: public rclcpp::Node
{
  public:
    ImuNode():Node("ImuNode_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"惯性计imu节点启动!");

      //创建odom话题通信发布方
      imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu",10);

    // 声明参数（带默认值）
    // 串口默认设备名（根据实际设备调整）
    this->declare_parameter("device_name", "/dev/ttyUSB0");
    //串口默认波特率
    this->declare_parameter("baud_rate", 115200);

    // 获取参数值
    const std::string device_name = this->get_parameter("device_name").as_string();
    const uint32_t baud_rate = this->get_parameter("baud_rate").as_int();

      // 创建串口配置对象
      // 波特率默认115200；不开启流控制；无奇偶效验；停止位1。
      drivers::serial_driver::SerialPortConfig config(
          baud_rate,
          drivers::serial_driver::FlowControl::NONE,
          drivers::serial_driver::Parity::NONE,
          drivers::serial_driver::StopBits::ONE);

      // 初始化串口
      try
      {
        io_context_ = std::make_shared<drivers::common::IoContext>(1);
        // 初始化 serial_driver_
        serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
        serial_driver_->init_port(device_name, config);
        serial_driver_->port()->open();

        RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
        RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
        RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
      }
      catch (const std::exception &ex)
      {
        RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
        return;
      }

      async_receive_message();   //进入异步接收
    }

  private:
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;

    std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
    std::shared_ptr<drivers::common::IoContext> io_context_;

    void async_receive_message()  //创建一个函数更方便重新调用
    {
      auto port = serial_driver_->port();

      // 设置接收回调函数
      port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size) 
      {
          if (size > 0)
          {
            RCLCPP_DEBUG(this->get_logger(),"\n");
            RCLCPP_DEBUG(this->get_logger(),"以下是接收到的IMU惯性计数据:");
            //创建imu消息类型
            auto imu_msg = sensor_msgs::msg::Imu();
            bool valid_frame = false;  // 添加有效帧标志
            bool valid_data = false;  // 添加有效帧标志

            for(int16_t i = 0;i < (int16_t)size;++i)
            {
              uint8_t data_buffer = data[i];
              // RCLCPP_INFO(this->get_logger(),"接收到的字节：%x",data_buffer);
              // 处理接收到的数据
              if(wit_imu_.rx.Data_Analysis(&data_buffer) == true)
              {
                valid_frame = true;  // 标记有有效帧需要处理
              }

              if(valid_frame == true)
              {
                if(wit_imu_.rx.data.cmd == 0x51)
                {
                  // 存储加速度计数据
                  wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.int16_buffer[2];
                  wit_imu_.rx.data.imu.Temp    = wit_imu_.rx.data.int16_buffer[3];

                  //加速度单位m/s2，温度单位℃摄氏度
                  wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.imu.Accel.X / 32768.0f * 16.0f * 9.8f;
                  wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.imu.Accel.Y / 32768.0f * 16.0f * 9.8f;
                  wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.imu.Accel.Z / 32768.0f * 16.0f * 9.8f;
                  wit_imu_.rx.data.imu.Temp    =  wit_imu_.rx.data.imu.Temp / 100.0f;

                  // 打印加速度计数据
                  RCLCPP_DEBUG(this->get_logger(), "加速度(XYZ): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Accel.X, wit_imu_.rx.data.imu.Accel.Y, wit_imu_.rx.data.imu.Accel.Z);
                  //减少时间戳带来的延迟
                  imu_msg.header.stamp = this->get_clock()->now();
                }

                else if(wit_imu_.rx.data.cmd == 0x52)
                {
                  // 存储陀螺仪数据
                  wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.int16_buffer[2];

                  //角速度单位rad/s
                  wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.imu.Gyro.X / 32768.0f * 2000.0f * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.imu.Gyro.Y / 32768.0f * 2000.0f * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.imu.Gyro.Z / 32768.0f * 2000.0f * (M_PI / 180.0f);

                  // 打印陀螺仪数据
                  RCLCPP_DEBUG(this->get_logger(), "角速度(XYZ): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Gyro.X, wit_imu_.rx.data.imu.Gyro.Y, wit_imu_.rx.data.imu.Gyro.Z);
                }

                else if(wit_imu_.rx.data.cmd == 0x54)
                {
                  // 存储磁力计数据(单位lsb)
                  wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.int16_buffer[2];

                  //磁场单位uT
                  // wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.imu.Magnet.X / 1.0f;
                  // wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.imu.Magnet.Y / 1.0f;
                  // wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.imu.Magnet.Z / 1.0f;

                  // 打印磁力计数据
                  RCLCPP_DEBUG(this->get_logger(), "磁场(XYZ): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Magnet.X, wit_imu_.rx.data.imu.Magnet.Y, wit_imu_.rx.data.imu.Magnet.Z);
                }

                else if(wit_imu_.rx.data.cmd == 0x53)
                {
                  // 存储欧拉角数据
                  wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.int16_buffer[2];

                  //欧拉角单位dec
                  wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll / 32768.0f * 180.0f;
                  wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch / 32768.0f * 180.0f;
                  wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw / 32768.0f * 180.0f;

                  // 打印欧拉角数据
                  RCLCPP_DEBUG(this->get_logger(), "欧拉角(XYZ_RPY): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Euler.roll, wit_imu_.rx.data.imu.Euler.pitch, wit_imu_.rx.data.imu.Euler.yaw);

                  //欧拉角单位rad
                  wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch  * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw  * (M_PI / 180.0f);
                }

                else if(wit_imu_.rx.data.cmd == 0x59)
                {
                  for(int16_t i = 0 ; i < 4 ; i++)
                  {
                    // 存储四元数数据
                    wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.int16_buffer[i];

                    //四元数是归一化的四元数
                    wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.imu.Quat.q[i] / 32768.0f;
                  }

                  // 打印四元数数据
                  //注意，0对应y，1对应x，-2才对应z。
                  RCLCPP_DEBUG(this->get_logger(), "四元数(xyzw): %.6f, %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Quat.q[1],wit_imu_.rx.data.imu.Quat.q[0],-wit_imu_.rx.data.imu.Quat.q[2],wit_imu_.rx.data.imu.Quat.q[3]);
                  valid_data = true;
                }
                wit_imu_.rx.data.cmd = 0x00;   //跑过一次就进行清0
                valid_frame = false;
              }
            }

            if(valid_data == true)
            {
              //时间戳
              // imu_msg.header.stamp = this->get_clock()->now();
              //位姿信息所参考的坐标系
              imu_msg.header.frame_id = "imu";

              // tf2::Quaternion q;
              // //DOF欧拉角单位rad
              // q.setRPY(roll_, pitch_, yaw_);
              // //DOF欧拉角（四元数）
              // imu_msg.orientation.x = q.x();
              // imu_msg.orientation.y = q.y();
              // imu_msg.orientation.z = q.z();
              // imu_msg.orientation.w = q.w();

              //DOF欧拉角（四元数）
              //注意对应关系
              // 传感器坐标系 -> ROS坐标系：
              // X -> Y
              // Y -> X
              // Z -> -Z
              imu_msg.orientation.x =   wit_imu_.rx.data.imu.Quat.q[1];
              imu_msg.orientation.y =   wit_imu_.rx.data.imu.Quat.q[0];
              imu_msg.orientation.z = - wit_imu_.rx.data.imu.Quat.q[2];
              imu_msg.orientation.w =   wit_imu_.rx.data.imu.Quat.q[3];

              //加速度
              imu_msg.linear_acceleration.x = wit_imu_.rx.data.imu.Accel.X;
              imu_msg.linear_acceleration.y = wit_imu_.rx.data.imu.Accel.Y;
              imu_msg.linear_acceleration.z = wit_imu_.rx.data.imu.Accel.Z;

              //角速度
              imu_msg.angular_velocity.x = wit_imu_.rx.data.imu.Gyro.X;
              imu_msg.angular_velocity.y = wit_imu_.rx.data.imu.Gyro.Y;
              imu_msg.angular_velocity.z = wit_imu_.rx.data.imu.Gyro.Z;

              imu_pub_->publish(imu_msg);
              valid_data = false;
            }

          }
          // 继续监听新的数据
          async_receive_message();
      }
      );
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<ImuNode>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

For such a framework, for a 250Hz gyroscope (output once every 4ms), the final /imu publish rate is 100Hz (published once every 10ms), and the efficiency is also acceptable.