基于手機(jī)多傳感器數(shù)據(jù)融合的測(cè)步長(zhǎng)算法

陸文英，胡海榮

(杭州電子科技大學(xué) 計(jì)算機(jī)應(yīng)用研究所，浙江 杭州 310018)

在室內(nèi)環(huán)境中，全球定位系統(tǒng)(Global Position System，GPS)難以提供高精準(zhǔn)的位置信息，因此室內(nèi)定位技術(shù)具有重要的研究意義[1]。測(cè)量步長(zhǎng)是室內(nèi)定位研究中的一個(gè)基本環(huán)節(jié)，越來(lái)越多的學(xué)者開(kāi)始研究測(cè)步長(zhǎng)方法。文獻(xiàn)[2～4]用統(tǒng)計(jì)方法分析出步幅和身高、步頻的關(guān)系，估算出步長(zhǎng)。文獻(xiàn)[5]通過(guò)圖像處理的方法，分析出位置關(guān)系，從而得出步長(zhǎng)。文獻(xiàn)[6]通過(guò)預(yù)先設(shè)置步長(zhǎng)，然后根據(jù)無(wú)線網(wǎng)絡(luò)得到的位置信息在線調(diào)整步長(zhǎng)。上述方法存在成本高、計(jì)算復(fù)雜、難操作的問(wèn)題。文獻(xiàn)[7～8]將傳感器固定在腳上，通過(guò)閾值判斷腳的靜止和搖擺階段，對(duì)搖擺階段水平方向上的加速度數(shù)據(jù)直接二次積分求得步長(zhǎng)。文獻(xiàn)[9]將傳感器固定在腰上，通過(guò)建立人體行走模型，對(duì)加速度傳感器獲得的垂直方向上的數(shù)據(jù)進(jìn)行積分得到位移。然而，上述研究成果主要針對(duì)的是高精度的傳感器，需要額外的硬件設(shè)備。

目前，智能手機(jī)集成了多種傳感器，測(cè)量成本低、易操作，且不需要額外的硬件設(shè)備，因此越來(lái)越多的學(xué)者開(kāi)始將手機(jī)傳感器運(yùn)用于慣性測(cè)量領(lǐng)域。當(dāng)前很多研究都是采用手機(jī)傳感器數(shù)據(jù)進(jìn)行計(jì)步，而對(duì)其進(jìn)行步長(zhǎng)測(cè)量的研究還比較少，為此本文基于手機(jī)傳感器設(shè)計(jì)了一種多傳感器數(shù)據(jù)融合的測(cè)步長(zhǎng)算法，該算法通過(guò)建立人體行走模型，采取幾何運(yùn)算得到步長(zhǎng)。為了提高測(cè)量結(jié)果的準(zhǔn)確性，本文把對(duì)初始數(shù)據(jù)進(jìn)行了誤差修正，將其從手機(jī)坐標(biāo)轉(zhuǎn)換成慣性坐標(biāo)，然后利用互補(bǔ)濾波器融合加速度計(jì)和陀螺儀兩種傳感器數(shù)據(jù)，對(duì)因手機(jī)抖動(dòng)產(chǎn)生的軌跡偏移角進(jìn)行了角補(bǔ)償操作，最后進(jìn)行步長(zhǎng)計(jì)算。實(shí)驗(yàn)結(jié)果表明，該算法可以有效地提高步長(zhǎng)數(shù)據(jù)的測(cè)量準(zhǔn)確度，為進(jìn)一步研究室內(nèi)定位提供了低成本、高可靠性的方法。

1 人體行走模型

人體行走過(guò)程主要包括骨盆、軀干、手臂三部分運(yùn)動(dòng)組成，腿部軀干向前推進(jìn)，促使盆骨產(chǎn)生向前的位移，即邁出一步[3]。行走的過(guò)程中，骨盆會(huì)產(chǎn)生上下重心位置偏移。以人體行走單步為例，通過(guò)建立人體行走的模型，可以分析出步長(zhǎng)和腿部軀干長(zhǎng)度、骨盆重心下降位移之間的幾何運(yùn)算關(guān)系。具體的數(shù)學(xué)運(yùn)動(dòng)模型如圖1所示。

圖1 人體行走數(shù)學(xué)模型

