基于角度補(bǔ)償?shù)氖謾C(jī)多傳感器數(shù)據(jù)融合測距算法*

陳 帥，王國英，莫路鋒

(浙江農(nóng)林大學(xué)信息工程學(xué)院，浙江臨安311300)

基于角度補(bǔ)償?shù)氖謾C(jī)多傳感器數(shù)據(jù)融合測距算法*

陳 帥，王國英，莫路鋒*

(浙江農(nóng)林大學(xué)信息工程學(xué)院，浙江臨安311300)

在慣性測量領(lǐng)域，單純利用加速度二次積分的方法并不能準(zhǔn)確感知目標(biāo)對象移動(dòng)的距離。加速度傳感器在感知呈線性運(yùn)動(dòng)的目標(biāo)對象時(shí)較為準(zhǔn)確和實(shí)用，但在三維空間運(yùn)動(dòng)時(shí)它的坐標(biāo)軸會(huì)隨物體發(fā)生方向的改變而不斷漂移。為解決該問題，提出了一種基于角度補(bǔ)償?shù)氖謾C(jī)多傳感器數(shù)據(jù)融合測距算法(ADC-R)，使用加速度傳感器測量物體運(yùn)動(dòng)的加速度，作為計(jì)算位移的原始數(shù)據(jù);采用手機(jī)陀螺儀傳感器測量運(yùn)動(dòng)物體的角速度，并以旋轉(zhuǎn)矢量傳感器輸出的數(shù)據(jù)作為參數(shù)把手機(jī)動(dòng)態(tài)坐標(biāo)系下測得的加速度值空間坐標(biāo)轉(zhuǎn)換到靜態(tài)的參考坐標(biāo)系下，然后進(jìn)行數(shù)據(jù)融合完成角度補(bǔ)償計(jì)算;最后根據(jù)物理學(xué)加速度和位移的關(guān)系運(yùn)用數(shù)學(xué)積分方法和進(jìn)一步修正誤差的技術(shù)得到最終移動(dòng)的距離。實(shí)驗(yàn)結(jié)果表明此方法在近距離測距方面精度較高，優(yōu)于加速度積分算法和加速度與陀螺儀融合算法。

手機(jī)傳感器;數(shù)據(jù)融合;空間坐標(biāo)轉(zhuǎn)換;距離測量。

在慣性測量領(lǐng)域，單純利用加速度二次積分的方法并不能準(zhǔn)確感知目標(biāo)對象移動(dòng)的距離［1－2］。這是因?yàn)榧铀俣葌鞲衅髦荒懿蹲揭粋€(gè)動(dòng)作而不能實(shí)現(xiàn)軌跡測量，所以單純利用加速度直接重積分獲得的位移量往往與真實(shí)結(jié)果的誤差很大。常見的傳感器數(shù)據(jù)誤差來源主要有人為誤差、系統(tǒng)誤差以及隨機(jī)誤差，噪聲，漂移，零點(diǎn)偏移，時(shí)間延遲和數(shù)據(jù)丟棄以及積分誤差等等因素。以上隨機(jī)影響因素的出現(xiàn)都有可能導(dǎo)致誤差偏離真實(shí)的結(jié)果。因此，為了準(zhǔn)確估算目標(biāo)對象的移動(dòng)距離，測距系統(tǒng)的算法設(shè)計(jì)以及誤差修正技術(shù)至關(guān)重要。

目前對慣性傳感器測量距離的方法已經(jīng)有了一些相關(guān)研究。羅印等［3］將采集的加速度數(shù)據(jù)通過積分算法轉(zhuǎn)換為位移量，此方法忽略了三維空間中手機(jī)設(shè)備坐標(biāo)系的改變，不能完全反映出物體的運(yùn)動(dòng)狀態(tài)。周英杰等［4］針對加速度積分出現(xiàn)的趨勢項(xiàng)干擾積分誤差偏大，提出一種基于頻域積分的“低頻衰減積分算法”，但是其控制參數(shù)選擇較為復(fù)雜，并且該方法在絕對誤差控制上收效甚微。張立峰等［5］采用的加權(quán)平均法是一種簡單、直觀的融合方法，它將加速度數(shù)據(jù)進(jìn)行加權(quán)平均的結(jié)果作為融合值，雖然適用于動(dòng)態(tài)環(huán)境但是運(yùn)算精度不高。

