孙 伟，丁 伟，闫慧芳

(辽宁工程技术大学测绘与地理科学学院，辽宁阜新123000)

K均值聚类自适应零速检测人员定位技术*

孙 伟*，丁 伟，闫慧芳

(辽宁工程技术大学测绘与地理科学学院，辽宁阜新123000)

零速区间检测准确度直接制约着零速修正算法对于改善人员定位系统提供位置精度的能力。针对现有零速检测方案存在阈值设定以及训练模型准确性等问题，提出一种基于加速度幅值滑动方差开展行走步态零速区间检测方法，采用K均值聚类方法自适应纠正初始检测结果中的误检状态，构造Kalman滤波器并在零速区间以惯性系统解算的速度信息为观测量进行量测更新来限制导航误差积累。开展人员多种运动状态下的行走测试，实验结果表明K均值聚类自适应算法能对零速区间进行有效地检测，获取的位置解算误差小于2%。

人员定位;K均值聚类;Kalman滤波;零速修正

相对其他室内人员定位系统而言，基于MEMS的定位系统无需预先设置节点，完全依靠固定在行人身上的微惯性测量单元 MIMU(Micro Inertial Measurement Unit)输出信息进行位置推算，具有较高的自主性［1－3］。但惯导系统的定位误差随时间三次方增长成为制约其应用时间和应用范围的主要因素［4］。对微惯性人员定位系统而言，零速区间检测准确程度是实施Kalman滤波及时清零速度误差并修正位置误差的前提［5－8］。人员行走过程中，其脚步运动具有周期特征，这一规律将反映到传感器输出数据上［9］。文献中已有检测算法可分为两类，第1类算法直接根据传感器输出数据通过设定阈值进行判断，利用的检测量包括加速度模值、角速率模值、加速度滑动方差、角速率能量等，这类方法计算简单，对特定行走步态的零速区间能做出较为准确的判断，但需要设定固定阈值［10］。因此对不同个体、不同运动状态适应性较差。第2类算法根据机器学习相关理论对原始数据进行分析判断，包括朴素贝叶斯分类、支持向量机(SVM)分类、隐马尔科夫模型(HMM)等，这类算法对不同运动状态有良好的适应性，但前提需要进行大量实验并利用包含各种运动状态的数据训练模型，因此算法复杂度高且依赖模型好坏程度［11－13］。本文以加速度滑动方差为检测量，通过阈值法得到初始分类结果后，统计各次静态、动态持续长度，然后利用K均值聚类将初始分类静、动态分别聚为两类，根据聚类性能自适应纠正初始检测结果得最终检测零速区间，并通过Kalman滤波约束惯导解算误差发散保证导航位置的可靠性和准确性。

1 定位算法编排

MEMS行人定位系统编排如图1所示。

图1 行人导航系统框图

1.1 初始对准

惯性导航系统在进入正式工作状态之前需要进行初始对准获取初始捷联矩阵Cnb，人员定位系统开始航位推算前双脚处于站立状态静止一段时间，此时MIMU只受重力影响，加速度计输出为重力在载体系的投影。以东北天(ENU)地理系为导航系，右前上(RFU)为载体系，则有

利用式(1)可求得俯仰角θ和横滚角γ，而方向位角ψ可利用MIMU中的磁强计确定［14］。

式中:cx、sx分别表示cosx、sinx(x=θ、γ)。磁方位角ψm与地理方位角ψ之间相差一个与纬度B有关的磁偏角α(可由世界地磁模型 WMM2015计算得到)［15－16］，所以

1.2 惯性解算

1.2.1 姿态更新

采用四元数微分方程进行系统姿态更新［4］

式中:RM、RN分别为地球子午圈曲率半径和卯酉圈曲率半径，B为纬度，h为高度。

对人员定位系统，由于人行走速度低，一般不超过1.4 m/s，最快不超过2.5 m/s，即使奔跑，也极少有人速度能达到10 m/s［17－18］。与之对应的ωnen的分量在10－7rad/s量级，且 ωnie的分量为10－6rad/s量级，因此可忽略这两项

进而可得四元数更新解析式:

式中:q(tk)、q(tk+1)分别对应当前时刻及下一时刻姿态四元数，Δθ为区间［tk，tk+1］内ωbib引起的角增量矩阵，

式中:ωx、ωy、ωz表示ωbib的3个分量。

将式(5)泰勒展开并取一阶得到姿态更新数值计算公式

式中:I为四阶单位阵。

1.2.2 速度更新

令vn=［vnEvnNvnU］T为导航系下速度矢量，则导航系下速度更新微分方程为［4］

忽略上式中哥氏加速度项－(2ωnie+ωnen)×vn的影响。进而由矩形积分可得速度更新数值计算公式

