張 海， 樊啟高， 莊祥鵬， 賈 捷， 張鵬松

(江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院，江蘇 無(wú)錫 214122)

0 引 言

室內(nèi)定位是指在室內(nèi)環(huán)境中實(shí)現(xiàn)位置定位，在應(yīng)急救援、物流和綜合性醫(yī)院等行業(yè)均展現(xiàn)出巨大的前景[1～4]。由于室內(nèi)定位的方法容易受到外部環(huán)境干擾。微機(jī)電慣性測(cè)量單元(MEMS-IMU)具有結(jié)構(gòu)簡(jiǎn)單、體積小點(diǎn)，同時(shí)慣性測(cè)量完全自主，全天候，不受外界環(huán)境干擾，無(wú)信號(hào)丟失，適合于室內(nèi)定位。

慣性導(dǎo)航定位技術(shù)以牛頓力學(xué)為基礎(chǔ)，通過(guò)測(cè)量載體在慣性參考系的加速度，對(duì)時(shí)間進(jìn)行積分，長(zhǎng)時(shí)間定位會(huì)存在累積誤差。文獻(xiàn)[5]利用Bagging模型深度學(xué)習(xí)慣導(dǎo)誤差之間的內(nèi)在關(guān)系，對(duì)慣性導(dǎo)航的誤差進(jìn)行抑制。文獻(xiàn)[6]提出慣性導(dǎo)航系統(tǒng)/超短基線/多普勒計(jì)程儀(INS/USBL/DVL)組合導(dǎo)航聯(lián)邦濾波方法，有效抑制慣導(dǎo)的累積誤差。針對(duì)特定的行人室內(nèi)定位，步態(tài)檢測(cè)模式被提出[7]。

在濾波算法方面，由于不能準(zhǔn)確估計(jì)系統(tǒng)噪聲和量測(cè)噪聲的統(tǒng)計(jì)特性，傳統(tǒng)卡爾曼(Kalman)濾波效果不佳[8]。文獻(xiàn)[9]引入模糊自適應(yīng)的卡爾曼濾波,可以在線自適應(yīng)調(diào)整量測(cè)噪聲方陣，提高了定位精度，但動(dòng)態(tài)變差；文獻(xiàn)[10]根據(jù)極大似然準(zhǔn)則構(gòu)造系統(tǒng)噪聲的估計(jì)模型,引入滾動(dòng)時(shí)域策略對(duì)模型進(jìn)行優(yōu)化,利用二次規(guī)劃方法求取噪聲統(tǒng)計(jì)的估計(jì)值,但計(jì)算量偏大。此外，Sage-Husa自適應(yīng)濾波也適用于定位系統(tǒng)，通過(guò)噪聲估計(jì)器實(shí)時(shí)估計(jì)和修正噪聲的統(tǒng)計(jì)特性，從而降低誤差[11～13]。Sage-Husa算法難以同時(shí)精確給出系統(tǒng)噪聲和量測(cè)噪聲的統(tǒng)計(jì)特性。

簡(jiǎn)化Sage-Husa自適應(yīng)算法是在某一噪聲變化不大情況下，在線估計(jì)和修正另一噪聲的統(tǒng)計(jì)統(tǒng)計(jì)特性，實(shí)用性較高[18]。

1 系統(tǒng)模型

1.1 行人定位模型

