足綁式PNS自適應滑動平均零速區間檢測方法

陳克川，李保國，劉思慶，李淑影

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

0　引言

現如今，為個人提供定位服務的導航設備在人們的生活中得到了廣泛的應用，已成為人們日常生活中相當重要的一部分。該功能可以有效地指導人們在陌生環境下的活動，有的個人導航系統設備可以通過無線通訊傳輸給監控單元，實時監測徒步人員的位置變化[1]。目前，絕大多數個人導航產品都是基于衛星導航定位，這種導航方式對室內應用無能為力。

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

但是市場上MEMS運動傳感器精度低、噪聲大，用于個人導航時誤差隨時間積累，發散十分快。因此，必須利用行人行走時落地零速的特點，對速度誤差、位置誤差和水平姿態角誤差進行零速修正(Zero Velocity Update, ZUPT)，以提高定位導航的精度。

根據行人運動特點，當腳著地時，捷聯在腳上的IMU速度為零。據此對姿態、速度和位置信息進行零速修正，可以有效抑制導航系統參數發散。零速修正的關鍵在于零速區間精確的檢測，本文在借鑒前人研究工作的基礎上，設計了基于自適應滑動平均算法的零速區間檢測方法。經實驗驗證，該方法能準確、有效地檢測出零速區間的范圍，證明了該算法的有效性。

1　自適應滑動平均零速區間檢測方法

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

在足綁式PNS中，靜止狀態即零速狀態的判定[5]，是零速修正的基礎。大多數文獻是基于陀螺或者加速度計或者二者兼有之的輸出進行零速判定，部分文獻介紹了利用壓電、超聲波[6]等輔助傳感器進行零速判定。

為不增加額外傳感器，同時更準確地進行零速區間判定，本文利用自適應滑動平均算法對三軸陀螺輸出和三軸加速度計輸出進行聯合判定。

1.1　自適應滑動平均算法

自適應滑動平均算法廣泛用于金融計算、雷達系統等，常用來解決尋找包含高噪聲動態信號的鋒利間斷點。算法首先鑒別出間斷點發生的時間段，然后在時間區間內通過自適應的調整滑動窗口大小，避免滑動平均算法造成的時間延遲。該算法是一種基于迭代滑動平均的自適應算法，算法根據滑動平均后數據的變化趨勢，通過比較樣本方差，改變滑動窗口的大小。因此，在數據變化的區間，窗口會自適應縮放，數據變化率越大，窗口越緊湊。

算法步驟：

1)取長度為N的采樣數據作為一幀，并在區間[2q+1,N-2q-1]內應用滑動平均算法KZq,k[X(t)]處理。其中，k為迭代次數，q為濾波長度。

應用q點滑動平均算法做數據預處理，KZq,k是一個迭代的滑動平均算法：

(1)

其中，Xi為原始數據，yt為濾波后的數據。

2)預處理后的數據yt將用于滑動平均的自適應濾波算法。在自適應濾波器中，窗口大小將自適應調整。濾波器分別由幀頭(qh)和幀尾(qt)構成，在數據變化率較大的區間，有效地調整其大小，而在間斷點處幀頭qh將減小。

3)根據滑動平均算法構造差分向量：

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

(2)

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

(3)

即：

(4)

(5)

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

(6)

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

應用自適應滑動平均算法處理含有間斷點數據，如圖1所示。

行人走動時，腳部的運動存在明顯的運動、停止、運動、停止……交替前進的特殊運動規律[7]。體現在慣性傳感器輸出上，則有明顯的間斷點。因此，用該算法可以有效地找到由動到靜或者由靜到動的間斷點，即零速區間的起始點。

1.2　零速區間檢測方法

零速區間檢測過程如下：

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

采用加速度的模進行判斷是否為零速，K時刻加速度的模為：

(7)

如圖2所示，在零速狀態下，即在沒有線性加速度干擾階段，理論上幾乎只有當地重力加速度g。因此，該階段加速度計輸出的模應在某一范圍內，將加速度計模值減g后取絕對值，設定閾值后即可進行零速點的檢測，本文設置閾值為thamin=-0.1g，thamax=0.1g。

加速度數據判定零速有效標志：

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

同樣采用角速度的模進行判斷是否零速，K時刻角速度的模為：

(8)

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

角速度數據判定零速有效標志：

最終，將基于角速度模值檢測的結果與基于加速度計檢測的結果相與作為檢測的結果，以便提高零速區間檢測的準確性。

