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

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

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

中圖分類號：TN91972文獻(xiàn)標(biāo)志碼：Adoi： 10.7535/hbgykj.2018yx03006

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

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

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

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

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

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

陀螺儀測量模型為

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

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

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

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

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

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

速度誤差方程：

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

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

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

位移誤差方程：

δ=δVn 。 （5）

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

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

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

3零速檢測誤差修正算法

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

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

假設(shè)人行走時，兩種運動狀態(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ù)最大似然估計法，當(dāng)人處于C0狀態(tài)時，θ0v={yak}n+N-1n。因此，

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

而當(dāng)人處于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）進(jìn)行與運算，如果結(jié)果為1則判定該時刻為零速時刻，如果結(jié)果為0則說明該時刻為運動狀態(tài)，具體的算法流程圖如圖5所示。

4擴展卡爾曼濾波算法

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

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

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

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

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

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

式中：k|k-1表示在k時刻預(yù)測的狀態(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）表示的是導(dǎo)航坐標(biāo)系中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é)果及分析

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

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

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

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

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

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

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

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

6結(jié)論

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

參考文獻(xiàn)/References：

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

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

[2]林瀅.基于ZigBee技術(shù)的融合性室內(nèi)定位系統(tǒng)研究與設(shè)計[D].南京：南京郵電大學(xué)，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)絡(luò)節(jié)點定位算法研究[D].南京：南京郵電大學(xué)，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]崔瀟， 秦永元， 周琪，等. 鞋式個人導(dǎo)航系統(tǒng)算法和試驗研究[J]. 測控技術(shù)， 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ù)理論在組合導(dǎo)航中的決策判斷[J].河北科技大學(xué)學(xué)報，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]. 武漢大學(xué)學(xué)報（信息科學(xué)版）， 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)絡(luò)的強魯棒性零速檢測方法[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] 王普， 潘凱， 任明榮，等. 基于偽自適應(yīng)閾值零速檢測法的室內(nèi)個人導(dǎo)航系統(tǒng)[J]. 北京工業(yè)大學(xué)學(xué)報， 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]秦永元. 慣性導(dǎo)航[M]. 北京：科學(xué)出版社， 2008：355-361.

[14]嚴(yán)恭敏. 車載自主定位定向系統(tǒng)研究[D]. 西安：西北工業(yè)大學(xué)， 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]. 南京：南京郵電大學(xué)， 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