基于多傳感器的行人航向推算算法研究

，

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

0 引言

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

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

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

1 相關(guān)研究

1.1 PDR定位原理

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

(1)

該算法包含4個核心步驟：步數(shù)檢測、步長估計、航向推算和位置計算。

1.2 步數(shù)檢測

本文使用智能手機的加速度計測量的3個方向的合加速度a作為檢測信號。

(2)

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

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

1.3 步長估計

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

(3)

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

1.4 方向角的卡爾曼濾波

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

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

(4)

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

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

1.5 方向角的硬磁場校正

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

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

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

2.1 平滑濾波

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

2.2 角速度卡爾曼濾波

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

(10)

2.3 硬磁場探測

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

(11)

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

2.4 轉(zhuǎn)向判斷

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

(12)

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

2.5 方向角卡爾曼濾波

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

(13)

2.6 方向角補償

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

(14)

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

3 實驗與結(jié)果分析

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

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

3.1 硬磁場干擾實驗

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

圖2 硬磁場干擾下角度變化情況

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

表2 硬磁場干擾下角度變化值

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

3.2 航位推算實驗

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

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

圖3 行人航位推算軌跡圖

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

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

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

4 結(jié)語

本文針對單一地利用陀螺儀進行航向推算時會出現(xiàn)陀螺儀誤差累計和估計的航向角在實際航向角兩側(cè)震蕩的問題，提出了一種基于多傳感器的航向推算系統(tǒng)。該系統(tǒng)利用KF來消除方向傳感器的信號干擾和陀螺儀的動態(tài)漂移，以濾波處理后的方向傳感器的角度值作為行人的航向角。針對室內(nèi)環(huán)境中可能存在硬磁場干擾的情況，引入陀螺儀來檢測和校正航向角，同時更新陀螺儀的絕對方向角來消除陀螺儀的累積誤差。該算法通過與HDE算法、AHDE算法比較，表現(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] 胡東旭. 基于互補濾波器的HDE算法在室內(nèi)行人航跡定位中的研究[J]. 呂梁學(xué)院學(xué)報, 2016, 6(2):22-27.

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

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

[7] 楊 智, 嚴 華. 基于SVR和粒子濾波的室內(nèi)導(dǎo)航方法[J]. 計算機測量與控制, 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手機的室內(nèi)定位技術(shù)研究與實現(xiàn)[D]. 武漢：華中師范大學(xué), 2014.

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