基于改进航向估计的行人航迹推算研究*

孙 艳， 樊启高， 孙璧文， 庄祥鹏

(江南大学 物联网工程学院，江苏 无锡 214122)

0 引 言

随着室内定位系统的快速发展，越来越多的技术开始兴起。如WiFi技术[1]，射频识别(radio frequency identification,RFID)技术[2,3]、超声波技术[4]、ZigBee技术[5,6]。但无线传输不能保证在所有条件下的定位的稳定性和准确性。因此，基于微机电惯性测量单元(micro-electro-mecha-nical system inertial measurement unit,MEMS IMU)[7]的行人室内定位技术(pedestrian dead reckoning,PDR)[8]兴起。PDR具有计算简便,成本低，发展空间大等特点。

PDR算法大多基于零速更新(zero velocity update,ZUPT)[9]建立不同的惯性导航系统，得到定位信息。但基于ZUPT的定位算法存在缺点:零速点未被完全提取，MEMS IMU的累积误差导致判断零速区间错误等。因此，本文提出了基于改进航向估计的行人航迹算法，利用步频、步长和航向计算出行人的位置，其中航向估计采用了基于四元数算法，并采用扩展卡尔曼滤波[10](extended Kalman filtering,EKF)对航向角的偏差进行修正，提高了定位的精度。

1 行人航迹推算

行人航迹推算由步频估计、步长估计以及航向估计组成。如图1所示，通过加速度计的输出值计算出步长以及估算出行走的步数。航向估计则通过加速度计、陀螺仪和磁强计的输出值计算得到，并利用EKF实时修正航向角的偏差，得到精确的航向角，提高了行人定位的精度。行人定位公式如下

(1)

(2)

式中ei与ni分别为行人东向和北向的步长；Ψk为每一步东向和北向的夹角；k为行走的步数。

图1 行人航迹推算的结构框图

1.1 步频估计

步频估计算法包括峰值检测法[11]、站立测频法[12]、零点交叉法[13，14]等。本文采用刘程的自适应计步检测算法[15]，利用加速度输出的Z向峰值来临时间不同，采用自适应时间窗的检测算法。

1.2 步幅估计

步幅估计算法通常有2种：1)根据经验，设定行人的步幅为固定值，这种做法会存在很大的累积误差；2)对行人的步幅进行动态的计算，本文利用加速度计输的峰值进行计算。公式如下

(3)

式中az max为在本时段加速度计的最高值；az min为本时段的加速度计的最低值；Lk为根据行人的行走特征所设定的固定值。

1.3 航向估计

航向估计为本文提出的行人航迹算法中是最重要的步骤，对行人的定位有重要的影响。首先，俯仰角θ和横滚角φ可以通过加速度计得到[16]

(4)

(5)

(6)

(7)

(8)

式中ax，ay，az为加速度计X，Y，Z轴的输出值，gn为重力加速度；θ为俯仰角；φ为横滚角；ψ为航向角；hx，hy，hz分别为磁强计X，Y，Z轴的输出值；δD为本地的磁偏角。

2 EKF

通过加速度计和磁强计计算的俯仰角、横滚角、航向角存在着累积误差，影响行人定位的精确性，因此,利用EKF修正航向角的偏差。

基于四元数的姿态矩阵

(9)

(10)

四元数更新公式

(11)

(12)

式中q=[q0q1q2q3];wx,wy,wz分别为陀螺仪X，Y，Z轴的输出值;dt为采样时间。

卡尔曼滤波的状态值X=[q0q1q2q3]T;量测值Z=[θφψ]T;根据文献[10]可得状态矩阵

(13)

量测矩阵

(14)

系统干扰方差矩阵为

(15)

量测噪声方差矩阵为

(16)

3 实 验

