基于惯性导航的室内定位误差修正算法

陈国通 王小娜 张晓旭 许文倩 张璞

摘要：针对惯性导航系统（INS）在室内定位过程中，位移误差随时间不断积累而导致定位精度不高的问题，通过分析人行走的特征，以及行走过程中零速点的特性，提出了基于惯性导航的室内定位误差修正算法。使用最大似然估计法对加速度计和陀螺仪的输出参数进行判断，确定零速点，然后通过扩展卡尔曼滤波（EKF）算法，分别建立定位系统的状态方程和观测方程对误差进行修正。利用Matlab搭建仿真平台，对算法进行了仿真。仿真实验结果表明：改进后的零速检测算法，提高了零速点检测准确率，使位移误差得到了有效抑制，并将定位误差控制在了3%以内。改进算法对室内定位误差修正具有一定的实用价值。

关键词：无线通信技术；室内定位；零速检测；惯性导航；MEMS；扩展卡尔曼滤波

中图分类号：TN91972文献标志码：Adoi： 10.7535/hbgykj.2018yx03006

近年来，随着物联网、智能家居的迅猛发展，室内导航成为目前导航与定位研究的主要方向之一。基于射频识别（radio-frequency identification，RFID）[1]、超声波、ZigBee[2]、无线传感器[3]的室内定位技术的不断兴起，已经被广泛应用到室内定位领域，且具有较高的定位精度。但这些技术均需要依靠外界的设备装置来辅助定位，并通过这些硬件设备进行数据传输来实现定位，当外界环境无法满足要求时，就无法实现定位。所以，随着人们对室内复杂环境定位需求的不断提升，第3期陈国通，等：基于惯性导航的室内定位误差修正算法河北工业科技第35卷基于自身传感器进行定位的惯性导航技术得到了大力发展[4-6]。但是单独使用惯性导航解算时，位移误差会以导航时间的三次方发散，迅速超出导航范围，失去导航定位的意义[7]。要解决这一问题有两种方法：一是利用组合导航的方式进行误差修正，例如GPS与惯性导航的组合[8]，但GPS信号容易受到建筑物的影响，不适合进行室内定位；二是在导航解算之前通过误差修正算法，消除速度累积误差，达到削弱位移误差的目的。2012年，瑞典皇家工学院信号处理实验室的John Olof Nilsson提出了一个开源的、实时的基于鞋绑式零速修正辅助的惯性导航系统，并对鞋绑式惯性导航系统误差模型的影响进行了研究，提出了一个基于步进式惯性导航和步进式航位推算的局部分散式系统框架。目前一些学者根据此系统，利用加速度计的输出进行零速检测[9-12]来修正误差值。文献＼[9＼]设计了一种基于零速检测的加速度量测幅值计步算法，实现了载体坐标系和行人地理坐标系的转换，利用惯性测量元件对人员行走进行检测，实现了行人运动姿态的有效识别和步数的精确统计。但这种方法易受到行走震动的影响，容易出现零速点误判和漏判等问题。针对此问题，本文提出了一种新的零速检测算法，该算法同时利用加速度计和陀螺仪的输出进行零速点检测，然后通过扩展卡尔曼滤波算法进行误差修正。

1MEMS室内定位系统及误差模型

基于零速检测的MEMS室内定位系统主要器件为加速度计和陀螺仪。系统的主要误差包括：加速度计的零位误差和陀螺漂移。基于零速检测的MEMS室内定位系统原理框图如图1所示。定位系统的基本原理：首先由加速度计和陀螺仪进行数据采集后，输入到导航解算单元进行速度、位置的解算，最后利用误差修正算法对导航结果进行反馈修正，达到精确解算的目的。

加速度计测量模型[13]为

b=（I+δKA+δA）fb+Δb， （1）

式中：b表示载体坐标系，fb为加速度计测量比力值；δKA为加速度计刻度因数误差系数；δA为加速度计的安装误差；Δb为零偏误差。

陀螺仪测量模型为

bib=（I+δKG+δG）ωbib+εb， （2）

式中：ωbib为陀螺仪测量的载体角速度；δKG为陀螺仪刻度因数误差系数；δG为陀螺仪的安装误差；εb为陀螺仪随机漂移误差。

假设惯性导航姿态误差角φ是小角度，并且不考虑地球重力模型误差，可以得到线性近似的MEMS室内定位系统误差模型[14]。

姿态误差方程：

=-ωnin×φ+δωnin-δωnib ， （3）

