基于角度補償的手機多傳感器數據融合測距算法*

陳 帥，王國英，莫路鋒

(浙江農林大學信息工程學院，浙江臨安311300)

基于角度補償的手機多傳感器數據融合測距算法*

陳 帥，王國英，莫路鋒*

(浙江農林大學信息工程學院，浙江臨安311300)

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

手機傳感器;數據融合;空間坐標轉換;距離測量。

在慣性測量領域，單純利用加速度二次積分的方法并不能準確感知目標對象移動的距離［1－2］。這是因為加速度傳感器只能捕捉一個動作而不能實現軌跡測量，所以單純利用加速度直接重積分獲得的位移量往往與真實結果的誤差很大。常見的傳感器數據誤差來源主要有人為誤差、系統誤差以及隨機誤差，噪聲，漂移，零點偏移，時間延遲和數據丟棄以及積分誤差等等因素。以上隨機影響因素的出現都有可能導致誤差偏離真實的結果。因此，為了準確估算目標對象的移動距離，測距系統的算法設計以及誤差修正技術至關重要。

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

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

1 算法思想

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

下面分別介紹圖1中的傳感器數據采集、空間坐標轉換、互補濾波融合、位移計算以及誤差修正等幾個環節的具體方法。

圖1 基于角度補償的手機多傳感器數據融合測距方法流程示意圖

1.1 傳感器數據采集

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

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

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

圖2 手機坐標系下三軸加速度和角速度示意圖

表1 旋轉量傳感器參數

1.2 采用四元數法進行空間坐標轉換

由于使用智能手機在測量過程中會發生不斷的移動，三軸加速度傳感器采集的數據因手機姿態的漂移很難同前一個時刻保持一致［7－8］。針對這個問題，本文提出利用空間坐標轉換算法將數據從手機坐標系映射至靜態的參考坐標系，從而確保數據在手機任意姿態下均能準確反映實際的運動狀態。

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

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

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

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

1.3 采用互補濾波融合計算角度

傳感器融合是指使用多個傳感器，集中它們的優點同時彌補各自缺點的一項方案。與單傳感器系統相比，運用傳感器融合技術在慣性測量方面能夠提高整個系統的可靠性和魯棒性，增強數據的可信

由于智能手機在測量過程中會發生不斷的移動，所以手機很難保持姿態的統一，對此在本文中使用角度來研究這種姿態的漂移。互補濾波的數據融合思想是針對加速度傳感器和陀螺儀傳感器采集的數據采用互補濾波方法實現數據融合完成角度計算，消除加速度計的高頻短時間隨機誤差分量和陀螺儀的低頻長時間積分誤差，得到一個更優的傾角近似值。如圖3角度計算方法示意圖所示。

圖3 角度計算方法

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

為了確定濾波權重系數β，需要確定時間常數τ。時間常數是衡量傳感器動態性能的主要描述指標，但是僅對一階系統有明確含義。一階系統的微分方程和傳遞函數為:

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

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

在式(6)和式(7)所示的計算過程中，兩個濾波系數滿足相加之和等于1，這樣能夠保證濾波結果達到一個較為精確的估值。在實現角度計算的同時，也增強了數據的可信度和精確度。

1.4 角度補償的位移計算

根據牛頓第二定律，對物體移動產生的加速度進行一次積分可以得到移動的速度，而進行二次積分則可以得到位移［6］。基于以上原理，本文首先將獲取實時物體移動的加速度和角速度通過空間坐標轉換和角度計算，并將轉換后的加速度與角度的余弦相乘，最后進行重積分轉化為位移量，最后將位移量疊加得到物體運動的距離。加速度與位移之間的關系如式(8)所示。

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

1.5 誤差修正

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

根據中心極限定理，測量數據的隨機誤差服從高斯分布，所選取的要平均的數據量越多，平均值的標準方差就越小［14］。本文針對同一目標重復測距10次，從而計算出平均值，則其平均值在統計上非常接近于真實值。

1.6 算法描述

ACD－R:Angle Data Compensated Ranging

算法1 角度數據補償的多傳感器數據融合測距算法

Input

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

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

q={q1，q2，q3，q4};//旋轉矢量傳感器數據

τ;//時間常數

t;//時間

Output

s;//測距結果

Begin

a'=(x'、y'、z')=convert(a，ω，q);//空間坐標轉換

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

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

Return s

End

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

2 測距實驗

2.1 實驗系統設計

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

圖4 測距系統總體框架圖

2.2 實驗方案

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

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

表2 實驗參數

3 結果與討論

3.1 實驗結果

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

圖5 不同方法的測距結果

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

圖6 不同方法測距結果的誤差

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

表3 30 cm距離實際測量結果

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

3.2 采樣周期對測量結果的影響

ACD-R算法在測量周期分別為0.2 s和0.02 s時的測量結果如圖7所示，對應的測量誤差結果如圖8所示。

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

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

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

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

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

4 結論

本文針對智能手機傳感器慣性測距的問題，提出了基于角度補償的手機多傳感器數據融合的測距算法ADC-R:以陀螺儀傳感器數據作為參數，把手機動態坐標系下測得的加速度值空間坐標轉換到靜態的參考坐標系下，通過數據融合完成角度補償，再對補償后的加速度進行積分得到初步測距結果，并采用水平校正、忽略地球重力和算術平均值等方法進行誤差修正后，得到最終測距結果。

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

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

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

［2］ 高云，周聰聰，田健，等.基于多傳感器的無創血壓測量系統的研究［J］.傳感技術學報，2015，28(5):763－767.

［3］ 羅印.基于智能手機傳感器的空間位移測量軟件的設計與實現［D］.北京:北京郵電大學，2015.

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

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

［6］ 杜瑾，李杰，馮凱強，等.多項式擬合在MEMS陀螺儀零點隨機漂移抑制中的應用研究［J］.傳感技術學報，2016，29(5):734－737.

［7］ 任明泉.基于加速度傳感器的運動物體軌跡檢測系統的研究［D］.南京:南京郵電大學，2013.

［8］ 朱致遠.參與式感知平臺數據采集關鍵技術研究與實現［D］.北京:北京郵電大學，2015.

［9］ 胡三慶.基于MEMS加速度傳感器的空間運動軌跡追蹤系統設計與實現［D］.武漢:華中科技大學，2009.

［10］陳孟元，謝義建，陳躍東.基于四元數改進型互補濾波的MEMS姿態解算［J］.電子測量與儀器學報，2015，29(9):1391－1397.

［11］趙宏，郭立淥.手機內置加速度傳感器數據的空間坐標轉換算法［J］.計算機應用，2016，36(2):301－306.

［12］姜延吉.多傳感器數據融合關鍵技術研究［D］.哈爾濱:哈爾濱工程大學，2010.

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

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

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

陳 帥(1990－)，男，碩士研究生，2014年就讀于浙江農林大學農業信息化專業，研究方向為無線傳感器網絡及其在農林業的應用，chenshuaics725@163.com;

王國英(1977－)，男，1999年本科畢業于北京交通大學，2004年碩士畢業于廣西大學。目前就職于浙江農林大學，研究方向為無線傳感網絡;

莫路鋒(1979－)，研究生導師，2004年碩士畢業于北京大學，2014年博士畢業于西安交通大學，目前就職于浙江農林大學，研究方向為無線傳感器網絡，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

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

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