基于三軸加速度傳感器的速度測(cè)量方法研究與實(shí)現(xiàn)*

張 敏， 姜秀柱， 陳 偉， 周軍娜

0 引 言

目前,針對(duì)中高速運(yùn)動(dòng)物體的速度測(cè)量方法很多：機(jī)械速度表法，結(jié)構(gòu)簡(jiǎn)單、安裝方便、無(wú)需能源且成本低，但其測(cè)量精度低、可靠性差、易受損；測(cè)速發(fā)電機(jī)法，采用電傳輸，安裝方便靈活、簡(jiǎn)單易行，但精度低，尤其表現(xiàn)在低速運(yùn)行時(shí)，且系統(tǒng)可靠性差；脈沖式轉(zhuǎn)速傳感器測(cè)速系統(tǒng)，通過(guò)與車(chē)輛軋輪同步旋轉(zhuǎn)的脈沖發(fā)生器產(chǎn)生與車(chē)輪轉(zhuǎn)速成正比的脈沖數(shù)，根據(jù)適當(dāng)比例對(duì)單位時(shí)間內(nèi)的脈沖數(shù)進(jìn)行計(jì)數(shù)和顯示獲取車(chē)輛運(yùn)行的速度[1]。文獻(xiàn)[2]研發(fā)了一種用于橋門(mén)式起重機(jī)的位置和速度測(cè)量的裝置，在機(jī)構(gòu)旋轉(zhuǎn)軸上增加技術(shù)齒輪，通過(guò)光電開(kāi)關(guān)感應(yīng)，產(chǎn)生脈沖信號(hào)的方式來(lái)測(cè)量起重機(jī)設(shè)備的速度和位置信息，方法結(jié)構(gòu)簡(jiǎn)單，但安裝繁瑣且局限性大[3]。

本文提出了利用加速度傳感器對(duì)低速大型機(jī)械的運(yùn)動(dòng)速度進(jìn)行實(shí)時(shí)測(cè)量的方法，無(wú)需改變大型機(jī)械自身結(jié)構(gòu)，且具有精度高，安裝簡(jiǎn)便，實(shí)用通用等特點(diǎn)。

1 系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

利用速度是加速度在時(shí)間上的積分思想設(shè)計(jì)了一種針對(duì)大型低速機(jī)械設(shè)備運(yùn)動(dòng)速度的測(cè)量系統(tǒng)?；驹砣鐖D1所示。

圖1 速度測(cè)量基本原理

1.1 重力補(bǔ)償[4]

當(dāng)傳感器靜止時(shí)，X軸、Y軸、Z軸分別有加速度值輸出，輸出值受重力影響，各軸合加速度與重力加速度gn相等。通常情況下，重力加速度補(bǔ)償公式為

(1)

式中 [axayaz]T為采集到的運(yùn)動(dòng)加速度；[x0y0z0]T為靜止時(shí)傳感器的輸出值。

1.2 速度計(jì)算方法

對(duì)運(yùn)動(dòng)物體加速度在時(shí)間上進(jìn)行積分，即可得到運(yùn)動(dòng)物體的運(yùn)動(dòng)速度。圖2為物體運(yùn)動(dòng)的加速度—時(shí)間曲線，橫軸為時(shí)間，縱軸為加速度(m/s2)，加速度值已經(jīng)過(guò)重力加速度補(bǔ)償。

圖2 加速度—時(shí)間曲線

假設(shè)從t0時(shí)刻開(kāi)始采樣，從t0～t時(shí)刻，連續(xù)時(shí)間域中速度v(t)和加速度a(t)的計(jì)算關(guān)系如式(2)所示

(2)

式中v(t0)為t0時(shí)刻的瞬時(shí)運(yùn)動(dòng)速度。

對(duì)式(2)進(jìn)行離散化，得到差分方程

(3)

根據(jù)數(shù)值分析原理，當(dāng)采樣時(shí)間間隔Δt足夠小時(shí)，可以近似地將曲線看作直線，采用梯形積分方法得到實(shí)際運(yùn)算的積分公式

(4)

又t1-t0=t2-t1=…=tn-tn-1=Δt，當(dāng)n>1時(shí)，可將式(4)改寫(xiě)為

a(2)+…+a(n-1)]Δt

(5)

