一種基于改進(jìn)Kalman濾波的多傳感器室內(nèi)定位方案

唐 嫻

(商丘學(xué)院計(jì)算機(jī)工程學(xué)院 河南 商丘 476000)

0 引 言

隨著人們對位置服務(wù)需求越來越大，無論基于GPS還是基于北斗的室外定位服務(wù)均已在現(xiàn)實(shí)生活中被廣泛應(yīng)用。然而，每當(dāng)人們進(jìn)入到室內(nèi)后就會(huì)發(fā)現(xiàn)定位精度逐漸降低，這是因?yàn)榛贕PS或北斗的定位服務(wù)在室內(nèi)受非視距和多徑效應(yīng)影響導(dǎo)致衛(wèi)星信號衰減較大。因此，室內(nèi)定位服務(wù)質(zhì)量問題已成為新一輪研究趨勢。移動(dòng)手機(jī)是現(xiàn)代社會(huì)發(fā)展最快的產(chǎn)業(yè)，其感知前端具有多傳感器融合、多媒體信息傳輸和存儲(chǔ)等優(yōu)點(diǎn)，可以直接利用移動(dòng)手機(jī)中內(nèi)置的慣性傳感器[1]進(jìn)行行人航位推算(Pedestrian dead reckoning,PDR)[2]，以此來實(shí)現(xiàn)較為精準(zhǔn)的室內(nèi)定位服務(wù)。

如果僅利用手機(jī)內(nèi)置慣性傳感器(主要包括加速度傳感器、陀螺儀、方向傳感器、磁力計(jì)等)進(jìn)行定位推算，將易造成累積誤差，會(huì)導(dǎo)致定位結(jié)果精度和可靠性偏低，所以需要將不同的定位技術(shù)進(jìn)行融合來得到更高的定位精度。Li等[3]提出了將慣性傳感器、Wi-Fi和地磁定位技術(shù)進(jìn)行融合，除了需要在行人航位推算中消除累積誤差，還需要考慮實(shí)際應(yīng)用中室內(nèi)人員的非步態(tài)干擾和手機(jī)設(shè)備的抖動(dòng)等，使得采集的多種傳感器信號易產(chǎn)生漂移和失真，即使PDR算法精度很高，也無法消除信號強(qiáng)度失真造成的定位誤差，所以必須對采集到的信號進(jìn)行濾波去噪處理。唐士凱等[4]利用單一的Kalman濾波對信號進(jìn)行處理，然而這種濾波方法對信號波動(dòng)明顯的采樣點(diǎn)效果不明顯，也會(huì)制約Kalman濾波的收斂速度。

針對多傳感器中信號易受室內(nèi)復(fù)雜環(huán)境、人員非步態(tài)干擾及自身漂移影響，導(dǎo)致的定位精度低、實(shí)時(shí)性差和魯棒性不強(qiáng)等問題，本文提出一種改進(jìn)的Kalman濾波多傳感器室內(nèi)定位方案。方案的主要思想包括以下幾點(diǎn)：(1) 由于GPS或北斗的定位服務(wù)在室內(nèi)定位上誤差較大，故本文利用智能手機(jī)中自帶的方向傳感器和加速度傳感器可實(shí)現(xiàn)較精準(zhǔn)的短距離室內(nèi)定位服務(wù)。(2) 采用PDA算法實(shí)現(xiàn)行人室內(nèi)定位，而該算法在計(jì)算行人位置前需先估算行人步伐長度和統(tǒng)計(jì)行人移動(dòng)的步數(shù)，由于室內(nèi)定位是短距離定位，采用常數(shù)模型即可滿足步長估算需要。(3) 步數(shù)統(tǒng)計(jì)通常采用三軸加速度矢量和的方法計(jì)算，但是由于直流分量和隨機(jī)噪聲使得加速度的測量結(jié)果不準(zhǔn)確(直流分量可以在系統(tǒng)初始化時(shí)對加速度傳感器進(jìn)行多次測量，然后再利用均值的方法預(yù)先加以矯正，所以加速度不準(zhǔn)確的原因是隨機(jī)噪聲)，因此采用Kalman濾波消除隨機(jī)噪聲，原因是Kalman濾波具有最優(yōu)化自回歸數(shù)據(jù)處理算法的特點(diǎn)，可以有效消除人員擾動(dòng)、信號多徑效應(yīng)等因素引起的信號噪聲。(4) 考慮到Kalman濾波在噪聲較大時(shí)會(huì)導(dǎo)致收斂速度變慢、數(shù)據(jù)穩(wěn)定性變差等問題，因此在Kalman濾波的基礎(chǔ)上結(jié)合均值濾波的方法進(jìn)行改進(jìn)，對干擾幅度較大、瞬時(shí)的脈沖信號具有較好的濾波消除效果。實(shí)驗(yàn)仿真說明，本文方案獲得了更高的定位精度和魯棒性更強(qiáng)的定位效果，對加速度處理、步數(shù)檢測、定位精度及系統(tǒng)魯棒性有一定的改善作用。