行人定位模型包含數(shù)據(jù)采集到位姿輸出的全過(guò)程，如圖1所示。通過(guò)MEMS-IMU獲取加速度、角速度以及磁場(chǎng)強(qiáng)度，并對(duì)其進(jìn)行預(yù)濾波處理。將加速度計(jì)和磁強(qiáng)計(jì)在穩(wěn)態(tài)下的姿態(tài)角，以及陀螺儀在動(dòng)態(tài)下的姿態(tài)角優(yōu)勢(shì)互補(bǔ)，進(jìn)行濾波融合，獲得較為精確的空間三維姿態(tài)角。將加速度進(jìn)行二次積分分別獲取行人的速度以及位置。采用零速閾值判斷對(duì)行人移動(dòng)過(guò)程中的零速區(qū)間進(jìn)行判斷，并進(jìn)行零速更新，提高速度的精確性。數(shù)據(jù)濾波部分是將慣性導(dǎo)航算法得到的角速度誤差、加速度誤差、速度誤差以及位置誤差作為定位系統(tǒng)的狀態(tài)矢量，以更新后的速度以及位置為定位系統(tǒng)的量測(cè)矢量。通過(guò)濾波算法，對(duì)系統(tǒng)內(nèi)部誤差進(jìn)行估計(jì)以及修正，提高定位系統(tǒng)的精度。

圖1 行人定位原理

1.2 小波預(yù)處理

受制造工藝和使用環(huán)境的影響，MEMS誤差成為制約慣性導(dǎo)航系統(tǒng)精度的主要原因。目前主要的去噪方法是小波去噪，首先將信號(hào)S分解為低頻噪聲A與高頻噪聲D，n次分解后為

(1)

根據(jù)信號(hào)和噪聲在各尺度上的不同特點(diǎn)，選擇合適的閾值準(zhǔn)則處理小波系數(shù)，最后重構(gòu)信號(hào)，實(shí)現(xiàn)了信噪分離。

1.3 融合濾波

姿態(tài)矩陣計(jì)算直接影響姿態(tài)、速度以及位置的精確性。陀螺儀存在著靜態(tài)漂移現(xiàn)象，在計(jì)算姿態(tài)時(shí)會(huì)產(chǎn)生累積誤差；加速度計(jì)和磁強(qiáng)計(jì)的動(dòng)態(tài)響應(yīng)較差，但沒(méi)有累積誤差。因此在靜態(tài)和動(dòng)態(tài)條件下分別采用加速度計(jì)，磁強(qiáng)計(jì)輸出的姿態(tài)信息和陀螺儀輸出的姿態(tài)信息，然后對(duì)兩者輸出進(jìn)行優(yōu)勢(shì)互補(bǔ)，提高姿態(tài)矩陣的精確度。融合濾波算法如圖2所示，將加速度計(jì)與磁強(qiáng)計(jì)計(jì)算得到的姿態(tài)角與最終姿態(tài)角的誤差進(jìn)行PI控制，再采用互補(bǔ)濾波算法對(duì)姿態(tài)信息進(jìn)行融合，提高姿態(tài)的精度。

圖2 融合濾波

(2)

1.4 零速更新算法

行人在移動(dòng)時(shí)左右腳交替移動(dòng)，每步移動(dòng)可近似為一個(gè)加速和減速的過(guò)程。同一只腳隨著腳后跟離地而加速，腳后跟著地而減速。根據(jù)加速度和角速度的變化，形成“加速—減速—零速—加速”的循環(huán)模式。

行走周期內(nèi)存在腳完全接觸地的情況，即零速區(qū)間。通過(guò)加速度和角速度變化來(lái)分析與識(shí)別行人的零速區(qū)間，并進(jìn)行零速更新。本文在此基礎(chǔ)上，采用多條件閾值判別算法對(duì)零速區(qū)間進(jìn)行判斷

(3)

(4)

2 加強(qiáng)抗差自適應(yīng)卡爾曼濾波

2.1 自適應(yīng)卡爾曼濾波

針對(duì)行人在二維坐標(biāo)系中的定位要求，本文以行人的角速度、加速度、速度以及位置的誤差作為該系統(tǒng)的狀態(tài)量；以速度以及位置的計(jì)算值為定位量測(cè)量。卡爾曼濾波利用信號(hào)與噪聲的狀態(tài)模型，采用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測(cè)值來(lái)更新對(duì)當(dāng)前狀態(tài)變量的估計(jì)

x(k+1)=Φ(k+1,k)x(k)+w(k)

(5)

