25 - IMU数据处理 | ROS2入门课程

🧠 IMU传感器原理

IMU(Inertial Measurement Unit)包含三轴加速度计、三轴陀螺仪和三轴磁力计(9-DOF)。加速度计测量线性加速度+重力，陀螺仪测量角速度，磁力计测量地磁场方向。

IMU传感器组合： ┌─────────────────────────────────────┐ │ 9-DOF IMU │ │ │ │ 加速度计: ax, ay, az (含重力) │ │ ┌──────┐ → 静止时: [0,0,9.8]m/s² │ │ │ ACC │ → 倾斜: 重力分量变化 │ │ └──────┘ │ │ 陀螺仪: gx, gy, gz (角速度) │ │ ┌──────┐ → 积分得角度(有漂移) │ │ │ GYRO │ → 短期精度高 │ │ └──────┘ │ │ 磁力计: mx, my, mz (地磁场) │ │ ┌──────┐ → 提供绝对航向 │ │ │ MAG │ → 受铁磁干扰 │ │ └──────┘ │ └─────────────────────────────────────┘ 融合策略： ACC → 低频姿态参考(无漂移，噪声大) GYRO → 高频角速度(短期精确，长期漂移) 融合 → 取长补短，互补滤波/卡尔曼滤波

📐 Imu消息格式

# sensor_msgs/msg/Imu std_msgs/Header header geometry_msgs/Quaternion orientation # 姿态四元数 float64[9] orientation_covariance # 姿态协方差 geometry_msgs/Vector3 angular_velocity # 角速度 rad/s float64[9] angular_velocity_covariance geometry_msgs/Vector3 linear_acceleration # 线性加速度 m/s² float64[9] linear_acceleration_covariance

🐍 Python：IMU姿态解算节点

#!/usr/bin/env python3 # imu_attitude_node.py - IMU姿态解算与互补滤波 # ✅ Docker验证通过 import math, rclpy, numpy as np from rclpy.node import Node from sensor_msgs.msg import Imu from geometry_msgs.msg import Quaternion, Vector3 from std_msgs.msg import String class IMUAttitudeNode(Node): def __init__(self): super().__init__('imu_attitude') self.declare_parameter('complementary_alpha', 0.98) # 互补滤波系数 self.declare_parameter('publish_rate', 50.0) self.declare_parameter('gravity', 9.81) self.declare_parameter('calibration_samples', 200) self.alpha = self.get_parameter('complementary_alpha').value self.g = self.get_parameter('gravity').value # 互补滤波状态 self.roll = 0.0 self.pitch = 0.0 self.yaw = 0.0 self.last_time = None self.is_calibrated = False self.calib_samples = [] self.calib_count = self.get_parameter('calibration_samples').value self.gyro_bias = np.zeros(3) # 订阅原始IMU self.imu_sub = self.create_subscription(Imu, '/imu/data', self._imu_cb, 10) # 发布滤波后IMU self.filtered_pub = self.create_publisher(Imu, '/imu/filtered', 10) self.attitude_pub = self.create_publisher(String, '/attitude_info', 10) self.get_logger().info('🧭 IMU姿态解算节点已启动') def _imu_cb(self, msg: Imu): # 校准阶段 if not self.is_calibrated: self.calib_samples.append([ msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z ]) if len(self.calib_samples) >= self.calib_count: self.gyro_bias = np.mean(self.calib_samples, axis=0) self.is_calibrated = True self.get_logger().info( f'✅ 陀螺仪校准完成: bias={self.gyro_bias}') return # 当前时间 now = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 if self.last_time is None: self.last_time = now return dt = now - self.last_time self.last_time = now if dt <= 0 or dt > 0.5: return # 获取去偏后的角速度 gx = msg.angular_velocity.x - self.gyro_bias[0] gy = msg.angular_velocity.y - self.gyro_bias[1] gz = msg.angular_velocity.z - self.gyro_bias[2] # 加速度计姿态(低频参考) ax, ay, az = msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z acc_roll = math.atan2(ay, az) acc_pitch = math.atan2(-ax, math.sqrt(ay*ay + az*az)) # 互补滤波 # 陀螺仪积分(高频) + 加速度计(低频) self.roll = self.alpha * (self.roll + gx * dt) + (1 - self.alpha) * acc_roll self.pitch = self.alpha * (self.pitch + gy * dt) + (1 - self.alpha) * acc_pitch self.yaw += gz * dt # 仅有陀螺仪积分(无磁力计参考) # 归一化yaw到[-π, π] self.yaw = (self.yaw + math.pi) % (2 * math.pi) - math.pi # 发布滤波后的IMU数据 filtered = Imu() filtered.header = msg.header # 四元数: roll(x), pitch(y), yaw(z) cy, sy = math.cos(self.yaw/2), math.sin(self.yaw/2) cp, sp = math.cos(self.pitch/2), math.sin(self.pitch/2) cr, sr = math.cos(self.roll/2), math.sin(self.roll/2) filtered.orientation.w = cr*cp*cy + sr*sp*sy filtered.orientation.x = sr*cp*cy - cr*sp*sy filtered.orientation.y = cr*sp*cy + sr*cp*sy filtered.orientation.z = cr*cp*sy - sr*sp*cy filtered.angular_velocity = msg.angular_velocity filtered.linear_acceleration = msg.linear_acceleration self.filtered_pub.publish(filtered) # 定期输出姿态信息 r_deg, p_deg, y_deg = math.degrees(self.roll), math.degrees(self.pitch), math.degrees(self.yaw) info = String() info.data = f'Roll:{r_deg:+.1f}° Pitch:{p_deg:+.1f}° Yaw:{y_deg:+.1f}°' self.attitude_pub.publish(info) def main(args=None): rclpy.init(args=args); rclpy.spin(IMUAttitudeNode()); rclpy.shutdown()

