雙加計(jì)與MEMS速率陀螺儀融合的滾轉(zhuǎn)角測量算法

王可寧，張雄星,田建平

(西安工業(yè)大學(xué) 光電工程學(xué)院,西安 710021)

?

雙加計(jì)與MEMS速率陀螺儀融合的滾轉(zhuǎn)角測量算法

王可寧，張雄星,田建平

(西安工業(yè)大學(xué) 光電工程學(xué)院,西安 710021)

為了解決高旋彈丸滾轉(zhuǎn)角測量精度的問題.采用Kalman濾波對加速度計(jì)法和微機(jī)電速率陀螺進(jìn)行數(shù)據(jù)融合,利用雙加速度計(jì)測量彈丸的轉(zhuǎn)角,對加速度計(jì)進(jìn)行低通濾波,得到加速度計(jì)的離心加速度,通過補(bǔ)償離心加速度得到重力加速度的投影,建立Kalman濾波的觀測方程；通過微機(jī)電陀螺儀得到彈丸的轉(zhuǎn)角速率,建立了Kalman濾波的狀態(tài)方程.在計(jì)算機(jī)通過對彈體滾轉(zhuǎn)角進(jìn)行Kalman濾波仿真.仿真結(jié)果表明：轉(zhuǎn)速測量存在系統(tǒng)誤差(偏置)和隨機(jī)噪聲，且雙加速度計(jì)轉(zhuǎn)角的測量誤差均值為20°,經(jīng)過Kalman濾波最后誤差收斂在3°,且不存在累積誤差.

Kalman；雙加計(jì)；滾轉(zhuǎn)角；陀螺儀

彈道修正是在彈丸飛行過程中對彈道進(jìn)行修正、制導(dǎo)的一種技術(shù),是常規(guī)化彈藥修正、制導(dǎo)的重要研究發(fā)展方向[1],其中彈體姿態(tài)實(shí)時測量是彈道修正的基礎(chǔ),特別是彈體的滾轉(zhuǎn)角的測量是亟待解決的關(guān)鍵問題.目前,國內(nèi)外彈體滾轉(zhuǎn)角測量方法很多,但是這些測量方法均具有局限性.文獻(xiàn)[2]利用太陽方位角法,這是一種常用的滾轉(zhuǎn)角測量方法,其定義為彈丸軸線與陽光矢量到彈丸質(zhì)心聯(lián)線的夾角,該角度的變化反應(yīng)了彈丸在飛行中滾轉(zhuǎn)姿態(tài)的變化,其原理簡單,但易受環(huán)境干擾,成本較高.采用框架式機(jī)械陀螺法測量滾轉(zhuǎn)角,不存在積累誤差,但體積大,抗過載能力弱,不能承載火炮發(fā)射過載.文獻(xiàn)[3-4]采用地磁測量法,以地磁場矢量為天然的參考方向,通過測量地磁場在彈體系的投影來計(jì)算滾轉(zhuǎn)角.地磁場傳感器基本不存在累積誤差,與飛行器自身的加速度無關(guān),只與當(dāng)?shù)氐卮糯艌鰪?qiáng)度有關(guān),其是一種結(jié)構(gòu)簡單、抗過載能力強(qiáng)的姿態(tài)測試傳感器.當(dāng)彈軸與地磁場矢量方向接近時,測量存在很大誤差,所以地磁測量法在實(shí)際應(yīng)用存在局限性；文獻(xiàn)[5]采用加速度傳感器進(jìn)行慣性測量,通過測量重力加速度在彈體系的投影來計(jì)算彈體姿態(tài),這類方法抗過載能力強(qiáng),但容易受到離心力和彈體橫向加速度的影響,精度有待提高.文獻(xiàn)[6]采用雙加速度計(jì)法,根據(jù)雙加速度傳感器提取彈丸外道橫向加速度采樣信號的功率譜,結(jié)合數(shù)字信號處理器(Digitial Signal Processor,DSP)來濾波,但是未能解決雙加計(jì)的安裝誤差；文獻(xiàn)[7]針對高速旋轉(zhuǎn)彈在捷聯(lián)慣性導(dǎo)航系統(tǒng)中采用三軸加速度計(jì)法,此方法減小了安裝誤差但是測量的滾轉(zhuǎn)角速度不高.文獻(xiàn)[8]利用六加速度計(jì)法,有效地解決了安裝誤差,但成本高,算法復(fù)雜,電路尺寸大限制了它在小型智能彈上的運(yùn)用.綜上所述，太陽方位角法、地磁測量法和加速度計(jì)法等方法均存在一定的缺陷，不能滿足轉(zhuǎn)角測量的要求.