Z(k)=H(k)x(k)+v(k)

(6)

式中x(k)為k時(shí)刻系統(tǒng)狀態(tài)量；Φ(k+1,k)為k時(shí)刻到k+1時(shí)刻的狀態(tài)轉(zhuǎn)移陣；w(k)為系統(tǒng)噪聲矢量；Z(k)為k時(shí)刻測(cè)量矢量；H(k)為k時(shí)刻測(cè)量方程的系數(shù)矩陣；v(k)為測(cè)量噪聲矢量。 卡爾曼濾波主要由預(yù)測(cè)過(guò)程和更新過(guò)程組成。

1)預(yù)測(cè)過(guò)程

(7)

2)更新過(guò)程

(8)

(9)

(10)

標(biāo)準(zhǔn)卡爾曼濾波在系統(tǒng)模型已知，過(guò)程噪聲和量測(cè)噪聲已知時(shí)才能實(shí)現(xiàn)最優(yōu)估計(jì)濾波，而實(shí)際應(yīng)用中，噪聲往往不是固定的。假設(shè)行人慣性定位系統(tǒng)中系統(tǒng)噪聲具有一定的穩(wěn)定性，只需對(duì)量測(cè)噪聲進(jìn)行估計(jì)。簡(jiǎn)化Sage-Husa自適應(yīng)濾波算法通過(guò)量測(cè)噪聲估計(jì)器，可以實(shí)時(shí)修正量測(cè)噪聲的統(tǒng)計(jì)特性

(11)

式中dk=(1-b)/(1-bk+1)，b為遺忘因子。

2.2 抗差估計(jì)

簡(jiǎn)化Sage-Husa自適應(yīng)濾波一定程度上提高了系統(tǒng)的魯棒性，但不能剔除量測(cè)值中的野值。本文將粗差歸入隨機(jī)模型，通過(guò)調(diào)節(jié)觀測(cè)值方差的大小來(lái)抑制粗差的影響。

定義k時(shí)刻的新息矩陣ek和對(duì)應(yīng)的協(xié)方差矩陣Ck為

(12)

當(dāng)系統(tǒng)沒(méi)有故障時(shí)，新息是零均值白噪聲序列。當(dāng)新息實(shí)際統(tǒng)計(jì)特性與理論不相符時(shí)，則判斷過(guò)程出現(xiàn)異常。在故障檢測(cè)方面，新息χ2檢測(cè)算法是一種有效的方法。

假設(shè)檢驗(yàn)統(tǒng)計(jì)量

(13)

新息ek為均值為零的正態(tài)分布，所以，Tk服從自由度為m(m為觀測(cè)值的維數(shù))的χ2分布。把故障檢測(cè)條件轉(zhuǎn)變?yōu)榧僭O(shè)檢驗(yàn)條件，根據(jù)χ2分布的定義

