基于可穿戴式MIMU的个人导航修正算法

刘 宇，向高军，路永乐，邸 克，曹 原，司学迁

（重庆邮电大学 光电信息感测与传输技术重庆市重点实验室，重庆 400065）

随着 MEMS（Micro-Electro-Mechanical System）技术的飞速发展，微惯性测量单元（Miniature Inertial Measurement Unit,MIMU）在体积、精度等方面得到了极大的优化和提高，由此基于可穿戴式MIMU的定位技术成为人们研究热点[1-2]。

行人自主定位是指仅依靠MIMU对速度、航向和位置进行解算的定位技术。为提高定位精度，国内外学者从器件到单元，再到系统等多个方面进行了研究。在航向修正方面，美国学者Jens W等人将MIMU固定在头部，提出了一种头戴式定向系统[3]。西班牙Alvarez J C等人通过将MIMU佩戴在腰部，提出了一种结合人体力学特征来进行定位的个人导航系统[4]，Luis E D等人提出基于腕部的增强型启发式漂移算法来修正航向，进而提高定位能力[5]。为提高步态检测精度，将MIMU固定在足部或鞋上的多条件零速检测算法[6]、新型零速修正方法[7]和自适应零速区间检测算法[8]等多种方法被提出，以及大连理工大学 QIU Sen等人通过将惯性单元分散固定在足部和腰部，来提高零速修正效果[9]。以上方法在一定程度提高了行人定位精度，然而在使用过程中将传感器固定在身体特定的部位（如脚、头、腰、手臂），不符合行人穿戴定位设备的随意性。YANG Xiuxin等人虽对传感器固定在身体的位置进行了探讨，但并未在各个位置下的导航算法进行优化[10]。

加拿大Melania S等人设计了手臂摆动和非手臂摆动的姿态识别算法，并在两种姿态下进行人员定位[11]，西北工业大学ZHANG Hemin等人在此基础上设计了基于支持向量机的姿态识别算法，对手持、电话、上衣口袋以及包里等四种姿态下进行人员定位[12]，以上两种方法虽然考虑了行人使用定位终端常见的姿态，但都基于手机平台进行实现，存在实时性差、精度低等问题，同时手机客户端较大的工作量使手机性能受到限制。

基于此，为实现MIMU佩戴在多个位置下的行人精确导航定位，本文提出了一种基于可穿戴式MIMU的个人导航修正算法。

1 定位原理

MIMU是一种集成微型加速度计、微型陀螺仪并能够提供物体姿态信息的测量单元。结合行人步态检测、步长估计算法，利用MIMU测得的行人航向信息，通过行人航迹推算算法（Pedestrian Dead Reckoning,PDR）提供位置信息，进而实现行人定位。

PDR算法从初始坐标开始，结合行人航向、步态、步长对下一时刻位置进行推算，其中位置推算公式如下：

式中，（xk,yk）、θi、di分别表示第i步的位置、航向角和步长。

使用窗函数滤波对MIMU数据进行处理，同时根据不同的步态特征，采用自适应加速度阈值方法进行步态检测[13]，相关公式表达如下：

其中，S(k)=1表示计步成功，ak为导航系下三轴合加速度；A-与A+分别为自适应加速度上、下阈值，且由固定时间段内加速度信号均值提取。

加速度的变化差值与步长存在相关性[14]，本文对步长进行估计采用方法如下：

式中：amax,k、min,ak在第k步时目标加速度的波峰值、波谷值，l为各个体步长系数，kd为第k步的步长。

利用四元数法进行捷联姿态解算获取航向角，并设计朴素贝叶斯分类器对MIMU所处位置进行识别，提出改进的启发式漂移算法对各位置下航向解算结果进行修正。

将修正的航向、步长和步态等信息，带入到 PDR算法获取位置信息，进而实现MIMU佩戴在多位置下高精度行人自主定位，具体实现框图如图1所示。

图1 算法框图Fig.1 Algorithm framework

2 定位终端穿戴方式与数据特征

本文采用实验室自主研发的MIMU进行50Hz的数据采集和算法实验验证。在行走过程中，人们习惯将随身携带物品放入衣物口袋中，结合人类衣物特点，通常可将各类口袋分为上衣口袋，右（左）衣口袋，右（左）前裤口袋，右（左）后裤口袋。同时行走过程中，行人身体左右部分近似对称，因此本文主要对定位终端MIMU存放于上衣口袋，右衣口袋，右前裤口袋，右后裤口袋等情况进行研究，图2给出了MIMU佩戴方式示意图。

