面向室内定位的改进行人航位推算方法

2022-09-29吉俊杰蒋佳杰姚航宇杨敏之吴向臣

计算机测量与控制 2022年9期

吉俊杰，蒋佳杰，姚航宇，杨敏之，吴向臣

(1. 华东宜兴抽水蓄能有限公司，江苏宜兴 214200;2. 河海大学计算机与信息学院，南京 211100)

0 引言

随着移动互联网技术的快速发展，对位置服务的需求也快速增长，人们在日常生活中更加频繁地使用定位导航技术。在室外环境中，全球卫星定位系统导航技术已发展成熟[1]，但在室内环境中，卫星信号由于建筑物的遮挡无法发挥作用，无法提供准确的位置信息。随着移动设备和智能手机的普及，及其内部集成传感器的快速发展，鉴于其精准度高且制造成本低，以智能设备内部集成传感器系统为基础的室内定位系统算法的研究具有广阔的应用前景[2-4]。例如在抽蓄电站厂房中，因安全性要求与地理位置限制依赖无线信号的定位方法难于适用，此时仅利用传感器信号采样与分析的定位方法便体现了其良好的适应性。

行人航位推算(PDR，pedestrian dead reckoning)是室内定位的一种常用方法，它利用行人运动过程中采集的加速度、磁场强度等数据推算出行人每一步的步长与航向，更新行人位置坐标，达到定位目的[5-6]。行人航位推算算法主要包含步数检测(计步)、步长估计和航向估计三方面内容。现今步数检测的主要方法有峰值检测法[7-8]，零速修正法[9]，自相关分析法[10]，动态时间规整算法以及步长周期识别等方法[11-12]。其中，峰值检测法实现简单但是算法窗口的设定难以适用于不同的行走状态；零速修正法需要严苛的设备条件，难以推广；自相关分析法、动态时间规整以及步长周期识别3种方法秉承相同的设计理念，其计算复杂度高，易受噪音干扰。步长估计的精度高低影响定位算法的效果，现有的步长估计有常数步长模型，线性步长模型[13]，倒立单摆模型[14]和非线性模型[15-16]等。然而，常数步长模型准确度较差；倒立单摆模型难以适用于不同行步状态；非线性步长模型参数较少难以泛化。在航向估计方面，传统的基于方向角与陀螺仪进行航向估计算法会产生航向角误差累计的问题。

航位推算作为一种无外源的室内定位方法主要依赖加速度传感器、陀螺仪传感器以及磁场强度传感器实时获取数据进行分析，计算出行人的步态、步长以及航向信息，再对这3种信息进行动态融合以推算出行人的实时运动轨迹，从而实现室内定位。

基于以上分析，本文对行人航位推算算法进行了系统性研究，对相关计步方法、步长估计方法和航向估计方法分别进行了改进，主要工作如下：

1)在计步上，针对传统的峰值检测算法过度计步问题，提出一种结合自相关分析法和峰值检测法的计步算法；通过实验发现，改进计步算法优于传统峰值检测法，还在一定程度上增强了抗干扰能力。

2)在估计上，提出一种基于Scarlett[17]模型融合前一步步长的改进步长估算模型。对Weinberg[18]、Scarlett和Kim[19]3种典型非线性步长模型做了对照试验，选取表现最优的Scarlett模型进行改进，将前一步步长作为自适应参数融入步长模型，实验结果表明该模型相较于Scarlett模型在精度上有所提升。

3)在航向估计上，基于方向传感器算法，提出通过设定主导航向，并利用卡尔曼滤波对行人沿直线行走时的航向进行处理来降低航向估计误差的方法；实验结果表明改进后方法减少了航向角的误差，提升了航向估计的稳定性。

1 步数检测算法改进

1.1 人体行步原理