1 基于多傳感器的PDA算法

多傳感器融合的室內(nèi)定位方法主要采用移動(dòng)手機(jī)內(nèi)置的方向傳感器和加速度傳感器來實(shí)現(xiàn)。其中：方向傳感器為用戶提供以正北方向?yàn)榛鶞?zhǔn)的磁偏角，能直接作為PDA的基本角度數(shù)據(jù)；加速度傳感器能為用戶提供實(shí)時(shí)的加速度數(shù)據(jù)并顯示行人的運(yùn)動(dòng)狀態(tài)。將兩者信息經(jīng)過PDA算法即可得到行人運(yùn)動(dòng)軌跡，其原理如圖1所示。

圖1 行人航位推算算法原理圖

已知行人t0時(shí)刻的初始位置為(X0，Y0)，Ln表示tk時(shí)刻的步伐長度，θn表示tk時(shí)刻航向角，則tk時(shí)刻的位置為：

(1)

可見，直接影響室內(nèi)定位PDA精度的主要因素有初始位置坐標(biāo)、磁力計(jì)的航向數(shù)據(jù)、計(jì)步數(shù)據(jù)的準(zhǔn)確性，以及可靠的步長估計(jì)模型。

1.1 行人步伐長度估計(jì)

由于人體的長時(shí)間內(nèi)行為習(xí)慣及路面情況的多變性，造成了步長估計(jì)困難。通常行人步長估計(jì)模型主要有常數(shù)模型、線性模型和非線性模型三種。

(1) 常數(shù)模型：

L=A

(2)

通過預(yù)期對大量的已知長度的步伐進(jìn)行測量，比較建模后得出一個(gè)在短距離估計(jì)下的步長平均值A(chǔ)。

(2) 線性模型：

L=a+bf+cv+γ

(3)

式中：a、b、c為通過訓(xùn)練求解得出的線性回歸參數(shù)；f為當(dāng)前的周期頻率值；v為當(dāng)前步伐加速度的方差；γ為高斯噪聲[5]。但由于環(huán)境因素的干擾及智能手機(jī)系統(tǒng)存在一定的延誤，會(huì)造成步伐頻率和加速度統(tǒng)計(jì)量的誤差和不規(guī)律性。

(3) 非線性模型：

(4)

式中：amax和amin分別表示三軸加速度的最大值和最小值；K為模型參量[6]，針對不同行人的不同環(huán)境情況建立不同非線性模型，其模型參量K值也不同。

