改進卡爾曼濾波的行人導航算法

鐘啟林，衷衛聲，熊 劍，郭 杭

改進卡爾曼濾波的行人導航算法

鐘啟林，衷衛聲，熊 劍，郭 杭

（南昌大學 信息工程學院，南昌 330031）

針對使用傳統白噪聲卡爾曼濾波算法實現行人導航時，存在因慣性測量單元（IMU）的有色噪聲影響，而使得行人導航位置精度過低的問題，提出一種基于有色噪聲改進卡爾曼濾波的行人導航算法：采用自回歸模型對微機電型IMU的有色噪聲進行建模；然后根據建模結果對卡爾曼濾波算法進行改進；最后構建基于改進卡爾曼濾波的行人導航算法。實驗結果表明，該法相對于采用白噪聲模型的行人導航算法具有更好的定位精度，定位精度可以提高24.21%。

有色噪聲；慣性測量單元；卡爾曼濾波；行人導航；步態檢測

0 引言

隨著人類生活方式的改變，人們生活和工作基本都在大的建筑物里面，遇到突發情況斷電時，基于無線射頻信號的無線保真（wireless fidelity, WIFI）、紅外線、超寬帶等定位方式都無法正常使用。慣性測量單元（inertial measurement unit, IMU）的數據輸出不易受環境的影響、具有自主性，根據IMU可推算出行人位置[1-2]。基于IMU的慣性行人導航方法，按照原理可劃分為行人航位推算（pedestrian dead reckoning,PDR）和捷聯慣性導航系統（strapdown inertial navigation system, SINS）[3]解算。SINS根據IMU輸出的加速度進行積分并與零速檢測相結合，解算得到位置。PDR主要根據IMU輸出數據、步頻等特征計算得到步長值，準確估計則需要采集行人的身高、性別、行走習慣等才能確定[4-5]，行走模式比較單一。因此本文采用SINS方法。

由于IMU器件輸出存在隨機噪聲，導航算法存在誤差累積[6]等問題，使得IMU的行人導航算法獲得的總體定位效果并不理想。陀螺儀、加速度計的輸出誤差均存在有色噪聲[6]?？柭鼮V波要求量測噪聲和系統噪聲模型已知，且為白噪聲，然而IMU的量測噪聲，行人導航系統噪聲均不是白噪聲，故需要對卡爾曼濾波進行改進。有色噪聲建模的方法，通常有成型濾波器法和時間序列分析法；常用的濾波器模型有向量擴充法[7]、一階馬爾科夫模型[8]、隨機模型補償法[9]等。文獻[3,5]將這些有色噪聲進行簡單的線性化后，按照白噪聲進行處理，這樣會產生較大的誤差，有時會導致推算異常[10]。文獻[11]采用擴充法，將過程噪聲和量測噪聲變為白噪聲，使用神經模糊網絡作為誤差估計器，得到卡爾曼濾波的估計誤差，對預測結果進行修正。實際上，有色噪聲不一定都能表示成白噪聲的函數，這些方法很難消除有色噪聲的影響，精度有限且容易造成濾波發散。有色噪聲在時間上存在相關性[12-13]；文獻[14]根據有色噪聲相鄰歷元間噪聲相關的特點，在系統噪聲和觀測噪聲均為有色噪聲的條件下，對噪聲的協方差矩陣進行了改進；文獻[15]利用協方差傳播定律，改進濾波的估計狀態協方差和增益矩陣，來提高濾波精度；文獻[16]通過時間序列分析法，對有色噪聲進行建模，改進量測噪聲為有色噪聲情況下的擴展卡爾曼濾波算法。這些方法都是假設相鄰歷元噪聲相關，忽略了不相鄰序列的噪聲相關性。因此采用基于時間序列分析法的自回歸模型（autoregressive model, AR）對噪聲進行建模，推導出有色噪聲的改進卡爾曼濾波算法；零速區間IMU的計算速度作為改進卡爾曼濾波的量測值，將改進卡爾曼濾波得到的估計值與慣性導航系統（inertial navigation system, INS）解算相結合構建行人導航算法，來提高定位精度。

1 改進的有色噪聲卡爾曼濾波模型

1.1 有色噪聲及其模型的建立

