一、ANO_Imu.c文件
/******************** © COPYRIGHT 2016 ANO Tech ***************************
- 作者 ：匿名科创
- 文件名 ：ANO_IMU.c
- 描述 ：姿态解算函数
- 官网 ：www.anotc.com
- 淘宝 ：anotc.taobao.com
- 技术Q群 ：190169595
*****************************************************************************/
#include “Ano_Imu.h”
#include “Ano_Math.h”
#include “Ano_Filter.h”
#include “Ano_DT.h”
//#include “ANO_RC.h”
//世界坐标平面XY转平面航向坐标XY
void w2h_2d_trans(float w[VEC_XYZ],float ref_ax[VEC_XYZ],float h[VEC_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];
}
//平面航向坐标XY转世界坐标平面XY
void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_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];
}
//载体坐标 转 世界坐标（ANO约定等同与地理坐标）
float att_matrix[3][3]; //必须由姿态解算算出该矩阵
void a2w_3d_trans(float a[VEC_XYZ],float w[VEC_XYZ])
{
for(u8 i = 0;i<3;i++)
{
float temp = 0;
for(u8 j = 0;j<3;j++)
{
temp += a[j] *att_matrix[i][j];
}
w[i] = temp;
}
}
//float mag_yaw_calculate(float dT,float mag_val[VEC_XYZ],float g_z_vec[VEC_XYZ],float h_mag_val[VEC_XYZ])//
//{
// vec_3dh_transition(g_z_vec, mag_val, h_mag_val);
// return (fast_atan2(h_mag_val[Y], h_mag_val[X]) *57.3f) ;//
//}
#define USE_MAG
#define USE_LENGTH_LIM
static float vec_err[VEC_XYZ];
static float vec_err_i[VEC_XYZ];
static float q0q1,q0q2,q1q1,q1q3,q2q2,q2q3,q3q3,q1q2,q0q3;//q0q0,
static float mag_yaw_err,mag_err_dot_prudoct,mag_val_f[VEC_XYZ];
static float imu_reset_val;
static u16 reset_cnt;
_imu_st imu_data = {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};
_imu_state_st imu_state = {1,1,1,1,1,1,1,1};//状态结构体：陀螺仪的p、i,磁力计的p obs_en等等
static float mag_2d_w_vec[2][2] = {{1,0},{1,0}};//地理坐标中，水平面磁场方向恒为南北 (1,0) mag_2d_w_vec[0][0]={1,0} mag_2d_w_vec[0][1]={1,0}
float imu_test[3];
//在文件Ano_FlightDataCal.c被调用
//惯性测量单元数据更新(惯性测量单元：陀螺仪、加速度计、磁力计)
//dT:采样周期 *state:imu(惯性测量单元)状态结构体指针(实际传入的是&imu_state)
//gyr[VEC_XYZ]：陀螺仪xyz方向的数据 acc[VEC_XYZ]：加速度计xyz方向的数据
//mag_val[VEC_XYZ]：磁力计xyz方向的数据 *imu:imu结构体指针(实际传入的是&imu_data)
//姿态更新
void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], s32 acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)
{
// const float kp = 0.2f,ki = 0.001f;
// const float kmp = 0.1f;
static float kp_use = 0,ki_use = 0,mkp_use = 0;
float acc_norm_l,acc_norm_l_recip,q_norm_l;
float acc_norm[VEC_XYZ];
float d_angle[VEC_XYZ];
// q0q0 = imu->w * imu->w;
q0q1 = imu->w * imu->x;
q0q2 = imu->w * imu->y;
q1q1 = imu->x * imu->x;
q1q3 = imu->x * imu->z;
q2q2 = imu->y * imu->y;
q2q3 = imu->y * imu->z;
q3q3 = imu->z * imu->z;
q1q2 = imu->x * imu->y;
q0q3 = imu->w * imu->z;
//obs_en始终为0---------------------obs机体-----------------------------------------------------
if(state->obs_en)
{
//计算机体坐标下的运动加速度观测量。坐标系为北西天
for(u8 i = 0;i<3;i++)
{
s32 temp = 0;
for(u8 j = 0;j<3;j++)
{
temp += imu->obs_acc_w[j] * att_matrix[j][i];//t[i][j] 转置为 t[j][i]
}
imu->obs_acc_a[i] = temp;//机体坐标系下的IMU acc_a[i]的值
imu->gra_acc[i] = acc[i] - imu->obs_acc_a[i]; // 地理坐标系下的 acc[i]=加速度计测量的值 - 机体坐标系下的值
}
}
//只执行下面的----------------------------------------------------------------------
else
{
for(u8 i = 0;i<3;i++)
{
imu->gra_acc[i] = acc[i];//把加速度计的测量值保存在指针里面gra_acc
//%%%地理坐标系下的加速度值等于加速度计测量的加速度值
}
}
//把加速度计的测量值平方和再开根号，再倒数，并保存
acc_norm_l_recip = my_sqrt_reciprocal(my_pow(imu->gra_acc[X]) + my_pow(imu->gra_acc[Y]) + my_pow(imu->gra_acc[Z]));
acc_norm_l = safe_div(1,acc_norm_l_recip,0);//再求倒数--->结果就是加速度计的测量值的平方和开根号
// 加速度计的读数，单位化。
for(u8 i = 0;i<3;i++)
{
acc_norm[i] = imu->gra_acc[i] *acc_norm_l_recip;
}
// 载体坐标下的x方向向量，单位化----> 单位化的旋转矩阵Rcb 与向量（1 0 0）的转置 相乘的结果
att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);
att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;
att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;
// 载体坐标下的y方向向量，单位----->单位化的旋转矩阵Rcb 与向量（0 1 0）的转置 相乘的结果
att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;
att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);
att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;
// 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。---->单位化的旋转矩阵Rcb 与向量（0 0 1）的转置 相乘的结果
att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;
att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;
att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);
//以上就把旋转矩阵求解出来了att_matrix[][]
//水平面方向单位向量 将水平坐标系的x,y平方和，开根号 并求倒数 reciprocal：倒数的
float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;
//*****************************************************************************************************
// 计算载体坐标下的运动加速度。
for(u8 i = 0;i<3;i++)
{
imu->a_acc[i] = (s32)(acc[i] - 981 * imu->z_vec[i]);//将加速度计测量的数据减去(载体坐标系下)z轴方向向量
//载体坐标系下的加速度 = 加速度计测量的加速度值 - 载体重力加速度在Z轴上的分量
}
//计算世界坐标下的运动加速度。坐标系为北西天-----------------原理就是：旋转矩阵(机体转世界)乘上机体下的加速度
for(u8 i = 0;i<3;i++)
{
s32 temp = 0;
for(u8 j = 0;j<3;j++)
{
temp += imu->a_acc[j] *att_matrix[i][j];//
}
imu->w_acc[i] = temp; //世界坐标系下的加速度值
}
w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);//将世界坐标系的运动加速度转换成水平面的运动加速度---------这是作甚么
// >>>>>>>>>>>>>>> 加速度计读取的单位方向 与 重力加速度单位方向的差值 = 两个方向的叉乘<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
vec_err[X] = (acc_norm[Y] * imu->z_vec[Z] - imu->z_vec[Y] * acc_norm[Z]);
vec_err[Y] = -(acc_norm[X] * imu->z_vec[Z] - imu->z_vec[X] * acc_norm[Z]);
vec_err[Z] = -(acc_norm[Y] * imu->z_vec[X] - imu->z_vec[Y] * acc_norm[X]);
#ifdef USE_MAG
//电子罗盘赋值为float矢量
for(u8 i = 0;i<3;i++)
{
mag_val_f[i] = (float)mag_val[i];
}
if(!(mag_val[X] ==0 && mag_val[Y] == 0 && mag_val[Z] == 0))//当磁力计的三维坐标有值时，就开始对磁力计的数据进行处理
{
//把载体坐标下的罗盘数据转换到世界坐标系下
a2w_3d_trans(mag_val_f,imu->w_mag);
//将磁力计数据 平方和->开根号->倒数
float l_re_tmp = my_sqrt_reciprocal(my_pow(imu->w_mag[0]) + my_pow(imu->w_mag[1]));
//将磁力计的数值归一化成方向向量
mag_2d_w_vec[1][0] = imu->w_mag[0] * l_re_tmp;//
mag_2d_w_vec[1][1] = imu->w_mag[1] * l_re_tmp;
//计算南北朝向方向向量的误差，地理坐标中，水平面磁场方向向量应恒为南北 (1,0)
//mag_2d_w_vec[1]:磁力计的测量数据归一化后的值-----实际值
//mag_2d_w_vec[0]:(1,0)----------------------------理论值
//两者叉乘可以得出磁力计数据的方向与实际方向的偏差----------------偏差值
mag_yaw_err = vec_2_cross_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
//计算南北朝向向量点乘，判断同向或反向
mag_err_dot_prudoct = vec_2_dot_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
//若反向，直接给最大误差
if(mag_err_dot_prudoct<0)
{
mag_yaw_err = my_sign(mag_yaw_err) *1.0f;//最大误差是1
}
//
}
#endif
for(u8 i = 0;i<3;i++)
{
//没有定义USE_EST_DEADZONE
#ifdef USE_EST_DEADZONE //故不进来
if(state->G_reset == 0 && state->obs_en == 0)
{
vec_err[i] = my_deadzone(vec_err[i],0,imu->gacc_deadzone[i]);
}
#endif
//定义了USE_LENGTH_LIM
#ifdef USE_LENGTH_LIM //限幅
if(acc_norm_l>1060 || acc_norm_l<900)//当加速度值过大或者过小，就忽略加速度计的误差，即设为0
{
vec_err[X] = vec_err[Y] = vec_err[Z] = 0;
}
#endif
//误差积分
vec_err_i[i] += LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;//加速度计的误差累计，且乘上了积分常数
// 构造增量旋转（含融合纠正）。
// d_angle[X] = (gyr[X] + (vec_err[X] + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;
// d_angle[Y] = (gyr[Y] + (vec_err[Y] + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;
// d_angle[Z] = (gyr[Z] + (vec_err[Z] + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;
#ifdef USE_MAG
//用叉积误差来做PI修正陀螺仪零偏，即抵消陀螺仪读数中的偏移量
//用加速度计方向的偏差PI 和 磁力计偏航方向的偏差P 来修正陀螺仪的角速度 = d_angle[i]
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use ) * dT / 2 ; //不进入这里
#endif
}
//*********************************** 姿态更新 *********************************************************************
//基于四元数的姿态解算的互补滤波算法
imu->w = imu->w - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
imu->x = imu->w*d_angle[X] + imu->x + imu->y*d_angle[Z] - imu->z*d_angle[Y];
imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y + imu->z*d_angle[X];
imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;
//四元数规范化（单位化）
q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
imu->w *= q_norm_l;
imu->x *= q_norm_l;
imu->y *= q_norm_l;
imu->z *= q_norm_l;
//*********************************** 更新结束 ***********************************************************************
/修正开关 正常情况下各个修正开关都是打开的，在开头可以看出///
#ifdef USE_MAG
if(state->M_fix_en==0)//磁力计修正开关=0
{
mkp_use = 0;//比例P常数 = 0
state->M_reset = 0;//罗盘清除复位标记
}
else//磁力计修正开关=1
{
if(state->M_reset)//罗盘修正复位
{
//通过增量进行对准
mkp_use = 10.0f;//修正复位
if(mag_yaw_err != 0 && ABS(mag_yaw_err)<0.01f)
{
state->M_reset = 0;//误差小于2的时候，清除复位标记
}
}
else //罗盘修正不复位
{
mkp_use = state->mkp; //正常修正
}
}
#endif
if(state->G_fix_en==0)//重力方向修正开关=0
{
kp_use = 0;//不修正
}
else
{
if(state->G_reset == 0)//正常修正
{
kp_use = state->gkp;
ki_use = state->gki;
}
else//快速修正，通过增量进行对准
{
kp_use = 10.0f;
ki_use = 0.0f;
// imu->est_speed_w[X] = imu->est_speed_w[Y] = 0;
// imu->est_acc_w[X] = imu->est_acc_w[Y] = 0;
// imu->est_acc_h[X] = imu->est_acc_h[Y] =0;
//计算静态误差是否缩小
// imu_reset_val += (ABS(vec_err[X]) + ABS(vec_err[Y])) *1000 *dT;
// imu_reset_val -= 0.01f;
imu_reset_val = (ABS(vec_err[X]) + ABS(vec_err[Y]));
imu_reset_val = LIMIT(imu_reset_val,0,1.0f);
if((imu_reset_val < 0.02f) && (state->M_reset == 0))//加速度计水平误差绝对值之和过小，且磁力计的修正开关=0---------相当于重力修正开关没必要打开
{
//计时
reset_cnt += 2;
if(reset_cnt>400)//如果计时到了一定值，效果还是很好并且磁力计修正开关关掉时，那就不用重力修正了
{ //如果加速度计的误差小，但是磁力计的修正开关还是打开的，那重力修正开关就会继续打开
reset_cnt = 0;
state->G_reset = 0;//已经对准，清除复位标记
}
}
else
{
reset_cnt = 0;
}
}
}
}
static float t_temp;
//计算欧拉角
void calculate_RPY()
{
///输出姿态角///
t_temp = LIMIT(1 - my_pow(att_matrix[2][0]),0,1);
//imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;
if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算
{
imu_data.pit = fast_atan2(att_matrix[2][0],my_sqrt(t_temp))*57.30f;
imu_data.rol = fast_atan2(att_matrix[2][1], att_matrix[2][2])*57.30f;
imu_data.yaw = -fast_atan2(att_matrix[1][0], att_matrix[0][0])*57.30f;
}
}
总结：该文件就只做了两件事：1、姿态更新 2、将姿态阵解算出姿态角
具体说明：
1、姿态更新
首先我们可以通过参考书知道姿态更新的方程式，就是与角速度有关。
对于角速度的测量主要依靠陀螺仪的数据，但它是有误差的，在这段代码中使用了加速度计和磁力计来修正陀螺仪的误差。修正完后根据更新式求出更新后的姿态阵。
2、由姿态阵解算出姿态角，可以直接根据公式求解出。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
