基于异构多源传感的行人长航时变步长误差修正技术

谢金池，王 伟，文丹丹，路永乐，王汶新，刘 宇*

(1.重庆邮电大学生物信息学院，重庆 400065；2.重庆邮电大学光电工程学院，重庆 400065)

载体的姿态信息和速度信息是确定物体在空间运动状态的重要信息，人们通过它们可以更加便捷地确定物体的运动方向和运动距离。惯性测量单元(inertial measurement unit，IMU)是测量物体三轴姿态角(或角速率)以及加速度的装置，被广泛地应用于航天、军事、航海、医学可穿戴式等领域[1]。陀螺仪及加速度计是IMU的主要元件，其精度直接影响到惯性系统的精度。微机电系统(micro-electro-mechanical system,MEMS)惯性器件因其体积和成本的巨大优势，已展现出良好的应用前景[2]。低成本IMU仍然受到高噪声水平和短期稳定性的影响，经研究，影响其IMU精度的因素有惯性传感器的确定性误差和随机性误差，其中最重要的便是确定性误差中的零位漂移误差[3]。

刘宇等[4]针对陀螺零偏会影响其导航精度问题。提出通过离线校准方式，设计了静态陀螺零位补偿的算法，提高了在短航时下的定位精度。许钰龙等[5]采用MEMS陀螺仪绑于脚面，判定零速时刻，提出航向自纠正算法，进行动态零位补偿，定位精度可达到2%。张伦东等[6]利用MEMS陀螺仪以及MEMS加速度计组合的方式嵌入鞋底，分析了零速修正的行人导航算法，在足底零速状态下进行步长以及航向的优化。

由于IMU的零位漂移所产生的误差是姿态测量系统的主要误差，因此需要在动态过程中对IMU进行零位补偿的校准工作。以上学者的研究成果，在长航时下会出现定位精度不高以及在判定零速时刻会出现错判漏判等状况，基于行人足底姿态的特征，提出加入压力传感器，与IMU组合嵌入鞋跟，设计多约束条件的零位捕捉方法，满足不同步态下的零位精准捕捉，并在零速时刻下通过误差修正算法补偿姿态测量系统的误差，优化步长及航向，达到提高定位系统精度的目的。

1 系统设计框架

图1为系统整体框架，首先采集陀螺、加速度计以及压力传感器的数据，通过陀螺以及加速度计的输出值进行姿态解算，得到载体的实时速度和位置信息[7]。并且根据行人足底姿态的特征，通过陀螺、加速度计以及压力传感器的输出值实时判断载体是否相对静止，当检测为非静止时，不对零位做任何操作，只更新时刻；当检测为静止时，理论上此时陀螺的输出值为零，加速度计的输出值为重力加速度，压力传感器的输出值处在一个峰值附近，触发零位捕捉算法，并对系统误差进行修正，最后进行姿态解算，更新速度位置信息，实现系统高定位精度的目的。

图1 系统设计框架图

2 算法设计

在进行姿态解算时，如果不对零位进行有效的捕捉以及补偿，最终更新得到的速度位置信息与真实信息会有较大偏差，系统误差会随着时间不断累积，导致定位功能失效[8]。在行人行走过程中，会有足底与地面完全接触的一段时间，称为相对静止阶段[9]。利用行走过程中脚着地时刻的速度为零，采用本文设计的零位捕捉以及补偿算法，并将补偿后的零位反馈给惯性定位单元进行误差校准，实现高精度定位功能。

2.1 行人步态分析

行人足底姿态特征如图2所示。对行人正常行走时的足底姿态特征进行分析可得，这是一个周期性的运动，这一周期性运动大致可分为三个阶段：足跟着地，全足着地以及足跟离地。因足跟着地到全足着地的时间可忽略不计，可认为足跟着地到足跟离地这一段时间为全足着地时间，也就是相对静止阶段(零速时刻)。

图2 行人步态周期图

2.2 多条件静止检测算法设计

当使用惯性导航算法时，速度和位置误差会随着时间累积，如果不对这些错误进行修改，则导致定位失败[10]。对于正常的步行，有一个每一步接触地面时静止不动的时刻。这种静止的力矩也称为零速度间隔。准确检测零速度间隔和对误差的实时修改间隔可以有效地抑制误差。在这项研究中，我们使用阈值方法结合了加速度、陀螺以及压力计的模块值。

