陳克川，李保國(guó)，劉思慶，李淑影

(北京航空航天大學(xué)，北京 100191)

0　引言

現(xiàn)如今，為個(gè)人提供定位服務(wù)的導(dǎo)航設(shè)備在人們的生活中得到了廣泛的應(yīng)用，已成為人們?nèi)粘Ｉ钪邢喈?dāng)重要的一部分。該功能可以有效地指導(dǎo)人們?cè)谀吧h(huán)境下的活動(dòng)，有的個(gè)人導(dǎo)航系統(tǒng)設(shè)備可以通過(guò)無(wú)線通訊傳輸給監(jiān)控單元，實(shí)時(shí)監(jiān)測(cè)徒步人員的位置變化[1]。目前，絕大多數(shù)個(gè)人導(dǎo)航產(chǎn)品都是基于衛(wèi)星導(dǎo)航定位，這種導(dǎo)航方式對(duì)室內(nèi)應(yīng)用無(wú)能為力。

針對(duì)室內(nèi)定位不同的應(yīng)用場(chǎng)合和功能需求，人們提出了使用多種不同的定位模型和定位方法推算室內(nèi)導(dǎo)航位置。個(gè)人導(dǎo)航系統(tǒng)(Personal Navigation System, PNS)，主要用于跟蹤定位徒步行走人員的實(shí)時(shí)位置[2]。行人徒步自主導(dǎo)航，通過(guò)在足部[3]綁定一個(gè)慣性測(cè)量單元(Inertial Measurement Unit, IMU)和磁強(qiáng)計(jì)，構(gòu)成一個(gè)足綁式個(gè)人導(dǎo)航系統(tǒng)。IMU和磁強(qiáng)計(jì)常采用低成本的微電子機(jī)械系統(tǒng)(Micro Electro Mechanical System, MEMS)傳感器，其成本低、體積小、功耗低、穿戴方便，受到人們的歡迎。

但是市場(chǎng)上MEMS運(yùn)動(dòng)傳感器精度低、噪聲大，用于個(gè)人導(dǎo)航時(shí)誤差隨時(shí)間積累，發(fā)散十分快。因此，必須利用行人行走時(shí)落地零速的特點(diǎn)，對(duì)速度誤差、位置誤差和水平姿態(tài)角誤差進(jìn)行零速修正(Zero Velocity Update, ZUPT)，以提高定位導(dǎo)航的精度。

根據(jù)行人運(yùn)動(dòng)特點(diǎn)，當(dāng)腳著地時(shí)，捷聯(lián)在腳上的IMU速度為零。據(jù)此對(duì)姿態(tài)、速度和位置信息進(jìn)行零速修正，可以有效抑制導(dǎo)航系統(tǒng)參數(shù)發(fā)散。零速修正的關(guān)鍵在于零速區(qū)間精確的檢測(cè)，本文在借鑒前人研究工作的基礎(chǔ)上，設(shè)計(jì)了基于自適應(yīng)滑動(dòng)平均算法的零速區(qū)間檢測(cè)方法。經(jīng)實(shí)驗(yàn)驗(yàn)證，該方法能準(zhǔn)確、有效地檢測(cè)出零速區(qū)間的范圍，證明了該算法的有效性。

1　自適應(yīng)滑動(dòng)平均零速區(qū)間檢測(cè)方法

行人在運(yùn)動(dòng)過(guò)程中，安裝于腳部的MEMS慣性測(cè)量單元能夠敏感腳部的比力及角速度變化，可將其用于行人運(yùn)動(dòng)或靜止的分析與識(shí)別。在行走過(guò)程中，足部與地面接觸的時(shí)間較短，一般情況下為0.2s～0.5s[4]，在跑步時(shí)觸地時(shí)間更短。因此，基于行人導(dǎo)航系統(tǒng)精度的要求，需要非常準(zhǔn)確地判斷零速區(qū)間。

在足綁式PNS中，靜止?fàn)顟B(tài)即零速狀態(tài)的判定[5]，是零速修正的基礎(chǔ)。大多數(shù)文獻(xiàn)是基于陀螺或者加速度計(jì)或者二者兼有之的輸出進(jìn)行零速判定，部分文獻(xiàn)介紹了利用壓電、超聲波[6]等輔助傳感器進(jìn)行零速判定。

