基于压力传感器辅助的行人室内定位零速修正方法

赵小明，邓芳瑾，杨松普，李 巍

（天津航海仪器研究所，天津 300131）

全球导航卫星系统（Global Navigation Satellite System, GNSS）因其可以为陆、海、空三大领域提供实时、全天候和全球性的导航服务，已广泛应用于军事及民用等各个领域，给人们的生活提供了极大的便利。然而在室内、高楼密集的城区道路或地下、丛林等类似的环境中，由于高楼、建筑材料的阻挡或是缺少接收器，GNSS信号将接收不到或接收很差，导致无法进行有效的定位与导航。因此，需要研发出能在室内运行、自主、可靠的定位系统，即室内定位系统。MIMU是一种新兴的且发展非常迅速的微型惯性传感器，具有成本低、体积小、功耗低及环境适应能力强等优点，被普遍应用于室内定位系统的研制。与其它惯性导航定位设备一样，MIMU具有很好的隐蔽性和抗干扰性，不依赖于任何外部信息，也不向外辐射能量，能够在空中、地面以及水下完成自主导航定位的功能，但同时也具有一些弊端，最明显的就是导航误差随时间积累问题，因而长时间工作后会产生不同程度的积累误差，此误差可以通过与一个或多个辅助传感器组合来使它达到有界，提高系统性能。相对来讲，零速修正技术是进行误差控制的一种简单而且有效的手段，其利用载体停止时惯性导航系统的速度输出作为系统速度误差的观测量，进而对其他各项误差进行修正。零速修正算法主要分为两个步骤，即零速检测和零速更新。目前，零速修正技术在车载以及个人导航等领域都得到了广泛应用，而零速检测算法的具体运用也不尽相同，需要根据实际载体、环境等条件选择使用不同的检测算法。

传统的行人室内定位系统如文献[1-3]综合MIMU中加速度计和陀螺仪的输出信息进行零速检测，在步行状态下取得较好定位效果。当跑动时，单纯依靠惯性信息进行零速检测，修正效果较差，若要扩大零速检测算法的适用性，还需引入其他运动参数。文献[4]中利用光学检测装置和压力传感器检测步行时不同压力区域质心的速度，当速度小于阈值时视为零速阶段，得到较好的效果，但其在MIMU和压力传感器外还需要配置光学检测装置，增加了系统的复杂性和成本。本文在传统捷联惯性导航基础上引进了零速修正技术，通过对步态特征的分析，设计了一种压力传感器辅助的多条件约束零速检测方法，采用低成本的薄膜式压力传感器辅助MIMU对零速区间进行判断，提高了行走时零速检测的鲁棒性。通过基于零速修正的卡尔曼滤波估计姿态误差、速度误差和位置误差，校正反馈后提高了系统的导航精度。

1 系统组成

典型的行人室内定位系统由三部分组成：微惯性测量单元、显示控制终端和指挥控制终端。MIMU将检测到的位置、姿态等信息发送到手持控制终端上，通过无线通讯技术将信息发送到指挥控制终端，控制端人员基于在指挥控制终端上的信息向现场人员发送指令。

图1 行人室内定位系统的原理方案框图Fig.1 Principle diagram of indoor positioning system

本文中设计的行人室内定位系统基于经典捷联惯性导航解算方法，对惯性器件的输出信息进行数值积分求解导航参数。惯性导航子单元由小型化、重量轻以及低功耗的MEMS陀螺、加速度计组成，压力检测子单元则选用适合穿戴的薄膜式压力传感器。行人室内定位系统原理方案框图如图1所示。

2 零速修正

图2所示为行走时的步伐周期。步伐周期从中间站立阶段开始，然后进入脚尖离地阶段，接着进入摆动阶段，最后进入脚跟触地阶段，完成了一个步伐周期的行为运动。在脚跟触地到脚尖离地的过程中，只有中间站立阶段是我们要检测的零速时间段。

图2 行走时的步伐周期Fig.2 Stance phase in human bipedal locomotion

2.1 IMU检测条件

步行时，固定在脚部的惯性元件敏感行走过程中加速度和角速度的变化，静止时比力模值稳定在重力附近，比力模值变化与变化幅度相较运动时明显较小，人脚的角速度变化趋于零。由此，我们可以根据加速度计和陀螺的输出来判断零速区间。通过第i时刻加速度计输出的比力信息陀螺仪输出的角速度信息进行零速检测。检测算法包括三个条件：加速度模值、加速度滑动方差和角速度模值，分别记为Q1、Q2、Q3。各条件实现方式如下：

2）比力滑动方差检测，即当滑动窗口大小为N时比力方差小于等于给定阈值时，条件Q2(i)=1。

IMU检测法为多条件复合检测方法，即同时满足以上三个条件时为零速区间：

2.2 压力辅助检测条件

当穿戴者跑动时，中间站立阶段十分短暂，简单对角速度或加速度设定阈值的方法很难将其识别出来。所以，我们引入了足底压力作为零速检测的另一个重要依据。

