【已验证】STM32+MPU6050 姿态解算 + 运动状态识别 + 四阶段摔倒检测

一、引言

MPU6050 集成三轴加速度计 + 三轴陀螺仪，广泛用于可穿戴设备、人体状态监测、跌倒报警等场景。博主大学也是经常使用这个模块，属于是能比较便宜的了，几块钱，不过也有一些国产芯片冒充就是了。最近买了几个十轴姿态传感器，等有空我们来整一下十轴的。

写这篇博文也主要是做一个记录吧，把手头已有的模块都驱动一下，也算给后面项目的合成提前做好准备，这个模块可以做一些大学常见的课题，比如老人健康检测系统、宠物检测系统等。

如果有开发需求的可以找我哈。

本文基于 STM32 HAL 库 实现 MPU6050 原始数据读取、姿态角解算，并从零实现运动状态识别（静止 / 行走 / 跑步）与四阶段摔倒检测算法。同时结合实际调试日志，复盘「检测不到摔倒、失重失效、状态机卡死」等问题的定位与解决过程，代码只保留核心片段，方便直接移植到项目中。

工程的基本框架包括DMP的移植借鉴了部分原子哥的代码，主打一个能用就行。

二、硬件与软件环境

2.1 硬件平台

主控：STM32F103/F407（HAL 库通用）
传感器：MPU6050 六轴传感器
通信方式：I2C
功能：姿态解算、运动状态识别、摔倒检测

2.2 软件环境

开发工具：STM32CubeMX + Keil MDK
库版本：STM32 HAL 库
算法：加速度计姿态解算、合加速度计算、有限状态机（FSM）摔倒判定

三、MPU6050 核心驱动（STM32 HAL 版）

3.1 I2C 底层驱动（核心适配）

MPU6050 依赖 I2C 进行寄存器读写，以下是适配 HAL 库的核心读写函数，无冗余代码，可直接

复制代码

// MPU6050 I2C 地址
#define MPU6050_ADDR    0x68

// 写寄存器
int MPU6050_WriteReg(uint8_t reg, uint8_t data)
{
    return HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR<<1, reg,
                             I2C_MEMADD_SIZE_8BIT, &data, 1, 100);
}

// 读寄存器
int MPU6050_ReadReg(uint8_t reg, uint8_t *data)
{
    return HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR<<1, reg,
                            I2C_MEMADD_SIZE_8BIT, data, 1, 100);
}

// 连续读数据
int MPU6050_ReadData(uint8_t reg, uint8_t len, uint8_t *buf)
{
    return HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR<<1, reg,
                            I2C_MEMADD_SIZE_8BIT, buf, len, 100);
}

3.2 MPU6050 初始化

MPU6050 上电默认休眠，必须唤醒并配置量程：

复制代码

uint8_t MPU6050_Init(void)
{
    uint8_t id = 0;
    MPU6050_ReadReg(0x75, &id);       // 读ID寄存器
    if(id != 0x68) return 1;          // ID校验失败

    MPU6050_WriteReg(0x6B, 0x00);     // 唤醒传感器
    MPU6050_WriteReg(0x1C, 0x00);     // 加速度量程±2g
    MPU6050_WriteReg(0x1B, 0x00);     // 陀螺仪量程±250°/s
    return 0;
}

3.3 原始数据读取与合加速度计算

人体状态检测核心是合加速度（三轴加速度矢量模值），静止时约等于 1g，失重时＜1g，撞击时＞2g：

复制代码

// 读取三轴加速度原始数据
void MPU6050_ReadAccel(int16_t *ax, int16_t *ay, int16_t *az)
{
    uint8_t buf[6];
    MPU6050_ReadData(0x3B, 6, buf);

    *ax = (int16_t)(buf[0]<<8 | buf[1]);
    *ay = (int16_t)(buf[2]<<8 | buf[3]);
    *az = (int16_t)(buf[4]<<8 | buf[5]);
}