flag=flag_a&flag_g

(9)

2　PNS算法設計

PNS采用成熟的SINS算法。由于個人導航系統的導航范圍有限，位置宜采用東-北-天導航坐標系，即地理坐標系。另外，行人行走速度較慢，計算過程中的小量可以忽略，而且MEMS陀螺儀精度比較差，地球自轉角速度完全淹沒在噪聲之中，因此導航方程可以做相應的簡化[8]。

2.1　PNS誤差方程

根據行人運動特點，推導了適用于行人導航的SINS的姿態誤差方程、速度誤差方程以及位置誤差方程。

(1)姿態誤差方程

忽略地球轉速以及行人相對地球角速度小量，SINS的姿態誤差方程的矢量形式為：

(10)

(2)速度誤差方程

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

(11)

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

(3)位置誤差方程

對位置方程求偏導，可得位置誤差方程的矢量形式：

(12)

2.2　ZUPT算法

零速檢測觸發零速修正，在檢測為零速的時間內將速度置為0。利用零速檢測估計更多誤差參數，根據導航誤差模型設計零速修正Kalman濾波器[9]，狀態量取為X=[φnTδVnTδPnTεbT▽bT]T。

量測量取為檢測為零速時刻的解算速度誤差δVn，所以量測矩陣為：

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

考慮到量測量δV只在檢測為零速的時刻才能獲取，且量測噪聲與具體運動和干擾相關，事先并不能準確獲知，因此Kalman濾波器需進行如下改良：Kalman濾波更新時刻tk，若零速檢測為運動，則濾波器只做時間更新；反之，若零速檢測為零速，則濾波器做完整更新，同時將估計結果反饋回系統。

ZUPT對于減小速度、位置以及水平姿態誤差是一項非常有效的技術，不過該過程無法有效地估計與修正航向誤差[10]。這樣，隨著導航時間的延長，方位誤差的影響就會逐漸取代器件誤差，成為MEMS個人導航系統的主要誤差源。因此要引入磁強計，對航向進行修正。

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

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

個人導航系統算法框圖如圖4所示。

3　實驗驗證

本實驗選用應美盛(InvenSense)公司的MPU9250模塊作為微慣性傳感器，MPU9250為9軸組合傳感器將6軸慣性測量單元(加速度計+陀螺儀)和3軸磁強計集成。利用ZigBee模塊通過標準IIC通信協議與MPU9250通信，然后ZigBee將數據打包發送至上位機進行導航解算。ZigBee無線通信模塊選用TI公司的CC2530單片機，本身帶有射頻的功能，外加一些簡單電路即可實現無線通信功能。ZigBee+ MPU9250構成足綁式個人導航系統無線采集模塊，如圖5所示。

將無線采集模塊固定在腳上，進行行走實驗。

為對比滑動平均與自適應滑動平均算法的效果，行走實驗為直線路線行走。行走方向為正北向，行走60步(包括一開始的磁強計羅差校正，原地轉動兩圈的16步)，距離為52.2m。實驗進行了10次，10次的結果對比如表1所示。

表1　導航實驗統計結果

從表1可以看出，兩種零速檢測方法都能準確地計算出步數。但在導航誤差上，采用自適應滑動平均算法得到零速區間進行ZUPT，導航誤差均小于采用自滑動平均算法。10次導航結果中，自適應滑動平均算法的導航誤差平均值為2.51%，滑動平均的誤差平均值為5.09%，效果改善了55.87%。

以第8次實驗為例，當零速檢測值由1變為0，對應著腳離地的動作；當零速檢測值由0變為1，對應著腳著地的動作。根據檢測值的變化可以得到一步經歷的時間，進而計算步頻和步數，如圖6所示。

由圖6可以看出，相對于滑動平均算法對零速的判斷，自適應滑動平均算法得到的零速區間范圍稍大一些。這是因為自適應算法減弱了傳統滑動平均算法檢測零速區間的延遲問題，從而得到了更加準確的零速區間判定。兩種算法得到的零速區間帶入導航解算中去，自適應算法的導航精度提高了57%，具體如圖7所示。

4　結論

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

[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]崔瀟,秦永元,周琪,等. 鞋式個人導航系統算法和試驗研究[J].測控技術, 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室內行人航位推算研究[J].測繪科學, 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濾波與組合導航原理[M].西安:西北工業大學出版社, 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的行人導航三軸磁強計在線校準[J]. 清華大學學報(自然科學版), 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.