IMU(惯性测量单元)--基础

从本节起，小编将以一个全新的系列，来认真分享ROS的基础知识，区分于前面的试水写作，该系列将会更加精炼，可复刻程度更高，话不多说，我们马上开始。

第一节，我们将创建一个工作空间，并且之后的项目实践都将在其基础上实现，小编将在接下来的文章中给出文件的结构和源代码，读者只需在理解的基础上加以实践即可。

# 1. 创建工作空间目录
mkdir -p ~/Desktop/wireshark_ws/src
cd ~/Desktop/wireshark_ws/src

# 2. 初始化ROS工作空间
catkin_init_workspace

# 3. 回到根目录完成初始编译（生成基础目录）
cd ..
catkin_make
source devel/setup.bash

IMU = 惯性测量单元（Inertial Measurement Unit）

是一种测量物体运动状态的传感器模块，用来感知加速度、角速度、倾角等物理量。

核心组成

一个标准 IMU 通常包含：

• 加速度计：测线性加速度（前后、左右、上下）
• 陀螺仪：测角速度 / 旋转（翻滚、俯仰、偏航）
• 高端 IMU 还会加：磁力计（相当于电子罗盘）

IMU的作用

• 知道设备朝哪、怎么动、动得多快
• 计算姿态、方向、位移、速度

我们需要创建的imu_filter的文件结构

这个结构是 ROS 为了实现标准化、可复用、可维护的软件生态而设计的，主要有以下几个原因：

cfg文件夹

MyStuff.cfg

#!/usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("twoKp", double_t, 0, "2* accelerometer proportional gain", 0.01, 0.0, 0.1)
gen.add("twoKi", double_t, 0, "2* integral gain", 0.002, 0.0, 0.01)

exit(gen.generate("imu_filter", "imu_filter", "MyStuff"))

以下是举例说明，帮助读者理解，

#! /usr/bin/env python
# 声明脚本解释器：指定使用系统默认的Python环境运行该脚本（ROS脚本标准开头）

# 定义该动态参数配置所属的ROS包名（需与package.xml中的包名对应）
PACKAGE = 'filter'

# 导入ROS动态参数配置的核心生成器模块（catkin版本专用）
from dynamic_reconfigure.parameter_generator_catkin import *

# 创建参数生成器对象，用于定义可动态调整的参数
gen = ParameterGenerator()

# 定义第一个动态参数：twoKp（加速度计比例增益的2倍）
# 参数格式：gen.add(参数名, 类型, 级别, 描述, 默认值, 最小值, 最大值)
# – 'double'：参数类型为浮点型
# – 0：参数级别（0为通用级别，无需权限）
# – 0.01：默认值，0~0.1是可调范围
gen.add("twoKp", 'double', 0, "2* accelerometer proportional gain", 0.01, 0, 0.1);

# 定义第二个动态参数：twoKi（积分增益的2倍）
# – 0.002：默认值，0~0.01是可调范围，用于IMU滤波算法的积分项调整
gen.add("twoKi", 'double', 0, "2* integral gain", 0.002, 0, 0.01);

# 生成动态参数配置文件并退出脚本
# 参数格式：gen.generate(包名, 节点名/命名空间, 配置名)
# 生成后可通过rqt_reconfigure工具可视化调整twoKp/twoKi参数，无需重启节点
exit(gen.generate(PACKAGE, "filter", "MyStuff"))

这段代码是 ROS 动态参数配置文件（.cfg），核心逻辑：

基于 dynamic_reconfigure 生成名为 MyStuff 的参数配置，定义 twoKp、twoKi 两个可动态调整的浮点参数，指定默认值、范围和描述，供运行时调参使用。

MyStuff2.cfg

#!/usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("beta", double_t, 0, "Madgwick filter beta", 0.1088, 0.0, 0.5)

exit(gen.generate("imu_filter", "imu_filter", "MyStuff2"))

src文件夹

bias_calculator.cpp：

