imu_gravity_detection v1.0 🧭 基于互补滤波的IMU重力方向

📖 概述

imu_gravity_detection 提供了一个完整的ROS节点和可复用的C++库，用于从原始IMU数据（角速度和线加速度）中提取稳定的重力向量。它使用经典互补滤波（Mahony/Madgwick思想简化），有效抑制加速度计噪声，同时利用陀螺仪快速响应动态变化。输出包括：

✨ 核心特性

• 高效互补滤波 — 融合加速度计低频准确性与陀螺仪高频动态，alpha参数可调 (默认0.98)。
• 即插即用ROS节点 — 订阅 /camera/imu 话题，发布重力向量、TF及可视化Marker。
• RealSense D455 专用启动文件 — 一键启动D455的IMU流并运行重力检测。
• RViz可视化支持 — 红色箭头表示重力方向，附带 gravity_direction 坐标系。
• 纯头文件库+Eigen — 核心算法封装于 IMUGravityDetector 类，易于集成。
• 倾斜角输出 — 实时俯仰(pitch)、横滚(roll)角度，用于导航或平衡应用。

⚡ 快速开始

使用RealSense D455运行完整示例：

# 启动重力检测与可视化 (需先连接D455)
roslaunch imu_gravity_detection gravity_visualization.launch

预期效果：终端打印重力向量和倾角，RViz窗口显示红色箭头指向重力方向。

仅运行重力检测节点（使用已有IMU话题）：

rosrun imu_gravity_detection imu_gravity_node _frame_id:=camera_link

📦 编译与依赖

Ubuntu 20.04/22.04 (ROS Noetic)

# 安装依赖 (假设已安装ROS)
sudo apt install ros-${ROS_DISTRO}-tf2 ros-${ROS_DISTRO}-tf2-ros \
                 ros-${ROS_DISTRO}-visualization-msgs libeigen3-dev

# 创建工作空间并编译
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone https://github.com/your/imu_gravity_detection.git
cd ..
catkin_make
source devel/setup.bash

CMakeLists.txt 片段 (已提供)

find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs ...)
find_package(Eigen3 REQUIRED)
add_library(imu_gravity_detection src/imu_gravity_detector.cpp)
add_executable(imu_gravity_node src/imu_gravity_node.cpp)
target_link_libraries(imu_gravity_node imu_gravity_detection ${catkin_LIBRARIES})

🏗️ 系统架构

• IMUGravityDetector (核心库) — 无ROS依赖，实现互补滤波、四元数积分、加速度校正。
• imu_gravity_node (ROS节点) — 订阅 /camera/imu，调用Detector，发布话题/TF/Marker。
• 启动文件 — 分别启动D455驱动、重力节点和RViz。
• 可视化 — RViz箭头 + TF变换 gravity_direction。

数据流：realsense2_camera → /camera/imu → imu_gravity_node → 重力向量、TF、Marker → RViz。

📐 IMUGravityDetector 类

头文件: imu_gravity_detection/imu_gravity_detector.h

构造函数

alpha：互补滤波系数 (0: 完全信任加速度计, 1: 完全信任陀螺仪积分，通常0.95~0.98)。

核心方法

实现细节

• 陀螺仪积分：q_dot = 0.5 * q * (0, gyro_rad)，一阶积分后归一化。
• 加速度校正：使用 FromTwoVectors 计算重力估计误差并修正四元数（简化Mahony）。
• 互补滤波：最终重力向量 = alpha * (旋转矩阵*全局重力) + (1-alpha)*归一化加速度计读数。

🎛️ ROS节点 imu_gravity_node

节点名称: imu_gravity_detection

订阅话题

发布话题

参数

🚀 启动文件

d455_imu.launch

仅启动D455相机并开启IMU数据流（无彩色/深度）。

<include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="enable_accel" value="true"/>
    <arg name="enable_gyro" value="true"/>
    <arg name="unite_imu_method" value="linear_interpolation"/>
    ...
</include>

gravity_visualization.launch

一键启动D455 IMU + 重力检测节点 + RViz（载入预设视图）。

roslaunch imu_gravity_detection gravity_visualization.launch

确保RViz配置文件 gravity_visualization.rviz 存在于 rviz/ 目录，显示TF和Marker。

👁️ 可视化与TF

TF树：节点发布从 frame_id (如camera_link) 到 gravity_direction 的静态旋转。该旋转使重力方向对齐到父坐标系的Z轴负方向。直观理解：gravity_direction 的Z轴指向地心。

Marker箭头：红色箭头起点在原点，方向与重力向量一致，长度0.5米，便于在RViz中观察重力指向。

RViz配置建议：添加TF显示，并添加Marker话题 /gravity_marker 为箭头类型。参考文件 gravity_visualization.rviz 已包含基本设置。

⚙️ 参数配置

目前节点仅通过私有参数 frame_id 配置。如需修改互补滤波系数alpha或发布频率，可直接更改源码 imu_gravity_node.cpp 中的 gravity_detector_(0.98) 和订阅队列大小。

常见调整

🔗 与Intel RealSense D455集成

本包专为D455设计，利用其内置IMU（BMI055）。关键点：

• 坐标系：D455的IMU frame 通常为 camera_imu_frame，但驱动将其与 camera_link 关联。本节点默认监听 camera_link，也可通过参数调整。
• 数据同步：启动文件中 unite_imu_method:="linear_interpolation" 确保加速度计和陀螺仪时间对齐。
• 零偏：驱动已提供去偏后的角速度，无需额外处理。

测试命令：rostopic echo /camera/imu 可查看原始数据。

💡 示例代码：自定义节点中使用Detector

#include "imu_gravity_detection/imu_gravity_detector.h"
#include <Eigen/Dense>
#include <iostream>

int main() {
    IMUGravityDetector detector(0.98f);
    float dt = 0.01f; // 假设100Hz

    // 模拟IMU数据 (静止)
    Eigen::Vector3f accel(0.0, 0.0, 9.8);
    Eigen::Vector3f gyro(0.0, 0.0, 0.0);

    detector.updateIMU(accel, gyro, dt);
    Eigen::Vector3f g = detector.getGravityDirection();

    std::cout << "重力方向: " << g.transpose() << std::endl;
    return 0;
}

🔍 故障排除

📊 性能调优建议

• 降低回调频率：如果CPU紧张，可以在 imuCallback 中增加简单的降采样（如每2帧处理一次）。
• 减少Marker发布：延长Marker的lifetime，或降低发布频率（当前每帧发布）。
• Eigen优化：编译时开启 -O3 -march=native 以利用SIMD。
• 实时性：互补滤波本身计算极轻，主要负载在ROS通信。如需要极高频率，可考虑合并节点。

📝 更新日志

版本 1.0.0 (2026-03-19)

• 初始稳定版发布。
• 实现互补滤波重力估计。
• 集成D455启动与RViz可视化。
• 提供TF变换和Marker发布。

版本 0.9.0 (2026-02-15)

• 原型：简单的加速度计低通滤波。

⚖️ 许可证