【陀螺仪】基于状态机：根据俯仰角的变化，判断一次运动发生

一、基本思路

1-1 状态机流程图

        +-------------------+      差值 > 阈值       +-------------------+
        |                   | --------------------->|                   |
        |      STEP1        |                       |      STEP2        |
        | (等待动作开始)     |<---------------------| (等待动作结束)     |
        |                   |      差值 < 阈值       |                   |
        +-------------------+                       +-------------------+
               ^                                              |
               |                                              |
               +----------------------------------------------+
                                   完成一次动作后计数+1

1-2 示例应用场景

俯卧撑计数

  1. 中立值：手臂伸直时的俯仰角（Eexercise_Value[0]）。

  2. 阈值：身体下沉时的角度变化量（Eexercise_Value[1] = 30°）。

  3. 计数逻辑：

    • 从 STEP1 → STEP2：身体下沉超过 30°。

    • 从 STEP2 → STEP1：身体抬起回到小于 30°，计数加 1。

二、上代码！

uint8_t Count_Exercise(uint16_t value)
{

	switch(MACHINE_EXAMPLE.Eexercise_Step_State)
	{
		case STEP1:
		{
			MACHINE_EXAMPLE.Eexercise_Value[3] = value - MACHINE_EXAMPLE.Eexercise_Value[0];
			if(MACHINE_EXAMPLE.Eexercise_Value[3] < 0) MACHINE_EXAMPLE.Eexercise_Value[3] = -MACHINE_EXAMPLE.Eexercise_Value[3];
			if(MACHINE_EXAMPLE.Eexercise_Value[3]>MACHINE_EXAMPLE.Eexercise_Value[1])//实际值与中立值的差值大于幅度值
			{
				MACHINE_EXAMPLE.Eexercise_Step_State = STEP2;//进入第二步
//				printf("Go to step2\r\n");
//				Uart_printf(DEBUG_Uart,DEBUG_COM,"Go to step2\r\nvalue:%d\r\nEexercise_Value[0]:%d\r\n   \
//				            MACHINE_EXAMPLE.Eexercise_Value[1]:%d\r\nEexercise_Value[2]:%d\r\n",
//				            value,MACHINE_EXAMPLE.Eexercise_Value[0],MACHINE_EXAMPLE.Eexercise_Value[1],
//				            MACHINE_EXAMPLE.Eexercise_Value[2]);	  
				return STEP2;
			}
			else
			{
//				printf("Keep step1\r\n");
//				Uart_printf(DEBUG_Uart,DEBUG_COM,"Keep step1\r\nvalue:%d\r\nEexercise_Value[0]:%d\r\n   \
//				            MACHINE_EXAMPLE.Eexercise_Value[1]:%d\r\nEexercise_Value[2]:%d\r\n",
//				            value,MACHINE_EXAMPLE.Eexercise_Value[0],MACHINE_EXAMPLE.Eexercise_Value[1],
//				            MACHINE_EXAMPLE.Eexercise_Value[2]);	
				return STEP1;
			}
		}
		case STEP2:
		{
			MACHINE_EXAMPLE.Eexercise_Value[3] = value - MACHINE_EXAMPLE.Eexercise_Value[0];
			if(MACHINE_EXAMPLE.Eexercise_Value[3] < 0) MACHINE_EXAMPLE.Eexercise_Value[3] = -MACHINE_EXAMPLE.Eexercise_Value[3];
			if(MACHINE_EXAMPLE.Eexercise_Value[3] < MACHINE_EXAMPLE.Eexercise_Value[1])//实际值与中立值的差值小于幅度值
			{
				MACHINE_EXAMPLE.Eexercise_Step_State = STEP1;//进入第一步,运动次数加1
				MACHINE_EXAMPLE.Eexercise_Count++;
//				printf("Go to step1\r\n");
//				Uart_printf(DEBUG_Uart,DEBUG_COM,"Go to step1,Eexercise_Count:%d\r\nvalue:%d\r\nEexercise_Value[0]:%d\r\n   \
//				            MACHINE_EXAMPLE.Eexercise_Value[1]:%d\r\nEexercise_Value[2]:%d\r\n",
//				            MACHINE_EXAMPLE.Eexercise_Count,value,MACHINE_EXAMPLE.Eexercise_Value[0],
//				            MACHINE_EXAMPLE.Eexercise_Value[1],MACHINE_EXAMPLE.Eexercise_Value[2]);	  
				return STEP1;
			}
			else
			{
//				printf("Keep step2\r\n");
//				Uart_printf(DEBUG_Uart,DEBUG_COM,"Keep step2\r\nvalue:%d\r\nEexercise_Value[0]:%d\r\n   \
//				            MACHINE_EXAMPLE.Eexercise_Value[1]:%d\r\nEexercise_Value[2]:%d\r\n",
//				            value,MACHINE_EXAMPLE.Eexercise_Value[0],MACHINE_EXAMPLE.Eexercise_Value[1],
//				            MACHINE_EXAMPLE.Eexercise_Value[2]);	
				return STEP2;
			}
		}
		default:
			return 0;
	}
}

