岑世欣,高振斌,于 明,曾 成
(河北工业大学 电子信息工程学院,天津 300401)
随着微机电系统(MEMS)的兴起,许多低成本的MEMS惯性传感器出现,这极大地促进了惯性导航系统(INS)在民用领域的发展。但MEMS惯性传感器存在严重的漂移误差,在低成本的MEMS惯性传感器上的误差表现尤为明显。这些误差会随着时间的变化不断积累,导致系统长时间精度较差,这是惯性导航技术面临的最大难题。利用零速修正技术抑制累积误差可以提高导航系统的精确性,其中零速修正技术包括零速区间检测和零速误差补偿,零速区间检测的精确性直接影响着零速修正的效果,是提高INS定位精度的重要一环[1]。
传统零速检测算法一般采用加速度和角速度平方和[1-3]或加速度滑动窗口方差[4]作为检测数据,然后利用固定阈值[1]检测零速区间。固定阈值法在固定运动步态条件下能实现零速区间的判断,但对于不同行人或不同运动状态适应性较差。除固定阈值法外,目前典型的零速检测方法主要有:
1) 多步态检测法[5-6]。它通过判断步态模式调节阈值检测零速区间,但对于非正常的步态模式检测效果较差。
2) 经验阈值法[7-8]。它根据加速度[7]和零速检测阈值或运动频率[8]与检测阈值的关系调节阈值,实现阈值自适应的效果,但需要大量的数据分析。
3) 机器学习方法[9]。它利用隐马尔可夫模型(HMM)对数据进行判断,但同样需要大量数据训练模型参数,且复杂度较高[1]。
针对上述问题,本文提出了一种基于自适应阈值的零速检测算法,通过安装在足部的MEMS惯性传感器采集数据,以合加速度A(k)、合角速度W(k)作为检测样本数据,利用滑动极值滤波器对数据滤波,将滤波后的检测数据映射到频数空间,分析其统计特征提取阈值,通过插值波峰检测法获得步态频率信息及峰值频数,以判断运动状态变化,继而更新滤波窗口长度得到新阈值来实现不同个体以及不同运动状态下的零速区间检测阈值的自适应性。
本文提出的基于自适应阈值的零速检测算法的结构框图如图1所示。本节将分别对自适应阈值的提取过程、更新过程以及整体算法流程进行介绍。
图1 基于自适应阈值的零速检测算法框图
人在步行时的步态过程如图2所示。根据零速检测原理,当运动到第三阶段时,整个足部与地面有短时间的接触,称为零速静止阶段[7]。在零速静止阶段时,足部速度为0。因此,此刻的合角速度值及去除重力分量的合加速度值在零速静止阶段都为0。当检测到零速区间,将速度误差重置,这种算法被称为零速更新算法[10],实现性能依赖于零速区间的检测精度。
图2 正常步态过程
A(k)、W(k)都能有效表现出步态过程中的零速静止阶段,合角速度包含更清晰的步态频率信息[1]。A(k)、W(k)分别为
(1)
(2)
式中:ax(k)、ay(k)、az(k)为惯性传感器的k时刻的三轴加速度值;wx(k)、wy(k)、wz(k)为惯性传感器k时刻的三轴角速度值。
通过多次进行运动检测的实验表明,一个有效区分运动和静止的指标是在滑动窗口内数据的极值变化。窗口内数据的峰值分布范围能有效反映该时刻传感器是否处于动态状态。统计特征可以提取数据分布属性,从而有效反映行人的当前状态,即将样本数据映射到有限个分组的频数空间获得统计特征,再利用其统计特征提取检测阈值,并且频数空间可用频数分布直方图直观表示。但利用式(1)、(2)计算得到A(k)后直接映射到频数空间会存在数据的峰值分布不明显的问题,因此,对A(k)进行预处理十分关键。利用滑动极值滤波处理非峰值数据以提升峰值在滑动窗口中的分布占比:
(3)
式中z(k)可分别取A(k)或W(k)。其中滑动窗口内最大值为
g[z(k)]=argmaxx-μsxsx+μ[z(x)]
(4)
式中μ为设定的延拓窗口初值。
图3为行走步态及跑步步态下滤波后合加速度的频数直方图。在直角坐标系中,x表示样本数据的连续可取数值,按A(k)的最小值a和最大值b把数据分为m组,各数据组的边界范围按左闭右开区间,如[a,a+d),[a+d,a+2d),…,组距为d=(b-a)/m。频数表示落在各组样本数据的个数。
图3 行走与跑步状态的统计直方图
频数分布直方图能清楚显示各组频数分布情况,又易于显示各组之间频数的差别,且可以获取直观、形象的数据分布情况。由于人体运动的数据中均混有零速区间和运动区间数据,造成了将样本数据映射到频数空间后服从双峰分布,即包含2个独立的偏态分布。
图3中两种不同步态的直方图均包含双峰分布,且分组数m=10。以行走状态为例,其中左偏态分布峰值(记为Peak I)由直方图横坐标分布区间段x=1的矩形组成;直方图横坐标x=5处为拐点,右峰值(记为Peak II)由直方图横坐标区间[5,10]的多个矩形组成。
组成Peak I的直方图中矩形逆映射到数据组边界范围是[a,a+d),分布于该组边界范围的样本数据对应运动过程中的零速区间;组成Peak II的直方图中矩形逆映射到数据组边界范围是[b-5d,b),分布于该组边界范围的样本数据对应运动过程中的运动区间。在上述两区间中存在一数据组,边界范围是[a+d,b-5d),对应直方图中被映射为直方图x∈[1,5]的矩形组成的区域,且位于样本数据的零速区间与运动区间之间,是两区间的分界区间。
在分界区间内,直方图存在一频数最小数据组,根据直方图定义可知,分布在该组所映射在样本数据空间边界范围内的样本数据最少。所以根据频数最小数据组逆映射于样本数据空间的位置,即零速检测最优阈值所在的数据组,能够区分零速和运动两种状态。但是由于直方图将样本数据映射到频数空间的分组有限,每组组距较大,利用频数最小数据组不能获得精确的阈值。因此,通过曲线拟合法近似地刻画频数空间中离散点之间的函数关系,用拟合曲线函数在分界区间的最小值,即图4中垂直于直方图横轴的分界线的横坐标值,逆映射到样本数据空间中可得到精确的零速检测阈值。具体选取阈值过程为:
1) 确定分界区间范围。频数空间中分界区间处在零速与运动区间之间,即Peak I与Peak II之间。因为拐点是曲线的凹凸分界点,利用Peak II的左拐点确定分界区间范围。根据直方图拟合函数F(x)的二阶导数F(x)和三阶导数F‴(x)确定拟合函数的拐点G在坐标系中横坐标值Gx为
Gx={xi|F(xi)=0 &F‴(xi)<0}
(5)
2) 求取分界线坐标。利用分界区间的边界点坐标值获得频数分布曲线拟合函数中的最小值minF(x)点在坐标系中横坐标值minF(x)x为
minF(x)x={xi|F′(xi)=0 &xi (6) 式中F(x)为F(x)的一阶导函数,小于Gx的条件是为了防止与拟合函数极大值后面的极小值在判断时混淆。根据最小值点的横坐标值可得分界线为x=minF(x)x。 3) 求取阈值。零速检测阈值可根据分界线逆映射到样本数据空间得到: ε=minF(x)x×(b-a)/m (7) 图4 统计直方图的曲线拟合 当运动速度或运动模式变化,但阈值未改变的情况下会出现检测不到、误检测等问题[6],需要更新阈值以适应变化的检测数据。运动状态变化根据频数空间Peak II的偏态分布与运动频率两方面判断。通过观测多组实验发现,相对于慢速度的运动,随着运动速度的增大,零速区间减小,代表运动区间的样本数据组边界范围内数据出现的频数增加,因此频数空间中组成Peak II的总频数会随之变化。由于组成Peak II在直方图中矩形个数,即直方图分组数不变,频数增加只引起矩形高度变化。运动状态的变化可以由峰值平均频数的变化判断。峰值平均频数Vave为 (8) 式中:h(x)为x的频数值,x可取[1,m]中任意整数;[Gx]表示一个小于或等于Gx的最大整数。 检测数据中的周期性波峰能够体现运动的频率信息,从而体现运动状态的变化。合角速度的周期性波峰分布较明显[1],根据合角速度的波峰信息可获得步态频率信息。但运动过程中抖动或传感器噪声等原因在主峰旁会出现伪波峰,如图5所示。 图5 角速度极大值拟合曲线示意图 由于伪波峰的存在,通过比较数据的值不能有效地提取峰值频率。因此,使用一、二阶差分信息来识别波峰。具体算法流程为: 1) 计算合角速度,并利用一、二阶差分运算得到所有极大值点,其中包含主波峰点和伪波峰点。所有极大值点横坐标的集合P′x为 P′x={xi|W′(xi)=0 &W″(xi)<0} (9) 式中:W′(xi)为W(xi)的一阶导函数;W″(xi)为W(xi)的二阶导函数。 2) 对第一步得到的所有极值点利用3次样条插值拟合出合角速度极大值曲线(见图5)。 3) 利用第一步中式(9)求极值法对极大值曲线求极值可得到主波峰点集合Px。根据主波峰点之间差值的倒数可得到人的步态频率为 (10) 式中:Px(i)为主波峰点集合中第i个点;fs为传感器采样频率。 本文提出的自适应阈值算法,是根据峰值频数和步态频率的变化共同作用来判断运动状态是否发生变化,进而重新计算统计直方图获得新的阈值。具体算法流程为: 1) 首先设定初始滑动窗口长为600个点,延拓窗口长为20个点。利用检测数据相邻两时刻差分值判断开始运动的时刻,根据时刻值在窗口内的位置判断运动前静止时长。利用式(1)~(4)计算检测数据并延拓峰值区间。 2) 对第一步得到数据计算统计直方图,根据直方图进行曲线拟合,计算拐点的横坐标Gx是否小于m/2,判断频数空间是否符合双峰分布。利用式(9)、(10)中插值波峰检测法计算其峰值初始滑动窗口内的f及Vave。根据阈值提取算法获得初始零速检测阈值。 图6 自适应阈值算法框图 实验采用的传感器为MPU9150模块,该模块封装了3轴陀螺仪、3轴加速度计,利用STM32开发板对传感器数据进行采集。为验证零速检测的有效性和实际效果,设计了阈值检测实验,实验采用的传感器固定在左脚足面上,采集了连续直线变速运动,第35 s前为行走状态,35 s后为跑步状态,主要对比验证自适应阈值和固定阈值检测效果,图7为截取的部分对比结果。固定阈值法无法准确检测到速度变化后的零速区间。而自适应阈值法对速度变化前、后零速区间都能成功检测。 图7 检测结果对比图 为验证算法的可靠性分别采集多组行走、跑步及上、下楼梯数据,行走、跑步步态每组采集100步,台阶数据每组采集50步,对零速区间检测的准确率如表1所示。 表1 零速检测结果 续表 第一组/步第二组/步第三组/步真实步数/步固定阈值行走10099102100跑步989498100台阶45474550 每个零速区间起点和终点的准确性对零速补偿以及导航轨迹推算至关重要,为验证每个零速区间始末位置的准确性,采用文献[1]的实验数据并利用本文提出的方法得到的检测结果计算导航轨迹,如图8所示。该实验数据采用的传感器为3DM-GX2模块,固定在鞋底上,实验数据采集路径为“8”字形闭环曲线轨迹,总行程为84 m,且采集起点和终点位置东向偏差和北向偏差在±1 cm内[1]。利用本文算法得到的检测结果结合卡尔曼零速修正法获得的导航轨迹如图8所示。 图8 运动轨迹图 将本文算法与固定阈值算法及多步态零速检测算法[5]利用图8的数据进行对比。将3种算法的检测结果结合零速修正算法生成导航轨迹,用终点平面误差[8]对比3种算法的检测效果。检测结果如表2所示。其中,终点平面误差为终点与起点位置的欧式距离,误差率为终点平面误差与总行程的比值。 表2 实验结果 本文提出了基于自适应阈值的MEMS行人惯性导航零速检测算法,首先对检测数据进行滑动极值滤波,利用统计分布特征提取最优阈值,通过插值波峰检测法获得步态频率,根据频率变化及数据频数分布特性更新阈值,以达到自适应阈值的目的。经多组实验验证在行走、跑步及台阶的运动模式下检测的准确率达到98%。相对于固定阈值法检测零速区间的精确度有较大提高,将检测结果用于导航位置解算的误差率小于1%。与其他自适应阈值算法相比,本文算法减少了运算量,且不需要辅助传感器,避免了机器学习类算法的高复杂度,具有精度高,成本低,实时性好等特点。对提升导航精度有很好的实际应用价值。1.3 阈值更新条件
1.4 自适应阈值算法
2 实验验证及结果
2.1 步数检测实验
2.2 零速区间检测实验
3 结束语