根據(jù)式(5)，僅需知道v(0)和加速度傳感器的輸出加速度值便可以求出運(yùn)動(dòng)速度。但當(dāng)n特別大時(shí)，計(jì)算量將會(huì)很大，且會(huì)消耗大量的系統(tǒng)存儲(chǔ)資源。為了提高計(jì)算效率，采用迭代的方法進(jìn)行計(jì)算，僅需上一時(shí)刻的速度和加速度信息以及當(dāng)前的加速度信息，便可計(jì)算出當(dāng)前瞬時(shí)速度。遞推公式如式(6)所示

(6)

同理，三軸加速度傳感器輸出的信息是在三維空間上的加速度信息，僅需根據(jù)式(6)分別計(jì)算X，Y，Z軸方向上的瞬時(shí)速度

(7)

(8)

(9)

則t時(shí)刻運(yùn)動(dòng)體在三維空間的瞬時(shí)速度為

(10)

1.3 硬件架構(gòu)

系統(tǒng)組成如圖3 所示，主要包括2部分：1)速度監(jiān)測(cè)數(shù)據(jù)采集模塊(即數(shù)據(jù)采集節(jié)點(diǎn))[5]，安裝在運(yùn)動(dòng)物體上，采集加速度數(shù)據(jù)并發(fā)送給中心節(jié)點(diǎn)，包括一只三維加速度傳感器和一個(gè)無(wú)線發(fā)射裝置；2)無(wú)線接收裝置(即數(shù)據(jù)中心節(jié)點(diǎn))[5]，用于接收數(shù)據(jù)采集模塊傳送的數(shù)據(jù)，并通過(guò)RS—232串口通信電路將數(shù)據(jù)傳至上位機(jī)進(jìn)行數(shù)據(jù)分析及運(yùn)算，最終獲得物體運(yùn)動(dòng)速度并顯示輸出。

圖3 速度監(jiān)測(cè)系統(tǒng)組成

系統(tǒng)設(shè)計(jì)，三軸加速度傳感器采用ADI公司的ADXL345[6]三軸數(shù)字輸出iMEMS加速度傳感器，具有高分辨率、可調(diào)量程、高靈敏度、體積小、功耗小以及多種運(yùn)動(dòng)狀態(tài)檢測(cè)等特性。無(wú)線通信模塊采用TI/Chipcon公司推出的第二代支持2.4GHz IEEE 802.15.4/ZigBee協(xié)議[7]的片上系統(tǒng)(system-on-a-chip,SoC)芯片CC2530[8]，CC2530內(nèi)部集成了工業(yè)標(biāo)準(zhǔn)增強(qiáng)型的8051CPU、高性能RF收發(fā)器、256 kB Flash ROM,8 kB RAM以及低功耗等其他許多強(qiáng)大功能[9]。

2 數(shù)據(jù)處理

2.1 隨機(jī)誤差處理

采用卡爾曼濾波算法處理隨機(jī)誤差,基本思想為：采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和現(xiàn)時(shí)刻的觀測(cè)值更新對(duì)狀態(tài)變量的估計(jì)，求得現(xiàn)時(shí)刻的估計(jì)值[10]。適用于實(shí)時(shí)數(shù)據(jù)的處理與運(yùn)算[11]。

卡爾曼濾波器分為2個(gè)階段：預(yù)測(cè)和更新。預(yù)測(cè)階段用于推測(cè)當(dāng)前狀態(tài)的估計(jì)值和誤差協(xié)方差的估計(jì)值；更新階段用于推算當(dāng)前卡爾曼濾波器的增益值，根據(jù)預(yù)測(cè)階段得到的估計(jì)值推算當(dāng)前狀態(tài)的狀態(tài)值和誤差協(xié)方差。預(yù)測(cè)階段包含2個(gè)方程，更新階段包含3個(gè)方程，如圖4所示。

圖4 卡爾曼濾波算法流程

從圖4中可以觀察到，實(shí)際使用卡爾曼濾波時(shí)需要提供2個(gè)參數(shù)：測(cè)量噪聲協(xié)方差R和過(guò)程激勵(lì)噪聲協(xié)方差Q。理想狀態(tài)下，Q和R為常數(shù)，Kg將會(huì)快速收斂并保持為常數(shù)[11]。但實(shí)際上R很難保持不變，且Q也隨濾波器的狀態(tài)發(fā)生改變，即，每次迭代運(yùn)算中的Q和R均不同。每次迭代計(jì)算時(shí)，還要增加計(jì)算當(dāng)前R和Q。

