獨(dú)立卡爾曼濾波器全加速度計(jì)復(fù)雜振動(dòng)角速度解算

汪偉，歹英杰，陳海龍，王平

（1.軍械工程學(xué)院，河北 石家莊 050003；2.防空兵學(xué)院，河南 鄭州 450052）

獨(dú)立卡爾曼濾波器全加速度計(jì)復(fù)雜振動(dòng)角速度解算

汪偉1，歹英杰2，陳海龍1，王平1

（1.軍械工程學(xué)院，河北 石家莊 050003；2.防空兵學(xué)院，河南 鄭州 450052）

對(duì)于運(yùn)動(dòng)體的多自由度振動(dòng)測(cè)試，準(zhǔn)確的振動(dòng)角速度是復(fù)雜振動(dòng)參數(shù)解算的關(guān)鍵。針對(duì)目前解算角速度單一方法測(cè)試精度不理想的問(wèn)題，分析由4個(gè)三軸加速度計(jì)組成傳感器陣列，測(cè)試角速度解算算法，設(shè)計(jì)一種獨(dú)立的卡爾曼濾波器用于角速度解算。通過(guò)仿真及實(shí)驗(yàn)證明：該方法解算結(jié)果的誤差最大值和標(biāo)準(zhǔn)差相對(duì)于單一方法可提高1個(gè)數(shù)量級(jí)，其得到的角速度幅值準(zhǔn)確性和曲線平滑度也優(yōu)于其他單一解算方法。

加速度計(jì)；角速度；卡爾曼濾波;復(fù)雜振動(dòng)

0 引言

振動(dòng)測(cè)試對(duì)產(chǎn)品的可靠性、工況分析以及機(jī)械故障診斷等都有著重要的參考意義，測(cè)試車輛、艦船、飛機(jī)等運(yùn)動(dòng)體的振動(dòng)狀態(tài)日益受到人們關(guān)注。針對(duì)運(yùn)動(dòng)體6個(gè)自由度運(yùn)動(dòng)同時(shí)進(jìn)行測(cè)試，測(cè)試方法分為接觸式和非接觸式兩種。接觸式測(cè)量可采用3個(gè)陀螺儀和3個(gè)加速度計(jì)同時(shí)測(cè)量3個(gè)方向角速度和加速度，此方法缺點(diǎn)在于陀螺儀動(dòng)態(tài)響應(yīng)范圍小，價(jià)格昂貴且容易損壞。普通的單自由度多點(diǎn)測(cè)量，存在安裝繁瑣、布線復(fù)雜、多個(gè)傳感器之間相對(duì)位置測(cè)量和同步測(cè)試?yán)щy等問(wèn)題[1]。非接觸式測(cè)量多采用多目CCD、光學(xué)經(jīng)緯儀等測(cè)試技術(shù)，相比于接觸式測(cè)量，此類方法具有對(duì)被測(cè)物運(yùn)動(dòng)無(wú)干擾、測(cè)試精度高等優(yōu)點(diǎn)；缺點(diǎn)在于測(cè)試設(shè)備復(fù)雜、操作困難、數(shù)據(jù)處理量大、動(dòng)態(tài)測(cè)試范圍小，對(duì)于運(yùn)動(dòng)到視場(chǎng)以外的目標(biāo)無(wú)法測(cè)量[2-3]。借鑒無(wú)陀螺捷聯(lián)慣性導(dǎo)航的相關(guān)技術(shù)，將傳感器陣列測(cè)試技術(shù)運(yùn)用到振動(dòng)環(huán)境中，可以有效地對(duì)較高頻率下多自由度線振動(dòng)與角振動(dòng)耦合效果進(jìn)行絕對(duì)測(cè)量[4]，具有成本低、反應(yīng)快、可靠性高等諸多優(yōu)點(diǎn)，在工程上具有較廣闊的發(fā)展前景。本文介紹傳感器陣列測(cè)試技術(shù)，分析常用角速度解法優(yōu)缺點(diǎn)，設(shè)計(jì)一種獨(dú)立的卡爾曼濾波器，運(yùn)用到角速度解算中，在不需要外部觀測(cè)的情況下提高角速度解算精度。

1 全加速度計(jì)解算運(yùn)動(dòng)參數(shù)模型

