基于室内合作场景智能识别的行人导航算法

朱超群，赖际舟，吕 品，叶素芬，袁 诚

(1.南京航空航天大学自动化学院, 南京 211106；2.物联网与控制技术江苏省高校重点实验室, 南京 211106)

0 引言

行人导航系统是导航定位领域的一个重要分支，近年来得到了越来越多研究人员的关注，被广泛应用于救援抢险、军事作战等领域。传统行人导航主要采用全球定位系统(Global Positioning System，GPS)定位技术，但GPS信号在室内以及城市环境下存在信号丢失现象，且民用精度较差，无法满足人们室内的导航需求[1-3]。随着微机电系统(Micro-Electro-Mechanical System, MEMS)技术的发展，MEMS惯性测量单元(Inertial Measure-ment Unit，IMU)的体积小、功耗低、质量小、便于携带等优点逐渐凸显[4]，以MEMS-IMU为基础的室内行人导航系统的研究也成为了热点。然而惯性传感器存在着漂移误差，是行人导航位置航向发散的主要误差来源。通过零速修正(Zero Velocity Update, ZUPT)算法，可以抑制速度误差发散并提高导航精度[5-8]。由于航向角误差的可观测性差[9]，ZUPT算法无法对航向角误差进行修正，行人航向及位置会随着时间的累积而发散，最终使得定位结果偏离真实轨迹。

为了降低航向及位置漂移对行人导航的影响，国内外学者对室内行人导航系统进行了广泛的研究。文献[10]提出了利用建筑物走向来约束航向角偏移的算法。文献[11]提出了一种偏航角误差自观测的算法，当行人沿直线行走时，该算法可以用于约束航向角漂移。文献[12]利用WIFI辅助行人导航，并使用无迹卡尔曼滤波对航向角进行了修正。以上方案都存在一定的局限性，即行人需沿直线行走或需额外添置传感器，在较为复杂的室内环境下无法对位置航向误差进行有效约束。文献[13]提出了一种基于斜坡检测的行人导航算法，通过检测行人行走过程中的斜坡场景并与合作环境进行地图匹配来对行人导航定位结果进行校正。文献[14]提出了一种多运动模式下的自适应阈值ZUPT算法，即通过训练机器学习模型，并将该模型用于判别行人当前运动模式，通过判别结果动态调整ZUPT阈值以提高零速判别精度。

考虑到在室内环境下，主要合作场景有上楼梯及下楼梯。因此，本文提出了一种基于室内合作场景智能识别的行人导航算法，通过随机森林算法对行人在室内行走的步态进行学习与分类，从而完成对平地步行及上下楼梯场景的识别。在此基础上，将场景识别结果与室内的先验地图信息进行关联，并利用该关联位置信息对行人轨迹进行校正优化。通过该算法可以有效抑制行人导航误差累积，提升定位精度。

1 基于随机森林的行走场景识别算法设计

当行人于室内步行时，主要行走场景有平地步行、上楼梯和下楼梯。为了实现对行走场景的高精度辨识，本文提出了一种基于随机森林的行走场景识别算法。该算法主要分为模型训练与场景关联2个步骤。

1.1 模型训练

1.1.1 数据分割

对采集到的数据进行精确地分割及截取，可以有效地提高场景识别的精度。在行人步行过程中，足部的运动具有周期性。并且对于单个步态周期而言，通过零速检测可将其分为运动及静止2个状态。因此，通过足部的周期性特征，可以完成对数据的分割，取单个步态周期内足部的运动状态作为该步态的特征取值范围。在利用MEMS-IMU获取得到原始数据(ax,ay,az,a,ωx,ωy,ωz,ω)后，通过捷联惯导解算及ZUPT算法，还可获取得到(px,py,pz,vx,vy,vz,γk,θk,φk)状态量。通过提取以上数据基于时域的特征及基于空间特性的特征，可以获取需要的机器学习特征向量。其中，利用MEMS-IMU进行数据采集的坐标系定义如图1所示。

图1 坐标系定义Fig.1 Definition of coordinate system

1.1.2 步态特征提取

由于行人在穿越楼梯场景时，足部的运动步态特性相较于平地步行明显不同，且在实际测试中发现，MEMS-IMU的各轴输出都会产生不同程度的变化，其中陀螺仪z轴的输出变化最为显著。因此，本文基于时域的具体特征选取如表1所示。

表1 基于时域的特征选取

在不同场景中步行时，由于行人足部高度及水平速度会产生明显变化，因此选择高度和速度等反映空间特性的变化量作为特征量。基于空间特性的特征选取如表2所示。

表2 基于空间特性的特征选取

其中，h(k+1)表示第k+1个步态的初始足部高度，h(k)表示第k个步态的初始足部高度；|v(k)max|表示在第k个步态内的水平速度最大值。

1.1.3 基于随机森林的楼梯场景识别算法

