改進卡爾曼濾波在多傳感器室內定位中的應用

涂巧玲，鄧 英，余成波

（重慶理工大學 電氣與電子工程學院，重慶 400054）

近年來，隨著人們對位置服務需求的逐漸擴大，不管是基于GPS或是北斗的室外定位都已在實際生活中廣為所用。但很多時候人們從室外進入室內環境后，會發現定位精度逐漸降低，這是由于GPS和北斗定位在室內環境中易受非視距與多徑效應的影響，從而使其衛星信號衰減嚴重，因此室內定位逐步成為新的研究熱點［1－7］。手機作為目前發展最迅速的產業，因其感知前端具有多傳感器融合、多媒體信息傳輸和存儲等突出優點，而被作為新的室內定位研究技術要點，直接利用手機內置的慣性傳感器［2］，包括加速度傳感器、陀螺儀、方向傳感器以及磁力計等進行行人航位推算（PDR），實現室內定位。

單一使用手機內置慣性傳感器進行定位推算，很容易出現累積誤差導致定位結果精度以及可靠性偏低的問題，故可將不同定位技術進行融合，以獲得更高的定位精度。文獻［3］提出了PDR與WiFi進行融合定位；文獻［4］融入了地磁數據進行定位，但在PDR中，除了消除累積誤差以外，在實際應用中，由于室內環境人員非步態干擾以及手機設備抖動等影響，采集的多種傳感器信號易產生漂移和失真，即使行人航位推算算法精度再高也無法消除信號強度失真造成的定位誤差，所以必須對采集到的信號進行濾波去噪處理。文獻［5－6］中采用單一的卡爾曼濾波和粒子濾波對信號作處理，但對于信號波動明顯的采樣點，其濾波效果不明顯，同時會制約卡爾曼濾波的收斂速度。

針對多傳感器中信號易受室內復雜環境、人員非步態干擾以及自身漂移的影響，導致定位精度低、實時性差和魯棒性不強等問題，本文提出一種改進的卡爾曼濾波對多傳感器信號進行處理，用于推算用戶最優估計位置，提高實際定位精度和穩定性。

1 基于多傳感器的行人航位推算

利用智能手機內置的慣性傳感器實現多傳感器融合的室內定位方法，主要采用方向傳感器和加速度傳感器。其中，方向傳感器是一種合成傳感器，可為用戶提供一個相對于正北方向的磁偏角，直接作為行人航位推算的基本角度數據。加速度傳感器可為用戶提供實時的加速度數據，并顯示行人的運動狀態，將兩者信息經過行人航位推算算法（pedestrian dead reckoning，PDR）［8］，獲得行人運動軌跡，其原理如圖1所示。

圖1 PDR原理示意圖

由圖1所示，已知行人t0時刻初始位置為：（X0，Y0），Ln為tk時刻的步伐長度，θn為tk時刻航向角，則tk時刻的位置用式（1）計算：

可見，直接影響室內定位行人航位推算精度的主要因素為初始位置坐標、方向傳感器的航向數據、計步數據的準確性以及可靠的步長估計模型。

1．1 行人步伐長度估計

由于人體在長時間內的行為習慣以及路面情況的多變性，造成步伐長度的估計困難。目前對行人步伐長度的估計主要有3種模型：常數模型、線性模型和非線性模型。其模型計算公式如下：

常數模型：

通過預期對大量的已知長度的步伐進行測量，比較建模后，得出一個在短距離估計下的步伐長度平均值A。

線性模型：

式中：a、b、c均為通過訓練求解得出的線性回歸參數；f為當前的周期頻率值；v為當前步伐加速度的方差；γ為高斯噪聲［9］。但由于環境因素的干擾以及智能手機系統存在一定的延誤，會造成步伐頻率和加速度統計量的誤差和不規律性。

非線性模型：

式中：amax、amin為三軸加速度的最大值和最小值；K為模型參量［10］，但針對不同行人的不同環境情況建立不同非線性模型，其模型參量K值也不同。

由于采用變長會增加算法時間復雜度，且本文主要研究濾波算法對多傳感器融合的室內定位精度的提高，在短距離內對行人信息的采集和定位，選用常數步長模型就能滿足本文的步伐長度估計要求［9］。本文選取了10名成年志愿者對其在正常狀態下的多組步伐長度進行測量，得出步伐長度均在0．5～0．7 m，故本文步伐長度取值為L＝0．6 m。

1．2 步數統計

研究行人步數特征，引入了X、Y、Z三軸加速度參數，通過計算三軸加速度矢量和的方法，來進一步統計行人步數，如式（5）所示。

理論上，行人在靜止情況下，手握手機設備進行數據獲取，得到的X、Y、Z三軸加速度矢量和應等于重力加速度g，但是在實際測量中，手機內置的多種傳感器檢測精度有限［11］，且存在行人非步態動作的干擾，在靜止狀態測得的加速度數據也會出現輕微的抖動現象，如圖2所示，并求出X、Y、Z三軸加速度矢量和。經過多次測量，得出加速度傳感器采集X、Y、Z三軸方向上的加速度，受非步態干擾的波動范圍約為0．3 m／s2，為降低這些干擾數據的影響，行人航位推算算法應首先對原始加速度數據進行預處理。

