ROS订阅节点中IMU角速度积分出现大幅偏移问题求助

数据老陈

2026-9-27

问题根因

你遇到的偏移问题核心是时间变量初始化逻辑缺失：

全局变量last_time_启动后未赋值，默认是Unix纪元0时间戳（1970-01-01 00:00:00）
第一次触发回调时，计算得到的dt等于当前时间减去0时间，数值量级刚好是百万级，乘以角速度后直接得到了几百万的错误初始偏移，后续所有积分都是在这个错误值基础上累加，所以结果完全异常。

修复方案

修改代码补充首次回调的时间初始化逻辑，同时增加异常校验，修改后代码如下：

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

ros::Time current_time_;
ros::Time last_time_;
double rotation = 0;
double dt = 0;
// 新增首次回调标记
bool is_first_callback = true;

void chatterCallback(const sensor_msgs::Imu::ConstPtr& scout_imu){
  current_time_ = scout_imu->header.stamp;
  // 首次回调仅初始化时间，不执行积分
  if(is_first_callback){
    last_time_ = current_time_;
    is_first_callback = false;
    return;
  }
  
  dt = (current_time_ - last_time_).toSec();
  // 异常dt校验：50Hz频率下正常dt约为0.02s，超过0.1s或为负数时跳过当前帧避免错误
  if(dt > 0.1 || dt < 0){
    ROS_WARN("异常时间差: %f，跳过当前帧", dt);
    last_time_ = current_time_;
    return;
  }

  rotation += scout_imu->angular_velocity.z * dt;
  ROS_INFO("转角: [%f], 时间差: [%f], 角速度z: [%f]", rotation, dt, scout_imu->angular_velocity.z);
  last_time_ = current_time_;
}

int main(int argc, char **argv){
  ros::init(argc, argv, "scout_subs");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("/mavros/imu/data_raw", 1000, chatterCallback);
  ros::spin();
  return 0;
}