雖然單一的加速度傳感器不能準(zhǔn)確測量目標(biāo)對象移動(dòng)的距離，但是通過多傳感器數(shù)據(jù)融合的方法能夠有效補(bǔ)償傳感器噪聲和積分等導(dǎo)致的誤差。本文提出基于角度補(bǔ)償?shù)氖謾C(jī)多傳感器數(shù)據(jù)融合測距算法ADC-R(Angle Data Compensated Ranging)。使用三軸加速度傳感器測量物體運(yùn)動(dòng)的加速度，作為計(jì)算位移的原始數(shù)據(jù);采用智能手機(jī)三軸陀螺儀傳感器測量物體運(yùn)動(dòng)的角速度，以旋轉(zhuǎn)矢量傳感器輸出的數(shù)據(jù)作為參數(shù)把手機(jī)動(dòng)態(tài)坐標(biāo)系下測得的加速度值空間坐標(biāo)轉(zhuǎn)換到靜態(tài)的參考坐標(biāo)系下，然后進(jìn)行數(shù)據(jù)融合完成角度補(bǔ)償計(jì)算;最后根據(jù)物理學(xué)加速度和位移的關(guān)系運(yùn)用數(shù)學(xué)積分方法和進(jìn)一步修正誤差的技術(shù)得到最終移動(dòng)的距離。本文的組織結(jié)構(gòu)如下:第1節(jié)詳細(xì)敘述基于角度補(bǔ)償?shù)氖謾C(jī)多傳感器數(shù)據(jù)融合測距的算法;第2節(jié)介紹了實(shí)驗(yàn)系統(tǒng)和實(shí)驗(yàn)方案;第3節(jié)描述了實(shí)驗(yàn)結(jié)果，并對結(jié)果進(jìn)行了分析與討論;最后第4節(jié)對全文進(jìn)行了總結(jié)。

1 算法思想

在Android傳感器編程中，根據(jù)三軸加速度傳感器、三軸陀螺儀傳感器、旋轉(zhuǎn)矢量傳感器的類型調(diào)用傳感器API接口，控制其工作方式，分別獲得加速度、角速度、旋轉(zhuǎn)角度等數(shù)據(jù)。然而這里的傳感器數(shù)據(jù)是以手機(jī)坐標(biāo)系為參考的，明顯不能反映出慣性坐標(biāo)系下的手機(jī)運(yùn)動(dòng)狀態(tài)，本文需要利用旋轉(zhuǎn)矢量傳感器輸出的旋轉(zhuǎn)軸和旋轉(zhuǎn)角度數(shù)據(jù)通過四元數(shù)法將手機(jī)坐標(biāo)系下的傳感器數(shù)值轉(zhuǎn)換到慣性坐標(biāo)系。隨后采用多傳感器數(shù)據(jù)融合的方法，陀螺儀傳感器獲得的角速度與坐標(biāo)轉(zhuǎn)換后的加速度進(jìn)行互補(bǔ)濾波數(shù)據(jù)融合完成角度計(jì)算。最后根據(jù)物理學(xué)加速度和位移的關(guān)系運(yùn)用數(shù)學(xué)積分的方法和進(jìn)一步修正誤差的技術(shù)估算得到最終的測距結(jié)果。

下面分別介紹圖1中的傳感器數(shù)據(jù)采集、空間坐標(biāo)轉(zhuǎn)換、互補(bǔ)濾波融合、位移計(jì)算以及誤差修正等幾個(gè)環(huán)節(jié)的具體方法。

圖1 基于角度補(bǔ)償?shù)氖謾C(jī)多傳感器數(shù)據(jù)融合測距方法流程示意圖

1.1 傳感器數(shù)據(jù)采集

Android系統(tǒng)支持目前所有已知的傳感器類型，本文需要獲取加速度、陀螺儀和旋轉(zhuǎn)矢量3種類型的傳感器數(shù)據(jù)。獲取傳感器數(shù)據(jù)采集主要流程是:首先根據(jù)3種傳感器類型初始化傳感器管理器，然后通過傳感器管理器注冊監(jiān)聽接口，最后通過監(jiān)聽接口方法獲取到3種類型的傳感器數(shù)值［6］。

三軸加速度傳感器和三軸陀螺儀傳感器的每次輸出3個(gè)數(shù)據(jù)分別代表沿X、Y、Z軸方向的加速度ax、ay、az和角速度ωx、ωy、ωz，如圖2所示，單位分別是m/s2和rad/s。