本文通过行人步态特征来完成对场景的识别，由于不同行人的行走步态特征不同，因此若只采用单一阈值无法对场景进行高精度识别。为了提高场景识别精度，本文提出了一种基于随机森林的场景识别算法。随机森林是一种以多棵决策树为基础，并通过集成学习后得到的一个强分类器，其最终结果是通过森林中的每棵决策树投票决定的。随机森林算法具有简单、易于实现、计算开销小等优点[15]。其原理示意图如图2所示。

根据图2可知，该场景识别算法的构建主要分为3个步骤：

1)利用Bootstrap采样为每棵决策树生成训练集

通过数据采集、数据分割及特征提取，获取得到训练集D。其中，D=(X,Y)={(xi,yi),i=1,2,…,m}，此处X为获取的步态特征矩阵，Y为对应的期望输出，m为输入的样本个数。xi=[xi1,xi2,…,xid]T，表示X中的第i个样本,d为输入样本的维数。Bootstrap采样方法即从原始数据集D中随机有放回地抽取与D同样大小的训练样本(D1,D2,…,Dn)，利用每个训练样本Di来构建一棵决策树。

2)构建决策树

图2 随机森林算法结构示意图Fig.2 Schematic diagram of random forest algorithm

节点分裂是决策树生成过程中的核心步骤。在本文的决策树构建中，使用的为分类和回归树(Classification And Regression Trees， CART)算法。CART算法在进行节点分裂时，采用的分裂规则是Gini值最小原则，其计算公式为

(1)

式中，pi为样本点属于第i类的概率。于是，通过在候选属性集合Ak中，选择使得划分后Gini指数最小的属性作为最优划分属性，完成了节点的分裂。通过对该步骤进行循环迭代，便完成了一棵决策树的训练。

3)随机森林的形成及算法执行

通过训练多棵决策树构成随机森林算法。在本文中，将平地步行场景标记为0，上楼梯场景标记为1，下楼梯场景标记为2。在算法执行过程中，输入当前行人的步态特征，利用训练完成的决策树对步态进行投票判别，最终，将得票最多的结果记为当前场景识别结果。

1.2 场景关联

若当前的场景被识别为上下楼梯场景时，则需将行人位置与对应的楼梯位置进行关联，因此需要事先建立楼梯位置数据库。在该算法内，将楼梯位置定义为进入楼梯时第一个台阶中心的位置。获取室内各个楼梯所对应的楼梯位置信息database={(xp,yp),p=1,2,…,s},其中s为室内楼梯的数量，将该数据以矩阵形式记录，并记其为楼梯位置数据库。对于同一个楼梯场景而言，上下楼梯位置点较为接近。为了减小误关联现象，将数据库分为2个，1个为上楼梯的数据库databaseu，1个为下楼梯的数据库databased。

行人在室内步行过程中，利用训练获取的分类器对行人所在场景进行识别，当行人在步行中且场景识别到行人进入上下楼梯场景时，即

(2)

其中SR(k-1)代表上一个步态的场景识别结果，SR(k)代表当前步态的场景识别结果。式(2)表示行人由平地步行进入上下楼梯场景时，需识别行人所在的特定楼梯。获取数据库中与行人当前位置最近的位置坐标，选取该点作为关联的楼梯位置点。

2 基于行走场景校正的行人导航算法设计

2.1 零速检测算法

由人体运动学可知，行人在步行过程中，双足的运动可近似为周期性运动。在一个步态周期内，主要可以分为5个阶段：脚跟触地，脚掌触地，站立，脚跟离地和脚掌离地[16]。在站立时间段内，行人足部的速度趋近于0，因此，将站立时间段定义为行人步态中零速区间。静止阶段的持续时间约为0.15～0.35s[17]，在此期间内，加速度计的理论输出在g值附近，陀螺仪的理论输出趋近于0。

为了能够精准判断零速时刻，提高检测精度，本文采用三条件判别法，即通过三轴总加速度检测、三轴总加速度方差检测和三轴总角速度检测联合实现零速时刻判别。以“1”表示检测结果中的零速度状态，“0”为非零速状态。k时刻零速判定公式为

(3)

(4)

(5)

(6)

(7)

(8)

其中，thamin、thamax、thωmax和thσmax为判别阈值，N为滑动窗口宽度。以上三种判别条件中，采用单一判别均会出现不同条件的误判别，无法得到精确的零速区间。因此，通过对其结果取与运算，即ZUPT(k)=C1(k)&C2(k)&C3(k),可以提高零速检测的准确率。

2.2 融合滤波器设计

本文的融合滤波器结构图如图3所示。首先，系统采集行人足部的MEMS-IMU输出，根据MEMS-IMU的输出判别零速时刻并运用卡尔曼滤波对行人的速度进行修正；其次，当行人在室内行走时，利用训练完成的模型对场景进行识别，若检测到当前的场景为上下楼梯，即可进行场景关联；最后利用行人当前位置与最近的楼梯位置的偏差作为观测量，通过卡尔曼滤波进行误差估计，提升定位精度。

图3 融合滤波器架构Fig.3 Fusion filter architecture

