将MPU6050的重力加速度从加速度计的数据中分离用以计算三维线性加速度

本文链接：https://blog.csdn.net/godmial/article/details/141157757

MPU6050简介

MPU6050由InvenSense公司生产，是一款高性能的微机电系统（MEMS）运动传感器。它结合了3轴陀螺仪和3轴加速度计，能够以极高的精度测量角速度和加速度。这款传感器支持多种量程设置，适用于不同的应用场景，同时，其内置的数字运动处理（DMP）引擎简化了数据处理流程，降低了主机处理器的负担。

三位线性加速度可以用来做什么？

可以用来做运动检测等等

去除重力加速度的影响

为了测量人体运动所产生的加速度，必须将重力加速度从加速度计的数据中分离．即将传感器测量值分解为重力加速度和线性加速度。

MPU6050所测量的加速度包含两部分：一是由运动引起的线性加速度，二是恒定的重力加速度。为了准确地分析运动特性，必须将这两者分开。这里采用的是低通滤波的方法，特别是采用一阶低通滤波器来提取重力加速度。（很多文章说需要欧拉角计算，实际上是不需要的）

低通滤波器原理

一阶低通滤波器的输出值由当前输入值和上一个输出值的加权平均构成，权重比例由滤波系数α决定。滤波系数α与滤波器的时间常数τc和采样周期dt有关，计算公式为：

其中，时间常数τc可以通过截止频率fcutoff来计算：

实现步骤

初始化：在滤波开始时，将重力加速度的估计值设为0。
读取原始加速度：从MPU6050读取三轴加速度值。
滤波处理：使用上述的滤波公式，计算出重力加速度的估计值。
线性加速度计算：从原始加速度中减去重力加速度的估计值，得到线性加速度。
更新状态：将本次计算出的重力加速度估计值保存，用于下一次迭代。

示例代码

我用的是STM32L431+RT-Thread，其他平台也可以参考

#include <stdio.h>
#include <stdbool.h>
#define MPU6XXX_DEVICE_NAME "i2c2"

#define FILTER_COEFFICIENT 0.8 // 滤波系数α

#define FILTER_WINDOW_SIZE 5 // 滑动窗口大小

// 假定以下函数和结构体已定义
typedef struct
{
    float ax, ay, az; // 线性加速度
    float gx, gy, gz; // 重力加速度
} AccelerationData;

typedef struct
{
    float buffer[FILTER_WINDOW_SIZE]; // 滤波器缓冲区
    int index;                        // 缓冲区索引
} FilterBuffer;

// 初始化函数，用于初始化重力加速度值
void init_acceleration(AccelerationData *acceleration)
{
    acceleration->gx = 0.0;
    acceleration->gy = 0.0;
    acceleration->gz = 0.0;
}

// 加速度分解函数
void decompose_acceleration(float adx, float ady, float adz,
                            AccelerationData *acceleration,
                            bool isFirstSample)
{
    if (isFirstSample)
    {
        acceleration->ax = adx;
        acceleration->ay = ady;
        acceleration->az = adz;
    }
    else
    {
        acceleration->gx = FILTER_COEFFICIENT * adx + (1 - FILTER_COEFFICIENT) * acceleration->gx;
        acceleration->gy = FILTER_COEFFICIENT * ady + (1 - FILTER_COEFFICIENT) * acceleration->gy;
        acceleration->gz = FILTER_COEFFICIENT * adz + (1 - FILTER_COEFFICIENT) * acceleration->gz;

        acceleration->ax = adx - acceleration->gx;
        acceleration->ay = ady - acceleration->gy;
        acceleration->az = adz - acceleration->gz;
    }
}

// 初始化滤波器缓冲区
void init_filter_buffer(FilterBuffer *filter)
{
    for (int i = 0; i < FILTER_WINDOW_SIZE; i++)
    {
        filter->buffer[i] = 0.0;
    }
    filter->index = 0;
}

// 数据预处理函数
float preprocess_data(float data, FilterBuffer *filter)
{
    filter->buffer[filter->index % FILTER_WINDOW_SIZE] = data;
    filter->index++;

    float sum = 0.0;
    for (int i = 0; i < FILTER_WINDOW_SIZE; i++)
    {
        sum += filter->buffer[i];
    }

    return sum / FILTER_WINDOW_SIZE;
}


