，

(四川大學(xué) 電子信息學(xué)院，成都 610065)

0 引言

在現(xiàn)代生活中，人們對(duì)室內(nèi)定位的需求日益增加，例如在城市高樓群、醫(yī)院病房、購(gòu)物商場(chǎng)、火災(zāi)現(xiàn)場(chǎng)等都需要基于個(gè)人位置的服務(wù)。雖然GPS在室外環(huán)境能提供出色的定位服務(wù)，但是GPS信號(hào)不能穿過(guò)墻體，因而不適用于室內(nèi)環(huán)境。在各種室內(nèi)定位技術(shù)中，基于傳感器的定位技術(shù)是近年來(lái)研究的熱門(mén)領(lǐng)域。其突出的優(yōu)勢(shì)是抗干擾能力強(qiáng)，能夠提供實(shí)時(shí)、連續(xù)、精準(zhǔn)的位置信息。1996年Levi和Judd首次提出行人航位推算(pedestrian dead reckoning PDR)根據(jù)行人步態(tài)特征，利用加速度計(jì)信息估計(jì)步數(shù)和步長(zhǎng)，再利用陀螺儀采集數(shù)據(jù)計(jì)算航向，結(jié)合3個(gè)信息來(lái)推算行人的位置信息。2010年美國(guó)密歇根大學(xué)Johann和Lauro等人提出啟發(fā)式漂移消除(Heuristic Drift Elimination HDE)算法，利用行人走直線時(shí)航向角的變化量對(duì)陀螺儀輸出進(jìn)行校正。2012年中國(guó)海洋大學(xué)洪峰等人在針對(duì)在手智能手機(jī)轉(zhuǎn)向時(shí)，陀螺儀會(huì)出現(xiàn)轉(zhuǎn)向角不足的問(wèn)題，提出了PSP(Pedestrian tracking system based on one smartphone residing in trousers’ front Pocket)算法，解決智能手機(jī)在轉(zhuǎn)向的時(shí)候出現(xiàn)陀螺儀轉(zhuǎn)向角不足的問(wèn)題[1-3]。文獻(xiàn)[4]同樣針對(duì)陀螺儀轉(zhuǎn)向角不足問(wèn)題提出了基于互補(bǔ)濾波的增強(qiáng)式啟發(fā)式漂移消除算法(Advanced Heuristic Drift Elimination AHDE)算法，解決了智能手機(jī)在轉(zhuǎn)向時(shí)HDE停止校正航向角出現(xiàn)漂移的情況，能很好的實(shí)現(xiàn)轉(zhuǎn)向時(shí)航向角的校正。

以上的研究都是單一地利用陀螺儀來(lái)進(jìn)行航向估計(jì)，均基于HDE算法來(lái)消除陀螺儀的累積誤差，即利用反饋系統(tǒng)來(lái)消除陀螺儀的靜態(tài)漂移和動(dòng)態(tài)漂移。由于對(duì)采集的陀螺儀的數(shù)據(jù)沒(méi)有進(jìn)行預(yù)處理，導(dǎo)致了估計(jì)的航向在實(shí)際航向左右震蕩，也就造成了航向推算精度下降。所以單一地利用陀螺儀進(jìn)行航向估計(jì)會(huì)造成航向推算精度不足的問(wèn)題。

針對(duì)單一采用陀螺儀存在航向推算精度不足的問(wèn)題，本文提出一種基于多傳感器的行人航向推算算法。該算法組合了方向傳感器和陀螺儀的優(yōu)勢(shì)特性，能夠準(zhǔn)確地進(jìn)行航向角推算，大大提高了航向推算的精度。

1 相關(guān)研究

1.1 PDR定位原理

PDR算法是利用行人行走一步時(shí)加速度值是周期性的變化這個(gè)特點(diǎn)來(lái)進(jìn)行步數(shù)檢測(cè)，然后采用步長(zhǎng)模型估計(jì)步長(zhǎng)，再結(jié)合從角度傳感器獲得的方向信息推算出行人的位置信息，位置計(jì)算如公式(1)。式中xn、yn為當(dāng)前時(shí)刻的位置信息，xn-1、yn-1為前一時(shí)刻的位置信息，l為估計(jì)的步長(zhǎng)，θ為估計(jì)的航向角。

(1)

該算法包含4個(gè)核心步驟：步數(shù)檢測(cè)、步長(zhǎng)估計(jì)、航向推算和位置計(jì)算。

1.2 步數(shù)檢測(cè)

本文使用智能手機(jī)的加速度計(jì)測(cè)量的3個(gè)方向的合加速度a作為檢測(cè)信號(hào)。

