首页 » 单片机 » 正文
单片机

一阶高通滤波+二阶Mahony滤波的四元数姿态解算

此次实验我使用icm20602进行

icm20602输出有以下特点：

  1. 3轴陀螺仪可选量程有±250dps，±500dps，±1000dps，±2000dps。（dps:degrees per
    second）； 3轴加速度计可选量程有±2g，±4g，±8g和±16g； 10MHz SPI和400kHz快速I2C。
  2. 3轴加速度计可选量程有±2g，±4g，±8g和±16g；
  3. 10MHz SPI和400kHz快速I2C。

  icm20602与常用的加速度计、陀螺仪不同，他的噪声相对来说比较小

  此次采到的数据经过16bit ADCs传到Sensor Registers，通过SPI协议从SR里读出数据。Sensor Register是只读寄存器，存储陀螺仪、加速度计、温度传感器信息。数据随时可读。
从寄存器中读出所需要的姿态换算的数据3轴加速度、3轴陀螺仪

有关一阶互补滤波的算法可以参考这篇文章，比较简单，不再重复
https://zhuanlan.zhihu.com/p/33773085

此次实验为一阶高通滤波+二阶Mahony滤波的四元数姿态解算

废话不多说，进入正题

姿态解算以一下方面进行：

表示

首先是加速度，磁力计的校准（由于icm20602没有三轴磁力计，故所计算的z轴欧拉角会比与实际值偏差比较大，我们用x，y轴即可）：


void ins_calibration(void)
{  
    
    vector3f_t _acc_var;           //存放加计方差
    vector3i_t _acc_vector[560];   //存放加计读取的原始数据
    vector3i_t _gyro_vector[550];  //存放角速度计读取的原始数据
    vector3f_t _gyro_average;      //存放角速度计平均值
    vector3f_t _acc_average;       //存放加计平均值
    vector3f_t _gyro_var;          //存放角速度计方差
    
    do{
        for(int i = 0; i < 500; i++)
        {
						
			get_icm20602_accdata_spi();
			get_icm20602_gyro_spi();
			_acc_vector[i].x = icm_acc_x;
			_acc_vector[i].y = icm_acc_y;
			_acc_vector[i].z = icm_acc_z;
			_gyro_vector[i].x = icm_gyro_x;
			_gyro_vector[i].y = icm_gyro_y;
			_gyro_vector[i].z = icm_gyro_z;
            _acc_average.x  += _acc_vector[i].x/500.0f;
            _acc_average.y  += _acc_vector[i].y/500.0f;
            _acc_average.z  += (_acc_vector[i].z - 8192)/500.0f; 
            _gyro_average.x += _gyro_vector[i].x/500.0f;
            _gyro_average.y += _gyro_vector[i].y/500.0f;
            _gyro_average.z += _gyro_vector[i].z/500.0f; 
	
            systick_delay_ms(1);
        }
        /* 计算方差 确保校准的时候是静止状态的 */
        for(int j = 0; j < 500; j++)
        {
            _acc_var.x +=  0.002 * (_acc_vector[j].x - _acc_average.x) * (_acc_vector[j].x - _acc_average.x);
            _acc_var.y +=  0.002 * (_acc_vector[j].y - _acc_average.y) * (_acc_vector[j].y - _acc_average.y);
            _acc_var.z +=  0.002 * (_acc_vector[j].z - _acc_average.z) * (_acc_vector[j].z - _acc_average.z);
            
            _gyro_var.x +=  0.002 * (_gyro_vector[j].x - _gyro_average.x) * (_gyro_vector[j].x - _gyro_average.x);
            _gyro_var.y +=  0.002 * (_gyro_vector[j].y - _gyro_average.y) * (_gyro_vector[j].y - _gyro_average.y);
            _gyro_var.z +=  0.002 * (_gyro_vector[j].z - _gyro_average.z) * (_gyro_vector[j].z - _gyro_average.z);

					
        }
        
        /* 快速计算 1/Sqrt(x) */
        _gyro_var.x = invSqrt(_gyro_var.x);
        _gyro_var.y = invSqrt(_gyro_var.y);
        _gyro_var.z = invSqrt(_gyro_var.z);
        if(   _gyro_var.x > VAR
           && _gyro_var.y > VAR 
           && _gyro_var.z > VAR )     
        {
            acc_vector_offset.x   = (int16_t)_acc_average.x;          //保存静止时的零偏值
            acc_vector_offset.y   = (int16_t)_acc_average.y;
            acc_vector_offset.z   = (int16_t)_acc_average.z;
            gyro_vector_offset.x  = (int16_t)_gyro_average.x;
            gyro_vector_offset.y  = (int16_t)_gyro_average.y;
            gyro_vector_offset.z  = (int16_t)_gyro_average.z;
            flag_ins_calibration = false;                            //校准标志位清零
        }
    }while(flag_ins_calibration);
}

  可以看出，我在校准方面：在初始化的时候进行500个数据的读取，并对其取方差，在取得的方差在合理范围内的话，该值有效，可以被视为静止状态，静止状态即为六轴零飘值。