白噪聲是一種理想化的噪聲，實際工程中十分少見，歷元間互不相關，協方差為零。在工程項目中經常遇到的是有色噪聲，有色噪聲具有噪聲序列中、任意一個時刻與另外一個時刻相關的特性[8,15]。在某些特定情況下，有色噪聲可以直接采用白噪聲進行簡化替代，以減少計算復雜程度。成型濾波器具有通用性，且精度有限，由于采用時間序列分析法，可以直接對噪聲進行高精度建模，故本文采用時間序列分析法進行建模。

根據文獻[7，13]對基于微機電系統（micro-electro-mechanical systems,MEMS）型IMU的誤差分析，IMU的噪聲包含隨機常數、白噪聲和有色噪聲，即輸出噪聲可以描述為

表1 AR(1)、AR(2)、AR(3)及AR(4)的AIC值

1.2 改進的有色噪聲卡爾曼濾波算法

將非線性離散系統的狀態方程和觀測方程表述為

將式（8）代入式（7）可得

因為

根據協方差傳播律[15]可得

式中為零矩陣。

因為

根據協方差傳播律[15]可得

將式（16）代入式（14）可得

根據文獻[15]中的極大后驗估計計算式，建立改進的狀態估值方程為

其中：

結合傳統擴展卡爾曼濾波協方差更新公式，并考慮有色噪聲的影響，則改進后的協方差公式為

其中

式（12）、式（17）至式（22）為改進后的基于有色噪聲的擴展卡爾曼濾波公式。

2 行人導航系統的設計

2.1 系統框架

根據已知初始靜止位置信息及使用IMU的磁力計、加速度計、陀螺儀進行靜態初始對準，獲得初始姿態信息。利用IMU輸出的加速度、角速度進行步態檢測，即零速區間的判斷。若檢測結果為零速區間，則對速度進行零速修正，同時將IMU計算得到的速度作為改進型卡爾曼濾波的量測值；用輸出的狀態誤差對慣性導航算法進行修正，以此推算出后一時刻的位置、速度、姿態等信息。行人導航系統如圖1所示。

圖1 行人導航系統框圖

2.2 步態檢測

行人連續行走時具有明顯的周期性和規律性，兩腳交替著往前跨步，本文將左右腳各往前邁一步視為一個復步運動，即兩步，行走步態圖[18]如圖2所示。從圖2中可知：中腳跟著地①到中腳跟離地④期間，右腳均未離開地面，這期間被稱為支撐期；腳尖離地⑤后，右腳開始往前擺動，直到右腳腳跟著地⑦，這期間稱為擺動期；③和⑥分別為右腳支撐左腳往前跨步、左腳支撐右腳往前跨步的某一時刻步態。右腳剛接觸地面，逐漸產生摩擦力，到右腳掌著地時，摩擦力最大，右腳速度為零；同時在步驟③至步驟④期間，右腳速度為零，因此將該區間稱為零速區。

圖2 行走步態圖[18]

2.3 行人導航算法

在靜止狀態下，初始的位置已知、速度為零，利用加速度計和磁力計求得IMU的初始姿態角[19]；在靜止60 s后，采用加速度計和磁力計的輸出值，計算IMU的初始姿態、加速度計和陀螺儀的初始誤差。由于方法比較固定，不展開詳細敘述，詳細論述見文獻[17]。

在已知當前時刻的位置、速度、姿態信息的前提下，根據陀螺儀和加速度的輸出以及卡爾曼濾波的狀態誤差估計，可推算出下一時刻的信息，主要有以下4個步驟：

3）消除本地重力的影響，即

4）估計時刻的位置、速度、姿態。時刻的速度、位置的估計值為

2.4 改進型卡爾曼濾波的應用

3 實驗驗證

實驗采用克森斯（XSENS）公司的運動跟蹤模塊（motion tracking module, MTi），其主要器件包含有三軸陀螺儀、加速度計和磁力計，其傳感器性能如表2所示。

表2 傳感器的性能指標

將MTi綁在實驗員的腳背上，實驗員圍繞事先已經測量好的長方形走廊正常行走并進行數據采集。MTi的采樣頻率設置為100 Hz，初始對準時間為60 s，實驗員通過數據線將實時采集到的數據上傳到電腦。為了驗證本方法在長距離方面的優勢，實驗選取的路線比較長，實驗場景及器材如圖3所示。