(2)

采用一階線性高通濾波方程[5]濾除重力加速度的影響。為了平滑加速度波形，方便進(jìn)行步數(shù)檢測(cè)，參考文獻(xiàn)[6-7]再將高通濾波后的加速度信號(hào)進(jìn)行6階低通巴特沃斯濾波。

常用的步數(shù)檢測(cè)方法有：峰值檢測(cè)、過(guò)零檢測(cè)法和平區(qū)檢測(cè)法。為了提高步數(shù)檢測(cè)的準(zhǔn)確性，采用文獻(xiàn)[8]中改進(jìn)的峰值檢測(cè)算法。

1.3 步長(zhǎng)估計(jì)

現(xiàn)有的步長(zhǎng)估算算法模型主要采用線性步長(zhǎng)模型和非線性步長(zhǎng)模型。由于線性步長(zhǎng)模型中統(tǒng)計(jì)特征量與步長(zhǎng)的之間的線性關(guān)系還沒(méi)有得到充分的理論證明，因此可靠性不高。非線性步長(zhǎng)模型是利用行人行走過(guò)程中的加速度值采用統(tǒng)計(jì)分析的方法來(lái)建立數(shù)學(xué)模型。非線性的估計(jì)方法中有一些通用的模型[9-10]，這些通用的模型并不適用于同一個(gè)人使用不同步長(zhǎng)行走時(shí)的情況，而文獻(xiàn)[11]提出了改進(jìn)的步長(zhǎng)估計(jì)算法來(lái)適應(yīng)不同步長(zhǎng)行走的情況。為此，采用文獻(xiàn)[11]中提出的步長(zhǎng)估計(jì)模型(3)來(lái)進(jìn)行步長(zhǎng)估計(jì)。

(3)

式中，lstep為估計(jì)的步長(zhǎng)；amax、amin分別為一個(gè)步態(tài)周期中最大加速度和最小加速度；k為比例因子，利用訓(xùn)練數(shù)據(jù)經(jīng)最小二乘法擬合得到。

1.4 方向角的卡爾曼濾波

在行人運(yùn)動(dòng)中，方向傳感器的測(cè)量值會(huì)有噪聲干擾導(dǎo)致測(cè)量值跟實(shí)際值有偏差，所以需要消除這種噪聲的干擾。由于KF器能夠利用運(yùn)動(dòng)目標(biāo)的動(dòng)態(tài)信息,去掉噪聲的影響，得到一個(gè)關(guān)于目標(biāo)位置的準(zhǔn)確的估計(jì)值，而方向傳感器的噪聲符合高斯正態(tài)分布且與狀態(tài)量相互獨(dú)立。因此可以引入KF消除方向傳感器的噪聲干擾。

KF的數(shù)學(xué)模型為：

(4)

式中,xn為n時(shí)刻系統(tǒng)的狀態(tài)量；yn為n時(shí)刻系統(tǒng)的觀測(cè)值；un為系統(tǒng)的控制量；wn為輸入噪聲；vn為觀測(cè)噪聲；Φn|n-1為狀態(tài)轉(zhuǎn)移矩陣；Bn為系統(tǒng)的輸入系數(shù)；Hn為觀測(cè)矩陣。

KF分為預(yù)測(cè)和校正兩個(gè)過(guò)程。預(yù)測(cè)過(guò)程是利用上一時(shí)刻最優(yōu)狀態(tài)量來(lái)估計(jì)出當(dāng)前時(shí)刻狀態(tài)量預(yù)測(cè)值，同時(shí)更新?tīng)顟B(tài)量預(yù)測(cè)值和狀態(tài)量測(cè)量值的置信比重。而校正過(guò)程則是將狀態(tài)量預(yù)測(cè)值和狀態(tài)量測(cè)量值依據(jù)置信比重進(jìn)行融合，估計(jì)出最優(yōu)狀態(tài)量，即得到最終的結(jié)果。

1.5 方向角的硬磁場(chǎng)校正

針對(duì)單一采用方向傳感器會(huì)存在硬磁場(chǎng)干擾導(dǎo)致方向傳感器測(cè)量值跟實(shí)際值有很大偏差的問(wèn)題，同時(shí)陀螺儀不受硬磁場(chǎng)的影響，可以引入陀螺儀來(lái)校正這種硬磁場(chǎng)干擾。與此同時(shí)，陀螺儀也會(huì)存在誤差累積問(wèn)題。

2 基于多傳感器的航向推算系統(tǒng)

