王騰 牛椏楓

摘要：該文在分析了Android設(shè)備相關(guān)傳感器后，對其工作中會(huì)出現(xiàn)的問題提出了新的方案，并通過圖表說明了其工作方式，提出了通過互補(bǔ)濾波器來實(shí)現(xiàn)傳感器融合，只要對加速感應(yīng)器和磁場感應(yīng)器作類似低通濾波的處理，而對陀螺儀作類似高通濾波的處理，然后整合它們的結(jié)果數(shù)據(jù)，就可以得到相對精確的結(jié)果，且通過軟件的形式來實(shí)現(xiàn)這一技術(shù)。

關(guān)鍵詞：互補(bǔ)濾波器；Android傳感器；傳感器融合

中圖分類號(hào)：TP391 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1009-3044（2014）25-5950-05

Sensor Fusion Technology to Achieve by Complementary Filter

WANG Teng1，NIU Ya-feng2

（1.Yangtze University College of Arts and Sciences， Jingzhou 434020， China；2.Tourism and Environment College of Shaanxi Normal University， xi 'an 710119，China）

Abstract： Based on the analysis of Android equipment related sensor， a new scheme to appear the problem in the work presented， and explained its ways of working through the chart， the complementary filter to realize sensor fusion technology， as long as similar low pass filter on the acceleration sensor and magnetic sensor， and similar high pass filtering of the gyroscope， and then integrate them data that can get relatively accurate results， but also by the form of software to realize this one technology.

Key words： complementary filter； Android sensor； sensor fusion

隨著科技的發(fā)展，智能設(shè)備的應(yīng)用越來越普及，越來越多的人開始使用智能手機(jī)，智能手機(jī)為我們的學(xué)習(xí)和生活帶來了太多的方便，也早就滲入人們的衣食住行，人們享受著它給我們帶來的快捷和方便，可是為什么智能手機(jī)如此強(qiáng)大呢？這跟里面的各種傳感器密切相關(guān)。當(dāng)今市場上，智能手機(jī)中，谷歌的android系統(tǒng)占據(jù)了很大一部分的市場，大多數(shù)的android設(shè)備都有內(nèi)置的測量運(yùn)動(dòng)、方向、和各種環(huán)境條件的傳感器。這些傳感器具有提供高精度和準(zhǔn)確度的原始數(shù)據(jù)的能力，可用于監(jiān)視設(shè)備在三維方向的移動(dòng)和位置、或者監(jiān)視設(shè)備周圍環(huán)境的變化。例如，一個(gè)游戲可能要從重力傳感器中讀取軌跡，以便推斷出復(fù)雜的用戶手勢和意圖，如傾斜、振動(dòng)、旋轉(zhuǎn)或擺動(dòng)等；同樣，有關(guān)天氣的應(yīng)用程序可能要使用設(shè)備的溫度傳感器和濕度傳感器來計(jì)算并報(bào)告露點(diǎn)；有關(guān)旅行的應(yīng)用程序可能要使用地磁場傳感器和加速度傳感器來報(bào)告羅盤方位。

1 安卓設(shè)備中陀螺儀的分析

android獲取方向是通過磁場感應(yīng)器和加速度感應(yīng)器共同獲得的，至于具體的算法SDK已經(jīng)封裝好了。也就是說現(xiàn)在獲取用戶方向有兩種方式，一是官方推薦的，通過SensorManager.getOrientation（）來獲取，這個(gè)方法表面看似容易，但實(shí)際上需要用到兩個(gè)感應(yīng)器共同完成工作，特點(diǎn)是更加的準(zhǔn)確。第二種方法非常簡單，直接得到三個(gè)軸上的數(shù)據(jù)。