为评估提出算法的定位效果，需采用惯性航姿系统Xsens Mti300进行相关实验，如图2所示为IMU硬件整体结构以及安装方式。陀螺仪、加速度计、磁强计、温度传感器和气压传感器等作为从设备挂载内部集成电路(inter-integrated circuit,I2C)总线，再通过数字信号处理器(digital signal processor,DSP)将数据从串口中输出到计算机上，得到方向、加速度以及转速。

图2 IMU结构及安装方式

图3为滤波前、后的航向角与真实航向角的对比，实验结果表明：航向角动态误差为-120.012°～149.0313°，但平均误差仅3.5699×10-6(°)，说明有极少个数据误差特别大，其他的则较小，残差率为0.0041，置信度为99.59%，因此，航向角的精度满足行人定位的需求。

图3 航向角对比

图4为轨迹对比，实验表明：误差范围为-0.3059～0.2415m,残差率为0.0172，置信度为98.28%，本文提出的行人定位算法具有准确性和稳定性。

图4 行人轨迹对比

4 结 论

通过IMU的陀螺仪、加速度计和磁强计的输出值，得到步长、步数。基于四元数法计算航向角，并采用EKF修正航向角的偏差提高航向角精度。实验结果表明：航向角的置信度为99.59%，定位轨迹动态误差为-0.3059～0.2415m，置信度为98.28% ，满足实际系统的要求。未来的工作，需要在更加复杂的环境下进行实验，并对滤波算法进一步改进，构建紧耦合模型，提高定位精度。

[1] Ma R,Guo Q,Hu C,et al.An improved WiFi indoor positioning algorithm by weighted fusion[J].Sensors,2015,15(9):21824-21843.

[2] 王小辉,汪云甲,张 伟.基于RFID的室内定位技术评述[J].传感器与微系统,2009,28(2):1-3.

[3] 郝金平，张建寄.超高频RFID发卡机射频匹配网络研究[J].自动化技术与应用，2016,35(5)：59-63.

[4] Hazas M,Hopper A.Broadband ultrasonic location systems for improved indoor positioning[J].IEEE Transactions on Mobile Computing,2006,5(5):536-547.

[5] 王 静,张会清.基于ZigBee的无线网络定位技术的研究与实现[J].传感器与微系统,2010,29(2):13-15.

[6] 罗 欢，张 健，席文平.基于ZigBee技术的足球机器人通信系统设计[J].无线电工程，2015,45(5)：16-19.

[7] 钱伟行,朱欣华,苏 岩.基于足部微惯性/地磁测量组件的个人导航方法[J].中国惯性技术学报,2012,20(5):567-572.

[8] 殷 红.基于foot-mounted的IMU室内行人航迹推算研究[D].南昌：南昌大学,2013.

[9] Lin T,Li L,Lachapelle G.Multiple sensors integration for pedestrian indoor navigation[C]∥2015International Conference on Indoor Positioning and Indoor Navigation(IPIN),IEEE,2015:1-9.

[10] 赵 斌，窦灵平，袁立军.卡尔曼滤波算法的硬件实现研究[J].无线电工程，2016,46(6)：65-68.

[11] 刘 程,阳 洪.自适应计步检测算法研究[J].压电与声光,2015,37(2):258-261.

[12] Levi R W,Judd T.Dead reckoning navigational system using accelerometer to measure foot impacts:US,US5583776[P].1999—12—06.

[13] Yan L,Wang J J.A robust pedestrian navigation algorithm with low cost IMU[C]∥2012IEEE International Conference on Indoor Positioning and Indoor Navigation(IPIN),Sydney,NSW,2012:1-7.

[14] 陈 伟.基于GPS和自包含传感器的行人室内外无缝定位算法研究[D].合肥：中国科学技术大学，2010.

[15] Pratama A R,Wan W,Hidayat R.Smartphonebased pedestrian dead reckoning as an indoor positioning system[C]∥IEEE International Conference on System Engineering and Technology,Bandung,2012:1-6．

[16] 贾瑞才.基于四元数EKF的低成本MEMS姿态估计算法[J].传感技术学报,2014,27(1):90-95.