devbox
小小林熬夜学编程
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

依据MPU6050角速度原始数据的计步算法_mpu6050计步算法

作者：小小林熬夜学编程 | 2024-06-11 18:52:37

赞

踩

mpu6050计步算法

 

一、简介

        依据MPU6050的角速度原始数据计算佩戴者步数，由于依据的是角速度，只适用与手环或者腿环等设备。本项目的主控芯片是nRF52832（SDK：Nordic SDK 17.0.2.），但算法通用，读取原始数据的完整工程来自艾克姆，已上传个人主页。

        不知为何，在nRF53832的带BLE功能的工程中，读取MPU6050的原始数据频率如果过快的话MCU会卡死。本算法仅需50ms读取一次原始数据即可，对CPU压力较小。

二、步态分析

        人在行走过程中，腿部垂直于人体矢状面方向的角速度变化最为明显，且具有一定的规律性，可以通过这一规律性来判断步数。角速度在行走过程之中的大致变化曲线如图一所示：

图一

        其中摆动中相就是走路过程中两腿并排的那一刻，此时角速度最大[1]。由图可以看出在每一步中，角速度有一个骤增和一个骤减的过程最具有标志性，若是能够通过算法捕获到这个现象，就可以实现计步功能了。

        大致思路如下：

        MCU配置一个50ms定时器，每50ms读取一下当前角速度原始数据(Gyro_new_sample)，并将上一个50ms的原始数据保存(Gyro_old_sample)。同时实时更新出现过的角速度原始数据最大值(Gyro_max)、最小值(Gyro_min)和二者平均值(Gyro_mid)。一旦出现下述两种情况：

        1.Gyro_old_sample < Gyro_mid < Gyro_new_sample (骤增现象);

        2.Gyro_new_sample < Gyro_mid < Gyro_old_sample (骤减现象);

        就记为一次有效的走步，如图二所示。

图二

        