2.2.1 压力传感器的固定位置

压力传感器选择电阻式薄膜压力传感器（Force Sensitive Resistor, FSR），该传感器由高分子聚合物薄膜构成，可以静态和动态地测试两个表面之间的力，具有纤薄、轻柔、小巧、灵敏度较高等特点，适合足底压力的检测。正常人平均动态足底峰值压力为（2.96±0.66）kg/cm2，系统中采用的FSR圆形有效区域的直径为12.7 mm，压力量程为10 kg。随着表面压力的增加，FSR的电阻值减小。

跑动时脚接触地面的时间一般只有几十到几百毫秒，中间站立阶段则更短，仅仅依靠陀螺仪和加速度计的输出判断零速阶段效果并不理想。但只要脚和地面接触，就会受到压力，我们检测脚触地阶段脚底压力模值对零速阶段进行判断，即当检测区压力达到预定阈值时视为零速阶段。行走时前脚掌和后足跟处压力信息特征较为明显，且重复性与稳定性较好，然而要将压力传感器判断的零速区间和MIMU判断的零速区间大致吻合需要将其放在同一位置，本系统将FSR固定在足底A处，MIMU固定在A处对应的脚面上，如图3所示。

图3 FSR实物及安装位置示意图Fig.3 Force sensitive resistor and its location

2.2.2 压力模值检测

FSR的电阻值随施加在其上的压力变化而变化，并根据电阻材料的不同存在特定的非线性关系。本文采用的FSR传感器感应面受到外界施加的压力时，电阻减小。压力检测模块原理图如图 4所示，其中 R2为滑动变阻器，U1为比较器。比较器的功能是将模拟电压信号与一个基准电压相比较，其两个输入端为模拟信号，输出则为二进制信号。滑动变阻器 R5可分为两部分Ra和Rb，当R1与RFSR的比值大于Ra和Rb的比值，即：

图4 FSR零速检测模块原理图Fig.4 Zero-velocity detector of pressure signal

时，比较器输出端输出一个低电平，此时视为零速阶段。调节滑动变阻器阻值即可改变阈值。通过多次试验测得零速检测效果最佳时滑动变阻器阻值。

综合压力传感器辅助惯性组件进行零速区间的检测，将惯组检测条件与压力检测条件进行―或‖运算：

应用以上提出的零速检测算法对采集的数据进行处理，零速区间检测结果如图5所示，其中：红色实线为IMU检测条件判断的结果，蓝色实线为压力检测条件判断的结果，黑色实线为压力辅助IMU检测条件判断的结果。从图5可看出，IMU检测条件判断的零速区间与压力检测条件相比较小，这是由于阈值设定的不同所致。但根据实践经验，通过改变阈值，在改变阈值加大IMU判断的零速区间的同时也引入了更多的野值。压力辅助IMU的检测结果剔除了前两种条件判断的野值，准确性最高。

图5 零速区间检测结果Fig.5 Detection results of zero velocity intervals

2.3 零速更新

由于单兵定位系统是非线性的，利用扩展卡尔曼滤波对非线性系统中的待估参数X进行线性化，得到线性化的模型，然后再利用标准Kalman滤波进行估计。选取地理坐标系中姿态误差角、速度误差、位置误差、陀螺常值漂移和加速度计常值漂移为状态量取系统速度误差δv为观测量，建立15维Kalman滤波器：

式中：F是根据误差模型和状态矢量构成的15×15维系统矩阵；W为15维系统随机过程噪声序列；V为三维系统随机观测噪声序列；是3× 15维观测矩阵。

在零速阶段人脚的速度为零，因此可以将零速作为外参考速度，应用Kalman滤波器对系统状态进行估计。

2.4 半实物仿真验证

为了验证零速修正算法的有效性，通过Matlab对实测数据进行半实物仿真，观察步行者行进方向的速度，即前向速度。图6中显示的为未经修正和分别通过两种方法修正后的前向速度。未经零速修正时，速度发散，在行走45 s到50 s后就发散到-20 m/s；用IMU条件检测零速区间并进行零速更新后，速度误差大大减小，但峰值均超过4.5 m/s；改用压力辅助IMU条件检测零速区间并进行零速更新后，速度峰值下降到 4.5m/s以下，误差进一步减小。未经零速修正的前向速度发散，经过零速修正后，速度误差明显减小，前向速度收敛，验证了零速修正方法的有效性。

图6 前向速度Fig.6 Forward velocity

3 试验验证

