【山东大学软件学院 21 级项目实训】IMU的数据处理和滤波以及姿态估计

首先是导入相应的port_imu_i2c.h文件

#include "port_imu_i2c.h"

• 宏定义和全局变量

​
#define IMU_THRESHOLD   1
#define IMU_THRESHOLD_CAL   2
#define IMU_MAX_ERROR 6
#define FILTER_SIZE 60
#define MPU_GYRO_CFG_REG            0X1B    //陀螺仪配置寄存器
​
static float error_offset_start = 0;
static int imu_port_id=0;
static int imu_init_state[10]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
struct Vector3f gypo_data;
static float error_tmp;

• rad_limit ()将弧度限制在 −𝜋 到 𝜋 之间

float rad_limit(float rad)
{
    while(1){
        if (rad < 0-PI)
            rad = 2*PI + rad;
        else if (rad >= PI)
            rad = rad - 2*PI;
        else
            return rad;
    }
}

• KalmanFilter() 卡尔曼滤波器函数，用于滤波处理传感器数据

float KalmanFilter(float inData)
{
    static float kalman = 0; //上次卡尔曼值(估计出的最优值)
    static float p = 10;
    float q = 0.001; //q:过程噪声
    float r = 0.001; //r:测量噪声
    float kg = 0; //kg:卡尔曼增益
 
    p += q;
    kg = p / ( p + r ); //计算卡尔曼增益
    kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值
    p = (1 - kg) * p; //更新测量方差
    
    // return kalman; //返回估计值
    return inData;
​
}

• 初始化的三维向量

​
void InitVector3f(struct Vector3f *v)
{
    v->x=0.000000001f;
    v->y=0.000000001f;
    v->z=0.0000000001f;
}

• 队列操作（初始化，判空，插入，判满等）

void initQueue(QueueF* queue)
{
    queue->front = 0;
    queue->rear = 0;
}
​
​
// 判空：front == rear
bool isEmpty(const QueueF* queue)
{
    if (queue->front == queue->rear)
        return true;
    else
        return false;
}
​
​
// 判满：(rear+1) % MAXSIZE == front
bool isFull(const QueueF* queue)
{
    if (queue->rear == (queue->front+1) % MAXSIZE)
        return true;
    else
        return false;
}
​
// 进队列
void addElem(QueueF* queue, float val)
{
    queue->data[queue->front] = val;
    queue->front = (queue->front + 1) % MAXSIZE;
}
​
// 出队列
// 获取队列长度：(rear - front) % MAXSIZE
int getLen(const QueueF* queue)
{
    return (queue->front - queue->rear + MAXSIZE) % MAXSIZE;
}

• 构建Imu状态结构体 ImuStateZ 记录IMU状态信息

• 导入 ImuData 数据结构体（角度，加速度，角速度等）

