首先我要吐槽一下InvenSense的DMP,很坑。没有什么特别需求的话,不要往nRF52832上移植,吃力不讨好。
一、简介
依据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的实际情况进行设置,这里的值仅作参考。
①全局变量定义
#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博客_加速度计步算法文章来源:https://www.toymoban.com/news/detail-449185.html
参考文献:[1] 李江慧,连春快,李玉榕 . 基于惯性传感器的穿戴式步态分析系统设计与实现[J] . 电气技术,2021(9):14-21文章来源地址https://www.toymoban.com/news/detail-449185.html
到了这里,关于依据MPU6050角速度原始数据的计步算法的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!