2.2.1 陀螺仪与加速度检测法

行人正常行走，当全足着地时，处于相对静止时刻，陀螺的理论输出值为零，加速度的理论输出值为重力加速度。传统的阈值方法仅假设加速度应是重力加速度，容易出现对静止时刻判断不精准等情况，因此，提出一种组合条件方法，可通过三轴陀螺输出值的平方和以及局部加速度方差选择不同的阈值，计算公式为

(1)

(2)

(3)

(4)

式中：ωk(i)为三轴角速度信息；ωk为对三轴陀螺仪数据平方和处理后的值；tωmax和tωmin为陀螺仪零速时刻阈值；fω为陀螺仪的静止时刻判断条件；a(i)为三轴加速度计加速度信息；ak为对三轴加速度计数据方差处理后的值；tamax和tamin为加速度计静止时刻阈值；fa为加速度计的静止时刻判断条件。

2.2.2 压力计检测法

上述提出的加速度计和陀螺仪检测法，对于行人导航定位精度的提升并不理想，因此，设计加入压力计进行辅助判断零速时刻。

压力计大多体积薄、质量轻，所以能够很好地嵌入在鞋跟，目前常用的压力计主要分为三类：压电式、压阻式、电容式。常用的压电式和和电容式压力计的实时性并不好，响应会滞后，最终导致行人运动状态下的足部压力值会失真。而压阻式传感器的材料是以单晶硅为基础，具有实时性好、灵敏度高、动态响应快等优点。因此本设计采用压阻式压力传感器，能够很好地敏感行人足底压力变化情况[11]。

对压力计的数据处理方法，与加速度计和陀螺检测法类似，为了准确检测行人步态轨迹，传统的方法是设置一个固定的压力阈值，当压力计敏感到的压力值超过这个阈值，则表示足部已经着地，若没有超过，则表示足部离地。但此方法有一个弊端，压力计对行人足部着地时敏感到压力值会随着不同的行人出现不同的压力值，并且在不同的环境，压力值也会出现不同程度的变化，阈值不好设置，所以此方法并不实用。设计一种以压力计的历史数据为基础的可变阈值调节法，公式为

T(k)=min[lG(k)]+α{max[lG(k)]-

min[lG(k)]}+β

(5)

lG(k)={F(k-l+1),F(k-l),…,F(k)}

(6)

式中：T(k)为第k个点所对应的阈值；α为加权系数；β为偏置量；lG(k)为包含l个压力数据且连续的数据集；F(k)是第k个点的预测压力值。

l、α、β共同决定这种可变阈值的大小和变化率，即使在不同行人穿戴以及不同环境下的运动，波形会发生变化，但可变阈值也会发生改变适应不同的环境，通过可变阈值就能测得足底是否离地或是着地。但是实验证明，如果只通过阈值判断，行人在高速运动下的压力值由于波动大的原因，经常性出现连续压力值峰值穿过阈值，会导致零速错判。通过观察行人步态周期可得一个规律，在一个周期里，同一个步态只能出现一次，也就是零速时刻在一个周期里只会出现一次。因此，本文设计一个窗口，窗口的长度由一个步态周期的长度决定，主要作用是将窗口里的出现波动的引起错判的压力值舍弃掉，窗口的长度也是一个可变值，公式为

W(n)=μϑ(n)， 0<μ<1

(7)

ϑ(n)=k(n)-k(n-1)

(8)

式中：ϑ(n)表示第n次步态周期的长度；k(n)表示第n次步态周期的第k个点的值，而得到步态周期长度之后，就能得到窗口的长度W(n)；μ为窗口系数，决定着窗口的长度。

可以得到足底着地时刻的判断条件fp为

(9)

可得三个判断静止时刻的条件fa、fω、fp。若采用与操作，则条件太过严格，容易将一些本来达标的时刻漏判，若采用或操作，容易误判。因此，本文通过概率学知识，设计三个条件只要满足其二，即可满足条件，以fp为基础条件，在满足fp条件的前提下，只要再满足fa和fω任意一种，就可满足判断静止时刻的条件。此操作既不会发生错判、漏判等问题。经过后续实验验证，此方法是可行的。判断条件中各个系数的值需在使用中根据实际情况进行调整。

2.3 误差修正算法设计

