1336 字
7 分钟
icm20602姿态解算
2017-01-23
硬件编程
传感器
/
icm20602
/
姿态解算

这是一个基于ICM-20602陀螺仪的姿态解算程序，使用了梯度下降滤波和四元数算法。此程序适用于ICM-20602及其他陀螺仪模块。为了帮助后来的开发者避免重复的调试过程，我将开源这个代码。

主要功能：#

  1. 获取ICM-20602传感器的加速度和陀螺仪数据。
  2. 使用梯度下降法对陀螺仪数据进行滤波。
  3. 通过四元数解算，计算出设备的俯仰角（Pitch）、滚转角（Roll）和偏航角（Yaw）。
// 包含头文件
#include "icm20602.h"


// 全局变量
float yaw, pitch, roll; // 欧拉角
GYRO_VAR gyroscope; // 陀螺仪原始数据


// 获取传感器数据
void icm20602_get_data(GYRO_VAR *gyro_var) {
    uint8_t data[6];


    if (gyro_var->fiter.offset_flag) {
        // 读取加速度数据
        icm_simspi_r_reg_bytes(ICM20602_ACCEL_XOUT_H, data, 6);
        gyro_var->orig.acc.x = (int16_t)((uint16_t)data[0] << 8 | data[1]);
        gyro_var->orig.acc.y = (int16_t)((uint16_t)data[2] << 8 | data[3]);
        gyro_var->orig.acc.z = (int16_t)((uint16_t)data[4] << 8 | data[5]);


        // 读取陀螺仪数据并减去零偏
        icm_simspi_r_reg_bytes(ICM20602_GYRO_XOUT_H, data, 6);
        gyro_var->orig.gyro.x = (int16_t)((uint16_t)data[0] << 8 | data[1]) - gyro_var->fiter.gyro_offset.x;
        gyro_var->orig.gyro.y = (int16_t)((uint16_t)data[2] << 8 | data[3]) - gyro_var->fiter.gyro_offset.y;
        gyro_var->orig.gyro.z = (int16_t)((uint16_t)data[4] << 8 | data[5]) - gyro_var->fiter.gyro_offset.z;
    } else {
        // 没有零偏的情况直接读取数据
        icm_simspi_r_reg_bytes(ICM20602_ACCEL_XOUT_H, data, 6);
        gyro_var->orig.acc.x = (int16_t)((uint16_t)data[0] << 8 | data[1]);
        gyro_var->orig.acc.y = (int16_t)((uint16_t)data[2] << 8 | data[3]);
        gyro_var->orig.acc.z = (int16_t)((uint16_t)data[4] << 8 | data[5]);


        icm_simspi_r_reg_bytes(ICM20602_GYRO_XOUT_H, data, 6);
        gyro_var->orig.gyro.x = (int16_t)((uint16_t)data[0] << 8 | data[1]);
        gyro_var->orig.gyro.y = (int16_t)((uint16_t)data[2] << 8 | data[3]);
        gyro_var->orig.gyro.z = (int16_t)((uint16_t)data[4] << 8 | data[5]);
    }
}


// 低通滤波器（1阶）
void LPF_1(float hz, float time, float input, float *output) {
    *output += (1 / (1 + 1 / (hz * 6.28f * time))) * (input - *output);
}


// 限幅滤波器
void limit_filter(float T, float hz, _lf_t *data, float input) {
    float abs_t;
    LPF_1(hz, T, input, &(data->lpf_1));
    abs_t = ABS(data->lpf_1);
    data->out = LIMIT(input, -abs_t, abs_t);
}