typedef struct _ImuStateZ
{
    uint8_t state;
    uint8_t count;
    int filter_count;
    float error_offset;
    QueueF data_z;
}ImuStateZ;
​
ImuStateZ imu_state_z;
​
//导入 ImuData 数据结构体（角度，加速度，角速度等）
ImuData imu_data;
​
//ACC传感器测量得到的加速度
struct  Vector3f acc_data;
//校准后的姿态角数据
struct Vector3f euler_angle;
​
​
float invSqrt(float x)  /*快速开平方求倒*/
{
    return 1.0f/sqrt(x);
}
​
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 最优估计四元数
//互补滤波使用变量
static float rMat[3][3]={   {0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f},{0.0f,0.0f,0.0f}  };/*旋转矩阵*/
static float exInt = 0.0f;
static float eyInt = 0.0f;
static float ezInt = 0.0f;      /*积分误差累计*/
/*计算旋转矩阵*/
void imuComputeRotationMatrix(void)
{
    float q1q1 = q1 * q1;
    float q2q2 = q2 * q2;
    float q3q3 = q3 * q3;
​
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q3 = q2 * q3;
​
    rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    rMat[0][1] = 2.0f * (q1q2 + -q0q3);
    rMat[0][2] = 2.0f * (q1q3 - -q0q2);
​
    rMat[1][0] = 2.0f * (q1q2 - -q0q3);
    rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    rMat[1][2] = 2.0f * (q2q3 + -q0q1);
​
    rMat[2][0] = 2.0f * (q1q3 + -q0q2);
    rMat[2][1] = 2.0f * (q2q3 - -q0q1);
    rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
​
//角度弧度转换系数
static const float RAD_TO_DEG =  180.0f / PI;
static const float DEG_TO_RAD =  PI / 180.0f;
​
// 这里的Kp,Ki是用于控制加速度计修正陀螺仪积分姿态的速度
static float Kp= 1.33f;//0.8f//10.0f  // 2.0f
static float Ki= 0.051f;//0.008f // 0.002f
​
void imuUpdate(struct Vector3f  acc, struct Vector3f  gyro, struct Vector3f *state , float dt)  /*数据融合 互补滤波*/
{
    float normalise;
    float ex, ey, ez;
    float halfT = 0.5f * dt;
    gyro.x = gyro.x * DEG_TO_RAD;   /* 度转弧度 */
    gyro.y = gyro.y * DEG_TO_RAD;
    gyro.z = gyro.z * DEG_TO_RAD;
​
    /* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
    if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
    {
        /*单位化加速计测量值*/
        normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
        acc.x *= normalise;
        acc.y *= normalise;
        acc.z *= normalise;
        
​
        /*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
        ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
        ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
        ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
        
        /*误差累计，与积分常数相乘*/
        exInt += Ki * ex * dt ;  
        eyInt += Ki * ey * dt ;
        ezInt += Ki * ez * dt ;
        
        /*用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量*/
        gyro.x += Kp * ex + exInt;
        gyro.y += Kp * ey + eyInt;
        gyro.z += Kp * ez + ezInt;
    }
    /* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
    float q0Last = q0;
    float q1Last = q1;
    float q2Last = q2;
    float q3Last = q3;
    q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
    q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
    q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
    q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
    
    /*单位化四元数*/
    normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= normalise;
    q1 *= normalise;
    q2 *= normalise;
    q3 *= normalise;
    
    imuComputeRotationMatrix(); /*计算旋转矩阵*/
    
    /*计算roll pitch yaw 欧拉角*/
    state->y/*pitch*/ = -asinf(rMat[2][0]) * RAD_TO_DEG; 
    state->x/*roll*/ = atan2f(rMat[2][1], rMat[2][2]) * RAD_TO_DEG;
    state->z/*yaw*/ = atan2f(rMat[1][0], rMat[0][0]) * RAD_TO_DEG;
​
}
​
// 插入排序
void insertionSort(float *arr, int size)
{
    for (int i = 1; i < size; i++)
    {
        int j = i - 1;
        float key = arr[i];
        /* Move all elements greater than key to one position */
        while (j >= 0 && key < arr[j])
        {
            arr[j + 1] = arr[j];
            j = j - 1;
        }
        /* Find a correct position for key */
        arr[j + 1] = key;
    }
}
​
//获取计算IMU在z轴方向的误差
void get_z_error(ImuStateZ state_z, float *error)
{
    
    if(state_z.filter_count > MAXSIZE){
        int start  = (state_z.data_z.front+(MAXSIZE - FILTER_SIZE)/2)% MAXSIZE;
        float errors[MAXSIZE];
        for(int i=0;i<FILTER_SIZE;i++){
            errors[i] = state_z.data_z.data[(start+i)%MAXSIZE];
            // error_t = error_t + state_z.data_z.data[(start+i)%MAXSIZE];
​
        }
        insertionSort(errors, FILTER_SIZE);
        float error_t = 0.0;
        for(int i=10;i<FILTER_SIZE-10;i++){
            error_t = error_t + errors[i];
        }
        *error = error_t/(FILTER_SIZE-20);
    }
}
​
static float last_seconds = 0.0;
static uint32_t ImuTime5ms = 0;
static uint32_t ImuTime10ms = 0;
static float error_offset = 0.0;
static void time_imu(void){
    ImuTime5ms++;
    ImuTime10ms++;
    if(ImuTime5ms >= 5){
        ImuTime5ms = 0;
        error_tmp = myDMP.gryoz - error_offset;
        if(fabs(myDMP.gryoz - error_offset) < IMU_THRESHOLD){
            addElem(&imu_state_z.data_z, myDMP.gryoz);
            imu_state_z.filter_count++;
            get_z_error(imu_state_z, &error_offset);
        }
        else{
            imu_state_z.filter_count=0;
        }
        
    }
    if(ImuTime10ms >= 10){
​
        ImuTime10ms = 0;
        acc_data.x=myDMP.accx;
        acc_data.y=myDMP.accy;
        acc_data.z=myDMP.accz;
        //陀螺传感器得到的角速度
        
        gypo_data.x=myDMP.gryox;
        gypo_data.y=myDMP.gryoy;
        gypo_data.z=myDMP.gryoz;
        imuUpdate(acc_data, gypo_data, &euler_angle , 0.01);    /*数据融合 互补滤波*/
​
        gypo_data.z=myDMP.gryoz - error_offset;
        // gypo_data.z=gro_z_filter - error_offset;
        if(fabs(gypo_data.z) < IMU_THRESHOLD_CAL){
            gypo_data.z = 0;
        }
        // euler_angle.z = euler_angle.z + gypo_data.z*0.01;
        float deta_t = seconds() - last_seconds;
        imu_data.angle_z = imu_data.angle_z + gypo_data.z*DEG_TO_RAD*deta_t;
        last_seconds = seconds();
    }
}
​
void get_imu_data(){
    while(1){
        gyroscope(imu_port_id,DMPCH_GYPO_Z);
        wait(0.01);
    }
    
}
​
// 设置MPU6050寄存器
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data){
    i2c_write(imu_port_id,DMP_IIC_Addr, OPTADDWIDTH_U8, reg, 1, &data);
    return 0;
}
​
uint8_t MPU_Read_Byte(uint8_t reg)
{
    uint8_t res;
    i2c_read(imu_port_id,DMP_IIC_Addr, OPTADDWIDTH_U8, reg, 1, &res);
    return res;     
}
​
// 初始化IMU校准角度，进行误差校准。
void imu_adjust_angle_init(int port_id, int couters){
    imu_port_id = port_id;
​
    SetI2C_Clock(100);//提速读取陀螺
    if(couters>200){
        couters = 200;  
    }
    else if(couters<40){
        couters = 40;
    }
    
    gyroscope(imu_port_id,DMPCH_GYPO_Z);
    wait(0.05);
    float error_result  = 0.0;
    float error_results[200];
    float error_tmp  = 0.0;
    int i = 0;
    
    for(int j=0;j<10;j++){
        gyroscope(imu_port_id,DMPCH_GYPO_Z);
        wait(0.005);
    }
    while (true)
    {
        error_tmp = gyroscope(imu_port_id,DMPCH_GYPO_Z);
        if(fabs(error_tmp)< IMU_MAX_ERROR){
            if(i == 0){
                
                error_results[i] = error_tmp;
                error_result = error_tmp;
                i=1;
            }
            else if(fabs(error_tmp - error_result) < IMU_THRESHOLD){
                // error_result = (error_result + error_tmp) / 2;
                error_results[i] = error_tmp;
                error_result = (error_result+error_tmp)/2;
                i++;
            }
            else{
                i = 0;
            }
            
        }
        if(i>couters){
            break;
        }
        wait(0.001);
    }
​
    insertionSort(error_results, couters);
    float error_t = 0.0;
    int start = (int)(couters / 6);
    for(int j=start;j<(couters-start);j++){
        error_t = error_t + error_results[j];
    }
​
    error_offset = error_t/(couters-start*2);
    error_offset_start = error_offset;
    wait(0.5);
    ThreadStart(get_imu_data);
    setTimerHandle(time_imu, 1);// 1ms定时器
​
}
​
// 重置IMU数据
void imu_reset()
{
    error_offset = error_offset_start;
    imu_data.angle_z = 0;
}
// 获取IMU姿态角数据。
float imu_get_atitude(uint8_t port_id, ImuData * imu_data_tar)
{
    if(imu_init_state[port_id]==0){
        imu_adjust_angle_init(port_id, 50);
        imu_init_state[port_id]=1;
    }
    if(port_id == imu_port_id){
        imu_data_tar->angle_z = rad_limit(imu_data.angle_z);
        imu_data_tar->angle_x = euler_angle.x*DEG_TO_RAD;
        imu_data_tar->angle_y = euler_angle.y*DEG_TO_RAD;
        imu_data_tar->gra_acc_x = myDMP.accx*9.8;
        imu_data_tar->gra_acc_y = myDMP.accy*9.8;
        imu_data_tar->gra_acc_z = myDMP.accz*9.8;
        imu_data_tar->w_x = gypo_data.x*DEG_TO_RAD;
        imu_data_tar->w_y = gypo_data.y*DEG_TO_RAD;
        imu_data_tar->w_z = gypo_data.z*DEG_TO_RAD;
        return error_offset;
    }
    else{
        memset(imu_data_tar, 0, sizeof(ImuData));
        return 0;
    }
}
// 获取IMU角度信息
float imu_get_angle(uint8_t port_id)
{
    if(imu_init_state[port_id]==0){
        imu_adjust_angle_init(port_id, 50);
        imu_init_state[port_id]=1;
    }
    return rad_limit(imu_data.angle_z)*RAD_TO_DEG;
}
​
// 获取IMU误差信息。
float imu_get_error(uint8_t port_id)
{
    return error_tmp;
}
​