圖3 實驗場景及器材圖

實驗場地位于信工樓6樓的B區環形走廊，實驗路徑為38 m×60 m的閉合矩形。實驗員沿著靠近建筑內側的走廊中間進行數據采集，共進行6組。實驗軌跡設計的直角轉彎處，行人運動狀態發生變化，誤差也會隨著發生改變，這有利于驗證算法的性能。

圖4為第三次實驗結果路徑對比圖，圖4中的實線為真實軌跡，傳統算法（圖中為多段線）為使用基于白噪聲卡爾曼濾波模型的行人導航算法，改進算法（圖中為點線）為基于有色噪聲模型的改進卡爾曼濾波算法下的行人導航算法。從圖4可以看出，在第一個直角轉彎之后，兩種算法的軌跡都開始偏離真實軌跡，這是由于行人運動狀態在轉彎處發生了變化引起誤差增大的后果；而基于有色噪聲的改進卡爾曼濾波行人導航算法，對位置誤差具有更加優秀的修正效果；改進卡爾曼濾波行人導航算法的軌跡終點，較普通白噪聲卡爾曼濾波行人導航算法更接近于實際終點（0,0），這進一步說明，改進的卡爾曼濾波算法能提高行人軌跡的位置精度。

圖4 實驗軌跡對比圖

通過對同一行走路線的多次實驗，定位誤差結果分布如表3所示。表3中的傳統算法，是指基于白噪聲卡爾曼濾波模型的行人導航算法，改進算法是指基于有色噪聲模型的改進卡爾曼濾波算法的行人導航算法；用解算所得的終點坐標與實際終點坐標（0,0）的距離值表示定位誤差；定位誤差提高比例是改進算法的定位誤差與傳統算法的定位誤差的差值占傳統算法的定位誤差的比例。

表3 定位誤差結果分析表

為便于觀察，繪制了如圖5所示的定位誤差對比柱形圖。

圖5 位置誤差對比圖

從圖5中6組算法的對比數據可以看出，改進的算法定位誤差更??；第2組和第5組數據相對來說誤差比較大，經過分析發現，主要原因是這兩組行走的步頻比正常行走高出了很多?；谟猩肼暤母倪M卡爾曼濾波行人導航算法的平均誤差為4.72 m，基于白噪聲的傳統卡爾曼濾波行人導航算法的平均誤差為6.22 m，改進卡爾曼濾波行人導航算法的定位誤差減小比例為24.21%，這一結果充分說明了改進卡爾曼濾波的行人導航算法具有更加優異的性能。

4 結束語

根據MEMS型IMU的噪聲特點，以及有色噪聲不相鄰時刻的噪聲也存在相關性，采用基于時間序列分析法的二階AR模型，對有色噪聲進行建模，根據該模型對卡爾曼濾波進行改進，基于步態檢測、零速修正構建了基于改進卡爾曼濾波的行人導航算法。在長距離矩形路徑中進行了6組實驗，實驗表明，該方法相對于傳統白噪聲卡爾曼濾波行人導航算法，能提高約24.21 %的定位精度。

[1]DAVIDSON P, PICHE R. A survey of selected indoor positioning methods for smartphones[J]. IEEE Communications Surveys & Tutorials, 2017, 19(2): 1347-1370.

[2]ZHUANG Yuan, HUA Luchi, QI Longning, et al. A survey of positioning systems using visible LED lights[J]. IEEE Communications Surveys & Tutorials, 2018, 20(3): 1963-1988.

[3]趙紅宇. 慣性行人導航系統的算法研究[D]. 大連: 大連理工大學, 2015.

[4]田曉春, 陳家斌, 韓勇強, 等. 多條件約束的行人導航零速區間檢測算法[J]. 中國慣性技術學報, 2016, 24(1): 1-5.

[5]ZHOU Zebo, MO Shanhui, WU Jin, et al. Behaviors classi?cation base distance measuring system for pedestrians via a foot-mounted inertial sensor[EB/OL]. [2020-04-08]. https: //hal. inria. fr/hal-01929624/document.

[6]宋海賓, 楊平, 徐立波. MEMS傳感器隨機誤差分析及處理[J]. 傳感技術學報, 2013, 26(12): 1719-1723.