式中:vn(tk)、vn(tk+1)分别对应当前时刻及下一时刻的速度矢量，Δt为速度更新周期。

1.2.3 位置更新

传统惯性导航系统位置更新通常采用经纬度和大地高作为位置更新微分方程参量，该方法优点是通过更新计算可直接获得载体实时地理位置。而对于人员定位系统而言，人行走速度较低且MEMS惯导系统采样率高，导致在更新周期内由行走引起的经纬度位置变化非常小，不利于数值计算，因此将人员位置更新采用以ENU 3个方向相对位置表示的位置更新微分方程

式中:rn=［rnErnNrnU］T为相对位置矢量。

位置更新计算 tk+1时刻位置时，tk时刻速度vn(tk)及tk+1时刻速度vn(tk+1)都已求得，因此解上述位置更新微分方程可采用梯形积分得

式中:rn(tk)、rn(tk+1)分别对应当前时刻及下一时刻的位置矢量。

2 K均值聚类零速检测

2.1 初始检测

初始检测利用加速度滑动方差与设定阈值比较得初始零速区间。加速度滑动方差按下式计算:

式中:2N+1为窗口大小，由加速度计采样频率确定。将计算出的加速度滑动方差与设定阈值比较，小于阈值的点即为静止状态的点。

2.2 检测结果优化

加速度滑动方差阈值法步态检测结果中存在误判导致步伐处于摆动过程中可能出现短时伪静态，而在静止过程中出现短时伪动态。针对如上问题通过设定阈值，将小于该阈值的短时状态修正为其相反的状态，即伪动态修正为静态，或伪静态修正为动态。本文利用K均值聚类方法根据各次状态持续长度分别将静态、动态自动聚类成两类。记录初始检测结果中各次静态(或动态)持续长度构成序列L={l1，l2，…，ln}，然后设定两个聚类簇 C={C1，C2}，得到聚类的目标是最小化平方误差［19］

按式(11)分别对初始检测结果静、动态聚类成两类，初始检测时的阈值设定不当引起静、动态各自聚类的效果不同。为此引入反映聚类性能度量的内部指标——Dunn指数(Dunn Index，DI)［19］

DI值越大则聚类性能越好。其中，dmin(C1，C2)=表示簇C1与簇C2最近样本间的距离;表示簇 Ck(k=1，2)内样本间的最远距离。

初始检测步态纠正方案如下:首先对初始检测静、动态分别聚类，然后计算聚类性能 DIStance和DISwing，最后选择DI值较大的聚类将对应的伪状态纠正为相反状态。若静态聚类效果优于动态聚类效果(DIStance＞DISwing)，则依据静态聚类结果对初始检测结果进行纠正，将聚类所得短时静态纠正为动态。

3 Kalman滤波零速修正

零速修正Kalman滤波是在非零速区间只进行时间更新，按惯导原理计算导航参数，由于MEMS陀螺测量数据存在误差量大，导致惯性解算位置误差以时间的三次方增长;在零速区间进行完整时间和量测更新，采用的量测量为惯导解算速度与零的差值，即假设零速区间行人脚步速度为零。由于行走特点及数据扰动，导致脚步速度在零速区间并不绝对为零，因此零速区间采用的这一伪观测量同样存在具有随机性且不随时间累积的误差。闭环Kalman滤波综合利用带误差的惯导解算数据及带误差的速度伪观测量得出导航参数误差的最优估计，进而实现导航参数的闭环修正。

3.1 微惯性人员定位系统误差方程

3.1.1 姿态误差方程

令Φn=［xyz］T表示失准角，则捷联惯导系统姿态误差方程为

由前面分析可知，在人员定位系统中ωnin为极小量，因此上式中－ωnin×Φn项及δωnin项均为极小量，忽略这两项可将姿态误差方程简化为

式中:εb为载体系下陀螺常值漂移，Cnb为姿态矩阵，在捷联计算姿态四元数更新后由下式计算实时获得

3.1.2 速度误差方程

令δvn=［δvnEδvnNδvnU］T表示速度误差，得到惯导系统速度误差方程:

同样由分析可知上式中－(2ωnie+ωnen)×δvn项及－(2δωnie+δωnen)×vn项均为极小量，因此忽略小量后MEMS人员定位系统速度误差方程简化为

3.1.3 位置误差方程

令δrn=［δrnEδrnNδrnU］T表示位置误差，则以相对位置矢量rn为基础的MEMS人员定位系统位置误差方程为

3.2 Kalman滤波器设计

根据MEMS人员定位系统误差方程设计闭环Kalman滤波状态方程