{H0:Tk～χ2(m,0),H1:Tk～χ2(m,λ)

(14)

若Tk≤TD，則判斷觀測(cè)量無(wú)異常，否則觀測(cè)量存在異常值，則需要引入加權(quán)因子對(duì)新息協(xié)方差放大，降低野值對(duì)濾波的影響。利用Huber函數(shù)構(gòu)造加權(quán)因子

(15)

令α=diag[α1,α2,…,αn]，i=1,2…n,觀測(cè)值的等價(jià)協(xié)方差更新為Rk=Rk/α。

2.3 強(qiáng)跟蹤濾波

由于系統(tǒng)模型的簡(jiǎn)化、實(shí)際系統(tǒng)的參數(shù)發(fā)生變動(dòng)等，系統(tǒng)模型存在大量的不確定性。因此，在Suge-Husa自適應(yīng)濾波算法的基礎(chǔ)上，引入強(qiáng)跟蹤濾波算法。強(qiáng)跟蹤濾波算法利用衰減記憶濾波思想，將漸消因子λk引入狀態(tài)預(yù)測(cè)誤差協(xié)方差矩陣Pk,k-1，實(shí)時(shí)調(diào)整濾波增益矩陣，強(qiáng)迫濾波殘差序列保持相互正交，使得強(qiáng)跟蹤濾波器保持對(duì)系統(tǒng)實(shí)際狀態(tài)的跟蹤。

引入漸消因子λk

(16)

式中λk≥1為自適應(yīng)漸消因子，通過(guò)下式進(jìn)行確定

(17)

3 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

3.1 實(shí)驗(yàn)環(huán)境

實(shí)驗(yàn)采用荷蘭Xsens公司的Mti 系列MEMS IMU，主要由3個(gè)相互正交的加速度計(jì)、3個(gè)陀螺儀及1只三軸磁傳感器組成。Xsens慣導(dǎo)模塊安裝在鞋背上。

3.2 實(shí)驗(yàn)結(jié)果與分析

3.2.1 信號(hào)預(yù)處理分析

為重構(gòu)出測(cè)量白噪聲和有色白噪聲，采用小波分解法。以加速度X軸輸出值為例，采用“db6”小波基函數(shù)，“rigrsure”的閾值準(zhǔn)則和軟閾值方法，獲得原始信號(hào)和有色噪聲的對(duì)比，如圖3所示。

圖3 小波分解對(duì)比

3.2.2 姿態(tài)分析

為驗(yàn)證姿態(tài)融合濾波算法對(duì)姿態(tài)角精度的提高，本文利用上述實(shí)驗(yàn)設(shè)置獲取姿態(tài)角的對(duì)比，如圖4所示。

圖4 姿態(tài)對(duì)比

由圖4可見(jiàn)，初始姿態(tài)值中存在著測(cè)量噪聲以及累積誤差。經(jīng)過(guò)融合濾波，測(cè)量噪聲被抑制，輸出值的毛刺大大削弱，姿態(tài)值的精確度得到提高。

3.2.3 零速修正分析

如圖5(a)所示，根據(jù)加速度計(jì)、磁強(qiáng)計(jì)和陀螺儀的輸出，利用零速更新算法得到零速區(qū)間。根據(jù)零速區(qū)間對(duì)行人速度進(jìn)行修正，如圖5(b)所示。零速校正作為一種誤差補(bǔ)償算法，有效降低了速度的累積誤差。

圖5 零速區(qū)間與速度修正

3.2.4 軌跡對(duì)比分析

為驗(yàn)證加強(qiáng)型抗差自適應(yīng)濾波算法的性能，本文在不同定位濾波算法下進(jìn)行了10組行人定位實(shí)驗(yàn)，并將實(shí)驗(yàn)數(shù)據(jù)進(jìn)行平均處理，獲得如圖6所示的軌跡對(duì)比，位置誤差對(duì)比如圖7。

圖6 軌跡對(duì)比

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

相比于傳統(tǒng)Kalman濾波，AKF和SAKF算法下的運(yùn)動(dòng)軌跡更接近實(shí)際運(yùn)動(dòng)軌跡。自適應(yīng)Kalman濾波在利用測(cè)量數(shù)據(jù)進(jìn)行濾波的，同時(shí)對(duì)噪聲統(tǒng)計(jì)特性進(jìn)行估計(jì)和修正。

與SAKF相比，AKF下的軌跡含有不少尖峰，相對(duì)波動(dòng)較大。AKF不能剔除野值的干擾，SAKF利用新息檢測(cè)野值，構(gòu)造等價(jià)協(xié)方差矩陣，可以有效的抑制野值的干擾。同時(shí)引入強(qiáng)跟蹤濾波算法，進(jìn)一步抑制發(fā)散。

4 結(jié) 論

將提出的SAKF算法與KF和AKF算法進(jìn)行比較。實(shí)驗(yàn)結(jié)果表明：SAKF濾波算法適用于行人慣性定位系統(tǒng)，有效提高了行人定位精度，有較強(qiáng)的實(shí)用性。