IMU 偏置计算器，用于计算陀螺仪 / 加速度计的静态偏置，为后续滤波提供校准数据。

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

double sum_gx = 0, sum_gy = 0, sum_gz = 0;
int count = 0;

void imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
sum_gx += msg->angular_velocity.x;
sum_gy += msg->angular_velocity.y;
sum_gz += msg->angular_velocity.z;
count++;

ROS_INFO("Bias: %.6f %.6f %.6f | samples: %d",
sum_gx/count, sum_gy/count, sum_gz/count, count);
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "bias_calculator");
ros::NodeHandle nh;

ros::Subscriber sub = nh.subscribe("/imu/data", 50, imu_callback);
ros::spin();

return 0;
}

Madgwick_filter.cpp：

Madgwick 滤波算法实现，通过梯度下降法融合 IMU 数据，输出高精度姿态角（四元数 / 欧拉角）。

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>

#include "imu_filter/MyStuff2Config.h"

ros::Publisher pub;

double beta = 0.1088;

double q0 = 1, q1 = 0, q2 = 0, q3 = 0;

void imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
double gx = msg->angular_velocity.x;
double gy = msg->angular_velocity.y;
double gz = msg->angular_velocity.z;

double ax = msg->linear_acceleration.x;
double ay = msg->linear_acceleration.y;
double az = msg->linear_acceleration.z;

double recipNorm;
double s0, s1, s2, s3;
double qDot1, qDot2, qDot3, qDot4;
double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

gx *= 0.5;
gy *= 0.5;
gz *= 0.5;

recipNorm = 1.0 / sqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

_2q0 = 2.0 * q0;
_2q1 = 2.0 * q1;
_2q2 = 2.0 * q2;
_2q3 = 2.0 * q3;
_4q0 = 4.0 * q0;
_4q1 = 4.0 * q1;
_4q2 = 4.0 * q2;
_8q1 = 8.0 * q1;
_8q2 = 8.0 * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;

s0 = -_2q2 * (ax – _2q1*az – _2q0*ay) + _2q1 * (ay + _2q0*ax – _2q3*az);
s1 = _2q3 * (ax – _2q1*az – _2q0*ay) + _2q0 * (ay + _2q0*ax – _2q3*az) – 4.0*q1*(1.0 – 2.0*q1q1 – 2.0*q2q2);
s2 = -_2q0 * (ax – _2q1*az – _2q0*ay) + _2q3 * (ay + _2q0*ax – _2q3*az) – 4.0*q2*(1.0 – 2.0*q1q1 – 2.0*q2q2);
s3 = _2q1 * (ax – _2q1*az – _2q0*ay) + _2q2 * (ay + _2q0*ax – _2q3*az);

recipNorm = 1.0 / sqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;

qDot1 = -q1*gx – q2*gy – q3*gz – beta*s0;
qDot2 = q0*gx + q2*gz – q3*gy – beta*s1;
qDot3 = q0*gy – q1*gz + q3*gx – beta*s2;
qDot4 = q0*gz + q1*gy – q2*gx – beta*s3;

q0 += qDot1;
q1 += qDot2;
q2 += qDot3;
q3 += qDot4;

recipNorm = 1.0 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;

geometry_msgs::Quaternion q;
q.w = q0;
q.x = q1;
q.y = q2;
q.z = q3;

pub.publish(q);
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "madgwick_imu_filter");
ros::NodeHandle nh;

pub = nh.advertise<geometry_msgs::Quaternion>("imu/madgwick_quaternion", 10);
ros::Subscriber sub = nh.subscribe("/imu/data", 10, imu_callback);

ros::spin();
return 0;
}

Mahony_filter.cpp：

Mahony 滤波算法实现，通过互补滤波融合 IMU 数据，计算量更小，适合资源受限的场景。

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>

#include "imu_filter/MyStuffConfig.h"

ros::Publisher pub;

double twoKp = 0.01;
double twoKi = 0.002;

