之前做智能车比赛的时候用到了icm20602,需要用到陀螺仪yaw,pitch,roll这几个角度。当时看了很多篇文章,用了他们的方法,有些不能用,有些效果不好。所幸最后花了几天时间还是弄了出来。
今天打算把代码开源出来,让后来人不重蹈覆辙。
除了icm20602,其他陀螺仪也能用。
程序用的是梯度下降滤波+四元数解算。
icm20602.c
GYRO_VAR gyroscope;
float yaw,pit,roll;
void icm20602_get_data(GYRO_VAR *gyro_var)
{
uint8 dat[6];
if (gyro_var->fiter.offset_flag)
{
icm_simspi_r_reg_bytes(ICM20602_ACCEL_XOUT_H, dat, 6);
gyro_var->orig.acc.x = (int16)(((uint16)dat[0]<<8 | dat[1])) ;
gyro_var->orig.acc.y = (int16)(((uint16)dat[2]<<8 | dat[3])) ;
gyro_var->orig.acc.z = (int16)(((uint16)dat[4]<<8 | dat[5])) ;
icm_simspi_r_reg_bytes(ICM20602_GYRO_XOUT_H, dat, 6);
gyro_var->orig.gyro.x = (int16)(((uint16)dat[0]<<8 | dat[1])) - gyro_var->fiter.gyro_offset.x;
gyro_var->orig.gyro.y = (int16)(((uint16)dat[2]<<8 | dat[3])) - gyro_var->fiter.gyro_offset.y;
gyro_var->orig.gyro.z = (int16)(((uint16)dat[4]<<8 | dat[5])) - gyro_var->fiter.gyro_offset.z;
}
else
{
icm_simspi_r_reg_bytes(ICM20602_ACCEL_XOUT_H, dat, 6);
gyro_var->orig.acc.x = (int16)(((uint16)dat[0]<<8 | dat[1]));
gyro_var->orig.acc.y = (int16)(((uint16)dat[2]<<8 | dat[3]));
gyro_var->orig.acc.z = (int16)(((uint16)dat[4]<<8 | dat[5]));
icm_simspi_r_reg_bytes(ICM20602_GYRO_XOUT_H, dat, 6);
gyro_var->orig.gyro.x = (int16)(((uint16)dat[0]<<8 | dat[1]));
gyro_var->orig.gyro.y = (int16)(((uint16)dat[2]<<8 | dat[3]));
gyro_var->orig.gyro.z = (int16)(((uint16)dat[4]<<8 | dat[5]));
}
}
void LPF_1(float hz,float time,float in,float *out)
{
*out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out );
}
void limit_filter(float T,float hz,_lf_t *data,float in)
{
float abs_t;
LPF_1(hz,T, in,&(data->lpf_1));
abs_t = ABS(data->lpf_1);
data->out = LIMIT(in,-abs_t,abs_t);
}
/*
陀螺仪采集零偏
*/
#define OFFSET_COUNT 500
void IMU_offset(GYRO_VAR *gyro_var)
{
uint32 i;
int64 temp[6] = {0};
for (i = 0; i < OFFSET_COUNT; i++)
{
//mpu6050_get_data(gyro_var);
icm20602_get_data(gyro_var);
//mpu6050_get_data(gyro_var);
//spi_icm20602_get_data(gyro_var);
systick_delay_ms(2);
temp[0] += gyro_var->orig.acc.x;
temp[1] += gyro_var->orig.acc.y;
temp[2] += gyro_var->orig.acc.z;
temp[3] += gyro_var->orig.gyro.x;
temp[4] += gyro_var->orig.gyro.y;
temp[5] += gyro_var->orig.gyro.z;
}
gyro_var->fiter.acc_offset.x = temp[0] / OFFSET_COUNT;
gyro_var->fiter.acc_offset.y = temp[1] / OFFSET_COUNT;
gyro_var->fiter.acc_offset.z = temp[2] / OFFSET_COUNT;
gyro_var->fiter.gyro_offset.x = temp[3] / OFFSET_COUNT;
gyro_var->fiter.gyro_offset.y = temp[4] / OFFSET_COUNT;
gyro_var->fiter.gyro_offset.z = temp[5] / OFFSET_COUNT;
//采集完标志
gyro_var->fiter.offset_flag = 1;
}
//快速计算 Sqrt(x)
float my_sqrt(float number)
{
long i;
float x, y;
const float f = 1.5F;
x = number * 0.5F;
y = number;
i = * ( long * ) &y;
i = 0x5f3759df - ( i >> 1 );
y = * ( float * ) &i;
y = y * ( f - ( x * y * y ) );
y = y * ( f - ( x * y * y ) );
return number * y;
}
/*
梯度下降滤波
*/
void steepest_descend(int32 arr[],uint8 len,_steepest_st *steepest,uint8 step_num,int32 in)
{
uint8 updw = 1;//0 dw,1up
int16 i;
uint8 step_cnt=0;
uint8 step_slope_factor=1;
uint8 on = 1;
int8 pn = 1;
//float last = 0;
float step = 0;
int32 start_point = 0;
int32 pow_sum = 0;
steepest->lst_out = steepest->now_out;
if( ++(steepest->cnt) >= len )
{
(steepest->cnt) = 0; //now
}
//last = arr[ (steepest->cnt) ];
arr[ (steepest->cnt) ] = in;
step = (float)(in - steepest->lst_out)/step_num ;//梯度
if(ABS(step)<1)//整形数据<1的有效判定
{
if(ABS(step)*step_num<2)
{
step = 0;
}
else
{
step = (step > 0) ? 1 : -1;
}
}
start_point = steepest->lst_out;
do
{
//start_point = steepest->lst_out;
for(i=0;i<len;i++)
{
// j = steepest->cnt + i + 1;
// if( j >= len )
// {
// j = j - len; //顺序排列
// }
pow_sum += my_pow(arr[i] - start_point );// /step_num;//除法减小比例**
//start_point += pn *(step_slope_factor *step/len);
}
if(pow_sum - steepest->lst_pow_sum > 0)
{
if(updw==0)
{
on = 0;
}
updw = 1;//上升了
pn = (pn == 1 )? -1:1;
}
else
{
updw = 0; //正在下降
if(step_slope_factor<step_num)
{
step_slope_factor++;
}
}
steepest->lst_pow_sum = pow_sum;
pow_sum = 0;
start_point += pn *step;//调整
if(++step_cnt > step_num)//限制计算次数
{
on = 0;
}
//
if(step_slope_factor>=2)//限制下降次数1次,节省时间,但会增大滞后,若cpu时间充裕可不用。
{
on = 0;
}
//
}
while(on==1);
steepest->now_out = start_point ;//0.5f *(start_point + steepest->lst_out);//
steepest->now_velocity_xdt = steepest->now_out - steepest->lst_out;
}
#define MPU_WINDOW_NUM 5
#define MPU_STEEPEST_NUM 5
#define MPU_WINDOW_NUM_ACC 15
#define MPU_STEEPEST_NUM_ACC 15
_steepest_st steepest_ax;
_steepest_st steepest_ay;
_steepest_st steepest_az;
_steepest_st steepest_gx;
_steepest_st steepest_gy;
_steepest_st steepest_gz;
int32 steepest_ax_arr[MPU_WINDOW_NUM_ACC ];
int32 steepest_ay_arr[MPU_WINDOW_NUM_ACC ];
int32 steepest_az_arr[MPU_WINDOW_NUM_ACC ];
int32 steepest_gx_arr[MPU_WINDOW_NUM ];
int32 steepest_gy_arr[MPU_WINDOW_NUM ];
int32 steepest_gz_arr[MPU_WINDOW_NUM ];
void Data_steepest(GYRO_VAR *gyro_var)
{
// icm20602_get_data(&gyroscope);
steepest_descend(steepest_ax_arr ,MPU_WINDOW_NUM_ACC ,&steepest_ax ,MPU_STEEPEST_NUM_ACC,(int32)gyro_var->orig.acc.x);
steepest_descend(steepest_ay_arr ,MPU_WINDOW_NUM_ACC ,&steepest_ay ,MPU_STEEPEST_NUM_ACC,(int32) gyro_var->orig.acc.y);
steepest_descend(steepest_az_arr ,MPU_WINDOW_NUM_ACC ,&steepest_az ,MPU_STEEPEST_NUM_ACC,(int32) gyro_var->orig.acc.z);
steepest_descend(steepest_gx_arr ,MPU_WINDOW_NUM ,&steepest_gx ,MPU_STEEPEST_NUM,(int32) gyro_var->orig.gyro.x);
steepest_descend(steepest_gy_arr ,MPU_WINDOW_NUM ,&steepest_gy ,MPU_STEEPEST_NUM,(int32) gyro_var->orig.gyro.y);
steepest_descend(steepest_gz_arr ,MPU_WINDOW_NUM ,&steepest_gz ,MPU_STEEPEST_NUM,(int32) gyro_var->orig.gyro.z);
gyro_var->acc_res.z = steepest_az.now_out;
gyro_var->acc_res.y = steepest_ay.now_out;
gyro_var->acc_res.x = steepest_ax.now_out;
gyro_var->fiter.acc_fiter.x = steepest_ax.now_out *2.3926f;
gyro_var->fiter.acc_fiter.y = steepest_ay.now_out *2.3926f;
gyro_var->fiter.acc_fiter.z = steepest_az.now_out *2.3926f;
gyro_var->gyro_res.x = steepest_gx.now_out;
gyro_var->gyro_res.y = steepest_gy.now_out;
gyro_var->gyro_res.z = steepest_gz.now_out;
gyro_var->fiter.gyro_fiter.x = gyro_var->gyro_res.x *0.0610f;
gyro_var->fiter.gyro_fiter.y = gyro_var->gyro_res.y *0.0610f;
gyro_var->fiter.gyro_fiter.z = gyro_var->gyro_res.z *0.0610f;
values[0]=gyro_var->fiter.acc_fiter.x;
values[1]= gyro_var->fiter.acc_fiter.y ;
values[2]=gyro_var->fiter.acc_fiter.z;
values[3]=gyro_var->fiter.gyro_fiter.x ;
values[4]=gyro_var->fiter.gyro_fiter.y ;
values[5]=gyro_var->fiter.gyro_fiter.z;
}
/*
四元素解算
*/
#define RAD_PER_DEG 0.017453293f
#define Kp 110.0f //35.65f10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.002f //0.008f //0.005f // integral gain governs rate of convergence of gyroscope biases
// 需要根据具体姿态更新周期来调整,T是姿态更新周期,T*角速度=微分角度
#define halfT 0.005f//0.005//0.00259 // half the sample period采样周期的一半
#define dT 0.008f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
_xyz_f_st vec_err_i;
_xyz_f_st x_vec;
_xyz_f_st y_vec;
_xyz_f_st z_vec;
_xyz_f_st a_acc;
_xyz_f_st w_acc;
_lf_t err_lf_x;
_lf_t err_lf_y;
_lf_t err_lf_z;
void Q_IMUupdata(GYRO_VAR *gyro_var)
{
//Data_steepest(&gyroscope);
static float q0q1,q0q2,q1q1,q1q3,q2q2,q2q3,q3q3,q1q2,q0q3;
float acc_length,q_length;
float w_q,x_q,y_q,z_q;
_xyz_f_st acc_norm;
_xyz_f_st vec_err;
static _xyz_f_st d_angle;
w_q = q0;
x_q = q1;
y_q = q2;
z_q = q3;
// 先把这些用得到的值算好 减少计算量
q0q1 = w_q * x_q;
q0q2 = w_q * y_q;
q1q1 = x_q * x_q;
q1q3 = x_q * z_q;
q2q2 = y_q * y_q;
q2q3 = y_q * z_q;
q3q3 = z_q * z_q;
q1q2 = x_q * y_q;
q0q3 = w_q * z_q;
// normalise the measurements
// 规范化测量(acc数据归一化)
acc_length = my_sqrt(my_pow(gyro_var->fiter.acc_fiter.x) +
my_pow(gyro_var->fiter.acc_fiter.y) +
my_pow(gyro_var->fiter.acc_fiter.z));
acc_norm.x = gyro_var->fiter.acc_fiter.x/acc_length;
acc_norm.y = gyro_var->fiter.acc_fiter.y/acc_length;
acc_norm.z = gyro_var->fiter.acc_fiter.z/acc_length;
// estimated direction of gravity
// 估计重力方向、流量
x_vec.x = 1 - (2*q2q2 + 2*q3q3);
x_vec.y = 2*q1q2 - 2*q0q3;
x_vec.z = 2*q1q3 + 2*q0q2;
y_vec.x = 2*q1q2 + 2*q0q3;
y_vec.y = 1 - (2*q1q1 + 2*q3q3);
y_vec.z = 2*q2q3 - 2*q0q1;
z_vec.x = 2*q1q3 - 2*q0q2;
z_vec.y = 2*q2q3 + 2*q0q1;
z_vec.z = 1 - (2*q1q1 + 2*q2q2);
// 计算载体坐标下的运动加速度。(与姿态解算无关)
a_acc.x = gyro_var->fiter.acc_fiter.x - 9800 *z_vec.x;
a_acc.y = gyro_var->fiter.acc_fiter.y - 9800 *z_vec.y;
a_acc.z = gyro_var->fiter.acc_fiter.z - 9800 *z_vec.z;
// 计算世界坐标下的运动加速度。(与姿态解算无关)
w_acc.x = x_vec.x *a_acc.x + x_vec.y *a_acc.y + x_vec.z *a_acc.z;
w_acc.y = y_vec.x *a_acc.x + y_vec.y *a_acc.y + y_vec.z *a_acc.z;
w_acc.z = z_vec.x *a_acc.x + z_vec.y *a_acc.y + z_vec.z *a_acc.z;
// 测量值与等效重力向量的叉积(计算向量误差)。
vec_err.x = (acc_norm.y * z_vec.z - z_vec.y * acc_norm.z);
vec_err.y = -(acc_norm.x * z_vec.z - z_vec.x * acc_norm.z);
vec_err.z = -(acc_norm.y * z_vec.x - z_vec.y * acc_norm.x);
//截止频率1hz的低通限幅滤波
limit_filter(dT,0.2f,&err_lf_x,vec_err.x);
limit_filter(dT,0.2f,&err_lf_y,vec_err.y);
limit_filter(dT,0.2f,&err_lf_z,vec_err.z);
//误差积分
vec_err_i.x += err_lf_x.out *dT *Ki;
vec_err_i.y += err_lf_y.out *dT *Ki;
vec_err_i.z += err_lf_z.out *dT *Ki;
// 构造增量旋转(含融合纠正)。
d_angle.x = (gyro_var->fiter.gyro_fiter.x *RAD_PER_DEG + (err_lf_x.out + vec_err_i.x) * Kp) * dT / 2 ;
d_angle.y = (gyro_var->fiter.gyro_fiter.y *RAD_PER_DEG + (err_lf_y.out + vec_err_i.y) * Kp) * dT / 2 ;
d_angle.z = (gyro_var->fiter.gyro_fiter.z *RAD_PER_DEG + (err_lf_z.out + vec_err_i.z) * Kp) * dT / 2 ;
// 计算姿态。
q0 = w_q - x_q*d_angle.x - y_q*d_angle.y - z_q*d_angle.z;
q1 = w_q*d_angle.x + x_q + y_q*d_angle.z - z_q*d_angle.y;
q2 = w_q*d_angle.y - x_q*d_angle.z + y_q + z_q*d_angle.x;
q3 = w_q*d_angle.z + x_q*d_angle.y - y_q*d_angle.x + z_q;
q_length = my_sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
// ormalise quaternion
q0 /= q_length;
q1 /= q_length;
q2 /= q_length;
q3 /= q_length;
// 欧拉角转换
gyro_var->euler.pit = (asin(2*q1q3 - 2*q0q2))*57.3f;
gyro_var->euler.roll = (atan2(2*q2q3 + 2*q0q1, -2*q1q1-2*q2q2 + 1))*57.3f;
gyro_var->euler.yaw = -(atan2(2*q1q2 + 2*q0q3, -2*q2q2-2*q3q3+1))*57.3f;
yaw=gyro_var->euler.yaw;
pit=gyro_var->euler.pit;
roll=gyro_var->euler.roll;
}
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
void IMU_quaterToEulerianAngles(void)
{
icm20602_get_data(&gyroscope);
Data_steepest(&gyroscope);
Q_IMUupdata(&gyroscope);
if(yaw > 360)
{
yaw -=360;
}
else if(yaw <0)
{
yaw +=360;
}
}
icm20602.h
typedef struct
{
int16 x;
int16 y;
int16 z;
} INT16_XYZ;
typedef struct
{
float x;
float y;
float z;
} FLOAT_XYZ;
typedef struct
{
float pit;
float roll;
float yaw;
} FLOAT_EULER;
typedef struct
{
int32 x;
int32 y;
int32 z;
} INT32_XYZ ;
typedef struct
{
INT16_XYZ gyro;
INT16_XYZ acc;
} GYRO_ORIG;
typedef struct
{
FLOAT_XYZ gyro_fiter;
FLOAT_XYZ acc_fiter;
INT16_XYZ gyro_offset;
INT16_XYZ acc_offset;
uint8 offset_flag;
} GYRO_FITER;
typedef struct
{
INT32_XYZ tar_ang_vel;
INT32_XYZ tar_ang_vel_last;
INT32_XYZ tar_ang;
} GYRO_TAR_ANG;
typedef struct
{
GYRO_TAR_ANG target;
GYRO_ORIG orig;
GYRO_FITER fiter;
INT16_XYZ newTd;
INT16_XYZ acc_res;
INT16_XYZ gyro_res;
FLOAT_EULER euler;
} GYRO_VAR;
typedef struct
{
uint8 cnt;
int32 lst_pow_sum;
int32 now_out;
int32 lst_out;
int32 now_velocity_xdt;
} _steepest_st;
typedef struct
{
float x;
float y;
float z;
} _xyz_f_st;
typedef struct
{
float lpf_1;
float out;
}_lf_t;
extern float yaw,pitch,roll;
extern GYRO_VAR gyroscope;
#define M_PI_F 3.141592653589793f
#define ABS(x) ( (x)>0?(x):-(x) )//????
#define my_pow(a) ((a)*(a))
#define LIMIT( x,min,max ) ( (x) < (min) ? (min) : ( (x) > (max) ? (max) : (x) ) )
float my_sqrt(float number);
void IMU_offset(GYRO_VAR *gyro_var);
void Q_IMUupdata(GYRO_VAR *gyro_var);
void Data_steepest(GYRO_VAR *gyro_var);
void steepest_descend(int32 arr[],uint8 len,_steepest_st *steepest,uint8 step_num,int32 in);
void IMU_offset(GYRO_VAR *gyro_var);