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

这篇具有很好参考价值的文章主要介绍了依据MPU6050角速度原始数据的计步算法。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

        首先我要吐槽一下InvenSense的DMP,很坑。没有什么特别需求的话,不要往nRF52832上移植,吃力不讨好。

一、简介

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

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

二、步态分析

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

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

        其中摆动中相就是走路过程中两腿并排的那一刻,此时角速度最大[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 (骤减现象);

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

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

        

三、计步算法

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

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

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

        ①全局变量定义

#define ABS(a) (0 - (a)) > 0 ? (-(a)) : (a)   //取a的绝对值
#define SAMPLE_NUM                10          //采样10次取平均值
#define MIN_RELIABLE_VARIATION    500         //最小可信赖变化量
#define MAX_RELIABLE_VARIATION    5000        //最大可信赖变化量

//三轴数据
typedef struct
{
	int16_t X;
	int16_t Y;
	int16_t Z;
}axis_value_t;              
axis_value_t old_ave_GyroValue, ave_GyroValue;
//极值数据
typedef struct
{
	axis_value_t max;
	axis_value_t min;
}peak_value_t;
peak_value_t peak_value;

         ②采样程序

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

void Gyro_sample_update(void)
{
	axis_value_t GyroValue;
	axis_value_t change;
	int sum[3] = {0};
	uint8_t success_num = 0;

	//保存上一次测量的原始数据
	old_ave_GyroValue.X = ave_GyroValue.X;
	old_ave_GyroValue.Y = ave_GyroValue.Y;
	old_ave_GyroValue.Z = ave_GyroValue.Z;

	//多次测量取平均值
	for(uint8_t i = 0; i < SAMPLE_NUM; i++)
	{
		if(MPU6050_ReadGyro(&GyroValue.X , &GyroValue.Y , &GyroValue.Z ) == true)
		{
			sum[0] += GyroValue.X;
			sum[1] += GyroValue.Y;
			sum[2] += GyroValue.Z;
			success_num ++;
		}
	}
	ave_GyroValue.X = sum[0] / success_num;
	ave_GyroValue.Y = sum[1] / success_num;
	ave_GyroValue.Z = sum[2] / success_num;

	//原始数据变化量
	change.X = ABS(ave_GyroValue.X - old_ave_GyroValue.X);
	change.Y = ABS(ave_GyroValue.Y - old_ave_GyroValue.Y);
	change.Z = ABS(ave_GyroValue.Z - old_ave_GyroValue.Z);
	
	//如果变化量超出可接受的变化值,则将原始数据退回到上一次的大小
	if(change.X < MIN_RELIABLE_VARIATION || change.X > MAX_RELIABLE_VARIATION)
	{
		ave_GyroValue.X = old_ave_GyroValue.X;
	}
	if(change.Y < MIN_RELIABLE_VARIATION || change.Y > MAX_RELIABLE_VARIATION)
	{
		ave_GyroValue.Y = old_ave_GyroValue.Y;
	}	
	if(change.Z < MIN_RELIABLE_VARIATION || change.Z > MAX_RELIABLE_VARIATION)
	{
		ave_GyroValue.Z = old_ave_GyroValue.Z;
	}

	//分别保存三轴角速度原始数据的最大值和最小值
	peak_value.max.X = MAX(peak_value.max.X , ave_GyroValue.X);
	peak_value.min.X = MIN(peak_value.min.X , ave_GyroValue.X);
	peak_value.max.Y = MAX(peak_value.max.Y , ave_GyroValue.Y);
	peak_value.min.Y = MIN(peak_value.min.Y , ave_GyroValue.Y);
	peak_value.max.Z = MAX(peak_value.max.Z , ave_GyroValue.Z);
	peak_value.min.Z = MIN(peak_value.min.Z , ave_GyroValue.Z);
}

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

    2、判断最活跃轴

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

        ①全局变量定义

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

        ②最活跃轴判断程序

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

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

void which_is_active(void)
{
	axis_value_t change;
	static axis_value_t active;        //三个轴的活跃度权重
	static uint8_t active_sample_num; 
 
	Gyro_sample_update();
	active_sample_num ++;

	//每隔一段时间,比较一次权重大小,判断最活跃轴
	if(active_sample_num >= ACTIVE_NUM)
	{
		if(active.X > active.Y && active.X > active.Z)
		{
			most_active_axis = ACTIVE_X;
		}
		else if(active.Y > active.X && active.Y > active.Z)
		{
			most_active_axis = ACTIVE_Y;
		}
		else if(active.Z > active.X && active.Z > active.Y)
		{
			most_active_axis = ACTIVE_Z;
		}
		else
		{
			most_active_axis = ACTIVE_NULL;
		}
		active_sample_num = 0;
		active.X = 0;
		active.Y = 0;
		active.Z = 0;
	}

	//原始数据变化量
	change.X = ABS(ave_GyroValue.X - old_ave_GyroValue.X);
	change.Y = ABS(ave_GyroValue.Y - old_ave_GyroValue.Y);
	change.Z = ABS(ave_GyroValue.Z - old_ave_GyroValue.Z);

	//增加三轴活跃度权重
	if(change.X > change.Y && change.X > change.Z)
	{
		active.X ++;
	}
	else if(change.Y > change.X && change.Y > change.Z)
	{
		active.Y ++;
	}
	else if(change.Z > change.X && change.Z > change.Y)
	{
		active.Z ++;
	}
}

      3、初步计步

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

        ①全局变量定义

uint16_t step_count;

         ②计步程序

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

void detect_step(void)
{
	int16_t mid;
	which_is_active();
	switch(most_active_axis)
	{
		case ACTIVE_NULL:
			break;
		//捕捉原始数据骤增和骤减现象
		case ACTIVE_X:
			mid = (peak_value.max.X + peak_value.min.X) / 2;
			if(old_ave_GyroValue.X < mid && ave_GyroValue.X > mid)
			{
				step_count ++;
			}
			else if(old_ave_GyroValue.X > mid && ave_GyroValue.X < mid)
			{
				step_count ++;
			}
			break;
		case ACTIVE_Y:
			mid = (peak_value.max.Y + peak_value.min.Y) / 2;
			if(old_ave_GyroValue.Y < mid && ave_GyroValue.Y > mid)
			{
				step_count ++;
			}
			else if(old_ave_GyroValue.Y > mid && ave_GyroValue.Y < mid)
			{
				step_count ++;
			}
			break;	
		case ACTIVE_Z:
			mid = (peak_value.max.Z + peak_value.min.Z) / 2;
			if(old_ave_GyroValue.Z < mid && ave_GyroValue.Z > mid)
			{
				step_count ++;
			}
			else if(old_ave_GyroValue.Z > mid && ave_GyroValue.Z < mid)
			{
				step_count ++;
			}
			break;
		default:
			break;
	}
}

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

    4、最终步数

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

uint16_t step;

void timer3_handler(nrf_timer_event_t event_type, void* p_context)
{
	static uint8_t step_time_count = 0;
	switch(event_type)
	{
		case NRF_TIMER_EVENT_COMPARE0:
			
			detect_step();
			step_time_count ++;
			if(step_time_count == 6)   //300ms
			{
				step_time_count = 0;
				if(step_count != 0)
				{
					step_count = 0;
					step ++;
				}
			}
			break;
		default:
			break;
	}
}

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

参考文献:[1] 李江慧,连春快,李玉榕 . 基于惯性传感器的穿戴式步态分析系统设计与实现[J] . 电气技术,2021(9):14-21文章来源地址https://www.toymoban.com/news/detail-449185.html

到了这里,关于依据MPU6050角速度原始数据的计步算法的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • [足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-4(1) 刚体的速度与角速度

    本文仅供学习使用,总结很多本现有讲述运动学或动力学书籍后的总结,从矢量的角度进行分析,方法比较传统,但更易理解,并且现有的看似抽象方法,两者本质上并无不同。 2024年底本人学位论文发表后方可摘抄 若有帮助请引用 本文参考: . 食用方法 求解逻辑:速度与

    2024年02月02日
    浏览(96)
  • 【stm32】stm32学习笔记(江科大)-详解stm32获取Mpu6050陀螺仪和加速度

    目录 I2C 起始条件: 终止条件:  发送一个字节 接收一个字节  接收发送应答  代码 I2C I2C.C I2C.h Mpu6050 Mpu6050.c Mpu6050.h Mpu6050Reg.h main.c 结果   要想获取Mpu6050陀螺仪和加速度那就需要了解一下Mpu6050。Mpu6050使用的是I2C通讯 先了解一下 起始条件: SCL高电平期间,SDA从高电平切换

    2024年02月16日
    浏览(67)
  • 【机器人学习】MPU6050数据的换算

    由于近期在搞IMU的姿态解算,需要用一个IMU传感器去进行角度的获取,因此采用了最实惠的MPU6050!通过IIC读取MPU6050后的数据只是简单的数值,并没有物理意义,因此本文主要记录下如何将MPU6050的数据换算成有意义的物理值。 要得到MPU6050的具体参数,那必须是官方的datashe

    2024年02月14日
    浏览(45)
  • 【物联网】MPU6050传感器数据采集与滤波算法

    当谈到嵌入式电子设备和机器人的姿态控制和运动检测时,MPU6050往往是一个备受关注的传感器模块。它是一款小巧但功能强大的六轴传感器,集成了三轴加速度计和三轴陀螺仪。在本博客中,我们将详细介绍MPU6050的特点、工作原理以及与stm32配合的使用方法,后面看情况更新

    2024年02月14日
    浏览(46)
  • 【MPU6050能读ID 但读数据值为零(一个硬件问题)】

            近期自己画一个MPU6050模块,电路设计只实现基本读取数据功能。在网上参考了不少的电路原理图,都大同小异,在一些电容电阻的取值有些许不同。笔者在实现电路时遇到了MPU6050能读ID但读值为零的硬件电路问题,经过许久调试最终能够读取数据值,在这里做个记录

    2024年02月15日
    浏览(55)
  • STM32的MPU6050卡尔曼滤波融合数据控制平衡车

    最近学习卡尔曼滤波的方法来融合MPU6050的加速度计所得的角度以及陀螺仪的角速度数据。一开始去B站搜视频看原理,然后找到CSDN上的一篇博客,参考了初始值的设定后写了下面的滤波算法。 该up主视频讲的很好,视频链接:精通(教你从理论到实践)_哔哩哔哩_bilibili 算法

    2024年04月15日
    浏览(41)
  • MPU6050(读取原数据、移植DMP、stm32f4、HAL库、KEIL5)

    记录一下自己遇到的问题及解决方法,希望能帮助到一些人。 第一步,读取芯片的原始数据。需要注意两点:1、对HAL库提供的IIC读取写入函数进行再包装。(千万不要觉的这步多此一举,后面移植DMP时用得到) 2、芯片的地址(这里面有俩坑)第一就是,芯片的 I2C 设备地址

    2023年04月08日
    浏览(54)
  • STM32MPU6050角度的读取(STM32驱动MPU6050)

    注:文末附STM32驱动MPU6050代码工程链接,需要的读者请自取。 MPU6050是一款集成了三轴陀螺仪和三轴加速度计的传感器芯片,由英国飞利浦半导体(现为恩智浦半导体)公司生产。它通过电子接口(如I2C或SPI)与微控制器进行通信,可用于测量物体的加速度和角速度,广泛应

    2024年02月20日
    浏览(54)
  • 【STM32+cubemx】0027 HAL库开发:MPU6050陀螺仪和加速度计数据的获取和校准

    在制作平衡车或者飞行器时,不可避免地需要知道设备本身的姿态,一般我们使用陀螺仪和加速度计来获取这些信息。 陀螺仪用来测量物体的角度。传统的机械式陀螺的原理,和我们小时候玩的陀螺一样,是利用了高速旋转的物体能保持轴线稳定的特性;机械式陀螺需要的加

    2023年04月08日
    浏览(50)
  • 学习记录之STM32F103C8T6最小系统板驱动MPU6050串口打印数据

    1.使用到的工具介绍 2.MPU6050和整体和简单介绍 3.程序的介绍 1.使用到的工具介绍 硬件方面:STM32F103C8T6最小系统板核心板,MPU6050模块三维角度传感器,经典的CH340烧写和串口作用,和若干个杜邦线。 软件方面:keil5编写程序软件,烧写软件FlyMcu.exe烧写工具,sscom.exe串口调试工

    2023年04月09日
    浏览(62)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包