針對(duì)方向傳感器會(huì)受到硬磁場(chǎng)干擾的問(wèn)題，引入陀螺儀。同時(shí)通過(guò)方向傳感器來(lái)更新陀螺儀的絕對(duì)角度消除誤差累積。設(shè)計(jì)了一個(gè)基于多傳感器的航向推算系統(tǒng)(圖1)。該系統(tǒng)首先利用方向傳感器和陀螺儀分別采集方向角信號(hào)和角速度信號(hào)，然后將采集到的信號(hào)進(jìn)行濾波處理，并且利用濾波后的信號(hào)來(lái)進(jìn)行硬磁場(chǎng)檢測(cè)和轉(zhuǎn)向判斷。最后將濾波后的方向傳感器的角度值作為新的航向角，同時(shí)根據(jù)硬磁場(chǎng)檢測(cè)的結(jié)果和轉(zhuǎn)向判斷的結(jié)果來(lái)對(duì)航向角進(jìn)行補(bǔ)償。

2.1 平滑濾波

在行人行走的過(guò)程中，從抬腳到落腳的過(guò)程稱(chēng)為一個(gè)步態(tài)周期。在一個(gè)步態(tài)周期中，由于傳感器的不穩(wěn)定性和人步行時(shí)產(chǎn)生的隨機(jī)噪聲，可能會(huì)造成方向傳感器的值出現(xiàn)很大瞬時(shí)干擾。此時(shí)需要加入一個(gè)平滑濾波，對(duì)這種瞬時(shí)干擾進(jìn)行初步的濾波處理，即去除一個(gè)步態(tài)周期中方向序列中的最大值和最小值。這樣可以提高后續(xù)的對(duì)方向傳感器的值的KF的精度。

2.2 角速度卡爾曼濾波

在行走過(guò)程中，陀螺儀存在動(dòng)態(tài)漂移，會(huì)影響測(cè)量的準(zhǔn)確度。建立陀螺儀角速度ω的Kalman數(shù)學(xué)模型，進(jìn)行濾波處理，消弱陀螺儀的動(dòng)態(tài)漂移。ω的數(shù)學(xué)模型為：

(10)

2.3 硬磁場(chǎng)探測(cè)

硬磁場(chǎng)探測(cè)部分主要是用來(lái)檢測(cè)當(dāng)前步態(tài)周期中是否有硬磁場(chǎng)干擾。在當(dāng)前步態(tài)周期中，通過(guò)判斷陀螺儀測(cè)量的角度和方向傳感器測(cè)量的角度差是否超過(guò)閾值，來(lái)判斷是否有硬磁場(chǎng)干擾。判斷的公式為：

(11)

式中,DF為判斷硬磁場(chǎng)干擾的標(biāo)志位，1為有硬磁場(chǎng)干擾，0為無(wú)硬磁場(chǎng)干擾；φo為方向傳感器的角度；φg為陀螺儀的角度值；φd為判斷是否有硬磁場(chǎng)干擾的閾值。

2.4 轉(zhuǎn)向判斷

使用KF處理后陀螺儀數(shù)據(jù)來(lái)進(jìn)行轉(zhuǎn)向判斷。判斷的公式為：

(12)

式中,TF為判斷轉(zhuǎn)向的標(biāo)志位，1為轉(zhuǎn)向，0為沒(méi)有轉(zhuǎn)向；φn為當(dāng)前時(shí)刻陀螺儀的角度；φn-1為前一個(gè)時(shí)刻陀螺儀的角度值；φn-2為前兩個(gè)時(shí)刻陀螺儀的角度值；φt為判斷是否轉(zhuǎn)向的閾值。

2.5 方向角卡爾曼濾波

通過(guò)轉(zhuǎn)向環(huán)節(jié)的輸出可以判斷行人是在直行階段還是轉(zhuǎn)向階段。在直行時(shí)，對(duì)方向傳感器的測(cè)量值進(jìn)行KF能夠很好的消除噪聲干擾。建立方向傳感器角度θ的Kalman數(shù)學(xué)模型為：

(13)

2.6 方向角補(bǔ)償

通過(guò)硬磁場(chǎng)檢測(cè)環(huán)節(jié)和轉(zhuǎn)向判斷環(huán)節(jié)傳遞過(guò)來(lái)的值，對(duì)角度進(jìn)行相應(yīng)的補(bǔ)償。在直行階段，如果沒(méi)有硬磁場(chǎng)干擾，則對(duì)方向傳感器的測(cè)量值進(jìn)行KF，此時(shí)補(bǔ)償角度值φc為0；如果有硬磁場(chǎng)干擾，則φc為前一時(shí)刻的角度值φ來(lái)和當(dāng)前方向傳感器的測(cè)量值φo的差值。當(dāng)在轉(zhuǎn)向階段時(shí)，如果沒(méi)有硬磁場(chǎng)干擾，則使用方向傳感器的值做為角度值。此時(shí)φc為0；當(dāng)有硬磁場(chǎng)干擾的時(shí)候使用陀螺儀的角度值，作為方向角。補(bǔ)償角計(jì)算公式為：