三、计步算法

    1、均值滤波采样、可信赖变化量、记录MAX和MIN

        单次采样数据必然不可信，所以要多次采样取平均值。

        取完平均值后的数据依然不一定是可信的数据，如果本次采样的数据和上一个50ms采样的数据差值过小或者过大，应当不予采纳，需要设置可信赖变化量的下限和上限，上限和下限的值需要开发者根据自己MCU的实际情况进行设置，这里的值仅作参考。

        ①全局变量定义

  1. #define ABS(a) (0 - (a)) > 0 ? (-(a)) : (a)   //取a的绝对值
  2. #define SAMPLE_NUM                10          //采样10次取平均值
  3. #define MIN_RELIABLE_VARIATION    500         //最小可信赖变化量
  4. #define MAX_RELIABLE_VARIATION    5000        //最大可信赖变化量
  5.  
  6. //三轴数据
  7. typedef struct
  8. {
  9. 	int16_t X;
  10. 	int16_t Y;
  11. 	int16_t Z;
  12. }axis_value_t;              
  13. axis_value_t old_ave_GyroValue, ave_GyroValue;
  14. //极值数据
  15. typedef struct
  16. {
  17. 	axis_value_t max;
  18. 	axis_value_t min;
  19. }peak_value_t;
  20. peak_value_t peak_value;

         ②采样程序

        大致过程是：保存上一次采样数据 → 均值采样本次数据 → 计算本次和上次的差值 → 检验差值大小(若超限则将本次数据回退到上一次的大小) → 保存最大值和最小值 。这个函数50ms会被调用一次。

  1. void Gyro_sample_update(void)
  2. {
  3. 	axis_value_t GyroValue;
  4. 	axis_value_t change;
  5. 	int sum[3] = {0};
  6. 	uint8_t success_num = 0;
  7.  
  8. 	//保存上一次测量的原始数据
  9. 	old_ave_GyroValue.X = ave_GyroValue.X;
  10. 	old_ave_GyroValue.Y = ave_GyroValue.Y;
  11. 	old_ave_GyroValue.Z = ave_GyroValue.Z;
  12.  
  13. 	//多次测量取平均值
  14. 	for(uint8_t i = 0; i < SAMPLE_NUM; i++)
  15. 	{
  16. 		if(MPU6050_ReadGyro(&GyroValue.X , &GyroValue.Y , &GyroValue.Z ) == true)
  17. 		{
  18. 			sum[0] += GyroValue.X;
  19. 			sum[1] += GyroValue.Y;
  20. 			sum[2] += GyroValue.Z;
  21. 			success_num ++;
  22. 		}
  23. 	}
  24. 	ave_GyroValue.X = sum[0] / success_num;
  25. 	ave_GyroValue.Y = sum[1] / success_num;
  26. 	ave_GyroValue.Z = sum[2] / success_num;
  27.  
  28. 	//原始数据变化量
  29. 	change.X = ABS(ave_GyroValue.X - old_ave_GyroValue.X);
  30. 	change.Y = ABS(ave_GyroValue.Y - old_ave_GyroValue.Y);
  31. 	change.Z = ABS(ave_GyroValue.Z - old_ave_GyroValue.Z);
  32. 	
  33. 	//如果变化量超出可接受的变化值，则将原始数据退回到上一次的大小
  34. 	if(change.X < MIN_RELIABLE_VARIATION || change.X > MAX_RELIABLE_VARIATION)
  35. 	{
  36. 		ave_GyroValue.X = old_ave_GyroValue.X;
  37. 	}
  38. 	if(change.Y < MIN_RELIABLE_VARIATION || change.Y > MAX_RELIABLE_VARIATION)
  39. 	{
  40. 		ave_GyroValue.Y = old_ave_GyroValue.Y;
  41. 	}	
  42. 	if(change.Z < MIN_RELIABLE_VARIATION || change.Z > MAX_RELIABLE_VARIATION)
  43. 	{
  44. 		ave_GyroValue.Z = old_ave_GyroValue.Z;
  45. 	}
  46.  
  47. 	//分别保存三轴角速度原始数据的最大值和最小值
  48. 	peak_value.max.X = MAX(peak_value.max.X , ave_GyroValue.X);
  49. 	peak_value.min.X = MIN(peak_value.min.X , ave_GyroValue.X);
  50. 	peak_value.max.Y = MAX(peak_value.max.Y , ave_GyroValue.Y);
  51. 	peak_value.min.Y = MIN(peak_value.min.Y , ave_GyroValue.Y);
  52. 	peak_value.max.Z = MAX(peak_value.max.Z , ave_GyroValue.Z);
  53. 	peak_value.min.Z = MIN(peak_value.min.Z , ave_GyroValue.Z);
  54. }

       注：MPU6050_ReadGyro(&GyroValue.X , &GyroValue.Y , &GyroValue.Z )是读取三轴角速度原始数据的函数，将数据保存在GyroValue中；succee_num是用来防止这个函数返回的是false从而影响结果。（但实际采用过程中大概率是不会采样失败的，可以删去，直接除以SAMPLE_NUM就行）

    2、判断最活跃轴

         只有知道MPU6050哪个轴是更接近垂直于人体矢状面的，才能确定究竟要使用哪个轴的原始数据进行计步。

        ①全局变量定义

  1. #define ACTIVE_NUM          30            //最活跃轴更新周期
  2. #define ACTIVE_NULL         0             //最活跃轴未知
  3. #define ACTIVE_X            1             //最活跃轴是X
  4. #define ACTIVE_Y            2             //最活跃轴是Y
  5. #define ACTIVE_Z            3             //最活跃轴是Z
  6. uint8_t most_active_axis = ACTIVE_NULL;   //记录最活跃轴

        ②最活跃轴判断程序

        这个函数也是50ms被调用一次。

        每一次被调用时，都会计算一次change值，也就是上一次和这一次原始数据的差值，然后比较这三个差值的大小，增加最大差值轴的活跃度权重。这里是每1.5秒(由ACTIVE_NUM决定)比较一次权重值，权重最大的轴就是最活跃轴，然后把权重都清零，下一个1.5秒再重新判断一次。

  1. void which_is_active(void)
  2. {
  3. 	axis_value_t change;
  4. 	static axis_value_t active;        //三个轴的活跃度权重
  5. 	static uint8_t active_sample_num; 
  6.  
  7. 	Gyro_sample_update();
  8. 	active_sample_num ++;
  9.  
  10. 	//每隔一段时间，比较一次权重大小，判断最活跃轴
  11. 	if(active_sample_num >= ACTIVE_NUM)
  12. 	{
  13. 		if(active.X > active.Y && active.X > active.Z)
  14. 		{
  15. 			most_active_axis = ACTIVE_X;
  16. 		}
  17. 		else if(active.Y > active.X && active.Y > active.Z)
  18. 		{
  19. 			most_active_axis = ACTIVE_Y;
  20. 		}
  21. 		else if(active.Z > active.X && active.Z > active.Y)
  22. 		{
  23. 			most_active_axis = ACTIVE_Z;
  24. 		}
  25. 		else
  26. 		{
  27. 			most_active_axis = ACTIVE_NULL;
  28. 		}
  29. 		active_sample_num = 0;
  30. 		active.X = 0;
  31. 		active.Y = 0;
  32. 		active.Z = 0;
  33. 	}
  34.  
  35. 	//原始数据变化量
  36. 	change.X = ABS(ave_GyroValue.X - old_ave_GyroValue.X);
  37. 	change.Y = ABS(ave_GyroValue.Y - old_ave_GyroValue.Y);
  38. 	change.Z = ABS(ave_GyroValue.Z - old_ave_GyroValue.Z);
  39.  
  40. 	//增加三轴活跃度权重
  41. 	if(change.X > change.Y && change.X > change.Z)
  42. 	{
  43. 		active.X ++;
  44. 	}
  45. 	else if(change.Y > change.X && change.Y > change.Z)
  46. 	{
  47. 		active.Y ++;
  48. 	}
  49. 	else if(change.Z > change.X && change.Z > change.Y)
  50. 	{
  51. 		active.Z ++;
  52. 	}
  53. }

      3、初步计步

        一切准备就绪，接下来要捕获原始数据骤增和骤减现象了。

        ①全局变量定义