旋轉(zhuǎn)矢量傳感器是組合了加速度傳感器、陀螺儀傳感器和磁力傳感器經(jīng)過算法計(jì)算后得出的一種合成傳感器。它每次輸出的4個(gè)數(shù)據(jù)分別代表沿X、Y、Z軸的3個(gè)旋轉(zhuǎn)矢量正弦分量和一個(gè)旋轉(zhuǎn)矢量余弦分量。具體參數(shù)如表1所示。

圖2 手機(jī)坐標(biāo)系下三軸加速度和角速度示意圖

表1 旋轉(zhuǎn)量傳感器參數(shù)

1.2 采用四元數(shù)法進(jìn)行空間坐標(biāo)轉(zhuǎn)換

由于使用智能手機(jī)在測量過程中會(huì)發(fā)生不斷的移動(dòng)，三軸加速度傳感器采集的數(shù)據(jù)因手機(jī)姿態(tài)的漂移很難同前一個(gè)時(shí)刻保持一致［7－8］。針對這個(gè)問題，本文提出利用空間坐標(biāo)轉(zhuǎn)換算法將數(shù)據(jù)從手機(jī)坐標(biāo)系映射至靜態(tài)的參考坐標(biāo)系，從而確保數(shù)據(jù)在手機(jī)任意姿態(tài)下均能準(zhǔn)確反映實(shí)際的運(yùn)動(dòng)狀態(tài)。

在三維空間數(shù)學(xué)中，一般采用歐拉角法、方向余弦算法和四元數(shù)法處理空間坐標(biāo)轉(zhuǎn)換問題［9－10］。本文主要使用四元數(shù)法解決空間坐標(biāo)轉(zhuǎn)換問題。當(dāng)智能手機(jī)內(nèi)置陀螺儀傳感器時(shí)，開發(fā)者可以調(diào)用旋轉(zhuǎn)矢量傳感器。通過獲取旋轉(zhuǎn)矢量數(shù)據(jù)利用四元數(shù)法能夠把手機(jī)坐標(biāo)系下的加速度值坐標(biāo)轉(zhuǎn)換為慣性坐標(biāo)系下的加速度值。使用四元數(shù)法條件是已知旋轉(zhuǎn)軸和旋轉(zhuǎn)角度，這些數(shù)據(jù)通過調(diào)用旋轉(zhuǎn)矢量傳感器已經(jīng)獲取。本文將四元數(shù)直接用來表示旋轉(zhuǎn)矩陣，這是一個(gè)很復(fù)雜的推導(dǎo)，趙宏［11］等經(jīng)過推導(dǎo)繞N軸旋轉(zhuǎn)角度θ的矩陣R(n，θ)可以表示為如式(1)所示。

智能手機(jī)旋轉(zhuǎn)矢量傳感器獲得的是標(biāo)準(zhǔn)四元組，這里用 q1、q2、q3、q4分別代表數(shù)值 value［3］、value［0］、value［1］、value［2］。其中，度并能達(dá)到提高精度的作用［12］。目前數(shù)據(jù)融合方法主要有加權(quán)平均融合、卡爾曼濾波法、貝葉斯估計(jì)法等。

將式(2)變形并代入q1、q2、q3、q4來替換式中的n和θ。最后能夠得到四元數(shù)構(gòu)造的旋轉(zhuǎn)矩陣:

最后根據(jù)式(3)，可求出轉(zhuǎn)換后的各軸的加速度數(shù)據(jù)，分別為x'、y'、z'，這里記作a'。

1.3 采用互補(bǔ)濾波融合計(jì)算角度

傳感器融合是指使用多個(gè)傳感器，集中它們的優(yōu)點(diǎn)同時(shí)彌補(bǔ)各自缺點(diǎn)的一項(xiàng)方案。與單傳感器系統(tǒng)相比，運(yùn)用傳感器融合技術(shù)在慣性測量方面能夠提高整個(gè)系統(tǒng)的可靠性和魯棒性，增強(qiáng)數(shù)據(jù)的可信

由于智能手機(jī)在測量過程中會(huì)發(fā)生不斷的移動(dòng)，所以手機(jī)很難保持姿態(tài)的統(tǒng)一，對此在本文中使用角度來研究這種姿態(tài)的漂移?；パa(bǔ)濾波的數(shù)據(jù)融合思想是針對加速度傳感器和陀螺儀傳感器采集的數(shù)據(jù)采用互補(bǔ)濾波方法實(shí)現(xiàn)數(shù)據(jù)融合完成角度計(jì)算，消除加速度計(jì)的高頻短時(shí)間隨機(jī)誤差分量和陀螺儀的低頻長時(shí)間積分誤差，得到一個(gè)更優(yōu)的傾角近似值。如圖3角度計(jì)算方法示意圖所示。

