c++ - 在 ROS 订阅者节点上集成角速度

Question

我正在尝试读取和计算两个量的积分，即线速度和角速度。但是由于某种原因，方向积分有很大的偏移量，大约为 6000000。鉴于仅在发布数据时（大约 50Hz）才调用循环并且 Imu 数据的大小在 0.05 的量级之外偏移量不能漂移。

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

  ros::Time current_time_;
  ros::Time last_time_;
  double rotation = 0;
  double dt = 0;
  
  
void chatterCallback(const sensor_msgs::Imu::ConstPtr& scout_imu){
  //data parsing    
  sensor_msgs::Imu imu_msg;
  imu_msg = *scout_imu;
  current_time_ = imu_msg.header.stamp;
    
  //the step size
  dt= (current_time_ - last_time_).toSec();
  
  //integration
  rotation =rotation + imu_msg.angular_velocity.z*dt;
  ROS_INFO("rotation: [%f,%f,%f]",rotation,dt,imu_msg.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;
}

阅读部分还可以。问题在于Integral的实施。 在此处输入图像描述

Answer 1

问题在于dt第一次迭代 ( last_time_ = 0) 的定义，这意味着dt = current_time_. 这就是大偏移的来源。