為不增加額外傳感器，同時(shí)更準(zhǔn)確地進(jìn)行零速區(qū)間判定，本文利用自適應(yīng)滑動(dòng)平均算法對(duì)三軸陀螺輸出和三軸加速度計(jì)輸出進(jìn)行聯(lián)合判定。

1.1　自適應(yīng)滑動(dòng)平均算法

自適應(yīng)滑動(dòng)平均算法廣泛用于金融計(jì)算、雷達(dá)系統(tǒng)等，常用來(lái)解決尋找包含高噪聲動(dòng)態(tài)信號(hào)的鋒利間斷點(diǎn)。算法首先鑒別出間斷點(diǎn)發(fā)生的時(shí)間段，然后在時(shí)間區(qū)間內(nèi)通過(guò)自適應(yīng)的調(diào)整滑動(dòng)窗口大小，避免滑動(dòng)平均算法造成的時(shí)間延遲。該算法是一種基于迭代滑動(dòng)平均的自適應(yīng)算法，算法根據(jù)滑動(dòng)平均后數(shù)據(jù)的變化趨勢(shì)，通過(guò)比較樣本方差，改變滑動(dòng)窗口的大小。因此，在數(shù)據(jù)變化的區(qū)間，窗口會(huì)自適應(yīng)縮放，數(shù)據(jù)變化率越大，窗口越緊湊。

算法步驟：

1)取長(zhǎng)度為N的采樣數(shù)據(jù)作為一幀，并在區(qū)間[2q+1,N-2q-1]內(nèi)應(yīng)用滑動(dòng)平均算法KZq,k[X(t)]處理。其中，k為迭代次數(shù)，q為濾波長(zhǎng)度。

應(yīng)用q點(diǎn)滑動(dòng)平均算法做數(shù)據(jù)預(yù)處理，KZq,k是一個(gè)迭代的滑動(dòng)平均算法：

(1)

其中，Xi為原始數(shù)據(jù)，yt為濾波后的數(shù)據(jù)。

2)預(yù)處理后的數(shù)據(jù)yt將用于滑動(dòng)平均的自適應(yīng)濾波算法。在自適應(yīng)濾波器中，窗口大小將自適應(yīng)調(diào)整。濾波器分別由幀頭(qh)和幀尾(qt)構(gòu)成，在數(shù)據(jù)變化率較大的區(qū)間，有效地調(diào)整其大小，而在間斷點(diǎn)處幀頭qh將減小。

3)根據(jù)滑動(dòng)平均算法構(gòu)造差分向量：

D(t)=|y(t+q)-y(t-q)|

(2)

4)計(jì)算導(dǎo)數(shù)：D′(t)=D(t+1)-D(t)。如果斜率為正，幀頭將按窗口全尺度以比例f[D(t)]縮小，幀尾將擴(kuò)大到全窗口長(zhǎng)度；如果斜率為負(fù)，幀頭將擴(kuò)大到全窗口長(zhǎng)度，幀尾將按窗口全尺度以比例f[D(t)]縮小。

(3)

即：

(4)

(5)

5)由此得到濾波器的輸出Yt為：

(6)

6)將輸出的qh和qt作為輸入，進(jìn)行上一步算法的迭代運(yùn)算。

應(yīng)用自適應(yīng)滑動(dòng)平均算法處理含有間斷點(diǎn)數(shù)據(jù)，如圖1所示。

行人走動(dòng)時(shí)，腳部的運(yùn)動(dòng)存在明顯的運(yùn)動(dòng)、停止、運(yùn)動(dòng)、停止……交替前進(jìn)的特殊運(yùn)動(dòng)規(guī)律[7]。體現(xiàn)在慣性傳感器輸出上，則有明顯的間斷點(diǎn)。因此，用該算法可以有效地找到由動(dòng)到靜或者由靜到動(dòng)的間斷點(diǎn)，即零速區(qū)間的起始點(diǎn)。

1.2　零速區(qū)間檢測(cè)方法

零速區(qū)間檢測(cè)過(guò)程如下：

(1)加速度零速檢測(cè)方法

采用加速度的模進(jìn)行判斷是否為零速，K時(shí)刻加速度的模為：

(7)