本系統(tǒng)中R，Q值的最優(yōu)估計(jì)只與初值以及當(dāng)前值相關(guān)，按比例獲取信息，預(yù)先設(shè)定比例因子a1(0≤a1≤1)、a2(0≤a2≤1)和初值R(0)，Q(0)。R和Q的計(jì)算過(guò)程為

v(k)=Z(k)-H(k)X(k|k-1)

(11)

R(k)=(1-a1)R(0)+a1[V(k)VT(k)-

HP(k|k-1)HT]

(12)

P(k|k)-AP(k-1|k-1)AT-2Kg(k)R(k)·

(13)

2.2 累積誤差處理

理想情況下，當(dāng)加速度傳感器靜止時(shí)，傳感器的輸出為零。但實(shí)際上加速度傳感器靜止時(shí)的測(cè)量值并不等于零，這是因?yàn)榄h(huán)境(如溫度)的變化和傳感器自身的一些不可控因素使得加速度傳感器產(chǎn)生固有的噪聲和偏移。除了噪聲和偏移，被測(cè)機(jī)械的抖動(dòng)也會(huì)引起加速度傳感器的輸出有微小變化。如果未為對(duì)噪聲及變化進(jìn)行任何處理，而直接對(duì)加速度進(jìn)行積分，則誤差會(huì)被累積到速度結(jié)果之中，使計(jì)算結(jié)果出現(xiàn)不可預(yù)估的誤差。為了盡可能減少累積誤差，本文提出了一種自適應(yīng)補(bǔ)償?shù)姆椒ā?/p>

自適應(yīng)補(bǔ)償算法根據(jù)靜止階段速度與加速度為零的特點(diǎn)對(duì)零值誤差與累積誤差進(jìn)行修正。修正原理如圖5所示。

圖5 加速度修正原理

修正公式推導(dǎo)如下：

設(shè)a(k)為加速度傳感器采集的一段時(shí)間內(nèi)的加速度序列；0與k3為數(shù)據(jù)采集的起始點(diǎn)；k1與k2為運(yùn)動(dòng)的起止時(shí)間點(diǎn)。

根據(jù)式(6)可知

(14)

式中 Δt為采樣間隔。

由于0～k1時(shí)間段內(nèi)被測(cè)物體并未開(kāi)始運(yùn)動(dòng)，此時(shí)加速度可以看成整個(gè)運(yùn)動(dòng)過(guò)程中都包含的偏移量，將0～k1時(shí)間段內(nèi)加速度強(qiáng)制置零，可得運(yùn)動(dòng)初始狀態(tài)校正結(jié)果

(15)

對(duì)加速度修正后，積分得到的速度初值為零，因此，運(yùn)動(dòng)初始狀態(tài)無(wú)需對(duì)速度進(jìn)行修正。

運(yùn)動(dòng)結(jié)束時(shí)刻的速度v(k2)可以看成整個(gè)運(yùn)動(dòng)過(guò)程中的累積誤差，由累積誤差與時(shí)間成正比的特性，可得

(16)

(17)

運(yùn)動(dòng)的起止時(shí)間k1與k2的獲得需要有明確且精準(zhǔn)的判定。采用閾值來(lái)判定運(yùn)動(dòng)的開(kāi)始點(diǎn)和結(jié)束時(shí)間點(diǎn)，思想如下：

假設(shè)0～k1時(shí)間內(nèi)連續(xù)輸出的n個(gè)加速度為被測(cè)物準(zhǔn)備運(yùn)動(dòng)前靜止?fàn)顟B(tài)的輸出。此處，n是一個(gè)經(jīng)驗(yàn)值，本文設(shè)n為5。首先，取前n個(gè)輸出值中的最大幅值記作Amax(k)，設(shè)閾值為

TA=KAAmax(k)

(18)

式中KA為一個(gè)經(jīng)驗(yàn)值，在文本設(shè)為2。

若第n之后的加速度超過(guò)閾值TA，則為運(yùn)動(dòng)的開(kāi)始點(diǎn)k1；若未超過(guò)TA，而超過(guò)了最大幅值，則將此值記為最大幅值，相應(yīng)地閾值亦隨之改變，實(shí)現(xiàn)了自適應(yīng)。