// 陀螺仪零偏校正
#define OFFSET_COUNT 500
void IMU_offset(GYRO_VAR *gyro_var) {
    uint32_t i;
    int64_t temp[6] = {0};


    for (i = 0; i < OFFSET_COUNT; i++) {
        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; // 标志位置为1，表示零偏采集完成
}


// 快速平方根计算
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_t arr[], uint8_t len, _steepest_st *steepest, uint8_t step_num, int32_t input) {
    uint8_t updw = 1; // 0 dw, 1 up
    int16_t i;
    uint8_t step_cnt = 0;
    uint8_t step_slope_factor = 1;
    uint8_t on = 1;
    int8_t pn = 1;
    float step = 0;
    int32_t start_point = 0;
    int32_t pow_sum = 0;


    steepest->lst_out = steepest->now_out;


    if (++(steepest->cnt) >= len) {
        steepest->cnt = 0;
    }


    arr[steepest->cnt] = input;
    step = (float)(input - steepest->lst_out) / step_num; // 计算梯度


    if (ABS(step) < 1) {
        if (ABS(step) * step_num < 2) {
            step = 0;
        } else {
            step = (step > 0) ? 1 : -1;
        }
    }


    start_point = steepest->lst_out;
    do {
        for (i = 0; i < len; i++) {
            pow_sum += my_pow(arr[i] - start_point);
        }


        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) {
            on = 0;
        }


    } while (on == 1);


    steepest->now_out = start_point;
    steepest->now_velocity_xdt = steepest->now_out - steepest->lst_out;
}


// 姿态解算（四元数法）
void Q_IMUupdata(GYRO_VAR *gyro_var) {
    static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数初始化
    float acc_length, q_length;
    _xyz_f_st acc_norm;
    _xyz_f_st vec_err;
    static _xyz_f_st d_angle;


    // 归一化加速度数据
    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;


    // 计算重力方向
    _xyz_f_st x_vec = {1 - 2 * (q2 * q2 + q3 * q3), 2 * (q1 * q2 - q0 * q3), 2 * (q1 * q3 + q0 * q2)};
    _xyz_f_st y_vec = {2 * (q1 * q2 + q0 * q3), 1 - 2 * (q1 * q1 + q3 * q3), 2 * (q2 * q3 - q0 * q1)};
    _xyz_f_st z_vec = {2 * (q1 * q3 - q0 * q2), 2 * (q2 * q3 + q0 * q1), 1 - 2 * (q1 * q1 + q2 * q2)};


    // 计算加速度误差并进行积分
    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);


    // 调用限幅滤波器
    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 -= q1 * d_angle.x - q2 * d_angle.y - q3 * d_angle.z;
    q1 += q0 * d_angle.x + q1 * d_angle.y - q3 * d_angle.z;
    q2 += q0 * d_angle.y - q1 * d_angle.x + q2 * d_angle.z;
    q3 += q0 * d_angle.z + q1 * d_angle.z - q2 * d_angle.x;


    // 四元数归一化
    q_length = my_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 /= q_length;
    q1 /= q_length;
    q2 /= q_length;
    q3 /= q_length;


    // 计算欧拉角
    gyro_var->euler.pit = asin(2 * q1 * q3 - 2 * q0 * q2) * 57.3f;
    gyro_var->euler.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3f;
    gyro_var->euler.yaw = -(atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1)) * 57.3f;


    // 更新全局变量
    yaw = gyro_var->euler.yaw;
    pitch = gyro_var->euler.pit;
    roll = gyro_var->euler.roll;
}


// 四元数到欧拉角的转换
void IMU_quaterToEulerianAngles(void) {
    icm20602_get_data(&gyroscope);
    Data_steepest(&gyroscope);
    Q_IMUupdata(&gyroscope);


    // 限制yaw角度在0-360度之间
    if (yaw > 360) {
        yaw -= 360;
    } else if (yaw < 0) {
        yaw += 360;
    }
}
icm20602姿态解算
https://hw.rscclub.website/posts/icm20602/
作者
杨月昌
发布于
2017-01-23
许可协议
CC BY-NC-SA 4.0
混合信号系统中的接地技术
32个EMC设计的标准电路图
© 2025 杨月昌. All Rights Reserved.