姿态解算

Imu.h

/* ---------------------------------------------------------------------
 * 
 * Imu库函数:
 * 姿态解算操作函数.
 *
 *   修订操作      版本号       日期         修订人
 * ------------    ------    ----------    ------------
 *     创建        1.0       2018.01.19      Universea
 *
 * ---------------------------------------------------------------------*/
#ifndef __IMU_H
#define __IMU_H
#include "stm32f4xx_hal.h"
#include "BasicData.h"
//IMU数据结构体
typedef struct 
{
    //四元数
    float q0;
    float q1;
    float q2;
    float q3;
    float XaxisVector[VECTOR_XYZ];
    //载体坐标下的x方向向量
    float YaxisVector[VECTOR_XYZ];
    //载体坐标下的y方向向量
    float ZaxisVector[VECTOR_XYZ];
    //载体坐标下的z方向向量,重力向量
    float HorizontalXaxisVector[VECTOR_XYZ];
    ////水平面方向向量
    float AirframeAccel[VECTOR_XYZ];
    //机体加速度
    float WorldAccel[VECTOR_XYZ];
    //世界加速度
    float AccelHorizontal[VECTOR_XYZ];
    //水平加速度
    float MagHorizontal[VECTOR_XYZ];
    //磁力计水平
    float gacc_deadzone[VECTOR_XYZ];
    float ObserverAccelWorld[VECTOR_XYZ];
    //世界加速度观测
    float ObserverAccelAirframe[VECTOR_XYZ];
    //机体加速度观测
    float GravityAccel[VECTOR_XYZ];
    //重力坐标系
    float EstimatorAccelAirframe[VECTOR_XYZ];
    //机体加速度估测
    float EstimatorAcceHorizontal[VECTOR_XYZ];
    //水平加速度估测
    float EstimatorAccelWorld[VECTOR_XYZ];
    //世界加速度估测
    float EstimatorSpeedHorizontal[VECTOR_XYZ];
    //水平速度估测
    float EstimatorSpeedWorld[VECTOR_XYZ];
    //世界速度估测
    float ROLL;
    //横滚角
    float PITCH;
    //俯仰角
    float YAW;
    //偏航角
}
imuStruct;
extern imuStruct imuData;
//IMU 状态结构体
typedef struct 
{
    float GyroKp;
    //陀螺仪比例系数
    float GyroKi;
    //陀螺仪积分系数
    float MagKp;
    //磁力计比例系数
    uint8_t GravityReset;
    //重力复位
    uint8_t MagReset;
    //磁力计复位
    uint8_t GravityFixEnable;
    //重力修正使能
    uint8_t MagFixEnable;
    //磁力计修正使能
    uint8_t ObserverEnable;
    //观测器使能
}
imuStateStruct;
extern imuStateStruct imuState;
void imuUpdate(float dT,imuStateStruct *,float gyro[VECTOR_XYZ],int32_t accel[VECTOR_XYZ],int16_t magValue[VECTOR_XYZ],imuStruct *imu);
//imu更新
void calculateAttitudeAngle(void);
//计算角度
void w2h_2d_trans(float w[VECTOR_XYZ],float ref_ax[VECTOR_XYZ],float h[VECTOR_XYZ]);
//世界转水平
void h2w_2d_trans(float h[VECTOR_XYZ],float ref_ax[VECTOR_XYZ],float w[VECTOR_XYZ]);
//水平转世界
#endif

Imu.c

/* ---------------------------------------------------------------------
 * 
 * Imu库函数:
 * 姿态解算操作函数.
 *
 *   修订操作      版本号       日期         修订人
 * ------------    ------    ----------    ------------
 *     创建        1.0       2018.01.19      Universea
 *
 * ---------------------------------------------------------------------*/
#include "MyMath.h"
#include "Imu.h"
#include "MyFilter.h"
#include <math.h>
#include "InvSensor.h"
/*参考坐标，定义为SKY坐标*
 *
 * 俯视，机头方向为x正方向 
 *       +x
         \
    +y --\--
         \
 */