圖1中的L代表腿長(zhǎng)，空心圓代表骨盆的位置，H代表下降的的高度，D代表步長(zhǎng)?？梢郧逦乜吹?，在完成一步時(shí)，骨盆有明顯的重心偏移。以腰部為骨盆的基準(zhǔn)位置計(jì)算，根據(jù)腰部的重心位移變化，就可以估測(cè)單步行進(jìn)的步長(zhǎng)。通過(guò)幾何運(yùn)算可得，如式(1)所示

(1)

2 測(cè)步長(zhǎng)算法的原理

根據(jù)上述行走模型可知，只要準(zhǔn)確計(jì)算出重心下降的位移就可通過(guò)式(1)得出步長(zhǎng)D。在行走過(guò)程中，可以通過(guò)手機(jī)加速度傳感器得到重心下降過(guò)程中的加速度，然后對(duì)加速度進(jìn)行二次積分便可得重心下降的高度，但是存在很大的誤差。造成誤差的原因有3個(gè)方面[10-11]：第一，手機(jī)傳感器計(jì)算的數(shù)值是相對(duì)手機(jī)坐標(biāo)系而言，不處于真實(shí)的世界坐標(biāo)系中；第二，手機(jī)傳感器自身會(huì)有一些固有的線性漂移誤差；第三，在行走過(guò)程中，放在兜里的手機(jī)會(huì)進(jìn)行抖動(dòng)，會(huì)對(duì)傳感器計(jì)算的數(shù)據(jù)產(chǎn)生干擾。為了最大限度地消除這些誤差，本文提出了一種基于手機(jī)多傳感器融合的測(cè)步長(zhǎng)算法。涉及的主要內(nèi)容包括以下幾方面。

2.1 步態(tài)周期劃分

在走路過(guò)程中，落腳的瞬間在垂直方向會(huì)產(chǎn)生一個(gè)加速度極小值，之后地面產(chǎn)生的反作用力會(huì)使得垂直加速度不斷增大，而當(dāng)全腳掌著地(站立)的時(shí)候會(huì)達(dá)到最大值，隨著繼續(xù)往前邁步，腳后跟離開(kāi)地面(抬腳)，身體重心下降，垂直加速度逐漸減小，當(dāng)腳尖蹬地的時(shí)候由于地面反作用力會(huì)產(chǎn)生一個(gè)小波峰，如圖2所示。

圖2 手機(jī)傳感器y軸加速度

通過(guò)觀察發(fā)現(xiàn)，手機(jī)傳感器在垂直方向上的加速度會(huì)呈現(xiàn)明顯的周期變化, 這與走路時(shí)人體的運(yùn)動(dòng)狀態(tài)是一致的。走路時(shí)，會(huì)有落腳、站立、抬腳3個(gè)比較明顯的動(dòng)作，這和圖2中標(biāo)注的3個(gè)特殊點(diǎn)一一對(duì)應(yīng)。在測(cè)量步長(zhǎng)之前，首先要計(jì)算得到每一步的抬腳點(diǎn)和站立點(diǎn)，為后續(xù)計(jì)算重心位移提供積分區(qū)間。此外可以將身體重心的位置變化抽象為簡(jiǎn)諧運(yùn)動(dòng)，落腳點(diǎn)和站立點(diǎn)的實(shí)際速度為零，可用于后續(xù)的速度誤差糾正。

2.2 慣性坐標(biāo)系轉(zhuǎn)換

手機(jī)傳感器讀出的數(shù)據(jù)是相對(duì)于手機(jī)坐標(biāo)系而言的[12]，如圖3(a)所示。以屏幕正中心為原點(diǎn)，窄側(cè)一邊為x軸的正方向，長(zhǎng)邊為y軸，垂直于手機(jī)內(nèi)側(cè)向外的方向是z軸。圖3(b)為可描述客觀世界物體運(yùn)動(dòng)的慣性坐標(biāo)系，可以看出兩者之間存在很大的差異。為得到準(zhǔn)確的重心下降加速度，需要將傳感器加速度數(shù)據(jù)轉(zhuǎn)換成慣性坐標(biāo)系下的數(shù)值。

圖3 坐標(biāo)系示意圖

(2)

(3)

2.3 多傳感器數(shù)據(jù)融合角補(bǔ)償計(jì)算