// 计算合加速度（单位：g）
float MPU6050_GetAccelNorm(void)
{
    int16_t ax, ay, az;
    MPU6050_ReadAccel(&ax, &ay, &az);

    float fx = ax / 16384.0f;   // ±2g 量程换算
    float fy = ay / 16384.0f;
    float fz = az / 16384.0f;

    return sqrt(fx*fx + fy*fy + fz*fz);
}

3.4 姿态角解算（Pitch/Roll）

摔倒判定依赖姿态角，使用加速度计计算俯仰角 / 横滚角：

复制代码

void MPU6050_GetPitchRoll(float *pitch, float *roll)
{
    int16_t ax, ay, az;
    MPU6050_ReadAccel(&ax, &ay, &az);

    *roll  = atan2(ay, az) * 180.0f / PI;
    *pitch = atan2(-ax, sqrt(ay*ay + az*az)) * 180.0f / PI;
}

四、应用层核心算法

4.1 数据结构定义

统一管理人体状态信息：

复制代码

// 运动状态枚举
typedef enum
{
    BODY_STATIC = 0,   // 静止
    BODY_WALK   = 1,   // 行走
    BODY_RUN    = 2,   // 跑步
    BODY_FALL   = 3    // 摔倒
}BodyStatus_TypeDef;

// 摔倒状态机阶段
typedef enum
{
    FALL_STAGE_NORMAL = 0,      // 正常状态
    FALL_STAGE_WEIGHTLESS = 1,  // 失重状态
    FALL_STAGE_IMPACT = 2,      // 撞击状态
    FALL_STAGE_STATIC_CHECK = 3 // 静止确认
}FallStage_TypeDef;

// 人体信息结构体
typedef struct
{
    float pitch;          // 俯仰角
    float roll;           // 横滚角
    float accel_norm;     // 合加速度
    BodyStatus_TypeDef status; // 当前状态
}BodyInfo_TypeDef;

BodyInfo_TypeDef body_info;

4.2 运动状态识别（静止 / 行走 / 跑步）

通过合加速度区间直接划分，简单高效：

复制代码

// 阈值定义
#define ACCEL_STATIC_MAX  1.1f  // 静止上限
#define ACCEL_WALK_MAX    1.8f  // 行走上限

void BodyStatus_Detect(void)
{
    float accel = body_info.accel_norm;

    if(accel < ACCEL_STATIC_MAX)
        body_info.status = BODY_STATIC;
    else if(accel < ACCEL_WALK_MAX)
        body_info.status = BODY_WALK;
    else
        body_info.status = BODY_RUN;
}

4.3 四阶段摔倒检测（核心算法）

采用工业级四阶段状态机 ，避免误触发，逻辑：正常 → 失重 → 撞击 → 姿态异常静止确认 → 摔倒

复制代码

// 摔倒判定阈值
#define GRAVITY_MIN        0.7f   // 失重阈值
#define GRAVITY_IMPACT     2.0f   // 撞击阈值
#define FALL_ANGLE_THRESH  45.0f  // 姿态异常角度
#define FALL_FILTER_FRAME  3      // 滤波帧数
#define STATIC_CHECK_MS    1000   // 静止确认时间

// 全局状态变量
FallStage_TypeDef fall_stage = FALL_STAGE_NORMAL;
uint16_t weightless_cnt = 0;
uint16_t impact_cnt = 0;
uint32_t static_start_time = 0;
float last_accel_norm = 0;