采用卡尔曼滤波算法对行人足底静止时刻进行零速修正。但是由于卡尔曼滤波器的量测更新只在静止时刻，这样就会限制滤波器的估计效果，主要体现在方位误差角不能得到有效估计。这样，随着导航时间的延长，方位误差的影响就会逐渐取代器件误差，成为MEMS个人导航系统的主要误差来源，为解决这一问题，采用粒子滤波算法对航向角以及步长进行优化。

2.3.1 零速修正

零位补偿算法是基于卡尔曼滤波模型理论构建的导航误差方程。经SINS简化后的误差模型为

(10)

(11)

式(11)中：X(t)为t时刻的系统状态；F(t)为t时刻对系统的控制量；W(t)为t时刻的噪声；Zk为k时刻的测量值；H为系统测量的参数；Xk为k时刻的系统状态。

卡尔曼滤波由静止时刻检测触发，卡尔曼滤波由静止时刻间隔在每个步骤的开始处触发。卡尔曼滤波离散化的时间不固定。将每一步的持续时间作为卡尔曼滤波。当检测到每一步足底静止时刻的开始时，进行卡尔曼滤波。

2.3.2 步长优化

粒子滤波算法是通过使用一组加权的随机样本来近似表示系统状态p(Xk，Z1:k)的后验概率分布[13]。新的状态分布可以通过这些随机样本的贝叶斯递归，公式为

(12)

(13)

有效粒子Neff可定义为

(14)

(15)

(16)

3 实验平台搭建

整个系统可分为上位机和下位机两部分，下位机主要包括小型IMU和压力传感器，惯性器件选择SCR2100，它包含陀螺仪和加速度计，压力传感器选择IMS-C20A，如图3所示。

图3 IMU-I300和IMS-C20A压力计

将IMU和压力传感器嵌入在鞋跟。将硬件嵌好之后，上位机和下位机通过433 d线模块进行通信，上位机实时接收传感器解算后的数据，得到位置信息，并实时在3D界面显示出来。图4为行人定位系统实物图。

图4 行人定位系统实物

4 实验验证

为验证行人步态检测算法和误差修正算法的可行性以及最后考量行人定位导航的精度，需要对本文所设计进行验证。

首先验证行人在运动时，各个传感器的数据是否满足所设计的步态检测算法。行人先做一段直行运动，根据前面行人步态规律可知，传感器数据会成一个周期性变化的趋势。图5为陀螺仪数据仿真图，在足底着地时刻，陀螺的数值在零附近，即可在此处设置阈值进行零速时刻判断。图6为角速度计数据仿真图，在足底着地时刻，加速度计的数值在重力加速度附近，满足本文所设计步态检测算法。图7为压力计数据仿真图，当足底离地时，压力传感器的数值是处在一个峰值附近变化，当足底着地时，对地会产生压力值。即可利用上文的理论进行行人步态的判断。

图5 陀螺数据仿真图

图6 加速度计数据仿真图

图7 压力计数据仿真图

为了验证本文所设计的零速检测算法的可靠性以及准确性，提前规划出某行人需要走100步的路程，即理想状态下有100次零速状态。做10组实验，检测在本文所设计的算法基础上能检测出零速状态的次数，结果如表1所示。

表1 零速状态检测次数

当行人步态检测算法满足要求之后，下一步需要检测的是零位是否得到有效的补偿。采集陀螺 1 h 未经处理的零位输出以及经过本文所设计的零位补偿算法之后零位的输出。如图8所示。红色的数据即为经过算法补偿之后数据，虽然随着时间的增加，也有一定的漂移，但是对比原始零位的漂移，零位是得到有效的补偿。

图8 零位输出仿真图

最后一步验证在零速检测算法以及滤波算法的基础上，行人定位导航精度是否得到有效的提升。规划出一段环形路线，以传统的足绑式导航与本文的多传感器零速判断的定位导航的方式绕此路线行走各两圈，采集数据并做仿真，结果如图9所示。观察其导航精度。

图9 行人导航对比图

5 结论

针对MEMS惯性定位导航系统中陀螺仪零位会随着时间产生偏移以及传统足绑式导航定位系统对于零速时刻的判断会出现错判和漏判等问题，根据行人足底姿态的周期性变化，本文提出一种以压力传感器辅助的结合惯性传感器的多源异构传感检测零速时刻的方法，在此基础上，建立卡尔曼滤波算法以及粒子滤波算法。有效地对微惯性器件的零位进行了补偿，提高了行人定位导航的精度，精度提高一个数量级。