uint16_t step_count;

         ②计步程序

        取最大值和最小值的均值mid，每当出现图二中的情况，则算作一次有效走步。

  1. void detect_step(void)
  2. {
  3. 	int16_t mid;
  4. 	which_is_active();
  5. 	switch(most_active_axis)
  6. 	{
  7. 		case ACTIVE_NULL:
  8. 			break;
  9. 		//捕捉原始数据骤增和骤减现象
  10. 		case ACTIVE_X:
  11. 			mid = (peak_value.max.X + peak_value.min.X) / 2;
  12. 			if(old_ave_GyroValue.X < mid && ave_GyroValue.X > mid)
  13. 			{
  14. 				step_count ++;
  15. 			}
  16. 			else if(old_ave_GyroValue.X > mid && ave_GyroValue.X < mid)
  17. 			{
  18. 				step_count ++;
  19. 			}
  20. 			break;
  21. 		case ACTIVE_Y:
  22. 			mid = (peak_value.max.Y + peak_value.min.Y) / 2;
  23. 			if(old_ave_GyroValue.Y < mid && ave_GyroValue.Y > mid)
  24. 			{
  25. 				step_count ++;
  26. 			}
  27. 			else if(old_ave_GyroValue.Y > mid && ave_GyroValue.Y < mid)
  28. 			{
  29. 				step_count ++;
  30. 			}
  31. 			break;	
  32. 		case ACTIVE_Z:
  33. 			mid = (peak_value.max.Z + peak_value.min.Z) / 2;
  34. 			if(old_ave_GyroValue.Z < mid && ave_GyroValue.Z > mid)
  35. 			{
  36. 				step_count ++;
  37. 			}
  38. 			else if(old_ave_GyroValue.Z > mid && ave_GyroValue.Z < mid)
  39. 			{
  40. 				step_count ++;
  41. 			}
  42. 			break;
  43. 		default:
  44. 			break;
  45. 	}
  46. }

        但是到这里还没有结束，step_count作为最终结果不够严谨，需要根据人类实际的走步速度，再对步数进行一次调整。

    4、最终步数

        这是nRF52832的一个50ms定时器回调函数，只需关注核心部分：detect_step() 每50ms被调用一次，正常人走路1秒内不会超过3步，所以每300ms查看一次step_count 是不是为0 ，只要不是0 ，无论是多大都只算作1步，step就是最终的步数。

  1. uint16_t step;
  2.  
  3. void timer3_handler(nrf_timer_event_t event_type, void* p_context)
  4. {
  5. 	static uint8_t step_time_count = 0;
  6. 	switch(event_type)
  7. 	{
  8. 		case NRF_TIMER_EVENT_COMPARE0:
  9. 			
  10. 			detect_step();
  11. 			step_time_count ++;
  12. 			if(step_time_count == 6)   //300ms
  13. 			{
  14. 				step_time_count = 0;
  15. 				if(step_count != 0)
  16. 				{
  17. 					step_count = 0;
  18. 					step ++;
  19. 				}
  20. 			}
  21. 			break;
  22. 		default:
  23. 			break;
  24. 	}
  25. }

 参考： (16条消息) 基于三轴加速度传感器的计步算法_Dancer__Sky的博客-CSDN博客_加速度计步算法

参考文献：[1] 李江慧，连春快，李玉榕 . 基于惯性传感器的穿戴式步态分析系统设计与实现[J] . 电气技术，2021（9）：14-21

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小小林熬夜学编程/article/detail/704392
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号