由于采用變步長會(huì)增加算法時(shí)間復(fù)雜度，且所給方案主要研究濾波算法對多傳感器融合的室內(nèi)定位精度的提高，是在短距離內(nèi)對行人信息的采集和定位，所以選用常數(shù)步長估計(jì)模型就能滿足本文方案的步伐長度估計(jì)要求。選取10名成年志愿者對其在正常狀態(tài)下多組步伐長度進(jìn)行測量，得出步伐長度均在0.5～0.7 m之間，因此本文方案步伐長度取值為L=0.6 m。

1.2 步數(shù)統(tǒng)計(jì)

所給方案引入X、Y、Z三軸加速度參數(shù)來研究行人步數(shù)特征，通過計(jì)算三軸加速度矢量和的方法來進(jìn)一步統(tǒng)計(jì)行人步數(shù)：

(5)

理論上，行人在靜止情況下，手握手機(jī)設(shè)備進(jìn)行數(shù)據(jù)獲取，得到的三軸加速度矢量和應(yīng)等于重力加速度g。但是在實(shí)際測量中，手機(jī)內(nèi)置的多種傳感器檢測精度有限[7]，且存在行人非步態(tài)動(dòng)作的干擾，在靜止?fàn)顟B(tài)測得的加速度數(shù)據(jù)也會(huì)出現(xiàn)輕微的抖動(dòng)現(xiàn)象，如圖2所示。經(jīng)多次測量得出加速度傳感器采集X、Y、Z三軸方向上的加速度，其受非步態(tài)干擾的波動(dòng)范圍約為0.3 m/s2。為降低以上干擾數(shù)據(jù)的影響，PDA算法首先應(yīng)對原始加速度數(shù)據(jù)進(jìn)行預(yù)處理。

圖2 靜止?fàn)顟B(tài)下三軸加速度以及矢量和曲線圖

1.3 基于閾值分級的步態(tài)檢測

為了進(jìn)一步統(tǒng)計(jì)行人步數(shù)，在運(yùn)動(dòng)過程中引入上述三軸加速度矢量和A，其值在重力加速度g附近上下波動(dòng)且具有周期性。因此，所給方案采用設(shè)置上下閾值來確定加速度矢量和的波峰波谷。假定選取的上閾值為(g+f)、下閾值為(g-f)，對閾值上下值進(jìn)行分級處理[7]：

(6)

式中：i為采樣點(diǎn)個(gè)數(shù)；Ai表示第i個(gè)樣本點(diǎn)經(jīng)過預(yù)處理濾波后的加速度矢量和，其中g(shù)=9.8 m/s2；f=Ai-g∈[Amin,Amax]，即為加速度矢量和減去重力加速度，考慮到運(yùn)動(dòng)過程中非步態(tài)及其他運(yùn)動(dòng)動(dòng)作的干擾，f取值為0.5[8]。

根據(jù)行人步伐特征，只有當(dāng)連續(xù)檢測到的狀態(tài)為“波峰狀態(tài)-過渡狀態(tài)-波谷狀態(tài)”時(shí)，才能將其判斷為一個(gè)有效的步數(shù)周期，以此來統(tǒng)計(jì)行人的步數(shù)。

2 非步態(tài)特征下Kalman濾波器數(shù)據(jù)預(yù)測

針對多傳感器在信息采集階段易受行人非步態(tài)干擾的問題，直接采集加速度傳感器數(shù)據(jù)而不做任何處理，即使在靜止?fàn)顟B(tài)下也會(huì)出現(xiàn)跳躍不穩(wěn)定或推擠現(xiàn)象，使得讀取的數(shù)據(jù)波動(dòng)明顯、誤差大且穩(wěn)定性差，因此在采集階段，利用Kalman濾波器對非步態(tài)特征下的數(shù)據(jù)進(jìn)行最優(yōu)預(yù)測，消除線性隨機(jī)微分系統(tǒng)和測量過程產(chǎn)生的高斯白噪聲。

2.1 Kalman濾波算法