图2 MIMU佩戴方式示意图Fig.2 MIMU wear mode

由于传感器数据易受噪声干扰，为提高输入数据精度，本文采用滑动窗口对目标加速度、角速度进行汉明窗滤波。图3给出了将MIMU佩戴于上衣口袋，右衣口袋，右前裤口袋，右后裤口袋分别行走5步时三轴加速度的滤波效果，从图可以看出此方法有效的剔除了原始数据的毛刺，为后续的佩戴方式识别提供了良好的基础。

图3 窗函数滤波图Fig.3 Window function filter

行走过程中身体各部位特征变化不一，MIMU不同的佩戴位置，使得传感器数据存在较大变化，这将严重影响行人定位过程中的航向信息。可见，如何有效的进行航向估算是行人精准定位的前提和基础。

3 航向修正算法

四元数是四维空间中的一个向量，利用四元数算法可以实现姿态更新。同时，贝叶斯分类器可在给定先验概率下完成对象分类[15]，针对可穿戴定位终端的复杂行为特征，朴素贝叶斯分类器可完成MIMU各种佩戴方式的准确区分。

本文采用四元数法对行人航向进行解算，利用朴素贝叶斯分类器对MIMU佩戴位置进行识别，从而对MIMU在各种位置下的行人航向进行修正，保证行人航向精度。

3.1 捷联解算

假设一个矢量在直角坐标系oxyz、ox′y′z′中投影分别为用四元数表示为：

R为坐标变换矩阵，且有：

q为坐标变换四元数，包含了一个唯一的坐标变换矩阵。以x、y、z方向表示载体的前、左、上方向，建立载体系（b 系），当地地平系作为导航系（n 系）。姿态欧拉角可由n系到b系的姿态变换矩阵计算，对应欧拉姿态矩阵为：

其中，θ、ψ、γ分别为载体俯仰角、航向角、横滚角。

姿态四元数到欧拉角的关系如下：

3.2 MIMU姿态识别

在行走过程中，由于身体各部位的行为特征差异明显，航向信息将受到MIMU佩戴方式的极大干扰，本节主要对MIMU佩戴方式的识别进行研究，为后续航向修正提供基础。

1）特征提取

MIMU采集的三轴 MEMS加速度信号和三轴MEMS角速度信号，每一轴数据都能提取出不同的特征量，同时特征向量的提取对于多种模式识别精度有着极大的影响。基于时域特征的提取计算复杂度低，耗时较短的特点，本文采用三轴加速度计的如下三种时域特征值对MIMU所处佩戴位置进行识别：

均值：

均方根：

标准差：

μ为均值。

2）分类器设计

朴素贝叶斯分类器是一种概率分类器，通过计算给定待分类项出现条件下各个类别出现的概率，进而对分类项的属性进行分类。

设A={A1,…,A9}为一个待分类项，A1,…,A9依次为X轴加速度的均值、均方根、标准差，Y轴加速度的均值、均方根、标准差，Z轴加速度的均值、均方根、标准差。

设类别集合C={C1,C2,C3,C4},C为 MIMU 佩戴的位置；如果p（Ck|A）=max{p（C1|A）,p（C2|A）,p（C3|A）,p（C4|A）}，则有A∈Ck。

假设概率模型中各个属性相互独立，根据贝叶斯定理可得Ci后验概率：

P（A）对于对所有类均为常数。

由条件独立假设：

则有：

其中，Z是比例因子，A1,…,A9是各个相互独立的属性，p（Ci）是类别先验概率，其值为样本数除以训练集总样本数。

考虑测试过程中特征值A1,…,A9出现的频次，

式中：N表示特征值出现的频次，TN(Aj,Ci) 表示值Aj在Ci位置测试过程中频次之和。

在本文的分类器中，利用三轴加速度计数据的时域特征量对MIMU常见4种佩戴方式：上衣口袋（C1）、右衣口袋（C2）、右前裤口袋（C3）、右后裤口袋（C4）进行识别。假设来自传感器的加速度数据误差符合高斯分布，且均值和方差取决于MIMU的佩戴类别C，基于公式(13)可计算每个类别出现的概率并对分类器进行训练。