圖3 角度計(jì)算方法

陀螺儀傳感器動(dòng)態(tài)性能好，能夠提供瞬時(shí)間的動(dòng)態(tài)角度變化，其值也不受加速度變化的影響，但是自身受到固有的特性、溫度、積分過程的影響，存在累積漂移誤差，所以測量值隨時(shí)間變化相對較大。而加速度傳感器易受外部干擾，但是測量值隨時(shí)間變化相對較小。為了克服兩者自身的不足，結(jié)合兩種傳感器運(yùn)用互補(bǔ)濾波數(shù)據(jù)融合方法，在短時(shí)間內(nèi)更信任陀螺儀測量值，有效補(bǔ)償陀螺儀的漂移誤差和加速度數(shù)據(jù)的動(dòng)態(tài)誤差。因此，當(dāng)使用智能手機(jī)在測量兩點(diǎn)距離時(shí)，首先根據(jù)低通濾波方法將手機(jī)獲取的加速度數(shù)據(jù)乘上一個(gè)較小的權(quán)重系數(shù)，以減弱異常數(shù)據(jù)對整體產(chǎn)生的影響;然后應(yīng)用高通濾波方法將手機(jī)獲取的陀螺儀數(shù)據(jù)乘上一個(gè)較大的權(quán)重系數(shù)，以抑制陀螺儀積分所帶來的漂移影響。

為了確定濾波權(quán)重系數(shù)β，需要確定時(shí)間常數(shù)τ。時(shí)間常數(shù)是衡量傳感器動(dòng)態(tài)性能的主要描述指標(biāo)，但是僅對一階系統(tǒng)有明確含義。一階系統(tǒng)的微分方程和傳遞函數(shù)為:

式中:X(t)為系統(tǒng)輸出，u(t)為系統(tǒng)輸入，G(s)為系統(tǒng)傳遞函數(shù)，c為常數(shù)。若方程(5)對單位階躍輸入求解，便得X(τ)=1－e－cτ。若τ=1/c，則其響應(yīng)為X(1/c)=0.632，人們把1/c這個(gè)量定義為時(shí)間常數(shù)，即由輸入階躍變化引起的響應(yīng)達(dá)到最終值的63.2%所需的時(shí)間。

已知加速度計(jì)測的角度數(shù)據(jù)為A_angle、陀螺儀數(shù)據(jù)為gyro和采樣時(shí)間為t的情況下，

在式(6)和式(7)所示的計(jì)算過程中，兩個(gè)濾波系數(shù)滿足相加之和等于1，這樣能夠保證濾波結(jié)果達(dá)到一個(gè)較為精確的估值。在實(shí)現(xiàn)角度計(jì)算的同時(shí)，也增強(qiáng)了數(shù)據(jù)的可信度和精確度。

1.4 角度補(bǔ)償?shù)奈灰朴?jì)算

根據(jù)牛頓第二定律，對物體移動(dòng)產(chǎn)生的加速度進(jìn)行一次積分可以得到移動(dòng)的速度，而進(jìn)行二次積分則可以得到位移［6］?；谝陨显?，本文首先將獲取實(shí)時(shí)物體移動(dòng)的加速度和角速度通過空間坐標(biāo)轉(zhuǎn)換和角度計(jì)算，并將轉(zhuǎn)換后的加速度與角度的余弦相乘，最后進(jìn)行重積分轉(zhuǎn)化為位移量，最后將位移量疊加得到物體運(yùn)動(dòng)的距離。加速度與位移之間的關(guān)系如式(8)所示。

通過以上的數(shù)值積分，在確定初速度和初始位移的情況下計(jì)算能夠得到一定時(shí)間內(nèi)從初始位移到終點(diǎn)位移的位移大小。對于積分函數(shù)的計(jì)算本文是通過矩形積分法進(jìn)行近似計(jì)算的。方法是當(dāng)采樣時(shí)間間隔足夠小時(shí)，采用兩點(diǎn)之間的中間值乘上他們的時(shí)間間隔，得到的矩形面積之和近似等于函數(shù)自身的面積。

1.5 誤差修正