圖2 靜止狀態下三軸加速度以及矢量和曲線圖

1．3 基于閾值分級的步態檢測方法

為了進一步統計行人步數，在運動過程中引入上述三軸加速度矢量和A，其值在重力加速度g附近上下波動，且具有周期性。因此，本文采用設置上下閾值的方法來確定加速度矢量和的波峰波谷。假定選取的上閾值為（g＋f），下閾值為（gf），對閾值上下值進行分級處理［11－12］。如式（6）所示：

式中：i為采樣點個數；Ai表示第i個樣本點經過預處理濾波后的加速度矢量和，其中g＝9．8 m／s2，f＝Ai－g∈［Amin，Amax］，即為加速度矢量和減去重力加速度［12］，考慮到運動過程中非步態以及其他運動動作的干擾，f取值為0．5［13］。

依據行人步伐特征，只有當連續檢測到的狀態為“波峰狀態－過度狀態－波谷狀態”時［13］，才能判斷為一個有效的步數周期，以此來統計行人的步數。

2 非步態特征下卡爾曼濾波器數據預測

針對多傳感器信息采集階段，易受行人非步態干擾，直接采集加速度傳感器數據而不做任何處理，即使在靜止狀態下也會出現跳躍不穩定或推擠的現象，使得讀取的數據波動明顯、誤差大且穩定性差，因此在采集階段，利用卡爾曼濾波器對非步態特征下的數據進行最優預測，消除線性隨機微分系統和測量過程產生的高斯白噪聲。

2．1 卡爾曼濾波算法

卡爾曼濾波算法（Kalman filter algorithm）是一種線性最小方差估計，其主要過程包括預估與校正，是以最小均方誤差為最佳估計準則，建立信號和噪聲狀態空間模型，利用前一時刻的估計值和當前時刻的觀測值來更新對狀態變量的估計，求出當前時刻的估計值［14］，具有最優化自回歸數據處理算法的特點。其流程如圖3所示。圖3中A、B為系統參數、U（k）是k時刻對系統的控制量、Z（k）是k時刻的測量值、H是測量系統的參數、W（k）和V（k）分別表示過程和測量的噪聲、Q和R分別為過程噪聲和觀測噪聲的協方差、U（k）為現在狀態的控制量、A′表示A的轉置矩陣。

2．2 加速度數據采集經卡爾曼濾波器處理

由于加速度傳感器測量在實際環境中存在人員擾動及其他設備的干擾，使得加速度傳感器讀出的數值和真實加速度存在一定的偏差，其影響測量結果的因素主要包括：直流分量和隨機噪聲，即：

其中，A為測量加速度值；a為真實加速度值；a′為直流分量值；n為隨機噪聲［15］。在系統初始化時，對加速度傳感器進行多次測量，利用均值的方法預先對直流分量加以矯正［15］。但是由于隨機噪聲的不可測量性，無法采用多次實驗進行預先矯正，所以本文采用卡爾曼濾波的方法消除隨機噪聲。其過程如圖4所示。

圖3 卡爾曼濾波流程框圖

圖4 加速度信號濾波流程框圖

根據以上條件得到加速度信號采集系統狀態模型如下［16］：

同理可得Y軸和Z軸加速度信號采集系統狀態模型為：

在實驗室環境下，采用華為榮耀8X智能Android手機作為測試機，已知初始位置信息，打開采樣App，其加速度傳感器采樣頻率為100 Hz，運動學指出行人正常行走的步伐頻率約在0．5～5 Hz，為便于測量，確定實際行人步伐頻率約為1 Hz。將手機平移到距地面1．2 m左右的高度按照預定路線做勻速移動，實際行走步數為30步，對采集到的X、Y、Z三軸加速度進行卡爾曼濾波處理后，求取三軸加速度矢量和，再根據閾值分級的步態檢測方法，得出經卡爾曼濾波處理后的行人步數值。其濾波結果如圖5所示。

未經卡爾曼濾波處理前的加速度矢量和噪聲波動大，經閾值分級步態檢測方法得到步數統計值為48步，與實際行走步數值相差較大。

由圖5可知：將卡爾曼濾波應用在多傳感器信號處理上，可以有效濾除由于人員擾動、信號多徑效應等因素引起的信號噪聲，其步數統計值為37步，提高了步數檢測精度；但在加速度數據噪聲過大時，其收斂速度變慢，且數據穩定性也較差。

2．3 改進的卡爾曼濾波器應用于實時信號采集