轉(zhuǎn)角測量的趨勢是多傳感器的融合,如磁阻傳感器與加速度計(jì)融合[9]、磁強(qiáng)計(jì)/太陽方位角傳感器的組合測量方法[10]及雙軸磁強(qiáng)計(jì)與GPS的融合[11]等方法.本文采用雙加計(jì)法和微機(jī)電(Micro Electro Mechanical Systems,MEMS)速率陀螺進(jìn)行融合.測量重力加速度在彈體系Y和Z軸上的投影,計(jì)算兩路加計(jì)信號的均值得到加計(jì)的離心加速度,從而對加計(jì)的安裝誤差進(jìn)行補(bǔ)償.同時采用MEMS速率陀螺儀測量彈體滾轉(zhuǎn)角速度,使用Kalman濾波對加計(jì)和MEMS速率陀螺進(jìn)行融合,并結(jié)合彈道仿真數(shù)據(jù)對滾轉(zhuǎn)測量方法進(jìn)行了檢驗(yàn),提高了滾轉(zhuǎn)角測量精度.

1 雙加計(jì)轉(zhuǎn)角測量的原理

在彈截面上安裝兩個加速度計(jì),加速度計(jì)B和加速度計(jì)A的敏感軸分別是彈體系Y軸和Z軸重合,如圖1所示.

圖1 雙加速度計(jì)

加速度計(jì)的輸出包含重力加速度的投影、離心加速度和平動加速度,兩個加速度計(jì)的輸出a1和a2的表達(dá)式為

(1)

式中：r1為A加速度傳感器到軸心O的距離；r2為B加速度傳感器到軸心O的距離；g為重力加速度；θ為俯仰角；φ為滾轉(zhuǎn)角；wx、wy和wz分別為滾轉(zhuǎn)角速度、偏航角速度和俯仰角速度；ay和az分別為平動加速度在彈體系Y軸方向和Z軸方向的投影.

根據(jù)測量數(shù)據(jù)仿真,得出當(dāng)彈高速滾轉(zhuǎn)時,離心加速度的變化緩慢,而重力加速度投影的頻率和滾轉(zhuǎn)頻率相同,且均值為零.對加速度計(jì)進(jìn)行低通濾波,可以得到離心加速度.離心加速度對加速度計(jì)測量值進(jìn)行補(bǔ)償，得到重力加速度的投影,如圖2所示.

圖2 雙加速度測量轉(zhuǎn)速原理圖

根據(jù)上述原理得出

(2)

式中：aavg1和aavg2分別為兩個加速度計(jì)輸出的離心加速度.

由式(1)和式(2)可以解出滾轉(zhuǎn)角φ,計(jì)算表達(dá)式為

(3)

其中φ為通過加速度計(jì)測量轉(zhuǎn)角的輸出值.

2 雙加計(jì)和MEMS陀螺的數(shù)據(jù)融合算法

轉(zhuǎn)速測量比轉(zhuǎn)角測量的精度高,但是使用轉(zhuǎn)速積分計(jì)算轉(zhuǎn)角時存在累計(jì)誤差.轉(zhuǎn)角測量精度受平動加速度影響較大,但是不存在累計(jì)誤差.因此將采用Kalman濾波算法雙加計(jì)的轉(zhuǎn)角測量值和MEMS速率陀螺的轉(zhuǎn)速測量值進(jìn)行融合.在Kalman濾波方程中,系統(tǒng)狀態(tài)方程表達(dá)式為