double q0 = 1, q1 = 0, q2 = 0, q3 = 0;
double integralFBx = 0, integralFBy = 0, integralFBz = 0;

void imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
double gx = msg->angular_velocity.x;
double gy = msg->angular_velocity.y;
double gz = msg->angular_velocity.z;

double ax = msg->linear_acceleration.x;
double ay = msg->linear_acceleration.y;
double az = msg->linear_acceleration.z;

double recipNorm;
double halfvx, halfvy, halfvz;
double halfex, halfey, halfez;
double qa, qb, qc;

if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

halfvx = q1 * q3 – q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 – 0.5 + q3 * q3;

halfex = ay * halfvz – az * halfvy;
halfey = az * halfvx – ax * halfvz;
halfez = ax * halfvy – ay * halfvx;

if(twoKi > 0.0) {
integralFBx += twoKi * halfex;
integralFBy += twoKi * halfey;
integralFBz += twoKi * halfez;

gx += integralFBx;
gy += integralFBy;
gz += integralFBz;
}

gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}

qa = q0;
qb = q1;
qc = q2;

q0 += (-qb * gx – qc * gy – q3 * gz) * 0.5;
q1 += ( qa * gx + qc * gz – q3 * gy) * 0.5;
q2 += ( qa * gy – qb * gz + q3 * gx) * 0.5;
q3 += ( qa * gz + qb * gy – qc * gx) * 0.5;

recipNorm = 1.0 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;

geometry_msgs::Quaternion q;
q.w = q0;
q.x = q1;
q.y = q2;
q.z = q3;

pub.publish(q);
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "mahony_imu_filter");
ros::NodeHandle nh;

pub = nh.advertise<geometry_msgs::Quaternion>("imu/mahony_quaternion", 10);
ros::Subscriber sub = nh.subscribe("/imu/data", 10, imu_callback);

ros::spin();
return 0;
}

这三份源码是 ROS Melodic 下的标准实现

package.xml

<?xml version="1.0"?>
<package format="2">
<name>imu_filter</name>
<version>0.0.0</version>
<description>imu_filter</description>
<maintainer email="a@a.com">a</maintainer>
<license>BSD</license>

<buildtool_depend>catkin</buildtool_depend>

<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf2</build_depend>
<build_depend>dynamic_reconfigure</build_depend>

<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>tf2</exec_depend>
<exec_depend>dynamic_reconfigure</exec_depend>
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(imu_filter)

# 1. 查找所有依赖（顺序固定）
find_package(catkin REQUIRED COMPONENTS
roscpp
sensor_msgs
geometry_msgs
tf
tf2
dynamic_reconfigure
)

# 2. 生成动态参数头文件（仅针对需要的两个滤波器）
generate_dynamic_reconfigure_options(
cfg/MyStuff.cfg
cfg/MyStuff2.cfg
)

# 3. Catkin 包导出配置（顺序固定）
catkin_package(
CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs tf tf2 dynamic_reconfigure
)

# 4. 头文件包含路径（顺序固定）
include_directories(
${catkin_INCLUDE_DIRS}
)

# 5. 第一步：创建可执行目标（严格按「先创建，后操作」）
add_executable(madgwick_imu_filter src/Madgwick_filter.cpp)
add_executable(mahony_imu_filter src/Mahony_filter.cpp)
add_executable(bias_calculator src/bias_calculator.cpp)

# 6. 第二步：添加依赖（仅给需要动态参数的两个目标加）
add_dependencies(madgwick_imu_filter ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})
add_dependencies(mahony_imu_filter ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})
# bias_calculator 无动态参数，无需添加此依赖

# 7. 第三步：链接库（所有目标都需要，顺序在创建和依赖之后）
target_link_libraries(madgwick_imu_filter ${catkin_LIBRARIES})
target_link_libraries(mahony_imu_filter ${catkin_LIBRARIES})
target_link_libraries(bias_calculator ${catkin_LIBRARIES})

编译验证步骤

