鐘啟林，衷衛(wèi)聲，熊 劍，郭 杭

改進(jìn)卡爾曼濾波的行人導(dǎo)航算法

鐘啟林，衷衛(wèi)聲，熊 劍，郭 杭

（南昌大學(xué) 信息工程學(xué)院，南昌 330031）

針對(duì)使用傳統(tǒng)白噪聲卡爾曼濾波算法實(shí)現(xiàn)行人導(dǎo)航時(shí)，存在因慣性測(cè)量單元（IMU）的有色噪聲影響，而使得行人導(dǎo)航位置精度過(guò)低的問(wèn)題，提出一種基于有色噪聲改進(jìn)卡爾曼濾波的行人導(dǎo)航算法：采用自回歸模型對(duì)微機(jī)電型IMU的有色噪聲進(jìn)行建模；然后根據(jù)建模結(jié)果對(duì)卡爾曼濾波算法進(jìn)行改進(jìn)；最后構(gòu)建基于改進(jìn)卡爾曼濾波的行人導(dǎo)航算法。實(shí)驗(yàn)結(jié)果表明，該法相對(duì)于采用白噪聲模型的行人導(dǎo)航算法具有更好的定位精度，定位精度可以提高24.21%。

有色噪聲；慣性測(cè)量單元；卡爾曼濾波；行人導(dǎo)航；步態(tài)檢測(cè)

0 引言

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

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

1 改進(jìn)的有色噪聲卡爾曼濾波模型

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

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

根據(jù)文獻(xiàn)[7，13]對(duì)基于微機(jī)電系統(tǒng)（micro-electro-mechanical systems,MEMS）型IMU的誤差分析，IMU的噪聲包含隨機(jī)常數(shù)、白噪聲和有色噪聲，即輸出噪聲可以描述為

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

1.2 改進(jìn)的有色噪聲卡爾曼濾波算法

將非線性離散系統(tǒng)的狀態(tài)方程和觀測(cè)方程表述為

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

因?yàn)?/p>

根據(jù)協(xié)方差傳播律[15]可得

式中為零矩陣。

因?yàn)?/p>

根據(jù)協(xié)方差傳播律[15]可得

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

根據(jù)文獻(xiàn)[15]中的極大后驗(yàn)估計(jì)計(jì)算式，建立改進(jìn)的狀態(tài)估值方程為

其中：

結(jié)合傳統(tǒng)擴(kuò)展卡爾曼濾波協(xié)方差更新公式，并考慮有色噪聲的影響，則改進(jìn)后的協(xié)方差公式為

其中

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

2 行人導(dǎo)航系統(tǒng)的設(shè)計(jì)

2.1 系統(tǒng)框架

根據(jù)已知初始靜止位置信息及使用IMU的磁力計(jì)、加速度計(jì)、陀螺儀進(jìn)行靜態(tài)初始對(duì)準(zhǔn)，獲得初始姿態(tài)信息。利用IMU輸出的加速度、角速度進(jìn)行步態(tài)檢測(cè)，即零速區(qū)間的判斷。若檢測(cè)結(jié)果為零速區(qū)間，則對(duì)速度進(jìn)行零速修正，同時(shí)將IMU計(jì)算得到的速度作為改進(jìn)型卡爾曼濾波的量測(cè)值；用輸出的狀態(tài)誤差對(duì)慣性導(dǎo)航算法進(jìn)行修正，以此推算出后一時(shí)刻的位置、速度、姿態(tài)等信息。行人導(dǎo)航系統(tǒng)如圖1所示。

圖1 行人導(dǎo)航系統(tǒng)框圖

2.2 步態(tài)檢測(cè)

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

圖2 行走步態(tài)圖[18]

2.3 行人導(dǎo)航算法

在靜止?fàn)顟B(tài)下，初始的位置已知、速度為零，利用加速度計(jì)和磁力計(jì)求得IMU的初始姿態(tài)角[19]；在靜止60 s后，采用加速度計(jì)和磁力計(jì)的輸出值，計(jì)算IMU的初始姿態(tài)、加速度計(jì)和陀螺儀的初始誤差。由于方法比較固定，不展開(kāi)詳細(xì)敘述，詳細(xì)論述見(jiàn)文獻(xiàn)[17]。