φ(k)=φ(k-1)+wx(k)·Δt+W(k)

(4)

式中：φ(k)為k時刻的轉(zhuǎn)角(雙加計(jì)的測量值)；wx(k)為k時刻的滾轉(zhuǎn)角頻率(MEMS速率陀螺的測量值)；Δt為觀測時間間隔；W(k)為系統(tǒng)過程的噪聲.該方程描述了轉(zhuǎn)角自身的變化規(guī)律.因?yàn)檗D(zhuǎn)速測量的精度較高,所以狀態(tài)方程具有較小的協(xié)方差R,其是由微機(jī)電速率陀螺測量轉(zhuǎn)速誤差引起.采用的微機(jī)電速率陀螺的誤差是?ω=2(°)·s-1.離散時間Δt為1 ms,那么經(jīng)計(jì)算狀態(tài)協(xié)方差表達(dá)式為

R=(?ω×Δt)2=0.4×10-5

(5)

加速度計(jì)系統(tǒng)的轉(zhuǎn)角測量值為觀測值,觀測測量方程表達(dá)式為

φ(k)=φ(k-1)+V(k)

(6)

(7)

(8)

其中?φ為測量角度誤差,但在實(shí)際中加速度計(jì)的測量角度誤差不大于0.1°,即得Q=(?φ)2=(0.1)2=0.01.

確定了狀態(tài)方程、觀測方程以及兩個方程的方差,再結(jié)合雙加計(jì)的轉(zhuǎn)角測量值和MEMS速率陀螺的轉(zhuǎn)速測量值,使用Kalman濾波算法對轉(zhuǎn)角進(jìn)行濾波.

3 仿真結(jié)果及分析

3.1 彈體姿態(tài)仿真分析

在上位機(jī)中進(jìn)行算法仿真 ,仿真條件：轉(zhuǎn)速為10 r·s-1,彈體仰角為0°,偏航角振動幅值為5°,頻率為2 Hz .彈體平動的加速度a取30 m·s-2,方向沿角振動的中心方向.兩個加計(jì)測量引入的高斯白噪聲是2 m·s-2誤差,滾轉(zhuǎn)陀螺的測量誤差為2(°)·s-1.

綜上，仿真得到兩個加速度計(jì)的輸出加速度值隨時間的變化曲線,如圖3所示,兩路信號為兩路加速度計(jì)輸出的原始信號波形,其相位差為90°.從圖3可以看出,仿真得到的兩路波形存在噪聲,其噪聲產(chǎn)生原因是器件自身產(chǎn)生噪聲以及受到平動加速度的影響.

圖3 兩路加計(jì)的輸出曲線圖

3.2 雙加速度計(jì)轉(zhuǎn)角測量誤差仿真分析

在仿真環(huán)境中建立雙加速度計(jì)轉(zhuǎn)角的測量.通過對兩個重力加速度的投影進(jìn)行反正切運(yùn)算，從而得到彈體的滾轉(zhuǎn)角.在測量過程中,存在轉(zhuǎn)角測量誤差，該誤差是通過雙加速度計(jì)測量的計(jì)算值與實(shí)際測量值作差得到的值.所測得的轉(zhuǎn)角誤差曲線如圖4所示,加速度計(jì)轉(zhuǎn)角測量誤差最大值超過35°,其期望均值約為20°.分析誤差產(chǎn)生原因是由于彈體受到平動加速度影響,現(xiàn)需采用進(jìn)一步的算法處理,降低該誤差對系統(tǒng)測量的影響.

3.3 Kalman濾波的轉(zhuǎn)角測量誤差仿真分析

為了解決雙加速度計(jì)轉(zhuǎn)角的測量中引入的轉(zhuǎn)角測量誤差,系統(tǒng)采用Kalman濾波算法,仿真后得到的轉(zhuǎn)角測量誤差曲線如圖5所示.

圖4 轉(zhuǎn)角測量誤差曲線圖

圖5 Kalman濾波融合的轉(zhuǎn)角誤差