如圖2所示，在零速狀態(tài)下，即在沒(méi)有線性加速度干擾階段，理論上幾乎只有當(dāng)?shù)刂亓铀俣萭。因此，該階段加速度計(jì)輸出的模應(yīng)在某一范圍內(nèi)，將加速度計(jì)模值減g后取絕對(duì)值，設(shè)定閾值后即可進(jìn)行零速點(diǎn)的檢測(cè)，本文設(shè)置閾值為thamin=-0.1g，thamax=0.1g。

加速度數(shù)據(jù)判定零速有效標(biāo)志：

(2)角速度零速檢測(cè)方法

同樣采用角速度的模進(jìn)行判斷是否零速，K時(shí)刻角速度的模為：

(8)

如圖3所示，在零速狀態(tài)下，理論上角速度幾乎為0，因此該階段陀螺儀輸出的模應(yīng)在某一范圍內(nèi)?？紤]到MEMS陀螺儀噪聲大，且固定在腳上也無(wú)法真正做到靜止，故本文設(shè)置閾值較大，為thg=20(°)/s。

角速度數(shù)據(jù)判定零速有效標(biāo)志：

最終，將基于角速度模值檢測(cè)的結(jié)果與基于加速度計(jì)檢測(cè)的結(jié)果相與作為檢測(cè)的結(jié)果，以便提高零速區(qū)間檢測(cè)的準(zhǔn)確性。

flag=flag_a&flag_g

(9)

2　PNS算法設(shè)計(jì)

PNS采用成熟的SINS算法。由于個(gè)人導(dǎo)航系統(tǒng)的導(dǎo)航范圍有限，位置宜采用東-北-天導(dǎo)航坐標(biāo)系，即地理坐標(biāo)系。另外，行人行走速度較慢，計(jì)算過(guò)程中的小量可以忽略，而且MEMS陀螺儀精度比較差，地球自轉(zhuǎn)角速度完全淹沒(méi)在噪聲之中，因此導(dǎo)航方程可以做相應(yīng)的簡(jiǎn)化[8]。

2.1　PNS誤差方程

根據(jù)行人運(yùn)動(dòng)特點(diǎn)，推導(dǎo)了適用于行人導(dǎo)航的SINS的姿態(tài)誤差方程、速度誤差方程以及位置誤差方程。

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

忽略地球轉(zhuǎn)速以及行人相對(duì)地球角速度小量，SINS的姿態(tài)誤差方程的矢量形式為：

(10)

(2)速度誤差方程

忽略哥氏加速度，同時(shí)忽略2階小量，可得速度誤差方程：

(11)

其中，fb為加速度計(jì)的輸出，aB為加速度計(jì)的零偏。

(3)位置誤差方程

對(duì)位置方程求偏導(dǎo)，可得位置誤差方程的矢量形式：

(12)

2.2　ZUPT算法

零速檢測(cè)觸發(fā)零速修正，在檢測(cè)為零速的時(shí)間內(nèi)將速度置為0。利用零速檢測(cè)估計(jì)更多誤差參數(shù)，根據(jù)導(dǎo)航誤差模型設(shè)計(jì)零速修正Kalman濾波器[9]，狀態(tài)量取為X=[φnTδVnTδPnTεbT▽bT]T。

量測(cè)量取為檢測(cè)為零速時(shí)刻的解算速度誤差δVn，所以量測(cè)矩陣為：

H=[03×3I3×303×303×303×3]T。

考慮到量測(cè)量δV只在檢測(cè)為零速的時(shí)刻才能獲取，且量測(cè)噪聲與具體運(yùn)動(dòng)和干擾相關(guān)，事先并不能準(zhǔn)確獲知，因此Kalman濾波器需進(jìn)行如下改良：Kalman濾波更新時(shí)刻tk，若零速檢測(cè)為運(yùn)動(dòng)，則濾波器只做時(shí)間更新；反之，若零速檢測(cè)為零速，則濾波器做完整更新，同時(shí)將估計(jì)結(jié)果反饋回系統(tǒng)。

ZUPT對(duì)于減小速度、位置以及水平姿態(tài)誤差是一項(xiàng)非常有效的技術(shù)，不過(guò)該過(guò)程無(wú)法有效地估計(jì)與修正航向誤差[10]。這樣，隨著導(dǎo)航時(shí)間的延長(zhǎng)，方位誤差的影響就會(huì)逐漸取代器件誤差，成為MEMS個(gè)人導(dǎo)航系統(tǒng)的主要誤差源。因此要引入磁強(qiáng)計(jì)，對(duì)航向進(jìn)行修正。