如图1中，人体行进步伐的一个周期由两步构成，可以详解为七个阶段[20]。第一阶段右脚迈出，脚后跟着地，左脚抬起离地；第二阶段右脚脚掌与地面接触，腿未完全伸直，左脚离地有向前的趋势；第三阶段右腿站定，此刻人处于比较稳定的状态，左脚迈出向前；第四阶段左脚向前跨步完成，左脚后跟即将接触地面，此时落后的右脚被带起，脚后跟稍许离开地面，这个阶段人体重心处于变化状态；第五阶段左脚基本站定，身体带动右脚向前；第六阶段重心稳定，右腿屈膝开始向前迈动；第七阶段人体微微向前，重心前移，右腿迈出，最终回到第一阶段。

图1 人体步数周期图

对人体行进步伐的一个周期进行分类，以重心状态分类可以将整个过程划分为右双支撑期，右单支撑期，左双支撑期和左单支撑期，分别占人体行进步伐周期的10%，40%，10%和40%。以右腿状态分类可以将整个过程划分为右站立相和右迈步相，即指定右边，右脚足跟着地和在地面支撑的时间为右站立相，右脚迈步不与地面接触的时间为右迈步相，约占40%。

分析可见，人的行步过程具有周期性，要想对步数进行识别从而分析人的行为最终为定位服务，还须从数据分析入手。从模型图和日常生活体验来看，在人平常走动时，无论垂直方向还是水平方向，都会出现加速阶段和减速阶段交替的情况，依据各个阶段人行走的特征，可以更容易理解行人行走过程中加速度值的周期性变化。如在第一阶段中，因为脚跟着地与地面撞击造成瞬间反冲力从而产生较大的瞬时加速度，故在垂直方向上，加速度值会达到一个顶峰。在右站立相这一阶段内，人体重心较为稳定，左脚摆动，水平方向也会有对应加速度值的波动。根据以上对不同计步方法的特征分析，选取峰值检测法作为改进对象。

1.2 峰值检测算法改进

在峰值检测法基础上自适应峰值检测法取得了良好的效果，能够同时较好地适应传感器姿态和运动状态变化，实现高准确率计步。但这仍受限于诸多条件，难以直接将原算法应用到各种复杂的实际行步环境中并形成良好的自适应性与泛化性的计步效果[19-20]。本文结合传感器的放置位置与实验参与者的行走状态，设计了一种基于峰值检测与自适应的改进算法，如算法1所示，步骤如下：

1)计算整体加速度，将三轴加速度数据合成表示以更好地反映人体垂直加速度变化，同时消除部分噪声，有利于更好地测算人体步态。

2)将滑动窗口sliding window设为20，用以遍历获得潜在峰值，再根据加速度阈值进行判断，加速度阈值α、β分别设定为1.25 g、3.5 g。

3)计算前一峰值与潜在波峰之间的时间差，判断是否满足行人行走一步的时间阈值范围，以防止将行人特殊情况下的运动行为误判成走步，时间阈值范围为(0.2 s，0.8 s)。

4)最后对检测结果中的波峰数据进行判断，利用行步过程中峰值变换幅度γ=2 g作为标准滤除伪波峰点。

改进波峰检测法结合了阈值检测与自适应检测的设计思想，其中阈值检测法固定了检测范围，缺乏灵活性，在行走速度变化较大或行人处于较为复杂的状态时，不规律的运动状态会使得检测结果的精度下降。自相关分析法比起常用的波峰检测算法可以很大程度上消除行人运动状态以及手机位置对手机计步器计步结果的影响，可以方便快捷地应用在室内人员定位中，但如上所述，此方法复杂度较高。因而，改进方法仅选取部分计算过程进行融合，在提高识别精度的同时也保证了计算的实时性。

1.3 实验数据获取

实验过程将统一使用RedmiK30U手机硬件设备，搭载Android9.0系统，手机内封装了ICM-40607六轴传感器和AK09918地磁传感器。实验以智能手机为研究工具，旨在研究快捷简单灵敏度高的算法以满足普适性，对设备的要求低并适用于大部分生活场景。