1.1 空間任意點(diǎn)加速度

定義OnXnYnZn為慣性系，ObXbYbZb為載體系，如圖1所示。

圖1 坐標(biāo)系關(guān)系圖

根據(jù)運(yùn)動(dòng)學(xué)知識(shí)可以得到載體系上任意一點(diǎn)i位置的加速度值[5]為

Lb——i點(diǎn)在載體坐標(biāo)系中的位置矢量；

θ——i點(diǎn)傳感器在載體坐標(biāo)系中敏感方向向量；

1.2 4個(gè)三軸加速度計(jì)配置方式

現(xiàn)有研究常用的有六加速度計(jì)、九加速度計(jì)、十二加速度計(jì)以及一些其他構(gòu)型的傳感器陣列[5-6]。本文使用的是4個(gè)三軸加速度傳感器組成陣列的方案，不僅減少傳感器陣列中傳感器的使用個(gè)數(shù)，而且保證安裝在同一坐標(biāo)軸的敏感方向的正交性，在狹小的空間中也便于安裝[7]。加速度計(jì)配置方式如圖2所示，其中A1～A4為傳感器安裝點(diǎn)位置，a1x～a4z方向?yàn)閭鞲衅鲗?duì)應(yīng)的敏感軸安裝方向。

4個(gè)三軸加速度計(jì)組成的十二加速度傳感器陣列的具體安裝位置和方向如表1所示。

圖2 加速度計(jì)陣列構(gòu)型

表1 十二加速度計(jì)安裝位置方向

1.3 振動(dòng)參數(shù)的解算

剛體的運(yùn)動(dòng)可以用3個(gè)自由度的線運(yùn)動(dòng)和3個(gè)自由度的角運(yùn)動(dòng)來(lái)描述[8]。用a1x，a1y，a1z，…，a4z表示傳感器測(cè)試得到的加速度值，將L，θ代入（1）式可以解算得到剛體運(yùn)動(dòng)參數(shù)與測(cè)試加速度值關(guān)系式為

可以看出，根據(jù)加速度計(jì)值可以得到傳感器陣列繞載體坐標(biāo)系的角加速度項(xiàng)和線加速度項(xiàng)。由角加速度積分等方法得到角速度后，將角速度代入姿態(tài)更新方程中，可以得到陣列任意時(shí)刻姿態(tài)，再由姿態(tài)信息和線加速度計(jì)算得到位置信息[8]。

2 角速度解算

2．1角速度解算算法

根據(jù)加速度計(jì)構(gòu)型不同，由參數(shù)解算方程可以得到式（2）、式（3）、式（4）中角速度相關(guān)項(xiàng)的一個(gè)或多個(gè)表達(dá)式。根據(jù)角速度相關(guān)項(xiàng)解算式解算角速度主要有以下3種算法：

1）積分法。對(duì)角加速度項(xiàng)式（3）直接積分，可以得到繞載體坐標(biāo)軸運(yùn)動(dòng)角速度表達(dá)式為

式中：t——采樣時(shí)刻；

T——采樣周期；

ω（t-T）——上一時(shí)刻積分得到的角速度。

2）開方法。對(duì)角速度平方項(xiàng)式（4）開方得到角速度的絕對(duì)值，再由通過(guò)式（6）積分得到的角速度確定角速度正負(fù)符號(hào)，即：

3）微分法。根據(jù)微分方程定義，可知

展開經(jīng)簡(jiǎn)單變換后得到角速度

此方法也可以利用角速度交叉乘積項(xiàng)獲得。

2．2角速度解算誤差分析

由于加速度計(jì)實(shí)際安裝過(guò)程中，安裝位置和方向不可能和理論位置完全重合，存在安裝方向誤差和安裝位置誤差；其中，安裝位置誤差包括三軸加速度計(jì)芯片內(nèi)部的敏感軸位置誤差和芯片整體安裝誤差[9]。在加速度計(jì)安裝、零位電壓和靈敏度等參數(shù)標(biāo)定存在一定誤差時(shí)，利用加速度計(jì)測(cè)試得到的加速度值必然存在測(cè)量誤差。設(shè)傳感器理論測(cè)量值為A，測(cè)量誤差為ΔA，則加速度計(jì)實(shí)際測(cè)量值為