(14)

最終，行人的航向角為φ=φo+φc，然后在更新陀螺儀的參考角度為φ，更新方向傳感器的前一時(shí)刻角度為φo。

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

為了驗(yàn)證算法的有效性，選擇Samsung Galaxy S Ⅲ智能手機(jī)作為測(cè)試設(shè)備，該手機(jī)內(nèi)嵌了加速度計(jì)、陀螺儀和方向傳感器。實(shí)驗(yàn)中，設(shè)置加速度計(jì)、陀螺儀和方向傳感器的采樣頻率為100 Hz。航向推算系統(tǒng)中的各個(gè)環(huán)節(jié)的參數(shù)設(shè)置如表1所示。參考文獻(xiàn)[12]設(shè)定轉(zhuǎn)向閾值φt的值為20°。通過(guò)多次實(shí)驗(yàn)結(jié)果比較，當(dāng)磁場(chǎng)判斷閾值φd小于15°時(shí)會(huì)出現(xiàn)硬磁場(chǎng)誤判的情況，即實(shí)際沒(méi)有硬磁場(chǎng)干擾時(shí)，系統(tǒng)會(huì)判定存在硬磁場(chǎng)干擾；當(dāng)φd大于15°的時(shí)候，系統(tǒng)會(huì)對(duì)硬磁場(chǎng)干擾判斷有延遲，即實(shí)際存在硬磁場(chǎng)干擾但是系統(tǒng)沒(méi)有檢測(cè)到?；谝陨蟽煞N情況都會(huì)導(dǎo)致硬磁場(chǎng)探測(cè)環(huán)節(jié)誤差增大，因此選擇φd為15°。

表1 參數(shù)設(shè)置

3.1 硬磁場(chǎng)干擾實(shí)驗(yàn)

為了驗(yàn)證算法中硬磁場(chǎng)檢測(cè)和方向角補(bǔ)償環(huán)節(jié)的有效性，實(shí)驗(yàn)選用一段直行的路線作為測(cè)試環(huán)境，路線長(zhǎng)度為18 m，中間設(shè)置了兩次硬磁場(chǎng)干擾。原始的方向角和校正后的方向角如圖2所示，具體的角度值變化如表2所示。

圖2 硬磁場(chǎng)干擾下角度變化情況

在圖2中行人沿著固定的方向角行走，實(shí)際的方向角變化軌跡應(yīng)該是一條平行于水平方向的直線，從圖2可知校正后的方向角軌跡更接近于直線。

表2 硬磁場(chǎng)干擾下角度變化值

由表2可知，在沒(méi)有受到硬磁場(chǎng)干擾的時(shí)候利用方向傳感器的值作為系統(tǒng)的航向角能夠很好的反應(yīng)實(shí)際航向角，同時(shí)可以校正陀螺儀的絕對(duì)角度來(lái)消除陀螺儀的累積誤差。而在第3步、第4步、第14步和第15步時(shí)方向傳感器受到硬磁場(chǎng)干擾，其測(cè)量值與實(shí)際值有很大的誤差，此時(shí)可以根據(jù)當(dāng)前的陀螺儀的角度值來(lái)校正方向傳感器的角度值。校正后的角度值和實(shí)際的方向值基本沒(méi)有偏差。

3.2 航位推算實(shí)驗(yàn)

為了驗(yàn)證本文提出的算法的總體性能，在室內(nèi)環(huán)境中搭建了一條45 m的封閉路線，分別采用HDE算法、AHDE算法和本文提出的算法進(jìn)行航向推算，再結(jié)合步數(shù)檢測(cè)值和步長(zhǎng)估計(jì)值進(jìn)行整體的航位推算。三種算法的軌跡圖如圖3所示，具體數(shù)據(jù)如表3中所示。

由圖3可知，在剛開(kāi)始的直行階段，由于HDE算法和AHDE算法是通過(guò)反饋的方式來(lái)消除陀螺儀的動(dòng)態(tài)偏移，它們的軌跡在實(shí)際航向的左右震蕩，并且剛開(kāi)始就偏離了實(shí)際方向，而本文提出的算法的軌跡則和實(shí)際軌跡基本重疊。而在多次轉(zhuǎn)向后，由于陀螺儀會(huì)出現(xiàn)轉(zhuǎn)向角不足的問(wèn)題，HDE算法的軌跡會(huì)偏離實(shí)際的軌跡，而AHDE算法能部分修正轉(zhuǎn)向角不足的問(wèn)題，本算法在轉(zhuǎn)向時(shí)同樣能表現(xiàn)出更好的性能。