🐍 Python：IMU数据记录器

#!/usr/bin/env python3 # imu_logger.py - IMU数据记录与分析 import rclpy, numpy as np, math, time from rclpy.node import Node from sensor_msgs.msg import Imu from collections import deque class IMULogger(Node): def __init__(self): super().__init__('imu_logger') self.declare_parameter('window_size', 1000) self.declare_parameter('report_interval', 5.0) window = self.get_parameter('window_size').value self.acc_data = deque(maxlen=window) self.gyro_data = deque(maxlen=window) self.timestamps = deque(maxlen=window) self.sub = self.create_subscription(Imu, '/imu/data', self._cb, 10) self.timer = self.create_timer( self.get_parameter('report_interval').value, self._report) self.get_logger().info('📊 IMU记录器已启动') def _cb(self, msg): self.acc_data.append([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]) self.gyro_data.append([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z]) self.timestamps.append(time.time()) def _report(self): if len(self.acc_data) < 10: return acc = np.array(self.acc_data) gyro = np.array(self.gyro_data) # 统计分析 acc_mean = np.mean(acc, axis=0) acc_std = np.std(acc, axis=0) gyro_mean = np.mean(gyro, axis=0) gyro_std = np.std(gyro, axis=0) # 检测运动状态 acc_mag = np.linalg.norm(acc_mean) is_stationary = np.all(gyro_std < 0.01) and abs(acc_mag - 9.81) < 0.5 # 频率估计 if len(self.timestamps) >= 2: dt = self.timestamps[-1] - self.timestamps[0] freq = (len(self.timestamps)-1) / dt if dt > 0 else 0 else: freq = 0 self.get_logger().info( f'📊 IMU报告 | ' f'频率:{freq:.0f}Hz | ' f'状态:{"静止" if is_stationary else "运动"} | ' f'ACC均值:[{acc_mean[0]:.2f},{acc_mean[1]:.2f},{acc_mean[2]:.2f}] | ' f'GYRO漂移:[{gyro_mean[0]:.4f},{gyro_mean[1]:.4f},{gyro_mean[2]:.4f}]' ) def main(args=None): rclpy.init(args=args); rclpy.spin(IMULogger()); rclpy.shutdown()

🔧 C++：Madgwick滤波器核心

// madgwick_core.cpp - Madgwick姿态滤波器核心 #include <cmath> class MadgwickFilter { float beta_; // 滤波增益 float q0_,q1_,q2_,q3_; // 四元数 public: MadgwickFilter(float beta=0.1) : beta_(beta), q0_(1), q1_(0), q2_(0), q3_(0) {} void update(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float norm = std::sqrt(ax*ax + ay*ay + az*az); if (norm < 1e-6f) return; ax /= norm; ay /= norm; az /= norm; // 辅助变量 float q0q0=q0_*q0_, q0q1=q0_*q1_, q0q2=q0_*q2_, q0q3=q0_*q3_; float q1q1=q1_*q1_, q1q2=q1_*q2_, q1q3=q1_*q3_; float q2q2=q2_*q2_, q2q3=q2_*q3_, q3q3=q3_*q3_; // 梯度下降修正 float f0 = 2*(q1q3-q0q2)-ax; float f1 = 2*(q0q1+q2q3)-ay; float f2 = 2*(0.5f-q1q1-q2q2)-az; float J00=-2*q2_, J01=2*q1_, J02=2*q0_, J03=-2*q3_; float J10=2*q1_, J11=2*q0_, J12=2*q3_, J13=2*q2_; float J20=0; float J21=-4*q1_; float J22=-4*q2_; float J23=0; // 梯度 float g0=J00*f0+J10*f1+J20*f2; float g1=J01*f0+J11*f1+J21*f2; float g2=J02*f0+J12*f1+J22*f2; float g3=J03*f0+J13*f1+J23*f2; norm = std::sqrt(g0*g0+g1*g1+g2*g2+g3*g3); if (norm > 1e-6f) { g0/=norm; g1/=norm; g2/=norm; g3/=norm; } // 四元数更新 q0_ += (gx*(-q1_)+gy*(-q2_)+gz*(-q3_)-beta_*g0)*0.5f*dt; q1_ += (gx*q0_+gy*q3_+gz*(-q2_)-beta_*g1)*0.5f*dt; q2_ += (gx*(-q3_)+gy*q0_+gz*q1_)-beta_*g2)*0.5f*dt; q3_ += (gx*q2_+gy*(-q1_)+gz*q0_)-beta_*g3)*0.5f*dt; norm=std::sqrt(q0_*q0_+q1_*q1_+q2_*q2_+q3_*q3_); q0_/=norm; q1_/=norm; q2_/=norm; q3_/=norm; } void getEuler(float& roll, float& pitch, float& yaw) const { roll = std::atan2(2*(q0_*q1_+q2_*q3_),1-2*(q1_*q1_+q2_*q2_)); pitch = std::asin(2*(q0_*q2_-q3_*q1_)); yaw = std::atan2(2*(q0_*q3_+q1_*q2_),1-2*(q2_*q2_+q3_*q3_)); } };

🎯 练习题

🏆 成就解锁

🏅 IMU处理专家

✅ 理解IMU传感器原理
✅ 实现互补滤波姿态解算
✅ 掌握四元数与欧拉角转换
✅ 学会IMU校准与去噪

经验值：+250 XP

🧭 第25课：IMU数据处理

📋 课程目标