測距系統(tǒng)采集的傳感器數(shù)據(jù)在進(jìn)行空間坐標(biāo)轉(zhuǎn)換和角度補(bǔ)償后位移計(jì)算得到的是初步測距結(jié)果。這時(shí)需要考慮到傳感器零點(diǎn)漂移和重力的影響，因此需要修正誤差。誤差修正主要包括水平校正、忽略地球重力和算術(shù)平均值。水平校正是指是獲得的傳感器數(shù)據(jù)需要減去水平靜止?fàn)顟B(tài)下的手機(jī)3個(gè)方向的加速度實(shí)現(xiàn)校正。這是解決慣性傳感器在靜止?fàn)顟B(tài)下數(shù)據(jù)存在零點(diǎn)漂移的問題［13］。忽略地球引力是考慮到三軸加速度數(shù)據(jù)包含重力的影響，需要排除重力加速度的作用。采用低通濾波器分離出重力加速度值，用高通濾波器剔除重力干擾，最后得到其實(shí)也就是線性加速度值。例如，在時(shí)間常數(shù)等于0.75 s，采樣時(shí)間間隔dt等于0.02 s時(shí)，過濾器常量β約等于0.8，則重力數(shù)據(jù)與排除重力作用后的加速度數(shù)據(jù)分別如式(9)和式(10)所示:

根據(jù)中心極限定理，測量數(shù)據(jù)的隨機(jī)誤差服從高斯分布，所選取的要平均的數(shù)據(jù)量越多，平均值的標(biāo)準(zhǔn)方差就越?。?4］。本文針對同一目標(biāo)重復(fù)測距10次，從而計(jì)算出平均值，則其平均值在統(tǒng)計(jì)上非常接近于真實(shí)值。

1.6 算法描述

ACD－R:Angle Data Compensated Ranging

算法1 角度數(shù)據(jù)補(bǔ)償?shù)亩鄠鞲衅鲾?shù)據(jù)融合測距算法

Input

a={ax，ay，az};//三軸加速度傳感器數(shù)據(jù)

ω={ωx，ωy，ωz};//三軸角速度傳感器數(shù)據(jù)

q={q1，q2，q3，q4};//旋轉(zhuǎn)矢量傳感器數(shù)據(jù)

τ;//時(shí)間常數(shù)

t;//時(shí)間

Output

s;//測距結(jié)果

Begin

a'=(x'、y'、z')=convert(a，ω，q);//空間坐標(biāo)轉(zhuǎn)換

β=τ/(τ+t);//確定濾波系數(shù)

angle=β*(angle+gyro*dt)+(1－β)*A_angle;//計(jì)算角度

Return s

End

偽代碼算法描述:輸入手機(jī)坐標(biāo)系下的傳感器數(shù)據(jù)(ax，ay，az，ωx，ωy，ωz，q1，q2，q3，q4)，通過四元數(shù)法將加速度空間坐標(biāo)轉(zhuǎn)換到靜態(tài)參考坐標(biāo)系下加速度a'(x'、y'、z')，根據(jù)過濾器觸發(fā)傳感器事件的時(shí)間確定時(shí)間常數(shù)τ，然后確定濾波系數(shù)β，通過互補(bǔ)濾波融合后求和完成角度計(jì)算angle，最后對加速度a'與角度余弦的乘積進(jìn)行矩形積分法處理，計(jì)算得到位移結(jié)果s。

2 測距實(shí)驗(yàn)

2.1 實(shí)驗(yàn)系統(tǒng)設(shè)計(jì)

為了進(jìn)行實(shí)驗(yàn)，我們開發(fā)了一個(gè)原型系統(tǒng)，總體框架如圖4所示。一是傳感器數(shù)據(jù)采集模塊，根據(jù)傳感器類型初始化傳感器管理器，然后通過傳感器管理器注冊監(jiān)聽接口，當(dāng)監(jiān)聽接口方法獲取到傳感器數(shù)據(jù)，并實(shí)時(shí)傳遞到數(shù)據(jù)顯示模塊。二是數(shù)據(jù)顯示模塊:首先，該模塊在用戶打開系統(tǒng)時(shí)自動(dòng)實(shí)時(shí)監(jiān)控并顯示加速度傳感器和陀螺儀傳感器的X、Y、Z 3個(gè)方向的加速度和角速度數(shù)據(jù)并以波形圖形式顯示出來;其次，當(dāng)用戶完成距離測量松開按鈕后系統(tǒng)會(huì)根據(jù)處理模塊的反饋顯示測距結(jié)果。三是用戶控制模塊:首先，當(dāng)用戶手持智能按住界面開始按鈕該模塊開始工作，在用戶移動(dòng)手機(jī)松開按鈕后完成距離測量;其次，用戶可以選擇勾選水平校正和忽略地球引力兩個(gè)復(fù)選框控件。四是傳感器數(shù)據(jù)處理模塊，系統(tǒng)在收到采集的數(shù)據(jù)，并勾選水平校正和忽略地球引力等校正數(shù)據(jù)后，利用本文提出的算法對采集數(shù)據(jù)處理后進(jìn)行積分，最后計(jì)算位移量。

