通過(guò)互補(bǔ)濾波器來(lái)實(shí)現(xiàn)的傳感器融合技術(shù)

王騰 牛椏楓

摘要：該文在分析了Android設(shè)備相關(guān)傳感器后，對(duì)其工作中會(huì)出現(xiàn)的問(wèn)題提出了新的方案，并通過(guò)圖表說(shuō)明了其工作方式，提出了通過(guò)互補(bǔ)濾波器來(lái)實(shí)現(xiàn)傳感器融合，只要對(duì)加速感應(yīng)器和磁場(chǎng)感應(yīng)器作類似低通濾波的處理，而對(duì)陀螺儀作類似高通濾波的處理，然后整合它們的結(jié)果數(shù)據(jù)，就可以得到相對(duì)精確的結(jié)果，且通過(guò)軟件的形式來(lái)實(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)用越來(lái)越普及，越來(lái)越多的人開(kāi)始使用智能手機(jī)，智能手機(jī)為我們的學(xué)習(xí)和生活帶來(lái)了太多的方便，也早就滲入人們的衣食住行，人們享受著它給我們帶來(lái)的快捷和方便，可是為什么智能手機(jī)如此強(qiáng)大呢？這跟里面的各種傳感器密切相關(guān)。當(dāng)今市場(chǎng)上，智能手機(jī)中，谷歌的android系統(tǒng)占據(jù)了很大一部分的市場(chǎng)，大多數(shù)的android設(shè)備都有內(nèi)置的測(cè)量運(yùn)動(dòng)、方向、和各種環(huán)境條件的傳感器。這些傳感器具有提供高精度和準(zhǔn)確度的原始數(shù)據(jù)的能力，可用于監(jiān)視設(shè)備在三維方向的移動(dòng)和位置、或者監(jiān)視設(shè)備周圍環(huán)境的變化。例如，一個(gè)游戲可能要從重力傳感器中讀取軌跡，以便推斷出復(fù)雜的用戶手勢(shì)和意圖，如傾斜、振動(dòng)、旋轉(zhuǎn)或擺動(dòng)等；同樣，有關(guān)天氣的應(yīng)用程序可能要使用設(shè)備的溫度傳感器和濕度傳感器來(lái)計(jì)算并報(bào)告露點(diǎn)；有關(guān)旅行的應(yīng)用程序可能要使用地磁場(chǎng)傳感器和加速度傳感器來(lái)報(bào)告羅盤方位。

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

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

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

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

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

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

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

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

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

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

fusedOrientation =

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

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

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

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

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

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

2.1初始化

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

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)聽(tīng)器

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

initListeners（）；}}

注意程序中使用的SensorEventListener接口，下面會(huì)使用2個(gè)函數(shù)onAccuracyChanged和onSensorChanged。這里的重點(diǎn)是onSensorChanged函數(shù)，它可以持續(xù)更新傳感器數(shù)據(jù)。傳感器監(jiān)聽(tīng)器的初始化通過(guò)下面的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)聽(tīng)器初始化完成后，如果有新的可用傳感器數(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ù)可以很方便的通過(guò)加速感應(yīng)器和磁場(chǎng)感應(yīng)器來(lái)獲取對(duì)地定向。

public void calculateAccMagOrientation（） {

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

SensorManager.getOrientation（rotationMatrix， accMagOrientation）；}}

如上所述，陀螺儀數(shù)據(jù)是不能直接用來(lái)做計(jì)算的，它需要進(jìn)行處理，其具體的處理方法在android幫助中已經(jīng)給出，這里就不再詳述，下面就是對(duì)這段代碼的引用，其中添加了一些參數(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è)開(kāi)始時(shí)間點(diǎn)狀態(tài)到一個(gè)結(jié)束時(shí)間點(diǎn)狀態(tài)的時(shí)間間隔）值（相對(duì)角度）。旋轉(zhuǎn)速度乘以最近的時(shí)間間隔（上面這個(gè)函數(shù)中的時(shí)間因子參數(shù)）。這個(gè)函數(shù)被用來(lái)在陀螺儀開(kāi)始測(cè)量時(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)生之后（通過(guò)變量accMagOrientation來(lái)控制），陀螺儀的數(shù)據(jù)才能被處理。被處理之后的數(shù)據(jù)就可以作為陀螺儀的初始方向了。另外，函數(shù)中的方向矩陣也會(huì)包含很多未知值，android設(shè)備的當(dāng)前方向和被計(jì)算過(guò)的陀螺儀旋轉(zhuǎn)向量會(huì)被轉(zhuǎn)化成旋轉(zhuǎn)矩陣（gyroMatrix變量保存其值）。

gyroMatrix變量中包含了所有被處理過(guò)的陀螺儀的方向數(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é)束語(yǔ)

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