ESP32-S3健康监测手环开发实战（二）
·
ESP32-S3健康监测手环开发实战（二）：MPU6050跌倒检测算法与状态机设计
本篇深入讲解基于MPU6050的跌倒检测算法实现，包括互补滤波姿态解算原理、三阶段跌倒检测状态机设计，以及关键阈值参数的选取。
1. 跌倒检测原理分析
1.1 人体跌倒的物理特征
人体跌倒通常经历三个阶段，每个阶段都有明显的物理特征：
正常站立 自由落体 撞击地面 倒地静止
│ │ │ │
加速度≈1g 加速度<0.4g 加速度>3g 姿态角>60°
│ │ │ │
▼ ▼ ▼ ▼
┌────┐ ┌────┐ ┌────┐ ┌────┐
│站立│ → │下落│ → │触地│ → │躺倒│
└────┘ └────┘ └────┘ └────┘
阶段特征分析：
| 阶段 | 加速度特征 | 持续时间 | 检测方法 |
|---|---|---|---|
| 自由落体 | 模值<0.4g | 100-500ms | 加速度阈值检测 |
| 撞击 | 模值>3g | 瞬间（<100ms） | 加速度峰值检测 |
| 倒地 | 姿态角>60° | 持续 | Roll/Pitch角度判断 |
1.2 三阶段检测模型
单一阈值检测容易产生误报（如快速蹲下、剧烈运动），本项目采用三阶段检测模型：
- 自由落体检测：加速度模值突然减小（<0.4g）
- 撞击检测：紧随自由落体后，加速度模值突然增大（>3g）
- 姿态判断：撞击后身体处于倒卧状态（Roll或Pitch>60°）
只有三个阶段依次满足，才判定为跌倒。
2. MPU6050传感器配置
2.1 传感器量程选择
// 跌倒检测推荐配置
ret = mpu6050_config(s_mpu6050, MPU6050_ACCE_FS_8G, MPU6050_GYRO_FS_1000DPS);
量程选择依据：
| 参数 | 选择 | 理由 |
|---|---|---|
| 加速度量程 | ±8g | 跌倒撞击峰值可达5-8g，±8g足够且分辨率适中 |
| 陀螺仪量程 | ±1000dps | 跌倒时角速度约100-500dps |
灵敏度对照表：
// 加速度计灵敏度 (LSB/g)
switch (fs) {
case MPU6050_ACCE_FS_2G: return 16384.0f; // 最高精度
case MPU6050_ACCE_FS_4G: return 8192.0f;
case MPU6050_ACCE_FS_8G: return 4096.0f; // 跌倒检测推荐
case MPU6050_ACCE_FS_16G: return 2048.0f; // 最大量程
}
// 陀螺仪灵敏度 (LSB/(度/秒))
switch (fs) {
case MPU6050_GYRO_FS_250DPS: return 131.0f;
case MPU6050_GYRO_FS_500DPS: return 65.5f;
case MPU6050_GYRO_FS_1000DPS: return 32.8f; // 跌倒检测推荐
case MPU6050_GYRO_FS_2000DPS: return 16.4f;
}
2.2 采样率配置
#define SAMPLE_PERIOD_MS 20 // 主循环周期 20ms (50Hz)
50Hz采样率的选择理由：
- 跌倒过程约0.5-1秒，50Hz可采集25-50个数据点，足够捕捉细节
- 更高采样率增加计算负担，收益有限
- FreeRTOS的tick精度为1ms，20ms周期稳定可靠
2.3 原始数据读取与转换
esp_err_t mpu6050_get_acce(mpu6050_handle_t handle, mpu6050_acce_t *acce)
{
uint8_t data[6];
esp_err_t ret = mpu6050_read_reg(dev, REG_ACCEL_XOUT_H, data, 6);
if (ret != ESP_OK) return ret;
float sensitivity = get_acce_sensitivity(dev); // 4096.0 for ±8g
// 原始值转换为g
int16_t raw_x = (int16_t)((data[0] << 8) | data[1]);
int16_t raw_y = (int16_t)((data[2] << 8) | data[3]);
int16_t raw_z = (int16_t)((data[4] << 8) | data[5]);
acce->x = raw_x / sensitivity; // 单位：g
acce->y = raw_y / sensitivity;
acce->z = raw_z / sensitivity;
return ESP_OK;
}
3. 互补滤波姿态解算
3.1 为什么需要姿态融合？
加速度计的问题：
- 优点：长期稳定，无漂移
- 缺点：易受运动干扰，动态响应慢
陀螺仪的问题：
- 优点：动态响应快，不受加速度干扰
- 缺点：积分漂移，长期不稳定
互补滤波的思想：
结合两者优点——短期用陀螺仪（响应快），长期用加速度计（无漂移）。
3.2 从加速度计计算静态角度
当设备静止时，加速度计测量的是重力加速度。根据重力在各轴的分量，可以计算出姿态角：
// 从加速度计计算角度
float acce_roll = atan2f(acce->y, acce->z) * RAD_TO_DEG;
float acce_pitch = atan2f(acce->x, acce->z) * RAD_TO_DEG;
公式推导：
Roll角（横滚）：设备绕X轴旋转的角度
tan(roll) = Ay / Az
roll = atan2(Ay, Az)
Pitch角（俯仰）：设备绕Y轴旋转的角度
tan(pitch) = Ax / Az
pitch = atan2(Ax, Az)
3.3 陀螺仪积分计算动态角度
陀螺仪测量的是角速度（度/秒），积分得到角度变化量：
// 陀螺仪角度增量
float gyro_roll = gyro->x * dev->dt; // 角速度 × 时间 = 角度
float gyro_pitch = gyro->y * dev->dt;
时间间隔dt的精确计算：
// 使用gettimeofday获取精确时间
struct timeval now, dt_tv;
gettimeofday(&now, NULL);
timersub(&now, &dev->last_time, &dt_tv);
dev->dt = (float)dt_tv.tv_sec + (float)dt_tv.tv_usec / 1000000.0f;
dev->last_time = now;
// 异常值保护
if (dev->dt <= 0 || dev->dt > 1.0f) {
dev->dt = 0.02f; // 默认 50Hz = 20ms
}
3.4 互补滤波融合算法
#define ALPHA 0.99f // 陀螺仪权重
// 互补滤波公式
angle->roll = ALPHA * (angle->roll + gyro_roll) + (1.0f - ALPHA) * acce_roll;
angle->pitch = ALPHA * (angle->pitch + gyro_pitch) + (1.0f - ALPHA) * acce_pitch;
公式解析：
新角度 = α × (上次角度 + 陀螺仪增量) + (1-α) × 加速度计角度
= α × 陀螺仪预测值 + (1-α) × 加速度计测量值
Alpha=0.99的含义：
- 99%权重给陀螺仪短期预测
- 1%权重给加速度计长期校正
- 时间常数约 τ = dt/（1-α）= 0.02/0.01 = 2秒
- 即：加速度计的校正作用在约2秒后才能完全体现
Alpha值选择建议：
| Alpha值 | 特点 | 适用场景 |
|---|---|---|
| 0.90 | 加速度计影响大，响应快但抖动大 | 静态测量 |
| 0.95 | 平衡配置 | 一般应用 |
| 0.99 | 陀螺仪主导，平滑但响应慢 | 动态检测（跌倒） |
3.5 第一次采样的特殊处理
dev->sample_count++;
// 第一次采样，直接使用加速度计角度初始化
if (dev->sample_count == 1) {
angle->roll = acce_roll;
angle->pitch = acce_pitch;
gettimeofday(&dev->last_time, NULL);
return ESP_OK;
}
首次采样时没有历史数据，直接使用加速度计角度作为初值。
4. 跌倒检测状态机设计
4.1 状态定义
typedef enum {
FALL_STATE_NORMAL = 0, // 正常状态
FALL_STATE_FREE_FALL, // 自由落体检测中
FALL_STATE_IMPACT, // 撞击检测中
FALL_STATE_DETECTED, // 跌倒已检测到
} app_fall_state_t;
4.2 状态转换图
┌──────────────────────────────────────┐
│ │
▼ │
┌──────────┐ │
│ NORMAL │◄─────────────────┐ │
└────┬─────┘ │ │
│ │ │
加速度<0.4g 超时1秒 │
│ │ │
▼ │ │
┌─────────────┐ │ │
│ FREE_FALL │──────────────────┘ │
└──────┬──────┘ │
│ │
加速度>3g │
│ │
▼ │
┌──────────┐ │
│ IMPACT │───────────超时2秒───────────────┘
└────┬─────┘
│
Roll>60° 或 Pitch>60°
│
▼
┌──────────┐
│ DETECTED │──────────手动重置───────────────────┐
└──────────┘ │
│
▼
回到 NORMAL
4.3 检测参数定义
// 跌倒检测参数
#define FALL_FREE_FALL_TH 0.4f // 自由落体阈值 (g)
#define FALL_IMPACT_TH 3.0f // 撞击阈值 (g)
#define FALL_ANGLE_THRESHOLD 60.0f // 角度阈值 (度)
#define FALL_FREE_FALL_TIMEOUT 1000 // 自由落体超时 (ms)
#define FALL_IMPACT_TIMEOUT 2000 // 撞击后超时 (ms)
参数选取依据：
| 参数 | 值 | 依据 |
|---|---|---|
| FREE_FALL_TH | 0.4g | 自由落体时接近0g，0.4g留有余量 |
| IMPACT_TH | 3.0g | 跌倒撞击通常3-8g，3g排除轻微碰撞 |
| ANGLE_THRESHOLD | 60° | 站立时接近0°，躺下时接近90° |
| FREE_FALL_TIMEOUT | 1000ms | 自由落体不超过1秒 |
| IMPACT_TIMEOUT | 2000ms | 撞击后2秒内判断姿态 |
4.4 加速度模值计算
static float calc_acce_magnitude(const mpu6050_acce_t *acce)
{
return sqrtf(acce->x * acce->x +
acce->y * acce->y +
acce->z * acce->z);
}
加速度模值的物理意义：
- 静止时：模值≈1g（只有重力）
- 自由落体：模值≈0g（重力被抵消）
- 撞击时：模值可达5-10g
4.5 状态机完整实现
bool app_mpu6050_detect_fall(const app_mpu6050_data_t *data)
{
if (data == NULL) return false;
uint32_t now = xTaskGetTickCount() * portTICK_PERIOD_MS;
float acce_mag = data->acce_mag;
float abs_roll = fabsf(data->angle.roll);
float abs_pitch = fabsf(data->angle.pitch);
switch (s_fall_state) {
case FALL_STATE_NORMAL:
// 检测自由落体（加速度模值突然减小）
if (acce_mag < FALL_FREE_FALL_TH) {
s_fall_state = FALL_STATE_FREE_FALL;
s_fall_timestamp = now;
ESP_LOGD(TAG, "检测到自由落体: mag=%.2fg", acce_mag);
}
break;
case FALL_STATE_FREE_FALL:
// 自由落体后检测撞击
if (now - s_fall_timestamp > FALL_FREE_FALL_TIMEOUT) {
// 超时1秒，重置状态（可能是误触发）
s_fall_state = FALL_STATE_NORMAL;
} else if (acce_mag > FALL_IMPACT_TH) {
// 检测到撞击，进入下一阶段
s_fall_state = FALL_STATE_IMPACT;
s_fall_timestamp = now;
ESP_LOGD(TAG, "检测到撞击: mag=%.2fg", acce_mag);
}
break;
case FALL_STATE_IMPACT:
// 撞击后检测姿态变化
if (now - s_fall_timestamp > FALL_IMPACT_TIMEOUT) {
// 超时2秒，可能只是碰撞而非跌倒
s_fall_state = FALL_STATE_NORMAL;
} else if (abs_roll > FALL_ANGLE_THRESHOLD ||
abs_pitch > FALL_ANGLE_THRESHOLD) {
// 姿态角大于60度，确认跌倒
s_fall_state = FALL_STATE_DETECTED;
ESP_LOGW(TAG, "跌倒检测! roll=%.1f, pitch=%.1f",
data->angle.roll, data->angle.pitch);
return true;
}
break;
case FALL_STATE_DETECTED:
// 已检测到跌倒，等待外部重置
return true;
default:
s_fall_state = FALL_STATE_NORMAL;
break;
}
return false;
}
4.6 状态重置
void app_mpu6050_reset_fall_state(void)
{
s_fall_state = FALL_STATE_NORMAL;
ESP_LOGI(TAG, "跌倒状态已重置");
}
在主程序中，跌倒警报显示5秒后手动重置：
if (fall_detected) {
app_oled_show_fall_alert(); // 显示警报
vTaskDelay(pdMS_TO_TICKS(5000)); // 显示5秒
app_mpu6050_reset_fall_state(); // 重置状态
app_oled_clear_fall_alert(); // 恢复正常显示
}
5. 代码架构总结
5.1 驱动层与应用层分离
┌─────────────────────────────────────────────────┐
│ 应用层 (app_mpu6050.c) │
│ ┌─────────────────────────────────────────┐ │
│ │ • 跌倒检测状态机 │ │
│ │ • 业务逻辑控制 │ │
│ │ • 阈值参数管理 │ │
│ └─────────────────────────────────────────┘ │
├─────────────────────────────────────────────────┤
│ 驱动层 (mpu6050.c) │
│ ┌─────────────────────────────────────────┐ │
│ │ • I2C寄存器读写 │ │
│ │ • 原始数据转换 │ │
│ │ • 互补滤波计算 │ │
│ └─────────────────────────────────────────┘ │
└─────────────────────────────────────────────────┘
5.2 关键文件清单
| 文件 | 功能 | 核心内容 |
|---|---|---|
app_mpu6050.h | 应用层头文件 | 参数定义、数据结构、API声明 |
app_mpu6050.c | 应用层实现 | 初始化、跌倒检测状态机 |
mpu6050.h | 驱动层头文件 | 量程定义、底层API声明 |
mpu6050.c | 驱动层实现 | I2C通信、互补滤波 |
6. 调试与优化建议
6.1 调试方法
- 使用ESP_LOGD输出调试信息
ESP_LOGD(TAG, "检测到自由落体: mag=%.2fg", acce_mag);
- 串口绘图观察波形
// 输出CSV格式，可用串口绘图工具查看
printf("%.2f,%.2f,%.2f,%.2f\n",
acce_mag, angle.roll, angle.pitch, (float)s_fall_state);
6.2 参数调优建议
如果误报率高（没跌倒却报警）：
- 提高FREE_FALL_TH（如0.3g→0.2g）
- 提高IMPACT_TH（如3g→4g）
- 提高ANGLE_THRESHOLD（如60°→70°）
如果漏报率高（跌倒了没报警）：
- 降低FREE_FALL_TH（如0.4g→0.5g）
- 降低IMPACT_TH（如3g→2g）
- 降低ANGLE_THRESHOLD（如60°→50°）
6.3 进阶优化方向
- 添加确认机制：检测到跌倒后，等待一定时间，如果用户无反应再报警
- 机器学习：收集跌倒/非跌倒数据，训练分类模型
- 多传感器融合：结合气压计检测高度变化
7. 总结
本篇详细讲解了基于MPU6050的跌倒检测算法：
- 互补滤波：Alpha=0.99，陀螺仪短期预测+加速度计长期校正
- 三阶段检测：自由落体(<0.4g) → 撞击(>3g) → 姿态判断(>60°)
- 状态机设计：四状态循环，带超时保护防止误判
- 参数可调：阈值参数可根据实际场景调优
下篇预告
在下一篇文章中，我们将讲解MAX30102心率血氧测量的实现：
- PPG光电容积脉搏波原理
- 中断驱动的数据采集架构
- 过零检测心率算法
- R值公式计算血氧饱和度
系列文章导航：
- 第一篇：项目架构设计与双I2C总线实现
- 本篇：MPU6050跌倒检测算法与状态机设计
- 第三篇：MAX30102心率血氧算法与PPG信号处理
- 第四篇：软件I2C实现与OLED显示优化
开发环境：ESP-IDF 5.3.2 + VS Code
硬件平台：ESP32-S3 + MPU6050
更多推荐
36
0- 0
已为社区贡献4条内容
所有评论(0)