式中：n表示导航坐标系；ωnin表示导航坐标系相对于惯性坐标系的转动角速度；δωnin表示ωnin的计算误差；δωnib表示导航坐标系相对于载体坐标系转动角速度的计算误差。

速度误差方程：

δn=fnsf×φ-（2ωnie+ωnen）×δvn+

vn×（2ωnie+ωnen）+δfnsf， （4）

式中：fnsf是加速度计测量的比力值；ωnie表示导航坐标系相对于地球的转动角速度；ωnen表示导航坐标系相对于地理坐标系的转动角速度；vn表示惯性导航计算的速度；δvn表示速度误差；δfnsf表示比力测量误差。

位移误差方程：

δ=δVn 。 （5）

2定位数据预处理

在加速度计采集数据前，先让加速度计预先工作一段时间，保证加速度计稳定后，以100 Hz的采样频率进行采样，采样时间为15 min。得到数据后，以Matlab为数据处理平台进行数据处理，见图2。

理论上x轴，y轴上的测量值为0，而z轴的测量值应与当地的重力加速度相等，然而从图2可以看出，在静止时刻，x轴，y轴测量值不等于零，而z轴的值也不等于g。因此，通过测量得出偏移量，在实际导航过程中需要对零值偏移进行补偿。修正后的测量值如图3所示，可以看出零位误差控制在10-2范围内，相比之前有了较大的改善，为后续导航解算的准确性提供了保障。

3零速檢测误差修正算法

人在行走时，脚部运动可以分为两种运动状态：迈步时为运动状态（C0），脚落地时为静止状态（C1），如图4所示。

理论上当人处于C1时刻时，此时脚部着地，加速度计水平方向输出值应为零，加速度计竖直方向应等于重力常量。根据此特征，可以利用加速度计输出参量的模值、方差以及幅值和已设定的阈值进行比较，提取人行走时的零速点，利用零速点来消除速度误差。但这种算法容易受到脚着地时震动的干扰，导致零点检测误差大，使行人的速度误差和位移误差增加。因此，为了减小震动的干扰，提出了新的零速检测算法，将加速度计与陀螺仪的零速判定进行比较运算，提高检测的准确率。

假设人行走时，两种运动状态的输出为

C0：θv≡{Sak}n+N-1k=n，C1：θv≡Sa， （6）

式中：θ表示加速度计输出信号的未知参数；N表示滑动窗口的取点个数；Sak表示k时刻加速度计输出的未知量；Sa表示零点时刻加速度计输出的未知量。

加速度计输出信号的概率密度函数为

p（zn；θ，Ci）=∏kp（yak；θ，Ci）=

1（2πσ2α）3/2·exp-12σ2a‖yak-Sak（θ）‖2，（7）

式中：zn{yk}n+N-1k=n；yak表示k时刻加速度计的输出量。

根据Neyman-Pearson定理，如果

L（zn）=p（zn；C1）p（zn；C0）>γ ， （8）

则判定为C1状态，其中γ为判定阈值。

阈值可以由式（9）求出：

PF=∫{zn；L（zn）>γ}p（zn；C0）dzn=α，（9）

式中：PF表示虚警概率p（C1/C0）；α表示显著水平，根据实际情况给定[15]。

根据最大似然估计法，当人处于C0状态时，θ0v={yak}n+N-1n。因此，

p（zan；0v，C0）=1（2πσ2α）3N/2。（10）

而當人处于C1状态时：

a=arg maxs（p（zan；s，C1））=

arg mins∑k∈Ωn‖yak-s‖2=an。 （11）

将式（11）代入式（7）可得：

p（zan；1v，C1）=p（zan；a，C1）=

1（2πσ2a）3N/2·exp-12σ2a∑k∈Ωn‖yak-an‖2。（12）

将式（10）与式（12）代入式（8）可得：

T′v（zan）=-2Nlnp（zan；1v，C1）p（zan；0v，C0）=

1σ2aN∑k∈Ωn‖yak-an‖2<γ′v。 （13）

另外，可以根据陀螺仪输出的幅值来判定C1状态，判定式如下：

Tω（zωn）=1N∑k∈Ωn‖yωk‖2<γω。 （14）

对T′v（zan）和Tω（zωn）进行与运算，如果结果为1则判定该时刻为零速时刻，如果结果为0则说明该时刻为运动状态，具体的算法流程图如图5所示。

4扩展卡尔曼滤波算法

