陳國通 王小娜 張曉旭 許文倩 張璞

摘要：針對慣性導航系統(tǒng)（INS）在室內(nèi)定位過程中，位移誤差隨時間不斷積累而導致定位精度不高的問題，通過分析人行走的特征，以及行走過程中零速點的特性，提出了基于慣性導航的室內(nèi)定位誤差修正算法。使用最大似然估計法對加速度計和陀螺儀的輸出參數(shù)進行判斷，確定零速點，然后通過擴展卡爾曼濾波（EKF）算法，分別建立定位系統(tǒng)的狀態(tài)方程和觀測方程對誤差進行修正。利用Matlab搭建仿真平臺，對算法進行了仿真。仿真實驗結(jié)果表明：改進后的零速檢測算法，提高了零速點檢測準確率，使位移誤差得到了有效抑制，并將定位誤差控制在了3%以內(nèi)。改進算法對室內(nèi)定位誤差修正具有一定的實用價值。

關鍵詞：無線通信技術；室內(nèi)定位；零速檢測；慣性導航；MEMS；擴展卡爾曼濾波

中圖分類號：TN91972文獻標志碼：Adoi： 10.7535/hbgykj.2018yx03006

近年來，隨著物聯(lián)網(wǎng)、智能家居的迅猛發(fā)展，室內(nèi)導航成為目前導航與定位研究的主要方向之一?；谏漕l識別（radio-frequency identification，RFID）[1]、超聲波、ZigBee[2]、無線傳感器[3]的室內(nèi)定位技術的不斷興起，已經(jīng)被廣泛應用到室內(nèi)定位領域，且具有較高的定位精度。但這些技術均需要依靠外界的設備裝置來輔助定位，并通過這些硬件設備進行數(shù)據(jù)傳輸來實現(xiàn)定位，當外界環(huán)境無法滿足要求時，就無法實現(xiàn)定位。所以，隨著人們對室內(nèi)復雜環(huán)境定位需求的不斷提升，第3期陳國通，等：基于慣性導航的室內(nèi)定位誤差修正算法河北工業(yè)科技第35卷基于自身傳感器進行定位的慣性導航技術得到了大力發(fā)展[4-6]。但是單獨使用慣性導航解算時，位移誤差會以導航時間的三次方發(fā)散，迅速超出導航范圍，失去導航定位的意義[7]。要解決這一問題有兩種方法：一是利用組合導航的方式進行誤差修正，例如GPS與慣性導航的組合[8]，但GPS信號容易受到建筑物的影響，不適合進行室內(nèi)定位；二是在導航解算之前通過誤差修正算法，消除速度累積誤差，達到削弱位移誤差的目的。2012年，瑞典皇家工學院信號處理實驗室的John Olof Nilsson提出了一個開源的、實時的基于鞋綁式零速修正輔助的慣性導航系統(tǒng)，并對鞋綁式慣性導航系統(tǒng)誤差模型的影響進行了研究，提出了一個基于步進式慣性導航和步進式航位推算的局部分散式系統(tǒng)框架。目前一些學者根據(jù)此系統(tǒng)，利用加速度計的輸出進行零速檢測[9-12]來修正誤差值。文獻＼[9＼]設計了一種基于零速檢測的加速度量測幅值計步算法，實現(xiàn)了載體坐標系和行人地理坐標系的轉(zhuǎn)換，利用慣性測量元件對人員行走進行檢測，實現(xiàn)了行人運動姿態(tài)的有效識別和步數(shù)的精確統(tǒng)計。但這種方法易受到行走震動的影響，容易出現(xiàn)零速點誤判和漏判等問題。針對此問題，本文提出了一種新的零速檢測算法，該算法同時利用加速度計和陀螺儀的輸出進行零速點檢測，然后通過擴展卡爾曼濾波算法進行誤差修正。

1MEMS室內(nèi)定位系統(tǒng)及誤差模型

基于零速檢測的MEMS室內(nèi)定位系統(tǒng)主要器件為加速度計和陀螺儀。系統(tǒng)的主要誤差包括：加速度計的零位誤差和陀螺漂移。基于零速檢測的MEMS室內(nèi)定位系統(tǒng)原理框圖如圖1所示。定位系統(tǒng)的基本原理：首先由加速度計和陀螺儀進行數(shù)據(jù)采集后，輸入到導航解算單元進行速度、位置的解算，最后利用誤差修正算法對導航結(jié)果進行反饋修正，達到精確解算的目的。

加速度計測量模型[13]為

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

式中：b表示載體坐標系，fb為加速度計測量比力值；δKA為加速度計刻度因數(shù)誤差系數(shù)；δA為加速度計的安裝誤差；Δb為零偏誤差。

陀螺儀測量模型為

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