圖4 測距系統(tǒng)總體框架圖

2.2 實(shí)驗(yàn)方案

為了測試本文提出的ADC-R算法，確定實(shí)驗(yàn)參數(shù)如表2所示。選用小米5智能手機(jī)作為實(shí)驗(yàn)機(jī)型;測量距離分別為10 cm、20 cm、30 cm、40 cm、50 cm、60 cm、70 cm、80 cm、90 cm、100 cm;傳感器采樣頻率為0.2 s(標(biāo)準(zhǔn)延遲)和0.02 s(游戲延遲)［15］;手機(jī)移動(dòng)速度基本保持10 cm/s。

為了分析提出的ADC-R算法的性能，將另外兩種測距算法加速度積分方法(AI-R)、加速度陀螺儀融合方法(AGF-R)在同樣環(huán)境中進(jìn)行實(shí)驗(yàn)，并計(jì)算各種測量距離結(jié)果的平均值及測距誤差，與ACD-R算法進(jìn)行對比分析。

表2 實(shí)驗(yàn)參數(shù)

3 結(jié)果與討論

3.1 實(shí)驗(yàn)結(jié)果

采用AI-R、AGF-R、ADC-R這3種方法對不同距離10次測量結(jié)果均值的結(jié)果如圖5所示，圖5(a)為采樣周期為0.2 s時(shí)3種方法的測距結(jié)果，圖5(b)為采樣周期為0.02 s時(shí)3種方法的測距結(jié)果。對應(yīng)的測距誤差如圖6所示，圖6(a)為采樣周期為0.2 s時(shí)3種方法的測距結(jié)果誤差，圖6(b)為采樣周期為0.02 s時(shí)3種方法的測距結(jié)果誤差。

圖5 不同方法的測距結(jié)果

從圖5可以看出，兩種采樣周期在不同距離和方法下的結(jié)果誤差A(yù)DC-R方法相比AI-R方法、AGF-R方法都要小，AGF-R方法相比AI-R方法小，并且 ADC-R方法最靠近真實(shí)值。主要原因是ADC-R方法采用了空間坐標(biāo)轉(zhuǎn)換和角度補(bǔ)償，有效減少來積分帶來的誤差積累。

圖6 不同方法測距結(jié)果的誤差

從圖5還可以看出，3種方法的測量結(jié)果都大于真實(shí)值，但這里呈現(xiàn)的正誤差并不意味測量結(jié)果都是大于真實(shí)值，而是針對每種方法下的不同距離分別測試10次后取算數(shù)平均值得到的。例如針對30 cm距離分別采用3種方法測試結(jié)果如表3所示，單次測量存在小于真實(shí)值的情況出現(xiàn)，但大部分測量結(jié)果都大于真實(shí)值。

表3 30 cm距離實(shí)際測量結(jié)果

從圖6可以看出，3種方法不同距離的測量誤差由ADC-R方法、AGF-R方法、AI-R方法依次遞增。并且3種方法的誤差隨測量距離的增加總體趨勢都是增長的，主要原因是積分方法帶來的誤差積累不斷增大。

3.2 采樣周期對測量結(jié)果的影響

ACD-R算法在測量周期分別為0.2 s和0.02 s時(shí)的測量結(jié)果如圖7所示，對應(yīng)的測量誤差結(jié)果如圖8所示。

圖7 兩種傳感器采樣周期的測距結(jié)果

從圖7可以看出，傳感器采樣周期為0.02 s時(shí)比0.2 s在不同測量距離時(shí)測距結(jié)果值要小，二者相比較，0.02 s時(shí)的測距結(jié)果更接近真實(shí)值。主要原因是在傳感器設(shè)定0.02 s的采樣頻率下，采集的數(shù)據(jù)更詳細(xì)，傳感器能夠更好的感知測量過程中手機(jī)動(dòng)作的連續(xù)性。

圖8 兩種傳感器采樣周期測距結(jié)果的誤差