cd ~/Desktop/wireshark_ws
catkin_make clean # 清空之前的错误缓存
catkin_make # 执行编译

imu_filter 包的核心功能

这个包包含 3 个可执行文件，分工明确，都是围绕 IMU（惯性测量单元）的实用功能：

补充说明：

• 两个滤波算法都是无磁强计版本（只用到角速度 + 加速度），适合没有磁力计的 IMU；
• 输出的四元数可以直接转换成欧拉角（滚转 / 俯仰 / 偏航），用于机器人姿态控制、导航等场景；
• 偏置计算器是 IMU 校准的基础步骤，能大幅提升姿态解算的精度。

如何使用 imu_filter（完整步骤）

前置条件：

1. 第一步：校准陀螺仪偏置（必做，提升精度）

陀螺仪静止时会有微小的零漂（比如显示 0.01 rad/s 但实际没动），先校准：

# 1. 确保IMU静止放置（放在水平桌面，不要动）
# 2. 运行偏置计算器
rosrun imu_filter bias_calculator
# 3. 此时终端会持续打印：Bias: 0.001234 0.000567 0.000890 | samples: 123
# 等待采样数达到500+（数据稳定），记录下最终的偏置值（比如：gx_bias=0.0012, gy_bias=0.0006, gz_bias=0.0009）

2. 第二步：运行姿态解算节点（二选一）

方式 1：运行 Madgwick 滤波器

# 1. 先启动IMU数据发布节点（比如你的IMU驱动，示例：rosrun imu_tools imu_publisher）
# 2. 运行Madgwick滤波器
rosrun imu_filter madgwick_imu_filter
# 3. 查看输出的姿态四元数
rostopic echo /imu/madgwick_quaternion

方式 2：运行 Mahony 滤波器

rosrun imu_filter mahony_imu_filter
# 查看输出
rostopic echo /imu/mahony_quaternion

3. 第三步：（可选）将四元数转换成欧拉角

如果需要更直观的 “滚转 / 俯仰 / 偏航” 角度，可以用 ROS 自带工具转换：

# 订阅四元数话题，转换成欧拉角（单位：弧度）
rostopic echo /imu/madgwick_quaternion –noarr | python -c "
import sys
import math
for line in sys.stdin:
if 'w:' in line or 'x:' in line or 'y:' in line or 'z:' in line:
exec(line.strip())
if '—' in line:
# 四元数转欧拉角（roll/pitch/yaw）
roll = math.atan2(2*(w*x + y*z), 1-2*(x*x + y*y))
pitch = math.asin(2*(w*y – z*x))
yaw = math.atan2(2*(w*z + x*y), 1-2*(y*y + z*z))
print(f'Roll: {roll:.3f} rad ({roll*180/math.pi:.1f}°), Pitch: {pitch:.3f} rad ({pitch*180/math.pi:.1f}°), Yaw: {yaw:.3f} rad ({yaw*180/math.pi:.1f}°)')
w=x=y=z=0
"

三、进阶使用：修改参数 / 话题名

如果你的 IMU 话题不是 /imu/data，或想调整滤波参数，可以通过 ROS 参数重配置：

1. 修改 IMU 订阅话题

# 运行时指定话题名（比如你的IMU话题是 /my_imu/data）
rosrun imu_filter madgwick_imu_filter _imu_topic:=/my_imu/data

2. 调整滤波算法参数（动态参数）

我们之前配置的 cfg 文件支持运行时动态调参，无需重启节点：

# 启动动态调参工具
rosrun rqt_reconfigure rqt_reconfigure
# 在界面中找到 imu_filter 节点，可调整：
# – Mahony算法：twoKp（比例增益）、twoKi（积分增益）
# – Madgwick算法：beta（增益系数）
# 调整后实时生效，能直观看到姿态解算效果的变化

四、实用建议

rviz
# 添加「Pose」显示，话题选择 /imu/madgwick_quaternion，就能看到IMU的姿态变化