式中:X=［(Φn)T(δvn)T(δrn)T(b)T(εb)T］T为 Kalman滤波状态向量;W=［(wbg)T(wba)T01×9］T为系统噪声向量，其中wbg=［wgwgwg］T为陀螺白噪声向量，wba=［wawawa］T为加速度计白噪声向量;F为状态转移矩阵

式中:(fn×)为加速度矢量fn对应的反对称矩阵。

假定零速区间行人脚步速度为零，取该区间内微惯性系统计算速度与零的差值为伪观测量，即令Z=vn，则可设计Kalman滤波零速修正的观测方程

式中:H=［I3×303×12］为观测矩阵。

4 实验结果与分析

利用荷兰Xsens公司生产的MTi－G－710开展多组人员行走实验，将传感器固定在鞋面。综合考虑数据量和解算精度两方面因素设置采样率为100 Hz，采集三轴陀螺、三轴加速度计和三轴磁强计数据。实验开始前保持脚面静止5 s，以便系统完成初始对准，对于闭合行走轨迹，行走终点要求与起点重合。实验过程中所采用的MEMS器件参数如表1所示。

表1 惯性器件参数

4.1 步态检测实验

为验证步态检测方法有效性，选取长距离行走数据进行实验验证。计算加速度滑动方差的窗口宽度设定为10，对应0.1个采样周期也即0.1 s的数据，阈值设定为2 m2/s4。初始步态检测将加速度滑动方差小于阈值的点置为静态(状态0)，大于阈值的点置为动态(状态1)。然后利用K均值聚类将初始检测静、动态分别聚类成两类(如图2)。分别按式(12)计算两类聚类的性能指标 DIStance=1.12、DISwing=0.25，即静态聚类效果优于动态聚类，对比图2得出相同结论。因此，可根据DI值自动选定步态纠正方案，对于本实验即为利用静态聚类结果将假静态纠正为动态。

图2 初始检测步态K均值聚类

利用加速度滑动方差阈值法检测步态结果及经K均值聚类纠正后步态检测结果如图3所示，对比两种检测结果表明仅依靠加速度滑动方差阈值法检测步态中包含较多短时假静态，而经K均值聚类纠正后的步态修复了这些假静态，检测结果更加合理。

图3 纠正前后步态检测结果对比

4.2 不同路径行走实验

为验证自适应步态检测零速修正算法对人员位置解算的精度，分别开展圆形及400 m跑道闭合行走实验。采用本文提出自适应零速修正融合闭环Kalman滤波算法解算位置变化，结果分别如图4所示。

圆形行走轨迹为直径34 m的小广场花坛，逆时针绕花坛一圈并回到起点，解算位置结果如图4所示。最终闭合效果较好，终点平面位置误差2.023 2 m，相对行走路径总长106.814 2 m的相对误差为1.89%。

图4 圆形行走轨迹

400 m跑道行走轨迹为足球场橡胶跑道最外道，逆时针绕跑道一圈后回到起点，其位置解算曲线如图5。由于行走距离较长，由器件误差引起的位置解算误差随时间逐渐增加，导致该长距离行走最终位置平面误差为6.253 1 m，相对误差为1.56%。

图5 400 m跑道行走轨迹

不同运动轨迹下的位置误差如表2所示。数据结果表明行走实验的解算位置误差虽然随行走距离增大而增加，但其相对误差均不超过2%，该结果对于采用单一MEMS惯导系统而言可满足实际人员定位需求。

表2 位置误差

5 结论

零速区间的有效检测是零速修正算法得以准确应用的基础，论文提出一种基于K均值聚类自适应零速修正的微惯性人员定位方法，通过简化惯导方程并建立微惯性人员定位解算方法，论证加速度滑动方差阈值法结合K均值聚类自适应检测零速区间的有效性。建立闭环Kalman滤波修正模型实现人员位置解算的零速修正。实验结果表明，论文提出的自适应步态检测方法显著提高零速区间检测的准确度，得到的位置解算相对误差优于2%，该测试结果可满足小范围内人员定位精度需求。

［1］ 张金亮，秦永元，梅春波.基于MEMS惯性技术的鞋式个人导航系统［J］.中国惯性技术学报，2011，19(3):253－256.

［2］ 孙伟，李婉秋，初婧，等.基于误差修正技术的井下人员MEMS定位方法［J］.传感技术学报，2014，27(7):898－904.

［3］ 宋镖，程磊，周明达，等.基于惯导辅助地磁的手机室内定位系统设计［J］.传感技术学报，2015，28(8):1249－1254.

［4］ Groves P D.Navigation Using Inertial Sensors［J］.Aerospace and Electronic Systems Magazine，IEEE，2015，30(2):42－69.

［5］ Skog I，Handel P，Nilsson J O，et al.Zero Velocity Detection an Algorithm Evaluation［J］.Biomedical Engineering，2010，57(11): 2657－2666.