这段代码实现了一个 基于状态机的运动计数功能，主要用于检测周期性动作（如俯卧撑、仰卧起坐等）。其核心逻辑是通过 俯仰角（或传感器数值）的变化幅度 来判断是否完成一次完整动作。下面是对代码的详细解析和改进建议：

2-1. 代码功能分析

核心逻辑

  • 状态机设计：

    • STEP1：检测动作是否开始（当前值与中立值的差值超过阈值）。

    • STEP2：检测动作是否结束（差值回落到阈值以下）。

    • 每次从 STEP2 回到 STEP1 时，计数 Eexercise_Count 加 1。

关键变量

  • Eexercise_Value[0]：中立值（初始基准值）。

  • Eexercise_Value[1]：幅度阈值（触发动作的最小变化量）。

  • Eexercise_Value[3]：当前值与中立值的绝对差值。

输入参数

  • value：实时传感器数据（如 MPU6050 的俯仰角或其他运动参数）。

2-2. 改进建议

(1) 增加去抖动处理

  • 问题：传感器噪声可能导致状态在阈值附近频繁切换，误触发计数。

  • 改进：在状态切换时加入 时间延迟判断（例如：需连续 3 次检测到超阈值才切换状态）。

     
    // 示例：STEP1 到 STEP2 的改进
static uint8_t debounce_counter = 0;
if (MACHINE_EXAMPLE.Eexercise_Value[3] > MACHINE_EXAMPLE.Eexercise_Value[1]) {
    debounce_counter++;
    if (debounce_counter >= 3) {  // 连续3次超阈值才切换
        MACHINE_EXAMPLE.Eexercise_Step_State = STEP2;
        debounce_counter = 0;
    }
} else {
    debounce_counter = 0;
}

(2) 动态校准中立值

  • 问题：初始中立值 Eexercise_Value[0] 可能因传感器漂移或用户姿势变化失效。

  • 改进：在空闲时（如检测到长时间无运动）自动更新中立值：

     
    if (no_motion_detected()) {
    MACHINE_EXAMPLE.Eexercise_Value[0] = value;  // 动态校准
}

(3) 区分运动方向（可选）

  • 场景：若需区分“抬头”和“低头”动作（如仰卧起坐的正向和反向）。

  • 改进：保留差值的符号，并根据方向计数：

     
    int16_t delta = value - MACHINE_EXAMPLE.Eexercise_Value[0];
if (delta > 0) 正向运动;
else if (delta < 0) 反向运动;

(4) 优化调试输出

  • 问题：注释掉的 printf 调试代码影响可读性。

  • 改进：使用宏控制调试输出：

    #define DEBUG_MODE 1
#if DEBUG_MODE
    printf("Go to step2, value=%d\n", value);
#endif

2-3. 完整改进代码片段

