基于加強抗差自適應卡爾曼濾波的行人室內定位系統*

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

(江南大學 物聯網工程學院，江蘇 無錫 214122)

0 引 言

室內定位是指在室內環境中實現位置定位，在應急救援、物流和綜合性醫院等行業均展現出巨大的前景[1～4]。由于室內定位的方法容易受到外部環境干擾。微機電慣性測量單元(MEMS-IMU)具有結構簡單、體積小點，同時慣性測量完全自主，全天候，不受外界環境干擾，無信號丟失，適合于室內定位。

慣性導航定位技術以牛頓力學為基礎，通過測量載體在慣性參考系的加速度，對時間進行積分，長時間定位會存在累積誤差。文獻[5]利用Bagging模型深度學習慣導誤差之間的內在關系，對慣性導航的誤差進行抑制。文獻[6]提出慣性導航系統/超短基線/多普勒計程儀(INS/USBL/DVL)組合導航聯邦濾波方法，有效抑制慣導的累積誤差。針對特定的行人室內定位，步態檢測模式被提出[7]。

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

簡化Sage-Husa自適應算法是在某一噪聲變化不大情況下，在線估計和修正另一噪聲的統計統計特性，實用性較高[18]。

1 系統模型

1.1 行人定位模型

行人定位模型包含數據采集到位姿輸出的全過程，如圖1所示。通過MEMS-IMU獲取加速度、角速度以及磁場強度，并對其進行預濾波處理。將加速度計和磁強計在穩態下的姿態角，以及陀螺儀在動態下的姿態角優勢互補，進行濾波融合，獲得較為精確的空間三維姿態角。將加速度進行二次積分分別獲取行人的速度以及位置。采用零速閾值判斷對行人移動過程中的零速區間進行判斷，并進行零速更新，提高速度的精確性。數據濾波部分是將慣性導航算法得到的角速度誤差、加速度誤差、速度誤差以及位置誤差作為定位系統的狀態矢量，以更新后的速度以及位置為定位系統的量測矢量。通過濾波算法，對系統內部誤差進行估計以及修正，提高定位系統的精度。

圖1 行人定位原理

1.2 小波預處理

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

(1)

根據信號和噪聲在各尺度上的不同特點，選擇合適的閾值準則處理小波系數，最后重構信號，實現了信噪分離。

1.3 融合濾波

姿態矩陣計算直接影響姿態、速度以及位置的精確性。陀螺儀存在著靜態漂移現象，在計算姿態時會產生累積誤差；加速度計和磁強計的動態響應較差，但沒有累積誤差。因此在靜態和動態條件下分別采用加速度計，磁強計輸出的姿態信息和陀螺儀輸出的姿態信息，然后對兩者輸出進行優勢互補，提高姿態矩陣的精確度。融合濾波算法如圖2所示，將加速度計與磁強計計算得到的姿態角與最終姿態角的誤差進行PI控制，再采用互補濾波算法對姿態信息進行融合，提高姿態的精度。

圖2 融合濾波

(2)

1.4 零速更新算法

行人在移動時左右腳交替移動，每步移動可近似為一個加速和減速的過程。同一只腳隨著腳后跟離地而加速，腳后跟著地而減速。根據加速度和角速度的變化，形成“加速—減速—零速—加速”的循環模式。

行走周期內存在腳完全接觸地的情況，即零速區間。通過加速度和角速度變化來分析與識別行人的零速區間，并進行零速更新。本文在此基礎上，采用多條件閾值判別算法對零速區間進行判斷

(3)

(4)

2 加強抗差自適應卡爾曼濾波

2.1 自適應卡爾曼濾波

針對行人在二維坐標系中的定位要求，本文以行人的角速度、加速度、速度以及位置的誤差作為該系統的狀態量；以速度以及位置的計算值為定位量測量。卡爾曼濾波利用信號與噪聲的狀態模型，采用前一時刻的估計值和當前時刻的觀測值來更新對當前狀態變量的估計

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

(5)

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

(6)

式中x(k)為k時刻系統狀態量；Φ(k+1,k)為k時刻到k+1時刻的狀態轉移陣；w(k)為系統噪聲矢量；Z(k)為k時刻測量矢量；H(k)為k時刻測量方程的系數矩陣；v(k)為測量噪聲矢量。 卡爾曼濾波主要由預測過程和更新過程組成。

1)預測過程