獲取安卓設(shè)備翻轉(zhuǎn)動(dòng)態(tài)的普通方法都是調(diào)用SensorManager.getOrientation（） 函數(shù)，這個(gè)函數(shù)可以通過所獲取設(shè)備的3個(gè)平面角度來得到安卓設(shè)備的翻轉(zhuǎn)動(dòng)態(tài)，其中2個(gè)角度數(shù)據(jù)是基于加速感應(yīng)器和磁場感應(yīng)器來獲取的。簡而言之，就是加速感應(yīng)器可以得到重力矢量數(shù)據(jù)（這個(gè)矢量是指向地心的方向），與此同時(shí)磁場感應(yīng)器作為指南針來做輔助工作。這兩個(gè)傳感器提供的信息數(shù)據(jù)就完全可以計(jì)算出設(shè)備的平面數(shù)據(jù)了。但是這兩個(gè)傳感器的數(shù)據(jù)時(shí)常都不夠準(zhǔn)確，特別是磁場感應(yīng)器很容易受到干擾。

很多安卓設(shè)備中都有陀螺儀這個(gè)模塊，它的測量精度較高并且設(shè)備模塊響應(yīng)時(shí)間也很短。陀螺儀的底部被托住，但是能夠各個(gè)方向旋轉(zhuǎn)。它能夠提供XYZ三個(gè)軸方向的角位移速度值。這3個(gè)值通過整合得到當(dāng)前設(shè)備的真實(shí)方向，其計(jì)算方法是角速度乘以設(shè)備從開始移動(dòng)到某一相對狀態(tài)的時(shí)間間隔，它的結(jié)果就得到一個(gè)旋轉(zhuǎn)增量。所有產(chǎn)生的旋轉(zhuǎn)增量之和就是設(shè)備的絕對方向。以上的處理過程看起來很好，但是陀螺儀的數(shù)據(jù)處理是一個(gè)循環(huán)往復(fù)的過程，在每一次的循環(huán)過程中都會(huì)產(chǎn)生細(xì)微的數(shù)據(jù)偏差，雖然這些偏差很細(xì)微，但是當(dāng)它們積累以后的結(jié)果，往往會(huì)導(dǎo)致陀螺儀的數(shù)據(jù)計(jì)算出錯(cuò)，這種錯(cuò)誤也就是我們常說的陀螺漂移。

為了避免以上兩種方法各自的弊端，即陀螺漂移和磁場干擾。就需要尋找新的方法來進(jìn)行計(jì)算測量，通過總結(jié)這三個(gè)模塊的特點(diǎn)，可知陀螺儀適合在方向角度迅速發(fā)生改變時(shí)做測量計(jì)算，但是在一個(gè)較長的使用周期內(nèi)，其數(shù)據(jù)會(huì)有錯(cuò)誤；而加速感應(yīng)器和磁場感應(yīng)器適合周期更長的測量。所以，只要對加速感應(yīng)器和磁場感應(yīng)器作類似低通濾波的處理，而對陀螺儀作類似高通濾波的處理，然后整合它們的結(jié)果數(shù)據(jù)，就可以得到相對精確的結(jié)果。3個(gè)傳感器融合并濾波的過程如圖1所示。

那么，傳感器的高通和低通濾波數(shù)據(jù)到底是什么意思呢？傳感器定時(shí)（這個(gè)時(shí)間間隔可以人為設(shè)置）傳送數(shù)據(jù)。這些數(shù)據(jù)值可以在一個(gè)以時(shí)間為單位的X軸的二維圖中顯示出來，這個(gè)圖的界面和聲波信號(hào)類似。加速感應(yīng)器和重力感應(yīng)器干擾信號(hào)的低通濾波是分布在一個(gè)連續(xù)時(shí)間窗口全方位角度之中的。

在下面的代碼中，通過引用從加速感應(yīng)器和重力感應(yīng)器到對地定向的值來實(shí)現(xiàn)上面所述的情況。

// 低通濾波，需要加一個(gè)方向變量factor的權(quán)值

accMagOrientation = （ 1 - factor ） * accMagOrientation+ factor * newAccMagValue；