在步行過(guò)程中，放在兜里的手機(jī)會(huì)反復(fù)進(jìn)行抖動(dòng)，使得傳感器數(shù)據(jù)偏離真實(shí)數(shù)據(jù)，產(chǎn)生一定的累積誤差，造成測(cè)量數(shù)據(jù)不準(zhǔn)確。為了消除手機(jī)抖動(dòng)產(chǎn)生的數(shù)據(jù)偏差，需要計(jì)算手機(jī)抖動(dòng)時(shí)偏離正常軌跡的偏移角。陀螺儀存在低頻漂移現(xiàn)象，在短時(shí)間內(nèi)可以提供較為準(zhǔn)確的值，但積分后受低頻運(yùn)動(dòng)干擾會(huì)產(chǎn)生較大誤差；加速度計(jì)主要受高頻運(yùn)動(dòng)干擾，測(cè)量值累積誤差小，但初始值不準(zhǔn)確[13]。可以利用陀螺儀和加速度計(jì)的優(yōu)點(diǎn)，把兩者結(jié)合在一起，設(shè)計(jì)出互補(bǔ)型濾波器實(shí)現(xiàn)這兩種傳感器數(shù)據(jù)的融合，從而計(jì)算出較為準(zhǔn)確的瞬時(shí)偏移角。這種互補(bǔ)濾波器[14]可以同時(shí)濾去高頻和低頻的干擾，具體實(shí)現(xiàn)模型如圖4所示。

圖4 互補(bǔ)濾波器模型

從頻域上分析，假設(shè)θ是軌跡偏移的真實(shí)角，加速度計(jì)受到低頻信號(hào)k1的干擾，測(cè)量角變?yōu)棣?=θ+k1，為了消除低頻噪聲信號(hào)k1的干擾，系統(tǒng)經(jīng)低通濾波器G1除噪；陀螺儀測(cè)量時(shí)引入了高頻噪聲信號(hào)k2，測(cè)量角為θ2，經(jīng)過(guò)高通濾波器G2可除去高頻噪聲k2的影響。低通濾波器和高通濾波器的傳遞函數(shù)分別設(shè)計(jì)如式(4)和式(5)所示

(4)

(5)

互補(bǔ)濾波器的傳遞函數(shù)需要滿G1(s)+G2(s)=1，加速度計(jì)和陀螺儀經(jīng)過(guò)系統(tǒng)濾波后，數(shù)據(jù)融合可算出真實(shí)偏移角的估計(jì)值。從系統(tǒng)傳遞函數(shù)可知，適當(dāng)調(diào)整權(quán)重因子K，可控制系統(tǒng)濾波器的截止頻率，從而得出比較準(zhǔn)確的偏移角θ′。

(6)

由θ2(n)=θ′(n-1)+ω(n)dt,可得系統(tǒng)時(shí)域表達(dá)式如式(7)所示。在測(cè)量步長(zhǎng)時(shí)，對(duì)應(yīng)手機(jī)傳感器測(cè)量的數(shù)據(jù)來(lái)看，在選定系統(tǒng)采樣頻率的時(shí)候，只要調(diào)整確定時(shí)間常數(shù)ξ，就可實(shí)現(xiàn)加速度計(jì)和陀螺儀兩者的數(shù)據(jù)融合，從而得計(jì)算出較為準(zhǔn)確的偏移角θ′。

(7)

2.4 步長(zhǎng)位移計(jì)算

根據(jù)牛頓第二定律，對(duì)傳感器的加速度數(shù)值進(jìn)行二次積分可得位移值[15]?；谏鲜龅臏?zhǔn)備工作，對(duì)于手機(jī)放在兜里的場(chǎng)景，將手機(jī)傳感器采集的y軸加速度數(shù)據(jù)經(jīng)過(guò)慣性坐標(biāo)轉(zhuǎn)換后，進(jìn)行角補(bǔ)償操作，最后對(duì)得到的加速度數(shù)據(jù)二次積分就可得到每一步較為準(zhǔn)確的重心下降高度H。重心下降速度計(jì)算如式(8)所示。

(8)

其中，n代表這一步有n個(gè)采樣點(diǎn)，a(i)是第i個(gè)采樣點(diǎn)在垂直方向上的加速度，Δt是采樣點(diǎn)的時(shí)間間隔，v(0)表示每一步初始的速度，v(n)是每一步結(jié)束時(shí)的速度。由此可得，重心下降高度H。傳感器在計(jì)算重心下降位移時(shí)，人體步行呈上下反復(fù)運(yùn)動(dòng)，重心最低和最高位置的速度為零，因此采用用零速率更新算法[16]來(lái)避免加速度積分帶來(lái)的累計(jì)誤差。重心下降位移的具體計(jì)算如式(9)所示，把計(jì)算的重心下降高度代入式(1)，就可以計(jì)算出步長(zhǎng)位移D。