Kalman濾波算法是一種線性最小方差估計(jì)，其主要過程包括預(yù)估與校正，以最小均方誤差為最佳估計(jì)準(zhǔn)則，建立信號和噪聲狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測值來更新對狀態(tài)變量的估計(jì)，求出當(dāng)前時(shí)刻的估計(jì)值[9]，具有最優(yōu)化自回歸數(shù)據(jù)處理算法的特點(diǎn)。其思想流程如圖3所示。

圖3 Kalman濾波流程

圖3中，E、F為系統(tǒng)參數(shù)；U(k)是k時(shí)刻對系統(tǒng)的控制量；Z(k)是k時(shí)刻的測量值；H是測量系統(tǒng)的參數(shù)；W(k)和V(k)分別表示過程和測量的噪聲；Q和R分是過程噪聲和觀測噪聲的協(xié)方差；U(k)為現(xiàn)在狀態(tài)的控制量。

2.2 加速度數(shù)據(jù)采集經(jīng)Kalman濾波器處理

由于加速度傳感器的測量在實(shí)際環(huán)境中存在人員擾動(dòng)及其他設(shè)備的干擾，使得加速度傳感器讀出的數(shù)值和真實(shí)加速度存在一定的偏差，其影響測量結(jié)果的因素主要包括直流分量和隨機(jī)噪聲，即：

A′=a+a′+n

(7)

式中：A′為測量加速度值；a為真實(shí)加速度值；a′為直流分量；n為隨機(jī)噪聲。在系統(tǒng)初始化時(shí)，對加速度傳感器進(jìn)行多次測量，利用均值的方法預(yù)先對直流分量加以矯正[10]。但由于隨機(jī)噪聲的不可測量性，無法采用多次實(shí)驗(yàn)進(jìn)行預(yù)先矯正，所以所給方案采用Kalman濾波方法消除隨機(jī)噪聲，其過程建立如圖4所示。

圖4 加速度信號濾波流程

根據(jù)以上條件得到加速度信號采集系統(tǒng)狀態(tài)模型如下：

(8)

(9)

同理可得Y軸和Z軸加速度信號采集系統(tǒng)狀態(tài)模型為：

(10)

(11)

(12)

(13)

在實(shí)驗(yàn)室環(huán)境下，采用華為榮耀8X智能Android手機(jī)作為測試機(jī)，已知初始位置信息，打開采樣App，其加速度傳感器采樣頻率為100 Hz，行人步伐頻率約為0.8 Hz。將手機(jī)平拿到距離地面1.2米左右高度按照預(yù)定路線勻速移動(dòng)，實(shí)際行走步數(shù)為30步，對采集到的X、Y、Z三軸加速度進(jìn)行Kalman濾波處理后，求取三軸加速度矢量和，再根據(jù)閾值分級的步態(tài)檢測方法，得出經(jīng)Kalman濾波處理后的行人步數(shù)值。其濾波結(jié)果如圖5所示。

(a) Kalman濾波前后加速度矢量和對比

(b) Kalman濾波后的閾值分級圖(步數(shù)統(tǒng)計(jì)為37步)圖5 Kalman濾波處理前后效果圖

未經(jīng)Kalman濾波處理前的加速度矢量和噪聲波動(dòng)大，經(jīng)閾值分級步態(tài)檢測方法得到步數(shù)統(tǒng)計(jì)值為48步，與實(shí)際行走步數(shù)值相差較大。

由圖5可知，將Kalman濾波應(yīng)用在多傳感信號處理上，可以有效濾除由于人員擾動(dòng)、信號多徑效應(yīng)等因素引起的信號噪聲，其步數(shù)統(tǒng)計(jì)值為37步，提高了步數(shù)檢測精度。但在加速度數(shù)據(jù)噪聲過大時(shí)，其收斂速度變慢，且數(shù)據(jù)穩(wěn)定性也較差。

2.3 改進(jìn)的Kalman濾波器應(yīng)用于實(shí)時(shí)信號采集