为方便进行本节及之后的试验，作者开发了一个采集数据的Android应用程序，用以收集各种情况下行人步进状态下加速度传感器、陀螺仪、磁力传感器以及方向传感器的数据，程序界面如图2所示，所采集数据保存为txt文件。

图2 数据采集程序

Algorithm1:Improved Peak Checking

Input:signal arrayD,the peak time of last timepl,queue sliding windowsw

Output:step numbers

whileDis not nulldo

Fetch an elementαfromD;

ifα>andα<βthen

Put α intosw;

esleContinue;

γ=MAX(sw);

d=γ-pl;

ifγ>γthen

if0.2

s+=1;

esleContinue;

else

if0.3

s+=1;

esleContinue;

pl=γ;

endwhile

1.4 实验结果及分析

实验地点设置为某大楼走廊。实验人员以3种不同的设备携带方式进行实验，每次以相同速率平稳行走，在传感器获得加速度数据后使用巴特沃斯滤波法进行预处理[23]，再使用传统的峰值检测法和改进计步算法进行步数统计和准确率分析。

由表1可知，本节改进算法在精确度上优于传统峰值检测法。实验发现，传统峰值检测法测算步数总是多于真实步数，这是因为在噪声影响下，阈值处理和潜在波峰识别方法并不能很好地区分真实波峰和伪波峰，导致结果误差较大。相较而言，改进算法在将设备拿在手上、置于上衣口袋、置于裤子口袋3种步行模式下都具有较好的准确率，误差步数都在4到6步之间，误差率也在2%左右。

表1 不同计步方法的实验结果

另外，为验证改进计步算法是否弥补了自相关分析法实时性的不足，在程序中添加了获取时间差值的代码，并记录每一步检测消耗的时间，最后求平均并进行比较，结果如表2所示，改进计步算法虽然略慢于传统峰值检测法，但在实时性上明显优于自相关分析法。

表2 单步计算消耗时间比较

最后，改进步数算法融合了自相关分析法以提高计步算法对复杂情况的适应性。实验设计对比了峰值检测法、小米手机应用商店中的计步器、自相关分析法和改进算法在不同类型手机运动模式下的计步情况。此处定义正常计数为误差在5%以内，大量计数为超出实际步数5%，少量计数为低于实际步数5%。试验结果如表3所示，在任意晃动手机情况下，改进计步算法与自适应相关分析法都只产生了少量计数，表现良好。

表3 计步结果

2 步长估计算法改进

根据人体行步过程中的影响因素分析现有经典步长模型，通过对比实验选择表现最优的模型，并基于此进行模型改进设计，最后通过实验验证改进模型的有效性。

2.1 步长估计模型选择

行人航位推算中的步长不是一成不变的，一般来说，人们正常行走一步的单步步长为60～80 cm，但是人的身高腿长，人在行进中的速度，步频等因素都能不同程度地影响到人行进过程中每一步的步长。

设计三组实验，评估3种非线性步长模型的稳定程度与准确程度，从而选择合适的模型。本节实验使用硬件设备与上一节保持一致。以正常走、慢走和快走的不同步速直线前进，并使用第一节中的数据采集软件和计步算法收集行进过程中的加速度数据和步长，在手机每次检测到行人走了一步时，将这一步时间段内的加速度数据写入到txt文件中并标注第几步与不同模型测算下的该步步长，再清除程序中存储加速度信息的数组以进行下一步的信息收集，如此便能收集到行人的步数以及每一步期间加速度变化的数据以及每一步的步长信息，步长信息用于本小节的实验比较，加速度信息可以用于后文的实验训练。

除了沿直线行走，还要求测试人员前进过程中尽量保持匀速，采集数据期间测试人员模拟现实行人导航寻路的状态，手持手机在胸前，手机保持水平状态。测试所在宿舍地面砖块尺寸为0.8 m*0.8 m，利用该地砖控制步长，共有3种模式，一块地砖一步走，两块地砖三步走，三块地砖四步走，所得到的单步实际步长分别为0.8 m，0.533 m和0.6 m。