int main(void)
{
    // 初始化MPU6XXX设备结构体指针
    struct mpu6xxx_device *dev;
    // 定义加速度、角速度和温度数据结构体
    struct mpu6xxx_3axes accel, gyro, temp;
    // 定义加速数据结构体
    AccelerationData acceleration;
    // 定义滤波器缓冲区结构体
    FilterBuffer filterBuffer;
    // 标记是否是首次采样
    bool isFirstSample = true;
    // 定义线性加速度分解后的三个轴向变量
    float adx, ady, adz;

    // 初始化MPU6XXX设备
    dev = mpu6xxx_init(MPU6XXX_DEVICE_NAME, RT_NULL);

    // 设置加速度计范围为2G
    mpu6xxx_set_param(dev, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_2G);
    // 设置陀螺仪范围为250DPS
    mpu6xxx_set_param(dev, MPU6XXX_GYRO_RANGE, MPU6XXX_GYRO_RANGE_250DPS);

    // 设置数字低通滤波器配置为188HZ
    mpu6xxx_set_param(dev, MPU6XXX_DLPF_CONFIG, MPU6XXX_DLPF_188HZ);
    // 设置采样率为1000Hz
    mpu6xxx_set_param(dev, MPU6XXX_SAMPLE_RATE, 1000);

    // 初始化加速数据结构体
    init_acceleration(&acceleration);
    // 初始化滤波器缓冲区结构体
    init_filter_buffer(&filterBuffer);

    // 主循环
    while (1)
    {
        // 读取加速度和陀螺仪数据
        mpu6xxx_get_accel(dev, &accel);
        mpu6xxx_get_gyro(dev, &gyro);

        // 将加速度和角速度数据转换为实际值
        acceleration.ax = accel.x / 100.0f;
        acceleration.ay = accel.y / 100.0f;
        acceleration.az = accel.z / 100.0f;
        acceleration.gx = gyro.x / 100.0f;
        acceleration.gy = gyro.y / 100.0f;
        acceleration.gz = gyro.z / 100.0f;

        // 分解加速度数据，获取线性加速度
        decompose_acceleration(adx, ady, adz, &acceleration, isFirstSample);
        // 标记为非首次采样
        isFirstSample = false;

        // 对线性加速度进行预处理
        acceleration.ax = preprocess_data(acceleration.ax, &filterBuffer);
        acceleration.ay = preprocess_data(acceleration.ay, &filterBuffer);
        acceleration.az = preprocess_data(acceleration.az, &filterBuffer);

        // 在这里你可以添加更多的处理逻辑，比如显示数据或保存数据
        printf(" gx %.3f,  gy %.3f,  gz %.3f\r", acceleration.ax, acceleration.ay, acceleration.az);

        // 假设 delay() 是一个延时函数，确保采样频率不超过1000Hz
        rt_thread_mdelay(5); // 这里延迟5毫秒，根据你的采样率进行调整
    }

    // 主循环结束，返回0表示程序正常结束
    return 0;
}

头文件

#ifndef MPU6050_H
#define MPU6050_H
#include <stdint.h>
#include <rtthread.h>
#include <stdbool.h>

/* Default configuration, please change according to the actual situation, support i2c and spi device name */
#define MPU6050_DEVICE_NAME "i2c1" // i2c2
#define FILTER_COEFFICIENT 0.8     // 滤波系数α
#define FILTER_WINDOW_SIZE 5       // 滑动窗口大小

typedef struct
{
    float ax, ay, az; // 线性加速度
    float gx, gy, gz; // 重力加速度
} sensor_data_t;

typedef struct
{
    float buffer[FILTER_WINDOW_SIZE]; // 滤波器缓冲区
    int index;                        // 缓冲区索引
} data_filter_t;

typedef struct
{
    bool isFirstSample;
    float adx, ady, adz;
} data_sample_t;

typedef struct
{
    struct mpu6xxx_device *dev;
    sensor_data_t acceleration;
    data_filter_t filterBuffer;
    data_sample_t data_sample;
} mpu6050_device_t;

int mpu6050_init();


#endif

参考文献：胡成全, 王凯, 何丽莉, 魏枫林, & 姜宇. (2017). 基于MEMS六轴传感器的上肢运动识别系统. 大连理工大学学报, 57(1), 92-99.