上述算法單獨(dú)使用Kalman濾波處理加速度，對于數(shù)據(jù)輸出波動(dòng)明顯的采集點(diǎn)，在環(huán)境干擾下產(chǎn)生的噪聲較大時(shí)會(huì)直接減慢Kalman濾波的收斂速度，使數(shù)據(jù)穩(wěn)定性變差[11]。所以，結(jié)合系統(tǒng)響應(yīng)速度及濾波效果，將濾波算法作改進(jìn)，結(jié)合均值濾波對干擾幅度較大、瞬時(shí)的脈沖信號具有較好的消除效果。采用多次濾波算法作為加速度數(shù)據(jù)的預(yù)處理，以均值濾波中連續(xù)N次采集數(shù)據(jù)為一組，進(jìn)行一次濾波，將濾波結(jié)果作為Kalman濾波的輸入量，進(jìn)行二次濾波，得到穩(wěn)定性、可靠性、實(shí)時(shí)性更好的濾波效果。

經(jīng)典均值濾波基本原理如下：

(14)

該算法針對連續(xù)采樣得到的數(shù)據(jù)，首先定義一個(gè)固定長度為M的觀察窗，每次將新采集到的樣本點(diǎn)數(shù)據(jù)放入隊(duì)首，并將隊(duì)尾的數(shù)據(jù)刪除，計(jì)算觀察窗中最新的M個(gè)數(shù)據(jù)的平均值作為濾波后的結(jié)果，其基本工作原理如圖6所示。

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

在多傳感器信號采集階段，考慮到濾波效果和算法對內(nèi)存的消耗會(huì)影響運(yùn)行時(shí)間，因此對加速度傳感器測得的X、Y、Z三軸加速度分別取M=10、M=15和M=20做均值濾波處理，以均值濾波處理后的數(shù)據(jù)作為一次測量值再利用Kalman濾波進(jìn)行遞歸運(yùn)算，求取出三軸加速度矢量和及行人步數(shù)值。其實(shí)驗(yàn)結(jié)果如圖7所示。

(a) M=10時(shí)，步數(shù)檢測為31，運(yùn)行時(shí)間為2.397 670 s

(b) M=15時(shí)，步數(shù)檢測為30，運(yùn)行時(shí)間為2.404 433 s

(c) M=20時(shí)，步數(shù)檢測為24，運(yùn)行時(shí)間為2.411 707 s圖7 不同M取值下的加速度矢量和及步數(shù)檢測值

對比圖7(a)-圖7(c)可知，當(dāng)M取10～20時(shí)，隨著M增大，濾波算法對加速度結(jié)果的優(yōu)化效果越明顯，但當(dāng)M取20時(shí)，會(huì)丟失掉有用的加速度數(shù)據(jù)。因此，結(jié)合濾波效果、步數(shù)檢測結(jié)果及運(yùn)行時(shí)間，當(dāng)M取15時(shí)，其濾波效果優(yōu)于單獨(dú)使用Kalman濾波，并且得到的步數(shù)檢測值為28步，與實(shí)際步數(shù)值更加接近。綜上，采用均值濾波+Kalman濾波對加速度數(shù)據(jù)處理，可以有效抑制信號干擾，提高相應(yīng)速度，降低平均運(yùn)行時(shí)間，提高步數(shù)檢測精度，并為實(shí)時(shí)定位提供更為穩(wěn)定、平滑的數(shù)據(jù)值。

根據(jù)上述改進(jìn)Kalman濾波處理，得到優(yōu)化后的多傳感器室內(nèi)定位流程[12]如圖8所示。

圖8 多傳感器室內(nèi)定位流程

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

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

實(shí)驗(yàn)區(qū)域選用10 m×36 m的辦公區(qū)域，其環(huán)境平面如圖9所示，包括辦公室和走廊，室內(nèi)環(huán)境復(fù)雜度較高。為加大實(shí)驗(yàn)數(shù)據(jù)，增加路徑二進(jìn)行數(shù)據(jù)分析，實(shí)際行走步數(shù)為30步，其中黑色線路代表路徑一，灰色線路代表路徑二。