uint8_t Count_Exercise(uint16_t value) {
    static uint8_t debounce_counter = 0;
    MACHINE_EXAMPLE.Eexercise_Value[3] = abs(value - MACHINE_EXAMPLE.Eexercise_Value[0]);

    switch(MACHINE_EXAMPLE.Eexercise_Step_State) {
        case STEP1:
            if (MACHINE_EXAMPLE.Eexercise_Value[3] > MACHINE_EXAMPLE.Eexercise_Value[1]) {
                debounce_counter++;
                if (debounce_counter >= 3) {  // 去抖动
                    MACHINE_EXAMPLE.Eexercise_Step_State = STEP2;
                    debounce_counter = 0;
                    return STEP2;
                }
            } else {
                debounce_counter = 0;
            }
            return STEP1;

        case STEP2:
            if (MACHINE_EXAMPLE.Eexercise_Value[3] < MACHINE_EXAMPLE.Eexercise_Value[1]) {
                debounce_counter++;
                if (debounce_counter >= 3) {  // 去抖动
                    MACHINE_EXAMPLE.Eexercise_Step_State = STEP1;
                    MACHINE_EXAMPLE.Eexercise_Count++;
                    debounce_counter = 0;
                    return STEP1;
                }
            } else {
                debounce_counter = 0;
            }
            return STEP2;

        default:
            return 0;
    }
}

总结

  • 当前代码：通过简单阈值和状态机实现运动计数，适合低噪声场景。

  • 改进方向：去抖动、动态校准、方向区分可提高鲁棒性。

  • 扩展性：可结合陀螺仪数据或卡尔曼滤波优化角度检测精度。

 

 

 

 扩展：其他方案

要根据 MPU6050 测得的俯仰角（Pitch）变化 来判断一次运动是否发生，可以结合 角度变化幅度、变化速率（角速度）和时间连续性 进行分析。以下是具体方法：

1. 设定阈值检测（静态→动态）

  • 原理：当俯仰角的变化超过设定的阈值时，认为发生了运动。

  • 步骤：

    1. 计算角度变化量：

      • 记录初始俯仰角 θ0（静止状态下的基准值）。

      • 实时监测当前俯仰角 θt，计算变化量 Δθ=∣θt−θ0∣。

    2. 设定角度阈值：

      • 例如：Δθ>5°（具体阈值需根据应用场景调整，避免噪声误触发）。

    3. 触发条件：

      • 当 Δθ 持续超过阈值一段时间（如 100ms），判定为有效运动。

2. 结合角速度（动态检测）

  • 原理：仅用角度可能响应较慢，可配合陀螺仪的 Y轴角速度（Pitch 方向角速度） 提高灵敏度。

  • 步骤：

    1. 读取陀螺仪的 Y 轴角速度 ωy（单位：°/s）。

    2. 设定角速度阈值（如 ∣ωy∣>10°/s）。

    3. 联合判断：

      • 若 Δθ>阈值 且 ωy>阈值，则判定为运动发生。

3. 时间窗口滤波（抗噪声）

  • 原理：避免瞬时噪声（如振动）误触发。

  • 方法：

    • 在 固定时间窗口（如 200ms） 内，若俯仰角变化持续超过阈值，才判定为运动。

    • 示例代码逻辑（伪代码）：

      python
      复制
      if abs(current_pitch - initial_pitch) > threshold:
    motion_counter += 1
    if motion_counter > time_window:
        trigger_motion_event()
else:
    motion_counter = 0

4. 运动方向判断（可选）

  • 通过俯仰角变化的 符号（正/负） 区分运动方向：

    • 正角度变化：抬头/向后倾斜（如从水平到仰头）。

    • 负角度变化：低头/向前倾斜（如从水平到俯身）。

5. 实际应用注意事项

  1. 校准初始状态：

    • 启动时需静止 1~2 秒，校准初始俯仰角 θ0（消除零偏）。

  2. 动态阈值调整：

    • 在高速运动场景中，可适当提高阈值；高精度场景则降低阈值。

  3. 传感器融合：

    • 若需更高可靠性，可结合加速度计和陀螺仪数据（互补滤波或卡尔曼滤波），减少漂移影响。

示例场景

  • 点头检测：

    • 俯仰角快速变化（如 Δθ>15°）且角速度 ωy>30°/s，判定为一次点头动作。

  • 跌倒检测：

    • 俯仰角持续增大（如 Δθ>60°）并保持，可能表示跌倒。

总结

通过 俯仰角变化量 + 角速度 + 时间滤波 的组合策略，可以可靠地检测运动事件。具体参数需根据实际传感器噪声、运动速度和环境干扰进行调优。

 
 
 
posted @ 2025-03-26 17:35  FBshark  阅读(189)  评论(0)    收藏  举报