從圖8可以看出，兩種采樣周期隨測量距離的增加誤差走勢大體呈“高低高”模式，傳感器采樣周期為0.02 s時(shí)比0.2 s在不同測量距離時(shí)測距結(jié)果的誤差要小。主要原因是ADC-R算法采用矩形積分法，當(dāng)采樣時(shí)間間隔足夠越小時(shí)，將兩點(diǎn)之間的中間值乘上他們的時(shí)間間隔，得到的矩形面積之和近似等于函數(shù)自身的面積。

此外，采樣周期為0.2 s的測距誤差曲線在30CM附近到達(dá)低谷，而采樣周期為0.02 s的測距誤差曲線的低谷點(diǎn)在20 cm附近。這說明在移動(dòng)速度確定的情況下，隨著采樣頻率的提高，ADC-R算法在30 cm之內(nèi)能夠減小測距結(jié)果的誤差。而兩種采樣周期的測量誤差曲線大于30 cm距離時(shí)差別幾乎是固定的，經(jīng)過數(shù)據(jù)計(jì)算，0.02 s采樣周期下10次測量結(jié)果的準(zhǔn)確率比0.2 s模式高約1%。因此，在傳感器采集周期為0.02 s時(shí)的實(shí)驗(yàn)結(jié)果更貼合真實(shí)距離值。當(dāng)目標(biāo)測量距離逐漸增加到1 m時(shí)實(shí)驗(yàn)結(jié)果的誤差率增加到接近10%。說明ADC-R算法的應(yīng)用范圍在1 m以內(nèi)能夠保持90%以上的準(zhǔn)確性。

4 結(jié)論

本文針對智能手機(jī)傳感器慣性測距的問題，提出了基于角度補(bǔ)償?shù)氖謾C(jī)多傳感器數(shù)據(jù)融合的測距算法ADC-R:以陀螺儀傳感器數(shù)據(jù)作為參數(shù)，把手機(jī)動(dòng)態(tài)坐標(biāo)系下測得的加速度值空間坐標(biāo)轉(zhuǎn)換到靜態(tài)的參考坐標(biāo)系下，通過數(shù)據(jù)融合完成角度補(bǔ)償，再對補(bǔ)償后的加速度進(jìn)行積分得到初步測距結(jié)果，并采用水平校正、忽略地球重力和算術(shù)平均值等方法進(jìn)行誤差修正后，得到最終測距結(jié)果。

實(shí)驗(yàn)結(jié)果表明，ADC-R算法在不同采樣頻率下的測量準(zhǔn)確性均可達(dá)到90%以上，采樣頻率越高，測距準(zhǔn)確性越高。通過與另外兩種測距算法加速度積分法(AI-R)和加速度與陀螺儀融合方法(AGFR)的對比實(shí)驗(yàn)，可以看出在各種測量距離、各種采樣頻率下，ADC-R的性能均優(yōu)于另外兩種算法。

從本文的研究可以看出，利用智能手機(jī)作為工具進(jìn)行近距離測量具有一定的適用性。但是測量結(jié)果的準(zhǔn)確性受到傳感器測量精度、加速度積分測距原理等因素的影響，這兩方面的改進(jìn)都會(huì)對測量結(jié)果產(chǎn)生影響。本文的工作主要是從第2個(gè)方面進(jìn)行的努力，在后面的研究中，我們將考慮采用其他的角度來解決測距準(zhǔn)確性的問題。

［1］ 李光偉，董林璽.基于MEMS加速度傳感器的位移檢測系統(tǒng)［J］.傳感器與微系統(tǒng)，2014，33(7):79－81.

［2］ 高云，周聰聰，田健，等.基于多傳感器的無創(chuàng)血壓測量系統(tǒng)的研究［J］.傳感技術(shù)學(xué)報(bào)，2015，28(5):763－767.

［3］ 羅印.基于智能手機(jī)傳感器的空間位移測量軟件的設(shè)計(jì)與實(shí)現(xiàn)［D］.北京:北京郵電大學(xué)，2015.

［4］ 周英杰.加速度測試積分位移算法及其應(yīng)用研究［D］.重慶:重慶大學(xué)，2013.

［5］ 張立峰，李媛媛.多傳感器自適應(yīng)加權(quán)融合算法及其應(yīng)用研究［J］.自動(dòng)化與儀器儀表，2008(2):10－13.

［6］ 杜瑾，李杰，馮凱強(qiáng)，等.多項(xiàng)式擬合在MEMS陀螺儀零點(diǎn)隨機(jī)漂移抑制中的應(yīng)用研究［J］.傳感技術(shù)學(xué)報(bào)，2016，29(5):734－737.

