此次实验我使用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飞控代码中）
给出测试结果：
（两张图测试环境不同，当然总体数据后者平滑度更好）
数据非常平滑，符合预期。
扫一扫
7849
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