传统的卡尔曼滤波算法无法应用在环境复杂的室内，而且随着滤波迭代次数的增加，舍入误差会逐渐积累，一旦卡尔曼滤波发散，就会失去其最优估计的作用。扩展卡尔曼滤波（extended Kalman filter，EKF）可以运用在非线性的系统，因此在惯性导航系统中有着广泛的应用。

EKF的状态估计值为

k，k=[n，kωb，kn，kn，kab，k]， （15）

式中：n，k表示横滚角、俯仰角以及航向角的误差值；ωb，k表示角速率的误差值；ab，k表示加速度的误差值。

状态一步预测方程：

k|k-1=Φkk-1|k-1+ωk-1 ， （16）

式中：k|k-1表示在k时刻预测的状态值；k-1|k-1表示在（k-1）时刻估计的状态值；ωk-1表示在（k-1）时刻系统噪声矩阵。

状态矩阵：

Φk=

E3Δt×Cbn，k|k-10000E300000E3Δt×E30-Δt×S（y′an，k）00E3Δt×Cbn，k|k-10000E3， （17）

式中：S（y′an，k）表示的是导航坐标系中y′an，k的反对称矩阵。

其中：

S（y′an，k）=

0-y′an，k（3）y′an，k（2）y′an，k（3）0-y′an，k（1）-y′an，k（2）y′an，k（1）0。（18）

量测方程：

Zk=Hk|k+nk， （19）

式中：Zk表示在k时刻的量测值；H表示量测矩阵；nk表示量测噪声矩阵。

5仿真结果及分析

综上所述，基于惯性导航的室内定位误差修正算法主要分为3步：第1步将加速度计和陀螺仪传感器采集的数据进行预处理，通过测量得出偏移量，在实际导航过程中对零值偏移进行补偿；第2步为零速点检测，利用加速度计输出参量的方差以及陀螺仪输出参量的幅值和已设定的阈值进行比较，确定人行走时的零速时刻；第3步利用EKF算法，建立状态方程和量测方程进行误差修正。

为了验证室内导航误差修正算法的有效性和定位准确性，进行了矩形路线行走实验，使用型号为JY901的九轴姿态模块进行数据采集，将采集的数据利用Matlab仿真平台进行数据分析和处理。行走总距离为45 m，采样频率为100 Hz。实验采用平均定位误差（mean position error， MPE）来度量定位准确性，如式（20）所示[16]：

MPE=1M∑Mi=12（xi-i）2+（yi-i）2， （20）

式中：M是总的采样点数：（i，i）和（xi，yi）分别是第i次定位估计位置坐标和真实位置坐标。

图6为人行走的速度和加速度方差零速点检测结果，图7为人行走的速度和改进后的零速检测结果，改进后的算法结合了加速度计方差和陀螺仪幅值。图6和图7中零速检测值为“1”时表示此刻为零速时刻，值为“0”时表示此刻为运动状态。对比后发现图7中的零速检测更加精准，并且可以看出运动状态分布更加均匀，与实际行走过程相吻合，算法性能优于图6单独使用加速度计方差的检测方法，零速点检测准确率提高了10%左右。

图8为速度误差曲线，从图中可以看出，在行走过程中速度误差得到了有效抑制，这是因为利用行走过程中“零速点”的特性，修正了速度误差。从图9位移误差曲线中可以看出，位移误差也得到了修正，最终定位误差约为118 m，定位误差控制在3%以内，减小了随时间发散的速度，提高了定位精度。

从图10行走轨迹中可以看出，算法改进前的轨迹随着行走时间的增加，偏离真实路径的误差不断增大。而改进后的行走轨迹与改进前的轨迹相比更加平滑，

这是因为消除了行走过程中震动的影响。改进后的轨迹与改进前相比，虽然误差得到了抑制，但与真实路径对比发现，行走的轨迹与真实路径还有一定差距，这是由于陀螺仪漂移误差导致航向计算有一定偏差。

6结论

本文提出的误差修正算法与单一使用加速度计的零速检测算法相比，提高了零速点的检测准确率，有效修正了速度误差，抑制了位移误差的发散速度，通过Matlab仿真平台验证了算法的有效性和性能，修正后的定位误差在3%左右。从轨迹图可以看出，虽然位移误差得到了修正，但是受到陀螺仪漂移的影响，导致方位上存在一定的偏差。如何有效地抑制方位的偏差，是接下来研究的重点，可以考虑使用磁力计、地图匹配等方法进行航向修正。

参考文献/References：

[1]纪敏. WiFi/RFID室内融合定位方法的研究[D].南京：南京邮电大学，2016.

