realsense中IMU的简单使用与学习

IMU的基本原理

（如有错误，望批评指正）
惯性测量单元（IMU）通常包含加速度计和陀螺仪组成的组合单元，陀螺仪就是内部有一个陀螺，它的轴由于陀螺效应始终与初始方向平行，这样就可以通过与初始方向的偏差计算出旋转方向和角度，这些都是角度变化值。而加速度计能敏感地感受到三个方向上加速度变化，如果在静止状态上，应该有一个垂直与xy轴向下，大小为9.8的加速度（通常都不是这个值），我们可以在静止下通过在每个方向上的加速度值得出初始pitch和roll角。

上图中，红色为x轴，绿色是y轴，蓝色是z轴; 一般相机正视方向为z轴。相当于左上角为原点的像素矩阵。

1. 相机在围绕x轴旋转产生pitch俯仰角$\theta$

2. 再旋转z轴产生roll翻滚角$\alpha$

根据几何关系：

$$\theta =atan2(\frac{z}{\sqrt{x^2+y^2}})$$
$$\alpha =atan2(\frac{x}{y})$$

简单的位姿解算代码

#include <iostream>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <boost/thread/thread.hpp>
#include <math.h>

using namespace message_filters;
using namespace std;

typedef struct imu6d{ //定义一个储存IMU数据的结构体
    double accel_x;
    double accel_y;
    double accel_z;
    double gyro_x;
    double gyro_y;
    double gyro_z;

    struct imu6d operator +(const struct imu6d &p) //重载+运算符
    {
        struct imu6d temp;
        temp.accel_x = p.accel_x + accel_x;
        temp.accel_y = p.accel_y + accel_y;
        temp.accel_z = p.accel_z + accel_z;
        temp.gyro_x = p.gyro_x + gyro_x;
        temp.gyro_y = p.gyro_y + gyro_y;
        temp.gyro_z = p.gyro_z + gyro_z;
        return temp;
    }

    friend ostream & operator << (ostream & os, const imu6d &p) //重载<<运算符方便查看数据
    {
        os << "accel_x: "<< p.accel_x << "\t accel_y: "<< p.accel_y << "\t accel_z: "<< p.accel_z<<"\n"
        << "gyro_x: "<< p.gyro_x<< "\t gyro_y: "<< p.gyro_y<< "\t gyro_z: "<< p.gyro_z;
        return os;
    }

} imu6d;

typedef struct imu3d{
    double x;
    double y;
    double z;

    struct imu3d operator +(const struct imu3d &p)
    {
        struct imu3d temp;
        temp.x = p.x + x;
        temp.y = p.y + y;
        temp.z = p.z + z;
        return temp;
    }

    struct imu3d operator *(const double p)
    {
        struct imu3d temp;
        temp.x = x * p;
        temp.y = y * p;
        temp.z = z * p;

        return temp;
    }

    friend ostream & operator << (ostream & os, const imu3d &p) //重载<<运算符方便查看数据
    {
        os << "x: "<< p.x << "\t y: "<< p.y << "\t z: "<< p.z;
        return os;
    }

} imu3d;


class imuTester{

public:
    ros::NodeHandle nh_;
    typedef sync_policies::ApproximateTime<sensor_msgs::Imu, sensor_msgs::Imu> syncPolicy; //ExactTime使用时间戳精确同步;ApproximateTime近似同步
    
    imuTester() //构造函数
    {
        time_s=0;
        flag = 1;
        total_angular = {0,0,0};
        Angle = {0,0,0};
        total_yaw = 0;
        //订阅加速度计、角速度计信息。下面的new是写类时需要的用法，不用类可参考官方写法
        accel_sub = new message_filters::Subscriber<sensor_msgs::Imu> (nh_,"/camera/accel/sample",2000,ros::TransportHints().tcpNoDelay());
        gyro_sub = new message_filters::Subscriber<sensor_msgs::Imu> (nh_,"/camera/gyro/sample",2000,ros::TransportHints().tcpNoDelay());
        
        //同步消息并进入回调，这里因为有boost::bind的多线程，导致不能delete sync
        sync = new Synchronizer<syncPolicy> (syncPolicy(10), *accel_sub, *gyro_sub);
        sync->registerCallback(boost::bind(&imuTester::imu_callback,this, _1, _2));
        IMU_pub = nh_.advertise<sensor_msgs::Imu>("/imu_data_pub", 1);
    }
    ~imuTester()//析构函数
    {
        cout<<"delete memory..."<<endl;
        delete accel_sub;
        delete gyro_sub;
        cout<<"delete memory finished!"<<endl;
    }


private:
    double time_s;
    double total_yaw; //记录偏航角变化
    bool flag; // 判断是否第一次进入回调
    message_filters::Subscriber<sensor_msgs::Imu>* accel_sub;
    message_filters::Subscriber<sensor_msgs::Imu>* gyro_sub;
    Synchronizer<syncPolicy>* sync;
    ros::Publisher IMU_pub;
    imu3d total_angular;
    imu3d Angle;