/* 转换 */
void w2h_2d_trans( float w[VECTOR_XYZ], float ref_ax[VECTOR_XYZ], float h[VECTOR_XYZ] ) 
{
    h[X]    = w[X] * ref_ax[X] + w[Y] * ref_ax[Y];
    h[Y]    = w[X] * (-ref_ax[Y]) + w[Y] * ref_ax[X];
}
void h2w_2d_trans( float h[VECTOR_XYZ], float ref_ax[VECTOR_XYZ], float w[VECTOR_XYZ] ) 
{
    w[X]    = h[X] * ref_ax[X] + h[Y] * (-ref_ax[Y]);
    w[Y]    = h[X] * ref_ax[Y] + h[Y] * ref_ax[X];
}
float mag_yaw_calculate( float dT, float magValue[VECTOR_XYZ], float g_ZaxisVector[VECTOR_XYZ], float MagHorizontal_val[VECTOR_XYZ] )
/*  */ 
{
    vec_3dh_transition( g_ZaxisVector, magValue, MagHorizontal_val );
    return(fast_atan2( MagHorizontal_val[Y], MagHorizontal_val[X] ) * 57.3f);
}
#define USE_MAG
#define USE_LENGTH_LIM
/* 初始化 */
imuStruct        imuData = { 1, 0, 0, 0,
                 { 0,    0, 0 },
                 { 0,    0, 0 },
                 { 0,    0, 0 },
                 { 0,    0, 0 },
                 { 0,    0, 0 },
                 { 0,    0, 0 },
                 0,        0, 0 };