磁強(qiáng)計(jì)可以得到絕對(duì)航向，且誤差不隨時(shí)間積累，但存在兩個(gè)問(wèn)題：一是磁強(qiáng)計(jì)易受外界干擾磁場(chǎng)影響；二是MEMS磁強(qiáng)計(jì)精度低，動(dòng)態(tài)精度更差。因此，引入磁強(qiáng)計(jì)進(jìn)行以下改進(jìn)：1)只選用靜態(tài)數(shù)據(jù)，即零速時(shí)磁強(qiáng)計(jì)輸出的磁航向，與SINS的航向之差作為航向誤差構(gòu)造量測(cè)量；2)采用模值法對(duì)磁強(qiáng)計(jì)是否受到外界干擾做判斷，同一地區(qū)磁場(chǎng)強(qiáng)度不變，即磁強(qiáng)計(jì)輸出模值應(yīng)為一常數(shù)。設(shè)定閾值范圍，在此范圍內(nèi)則認(rèn)為未受到外界干擾，磁航向可以使用。符合以上兩個(gè)條件時(shí)，量測(cè)矩陣改為：

H=[diag(0,0,1)I3×303×303×303×3]T。

個(gè)人導(dǎo)航系統(tǒng)算法框圖如圖4所示。

3　實(shí)驗(yàn)驗(yàn)證

本實(shí)驗(yàn)選用應(yīng)美盛(InvenSense)公司的MPU9250模塊作為微慣性傳感器，MPU9250為9軸組合傳感器將6軸慣性測(cè)量單元(加速度計(jì)+陀螺儀)和3軸磁強(qiáng)計(jì)集成。利用ZigBee模塊通過(guò)標(biāo)準(zhǔn)IIC通信協(xié)議與MPU9250通信，然后ZigBee將數(shù)據(jù)打包發(fā)送至上位機(jī)進(jìn)行導(dǎo)航解算。ZigBee無(wú)線通信模塊選用TI公司的CC2530單片機(jī)，本身帶有射頻的功能，外加一些簡(jiǎn)單電路即可實(shí)現(xiàn)無(wú)線通信功能。ZigBee+ MPU9250構(gòu)成足綁式個(gè)人導(dǎo)航系統(tǒng)無(wú)線采集模塊，如圖5所示。

將無(wú)線采集模塊固定在腳上，進(jìn)行行走實(shí)驗(yàn)。

為對(duì)比滑動(dòng)平均與自適應(yīng)滑動(dòng)平均算法的效果，行走實(shí)驗(yàn)為直線路線行走。行走方向?yàn)檎毕?，行?0步(包括一開(kāi)始的磁強(qiáng)計(jì)羅差校正，原地轉(zhuǎn)動(dòng)兩圈的16步)，距離為52.2m。實(shí)驗(yàn)進(jìn)行了10次，10次的結(jié)果對(duì)比如表1所示。

表1　導(dǎo)航實(shí)驗(yàn)統(tǒng)計(jì)結(jié)果

從表1可以看出，兩種零速檢測(cè)方法都能準(zhǔn)確地計(jì)算出步數(shù)。但在導(dǎo)航誤差上，采用自適應(yīng)滑動(dòng)平均算法得到零速區(qū)間進(jìn)行ZUPT，導(dǎo)航誤差均小于采用自滑動(dòng)平均算法。10次導(dǎo)航結(jié)果中，自適應(yīng)滑動(dòng)平均算法的導(dǎo)航誤差平均值為2.51%，滑動(dòng)平均的誤差平均值為5.09%，效果改善了55.87%。

以第8次實(shí)驗(yàn)為例，當(dāng)零速檢測(cè)值由1變?yōu)?，對(duì)應(yīng)著腳離地的動(dòng)作；當(dāng)零速檢測(cè)值由0變?yōu)?，對(duì)應(yīng)著腳著地的動(dòng)作。根據(jù)檢測(cè)值的變化可以得到一步經(jīng)歷的時(shí)間，進(jìn)而計(jì)算步頻和步數(shù)，如圖6所示。

