在 ROS 订阅者节点上集成角速度答案

【问题标题】：Integrating a Angular speed on a ROS subscrriber Node在 ROS 订阅者节点上集成角速度
【发布时间】：2021-11-02 16:45:04
【问题描述】：

我正在尝试读取和计算两个量的积分，即线速度和角速度。但是由于某种原因，方向积分有很大的偏移量，大约为 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的实施。 enter image description here

【问题讨论】：

查看代码，您似乎正在尝试从 IMU 计算速度和方向，以通过航位推算进行定位，对吗？如果是这种情况，作为旁注，我会强烈建议不要使用这种方法，因为它最终会产生非常不准确的姿势估计。
double dt, dt1,dist,rotation; 这些变量未初始化，因此您可能会遇到任何类型的错误输出。
请修剪您的代码，以便更容易找到您的问题。请按照以下指南创建minimal reproducible example。
我把代码剪了一点，现在只是订阅Imu数据，问题还是一样。我添加了输出终端的截图。

标签： c++ ros imu

【解决方案1】：

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

【讨论】：