static float    vectorError[VECTOR_XYZ];
static float    vectorERR_I[VECTOR_XYZ];
static float    q0q1, q0q2, q1q1, q1q3, q2q2, q2q3, q3q3, q1q2, q0q3;
/* q0q0, */
static float    magYawError, magValuefloat[VECTOR_XYZ];
static float    imuResetValue;
static uint16_t resetCount;
imuStateStruct    imuState = { 1, 1, 1, 1, 1, 1, 1, 1 };
float    airframeMatrix[3][3], airframeTemp;
float imu_test[3];
void imuUpdate( float dT, imuStateStruct *state, float gyro[VECTOR_XYZ], int32_t accel[VECTOR_XYZ], int16_t magValue[VECTOR_XYZ], imuStruct *imu ) 
{
    static float    kpUsing = 0, kiUsing = 0, maGyroKpUsing = 0;
    //修正系数
    float        accelNormalise, accelNormaliseReciprocal,quaternionNormalise;
    //单位化
    float        accelNorm[VECTOR_XYZ];
    float        gyroCompensate[VECTOR_XYZ];
    /* 四元数组合 */
    /*        q0q0 = imu->q0 * imu->q0; */
    q0q1    = imu->q0 * imu->q1;
    q0q2    = imu->q0 * imu->q2;
    q1q1    = imu->q1 * imu->q1;
    q1q3    = imu->q1 * imu->q3;
    q2q2    = imu->q2 * imu->q2;
    q2q3    = imu->q2 * imu->q3;
    q3q3    = imu->q3 * imu->q3;
    q1q2    = imu->q1 * imu->q2;
    q0q3    = imu->q0 * imu->q3;
    if ( state->ObserverEnable ) 
    /* 观测器使能 */ 
    {
        /* 计算机体坐标下的运动加速度观测量。坐标系为北西天 */
        for ( uint8_t i = 0; i < 3; i++ ) 
        {
            int32_t temp = 0;
            for ( uint8_t j = 0; j < 3; j++ ) 
            {
                temp += imu->ObserverAccelWorld[j] * airframeMatrix[j][i];
                /* airframeMatrix[i][j] 转置为 airframeMatrix[j][i] */
            }
            imu->ObserverAccelAirframe[i]    = temp;
            imu->GravityAccel[i]        = accel[i] - imu->ObserverAccelAirframe[i];
        }
    } else 
    {
        /* 观测器失能 */
        for ( uint8_t i = 0; i < 3; i++ ) 
        {
            imu->GravityAccel[i] = accel[i];
            /* 重力坐标系 */
        }
    }
    /*单位化加速计测量值*/
    accelNormaliseReciprocal = my_sqrt_reciprocal( my_pow( imu->GravityAccel[X] ) + my_pow( imu->GravityAccel[Y] ) + my_pow( imu->GravityAccel[Z] ) );
    /* 反倒数 */
    accelNormalise = safeDivision(1,accelNormaliseReciprocal, 0 );
    /* 限幅0-1 */
    /* 加速度计的读数，单位化。 */
    for ( uint8_t i = 0; i < 3; i++ ) 
    {
        accelNorm[i] = imu->GravityAccel[i] * accelNormaliseReciprocal;
    }
    /* 载体坐标下的x方向向量，单位化。 */
    airframeMatrix[0][0] = imu->XaxisVector[X] = 1 - (2 * q2q2 + 2 * q3q3);
    airframeMatrix[0][1] = imu->XaxisVector[Y] = 2 * q1q2 - 2 * q0q3;
    airframeMatrix[0][2] = imu->XaxisVector[Z] = 2 * q1q3 + 2 * q0q2;
    /* 载体坐标下的y方向向量，单位化。 */
    airframeMatrix[1][0] = imu->YaxisVector[X] = 2 * q1q2 + 2 * q0q3;
    airframeMatrix[1][1] = imu->YaxisVector[Y] = 1 - (2 * q1q1 + 2 * q3q3);
    airframeMatrix[1][2] = imu->YaxisVector[Z] = 2 * q2q3 - 2 * q0q1;
    /* 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。 */
    airframeMatrix[2][0] = imu->ZaxisVector[X] = 2 * q1q3 - 2 * q0q2;
    airframeMatrix[2][1] = imu->ZaxisVector[Y] = 2 * q2q3 + 2 * q0q1;
    airframeMatrix[2][2] = imu->ZaxisVector[Z] = 1 - (2 * q1q1 + 2 * q2q2);
    /* 水平面方向向量 */
    imu->HorizontalXaxisVector[X]    = airframeMatrix[0][0];
    imu->HorizontalXaxisVector[Y]    = airframeMatrix[1][0];
    /* 计算载体坐标下的运动加速度。(与姿态解算无关) */
    for ( uint8_t i = 0; i < 3; i++ ) 
    {
        imu->AirframeAccel[i] = (int32_t) (accel[i] - 981 * imu->ZaxisVector[i]);
    }
    /* 计算世界坐标下的运动加速度。坐标系为北西天 */
    for ( uint8_t i = 0; i < 3; i++ ) 
    {
        int32_t temp = 0;
        for ( uint8_t j = 0; j < 3; j++ ) 
        {
            temp += imu->AirframeAccel[j] * airframeMatrix[i][j];
        }
        imu->WorldAccel[i] = temp;
    }
    w2h_2d_trans( imu->WorldAccel, imuData.HorizontalXaxisVector, imu->AccelHorizontal );
    /* 测量值与等效重力向量的叉积（计算向量误差）。 */
    /* 将地理坐标系转换到机体坐标系下的重力向量与机体坐标系测量的向量外积（叉积），得到的就是两坐标系的误差 */
    vectorError[X]    =  (accelNorm[Y] * imu->ZaxisVector[Z] - imu->ZaxisVector[Y] * accelNorm[Z]);
    vectorError[Y]    = -(accelNorm[X] * imu->ZaxisVector[Z] - imu->ZaxisVector[X] * accelNorm[Z]);
    vectorError[Z]    = -(accelNorm[Y] * imu->ZaxisVector[X] - imu->ZaxisVector[Y] * accelNorm[X]);
    #ifdef USE_MAG
    /* 计算航向yaw误差 */
    for ( uint8_t i = 0; i < 3; i++ ) 
    {
        magValuefloat[i] = (float) magValue[i];
    }
    if ( !(magValue[X] == 0 && magValue[Y] == 0 && magValue[Z] == 0) ) 
    {
        magYawError    = mag_yaw_calculate( dT, magValuefloat, (imu->ZaxisVector), (imu->MagHorizontal) ) - imu->YAW;
        magYawError    = range_to_180deg( magYawError );
    }
    #endif
        for ( uint8_t i = 0; i < 3; i++ ) 
    {
        #ifdef USE_EST_DEADZONE
                if ( state->GravityReset == 0 && state->ObserverEnable == 0 ) 
        {
            vectorError[i] = my_deadzone( vectorError[i], 0, imu->gacc_deadzone[i] );
        }
        #endif
                        #ifdef USE_LENGTH_LIM
                if ( accelNormalise > 1060 || accelNormalise < 900 ) 
        {
            vectorError[X] = vectorError[Y] = vectorError[Z] = 0;
        }
        #endif
        /* 误差积分 */
        vectorERR_I[i] += LIMIT( vectorError[i], -0.01f, 0.01f ) * dT * kiUsing;
        /*构造增量旋转（含融合纠正）。
                因为陀螺仪会有误差，且四元数更新姿态是用陀螺仪来跟新的，所以陀螺仪的
                误差是导致机体坐标系产生误差的根本原因，这里用两坐标系误差的 PI 来补偿陀螺仪，
                使得到的机体坐标系更加准确。*/
        #ifdef USE_MAG
                gyroCompensate[i] = (gyro[i] + (vectorError[i] + vectorERR_I[i]) * kpUsing - magYawError * imu->ZaxisVector[i] * maGyroKpUsing * RAD_PER_DEG) * dT / 2;
        # else
                gyroCompensate[i] = (gyro[i] + (vectorError[i] + vectorERR_I[i]) * kpUsing) * dT / 2;
        #endif
    }
    /* 四元数姿态更新 */
    imu->q0        = imu->q0 - imu->q1 * gyroCompensate[X] - imu->q2 * gyroCompensate[Y] - imu->q3 * gyroCompensate[Z];
    imu->q1        = imu->q0 * gyroCompensate[X] + imu->q1 + imu->q2 * gyroCompensate[Z] - imu->q3 * gyroCompensate[Y];
    imu->q2        = imu->q0 * gyroCompensate[Y] - imu->q1 * gyroCompensate[Z] + imu->q2 + imu->q3 * gyroCompensate[X];
    imu->q3        = imu->q0 * gyroCompensate[Z] + imu->q1 * gyroCompensate[Y] - imu->q2 * gyroCompensate[X] + imu->q3;
    //由于误差的引入使得四元数的模不再等于 1，四元数失去了规范性，因此在利用更新后的四元数计算欧拉角时，必须对四元数进行规范化处理
    quaternionNormalise    = my_sqrt_reciprocal( imu->q0 * imu->q0 + imu->q1 * imu->q1 + imu->q2 * imu->q2 + imu->q3 * imu->q3 );
    imu->q0        *= quaternionNormalise;
    imu->q1        *= quaternionNormalise;
    imu->q2        *= quaternionNormalise;
    imu->q3        *= quaternionNormalise;
    /* ///////////////////修正开关/////////////////////////// */
    #ifdef USE_MAG
        if ( state->MagFixEnable == 0 )
    /* 磁力 */ 
    {
        maGyroKpUsing = 0;
        /*不修正 */
        state->MagReset = 0;
        /* 罗盘修正不复位，清除复位标记 */
    } else 
    {
        if ( state->MagReset )
        /*  */ 
        {
            /* 通过增量进行对准 */
            maGyroKpUsing = 5.0f;
            if ( magYawError != 0 && ABS( magYawError ) < 2 ) 
            {
                state->MagReset = 0;
                /* 误差小于2的时候，清除复位标记 */
            }
        } else 
        {
            maGyroKpUsing = state->MagKp;
            /* 正常修正 */
        }
    }
    #endif
        if ( state->GravityFixEnable == 0 )
    /* 重力方向修正 */ 
    {
        kpUsing = 0;
        /*不修正 */
    } else 
    {
        if ( state->GravityReset == 0 )
        /* 正常修正 */ 
        {
            kpUsing    = state->GyroKp;
            kiUsing    = state->GyroKi;
        } else 
        {
            /* 快速修正，通过增量进行对准 */
            kpUsing    = 5.0f;
            kiUsing    = 0.5f;
            /*
             *                      imu->EstimatorSpeedWorld[X] = imu->EstimatorSpeedWorld[Y] = 0;
             *                      imu->EstimatorAccelWorld[X] = imu->EstimatorAccelWorld[Y] = 0;
             *                      imu->EstimatorAcceHorizontal[X] = imu->EstimatorAcceHorizontal[Y] =0;
             */
            /* 计算静态误差是否缩小 */
            imuResetValue    += (ABS( vectorError[X] ) + ABS( vectorError[Y] ) ) * 1000 * dT;
            imuResetValue    -= 0.01f;
            imuResetValue    = LIMIT( imuResetValue, 0, 1.0f );
            if ( (imuResetValue < 0.01f) && (state->MagReset == 0) ) 
            {
                /* 计时 */
                resetCount += 2;
                if ( resetCount > 500 ) 
                {
                    resetCount    = 0;
                    state->GravityReset    = 0;
                    /* 已经对准，清除复位标记 */
                }
            } else 
            {
                resetCount = 0;
            }
        }
    }
}
void calculateAttitudeAngle() 
{
    /* /////////////////////输出姿态角/////////////////////////////// */
    airframeTemp = LIMIT( 1 - my_pow( airframeMatrix[2][0] ), 0, 1 );
    /* imuData.PITCH = asin(2*q1q3 - 2*q0q2)*57.30f; */
    if ( ABS( imuData.ZaxisVector[Z] ) > 0.05f )
    /* 避免奇点的运算 */ 
    {
        imuData.PITCH    = fast_atan2( airframeMatrix[2][0], my_sqrt( airframeTemp ) ) * 57.30f;
        imuData.ROLL    = fast_atan2( airframeMatrix[2][1], airframeMatrix[2][2] ) * 57.30f;
        imuData.YAW    = -fast_atan2( airframeMatrix[1][0], airframeMatrix[0][0] ) * 57.30f;
    }
}