圖9 實(shí)驗(yàn)環(huán)境平面圖

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

改進(jìn)Kalman濾波對路徑一和路徑二的加速度信號做處理，得到優(yōu)化后的傳感器數(shù)據(jù)，進(jìn)行行人航位推算，得到路徑實(shí)驗(yàn)結(jié)果對比圖，如圖10所示。

(a) 路徑一

(b) 路徑二圖10 不同濾波處理下的路徑結(jié)果對比圖

在行人非步態(tài)干擾和傳感器自身測量漂移下，改進(jìn)的Kalman濾波對加速度處理后得到的定位路徑相比于單一的Kalman濾波處理更加貼近真實(shí)路徑，且步數(shù)檢測也更加接近真實(shí)值。不同濾波處理下的位置誤差累積概率如圖11所示。

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

可以看出，在本次定位處理中，無濾波處理的定位精度較低，在短距離4 m內(nèi)的定位精度只有26.8%；在經(jīng)過Kalman濾波處理后，3 m內(nèi)的定位精度可以達(dá)到70.7%，但在2 m內(nèi)的定位精度只有31.7%；為此，以均值濾波處理后的數(shù)據(jù)作為一次測量值再利用Kalman濾波進(jìn)行遞歸運(yùn)算，其定位精度在2 m內(nèi)達(dá)到了89%，1 m內(nèi)也能達(dá)到68.2%。

3.3 真實(shí)實(shí)驗(yàn)結(jié)果及分析

Android手機(jī)端實(shí)時(shí)采集信號及定位結(jié)果如圖12所示。

(a) 實(shí)時(shí)信號結(jié)果圖 (b)實(shí)時(shí)定位結(jié)果圖圖12 Android端信號采集及定位結(jié)果示意圖

將路徑一和路徑二總的采樣點(diǎn)作濾波處理，得到多個(gè)待測點(diǎn)進(jìn)行平均定位誤差、誤差標(biāo)準(zhǔn)差計(jì)算，比較定位精度與實(shí)時(shí)性效果，結(jié)果如表1所示。

表1 幾種濾波算法定位精度與實(shí)時(shí)性的比較

對表1分析可得，改進(jìn)的Kalman濾波相對于無濾波處理和Kalman濾波，在平均定位精度方面分別提高了84.61%、69.14%；定位最大誤差也在減小，同時(shí)定位穩(wěn)定性也隨著濾波處理方法的改進(jìn)而增強(qiáng)。但由于改進(jìn)濾波處理方法的復(fù)雜性和多樣性，平均運(yùn)行時(shí)間會(huì)有所增加，因此會(huì)犧牲一部分運(yùn)算速度，獲得定位精度更高、魯棒性更強(qiáng)的定位效果。實(shí)驗(yàn)結(jié)果表明改進(jìn)的Kalman濾波算法對加速度處理、步數(shù)檢測、定位精度及系統(tǒng)魯棒性有一定的改善作用。

4 結(jié) 語

針對多傳感器中信號易受室內(nèi)復(fù)雜環(huán)境、人員非步態(tài)干擾及自身漂移的影響，本文給出了一種改進(jìn)的Kalman濾波多傳感器室內(nèi)定位方案。實(shí)驗(yàn)結(jié)果表明，本文濾波算法相較于Kalman濾波算法，在2 m內(nèi)的定位精度由31.7%提高到了89%，其平均定位精度提高了69.14%，同時(shí)運(yùn)行穩(wěn)定性也有所加強(qiáng)，但多傳感器信息融合定位在長時(shí)間的定位過程中，會(huì)出現(xiàn)累積誤差，所以未來可以針對累積誤差進(jìn)行多種定位方法的融合，進(jìn)一步提高定位精度和穩(wěn)定性。