多传感器行人航位推算算法研究*

陈新东 蔡 鹏 高大远 王 超 朱海荣

（海军潜艇学院 青岛 266000）

1 引言

全球定位系统（GPS）能提供三维位置和速度信息，已经广泛应用于人们的日常生活中，但GPS信号容易受卫星信号的衰减以及干扰信息的影响，在室内、地下建筑以及城市街道等环境下信号弱，信息可用性差，GPS不能进行准确的定位［1］。目前有很多学者对室内定位技术进行了研究，如WiFi定位技术、超宽带技术、超声波技术、视觉定位、红外技术、FRID定位、蓝牙技术等定位传感技术［2～7］。然而多传感器的室内定位技术是比较热门的一种定位手段。当前，国内外有不少学者对多传感器定位技术进行了研究。Foxlin等开发了”NavSheo”的定位系统［8］；邓林坤等设计了低成本多传感器的室内行人航位推算（PDR）系统，并且在ipad上进行了实验［9］；Ruiz和 Madrid 等利用惯性导航系统与RFID定位系统进行结合实现了室内行人的定位［10］。

本文提出了一种基于足绑式的多传感器PDR算法，并采用磁角速率更新（MARU）很好地克服了磁干扰的影响，利用惯性测量单元（IMU）对行人的线运动以及角运动等信息进行采集，同时利用三轴磁力计来计算得到对应的磁感应电势信息，采用EKF算法对各个传感器的误差进行补偿与修正，最终推算得到行人的位置。

2 初始对准

静止状态时，利用加速度计与陀螺仪的输出进行初始对准。加速度计测量重力加速度，可据此来计算俯仰角和横滚角。陀螺仪测量地球自转角速度，可计算得到航向角。但是对于低成本的MEMS IMU来说，MEMS IMU陀螺仪的漂移非常大，用陀螺仪的输出来计算航向角，其误差非常大。因此本文采用静止条件下加速度计和磁力计获得初始IMU的姿态角，导航坐标系（n系）为北西天坐标系，载体坐标系（b系）固连在传感器上。一般情况下，由加速度计和磁力计的输出可得：

3 系统模型

3.1 状态模型

行人导航系统状态方程与状态向量呈非线性关系，可以进行线性化处理［11-13］。系统误差状态向量可以表示为

式中：δΨ 、Δω 、δr、δṙ、Δa分别代表k时刻的姿态误差信息，陀螺零偏信息，位置偏差信息，速度偏差信息，加计零偏信息，总共15维信息当做系统的误差状态向量。

经过线性化处理得状态方程：

式中：Xk-1代表k-1时刻的误差状态向量后验估计，Xk代表k时刻误差状态向量先验估计，Wk-1代表k-1时刻的过程噪声，它的协方差矩阵是Qk=E(WkWkT)，Φk/k-1则代表状态转移矩阵。其表达式如下：

其中：

ab(k)=Acc(k)-Δa为EKF修正后的比力，为k时刻加计的量测值，是k时刻b系经过坐标变换到n系方向余弦矩阵的先验估计。

3.2 量测模型

当足部处于静止或者速度为0时，此时相对于地面，速度接近于0，ZUPT算法把速度误差Δvk,k-1=vk,k-1-[0,0,0]T作为EKF的观测量。

在室内这种环境下，磁的干扰不容易估计，很容易会受到室内建筑等设施的干扰从而引入更多的误差，为了避免外界磁干扰的影响，引入MARU来进行航向修正，即行人足部处于零速或静止时，磁感应强度恒定不变。而MEMS转动时，三轴磁力计分量会发生旋转，且旋转速率与陀螺仪角速率ω(k)变化有关，所以将磁感应强度变化作为量测矢量，对ω(k)进行误差估计补偿来抑制航向角的发散。

当MEMS转动时，磁增量为

式中：Bb(k)和Bb(k-1)分别表示k时刻和k-1时刻的在载体坐标系下磁感应强度。

4 步态检测

行人行走时，走的步态是周期性变化的；本实验对行人行走的数据进行采集，每个传感器的三个轴都具有周期性变化的特征。本文利用三个传感器，分别是加速度计、陀螺仪以及磁力计，通过三者结合对行人行走进行检测测定。

其中γα加速度幅值阈值，γω为角速度幅值阈值，Δt为采样时间间隔。

通过上述判定条件，行人步态检测如图1所示。

图1 步态检测

文中仅仅截取了一小段步态检测数据用来进行描述与分析，图中X轴代表传感器采样样本数，Y轴代表数值大小，无固定含义。从图中可以看出，行人行走步态呈现出周期性变化规律，步态静止时，行人静止，步态移动时，行人则处于移动状态。

5 实验数据结果分析

实验采用的IMU是XSENS公司的MTI产品，实验在室内完成，本次实验将IMU绑在实验人员的足部，依据已经设定好的路线行走，总共做了两次实验，分别沿着不同路线行走，采样频率设置为100HZ，图2和图3分别为Matlab软件下仿真的实验结果对比图。

图2 总路程28.8m测试对比结果

图3 总路程110.7m测试对比结果

为了更加清晰地看出文中算法的效果，对两种方法产生的定位误差进行相应的统计，如表1所示。定位误差采用实验路线终点值减去路线真实终点值，得到的绝对差值占总路程的比值来进行表示。表1记录了两种不同方法的对比，其中初始对准时间为120s。从表中定位误差结果对比可得：在EKF基础上的ZUPT+MARU融合的方法，对位置误差具有改善作用，定位精度可以得到一定的提高。

表1 100HZ两种算法IMU数据的计算结果比较

6 结语

文中提出了一种多传感器行人航位推算方法，该方法可以减少加计零偏，同时修正行人步态误差；对陀螺仪角速率进行误差估计补偿，抑制了外界磁干扰，增强了对行人航向跟踪性能；通过引入ZUPT+MARU融合构建量测模型，有效地修正行人行走时航向的偏差，实验结果表明：在短时间内，该系统能够满足行人在室内的定位需求，可以实现短距离的导航，位置误差能够控制在0.56%范围内，定位效果良好。