根據(jù)實(shí)際測(cè)量值解算角速度時(shí)，假設(shè)角加速度項(xiàng)解算誤差為Δω˙（t），則Δω˙（t）可表示為

利用積分法解算角速度時(shí)，角速度表達(dá)式為

由式（13）可以看出，積分法角速度每一時(shí)刻的誤差都會(huì)帶到下一時(shí)刻角速度計(jì)算中，當(dāng)角加速度誤差為非零均值時(shí)，式（13）得到的角速度會(huì)出現(xiàn)積分誤差，且隨測(cè)試時(shí)間增加誤差迅速增大[8]。

利用開方法解算角速度時(shí)，角速度表達(dá)式為

其中Δω2（t）表達(dá)式可由式（4）得到。利用式（14）得到的角速度不存在積分誤差問(wèn)題，但隨著測(cè)試時(shí)間增加，由積分法得到的角速度符號(hào)可能越來(lái)越不準(zhǔn)確，在角速度較小時(shí)可能出現(xiàn)符號(hào)誤判。

利用微分法解算角速度時(shí)，角速度表達(dá)式為

可以看出微分法求解角速度同樣不存在累積誤差，避免開方過(guò)程，不需要判斷符號(hào)。利用相鄰時(shí)刻差值解算，還可以消減一部分加速度計(jì)常值誤差，但在分母較小時(shí)會(huì)出現(xiàn)粗大誤差的情況[7]。

3 基于獨(dú)立卡爾曼濾波器的角速度解算

綜上可知，運(yùn)用單一積分法、開方法或微分法求解振動(dòng)角速度都有不可避免的缺陷：積分法得到的角速度誤差會(huì)累積發(fā)散，但誤差波動(dòng)較??；開方法判斷符號(hào)受限于積分法角速度精度；微分法得到的角速度誤差不會(huì)累積發(fā)散，但相對(duì)于積分法，其誤差波動(dòng)較大。雖然可以通過(guò)誤差補(bǔ)償方式，減少加速度計(jì)的測(cè)量誤差，但不可能完全消除測(cè)量誤差對(duì)角速度解算結(jié)果的影響。因此，提出利用積分法得到的角速度作為狀態(tài)變量，微分法得到的角速度作為觀測(cè)值，設(shè)計(jì)一種獨(dú)立的卡爾曼濾波器。

與從諸多信號(hào)中提取所需要信號(hào)的常規(guī)濾波有著本質(zhì)的區(qū)別，卡爾曼濾波器通過(guò)一系列遞歸數(shù)學(xué)公式描述，從被提取信號(hào)相關(guān)量測(cè)量中通過(guò)算法估計(jì)出所需的信號(hào)[10]。因此，獨(dú)立卡爾曼濾波器適用于振動(dòng)測(cè)試的隨機(jī)信號(hào)處理，而不需提前確定振動(dòng)信號(hào)變化規(guī)律和其他角速度測(cè)量設(shè)備。

3．1系統(tǒng)狀態(tài)方程建立

根據(jù)積分法求解角速度的算法，角速度Xk可以表示為

其中Xk=[ωxkωykωzk]T為繞三軸轉(zhuǎn)動(dòng)角速度，建立系統(tǒng)狀態(tài)方程為

式中Wk為未知系統(tǒng)噪聲，其協(xié)方差矩陣為Q；uk-1為狀態(tài)轉(zhuǎn)換控制項(xiàng)，表達(dá)式為

3．2觀測(cè)方程建立

根據(jù)開方法求解角速度法建立系統(tǒng)觀測(cè)方程：

式中Vk為觀測(cè)噪聲，其協(xié)方差矩陣為R。

3．3狀態(tài)更新方程

在得到k-1時(shí)刻角速度最優(yōu)估值后，其更新算法流程[11]如圖3所示。

圖3 卡爾曼濾波算法流程

圖4 4種算法角速度及誤差曲線

1）狀態(tài)一步預(yù)估方程：

2）一步預(yù)測(cè)均方差：

3）濾波增益為

4）由觀測(cè)變量更新狀態(tài)變量：

5）更新估計(jì)均方差：

4 仿真驗(yàn)證