針對上述單獨使用卡爾曼濾波處理加速度數據時，對于數據輸出波動明顯的采集點，在環境干擾下產生的噪聲較大時會直接影響卡爾曼濾波的收斂速度變慢、數據穩定性變差［17］。所以，結合系統響應速度以及濾波效果，將濾波算法作改進，結合均值濾波對干擾幅度較大、瞬時的脈沖信號具有較好的消除效果。采用多次濾波算法作為加速度數據的預處理，以均值濾波中連續N次采集數據為一組，進行一次濾波，將濾波結果作為卡爾曼濾波的輸入量，進行二次濾波［18］，得到穩定性、可靠性以及實時性更好的濾波效果。

經典均值濾波基本原理為：

該算法針對連續采樣得到的數據，首先定義一個固定長度為M的觀察窗，將新采集到的樣本點數據放入隊首，并將隊尾的數據刪除，計算觀察窗中最新的M個數據的平均值作為濾波后的結果［20］。其基本原理如圖6所示。

圖6 均值濾波器基本原理框圖

在多傳感器信號采集階段，考慮到濾波效果和算法對內存的消耗會影響運行時間，故對加速度傳感器測得的X、Y、Z三軸加速度分別取M＝10、M＝15和M＝20作均值濾波處理［19］，以處理后的數據作為一次測量值，再利用卡爾曼濾波進行遞歸運算［21］，求取三軸加速度矢量和以及行人步數值。其實驗結果如圖7所示。

對比圖7（a）～（c）可知：當M取10～20時，隨著M值的增大，濾波算法對加速度結果的優化效果越明顯，但當M取20時，會丟失掉有用的加速度數據，故結合濾波效果、步數檢測結果以及運行時間，當M取15時，其濾波效果優于單獨使用卡爾曼濾波，并且得到的步數檢測值為28步，與實際步數值更加接近。綜上，采用均值濾波＋卡爾曼濾波對加速度數據處理，可以有效抑制信號干擾，提高相應速度，降低平均運行時間，提高步數檢測精度，并為實時定位提供更為穩定、平滑的數據值。

圖7 不同M取值下的加速度矢量和以及步數檢測值

根據以上改進卡爾曼濾波處理，得到優化后的多傳感器室內定位流程［22］，如圖8所示。

圖8 多傳感器室內定位流程框圖

3 實驗結果與分析

3．1 實驗環境布置

實驗驗證本文對濾波算法的改進，實驗區域選用10 m×36 m的辦公區域，其環境平面如圖9所示，包括辦公室和走廊，室內環境復雜度較高。為了加大實驗數據，增加路徑2進行數據分析，實際行走步數為30步，其中黑色線路代表路徑1，灰色線路代表路徑2。

圖9 實驗環境平面示意圖

3．2 仿真實驗結果及分析

改進卡爾曼濾波對路徑1和路徑2的加速度信號作處理，得到優化后的傳感器數據，進行行人航位推算，得到路徑實驗結果，如圖10所示。

圖10 不同濾波處理下的路徑結果

在行人非步態干擾和傳感器自身測量漂移下，改進的卡爾曼濾波對加速度處理后得到的定位路徑相比單一的卡爾曼濾波處理更加貼近真實路徑［23］，且步數檢測也更加接近真實值。

由圖11可知：在本次定位處理中，無濾波處理的定位精度較低，在短距離4 m內的定位精度只有26．8%；在經過卡爾曼濾波處理后，3 m內的定位精度可達70．7%，但在2 m內的定位精度只有31．7%。為此，以均值濾波處理后的數據作為一次測量值再利用卡爾曼濾波進行遞歸運算，其定位精度在2 m 內達到89%，在1 m 內達到68．2%。

3．3 真實實驗結果及分析

Android手機端實時采集信號及定位結果如圖12所示。

圖11 不同濾波處理下的位置誤差累積概率

圖12 Android端信號采集及定位結果示意圖

表1為對路徑1和路徑2總的采樣點作濾波處理，對多個待測點進行平均定位誤差、誤差標準差計算得到的定位精度與實時性效果。

表1 幾種濾波算法定位精度與實時性效果

對表1進行分析，改進爾曼濾波相對于無濾波處理和卡爾曼濾波，在平均定位精度方面分別提高84．61%、69．14%；定位最大誤差也在減小，同時定位穩定性也隨著濾波處理方法的改進而增強。但由于改進濾波處理方法的復雜性和多樣性，平均運行時間會有所增加，故會犧牲一部分運算速度，以獲得定位精度更高、魯棒性更強的定位效果，從而說明本文中所提改進卡爾曼濾波算法對加速度處理、步數檢測、定位精度以及系統魯棒性有改善作用。

4 結論

針對多傳感器中信號易受室內復雜環境、人員非步態干擾以及自身漂移的影響，提出一種改進卡爾曼濾波對加速度信號進行處理。實驗結果表明：所提出的濾波算法相較于卡爾曼濾波算法，在2 m內的定位精度由31．7%提高到89%，其平均定位精度提高69．14%，同時運行穩定性也有所加強，但多傳感器信息融合定位在長時間的定位過程中，會出現累積誤差，所以下一步的改進方向為針對累積誤差進行多種定位方法的融合，進一步提高定位精度和穩定性。