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

此次实验我使用icm20602进行

icm20602输出有以下特点：

3轴陀螺仪可选量程有±250dps，±500dps，±1000dps，±2000dps。（dps:degrees per
second）； 3轴加速度计可选量程有±2g，±4g，±8g和±16g； 10MHz SPI和400kHz快速I2C。
3轴加速度计可选量程有±2g，±4g，±8g和±16g；
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飞控代码中）

给出测试结果：

（两张图测试环境不同，当然总体数据后者平滑度更好）
数据非常平滑，符合预期。

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

四元数姿态解算及多传感器融合详细解析
代码路径ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp ...
四元数姿态解算算法基础
文章目录姿态的表示方法四元数表示姿态的物理意义使用四元数进行载体姿态更新方程四元数微分方程四元数初始值确定姿态的表示方法载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数. 欧 ...
基于四元数互补滤波的无人机姿态解算
导航坐标系为东北天(ENU),其与机体坐标系(b)的方向余弦矩阵为CbcC_{b}^{c}Cbc
四元数姿态解算c语言例程_（21）用四元数插值来对齐IMU和图像帧
最近一直在外实习,好久没更新博文了,实在罪过哈. 面临秋招,亚历山大,宝宝想哭,但要坚强. 前不久看到一句话,很有感触,送给一起秋招的小伙伴拉: 不要等到准备充足了才去开始,放出去溜溜吧拉1 说正事哈 ...
四元数姿态解算详细步骤
获取陀螺仪和加速度计原始数据 short gryox,gyroy,gyroz; short accx,accy,accz; void IMU_GetData(void) {MPU_Get_Gyrosc ...
姿态解算四元数、方向余弦、欧拉角、Mahony滤波、四轴
姿态解算四元数.方向余弦.欧拉角.Mahony滤波说明:本文只是做了一些总结,需要一些对这方面的基础概念的了解. 一般人千万不要试图去深入探讨四元数 1. 方向余弦矩阵方向余弦矩阵是使用欧拉角( ...
stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
四旋翼无人机飞控系统设计(姿态解算)
姿态解算姿态传感器读出加速度和角速度,而对一个系统的自动控制往往需要更加上层和贴近应用的的一个属性:角度.所以需要通过加速度和角速度进行数据融合转化得到姿态角度. 以MPU6050为例,姿态 ...
Pixhawk代码分析-姿态解算篇B
姿态解算篇B 前言本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是William Premerlani and Paul Bizard的关于DCM的一篇经典论文< ...

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

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

最新文章

热门文章