void BodyStatus_Judge(void)
{
    uint32_t now = HAL_GetTick();
    float accel = body_info.accel_norm;
    uint8_t is_angle_bad = (fabs(body_info.pitch) > FALL_ANGLE_THRESH) ||
                           (fabs(body_info.roll) > FALL_ANGLE_THRESH);

    switch(fall_stage)
    {
        case FALL_STAGE_NORMAL:
            BodyStatus_Detect(); // 识别静/走/跑
            // 失重检测
            if(accel < GRAVITY_MIN)
            {
                weightless_cnt++;
                if(weightless_cnt >= FALL_FILTER_FRAME)
                {
                    fall_stage = FALL_STAGE_WEIGHTLESS;
                    weightless_cnt = 0;
                }
            }
            else weightless_cnt = 0;
            break;

        case FALL_STAGE_WEIGHTLESS:
            // 撞击检测
            if(accel > GRAVITY_IMPACT)
            {
                impact_cnt++;
                if(impact_cnt >= FALL_FILTER_FRAME)
                {
                    fall_stage = FALL_STAGE_IMPACT;
                    impact_cnt = 0;
                }
            }
            break;

        case FALL_STAGE_IMPACT:
            // 姿态异常检查
            if(is_angle_bad)
            {
                static_start_time = now;
                fall_stage = FALL_STAGE_STATIC_CHECK;
            }
            else fall_stage = FALL_STAGE_NORMAL;
            break;

        case FALL_STAGE_STATIC_CHECK:
            // 静止确认：姿态异常+加速度稳定1g
            if(is_angle_bad && (accel >= 0.9f && accel <= 1.1f))
            {
                if(now - static_start_time >= STATIC_CHECK_MS)
                {
                    body_info.status = BODY_FALL;
                }
            }
            else
            {
                fall_stage = FALL_STAGE_NORMAL;
                body_info.status = BODY_STATIC;
            }
            break;
    }

    // 起身恢复：角度正常则退出摔倒
    if(body_info.status == BODY_FALL && !is_angle_bad)
    {
        body_info.status = BODY_STATIC;
        fall_stage = FALL_STAGE_NORMAL;
    }
}

五、调试问题定位与解决（实战踩坑）

结合实际调试日志，复盘最容易踩的 3 个坑：

5.1 问题一：完全检测不到摔倒，状态机卡死

现象：模拟摔倒时，状态始终停留在 NORMAL，无法进入失重阶段。原因：

GRAVITY_MIN 阈值设置过小（如 0.2g），实际摔倒失重达不到；
全局变量未初始化，状态机乱跳；
加速度未正确换算，数值异常。

解决：

阈值调整为 0.7f，适配人体摔倒真实失重幅度；
开机强制初始化全局状态变量；
确认加速度量程换算正确（±2g 用 16384）。

5.2 问题二：能进入失重 / 撞击，但不判定摔倒

现象：状态走到 STATIC_CHECK，但始终不输出 FALL。原因：

姿态角阈值过大，倒地后不触发 is_angle_bad；
静止确认时间过长，调试时未等待足够时长；
加速度抖动，不在 0.9~1.1g 区间。

解决：

角度阈值设为 45°，人体倒地必然触发；
调试时保持姿态异常静止 1 秒以上；
对合加速度做简单滑动滤波。

5.3 问题三：摔倒后无法自动恢复

现象：判定摔倒后，起身姿态正常，仍锁定摔倒状态。原因：起身恢复逻辑位置错误，未在主循环生效。

解决：将起身恢复逻辑放在状态机外部，每次调用都执行判断。

六、主循环调用逻辑

复制代码

int main(void)
{
    HAL_Init();
    SystemClock_Config();
    MX_I2C1_Init();
    MX_USART1_UART_Init();

    MPU6050_Init();      // 传感器初始化
    body_info.status = BODY_STATIC;

    while(1)
    {
        // 更新姿态与加速度
        MPU6050_GetPitchRoll(&body_info.pitch, &body_info.roll);
        body_info.accel_norm = MPU6050_GetAccelNorm();

        // 状态与摔倒判定
        BodyStatus_Judge();

        // 串口打印调试
        printf("状态:%d | accel:%.2f | 姿态异常:%d\r\n",
               fall_stage, body_info.accel_norm,
               (fabs(body_info.pitch)>45||fabs(body_info.roll)>45));

        HAL_Delay(50);
    }
}

七、测试效果

姿态恢复后可自动退出摔倒状态，无卡死、无误判。

其实结果还算是准确吧，毕竟6轴，作为课设毕设使用完全足够，能说名问题。

有测试代码需要的朋友可以后台联系博主哈。