    imu3d calculate_accel(const imu6d data) //计算加速度计算出的翻滚角roll和俯仰角pitch，由于重力方向垂直向下，所以初始的偏航角无法计算也不重要
    {
        imu3d angular;
        angular.x = atan2(data.accel_x, data.accel_y);//翻滚角
        angular.y = atan2(data.accel_z, sqrt(data.accel_x*data.accel_x + data.accel_y*data.accel_y));//俯仰角
        angular.z = total_yaw; //偏航角不能靠重力加速度求得

        cout<< "Accel Roll: " <<angular.x * (180/3.14159)<<" \t Accel Pitch: "<<angular.y * (180/3.14159)<<"\t Accel Yaw: "<<angular.z * (180/3.14159)<<endl;
        cout<< "Accel X: " <<data.accel_x <<"\t Accel Y: "<<data.accel_y<<"\t Accel Z: "<<data.accel_z <<endl;
        return angular;
    }

    imu3d calculate_gyro(const imu6d data, const double duration_time)//初始化角度为0,在整个运动过程中计算累计角度作为运动角度
    {
        imu3d delta_angular;
        cout<<duration_time<<endl;
        //相机坐标系中，相机的朝前是z轴，朝左是x轴，朝下是x轴
        delta_angular.x = data.gyro_x * duration_time;
        delta_angular.y = data.gyro_y * duration_time;
        delta_angular.z = data.gyro_z * duration_time;
            
        total_angular.x += delta_angular.x; // 绕x轴转动，俯仰角pitch
        total_angular.y += delta_angular.y; // 绕y轴转动，偏航角yaw
        total_angular.z += delta_angular.z; // 绕z轴转动，翻转角roll

        cout<< "Gyro Roll: " <<total_angular.z * (180/3.14159)<<" \t Gyro Pitch: "<<total_angular.x * (180/3.14159)<<"\t Gyro Yaw: "<<total_angular.y * (180/3.14159)<<endl;

        imu3d delta_angle; //把陀螺仪对roll、pitch、yaw的影响转换到加速度计roll、pitch、yaw所对应的相应的位置
        delta_angle.x = -delta_angular.z;
        delta_angle.y = delta_angular.x;
        delta_angle.z = delta_angular.y;

        return delta_angle;
    }

    imu3d complementaryFilter(const imu6d data, const double duration_time) //补偿滤波计算位姿
    {
        Angle = (Angle + calculate_gyro(data, duration_time)) *0.95 + calculate_accel(data)*0.05;
        cout<< "Filter Roll: " <<Angle.x * (180/3.14159)<<" \t Filter Pitch: "<<Angle.y * (180/3.14159)<<"\t Filter Yaw: "<<Angle.z * (180/3.14159)<<endl;
        total_yaw = Angle.z;
        return Angle;
    }

    
    void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg_accel, const sensor_msgs::ImuConstPtr &imu_msg_gyro)
        {
            cout<<(imu_msg_accel->header.stamp.toSec() == imu_msg_gyro->header.stamp.toSec())<<endl;
            double t = imu_msg_accel->header.stamp.toSec();
            
            imu6d imu_data;
            imu_data.accel_x = imu_msg_accel->linear_acceleration.x;
            imu_data.accel_y = -imu_msg_accel->linear_acceleration.y;
            imu_data.accel_z = imu_msg_accel->linear_acceleration.z;

            imu_data.gyro_x = imu_msg_gyro->angular_velocity.x;
            imu_data.gyro_y = imu_msg_gyro->angular_velocity.y;
            imu_data.gyro_z = imu_msg_gyro->angular_velocity.z;

            if (flag == 1){
                time_s = t;
                flag = 0;
                Angle = calculate_accel(imu_data);
                cout <<"first angle: "<<Angle<<endl;
                return;
            }

            double duration_time = t-time_s;
            time_s = t;

            complementaryFilter(imu_data, duration_time);
            
            //把加速度计信息和里程计信息的消息弄一起发布
            sensor_msgs::Imu imu_data_pub;
            imu_data_pub.header.stamp = imu_msg_accel->header.stamp;
            imu_data_pub.header.frame_id = imu_msg_accel->header.frame_id;
 
            //线加速度
            imu_data_pub.linear_acceleration.x = imu_msg_accel->linear_acceleration.x;
            imu_data_pub.linear_acceleration.y = imu_msg_accel->linear_acceleration.y;
            imu_data_pub.linear_acceleration.z = imu_msg_accel->linear_acceleration.z;
            //角速度
            imu_data_pub.angular_velocity.x = imu_msg_gyro->angular_velocity.x;
            imu_data_pub.angular_velocity.y = imu_msg_gyro->angular_velocity.y;
            imu_data_pub.angular_velocity.z = imu_msg_gyro->angular_velocity.z;
            IMU_pub.publish(imu_data_pub);

        }

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "imuTester");
    imuTester imut;
    ros::spin();
    return 0;
}