本文所設(shè)計(jì)的速度無(wú)線測(cè)量系統(tǒng)主要用于大型器械的運(yùn)動(dòng)速度測(cè)量，一般運(yùn)動(dòng)距離不長(zhǎng)、運(yùn)動(dòng)速度不高，且不會(huì)出現(xiàn)勻高速運(yùn)動(dòng)，因此，運(yùn)動(dòng)過(guò)程中，當(dāng)有連續(xù)m個(gè)加速度幅值低于閾值時(shí)，即可認(rèn)為運(yùn)動(dòng)停止，連續(xù)m個(gè)值中的最小值對(duì)應(yīng)的時(shí)間點(diǎn)可看作運(yùn)動(dòng)停止時(shí)間k2。本文m為經(jīng)驗(yàn)值，設(shè)為5。

3 實(shí)驗(yàn)驗(yàn)證與分析

3.1 基于噪聲自適應(yīng)的卡爾曼濾波實(shí)驗(yàn)與分析

對(duì)水平運(yùn)動(dòng)狀態(tài)下的測(cè)試小車(chē)進(jìn)行加速度采集，并使用本文改進(jìn)的噪聲自適應(yīng)卡爾曼濾波對(duì)數(shù)據(jù)進(jìn)行處理，濾波結(jié)果如圖6。其中，圖6(a)為X軸數(shù)據(jù)濾波處理結(jié)果對(duì)比，圖6(b)為Y軸數(shù)據(jù)濾波處理結(jié)果對(duì)比，圖6(c)為Z軸數(shù)據(jù)濾波處理結(jié)果對(duì)比?？煽闯觯焊倪M(jìn)的噪聲自適應(yīng)卡爾曼算法能夠有效濾除數(shù)據(jù)中的噪聲干擾。

圖6 加速度濾波處理結(jié)果

圖7為本文改進(jìn)的算法與標(biāo)準(zhǔn)卡爾曼濾波(KF)的均方根誤差(root mean square error,RMSE)對(duì)比，均方根誤差對(duì)一組測(cè)量中的特大或特小誤差反映非常敏感，因此，能夠很好地反映出測(cè)量的精密度。由圖7可看出，濾波剛開(kāi)始階段，標(biāo)準(zhǔn)卡爾曼濾波算法誤差低，測(cè)量精度稍高，但隨著時(shí)間的增加，標(biāo)準(zhǔn)卡爾曼濾波的誤差越來(lái)越大，而本文改進(jìn)的噪聲自適應(yīng)卡爾曼濾波的誤差則一直很平穩(wěn)，并無(wú)增大趨勢(shì)，因此，本文改進(jìn)的濾波算法更加穩(wěn)定，更有效地濾除了運(yùn)動(dòng)中的抖動(dòng)及外界環(huán)境的干擾噪聲。

圖7 濾波誤差對(duì)比

表1為本文改進(jìn)的噪聲自適應(yīng)卡爾曼濾波算法與標(biāo)準(zhǔn)卡爾曼濾波算法耗時(shí)比較。

可以看出，隨著樣本集數(shù)據(jù)量的增大，與標(biāo)準(zhǔn)卡爾曼濾波算法相比，本文改進(jìn)的噪聲自適應(yīng)卡爾曼濾波算法時(shí)耗逐漸增大。因?yàn)闃?biāo)準(zhǔn)卡爾曼濾波計(jì)算過(guò)程中無(wú)需計(jì)算噪聲統(tǒng)計(jì)特性，而本文改進(jìn)的濾波算法每次都需要計(jì)算噪聲統(tǒng)計(jì)特性，但根據(jù)圖7可知，本文算法的濾波效果較標(biāo)準(zhǔn)卡爾曼濾波較好，精度高，并且可自適應(yīng)的調(diào)整測(cè)量噪聲。

表1 2種濾波算法耗時(shí)比較 s

3.2 自適應(yīng)零值誤差補(bǔ)償算法的實(shí)驗(yàn)與分析