陀螺儀數(shù)據(jù)的高通濾波數(shù)據(jù)是使用相應(yīng)的陀螺儀方位數(shù)據(jù)替換accMagOrientation中已經(jīng)過濾過的高頻率部分。

fusedOrientation =

（1 - factor） * newGyroValue // high-frequency component+ factor * newAccMagValue；

事實(shí)上，這個(gè)結(jié)果就已經(jīng)可以作為混合濾波器了。

假設(shè)安卓設(shè)備在某方向上翻轉(zhuǎn)90度然后迅速復(fù)位，這其中的信號(hào)通過濾波的過程如圖2所示。注意在陀螺儀的陀螺漂移信號(hào)。

圖2 信號(hào)濾波過程（翻轉(zhuǎn)90度并迅速復(fù)位）

2 融合技術(shù)的實(shí)現(xiàn)

下面通過一個(gè)安卓應(yīng)用程序來看一下它的使用。

2.1初始化

首先通過成員變量的賦值來建立這個(gè)APP，獲取SensorManager，通過OnCreate函數(shù)來初始化傳感器監(jiān)聽器。

public class SensorFusionActivity extends Activity implements SensorEventListener{

private SensorManager mSensorManager = null

private float[] gyro = new float[3]；

private float[] gyroMatrix = new float[9]；

private float[] gyroOrientation = new float[3]；

private float[] magnet = new float[3]；

private float[] accel = new float[3]；

private float[] accMagOrientation = new float[3]；

private float[] fusedOrientation = new float[3]；

private float[] rotationMatrix = new float[9]；

public void onCreate（Bundle savedInstanceState） {

super.onCreate（savedInstanceState）；

setContentView（R.layout.main）；

gyroOrientation[0] = 0.0f；

gyroOrientation[1] = 0.0f；

gyroOrientation[2] = 0.0f；

gyroMatrix[0] = 1.0f； gyroMatrix[1] = 0.0f； gyroMatrix[2] = 0.0f；

gyroMatrix[3] = 0.0f； gyroMatrix[4] = 1.0f； gyroMatrix[5] = 0.0f；

gyroMatrix[6] = 0.0f； gyroMatrix[7] = 0.0f； gyroMatrix[8] = 1.0f；

// 獲取sensorManager，初始化傳感器監(jiān)聽器

mSensorManager = （SensorManager） this.getSystemService（SENSOR_SERVICE）；

initListeners（）；}}

注意程序中使用的SensorEventListener接口，下面會(huì)使用2個(gè)函數(shù)onAccuracyChanged和onSensorChanged。這里的重點(diǎn)是onSensorChanged函數(shù)，它可以持續(xù)更新傳感器數(shù)據(jù)。傳感器監(jiān)聽器的初始化通過下面的initListeners函數(shù)。

public void initListeners（）{

mSensorManager.registerListener（this，

mSensorManager.getDefaultSensor（Sensor.TYPE_ACCELEROMETER），

SensorManager.SENSOR_DELAY_FASTEST）；

mSensorManager.registerListener（this，

mSensorManager.getDefaultSensor（Sensor.TYPE_GYROSCOPE），

SensorManager.SENSOR_DELAY_FASTEST）；

mSensorManager.registerListener（this，

mSensorManager.getDefaultSensor（Sensor.TYPE_MAGNETIC_FIELD），

SensorManager.SENSOR_DELAY_FASTEST）；}

2.2獲取和處理傳感器數(shù)據(jù)

監(jiān)聽器初始化完成后，如果有新的可用傳感器數(shù)據(jù)產(chǎn)生，onSensorChanged函數(shù)會(huì)被自動(dòng)調(diào)用，onSensorChanged函數(shù)代碼如下所示。