首先，在4个已知佩戴方式下分别采集300步三轴加速度，取峰值数据按公式(8)～(10)计算特征值，结果如表1所示；结合表1特征值可对分类器进行训练，算法步骤如下：

步骤①：计算训练样本中各个MIMU佩戴方式出现的频率P(C1)、P(C2)、P(C3)、P(C4)；

步骤②：每相连三步加速度取峰值，分别计算特征值作为一个待分类项Aj；

步骤③：利用步骤②得到的Aj，统计特征值归属为表1中最邻近特征值向量，从而得到频率统计P(Aj|Ci)；

步骤④：重复步骤②③得到分类器参数；

步骤⑤：输出训练好的贝叶斯分类器。

为了测试本文设计的姿态识别算法的精度，将MIMU分别固定在上述4个位置分别进行100次直线行走测试，并选取相连三步三轴加速度峰值作为实验数据特征值进行计算，识别结果如表2所示。由表2可以看出本文设计的分类方法可以很好的将 MIMU的4种位置区分开来，且识别率高于99.0%。

表1 多位置下加速度特征向量Tab.1 Feature vector of acceleration at multi-position g/m2

表2 姿态识别算法精度分析Tab.2 Gesture recognition accuracy analysis

3.3 航向修正算法

图4 多位置下航向角变化图Fig.4 Heading changes at multi-position

图4给出了MIMU佩戴在身体4个位置时行人航向变化情况。可以看出，当行人沿直线行走或转向（以90°为例）时，航向通常都会偏离期望航向且保持在不同常值范围内波动。基于这个事实，可以通过航向的变化对行人正确航向进行估计。

基于此，本文在传统启发式漂移消除算法基础上，对MIMU在多位置下的行人航向进行估算，公式如下：

式中，m1、m2、m3、m4分别表示 MIMU佩戴在行人上衣口袋，右衣口袋，右前裤口袋，右后裤口袋时的航向信息，其中1表示直行状态，0表示转弯；ψk代表当前步的航向角，ψk-1、ψk-2、ψk-3分别表示前一步、前两步及前三步的航向角，δψi(i=1,2,3,4)为各佩戴位置对应航向偏差阈值。当检测到行人沿直线行走时取上一时刻航向作为当前航向。

4 实验验证与测试

选取重庆邮电大学风华操场场景作为测试地点。测试时，分别将MIMU固定在上衣口袋，右衣口袋，右前裤口袋，右后裤口袋沿着图5给出的轨迹以正常速度行走一圈。

图5 行走参考轨迹Fig.5 Walking reference track

测试时，设定出发航向为130°，初始平面坐标为(0,0)，行走结束终点至起点并重合，同时转向出发时方向，全长里程D为360 m。测试结束后记录定位信息，若起点和终点最后的误差越小，则说明系统的定位效果越好。结合PDR进行航迹推算，图6给出了传统 MIMU固定佩戴方式航向反馈修正算法下行走轨迹图，图7是改进航向修正算法得到的轨迹图。

结合图6、图7可知，航向极易受到MIMU佩戴方式影响。当MIMU分别佩戴在4个位置时，传统算法对于MIMU佩戴在多个位置下适应性较差，这使得行走轨迹与真实轨迹存在较大偏差；而经本文所提的方法修正后，4种配戴方式下的轨迹与真实轨迹基本吻合。表3给出了相应测试结果，本文提出的算法定位误差均在0.49%D以下，有效地解决了行人4种佩戴定位终端时航向发散问题，提高了可穿戴式定位设备的定位精度。

图6 传统算法行走轨迹Fig.6 Walking effect before using the improved algorithm

图7 改进算法行走轨迹Fig.7 Walking effect after using the improved algorithm

表3 不同姿态下测试结果Tab.3 Different pedestrian test results

5 结 论

在行人自主定位中，针对传统PDR算法在定位过程中需将MIMU固定在特定位置，分析了行人使用可穿戴设备时常见的4种位置，设计了一种基于可穿戴式MIMU的个人导航修正算法。同时，MIMU佩戴在各位置的识别精度高于 99.0%，且定位误差均小于0.49%D。该算法能够对 MIMU的姿态进行准确的识别，实现了4种位置下的自主定位，突破了惯性定位中测量单一姿态的瓶颈，将传统MIMU固定佩戴方式定位技术扩展成多种佩戴方式定位，从而提高了惯性定位技术在可穿戴设备方面的适应性。