本文建立的状态方程及量测方程如下

Xk=φk,k-1Xk-1+Γk-1Wk-1

(9)

Zk=HkXk+Vk

(10)

式中：Xk=[δpkδvkδΨk],δΨk为3个姿态角误差，δvk=[δvxδvyδvz]为三轴速度误差，δpk=[δpxδpyδpz]为三轴位置误差；φk,k-1为系统的状态转移矩阵；Γk-1为系统噪声矩阵；Wk-1为k-1时刻的系统噪声，Zk为观测量；Hk为k时刻的量测矩阵；Vk为k时刻的量测噪声矩阵。并有

(11)

(12)

在本文中，将传统的ZUPT算法与基于场景识别的定点校正算法相结合，通过零速检测及场景识别引入了速度误差与水平位置误差2个观测值。当行人在行走过程中处于站立状态时，速度误差可以直接被观测到，如式(10)所示

(13)

其量测方程如下所示

Zv_k=HvXk+Vv

(14)

(15)

(16)

当成功检测到行人进入楼梯时，则利用行人当前位置与数据库内取得位置点的偏差来估计水平位置误差，如下

δPxy=Pxy(k)-Pdatabase(k)

(17)

其中，Pxy(k)为当前的位置，Pdatabase(k)为从数据库取得的位置点。为了避免由于误检测而造成定位与楼梯的误关联，此处加入了一个防止误检测的步骤，即若检测到进入上下楼梯场景时行人位置与数据库中最近点的距离大于阈值thst，则不进行校正。此处，设置阈值thst为

thst=0.03SPD

(18)

其中，SPD为上一个位置修正点到行人当前位置的步行距离。若选取的位置点满足要求，则将δPxy作为系统的观测值，并利用卡尔曼滤波修正位置误差。

其量测方程如下所示

Zp_k=HpXk+Vp

(19)

(20)

(21)

3 实验验证

3.1 实验条件

本实验采用的传感器为荷兰Xsens公司生产的型号为MTW awinda的惯性器件，其主要性能指标如表3所示。

表3 MTW awinda参数

在实验时将该惯性器件安装在脚背处，该MEMS器件及其安装方式如图4所示。

图4 MEMS器件及其安装方式 Fig.4 Installation of MEMS devices

在实验过程中，通过绕行实验楼一、二层楼走廊及楼梯进行数据采集，其中每层楼共有4个楼梯口。实验场景如图5所示。

图5 实验场景Fig.5 Experimental scene

3.2 实验结果分析

3.2.1 行人行走场景识别试验

为了利用RF算法训练模型并测试其分类效果，在实验场地进行了多组数据采集。本实验中共有10名测试人员，其中有6名男性，4名女性。每位测试人员分别在实验楼里步行、上楼梯和下楼梯。在数据采集过程中，不对测试人员进行步态约束，测试人员均按个人习惯步行。采用10-折交叉法对算法性能进行评估，即取9位测试人员的数据集作为训练集，另外1位测试人员的数据集作为测试集，识别精度如表4所示。

表4 场景识别精度

根据表4中10-折交叉测试结果，其总识别率为99.8%，该识别成功率说明了本算法能够对场景进行有效识别，进而根据其识别结果对行人导航进行位置修正。

3.2.2 行人定位精度试验

在本组实验中，由1名实验人员在实验场地内绕着走廊行走，并伴随有平地步行、上楼梯和下楼梯，最终走回预设的起点处。分别在走廊不同位置设置标志点，行人在行进过程中需穿越标志点并记录对应位置，用于与标志点真实位置进行对比评估导航精度。本次实验共穿越了16次标志点，总行走里程为1100m。在相同的环境下，分别采用传统算法和本文提出的算法分别进行解算，得到的结果分别如图6及图7所示。其中，上图为解算结果平面示意图，下图为三维示意图，实验整体定位精度如表5所示。可以看出，因为传统ZUPT算法无法对航向误差进行补偿修正，因此在行人行进过程中位置及航向漂移较大，使得整体导航轨迹图误差较大，偏离了真实轨迹。在加入了本文提出的基于室内合作场景智能识别的行人导航算法以后，定位精度显著提高。该方法与传统ZUPT算法相比，有效地解决了行人在室内长距离行走状况下由于位置及航向发散导致定位不准的问题，显著提升了导航系统的定位精度。

图6 传统算法解算结果Fig.6 The computed result of traditional algorithm

图7 本文提出算法解算结果Fig.7 The computed result of improved algorithm in this paper

表5 定位精度对比

4 结论

针对传统的基于足绑式MEMS-IMU的行人导航存在漂移误差，导致行人定位误差较大的问题，本文提出了一种基于室内合作场景智能识别的行人导航算法。在传统 ZUPT算法的基础上，通过训练机器学习模型，并利用该模型对室内合作场景进行智能识别，根据楼梯位置对行人位置进行修正。在实验中，利用该算法可以有效地将导航位置误差由11.45m降为1.85m，验证了算法的可行性，取得了较好的定位精度。