由圖6可以看出，相對(duì)于滑動(dòng)平均算法對(duì)零速的判斷，自適應(yīng)滑動(dòng)平均算法得到的零速區(qū)間范圍稍大一些。這是因?yàn)樽赃m應(yīng)算法減弱了傳統(tǒng)滑動(dòng)平均算法檢測(cè)零速區(qū)間的延遲問(wèn)題，從而得到了更加準(zhǔn)確的零速區(qū)間判定。兩種算法得到的零速區(qū)間帶入導(dǎo)航解算中去，自適應(yīng)算法的導(dǎo)航精度提高了57%，具體如圖7所示。

4　結(jié)論

本文設(shè)計(jì)了自適應(yīng)滑動(dòng)平均算法檢測(cè)零速修正區(qū)間，能及時(shí)跟蹤模值變化趨勢(shì)，且可以起到平滑抖動(dòng)的作用。本算法能避免平滑算法延時(shí)問(wèn)題，得到更準(zhǔn)確的零速區(qū)間。考慮到現(xiàn)實(shí)生活中行人的運(yùn)動(dòng)形式多種多樣，除了正常的行走外，還有跑、跳、跨、上下樓梯等形式，本文給出的算法還不能適應(yīng)所有運(yùn)動(dòng)形式，針對(duì)這種情況還需要做進(jìn)一步的研究。

[1]Krach B, Roberston P. Cascaded estimation architecture for integration of foot-mounted inertial sensors[C]. IEEE Position Location and Navigation Symposium, 2007: 112-119.

[2]Renaudin V, Yalak O, Tomé P, et al. Indoor navigation of emergency agents[J]. European Journal of Navigation, 2007,5(3): 36-45.

[3]Yun X P, Bachmann E R, Moore H, et al. Self-contained position tracking of human movement using small inertial/magnetic sensor modules [C]. IEEE International Conference on Robotics and Automation， 2007:2526-2533.

[4]Park S K, Suh Y S. A zero velocity detection algorithm sing inertial sensors for pedestrian navigation system[J].Sensors, 2010, 10(10): 9163-9178.

[5]Skog I, Nilsson J O, H?ndel P. Evaluation of zero-velocity detectors for foot-mounted inertial navigation systems[C]. International Conference on Indoor Positioning & Indoor Navigation, 2010:1-6.

[6]Zhou C M, Downey J, Choi J, et al. A shoe-embedded RF sensor for motion detection [J]. IEEE Microwave and Wireless Components Letters, 2011,21(3):169-171.

[7]崔瀟,秦永元,周琪,等. 鞋式個(gè)人導(dǎo)航系統(tǒng)算法和試驗(yàn)研究[J].測(cè)控技術(shù), 2013, 32(3):138-142.

CUI Xiao, QIN Yong-yuan, ZHOU Qi, et al. Algorithm and test research on shoe-mounted personal navigation system[J]. Measurement & Control Technology, 2013, 32(3):138-142.

[8]殷紅,郭杭,鄧曉華.基于foot-mounted的IMU室內(nèi)行人航位推算研究[J].測(cè)繪科學(xué), 2014, 39(1):20-23.

YIN Hong, GUO Hang, DENG Xiao-hua. A research of

IMU indoor pedestrian dead reckoning based on foot-mounted[J]. Science of Surveying and Mapping, 2014, 39(1):20-23.

[9]秦永元,張洪鉞,汪叔華.Kalman濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社, 1998.

QIN Yong-yuan, ZHANG Hong-yue, WANG Shu-hua. Kalman filter and integrated navigation principle[M]. Xi’an: Northwestern Polytechnical University Press, 1998.

[10]張新喜, 張嶸, 郭美鳳, 等. 基于足綁式INS的行人導(dǎo)航三軸磁強(qiáng)計(jì)在線校準(zhǔn)[J]. 清華大學(xué)學(xué)報(bào)(自然科學(xué)版), 2016, 56(2) :211-217.

ZHANG Xin-xi, ZHANG Rong, GUO Mei-feng, et al. Online three-axis magnetometer calibration for a pedestriannavigation system using a foot-mounted inertial navigation system[J]. Journal of Tsinghua University(Science and Technology), 2016, 56(2) :211-217.