• imu绘制运动轨迹

    目录

    写在前面

    1、本文内容
    读取IMU实时数据绘制运动轨迹

    2、平台
    ubuntu1804, ros melodic
    3、转载请注明出处：
    https://blog.csdn.net/qq_41102371/article/details/126153277

    代码

    mkdir -p imu_ws/src
cd imu_ws/src
catkin_create_pkg imu_path roscpp rospy tf nav_msgs
cd ..

    • 1
    • 2
    • 3
    • 4

    把imu_tf_pose.cpp放进src/imu_path/src
    在src/imu_path/CMakeLists.txt中添加

    add_executable(imu_tf_pose src/imu_tf_pose.cpp)
target_link_libraries( imu_tf_pose ${catkin_LIBRARIES})# ${SERIAL_LIB}

    • 1
    • 2

    在这里插入图片描述

    imu_tf_pose.cpp

    #include 
#include 
#include 
#include 
#include 
#include 
#include "nav_msgs/Path.h"

/* 参考ROS wiki
 * http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29
 * */

int count=0;

Eigen::Quaterniond Qwb(1.0,0.0,0.0,0.0);//Eigen::Quaterniond的顺序w, x, y z,

Eigen::Vector3d gw(0,0,-9.81);    // ENU frame

Eigen::Vector3d Vw=Eigen::Vector3d::Zero();    //
Eigen::Vector3d Pwb(1.0,1.0,0.0); 
ros::Publisher m_midPath_pub;
void PublishPath(ros::Publisher& puber, std::vector<Eigen::Vector3d> position, std::vector<Eigen::Quaterniond> rotation)
{
    nav_msgs::Path path_msg;
    geometry_msgs::PoseStamped pose;

    pose.header.stamp = ros::Time::now();
    // pose.header.frame_id = "/world";
    pose.header.frame_id = "/vio_odom_link";

    for(int i = 0; i < position.size();i++)
    {
        Eigen::Quaterniond Q_from_R = rotation[i];
        Eigen::Vector3d traj_node = position[i];
        pose.pose.position.x = traj_node(0);
        pose.pose.position.y = traj_node(1);
        pose.pose.position.z = traj_node(2);
        pose.pose.orientation.x = Q_from_R.x();
        pose.pose.orientation.y = Q_from_R.y();
        pose.pose.orientation.z = Q_from_R.z();
        pose.pose.orientation.w = Q_from_R.w();
        path_msg.poses.push_back(pose);
    }

    path_msg.header.stamp = ros::Time::now();
    path_msg.header.frame_id = "/vio_odom_link";
    // path_msg.header.frame_id = "/world";
    puber.publish(path_msg);
}