public void onSensorChanged（SensorEvent event） {

switch（event.sensor.getType（）） {

case Sensor.TYPE_ACCELEROMETER：

// 把加速傳感器的數(shù)據(jù)復(fù)制給accel數(shù)組，然后計(jì)算新的方向數(shù)據(jù)

System.arraycopy（event.values， 0， accel， 0， 3）；

calculateAccMagOrientation（）；

break；

case Sensor.TYPE_GYROSCOPE：

// process gyro data

gyroFunction（event）；

break；

case Sensor.TYPE_MAGNETIC_FIELD：

// copy new magnetometer data into magnet array

System.arraycopy（event.values， 0， magnet， 0， 3）；

break；}}

android API提供了很多函數(shù)可以很方便的通過加速感應(yīng)器和磁場感應(yīng)器來獲取對地定向。

public void calculateAccMagOrientation（） {

if（SensorManager.getRotationMatrix（rotationMatrix， null， accel， magnet）） {

SensorManager.getOrientation（rotationMatrix， accMagOrientation）；}}

如上所述，陀螺儀數(shù)據(jù)是不能直接用來做計(jì)算的，它需要進(jìn)行處理，其具體的處理方法在android幫助中已經(jīng)給出，這里就不再詳述，下面就是對這段代碼的引用，其中添加了一些參數(shù)。

public static final float EPSILON = 0.000000001f；

private void getRotationVectorFromGyro（float[] gyroValues，

float[] deltaRotationVector，

float timeFactor）

{float[] normValues = new float[3]；

// 計(jì)算角速度

float omegaMagnitude =

（float）Math.sqrt（gyroValues[0] * gyroValues[0] +

gyroValues[1] * gyroValues[1] +

gyroValues[2] * gyroValues[2]）；

if（omegaMagnitude > EPSILON） {

normValues[0] = gyroValues[0] / omegaMagnitude；

normValues[1] = gyroValues[1] / omegaMagnitude；

normValues[2] = gyroValues[2] / omegaMagnitude；}

float thetaOverTwo = omegaMagnitude * timeFactor；

float sinThetaOverTwo = （float）Math.sin（thetaOverTwo）；

float cosThetaOverTwo = （float）Math.cos（thetaOverTwo）；

deltaRotationVector[0] = sinThetaOverTwo * normValues[0]；

deltaRotationVector[1] = sinThetaOverTwo * normValues[1]；

deltaRotationVector[2] = sinThetaOverTwo * normValues[2]；

deltaRotationVector[3] = cosThetaOverTwo；}

上面這個(gè)函數(shù)可以計(jì)算出deltaRotationVector數(shù)組的值，這個(gè)值是一個(gè)旋轉(zhuǎn)向量，這個(gè)向量包含4個(gè)值。它是android設(shè)備中陀螺儀的旋轉(zhuǎn)狀態(tài)（從一個(gè)開始時(shí)間點(diǎn)狀態(tài)到一個(gè)結(jié)束時(shí)間點(diǎn)狀態(tài)的時(shí)間間隔）值（相對角度）。旋轉(zhuǎn)速度乘以最近的時(shí)間間隔（上面這個(gè)函數(shù)中的時(shí)間因子參數(shù)）。這個(gè)函數(shù)被用來在陀螺儀開始測量時(shí)的陀螺儀運(yùn)算函數(shù)中調(diào)用。

private static final float NS2S = 1.0f / 1000000000.0f；

private float timestamp；

private boolean initState = true；

public void gyroFunction（SensorEvent event） {

if （accMagOrientation == null）

return；

if（initState） {

float[] initMatrix = new float[9]；

initMatrix = getRotationMatrixFromOrientation（accMagOrientation）；

float[] test = new float[3]；

SensorManager.getOrientation（initMatrix， test）；

gyroMatrix = matrixMultiplication（gyroMatrix， initMatrix）；

initState = false；}

float[] deltaVector = new float[4]；

if（timestamp ！= 0） {

final float dT = （event.timestamp - timestamp） * NS2S；

System.arraycopy（event.values， 0， gyro， 0， 3）；

getRotationVectorFromGyro（gyro， deltaVector， dT / 2.0f）；}

timestamp = event.timestamp；

float[] deltaMatrix = new float[9]；

SensorManager.getRotationMatrixFromVector（deltaMatrix， deltaVector）；

gyroMatrix = matrixMultiplication（gyroMatrix， deltaMatrix）；

SensorManager.getOrientation（gyroMatrix， gyroOrientation）；}