從圖5可以看出，轉(zhuǎn)角測量誤差在0.25 s后收斂,且收斂值為3°,滾轉(zhuǎn)陀螺的測量誤差為2(°)·s-1.經(jīng)過對比圖4和圖5可知,滾轉(zhuǎn)陀螺的測量存在累積誤差,當(dāng)經(jīng)過Kalman濾波算法處理后，轉(zhuǎn)角測量誤差收斂值為3°,且不存在累積誤差.

經(jīng)上述分析及對比結(jié)果可以看出,轉(zhuǎn)角測量系統(tǒng)仿真所采用的Kalman濾波對加計(jì)和MEMS速率陀螺進(jìn)行融合的方法有效,并且提高了轉(zhuǎn)角測量的精度.

4 結(jié) 論

1) 對雙加計(jì)的輸出信號采用低通濾波算法,解決了雙軸加速度計(jì)安裝誤差的問題,從而提高了角度測量精度.

2) 引入Kalman濾波算法將滾轉(zhuǎn)陀螺測量轉(zhuǎn)速和雙加計(jì)測量轉(zhuǎn)角數(shù)據(jù)進(jìn)行融合,經(jīng)過Kalman濾波最后誤差收斂在3°,且不存在累積誤差.

3) Kalman濾波實(shí)現(xiàn)了加速度計(jì)和微機(jī)電速率陀螺的數(shù)據(jù)融合，此方法的轉(zhuǎn)角測量誤差小且不存在累積誤差.但彈體在拐彎或者彈體加速時產(chǎn)生平動加速度，轉(zhuǎn)角測量的結(jié)果受到影響.為解決平動加速度對測量結(jié)果的影響，可將地磁傳感器、加速度計(jì)和MEMS速率陀螺三者測量的數(shù)據(jù)進(jìn)行融合,有望進(jìn)一步提高彈體滾轉(zhuǎn)角的測量精度.

[1] 曲罄.基于地磁測量的彈丸滾轉(zhuǎn)角實(shí)時解算[D].南京：南京理工大學(xué),2009.

QU Qing.Real-time Solution of Projectile Rolling Angle Based on Geomagnetic Measurement[D].Nanjing：Nanjing University of Science and Technology,2009.(in Chinese)

[2] 桂延寧,楊燕.基于太陽方位角原理的炮彈飛行姿態(tài)遙測[J].兵工學(xué)報,2003,24(2):250.

GUI Yanning,YANG Yan.Telemetering for the Attitude of Projectile in Flight Based on the Principle of Solar Aspect Angle[J].Acta Armamentarii,2003,24(2):250.(in Chinese)

[3] 王廣龍,祖靜,張文棟,等.地磁場傳感器及其在飛行體姿態(tài)測量中的應(yīng)用[J].北京理工大學(xué)學(xué)報,1999,19(3):361.

WANG Guanglong,ZU Jing,Zhang Wendong,et al.Ground Magnetic Field Sensor and Its Application in Attitude Measurement of Flying Body[J].Journal of Beijing Institute of Technology,1999,19(3):361.(in Chinese)

[4] 呂清利,高旭東,王曉鳴,等.基于地磁測量的彈丸滾轉(zhuǎn)角實(shí)時解算研究[J].彈箭與制導(dǎo)學(xué)報,2011,31(3):29.

LYU Qingli,GAO Xudong,WANG Xiaoming,et al.Real Time Solution of Projectile Roll Angle Based on Geomagnetic Survey[J].Journal of Projectile,Rockets,Missile and Guidance,2011,31(3):29.(in Chinese)

[5] 高峰,張合,程翔,等.彈道修正引信中的地磁信號及其抗干擾研究[J].彈道學(xué)報,2008,20(4):45.

GAO Feng,ZHANG He,CHENG Xiang,et al.Research on the Geomagnetic Signal and Its Anti-interference for Fuze of Trajectory Correction Projectile[J].Journal of Ballistic,2008,20(4)45.(in Chinese)