式中：ωbib為陀螺儀測量的載體角速度；δKG為陀螺儀刻度因數(shù)誤差系數(shù)；δG為陀螺儀的安裝誤差；εb為陀螺儀隨機漂移誤差。

假設慣性導航姿態(tài)誤差角φ是小角度，并且不考慮地球重力模型誤差，可以得到線性近似的MEMS室內(nèi)定位系統(tǒng)誤差模型[14]。

姿態(tài)誤差方程：

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

式中：n表示導航坐標系；ωnin表示導航坐標系相對于慣性坐標系的轉(zhuǎn)動角速度；δωnin表示ωnin的計算誤差；δωnib表示導航坐標系相對于載體坐標系轉(zhuǎn)動角速度的計算誤差。

速度誤差方程：

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

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

式中：fnsf是加速度計測量的比力值；ωnie表示導航坐標系相對于地球的轉(zhuǎn)動角速度；ωnen表示導航坐標系相對于地理坐標系的轉(zhuǎn)動角速度；vn表示慣性導航計算的速度；δvn表示速度誤差；δfnsf表示比力測量誤差。

位移誤差方程：

δ=δVn 。 （5）

2定位數(shù)據(jù)預處理

在加速度計采集數(shù)據(jù)前，先讓加速度計預先工作一段時間，保證加速度計穩(wěn)定后，以100 Hz的采樣頻率進行采樣，采樣時間為15 min。得到數(shù)據(jù)后，以Matlab為數(shù)據(jù)處理平臺進行數(shù)據(jù)處理，見圖2。

理論上x軸，y軸上的測量值為0，而z軸的測量值應與當?shù)氐闹亓铀俣认嗟龋欢鴱膱D2可以看出，在靜止時刻，x軸，y軸測量值不等于零，而z軸的值也不等于g。因此，通過測量得出偏移量，在實際導航過程中需要對零值偏移進行補償。修正后的測量值如圖3所示，可以看出零位誤差控制在10-2范圍內(nèi)，相比之前有了較大的改善，為后續(xù)導航解算的準確性提供了保障。

3零速檢測誤差修正算法

人在行走時，腳部運動可以分為兩種運動狀態(tài)：邁步時為運動狀態(tài)（C0），腳落地時為靜止狀態(tài)（C1），如圖4所示。

理論上當人處于C1時刻時，此時腳部著地，加速度計水平方向輸出值應為零，加速度計豎直方向應等于重力常量。根據(jù)此特征，可以利用加速度計輸出參量的模值、方差以及幅值和已設定的閾值進行比較，提取人行走時的零速點，利用零速點來消除速度誤差。但這種算法容易受到腳著地時震動的干擾，導致零點檢測誤差大，使行人的速度誤差和位移誤差增加。因此，為了減小震動的干擾，提出了新的零速檢測算法，將加速度計與陀螺儀的零速判定進行比較運算，提高檢測的準確率。

假設人行走時，兩種運動狀態(tài)的輸出為

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

式中：θ表示加速度計輸出信號的未知參數(shù)；N表示滑動窗口的取點個數(shù)；Sak表示k時刻加速度計輸出的未知量；Sa表示零點時刻加速度計輸出的未知量。

加速度計輸出信號的概率密度函數(shù)為

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時刻加速度計的輸出量。

根據(jù)Neyman-Pearson定理，如果

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

則判定為C1狀態(tài)，其中γ為判定閾值。

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

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

式中：PF表示虛警概率p（C1/C0）；α表示顯著水平，根據(jù)實際情況給定[15]。

根據(jù)最大似然估計法，當人處于C0狀態(tài)時，θ0v={yak}n+N-1n。因此，

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

而當人處于C1狀態(tài)時：

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）

另外，可以根據(jù)陀螺儀輸出的幅值來判定C1狀態(tài)，判定式如下：

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

對T′v（zan）和Tω（zωn）進行與運算，如果結(jié)果為1則判定該時刻為零速時刻，如果結(jié)果為0則說明該時刻為運動狀態(tài)，具體的算法流程圖如圖5所示。

4擴展卡爾曼濾波算法

傳統(tǒng)的卡爾曼濾波算法無法應用在環(huán)境復雜的室內(nèi)，而且隨著濾波迭代次數(shù)的增加，舍入誤差會逐漸積累，一旦卡爾曼濾波發(fā)散，就會失去其最優(yōu)估計的作用。擴展卡爾曼濾波（extended Kalman filter，EKF）可以運用在非線性的系統(tǒng)，因此在慣性導航系統(tǒng)中有著廣泛的應用。

EKF的狀態(tài)估計值為

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

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

狀態(tài)一步預測方程：

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