在已知當(dāng)前時(shí)刻的位置、速度、姿態(tài)信息的前提下，根據(jù)陀螺儀和加速度的輸出以及卡爾曼濾波的狀態(tài)誤差估計(jì)，可推算出下一時(shí)刻的信息，主要有以下4個(gè)步驟：

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

4）估計(jì)時(shí)刻的位置、速度、姿態(tài)。時(shí)刻的速度、位置的估計(jì)值為

2.4 改進(jìn)型卡爾曼濾波的應(yīng)用

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

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

表2 傳感器的性能指標(biāo)

將MTi綁在實(shí)驗(yàn)員的腳背上，實(shí)驗(yàn)員圍繞事先已經(jīng)測(cè)量好的長(zhǎng)方形走廊正常行走并進(jìn)行數(shù)據(jù)采集。MTi的采樣頻率設(shè)置為100 Hz，初始對(duì)準(zhǔn)時(shí)間為60 s，實(shí)驗(yàn)員通過(guò)數(shù)據(jù)線將實(shí)時(shí)采集到的數(shù)據(jù)上傳到電腦。為了驗(yàn)證本方法在長(zhǎng)距離方面的優(yōu)勢(shì)，實(shí)驗(yàn)選取的路線比較長(zhǎng)，實(shí)驗(yàn)場(chǎng)景及器材如圖3所示。

圖3 實(shí)驗(yàn)場(chǎng)景及器材圖

實(shí)驗(yàn)場(chǎng)地位于信工樓6樓的B區(qū)環(huán)形走廊，實(shí)驗(yàn)路徑為38 m×60 m的閉合矩形。實(shí)驗(yàn)員沿著靠近建筑內(nèi)側(cè)的走廊中間進(jìn)行數(shù)據(jù)采集，共進(jìn)行6組。實(shí)驗(yàn)軌跡設(shè)計(jì)的直角轉(zhuǎn)彎處，行人運(yùn)動(dòng)狀態(tài)發(fā)生變化，誤差也會(huì)隨著發(fā)生改變，這有利于驗(yàn)證算法的性能。

圖4為第三次實(shí)驗(yàn)結(jié)果路徑對(duì)比圖，圖4中的實(shí)線為真實(shí)軌跡，傳統(tǒng)算法（圖中為多段線）為使用基于白噪聲卡爾曼濾波模型的行人導(dǎo)航算法，改進(jìn)算法（圖中為點(diǎn)線）為基于有色噪聲模型的改進(jìn)卡爾曼濾波算法下的行人導(dǎo)航算法。從圖4可以看出，在第一個(gè)直角轉(zhuǎn)彎之后，兩種算法的軌跡都開(kāi)始偏離真實(shí)軌跡，這是由于行人運(yùn)動(dòng)狀態(tài)在轉(zhuǎn)彎處發(fā)生了變化引起誤差增大的后果；而基于有色噪聲的改進(jìn)卡爾曼濾波行人導(dǎo)航算法，對(duì)位置誤差具有更加優(yōu)秀的修正效果；改進(jìn)卡爾曼濾波行人導(dǎo)航算法的軌跡終點(diǎn)，較普通白噪聲卡爾曼濾波行人導(dǎo)航算法更接近于實(shí)際終點(diǎn)（0,0），這進(jìn)一步說(shuō)明，改進(jìn)的卡爾曼濾波算法能提高行人軌跡的位置精度。

圖4 實(shí)驗(yàn)軌跡對(duì)比圖