［6］ 田晓春，陈家斌，韩勇强，等.多条件约束的行人导航零速区间检测算法［J］.中国惯性技术学报，2016，24(1):1－5.

［7］ Harle R.A Survey of Indoor Inertial Positioning Systems for Pedestrians［J］.Communications Surveys and Tutorials，IEEE，2013，15 (3):1281－1293.

［8］ Hung M L，Luan V N，Trung H D.Dynamic Human Gait Phase Detection Algorithm［C］//International Conference on Modeling of Complex Systems and Environments，2015.

［9］ Wang Z，Zhao H，Qiu S，et al.Stance Phase Detection for ZUPT-aided Foot-Mounted Pedestrian Navigation System［J］.IEEE/ ASME Transactions on Mechatronics，2015，20(6):3170－3181.

［10］ Park S K，Suh Y S.A Zero Velocity Detection Algorithm Using Inertial Sensors for Pedestrian Navigation Systems［J］.Sensors，2010，10(10):9163－9178.

［11］吴哲君，赵忠华，唐雷.基于SVM的行人步态实时分类方法［J］.电子测量技术，2015，38(7):41－44.

［12］Ren M，Pan K，Liu Y，et al.A Novel Pedestrian Navigation Algorithm for a Foot Mounted Inertial Sensor Based System［J］.Sensors，2016，16(1):139－152.

［13］费程羽，苏中，李擎.行人惯性导航零速检测算法［J］.传感器与微系统，2016，35(3):147－150.

［14］钱伟行，朱欣华，苏岩.基于足部微惯性/地磁测量组件的个人导航方法［J］.中国惯性技术学报，2012，20(5):567－572.

［15］ Chulliat A S，Macmillan P，Alken C，et al.The US/UK World Magnetic Model for 2015－2020［R］.National Geophysical Data Center，NOAA，2014.

［16］Widodo B，Andrei V V，Gulnara R S.Web Based Information System for Modeling and Analysis of Parameters of Geomagnetic Field［J］.Procedia Computer Science，2015，59:73－82.

［17］包志军，马培荪，仝建刚，等.人行走速度规律的实验研究［J］.实验室研究与探索，2000，6:39－42.

［18］Browning R C，Baker E A，Herron J A，et al.Effects of Obesity and Sex on the Energetic Cost and Preferred Speed of Walking［J］.Journal of Applied Physiology，2006，100(2):390－398.

［19］周志华.机器学习［M］.北京:清华大学出版社，2016.

孙 伟(1984)，男，教授，博士生导师，黑龙江萝北县人。2007年于哈尔滨工程大学获得学士学位，2011年于哈尔滨工程大学获得博士学位。长期从事惯性导航技术研究，sunwei－3775235@163.com;

丁 伟(1992－)，男，四川省巴中市人。现为辽宁工程技术大学测绘与地理科学学院硕士研究生。从事惯性导航与组合导航方法研究。

Pedestrian Positioning Technology with K-Means Clustering Adaptive Zero Velocity Detection*

SUN Wei*，DING Wei，YAN Huifang
(School of Geomatics，Liaoning Technical University，Fuxin Liaoning123000，China)

The detection accuracy of Zero velocity interval is the premise to implement Kalman filtering to clear velocity error and correct position error in time.For the existing zero velocity detection scheme，there are some problems such as threshold setting and training model accuracy.This paper proposes a zero velocity interval detection method in walking gaits based on moving variance of acceleration amplitude，an adaptive K-means clustering method is used to correct the false identified states in initial detection results.A Kalman filter is constructed in zero velocity interval，which adopt the velocity information calculated by inertial system as measurements and carry out measurement update to restrict navigation error accumulation.Series experiments are carried out，experimental results show that K-means adaptive algorithm can effectively detect zero velocity interval，and the error of the obtained position calculation is less than 2%.

pedestrian navigation;K-means clustering;Kalman filter;ZUPT

U666.12

A

1004－1699(2017)02－0296－06

C:7120;7230M;7320E

10.3969/j.issn.1004－1699.2017.02.022

项目来源:国家自然科学基金项目(41304032);高等学校博士学科点专项科研基金(新教师类)项目(20132121120005);第8批中国博士后科学基金特别项目(2015T80265);辽宁省高等学校杰出青年学者成长计划项目(LJQ2015044);对地观测技术国家测绘地理信息局重点实验室开放基金项目(K201401);地球空间环境与大地测量教育部重点实验室开放基金项目(14－01－05);江西省数字国土重点实验室开放研究基金项目(DLLJ201501);辽宁工程技术大学研究生教育创新计划(YS201608)

2016－07－05 修改日期:2016－10－12