(9)

2.5 誤差校正

誤差校正分為兩個(gè)方面：水平校正和消除重力影響。水平校正是為了消除傳感器零點(diǎn)漂移產(chǎn)生的影響，通常的做法是用傳感器測(cè)量數(shù)值減去靜止時(shí)相應(yīng)的測(cè)量值[17]。消除重力影響是用測(cè)量值減去重力加速度所產(chǎn)生的加速度分量。

3 算法實(shí)現(xiàn)和實(shí)驗(yàn)結(jié)果分析

3.1 算法實(shí)現(xiàn)

為了整合算法上述實(shí)現(xiàn)步驟，基于上述算法原理，本文在手機(jī)小米4上設(shè)計(jì)了測(cè)試原型系統(tǒng)。算法的實(shí)現(xiàn)架構(gòu)如圖5所示。

3.2 實(shí)驗(yàn)數(shù)據(jù)分析

為了測(cè)試本文提出的測(cè)步長(zhǎng)算法，采樣頻率取50 Hz，時(shí)間常數(shù)取0.7，真實(shí)距離的參考值為30 m。分別做6組測(cè)步長(zhǎng)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表1所示。

表1 實(shí)驗(yàn)結(jié)果數(shù)據(jù)表

利用本文算法分別計(jì)算出行進(jìn)中的每一步距離，通過(guò)步長(zhǎng)累加就可以得出行進(jìn)的總位移。從表1實(shí)驗(yàn)結(jié)果數(shù)據(jù)來(lái)看，該算法計(jì)算的實(shí)測(cè)距離數(shù)據(jù)誤差基本上控制在5%之內(nèi)，每一步的步長(zhǎng)數(shù)據(jù)呈現(xiàn)平穩(wěn)變化。

為了測(cè)試傳感器數(shù)據(jù)融合進(jìn)行角補(bǔ)償對(duì)測(cè)量步長(zhǎng)數(shù)據(jù)的有效性，接下來(lái)的實(shí)驗(yàn)中，將傳感器采樣頻率統(tǒng)一設(shè)為50 Hz，分別采取無(wú)角補(bǔ)償算法和有角補(bǔ)償算法測(cè)量步長(zhǎng)數(shù)據(jù)。設(shè)置參考距離為10 m，20 m，30 m，40 m，50 m，60 m，70 m，80 m，90 m，100 m，分別反復(fù)測(cè)量10次，取測(cè)量值的平均值和實(shí)際參考距離做誤差分析，實(shí)驗(yàn)結(jié)果如圖7所示。

圖6 誤差比較

從圖6中可以看出，帶有角補(bǔ)償?shù)乃惴ǖ臏y(cè)量數(shù)值誤差基本控制在5%之內(nèi)，而且測(cè)量誤差走勢(shì)較為平穩(wěn)；沒(méi)有進(jìn)行角補(bǔ)償?shù)乃惴ǖ臏y(cè)量值的誤差范圍較大，而且有時(shí)會(huì)出現(xiàn)多個(gè)異常點(diǎn)，呈現(xiàn)不規(guī)律的誤差走向。實(shí)現(xiàn)結(jié)果表明，多傳感器數(shù)據(jù)融合采取的角補(bǔ)償計(jì)算對(duì)提高測(cè)量步長(zhǎng)的精準(zhǔn)度具有一定的貢獻(xiàn)。

4 結(jié)束語(yǔ)

本文利用手機(jī)集成的多傳感器設(shè)計(jì)了測(cè)步長(zhǎng)算法，易操作且準(zhǔn)確度高。通過(guò)搭建行走幾何模型、角補(bǔ)償處理、誤差校正等方法消除了手機(jī)測(cè)量步長(zhǎng)數(shù)據(jù)時(shí)存在的一定誤差。實(shí)驗(yàn)結(jié)果表明，該算法測(cè)量的步長(zhǎng)與真實(shí)數(shù)據(jù)的誤差在5%之內(nèi)，可以滿足室內(nèi)定位的需求。