在实验设计中Weinberg、Scarlett和Kim模型的取值分别为0.375，1.724和1.983，相较而言Scarlett步长估计模型较为稳定且与实际步长接近。虽然实验过程中，实验人员按格子迈步，尽量保证匀速，但是实际上人走路难以保持步长不变，所以我们对非线性模型步长估计实验结果进行统计分析，如表4所示。

表4 3种非线性步长模型比较实验步态实验方法实际

根据表4可以看出，Scarlett模型相较 Weinberg、Kim 模型在平均误差率和方差上都较小，步长估计的稳定性好、精度高，所以我们选择Scarlett模型进行改进。

2.2 步长模型改进

考虑到在行人正常行走情况下，无论是慢速走、常速走还是快速走，行人的步长一般不会突变，前一步步长与当前步步长应具有连续性，行人第步步长应该与第步具有相关性。基于这一观察，在Scarlett模型基础上融合上一步步长信息，提出一种新的步长估计模型：

(1)

其中:Lk-1为上一步步长，q为权重因子，K是一个常量参数，amax和amin是第k速度信号中的最大值和最小值，ai是第i个加速度信号值。接下来，利用收集的加速度信息求取最优参数值，在原先K值基础上使用最小二乘法求取权重因子q值，再以此时的K值与q值作为初始状态使用梯度下降法进行参数最优估计。改进步长估计的计算步骤如算法2所示。

Algorithm2:Improved Stride Estimation

Input:signal arrayD,the stride length of last timeLk,1

Output:Stride lengthLk

Presetα,β,t;

αmax=MAX(Lk);

αmin=MIN(Lk);

w=(α,β);

whileJ(wt)>cdo

t=t+1;

endwhile

首先将步长估计式(1)改写如下：

于是可将步长估计问题总结为如下公式，

Lk=wx+b=WTX

其中:w为参数，x和b是已知信息，代表实际变量。

当X满秩时，最小二乘法可以根据如下公式计算参数：

W=(XTX)-1XTLk

试验收集3种不同步行状态下各200步共600步的加速度数据进行计算，对比两种不同的数据预处理方式，第一种是标准化处理，对相同步行状态下的数据，数值减去平均值，除以其标准差，得到均值为0、标准差为1的服从标准正态分布的数据。第二种是归一化，对相同状态下的数据，数值减去最小值，再除以最大值与最小值的差值，得到新数据范围在[0,1]之间。

评价指标为均值方差和均方根误差，权重因子q取四位小数，结果如表5所示。

表5 不同处理方式下最小二乘法结果

取得较优的权重因子q值为0.276 5。

有了初始值，可将(1)式改写以应用梯度下降法，公式如下：

根据权重因子q值0.276 5和常量K值1.724可以求得值为0.276 5，值为1.247 314，m取值为训练集合的大小，将这些作为初始值应用梯度下降法。我们将收集的3种不同步行状态下各200步的数据中每组取180步作为训练集，20步作为测试集，因此整个训练集大小为540，测试集为60。

可将问题描述为如下公式:

Lk=WTX

定义损失函数为：

更新参数表达式如下：

其中:k为梯度下降的步长，最终得到较优的α与β估计值，分别为0.273 4和1.245 3，在测试集上的均值方差和均方根误差分别为0.001 936和0.044。

2.3 实验结果及分析

实验设计与上一节的计算步骤一致，收集新的数据，以对比所提出的改进步长估算模型与Scarlett步长模型，测算结果如表6所示。可以看出，在快速走常速走与慢速走3种步数下，改进步长估算模型在误差率和方差表现都略优于Scarlett模型，表明改进后的模型的准确度与稳定性都有一定提升。

表6 改进前后Scarlett模型对比

上一节实验仍有不足之处，一是行人步长并不完全精确，二是新模型利用3种步长的数据训练，再在相似的场景下测试说服力略显不足。所以，本节实验旨在测试新模型在行人自由行走状态下是否能对行人行走距离进行较为精确的测算。