[7]秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導航原理[M]. 西安: 西北工業大學出版社, 2012, 128-129.

[8]劉俊釗, 林旭, 劉奕凡. 一步相關卡爾曼濾波估計[J]. 測繪, 2018, 41(5): 200-203.

[9]孫強, 惠曉濱, 黃鶴. 一種色噪聲下的自適應Kalman跟蹤濾波器[J]. 火力與指揮控制, 2011, 36(3): 49-51.

[10]熊雪, 郭敏華, 李偉杰, 等. 基于有色噪聲的改進卡爾曼濾波方法[J]. 中國慣性技術學報, 2017, 25(1): 33-36.

[11]宋國東, 姜守達, 林連雷. 復雜有色噪聲廣義系統信息融合Kalman濾波器[J]. 儀器儀表學報, 2013, 34(5): 1195-1200.

[12]XIA Youshen, WEI Qingquan. An effective Kalman filtering method for enhancing speech in the presence of colored noise[C]//The Institute of Electrical and Electronic Engineers (IEEE). Processing of 2016 International Conference on Audio, Language and Image. Shanghai: IEEE, 2016: 469-474[2020-04-08].

[13]吳永亮, 王田苗, 梁建宏. 基于小波變換的MIMU隨機誤差建模[J]. 中國慣性技術學報, 2010, 18(6): 660-664.

[14]陳方, 吳小役, 劉文逸, 等. 有色噪聲條件下的卡爾曼濾波[J]. 西安郵電大學學報, 2014, 19(2): 56-59.

[15]張奇正, 黃雪梅, 蔡述江. 有色噪聲條件下擴展卡爾曼濾波算法研究[J]. 計算機仿真, 2019, 36(6): 273-276.

[16]NASSAR S, SCHWARZ K P, NOURELDIN A, et al. Modeling inertial sensor errors using autoregressive (AR) models[J]. Navigation, 2004, 51(4), 259-268.

[17]CHANG Guobin. On Kalman filter for linear system with colored measurement noise[J]. Journal of Geodesy, 2014, 88(12): 1163-1170.

[18]錢偉行, 周紫君, 謝非, 等. 基于機器學習與步態特征輔助的行人導航方法[J]. 中國慣性技術學報, 2019, 27(4): 433-439, 447.

[19]殷紅. 基于foot-mounted的IMU室內行人航跡推算研究[D]. 南昌: 南昌大學, 2013.

Pedestrian navigation algorithm based on improved Kalman filtering

ZHONG Qilin, ZHONG Weisheng, XIONG Jian, GUO Hang

（School of Information Engineering,Nanchang University, Nanchang 330031,China）

Aiming at the problem that the position accuracy of pedestrian navigation is low due to the influence of the colored noise of the Inertial Measurement Unit (IMU) during pedestrian navigation using the traditional white noise Kalman filter algorithm, the paper proposed a pedestrian navigation algorithm based on colored noise improved Kalman filter: the auto-regressive model was used to model the colored noise of the Micro-Electro-Mechanical System (MEMS) IMU; and the Kalman filter algorithm was improved according to the modeling results; finally the pedestrian navigation algorithm based on the improved Kalman filter was constructed. Experimental result showed that the proposed algorithm would havehigher positioning accuracy than the pedestrian navigation algorithm using the white noise model, and the final positioning accuracy could be improved by 24.21%.

colored noise; inertial measurement unit; Kalman filtering; pedestrian navigation; gait detection

P228

A

2095-4999(2021)02-0028-07

鐘啟林，衷衛聲，熊劍，等. 改進卡爾曼濾波的行人導航算法[J]. 導航定位學報,2021,9(2): 28-34.（ZHONG Qilin, ZHONG Weisheng, XIONG Jian, et al. Pedestrian navigation algorithm based on improved Kalman filtering[J]. Journal of Navigation and Positioning, 2021, 9(2): 28-34.）

10.16547/j.cnki.10-1096.20210205.

2020-04-09

國家自然科學基金項目（41764002）。

鐘啟林（1994—），男，江西贛州人，碩士研究生，研究方向為行人導航及數據融合算法。

衷衛聲（1967—），男，江西宜春人，碩士，教授，碩士生導師，研究方向為組合導航、儀器儀表及智能控制。