采用如下模型進(jìn)行仿真：仿真采樣頻率10kHz，采樣時(shí)間10s；不考慮加速度計(jì)安裝誤差，加速度傳感器常值誤差0.0001g，隨機(jī)噪聲的均方差為0.001g，安裝距離0.04m。角速度參數(shù)設(shè)置為

理論角速度，積分法、開方法、微分法和卡爾曼濾波算法4種算法計(jì)算角速度ωz及相應(yīng)誤差如圖4所示。

由圖可知，積分法角速度誤差隨時(shí)間增長(zhǎng)不斷增大。開方法角速度誤差收斂，角速度值在0附近時(shí)誤差較大，當(dāng)角速度＞0且符號(hào)判斷正確時(shí)，誤差較小。微分法在角速度變化平緩時(shí)，容易造成粗大誤差，需要剔除，角速度變化較大時(shí)誤差較小。設(shè)計(jì)獨(dú)立的卡爾曼濾波器，根據(jù)積分法建立系統(tǒng)狀態(tài)方程，得到前一時(shí)刻的最優(yōu)估計(jì)值后，利用加速度值作為狀態(tài)轉(zhuǎn)換的控制項(xiàng)進(jìn)行一步預(yù)估，利用微分法得到的角速度誤差不發(fā)散的特點(diǎn)作為系統(tǒng)觀測(cè)值，對(duì)角速度值進(jìn)行修正。表2為4種算法得到的角速度誤差統(tǒng)計(jì)。

表2 角速度解算誤差統(tǒng)計(jì)rad/s

可以看出，卡爾曼濾波算法精度要高于其他單一解算精度，在誤差最大值和標(biāo)準(zhǔn)差方面準(zhǔn)確度提高了1個(gè)數(shù)量級(jí)。因此可以假設(shè)此方法可以用于振動(dòng)測(cè)試中被測(cè)物振動(dòng)角速度解算。

5 實(shí)驗(yàn)驗(yàn)證

在角振動(dòng)臺(tái)上進(jìn)行角振動(dòng)實(shí)驗(yàn)，傳感器陣列坐標(biāo)原點(diǎn)偏離旋轉(zhuǎn)中心一定距離，安裝位置示意圖如圖5所示。信號(hào)采集頻率1 kHz，時(shí)間20 s。振動(dòng)臺(tái)以幅值30°、頻率2Hz的正弦模式進(jìn)行角轉(zhuǎn)動(dòng)。

圖6 實(shí)驗(yàn)角速度解算曲線

測(cè)試得到12組加速度計(jì)輸出后，分別用積分法、開方法、微分法和卡爾曼濾波算法解算載體轉(zhuǎn)動(dòng)角速度，結(jié)果如圖6所示。

由圖可知，積分法解算得到的角速度，隨時(shí)間增長(zhǎng)，按照一定趨勢(shì)不斷增大，偏離了實(shí)際結(jié)果。由于積分角速度已經(jīng)偏離實(shí)際結(jié)果，造成開方法角速度的符號(hào)誤判，解算得到的角速度也偏離了實(shí)際結(jié)果。微分法解算得到的角速度在振動(dòng)開始前，有很大誤差，在振動(dòng)開始后，誤差變?。坏诮撬俣茸兓^小時(shí)，其誤差相對(duì)來(lái)說(shuō)也很大?？柭鼮V波算法得到的角速度，在振動(dòng)臺(tái)轉(zhuǎn)動(dòng)之前，由于微分法角速度誤差較大，卡爾曼濾波算法得到的角速度誤差較大，振動(dòng)臺(tái)轉(zhuǎn)動(dòng)以后，測(cè)試效果要優(yōu)于其他單一角速度解算算法。在利用開方法解算角速度時(shí)，對(duì)于出現(xiàn)的異常值可以通過(guò)預(yù)先判斷進(jìn)行剔除，提高卡爾曼濾波算法角速度解算準(zhǔn)確度。

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

