利用IMU数据来计算位移

目标：

利用IMU测得的加速度信息来计算位移，很简单假设是匀加速运动或是匀速运动都可以，主要是看问题的背景来具体去确定运动模型，这里我就取个简单的匀加速运动模型。

---

学习过程：

1、了解IMU的相关原理知识

2、阅读产品自带的SDK包

3、自己添加并修改相关代码

---

学习内容：

这里主要介绍我是如何添改的代码。因为想要直接获取位移需要时间和加速度，这里的出速度我设为0，其加速度的获取可以阅读产品自带的功能包中的源码，我在编写过程中主要是在时间的获取上出现了一点问题，需要获得IMU开启时的时间戳以及当前帧的时间戳。

在运行产品自带的源码时，可以看见每一帧都对应着一个 seq 和 stamp ，所以只要把 seq 为1的那一帧对应的时间戳提取出来作为起始时间即可，如图所示，同样的方法去提取当前帧的时间戳。

---

学习产出：

#include <ahrs_driver.h>
#include <Eigen/Eigen>
using namespace std;
void my_handle_imu_data(const sensor_msgs::Imu::ConstPtr& imu_data)
{
    cout<<"accelaration X is :"<<imu_data->linear_acceleration.x<<endl;
    cout<<"accelaration Y is :"<<imu_data->linear_acceleration.y<<endl;

    static int start_time;
    int now_time;
    if(imu_data->header.seq == 50)
    {
        start_time = int(imu_data->header.stamp.toSec());
        cout<<"when the seq is "<<imu_data->header.seq<<" "<<" the stamp is "<<start_time<<endl;
    }

    cout<<"the start time is :"<<start_time<<endl;
    now_time = int(imu_data->header.stamp.toSec());
    if(start_time == now_time)
    {
        cout<<"the shift on X is :"<< 0 <<endl;
        cout<<"the shift on Y is :"<< 0 <<endl;
    }
    else if(now_time > start_time)
    {
        double X_Shift,Y_Shift;
        int Dealt_Time;
        Dealt_Time = now_time - start_time;
        X_Shift = 0.5 * imu_data->linear_acceleration.x * Dealt_Time * Dealt_Time;
        Y_Shift = 0.5 * imu_data->linear_acceleration.y * Dealt_Time * Dealt_Time;
        cout<<"the shift on X is :"<<X_Shift<<endl;
        cout<<"the shift on Y is :"<<Y_Shift<<endl;
        cout<<"now time is :"<<now_time<<endl;
        cout<<"start_time is :"<<start_time<<endl;
        cout<<imu_data->header.seq<<endl;
        //cout<<imu_data->header.stamp<<endl;
        cout<<"*********************************************"<<endl;
    }
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"cal_shift");
    ros::NodeHandle nh;
    ros::Subscriber sub_imu_data_ = nh.subscribe<sensor_msgs::Imu>("/mobile_base/sensors/imu_data",10,my_handle_imu_data);
    ros::spin();
    return 0;
}

代码很短，原理也很简单，主要是需要注意一下 start_time 变量前的 static 关键字，它可以保证该变量在程序结束时，再释放变量。同学可以试一下删除 static 之后变量的值是什么。

然后就是ros相关的知识，想要学习的话可以在这里学习：保姆级教程

【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程_哔哩哔哩_bilibili