获取零飘值后，接下来对icm20602读取的加速度与角速度减去零飘即为合理的实际值。

void ins_update(void)
{
    /* 保存最近三次的数据 */
    static vector3i_t gyro_vector_last[3];
    static vector3i_t acc_vector_last[3];
    static uint8_t num = 0;
    if(num > 2) num = 0;
    
//    if(flag_ins_calibration)  //如果需要校准
//    {
//        ins_calibration();    //校准
//    }
		
		/*  数据获取 */
	get_icm20602_accdata_spi();
	get_icm20602_gyro_spi();
    acc_vector_last[num].x = icm_acc_x;
	acc_vector_last[num].y = icm_acc_y;
	acc_vector_last[num].z = icm_acc_z;
	gyro_vector_last[num].x = icm_gyro_x;
	gyro_vector_last[num].y = icm_gyro_y;
	gyro_vector_last[num].z = icm_gyro_z;
	
		
    /* 去零偏 */
    acc_vector_last[num].x -= acc_vector_offset.x;   
    acc_vector_last[num].y -= acc_vector_offset.y;   
    acc_vector_last[num].z -= acc_vector_offset.z;   
    gyro_vector_last[num].x -= gyro_vector_offset.x; 
    gyro_vector_last[num].y -= gyro_vector_offset.y; 
    gyro_vector_last[num].z -= gyro_vector_offset.z; 
    
    
    /* 平均 低通滤波 */
    acc_vector.x = LowPassFilter_apply(&low_filter_acc_x, (acc_vector_last[0].x + acc_vector_last[1].x + acc_vector_last[2].x)/3);
    acc_vector.y = LowPassFilter_apply(&low_filter_acc_y, (acc_vector_last[0].y + acc_vector_last[1].y + acc_vector_last[2].y)/3);
    acc_vector.z = LowPassFilter_apply(&low_filter_acc_z, (acc_vector_last[0].z + acc_vector_last[1].z + acc_vector_last[2].z)/3);
    
    gyro_vector.x = LowPassFilter_apply(&low_filter_gyro_x, (gyro_vector_last[0].x + gyro_vector_last[1].x + gyro_vector_last[2].x)/3);
    gyro_vector.y = LowPassFilter_apply(&low_filter_gyro_y, (gyro_vector_last[0].y + gyro_vector_last[1].y + gyro_vector_last[2].y)/3);
    gyro_vector.z = LowPassFilter_apply(&low_filter_gyro_z, (gyro_vector_last[0].z + gyro_vector_last[1].z + gyro_vector_last[2].z)/3);
    
//    gyro_vector.x = (gyro_vector_last[0].x + gyro_vector_last[1].x + gyro_vector_last[2].x)/3;
//    gyro_vector.y = (gyro_vector_last[0].y + gyro_vector_last[1].y + gyro_vector_last[2].y)/3;
//    gyro_vector.z = (gyro_vector_last[0].z + gyro_vector_last[1].z + gyro_vector_last[2].z)/3;
//    
    //加速度AD值 转换成 米/平方秒 
	acc_vector.x *=  Acc_Gain * G;
	acc_vector.y *=  Acc_Gain * G;
	acc_vector.z *=  Acc_Gain * G;
    
	//陀螺仪AD值 转换成 弧度/秒    
	gyro_vector.x *=  Gyro_Gr;  
	gyro_vector.y *=  Gyro_Gr;
	gyro_vector.z *=  Gyro_Gr;
       
    num++;
}

  这里面的LowPassFilter_apply（）函数为低通滤波器；我们通过记录两次过去值与现在值进行平均后，将该值带入低通滤波器进行预测。

void LowPassFilter_Init(filter_t *filter, float sample_freq, float cutoff_freq)
{
    filter->sample_freq = sample_freq;
    filter->cutoff_freq = cutoff_freq;
    if (filter->cutoff_freq <= 0.0f || filter->sample_freq <= 0.0f) {
        filter->alpha = 1.0;
    } else {
        float dt = 1.0/filter->sample_freq;
        float rc = 1.0f/(M_2PI*filter->cutoff_freq);
        filter->alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
    }
}

/*需要滤波的信号sample  上次的输出信号_output*/
float LowPassFilter_apply(filter_t *filter, float sample) 
{
    filter->oupt += (sample - filter->oupt) * filter->alpha;
    return filter->oupt;
}

  一阶低通滤波后，再通过将四个数据，通过四元数的转化后可以直接求得欧拉角。