采集到的初始加速度數(shù)據(jù)經(jīng)過(guò)去重力補(bǔ)償和濾波處理后，即可以進(jìn)入速度計(jì)算以及誤差補(bǔ)償處理階段。測(cè)量過(guò)程中，測(cè)試小車(chē)水平直線運(yùn)動(dòng)，理想情況下，Z軸輸出應(yīng)為零，但由于傳感器非常靈敏且路面不夠平滑，運(yùn)行過(guò)程中，Z軸方向也有數(shù)據(jù)輸出，且小車(chē)運(yùn)行時(shí)自身也會(huì)有抖動(dòng)，導(dǎo)致運(yùn)行前傳感器有數(shù)據(jù)輸出，而濾波器只能濾除隨機(jī)誤差無(wú)法去除抖動(dòng)。圖8為各分軸速度補(bǔ)償處理結(jié)果對(duì)比。結(jié)果表明：經(jīng)過(guò)累積誤差自適應(yīng)補(bǔ)償算法處理后，Z軸速度雖有輕微波動(dòng)，但基本處于零軸附近，較接近真實(shí)結(jié)果。

圖8 速度累積誤差補(bǔ)償結(jié)果對(duì)比

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

針對(duì)低速運(yùn)動(dòng)的大型機(jī)械，提出了一種基于ADXL345加速度傳感器的速度檢測(cè)方法并設(shè)計(jì)了速度測(cè)量系統(tǒng)，系

統(tǒng)引入了噪聲自適應(yīng)的卡爾曼濾波算法對(duì)傳感器數(shù)據(jù)降噪，使用零值誤差自適應(yīng)補(bǔ)償方法處理累積誤差，并設(shè)計(jì)程序?qū)崿F(xiàn)了算法。實(shí)驗(yàn)分析結(jié)果表明：經(jīng)本文改進(jìn)的濾波算法處理隨機(jī)誤差，并通過(guò)本文累積誤差補(bǔ)償算法計(jì)算得出的運(yùn)動(dòng)速度的精度可達(dá)到0.01 m/s，誤差為1 %～2 %。系統(tǒng)具有安裝靈活、算法簡(jiǎn)單、準(zhǔn)確性高的優(yōu)點(diǎn)。

參考文獻(xiàn):

[1] 韓先一.車(chē)輛運(yùn)行速度測(cè)量方法的研究[D].西安：西安理工大學(xué),2004.

[2] 張世璞.汽車(chē)速度測(cè)量方法研究[D].天津：天津理工大學(xué),2011.

[3] 蔣劍鋒,陸紀(jì)法,王漢煒，等.橋式起重機(jī)位置和速度測(cè)量裝置的研發(fā)及應(yīng)用[J].裝備制造技術(shù),2011，3(1):157-158.

[4] Tan C,Park S.Design of accelerometer-based inertial navigation systems[J].IEEE Transactions on Instrumentation and Measurement,2005,54(6):2520-2530.

[5] 鄧德玖.起重機(jī)能耗檢測(cè)無(wú)線傳感網(wǎng)數(shù)據(jù)中心節(jié)點(diǎn)的研究與設(shè)計(jì)[D].徐州 : 中國(guó)礦業(yè)大學(xué),2014.

[6] 袁 西，陳 棟，田 湘，等.三軸數(shù)字加速度計(jì)ADXL345及其在捷聯(lián)慣導(dǎo)中的應(yīng)用[J].電子設(shè)計(jì)工程,2010,18(3):138-140.

[7] 李 娜.基于ZigBee的無(wú)線傳感器網(wǎng)絡(luò)在工業(yè)監(jiān)測(cè)中的應(yīng)用[D].西安：長(zhǎng)安大學(xué)，2009.

[8] 蔡利婷，陳平華，羅 彬，等.基于CC2530的ZigBee數(shù)據(jù)采集系統(tǒng)的設(shè)計(jì)[J].計(jì)算機(jī)技術(shù)與發(fā)展，2012,11(2):197-200.

[9] 徐志國(guó)，陳正宇.基于CC2530的實(shí)時(shí)環(huán)境監(jiān)測(cè)系統(tǒng)設(shè)計(jì)[J].金陵科技學(xué)院學(xué)報(bào)，2014,30(2):23-28.

[10] 彭丁聰.卡爾曼濾波的基本原理及應(yīng)用[J].軟件導(dǎo)報(bào)，2009,11(8):32-34.

[11] 鄧自立.最優(yōu)估計(jì)理論及其應(yīng)用:建模、濾波、信息融合估計(jì)[M].哈爾濱:哈爾濱工業(yè)大學(xué)出版社,2005:51-56.