借鑒無(wú)陀螺捷聯(lián)慣導(dǎo)知識(shí)，將加速度傳感器陣列技術(shù)用于運(yùn)動(dòng)體復(fù)雜振動(dòng)測(cè)試。相比慣性制導(dǎo)，對(duì)計(jì)算實(shí)時(shí)性要求低，因此數(shù)據(jù)處理方法也更加靈活。本文介紹了傳感器陣列的排布構(gòu)型和振動(dòng)測(cè)試參數(shù)解算的方法，分析3種角速度解算常用方法的優(yōu)缺點(diǎn)后，利用積分法和微分法分別作為系統(tǒng)狀態(tài)方程和觀測(cè)方程，設(shè)計(jì)一種不需要外部觀測(cè)的獨(dú)立卡爾曼濾波器。通過(guò)仿真證明，此方法應(yīng)用于振動(dòng)角速度解算中，其解算結(jié)果的誤差最大值和標(biāo)準(zhǔn)差相對(duì)于單一方法可提高1個(gè)數(shù)量級(jí)；通過(guò)實(shí)驗(yàn)也證明該方法得到的角速度幅值準(zhǔn)確性和曲線平滑度優(yōu)于其他單一解算方法。

[1]張新，費(fèi)業(yè)泰.多維加速度傳感器的設(shè)計(jì)及優(yōu)化[J].中國(guó)測(cè)試，2007，33（3）：133-135.

[2]王宏亮，周浩強(qiáng)，高宏，等.基于雙等強(qiáng)度懸臂梁的光柵加速度振動(dòng)傳感器[J].光電子·激光，2013，24（3）：635-637.

[3]羅波，李偉鵬，黃海.基于Stewart平臺(tái)的大柔性空間桁架結(jié)構(gòu)振動(dòng)控制[J].振動(dòng)與沖擊，2012，31（23）：147-149.

[4]PARK S，WOO T C，PARKA J.A scheme for improving the performance of a gyroscope-free inertialmeasurement unit[J].Sensors and Actuators，2005（121）：410-420.

[5]史震，于秀萍.無(wú)陀螺捷聯(lián)式慣性導(dǎo)航系統(tǒng)[M].哈爾濱：哈爾濱工程大學(xué)出版社，2007：59-78.

[6]張會(huì)新，王世昌，楊運(yùn)良，等.全加速度計(jì)慣性測(cè)量系統(tǒng)角速度解算方法的優(yōu)化[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2008，16（6）：672-675.

[7]焦建超，唐力偉，汪偉，等.六自由度振動(dòng)測(cè)試新方案[J].新技術(shù)新工藝，2011（9）：67-69.

[8]尤晶晶，李成剛，吳洪濤.基于并聯(lián)機(jī)構(gòu)的六維加速度傳感器的方案設(shè)計(jì)及建模研究[J].振動(dòng)工程學(xué)報(bào)，2012，25（6）：658-665.

[9]LEE S C，CHENG Y L.Innovative estimation method with own-ship estimator for an accelerometer type inertial navigation system[J].International J of Systems Science，1999，30（12）：1259-1266.

[10]王璐，潘紫微，葉金杰.基于EKF訓(xùn)練的RBF神經(jīng)網(wǎng)絡(luò)及其故障診斷應(yīng)用[J].振動(dòng)、測(cè)試與診斷，2008，28（4）：358-361.

[11]顏翚，葛彤，楊柯，等.水下攻泥器隨鉆姿態(tài)慣性測(cè)量方法[J].上海交通大學(xué)學(xué)報(bào)，2012，46（3）：447-449.

（編輯：李剛）

App lication of Kalman filter to calculate angular velocity of com plicated vibration by full acceleration sensor

WANG Wei1，DAI Yingjie2，CHEN Hailong1，WANG Ping1
（1.Ordnance Engineering College，Shijiazhuang 050003，China；2.AIR Defense Forces Academy，Zhengzhou 450052，China）

For multi-freedom vibration test，the key of complicated vibration test is calculating the angular velocity.However，single method to calculate the angular velocity is not very ideal. Analyzing the angular velocity calculating algorithms by 4 tri-axial accelerometers，this paper puts forward Kalman filter to raise the accuracy of angular velocity.The simulation and experimental results show that this method is better and more accurate.

acceleration sensor；angular velocity；Kalman filter；complicated vibration

A

1674-5124（2015）05-0105-05

10.11857/j.issn.1674-5124.2017.05.022

2016-07-15；

2016-08-13

汪偉（1963-），男，教授，博士生導(dǎo)師，主要從事測(cè)試技術(shù)理論研究工作。