圖3 行人航位推算軌跡圖

圖中可以看出HDE算法的最大誤差為2.2 m,AHDE算法的最大誤差為1.6 m,本算法的最大誤差為1 m。同時(shí)可以觀察到HDE算法和AHDE算法的平均誤差遠(yuǎn)大于本算法的平均誤差。

表3 行人航位推算效果比較

由表3可以看出，本算法在行人軌跡的總長(zhǎng)測(cè)量誤差上比AHDE算法提高了0.9 m，并且對(duì)于行人行進(jìn)過(guò)程中每一步的位置估計(jì)的精確度較AHDE算法有很大程度的提升。在表格中誤差和指的是行人軌跡中每一步實(shí)際位置和估算位置的誤差的總和，而平均誤差則是指平均單點(diǎn)誤差。如HDE算法中，單點(diǎn)誤差和為87.9 m，測(cè)量步數(shù)為55步，則平均誤差為1.6 m。實(shí)驗(yàn)結(jié)果表明，本文提出的算法比AHDE算法性能提升了46%。

4 結(jié)語(yǔ)

本文針對(duì)單一地利用陀螺儀進(jìn)行航向推算時(shí)會(huì)出現(xiàn)陀螺儀誤差累計(jì)和估計(jì)的航向角在實(shí)際航向角兩側(cè)震蕩的問(wèn)題，提出了一種基于多傳感器的航向推算系統(tǒng)。該系統(tǒng)利用KF來(lái)消除方向傳感器的信號(hào)干擾和陀螺儀的動(dòng)態(tài)漂移，以濾波處理后的方向傳感器的角度值作為行人的航向角。針對(duì)室內(nèi)環(huán)境中可能存在硬磁場(chǎng)干擾的情況，引入陀螺儀來(lái)檢測(cè)和校正航向角，同時(shí)更新陀螺儀的絕對(duì)方向角來(lái)消除陀螺儀的累積誤差。該算法通過(guò)與HDE算法、AHDE算法比較，表現(xiàn)出更好的航向推算準(zhǔn)確性。

參考文獻(xiàn)：

[1] Levi R W, Judd T. Dead reckoning navigational system using accelerometer to measure foot impacts[P]. US, US 5583776 A,1996.

[2] Ojeda J B L. Heuristic Drift Elimination for Personnel Tracking Systems[J]. Journal of Navigation, 2010, 63(4):591-606.

[3] Hong F, Chu H, Wang L, et al. Pocket Mattering: Indoor Pedestrian Tracking with Commercial Smartphone[A]. International Conference on Indoor Positioning and Indoor Navigation[C]. 2012.

[4] 胡東旭. 基于互補(bǔ)濾波器的HDE算法在室內(nèi)行人航跡定位中的研究[J]. 呂梁學(xué)院學(xué)報(bào), 2016, 6(2):22-27.

[5] 郗艷華, 張玉葉. 高通濾波器分析及其仿真[J]. 信息技術(shù), 2011(8):29-31.

[6] 王大偉, 賈榮叢, 王劃一. 基于Matlab的巴特沃斯濾波器設(shè)計(jì)[J]. 現(xiàn)代電子技術(shù), 2012, 35(21):71-72.

[7] 楊 智, 嚴(yán) 華. 基于SVR和粒子濾波的室內(nèi)導(dǎo)航方法[J]. 計(jì)算機(jī)測(cè)量與控制, 2016, 24(9):231-233.

[8] Kang W, Han Y. SmartPDR: smartphone-Based Pedestrian Dead Reckoning for Indoor Localization[J]. IEEE Sensors Journal, 2015, 15(5):2906-2916.

[9] Weinberg H. Using the ADXL202 in pedometer and personal navigation applications[J].2002.

[10] Kim J W, Han J J, Hwang D H, et al. A step, stride and heading determination for the pedestrian navigation system[J]. Positioning, 2004, 3(1&2):273-279.

[11] 徐 偉. 基于Android手機(jī)的室內(nèi)定位技術(shù)研究與實(shí)現(xiàn)[D]. 武漢：華中師范大學(xué), 2014.

[12] 趙 輝, 李 擎, 李 超. 基于主方向的行人自主定位航向修正算法[J]. 電子技術(shù)應(yīng)用, 2016, 42(11):108-111.