式中：k|k-1表示在k時刻預測的狀態(tài)值；k-1|k-1表示在（k-1）時刻估計的狀態(tài)值；ωk-1表示在（k-1）時刻系統(tǒng)噪聲矩陣。

狀態(tài)矩陣：

Φ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仿真結(jié)果及分析

綜上所述，基于慣性導航的室內(nèi)定位誤差修正算法主要分為3步：第1步將加速度計和陀螺儀傳感器采集的數(shù)據(jù)進行預處理，通過測量得出偏移量，在實際導航過程中對零值偏移進行補償；第2步為零速點檢測，利用加速度計輸出參量的方差以及陀螺儀輸出參量的幅值和已設定的閾值進行比較，確定人行走時的零速時刻；第3步利用EKF算法，建立狀態(tài)方程和量測方程進行誤差修正。

為了驗證室內(nèi)導航誤差修正算法的有效性和定位準確性，進行了矩形路線行走實驗，使用型號為JY901的九軸姿態(tài)模塊進行數(shù)據(jù)采集，將采集的數(shù)據(jù)利用Matlab仿真平臺進行數(shù)據(jù)分析和處理。行走總距離為45 m，采樣頻率為100 Hz。實驗采用平均定位誤差（mean position error， MPE）來度量定位準確性，如式（20）所示[16]：

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

式中：M是總的采樣點數(shù)：（i，i）和（xi，yi）分別是第i次定位估計位置坐標和真實位置坐標。

圖6為人行走的速度和加速度方差零速點檢測結(jié)果，圖7為人行走的速度和改進后的零速檢測結(jié)果，改進后的算法結(jié)合了加速度計方差和陀螺儀幅值。圖6和圖7中零速檢測值為“1”時表示此刻為零速時刻，值為“0”時表示此刻為運動狀態(tài)。對比后發(fā)現(xiàn)圖7中的零速檢測更加精準，并且可以看出運動狀態(tài)分布更加均勻，與實際行走過程相吻合，算法性能優(yōu)于圖6單獨使用加速度計方差的檢測方法，零速點檢測準確率提高了10%左右。

圖8為速度誤差曲線，從圖中可以看出，在行走過程中速度誤差得到了有效抑制，這是因為利用行走過程中“零速點”的特性，修正了速度誤差。從圖9位移誤差曲線中可以看出，位移誤差也得到了修正，最終定位誤差約為118 m，定位誤差控制在3%以內(nèi)，減小了隨時間發(fā)散的速度，提高了定位精度。

從圖10行走軌跡中可以看出，算法改進前的軌跡隨著行走時間的增加，偏離真實路徑的誤差不斷增大。而改進后的行走軌跡與改進前的軌跡相比更加平滑，

這是因為消除了行走過程中震動的影響。改進后的軌跡與改進前相比，雖然誤差得到了抑制，但與真實路徑對比發(fā)現(xiàn)，行走的軌跡與真實路徑還有一定差距，這是由于陀螺儀漂移誤差導致航向計算有一定偏差。

6結(jié)論

本文提出的誤差修正算法與單一使用加速度計的零速檢測算法相比，提高了零速點的檢測準確率，有效修正了速度誤差，抑制了位移誤差的發(fā)散速度，通過Matlab仿真平臺驗證了算法的有效性和性能，修正后的定位誤差在3%左右。從軌跡圖可以看出，雖然位移誤差得到了修正，但是受到陀螺儀漂移的影響，導致方位上存在一定的偏差。如何有效地抑制方位的偏差，是接下來研究的重點，可以考慮使用磁力計、地圖匹配等方法進行航向修正。

參考文獻/References：

[1]紀敏. WiFi/RFID室內(nèi)融合定位方法的研究[D].南京：南京郵電大學，2016.

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

[2]林瀅.基于ZigBee技術的融合性室內(nèi)定位系統(tǒng)研究與設計[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]沙朝恒. 基于矩陣補全的無線傳感器網(wǎng)絡節(jié)點定位算法研究[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]崔瀟， 秦永元， 周琪，等. 鞋式個人導航系統(tǒng)算法和試驗研究[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ù)理論在組合導航中的決策判斷[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] 苑寶貞， 蘇中， 李擎，等. 基于貝葉斯網(wǎng)絡的強魯棒性零速檢測方法[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 的室內(nèi)定位誤差修正方法研究[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] 王普， 潘凱， 任明榮，等. 基于偽自適應閾值零速檢測法的室內(nèi)個人導航系統(tǒng)[J]. 北京工業(yè)大學學報， 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]嚴恭敏. 車載自主定位定向系統(tǒng)研究[D]. 西安：西北工業(yè)大學， 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]劉玉杰. 基于擴展卡爾曼濾波算法的融合室內(nèi)定位系統(tǒng)研究與實現(xiàn)[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期河北工業(yè)科技Vol.35，No.3