JI Min.Study of WiFi/RFID Indoor Fusion Positioning Method[D].Nanjing：Nanjing University of Posts and Telecommunications，2016.

[2]林瀅.基于ZigBee技术的融合性室内定位系统研究与设计[D].南京：南京邮电大学，2016.

LIN Ying.The Research and Implementation of the Technology of Indoor Positioning Based on ZigBee[D].Nanjing：Nanjing University of Posts and Telecommunications，2016.

[3]沙朝恒. 基于矩阵补全的无线传感器网络节点定位算法研究[D].南京：南京邮电大学，2016.

SHA Zhaoheng.Localization for Wireless Sensor Networks via Matrix Completion[D].Nanjing：Nanjing University of Posts and Telecommunications，2016.

[4]GROVES P D. Navigation using inertial sensors[J]. IEEE Aerospace and Electronic Systems Magazine， 2015， 30（2）：42-69.

[5]HARLE R. A survey of indoor inertial positioning systems for pedestrians[J]. IEEE Communications Surveys & Tutorials， 2013， 15（3）：1281-1293.

[6]LARGE D R， BURNETT G， BENFORD S， et al. Crowdsourcing good landmarks for in-vehicle navigation systems[J]. Behaviour & Information Technology， 2016， 35（10）：807-816.

[7]崔潇， 秦永元， 周琪，等. 鞋式个人导航系统算法和试验研究[J]. 测控技术， 2013， 32（3）：138-142.

CUI Xiao， QIN Yongyuan， ZHOU Qi， et al.Algorithm and test research on shoe-mounted personal navigation system[J]. Measurement and Control Technology， 2013， 32（3）：138-142.

[8]张恒浩， 刘藻珍， 孟秀云，等.D-S证据理论在组合导航中的决策判断[J].河北科技大学学报，2011，32（6）：571-574.

ZHANG Henghao，LIU Zaozhen，MENG Xiuyun，et al.Application of D-S evidence theory in integrated navigation[J].Journal of Hebei University of Science and Technology，2011，32（6）：571-574.

[9]陈国良， 杨洲. 基于加速度量测幅值零速检测的计步算法研究[J]. 武汉大学学报（信息科学版）， 2017， 42（6）：726-730.

CHEN Guoliang， YANG Zhou. Step counting algorithm based on zero velocity update[J]. Geomatics and Information Science of Wuhan University， 2017， 42 （6）： 726-730.

[10] 苑宝贞， 苏中， 李擎，等. 基于贝叶斯网络的强鲁棒性零速检测方法[J]. 计算机测量与控制， 2016， 24（3）：200-203.

YUAN Baozhen， SU Zhong， LI Qing， et al. A robust zero velocity detection method based on bayesian networks[J]. Computer Measurement and Control， 2016， 24 （3）： 200-203.

[11]朱彩杰， 趙冬青， 杨洲. 基于 MEMS 的室内定位误差修正方法研究[J]. 测绘工程， 2017， 26（5）：57-61.

ZHU Caijie， ZHAO Dongqing，YANG Zhou.A study of indoor positioning error correction method based on MEMS[J].Engineering of Surveying and Mapping， 2017， 26（5）：57-61.

[12] 王普， 潘凯， 任明荣，等. 基于伪自适应阈值零速检测法的室内个人导航系统[J]. 北京工业大学学报， 2015，41（9）：1308-1313.

WANG Pu， PAN Kai， REN Mingrong， et al. Indoor personal navigation system based on zero velocity detection of pseudo adaptive threshold [J].Journal of Beijing University of Technology， 2015，41 （9）： 1308-1313.

[13]秦永元. 惯性导航[M]. 北京：科学出版社， 2008：355-361.

[14]严恭敏. 车载自主定位定向系统研究[D]. 西安：西北工业大学， 2006.

YAN Gongmin.Research on Vehicle Autonomous Positioning and Orientation System[D].Xian：Northwestern Polytechnical University，2006.

[15]KAY S M. Fundamentals of Statistical Signal Processing： Estimation Theory [M]. NJ： Prentice-Hall Inc Upper Saddle River， 1993.

[16]刘玉杰. 基于扩展卡尔曼滤波算法的融合室内定位系统研究与实现[D]. 南京：南京邮电大学， 2016.

LIU Yujie.Research and Implementation of Indoor Positioning System Based on Extended Kalman Filter[D].Nanjing：Nanjing University of Posts and Telecommunications，2016.第35卷第3期河北工业科技Vol.35，No.3