图7为试验中所用行人室内定位导航系统实物图，由手持终端和导航定位单元组成。导航定位单元又包含惯性导航子单元和压力检测子单元，其中：惯性导航子单元包含一个三轴陀螺仪和一个三轴加速度计，陀螺仪的零偏稳定性为14.5 (°)/h，加速度计的零偏稳定性为0.25 mg；压力检测子单元包含了一个单点式薄膜压力传感器。为了验证零速修正算法在行人定位系统中的应用效果，进行了矩形闭合曲线行走试验，通过Matlab对实验数据进行仿真，将分别用两种检测方法得到的结果进行对比，验证压力辅助MIMU零速修正方法的效果，最后在室内环境下进行闭合曲线行走试验，并在上位机实时显示行走轨迹。

共进行两组矩形行走试验，行进距离为176 m。试验1为正常步行试验，试验2为跑动试验。每组试验对MIMU和压力辅助 MIMU两种零速修正方法进行仿真，行进轨迹如图8～11所示。

图7 系统实物图Fig.7 Picture of the system

从表1和表2的试验结果可以看出，融合压力信息的零速检测算法适用范围更广，定位精度有显著提升，试验1（正常步行）的水平定位精度从0.56%D提升到0.19%D，试验2（跑动）的水平定位精度从2.17%D提升到0.84%D。

表1 正常行走试验结果Tab.1 Results of natural walking experiment

表2 跑动试验结果Tab.2 Results of running experiment

在已知平面图的室内环境下进行闭合曲线行走试验，并在上位机实时显示行走轨迹，如图12所示。起点和终点几乎重合，行走轨迹符合实际情况，水平定位精度优于1%D。

图8 正常行走无压力辅助轨迹图Fig.8 Trajectory of natural walking without pressure information aiding

图9 正常行走加压力辅助轨迹图Fig.9 Trajectory of natural walking with pressure information aiding

图10 跑动下无压力辅助轨迹图Fig.10 Trajectory of running without pressure information aiding

图11 跑动下加压力辅助轨迹图Fig.11 Trajectory of running with pressure information aiding

图12 实时行走轨迹Fig.12 Real-time trajectory

4 结 论

本文提出了一种基于MIMU和压力传感器的多条件约束零速修正方法。通过比力模值、比力滑动方差、角速度模值和足底压力模值，综合检测零速区间。该方法易于实现并且可以有效减小MEMS惯性元件的漂移误差，相较单纯依靠加速度和角速度的零速修正方法，其水平定位精度有显著提高，正常步行和跑动下水平定位精度优于1%D。由于条件所限本方法未在跳跃或非平坦路面等运动方式下进行试验比较，定位精度还需进一步试验验证，以实现多种步态的导航定位。

参考文献（References）：

[1] 田晓春, 陈家斌, 韩勇强, 等. 多条件约束的行人导航零速区间检测算法[J]. 中国惯性技术学报, 2016, 24(1): 1-5.Tian X C, Chen J B, Han Y Q, et al. Zero-velocity interval detection algorithm with multi-condition constraint for pedestrian navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(1): 1-5.

[2] 费程羽, 苏中, 李擎. 行人惯性导航零速检测算法[J]. 传感器与微系统, 2016, 35(3): 147-153.Fei C Y, Su Z, Li Q. Zero velocity detection algorithm for pedestrian inertial navigation[J]. Transducer and Microsystem Technologies,2016, 35(3): 147-153.

[3]Ganesh A, Tech M, Ganeshkumar K, et al. Multi sensor for reliable soldier and indoor navigation system[J]. International Journal of Advancements in Research & Technology, 2014, 3(2): 74-80.

[4]Nilsson J O, Skog I, Händel P.Foot-mounted INS for everybody– an open-source embedded implementation[C]//IEEE/ION Position Location and Navigation Symposium. 2012: 140-145.

[5]Nilsson J O, Gupta A K, Händel P.Foot-mounted inertial navigation made easy[C]//International Conference on Indoor Positioning and Indoor Navigation, Busan, Korea,2014.

[6]Wang Z L, Zhao H Y, Qiu S.Stance-phase detection for ZUPT-aided foot-mounted pedestrian navigation system[J]. ASME Transactions on Mechatronics, 2015, 20(6):3170-3181.

[7]Abirami S, Daniel S J. Navigation system for multi floor positioning using MEMS[J]. International Journal of Modern Trends in Engineering and Research, 2015, 2(1):293- 300.

[8]Harle R, Taherian S, Pias M. Towards real-time profiling of sprints using wearable pressure sensors[J]. Computer Communications,2012, 35: 650-660.

[9]李超, 苏中, 朱嘉林. 可穿戴式自主定位技术的零速触发算法研究[J]. 传感技术学报, 2014,27(5): 627-632.Li C, Su Z, Zhu J L. A research about zero velocity trigger algorithm used in the wearable autonomous positioning technology[J].Chinese Journal of Sensors and Actuators, 2014, 27(5): 627-632.

[10]Fourati H. Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[J]. Transactions on Instrumentation and Measurement,2015, 64(1): 221-229.

[11]Xu Y, Chen X Y, Wang Y M. Zero velocity detection algorithm for pedestrian inertial navigation[J].Journal of Chinese Inertial Technology, 2016, 24(3): 325-329.