當(dāng)加速感應(yīng)器和重力感應(yīng)器的方向角度數(shù)據(jù)產(chǎn)生之后（通過變量accMagOrientation來控制），陀螺儀的數(shù)據(jù)才能被處理。被處理之后的數(shù)據(jù)就可以作為陀螺儀的初始方向了。另外，函數(shù)中的方向矩陣也會(huì)包含很多未知值，android設(shè)備的當(dāng)前方向和被計(jì)算過的陀螺儀旋轉(zhuǎn)向量會(huì)被轉(zhuǎn)化成旋轉(zhuǎn)矩陣（gyroMatrix變量保存其值）。

gyroMatrix變量中包含了所有被處理過的陀螺儀的方向數(shù)據(jù)。deltaMatrix變量保存最新的旋轉(zhuǎn)時(shí)間間隔值，這個(gè)數(shù)據(jù)在gyroMatrix變量的后面計(jì)算中會(huì)用到，相當(dāng)于把gyroMatrix變量所表示的矩陣轉(zhuǎn)置。如下所示的函數(shù)中會(huì)調(diào)用matrixMultiplication函數(shù)，注意給這個(gè)函數(shù)的兩個(gè)形參賦值時(shí)，實(shí)參的次序不要顛倒了，因?yàn)檫@里涉及到兩個(gè)矩陣相乘（左乘和右乘是不一樣的）。調(diào)用getRotationMatrixFromVector函數(shù)可以把旋轉(zhuǎn)向量轉(zhuǎn)換成矩陣，代碼如下所示。

private float[] getRotationMatrixFromOrientation（float[] o） {

float[] xM = new float[9]；

float[] yM = new float[9]；

float[] zM = new float[9]；

float sinX = （float）Math.sin（o[1]）；

float cosX = （float）Math.cos（o[1]）；

float sinY = （float）Math.sin（o[2]）；

float cosY = （float）Math.cos（o[2]）；

float sinZ = （float）Math.sin（o[0]）；

float cosZ = （float）Math.cos（o[0]）；

xM[0] = 1.0f； xM[1] = 0.0f； xM[2] = 0.0f；

xM[3] = 0.0f； xM[4] = cosX； xM[5] = sinX；

xM[6] = 0.0f； xM[7] = -sinX； xM[8] = cosX；

yM[0] = cosY； yM[1] = 0.0f； yM[2] = sinY；

yM[3] = 0.0f； yM[4] = 1.0f； yM[5] = 0.0f；

yM[6] = -sinY； yM[7] = 0.0f； yM[8] = cosY；

zM[0] = cosZ； zM[1] = sinZ； zM[2] = 0.0f；

zM[3] = -sinZ； zM[4] = cosZ； zM[5] = 0.0f；

zM[6] = 0.0f； zM[7] = 0.0f； zM[8] = 1.0f；

float[] resultMatrix = matrixMultiplication（xM， yM）；

resultMatrix = matrixMultiplication（zM， resultMatrix）；

return resultMatrix；}

3 結(jié)束語

在使用互補(bǔ)濾波器時(shí)，為了更好的控制它的輸出，可以通過獨(dú)立的線程來處理濾波。傳感器信號(hào)的質(zhì)量高低很大程度上取決于其采樣頻率，也就是濾波函數(shù)在單位時(shí)間被調(diào)用的次數(shù)。這也就是把所有計(jì)算放在TimerTask類中，但是把關(guān)于取樣的時(shí)間間隔變量定義放在每次調(diào)用它的函數(shù)中。