void ImuCallback(const sensor_msgs::ImuConstPtr& imu_data) {
    static tf::TransformBroadcaster br;//广播器
    tf::Transform transform;

    static std::vector<Eigen::Vector3d> midPosition;
    static std::vector<Eigen::Quaterniond> midRotation; 
    
    static double last_time;
    ++count;
    if(count<=50){
    // std::cout<header.seq<
    last_time=imu_data->header.stamp.toSec();
    return;
    }
    
    double now_time=imu_data->header.stamp.toSec();
    double dt=now_time-last_time;
    // std::cout<
    last_time=now_time;

    // //pre_integration_euler 欧拉积分
    // Eigen::Quaterniond dq;
    // Eigen::Vector3d dtheta_half ={imu_data->angular_velocity.x, imu_data->angular_velocity.y, imu_data->angular_velocity.z};
    // // std::cout<<"imu_data->angular_velocity: "<angular_velocity<
    // dtheta_half=dtheta_half*dt/2.0;
    // // std::cout<<"dtheta_half "<
    // dq.w() = 1;
    // dq.x() = dtheta_half.x();
    // dq.y() = dtheta_half.y();
    // dq.z() = dtheta_half.z();
    // Eigen::Vector3d imu_acc(imu_data->linear_acceleration.x,imu_data->linear_acceleration.y,imu_data->linear_acceleration.z);
    // Eigen::Vector3d acc_w = Qwb.normalized() * (imu_acc)+gw;
    // // std::cout<
    // Qwb = Qwb * dq;
    // Qwb.normalize();
    // std::cout<<"Vw:\n"<
    // Vw = Vw + acc_w * dt;
    // Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

    //pre_integration_mid 中值积分
    static Eigen::Vector3d last_acc = Eigen::Vector3d::Zero() ;
    static Eigen::Vector3d last_gyr = Eigen::Vector3d::Zero() ;
    static bool first_flag = true;

    Eigen::Vector3d imu_acc(imu_data->linear_acceleration.x,imu_data->linear_acceleration.y,imu_data->linear_acceleration.z);
    Eigen::Vector3d imu_gyro(imu_data->angular_velocity.x, imu_data->angular_velocity.y, imu_data->angular_velocity.z);
    if(first_flag == true)
    {
        first_flag = false;
        // last_acc = Qwb.normalized() * (Eigen::Vector3d::Zero()) + gw;
        // last_gyr =Eigen::Vector3d::Zero();
        last_acc = Qwb.normalized() * (imu_acc) + gw;
        last_gyr =imu_gyro;
    }
    Eigen::Quaterniond dq;
    Eigen::Vector3d dtheta_half =  (imu_gyro + last_gyr) / 2.0 * dt / 2.0;
    dq.w() = 1;
    dq.x() = dtheta_half.x();
    dq.y() = dtheta_half.y();
    dq.z() = dtheta_half.z();
    Eigen::Vector3d acc_k = Qwb.normalized() * (imu_acc) + gw;
    Eigen::Vector3d acc_mid = (acc_k + last_acc) / 2.0;  
    Qwb = Qwb.normalized() * dq.normalized();
    Vw = Vw+acc_mid * dt;
    Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_mid;

    // std::cout<<"count "<
    // std::cout<<"dt: "<
    // std::cout<<"imu_gyro: \n"<
    // std::cout<<"last_acc: \n"<
    // std::cout<<"imu_acc: \n"<
    // std::cout<<"acc_k: \n"<
    // std::cout<<"acc_mid: \n"<
    // std::cout<<"Vw: \n"<
    last_acc = acc_k;
    last_gyr = imu_gyro;

             // save path
    midPosition.push_back(Pwb);
    midRotation.push_back(Qwb);
    
    if(count % 5 == 0)
    {
        PublishPath(m_midPath_pub, midPosition, midRotation);
        std::cout << " mid-integration pub path : " << count << std::endl;
    }


    transform.setOrigin(tf::Vector3(Pwb.x(), Pwb.y(), Pwb.z()));//设置平移部分
    // transform.setOrigin(tf::Vector3(position_x, position_y, position_z));//设置平移部分

    //从IMU消息包中获取四元数数据
    tf::Quaternion q;
    q.setX(Qwb.x());
    q.setY(Qwb.y());
    q.setZ(Qwb.z());
    q.setW(Qwb.w());
    q.normalized();//归一化

    transform.setRotation(q);//设置旋转部分
    //广播出去
    // br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "imu"));
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "vio_odom_link", "imu_sim"));
}
int main (int argc, char ** argv) {
    ros::init(argc, argv, "imu_data_to_tf");
    
    ros::NodeHandle node;

    m_midPath_pub = node.advertise<nav_msgs::Path>("/imu_path", 10);
	//IMU_data改成自己imu的话题就行了
    ros::Subscriber sub = node.subscribe("/IMU_data", 10, &ImuCallback);

    ros::spin();
    return 0;
}


    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150
    • 151
    • 152
    • 153
    • 154
    • 155
    • 156
    • 157
    • 158
    • 159
    • 160
    • 161
    • 162
    • 163
    • 164
    • 165
    • 166
    • 167
    • 168

    编译运行

    catkin_make
source ./devel/setup.bash

    • 1
    • 2

    连上IMU，打开rviz，rivz配置如下图

    rosrun imu_path imu_tf_pose

    • 1

    摆动IMU会看到轨迹
    放置IMU在水平面不动也会看到轨迹在动，因为平面一般都不是真正完全水平，重力加速度会在水平xy方向有加速度分量
    在这里插入图片描述

    注意

    	//IMU_data改成自己imu的话题就行了
    ros::Subscriber sub = node.subscribe("/IMU_data", 10, &ImuCallback);

    • 1
    • 2

    参考

    https://github.com/robosu12/imu_data_simulation
    数值积分方法：欧拉积分、中点积分和龙格－库塔法积分 https://blog.csdn.net/qq_41102371/article/details/125935198

    完

    如有错漏，敬请指正
    --------------------------------------------------------------------------------------------202207

  • 相关阅读:
    【电脑使用】CCleaner使用的坑
    GPC规范-SCP02
    ChatGPT 4.0：革新文献检索与推荐体验
    【C/C++】函数指针与指针函数
    设计模式之备忘录模式
    【delphi】中 TNetHTTPClient 注意事项
    图文并茂演示小程序movable-view的可移动范围
    I/O多路转接-epoll
    ViewConfiguration
    Python动态建模（4）
  • 原文地址：https://blog.csdn.net/qq_41102371/article/details/126153277