[6] 路永樂,潘英俊,黎蕾蕾,等.采用雙軸加速度計(jì)的旋轉(zhuǎn)彈運(yùn)動姿態(tài)角速率測量方法[J].中國慣性技術(shù)學(xué)報,2015,23(2)：160.

LU Yongle,PAN Yingjun,LI Leilei,et al.Measurement Method of Projectile’s Heading and Pitching Angle Velocities Based on Biaxial Accelerometer[J].Journal of Chinese Inertial Technology,2015,23(2)：160.(in Chinese)[7] 范成葉,李杰,景增增,等.旋轉(zhuǎn)彈用三軸加速度計(jì)安裝位置誤差標(biāo)定補(bǔ)償技術(shù)[J].傳感技術(shù)學(xué)報,2013,26(10)：1352.

FAN Chengye,LI Jie,JING Zengzeng,et al.Calibration and Compensation Method on Installation Position Error of Tri-axis Accelerometer Units Used in Spinning Projectiles[J].Chinese Journal of Sensors and Actuators,2013,26(10):1352.(in Chinese)

[8] HUNG C Y,LEE S C.A Calibration Method for Six-accelerometer INS[J].International Journal of Control Automation & System,2006,4(5):615.

[9] 唐玉發(fā),張合,劉建敬.基于磁阻傳感器與加速度計(jì)復(fù)合的姿態(tài)角檢測技術(shù)[J].傳感器與微系統(tǒng),2013,32(1):56.

TANG Yufa,ZHANG He,LIU Jianjing.Attitude Angle Detection Technology Based on Combination of Magnetoresistive Sensor and Accelerometer[J].Sensor and Micro-system Technogies,2013,32(1):56.(in Chinese)

[10] 馬國梁.高轉(zhuǎn)速彈丸磁強(qiáng)計(jì)/太陽方位角傳感器組合測姿方法[J].南京理工大學(xué)學(xué)報,2013,37(1):139.

MA Guoliang.Magnetometer/Yawsonde Integrated Attitude Measurement for High Spin Rate Projectiles[J].Journal of Nanjing University of Science and Technology,2013,37(1):139.(in Chinese)

[11] 李冰,霍鵬飛,祁克玉.基于雙軸磁強(qiáng)計(jì)及 GPS 的滾轉(zhuǎn)角測量方法[J].現(xiàn)代電子技術(shù),2012,35(11):58.

LI Bing,HUO Pengfei,QI Keyu.Method to Measure Roll Angle by Two-axis Magnetometer and GPS[J].Modern Electronics Technique,2012,35(11):58.(in Chinese)

(責(zé)任編輯、校對 潘秋岑)

A Rolling Angle Measurement Algorithm Based on Fusion of Two-Axis Accelerometer and MEMS Gyroscope

WANGKening,ZHANGXiongxing,TIANJianping

( School of Optoelectronics Engineering,Xi’an Technological University,Xi’an 710021,China)

In order to solve the measurement accuracy of rolling angle of high spinning projectile，Kalman filter is used for data fusion of acceleration meter method and MEMS rate gyros.Two accelerometers are used to measure projectile angle.The acceleration is low pass filtered to get the centrifugal acceleration of accelerometer,which is compensated to obtain the projection of the acceleration of gravity,and to establish observation equation of Kalman filter.The projectile angle rate is gotten by MEMS gyroscope to establish the state equation of the Kalman filter.The Kalman filtering of projectile rolling angle is simulated with computer.The simulation results show:The speed measurement exists system error(bias) and random noise,average error of angle measurement is 20°which converges at 3°through Kalman filtering,and there is no accumulated error.

Kalman filtering;two-axis accelerater;roll angle;gyroscope

10.16185/j.jxatu.edu.cn.2016.09.008

2015-12-11

國防基礎(chǔ)科研重點(diǎn)項(xiàng)目(JCKY2013208B002)

王可寧(1975-)，女，西安工業(yè)大學(xué)副教授，主要研究方向?yàn)镸EMS、引信技術(shù)和數(shù)字信號處理.E-mail:77319436@qq.com.

TJ430

A

1673-9965(2016)09-0726-04