但是，在我的尝试过程中，曲线仍然没有想象中的完美：
一阶低通滤波
  于是我开始寻找其他的滤波方案，让曲线更加平缓
  我采用的方案是在四元数转化的过程中加入Mahony滤波（此次没有选择卡尔曼滤波，因为计算所用的时间 : Mahony < 高低通 < EKF），考虑到MCU有其他大量数据处理，5ms中断一次的卡尔曼滤波+低通滤波会造成单片机资源的大量占用，因此我选择了更加高效的方式。
下面给出代码：

/****函数  AHRS_quat_update
	*作用  更新四元数
	*参数
	*返回值
	***/
void AHRS_quat_update(vector3f_t gyro, vector3f_t acc, float interval)
{
	float q0 = Q4.q0;
	float q1 = Q4.q1;
	float q2 = Q4.q2;
	float q3 = Q4.q3;
/***********  模长  ************/	
	float norm = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
/***********  加计测出的机体坐标系   **********/
	float ax = acc.x * norm;
	float ay = acc.y * norm;
	float az = acc.z * norm;
/***********  四元数解算出的机体坐标系  ***************/
	float half_vx = q1*q3 - q0*q2;
	float half_vy = q2*q3 + q0*q1;
	float half_vz = -0.5f + q0*q0 + q3*q3;
/***********  叉积求加计机体坐标和上次四元数解算机体坐标差  ************/	
	float half_ex = ay*half_vz - az*half_vy;
	float half_ey = az*half_vx - ax*half_vz;
	float half_ez = ax*half_vy - ay*half_vx;
/***********  使用PI控制器 修正机体坐标 *************/	
	integral.x += half_ex * ahrs_ki * interval;
	integral.y += half_ey * ahrs_ki * interval;
	integral.z += half_ez * ahrs_ki * interval;
	
	float gx = (gyro.x + ahrs_kp * half_ex + integral.x) * 0.5f * interval;
	float gy = (gyro.y + ahrs_kp * half_ey + integral.y) * 0.5f * interval;
	float gz = (gyro.z + ahrs_kp * half_ez + integral.z) * 0.5f * interval;
	
/**********  更新四元数  ********/
	Q4.q0 += (-q1 * gx - q2 * gy - q3 * gz); 
	Q4.q1 += ( q0 * gx + q2 * gz - q3 * gy); 
	Q4.q2 += ( q0 * gy - q1 * gz + q3 * gx); 
	Q4.q3 += ( q0 * gz + q1 * gy - q2 * gx); 
  //单位化四元数 	
	norm = invSqrt(Q4.q0 * Q4.q0 + Q4.q1 * Q4.q1 + Q4.q2 * Q4.q2 + Q4.q3 * Q4.q3);
	
	Q4.q0 *= norm;
	Q4.q1 *= norm;
	Q4.q2 *= norm;
	Q4.q3 *= norm;
}



/****函数  AHRS_quat_to_angle
	*作用  更新姿态角
	*参数
	*返回值
	***/
void AHRS_quat_to_angle(void)
{
	float conv_x = 2.0f * (Q4.q0 * Q4.q2 - Q4.q1 * Q4.q3);  
	float conv_y = 2.0f * (Q4.q0 * Q4.q1 + Q4.q2 * Q4.q3);
	float conv_z = Q4.q0 * Q4.q0 - Q4.q1 * Q4.q1 - Q4.q2 * Q4.q2 + Q4.q3 * Q4.q3;
/*******  姿态解算  ********/
	ahrs_angle.x = fast_atan(conv_y * invSqrt(conv_x * conv_x + conv_z * conv_z)) * 57.2958f;
	ahrs_angle.y = asin(2 * (Q4.q0 * Q4.q2 - Q4.q3 * Q4.q1)) * 57.2958f;
	ahrs_angle.z = atan2(2 * (Q4.q0 * Q4.q3 + Q4.q1 * Q4.q2), 1 - 2 * (Q4.q2 * Q4.q2 + Q4.q3 * Q4.q3)) * 57.2958f;   
    
    ahrs_angle.x = constrain_float(ahrs_angle.x, -90, 90);
    ahrs_angle.y = constrain_float(ahrs_angle.y, -90, 90);
}

（移植从APM飞控代码中）

给出测试结果：
请添加图片描述
（两张图测试环境不同，当然总体数据后者平滑度更好）
数据非常平滑，符合预期。

版权声明：本文为CSDN博主「L Z C」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_50595950/article/details/121891734

标签： · ·
生成海报
点击下载海报
加载中
点赞 0

L Z C

我还没有学会写个人说明！

暂无评论

发表评论

相关推荐

nodemcu 模块用mciropython 使用SD卡
STM32 2023-01-27

nodemcu 模块用mciropython 使用SD卡

使用sd卡可以扩大8266的存储器，使用时候很重要的是接线。 其他很容易。下面程序就是初始化、挂载sd卡、读sd卡里面main.cpp里面的代码行。 import machine, sdcard, os from machine

分享到：