［7］ 任明泉.基于加速度傳感器的運(yùn)動(dòng)物體軌跡檢測系統(tǒng)的研究［D］.南京:南京郵電大學(xué)，2013.

［8］ 朱致遠(yuǎn).參與式感知平臺(tái)數(shù)據(jù)采集關(guān)鍵技術(shù)研究與實(shí)現(xiàn)［D］.北京:北京郵電大學(xué)，2015.

［9］ 胡三慶.基于MEMS加速度傳感器的空間運(yùn)動(dòng)軌跡追蹤系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)［D］.武漢:華中科技大學(xué)，2009.

［10］陳孟元，謝義建，陳躍東.基于四元數(shù)改進(jìn)型互補(bǔ)濾波的MEMS姿態(tài)解算［J］.電子測量與儀器學(xué)報(bào)，2015，29(9):1391－1397.

［11］趙宏，郭立淥.手機(jī)內(nèi)置加速度傳感器數(shù)據(jù)的空間坐標(biāo)轉(zhuǎn)換算法［J］.計(jì)算機(jī)應(yīng)用，2016，36(2):301－306.

［12］姜延吉.多傳感器數(shù)據(jù)融合關(guān)鍵技術(shù)研究［D］.哈爾濱:哈爾濱工程大學(xué)，2010.

［13］林生榮，張輝.三軸加速度傳感器校正方法研究［J］.傳感器與微系統(tǒng)，2011，30(11):72－74.

［14］鄒廣玉.混合隨機(jī)變量序列的幾乎處處中心極限定理［D］.長春:吉林大學(xué)，2013.

［15］Greg M，Adam S.Android傳感器高級編程［M］.裴佳迪，譯.北京:清華大學(xué)出版社，2013:103－110.

陳 帥(1990－)，男，碩士研究生，2014年就讀于浙江農(nóng)林大學(xué)農(nóng)業(yè)信息化專業(yè)，研究方向?yàn)闊o線傳感器網(wǎng)絡(luò)及其在農(nóng)林業(yè)的應(yīng)用，chenshuaics725@163.com;

王國英(1977－)，男，1999年本科畢業(yè)于北京交通大學(xué)，2004年碩士畢業(yè)于廣西大學(xué)。目前就職于浙江農(nóng)林大學(xué)，研究方向?yàn)闊o線傳感網(wǎng)絡(luò);

莫路鋒(1979－)，研究生導(dǎo)師，2004年碩士畢業(yè)于北京大學(xué)，2014年博士畢業(yè)于西安交通大學(xué)，目前就職于浙江農(nóng)林大學(xué)，研究方向?yàn)闊o線傳感器網(wǎng)絡(luò)，molufeng@gmail.com。

Angle Data Compensated Ranging Using Multiple Sensors Data Fusion of Smart Phone*

CHEN Shuai，WANG Guoying，MO Lufeng*
(1.School of Information Engineering，Zhejiang A＆F University，Lin’an Zhejiang 311300，China)

In the field of inertial measurement，it does not accurately perceive the target object movement distance by simply using double integration of acceleration method.Acceleration sensor in the sense of linear movement of the target object is more accurate and practical，but it will change the direction of the drift continually occur in the three-dimensional movement of its axis with the object.To solve this problem，this paper proposes an algorithm that angle data compensated ranging(ADC-R)using multiple sensors data fusion of smart phone:acceleration sensor is used to measure the movement of object acceleration as calculated displacement of the original data;it is using gyroscope sensor to measure angular velocity of the moving object and the output data of the rotation vector sensor is used as the parameter to convert the measured acceleration value space coordinate of mobile phone to the static reference coordinate system.According to the relationship between acceleration and displacement of physics，the method of mathematical integration and the technique of further error correction are used to get the final moving distance.The experimental results show that this method has high precision in the near distance ranging and it is better than the acceleration integral ranging algorithm and acceleration gyroscope fusion ranging algorithm.

mobile sensors;data fusion;spatial coordinate conversion;distance measurement

C:7230

10.3969/j.issn.1004－1699.2017.02.015

TP212.9

A

1004－1699(2017)02－0253－07

項(xiàng)目來源:國家自然科學(xué)基金項(xiàng)目(61190114，61303236);浙江省科技計(jì)劃項(xiàng)目重大科技專項(xiàng)項(xiàng)目(2012C13011－1);浙江省自然科學(xué)基金項(xiàng)目(LY16F020036)

2016－06－30 修改日期:2016－09－14