(7)

2)更新過程

(8)

(9)

(10)

標準卡爾曼濾波在系統模型已知，過程噪聲和量測噪聲已知時才能實現最優估計濾波，而實際應用中，噪聲往往不是固定的。假設行人慣性定位系統中系統噪聲具有一定的穩定性，只需對量測噪聲進行估計。簡化Sage-Husa自適應濾波算法通過量測噪聲估計器，可以實時修正量測噪聲的統計特性

(11)

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

2.2 抗差估計

簡化Sage-Husa自適應濾波一定程度上提高了系統的魯棒性，但不能剔除量測值中的野值。本文將粗差歸入隨機模型，通過調節觀測值方差的大小來抑制粗差的影響。

定義k時刻的新息矩陣ek和對應的協方差矩陣Ck為

(12)

當系統沒有故障時，新息是零均值白噪聲序列。當新息實際統計特性與理論不相符時，則判斷過程出現異常。在故障檢測方面，新息χ2檢測算法是一種有效的方法。

假設檢驗統計量

(13)

新息ek為均值為零的正態分布，所以，Tk服從自由度為m(m為觀測值的維數)的χ2分布。把故障檢測條件轉變為假設檢驗條件，根據χ2分布的定義

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

(14)

若Tk≤TD，則判斷觀測量無異常，否則觀測量存在異常值，則需要引入加權因子對新息協方差放大，降低野值對濾波的影響。利用Huber函數構造加權因子

(15)

令α=diag[α1,α2,…,αn]，i=1,2…n,觀測值的等價協方差更新為Rk=Rk/α。

2.3 強跟蹤濾波

由于系統模型的簡化、實際系統的參數發生變動等，系統模型存在大量的不確定性。因此，在Suge-Husa自適應濾波算法的基礎上，引入強跟蹤濾波算法。強跟蹤濾波算法利用衰減記憶濾波思想，將漸消因子λk引入狀態預測誤差協方差矩陣Pk,k-1，實時調整濾波增益矩陣，強迫濾波殘差序列保持相互正交，使得強跟蹤濾波器保持對系統實際狀態的跟蹤。

引入漸消因子λk

(16)

式中λk≥1為自適應漸消因子，通過下式進行確定

(17)

3 實驗驗證與結果分析

3.1 實驗環境

實驗采用荷蘭Xsens公司的Mti 系列MEMS IMU，主要由3個相互正交的加速度計、3個陀螺儀及1只三軸磁傳感器組成。Xsens慣導模塊安裝在鞋背上。

3.2 實驗結果與分析

3.2.1 信號預處理分析

為重構出測量白噪聲和有色白噪聲，采用小波分解法。以加速度X軸輸出值為例，采用“db6”小波基函數，“rigrsure”的閾值準則和軟閾值方法，獲得原始信號和有色噪聲的對比，如圖3所示。

圖3 小波分解對比

3.2.2 姿態分析

為驗證姿態融合濾波算法對姿態角精度的提高，本文利用上述實驗設置獲取姿態角的對比，如圖4所示。

圖4 姿態對比

由圖4可見，初始姿態值中存在著測量噪聲以及累積誤差。經過融合濾波，測量噪聲被抑制，輸出值的毛刺大大削弱，姿態值的精確度得到提高。

3.2.3 零速修正分析

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

圖5 零速區間與速度修正

3.2.4 軌跡對比分析

為驗證加強型抗差自適應濾波算法的性能，本文在不同定位濾波算法下進行了10組行人定位實驗，并將實驗數據進行平均處理，獲得如圖6所示的軌跡對比，位置誤差對比如圖7。

圖6 軌跡對比

圖7 位置誤差對比

相比于傳統Kalman濾波，AKF和SAKF算法下的運動軌跡更接近實際運動軌跡。自適應Kalman濾波在利用測量數據進行濾波的，同時對噪聲統計特性進行估計和修正。

與SAKF相比，AKF下的軌跡含有不少尖峰，相對波動較大。AKF不能剔除野值的干擾，SAKF利用新息檢測野值，構造等價協方差矩陣，可以有效的抑制野值的干擾。同時引入強跟蹤濾波算法，進一步抑制發散。

4 結 論

將提出的SAKF算法與KF和AKF算法進行比較。實驗結果表明：SAKF濾波算法適用于行人慣性定位系統，有效提高了行人定位精度，有較強的實用性。