在此次实验中，行人不控制步长，在宿舍楼道中进行100 m的直行，最后统计步数步长信息，测算不同步长模型统计出的行人运动距离，试验结果如表7所示。

表7 不同步长模型实验对比

从表中信息可知，在行人自由直线行走情况下，改进后的Scarlett步长模型估计结果误差最小，明显优于其他步长模型。

3 航向估计算法改进

3.1 航向估计分析

Android平台的方向传感器在实际应用中反应灵敏，精度高，主要缺点在于受磁场影响较大，在室外环境下磁场影响较小，但是室外使用GPS导航更为方便精确。在室内环境下，因为各种不同物体对磁场的影响，测得航向角会出现一定误差，且在一段时间行走后，由于航向测算误差过大导致整个行人航位推算算法处于失灵状态[24]；另外，行人手持设备导航时，随着行人身体晃动手机会出现轻微晃动导致测得航向角与真实航向角之间出现误差。这两种现象都会在行人沿着某一方向行走时导致航向误差较大，室内环境越复杂，误差越大。本节在SHS-PDR(Step and Heading Systems PDR)算法框架[25]基础上，设计了一种新算法，适用于磁场影响较小的室内环境。

3.2 改进航向算法

首先引入主导航向的概念，主导航向就是设备认为行人此时行走的方向，从历史航向中推测出来，当行人的航向偏角在合理变化范围内，则认为行人沿直线行走，在此基础上结合卡尔曼滤波对航向信息进行修正，减少误差提高定位精度。改进航向算法如算法3所示，具体步骤如下。

第一步：设定主导航向。

主导航向由方向传感器测算所得航向序列计算可得，取一组航向平均值作为主导航向，定义下式为当前时刻的主导航向。

第二步：辨别是否改变主导航向。

D=(|φ(t)-φ(t-1)|+|φ(t)-φ(t-2)|)/2

其中:φ(t)、φ(t-1)和φ(t-2)分别代表当前时刻和前两个时刻测得的航向，当D小于10度时判定行人沿直线行走，主导航向未变；若大于10度，则判定行人不再沿当前直线行走，改变主导航向，此时清空之前存储航向值，从当前航向值开始重新获取并存储航向序列直至下次主导航向发生改变。

Algorithm3:Improved Heading Estimation

Input:direction signal arrayO

Output:orientationdi,1

Initialize array headSquence;

WhileOis not nulldo

Fetch an elementofromO;

Append o to headSquence;

md=(|o-op|+|p-opr|)/2;

ifp>then

di=Kalman-filtering(headSquence);

else

di=md,op=o,opr=op;

Clear headSquence;

endwhile

在未改变主导航向情况下，计算当前航向与主导航向的差值θ(i)：

θ(t)=|φ(t)-φ(t)′|

第三步：卡尔曼滤波处理。

令状态转移矩阵为单位矩阵，只考虑噪声，得到预测方程 (2)：

Xt∣t-1=Xk-1∣k-1+ωk

(2)

计算误差方差阵 (3)：

Pt∣t-1=Pt-1∣t-1+Qt

(3)

观测方程为(4)：

Z(t)=θ(t)+V(t)

(4)

计算卡尔曼增益，令观测矩阵为单位矩阵如下：

Kt=Pt∣t-1(Pt∣t-1+Rt)-1

然后，结合观测值进行状态估计值进行优化，得到状态估计值，即：

Xt∣t=Xt∣t-1+Kt(Zt-Xt∣t-1)

接下来，更新误差协方差矩阵P(t|t)：

Pt∣t=(I-Kt)Pt∣t-1

从而得到最优估计偏差，进而计算当前航向角最优值并存入下式的航向序列：

3.3 实验结果验证及分析