通過(guò)對(duì)同一行走路線的多次實(shí)驗(yàn)，定位誤差結(jié)果分布如表3所示。表3中的傳統(tǒng)算法，是指基于白噪聲卡爾曼濾波模型的行人導(dǎo)航算法，改進(jìn)算法是指基于有色噪聲模型的改進(jìn)卡爾曼濾波算法的行人導(dǎo)航算法；用解算所得的終點(diǎn)坐標(biāo)與實(shí)際終點(diǎn)坐標(biāo)（0,0）的距離值表示定位誤差；定位誤差提高比例是改進(jìn)算法的定位誤差與傳統(tǒng)算法的定位誤差的差值占傳統(tǒng)算法的定位誤差的比例。

表3 定位誤差結(jié)果分析表

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

圖5 位置誤差對(duì)比圖

從圖5中6組算法的對(duì)比數(shù)據(jù)可以看出，改進(jìn)的算法定位誤差更??；第2組和第5組數(shù)據(jù)相對(duì)來(lái)說(shuō)誤差比較大，經(jīng)過(guò)分析發(fā)現(xiàn)，主要原因是這兩組行走的步頻比正常行走高出了很多。基于有色噪聲的改進(jìn)卡爾曼濾波行人導(dǎo)航算法的平均誤差為4.72 m，基于白噪聲的傳統(tǒng)卡爾曼濾波行人導(dǎo)航算法的平均誤差為6.22 m，改進(jìn)卡爾曼濾波行人導(dǎo)航算法的定位誤差減小比例為24.21%，這一結(jié)果充分說(shuō)明了改進(jìn)卡爾曼濾波的行人導(dǎo)航算法具有更加優(yōu)異的性能。

4 結(jié)束語(yǔ)

根據(jù)MEMS型IMU的噪聲特點(diǎn)，以及有色噪聲不相鄰時(shí)刻的噪聲也存在相關(guān)性，采用基于時(shí)間序列分析法的二階AR模型，對(duì)有色噪聲進(jìn)行建模，根據(jù)該模型對(duì)卡爾曼濾波進(jìn)行改進(jìn)，基于步態(tài)檢測(cè)、零速修正構(gòu)建了基于改進(jìn)卡爾曼濾波的行人導(dǎo)航算法。在長(zhǎng)距離矩形路徑中進(jìn)行了6組實(shí)驗(yàn)，實(shí)驗(yàn)表明，該方法相對(duì)于傳統(tǒng)白噪聲卡爾曼濾波行人導(dǎo)航算法，能提高約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ǎo)航系統(tǒng)的算法研究[D]. 大連: 大連理工大學(xué), 2015.

[4]田曉春, 陳家斌, 韓勇強(qiáng), 等. 多條件約束的行人導(dǎo)航零速區(qū)間檢測(cè)算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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ī)誤差分析及處理[J]. 傳感技術(shù)學(xué)報(bào), 2013, 26(12): 1719-1723.

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

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

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

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

[11]宋國(guó)東, 姜守達(dá), 林連雷. 復(fù)雜有色噪聲廣義系統(tǒng)信息融合Kalman濾波器[J]. 儀器儀表學(xué)報(bào), 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ī)誤差建模[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2010, 18(6): 660-664.

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

[15]張奇正, 黃雪梅, 蔡述江. 有色噪聲條件下擴(kuò)展卡爾曼濾波算法研究[J]. 計(jì)算機(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ī)器學(xué)習(xí)與步態(tài)特征輔助的行人導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2019, 27(4): 433-439, 447.

[19]殷紅. 基于foot-mounted的IMU室內(nèi)行人航跡推算研究[D]. 南昌: 南昌大學(xué), 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

鐘啟林，衷衛(wèi)聲，熊劍，等. 改進(jìn)卡爾曼濾波的行人導(dǎo)航算法[J]. 導(dǎo)航定位學(xué)報(bào),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

國(guó)家自然科學(xué)基金項(xiàng)目（41764002）。

鐘啟林（1994—），男，江西贛州人，碩士研究生，研究方向?yàn)樾腥藢?dǎo)航及數(shù)據(jù)融合算法。

衷衛(wèi)聲（1967—），男，江西宜春人，碩士，教授，碩士生導(dǎo)師，研究方向?yàn)榻M合導(dǎo)航、儀器儀表及智能控制。