本节实验主要有两组实验过程组成，第一组实验旨在验证卡尔曼滤波的有效性及算法能否正确识别转向。行人首先手持手机在胸前沿90度方向以正常步速行走，对比滤波前后方向传感器测算所得航向角；之后行人改变航向行走，记录此时行人航向角估计值变化，实验结果如图3～5所示。

图3 滤波前航向角变化

图4 滤波后航向角变化

图5 航向角变化

对比发现使用卡尔曼滤波修正后航向角变化相对平稳，偏移误差也由±4度减少为±2度，表明卡尔曼滤波法有效。

行人在转向前后沿直线行走，主导航向变化平稳，从图5看，对转向反应灵敏，表明改进航向算法有效。

第二组实验设计主要有两个目的：一是对比改进前后的航向估计算法的有效性；二是将改进前后的航向估计算法与第1节的改进计步算法和第2节的改进步长估算模型整合起来，通过在每一步检测中对三者计算结果的融合推算并可视化展示出行人的实时航迹，以验证改进的行人航位推算算法在室内导航定位上的优化效果。

实验地点选取在某室内场所中，房屋长11.42 m，宽10.9 m，设计了由简至繁3种路线，每条线路用红色线条标记，其起点是绿点，终点为红点。第一条路线由前门门口出发，向北步行至客厅中央后转向行走至小屋门口，进入屋内后继续向北行走直至墙面；第二条路线由后门出发，经过4个直角转弯后行至大屋墙面；第三条路线从前门门口出发，进出3个房屋后回到起始原点。将两种算法行人步行轨迹绘制在家中平面图上以便比较，实验结果如图6所示。

图6 改进算法效果对比

图6(a)为方向传感器算法结合前文改进步长估算模型和改进步数算法计算出的行人位移轨迹，子图6(b)为改进航向估计算法结合两种算法计算出的位移轨迹。可以看到在第一、二两条路线中，改进前算法在几次航向改变后就不再准确，而改进后算法表现良好；在第三条路线中，因为行走轨迹复杂，改进前算法已经完全失灵，而改进后算法的推算轨迹基本正确，且终点也几乎落在起点旁，误差在1.2米左右。因此，改进后航向估计算法明显优于改进前。

此外，从图6(b)中还可以看出，融合计步、步长估计、航向估计3种改进方法的综合行人航位推算算法可以实现较为准确的综合定位效果，还不会随着实验路线变得繁杂而导致定位精度降低，这表明改进的行人航位推算算法不仅提高了室内定位的精度，而且具有较好的鲁棒性。

4 结束语

本文在智能手机设备上研究改进室内行人航位推算算法，对行人航位推算的计步、步长估算以及航向估计方法进行改进。本文提出了一种结合自相关分析法和峰值检测法的计步算法，增强了抗干扰能力；提出一种基于Scarlett模型融合前一步步长的改进步长估算模型，相较于Scarlett基本模型具有更高的检测精度；通过设定主导航向并对采样结果进行卡尔曼滤波，减少了航向估计误差。最后，将3种改进方法相结合实现了室内定位系统。实验表明，改进方法取得了较好的室内定位效果。本文提出的算法已应用于某抽蓄电站厂房运维人员定位，因其易于部署、成本低与良好的人员及环境适应性，能较好地满足这一场景下的的定位需求。

本文方法仍有改进空间，这将是下一步的研究工作。对于复杂环境下的行人计步，一是引入气压传感器测算高度信息，将行人的轨迹拓展到三维层面；二是考虑对不同情况下行人加速度数据的特征进行收集，结合机器学习等方法进行识别。航向估计可以结合陀螺仪积分方法进行研究，陀螺仪不受磁场影响，但积分累计误差加上本身漂移误差严重，可以尝试将两种方法结合。对于行人初始位置的确定，可考虑在更精密的设备中将行人在接收不到GPS信号前的最后位置作为初始位置，在行人航位推算算法由于长时间误差累积而精度减弱后，还可结合外源信号定位技术如蓝牙和UWB等辅助获取行人初始位置。