基于陀螺和衛(wèi)星定位組合的滾轉(zhuǎn)角測(cè)量方法

王　超，王永驥，霍鵬飛，周　翔，楊小會(huì)

(1.西安機(jī)電信息技術(shù)研究所，陜西 西安 710065;2.華中科技大學(xué)自動(dòng)化學(xué)院，湖北 武漢 430074)

?

基于陀螺和衛(wèi)星定位組合的滾轉(zhuǎn)角測(cè)量方法

王超1，王永驥2，霍鵬飛1，周翔1，楊小會(huì)1

(1.西安機(jī)電信息技術(shù)研究所，陜西 西安 710065;2.華中科技大學(xué)自動(dòng)化學(xué)院，湖北 武漢 430074)

摘要:針對(duì)現(xiàn)有微旋彈滾轉(zhuǎn)角測(cè)量方法易受天氣條件限制或環(huán)境電磁干擾的問(wèn)題，提出了基于陀螺和衛(wèi)星定位組合的滾轉(zhuǎn)角測(cè)量方法。該方法在分析陀螺測(cè)量滾轉(zhuǎn)角原理的基礎(chǔ)上，對(duì)彈丸滾轉(zhuǎn)角三角函數(shù)的兩種表達(dá)方式求和，將三軸陀螺和衛(wèi)星定位數(shù)據(jù)融合建立卡爾曼濾波方程估計(jì)彈丸滾轉(zhuǎn)角。仿真和實(shí)驗(yàn)表明：濾波估計(jì)滾轉(zhuǎn)角準(zhǔn)確且濾波算法的收斂速度快，測(cè)量誤差均值小于2.5°，均方差小于3°，可用于微旋彈或微旋引信的滾轉(zhuǎn)角測(cè)量。

關(guān)鍵詞:二維彈道修正；滾轉(zhuǎn)角；卡爾曼濾波；三軸陀螺

0引言

目前國(guó)際上局部戰(zhàn)爭(zhēng)結(jié)果表明，精確打擊彈藥在未來(lái)戰(zhàn)爭(zhēng)中起到重要的作用。然而，精確打擊彈藥成本太高，大量裝備部隊(duì)目前還不太現(xiàn)實(shí)。因而，常規(guī)兵器的制導(dǎo)化，制導(dǎo)武器的小型化成為未來(lái)武器系統(tǒng)的發(fā)展趨勢(shì)[1]。

二維彈道修正彈藥是在常規(guī)彈藥基礎(chǔ)上，將傳統(tǒng)引信更換為二維彈道修正引信就可以實(shí)現(xiàn)彈丸的靈巧化改造，具有很高的效費(fèi)比，是目前各國(guó)爭(zhēng)相研究的熱點(diǎn)方向，在二維彈道修正引信研究中，彈丸的滾轉(zhuǎn)角精確測(cè)量是其中一項(xiàng)關(guān)鍵技術(shù)。

目前，微旋彈二維彈道修正引信滾轉(zhuǎn)角測(cè)量方法，已有基于太陽(yáng)方位角的微旋彈滾轉(zhuǎn)角測(cè)量方法[2]，但該方法受天氣的限制，必須在白天光照充足的情況下使用，具有較大的應(yīng)用局限性。另一種基于磁傳感器和衛(wèi)星定位組合的微旋彈滾轉(zhuǎn)角測(cè)量方法[3]，但磁傳感器有時(shí)容易受到二維修正引信電機(jī)電磁干擾，造成滾轉(zhuǎn)角測(cè)量誤差較大。本文針對(duì)上述問(wèn)題，提出了基于陀螺和衛(wèi)星定位組合的滾轉(zhuǎn)角測(cè)量方法。

1微旋彈滾轉(zhuǎn)角測(cè)量原理

彈丸在飛行過(guò)程中，由于自身重力作用，彈道向下彎曲；在彈丸自身旋轉(zhuǎn)情況下，安裝在彈丸中徑向陀螺的輸出為彈丸自轉(zhuǎn)角速度對(duì)彈道彎曲角速度的調(diào)制信號(hào)，若僅采用陀螺測(cè)量無(wú)法得到準(zhǔn)確的彈丸滾轉(zhuǎn)角,因此采用陀螺與衛(wèi)星定位組合,通過(guò)衛(wèi)星定位信息解算得到彈道彎曲角速度,結(jié)合陀螺測(cè)量信息,通過(guò)濾波估計(jì)得到彈丸滾轉(zhuǎn)角。陀螺在彈丸中的相對(duì)位置關(guān)系如圖1所示。

圖1　三軸陀螺在彈丸中的相對(duì)位置關(guān)系Fig.1　Triaxial gyro in the relative position of the projectile

根據(jù)圖1可知，三個(gè)單軸陀螺在彈丸中的是互相正交的組合關(guān)系，其中，軸向安裝的陀螺測(cè)量彈丸滾轉(zhuǎn)角速度，徑向安裝的兩個(gè)陀螺測(cè)量彈道俯仰角速度在彈丸徑向的投影。

在彈丸發(fā)射后，衛(wèi)星定位接收機(jī)開(kāi)始測(cè)量彈丸的速度信息，衛(wèi)星定位測(cè)量的速度信息可轉(zhuǎn)化到地面坐標(biāo)系下三個(gè)方向的速度Vx、Vy、Vz，根據(jù)衛(wèi)星定位測(cè)量的速度信息，可以解算出準(zhǔn)彈體坐標(biāo)系下的俯仰角?和偏航角ψ。

(1)

通過(guò)式(1)分別求導(dǎo)得到彈丸俯仰角速度和偏航角速度，結(jié)合三個(gè)陀螺測(cè)量的角速度信息，通過(guò)卡爾曼濾波算法估計(jì)出彈丸的滾轉(zhuǎn)角，圖2為測(cè)量流程圖。

自2007年開(kāi)始，先后組織4次入河排污口聯(lián)合執(zhí)法檢查行動(dòng)，整改企業(yè)違法違規(guī)行為，效果顯著。2011年，黃河流域在全國(guó)率先啟動(dòng)流域入河排污口全面核查，并已取得成果，為入河排污口監(jiān)管提供了準(zhǔn)確可靠的基礎(chǔ)依據(jù)。

圖2　基于陀螺和衛(wèi)星定位組合的滾轉(zhuǎn)角測(cè)量流程圖Fig.2　Based on the gyro and satellite positioning combination of roll Angle measurement flow chart

2基于卡爾曼濾波的彈丸滾轉(zhuǎn)角估計(jì)

2.1卡爾曼濾波原理

1960年卡爾曼首次提出的卡爾曼濾波是一種線形最小方差估計(jì)，該理論一經(jīng)提出立即受到了工程應(yīng)用的重視，阿波羅登月飛行和C-5A飛機(jī)導(dǎo)航系統(tǒng)的設(shè)計(jì)是早期應(yīng)用中的最成功者。目前，卡爾曼濾波理論作為一種最重要的最優(yōu)估計(jì)理論被廣泛應(yīng)用于機(jī)器人導(dǎo)航、控制、傳感器數(shù)據(jù)融合以及導(dǎo)彈跟蹤等方面。

卡爾曼濾波系統(tǒng)方程和量測(cè)方程如下[4]：

(2)

其中，Wk為系統(tǒng)噪聲，Vk為量測(cè)噪聲，均為高斯白噪聲。

采用卡爾曼濾波迭代計(jì)算過(guò)程如下：

Xk/k-1=Φk,k-1Xk-1

(3)

(4)

(5)

Pk=(I-KkHk)Pk/k-1

(6)

Xk=Xk/k-1+Kk(Zk-HkXk/k-1)

(7)

其中，Xk-1表示濾波狀態(tài)變量，Φk,k-1表示狀態(tài)方程一步轉(zhuǎn)移陣，Pk-1表示濾波估計(jì)均方誤差，Qk表示濾波噪聲陣，Rk為系統(tǒng)噪聲方差陣，Hk表示濾波量測(cè)陣， Zk表示量測(cè)值，Kk表示濾波增益陣。

2.2卡爾曼濾波方程建立

根據(jù)外彈道學(xué)理論可知[5]，固連坐標(biāo)系下的Y軸和Z軸角速度輸出為：

(8)

(9)

(10)

根據(jù)X軸陀螺輸出的角速度，彈體滾轉(zhuǎn)角的正弦函數(shù)還可表示為：

sinγ(t)=sin(P·t+Φp)

(11)

式(11)中，Φp為滾轉(zhuǎn)角的初始相位。

對(duì)于卡爾曼濾波來(lái)說(shuō)，濾波量測(cè)方程中選取的測(cè)量值越多，濾波估計(jì)越準(zhǔn)確，由式(10)和式(11)可以看出，彈丸滾轉(zhuǎn)角三角函數(shù)兩種不同的表達(dá)方式與X軸陀螺、Y軸和Z軸陀螺測(cè)量數(shù)據(jù)和衛(wèi)星測(cè)量數(shù)據(jù)都有關(guān)系，因此將兩個(gè)公式求和，可以將更多的測(cè)量信息用于濾波量測(cè)方程的建立，其滾轉(zhuǎn)角三角函數(shù)中的正弦函數(shù)求和后為：

(12)

即：

cosΦp+0.5cos(P·t)·sinΦp

(13)

同理可知，彈丸滾轉(zhuǎn)角三角函數(shù)中的余弦函數(shù)求和后為：

0.5cos(P·t)·cosΦp-0.5sin(P·t)·sinΦp

(14)

因此，根據(jù)上式推導(dǎo)，得到卡爾曼濾波狀態(tài)變量與三軸陀螺和衛(wèi)星定位測(cè)量數(shù)據(jù)的三角函數(shù)關(guān)系，建立卡爾曼濾波量測(cè)方程為：

(15)

建立卡爾曼濾波系統(tǒng)狀態(tài)方程為：

(16)

將建立的濾波狀態(tài)變量，濾波量測(cè)方程和狀態(tài)方程代入到卡爾曼濾波迭代方程中，估計(jì)出彈丸滾轉(zhuǎn)角正弦和余弦函數(shù)值，對(duì)函數(shù)值求反正切值，結(jié)合滾轉(zhuǎn)角正弦和余弦函數(shù)值的正負(fù)號(hào)進(jìn)行象限判斷，可得到彈丸的滾轉(zhuǎn)角。

(17)

3仿真和測(cè)試驗(yàn)證

3.1仿真驗(yàn)證

首先采用Maltab軟件建立仿真模型，驗(yàn)證卡爾曼濾波方法估計(jì)彈丸滾轉(zhuǎn)角的正確性，由于迫彈是微旋彈，因此選取迫彈為仿真驗(yàn)證平臺(tái)，建立6D外彈道模型計(jì)算生成彈道數(shù)據(jù)，然后引入三軸陀螺和衛(wèi)星定位測(cè)量誤差，模擬生成陀螺測(cè)量數(shù)據(jù)和衛(wèi)星定位測(cè)量數(shù)據(jù)，用建立的卡爾曼濾波估計(jì)方法對(duì)模擬的測(cè)量數(shù)據(jù)進(jìn)行處理，并將濾波估計(jì)滾轉(zhuǎn)角與外彈道解算的真實(shí)滾轉(zhuǎn)角比較，結(jié)果如圖3和圖4所示。

圖3　Y軸和Z軸陀螺輸出曲線Fig.3　Y axis and Z axis gyro output curve

圖4　濾波估計(jì)滾轉(zhuǎn)角誤差曲線Fig.4　Filter estimate roll Angle error curve

通過(guò)對(duì)仿真模擬的測(cè)量數(shù)據(jù)進(jìn)行卡爾曼濾波估計(jì)可以看出，在引入測(cè)量誤差的情況下，卡爾曼濾波估計(jì)滾轉(zhuǎn)角的誤差均值小于0.3°，均方差小于2.5°，證明了本文提出的卡爾曼濾波算法的正確性。

3.2實(shí)驗(yàn)室測(cè)試驗(yàn)證

為了進(jìn)一步驗(yàn)證濾波算法的有效性，在實(shí)驗(yàn)室采用MEMS傳感器三軸標(biāo)定轉(zhuǎn)臺(tái)近似模擬彈丸飛行環(huán)境，驗(yàn)證濾波算法的正確性，該轉(zhuǎn)臺(tái)可以三軸同時(shí)轉(zhuǎn)動(dòng)，并且可以輸出實(shí)時(shí)的三軸姿態(tài)角信息。

具體驗(yàn)證方法是將三軸陀螺測(cè)量樣機(jī)固定在轉(zhuǎn)臺(tái)上，控制轉(zhuǎn)臺(tái)模擬彈丸運(yùn)動(dòng)過(guò)程，通過(guò)三軸陀螺測(cè)量角速度數(shù)據(jù)，采用轉(zhuǎn)臺(tái)輸出的俯仰角速度和偏航角速度信息模擬衛(wèi)星定位測(cè)量信息，其中轉(zhuǎn)臺(tái)輸出的滾轉(zhuǎn)角信息作為彈丸滾轉(zhuǎn)角的真實(shí)值，將三軸陀螺測(cè)量數(shù)據(jù)和模擬的衛(wèi)星定位測(cè)量數(shù)據(jù)輸入到卡爾曼濾波算法中估計(jì)彈丸滾轉(zhuǎn)角，通過(guò)比較濾波估計(jì)的滾轉(zhuǎn)角與轉(zhuǎn)臺(tái)輸出滾轉(zhuǎn)角的誤差大小，評(píng)價(jià)濾波估計(jì)滾轉(zhuǎn)角的有效性，具體過(guò)程如下：

首先將測(cè)量樣機(jī)安裝在轉(zhuǎn)臺(tái)上，轉(zhuǎn)臺(tái)初始對(duì)準(zhǔn)，設(shè)置轉(zhuǎn)臺(tái)三個(gè)方向角速度的初值，滾轉(zhuǎn)角速度為1080°/s，俯仰角速度為3°/s，偏航角速度為0.001°/s，然后測(cè)量樣機(jī)上電采集測(cè)量數(shù)據(jù)，并控制轉(zhuǎn)臺(tái)模擬彈丸飛行過(guò)程中的姿態(tài)，依次控制轉(zhuǎn)臺(tái)滾轉(zhuǎn)、俯仰、偏航三個(gè)方向轉(zhuǎn)動(dòng)，采集25s后斷電，并讀取采集數(shù)據(jù)，將采集的三軸陀螺數(shù)據(jù)和設(shè)置的俯仰角速度和偏航角速度數(shù)據(jù)輸入到濾波算法中估計(jì)出滾轉(zhuǎn)角，將估計(jì)的滾轉(zhuǎn)角與轉(zhuǎn)臺(tái)輸出的滾轉(zhuǎn)角比較，實(shí)際三軸陀螺測(cè)量曲線和處理曲線如圖5-圖7所示。

圖5　三軸陀螺測(cè)量曲線Fig.5　Triaxial gyro measurement curve

圖6　濾波估計(jì)滾轉(zhuǎn)角與轉(zhuǎn)臺(tái)輸出滾轉(zhuǎn)角曲線Fig.6　Filtered estimate roll Angle with the turntable output curve

圖7　濾波估計(jì)滾轉(zhuǎn)角相對(duì)轉(zhuǎn)臺(tái)輸出滾轉(zhuǎn)角誤差曲線Fig.7　Filtered estimate roll Angle relative to the output angular roll Angle error of curve

通過(guò)實(shí)驗(yàn)室MEMS傳感器三軸標(biāo)定轉(zhuǎn)臺(tái)測(cè)試驗(yàn)證，可以看出滾轉(zhuǎn)角測(cè)量誤差均值小于2.5°，均方差小于3°，且濾波收斂速度快、濾波估計(jì)滾轉(zhuǎn)角精度高，進(jìn)一步在實(shí)驗(yàn)室下，證明了本文提出的卡爾曼濾波方法的正確性。

4結(jié)論

本文提出了基于陀螺和衛(wèi)星定位組合的滾轉(zhuǎn)角測(cè)量方法。該方法在分析陀螺測(cè)量滾轉(zhuǎn)角測(cè)量原理的基礎(chǔ)上，對(duì)彈丸滾轉(zhuǎn)角三角函數(shù)的兩種表達(dá)方式求和，將三軸陀螺和衛(wèi)星定位數(shù)據(jù)融合建立濾波量測(cè)方程估計(jì)彈丸滾轉(zhuǎn)角。仿真和實(shí)驗(yàn)室驗(yàn)證表明：濾波估計(jì)滾轉(zhuǎn)角準(zhǔn)確且濾波算法的收斂速度快，測(cè)量誤差均值小于2.5°，均方差小于3°。擬用于微旋彈或微旋引信的滾轉(zhuǎn)角測(cè)量，下一步可通過(guò)炮射試驗(yàn)進(jìn)一步驗(yàn)證方法的正確性，為工程化研究奠定基礎(chǔ)。

參考文獻(xiàn):

[1]馬芮.MIMU/GNSS組合制導(dǎo)技術(shù)在制導(dǎo)炮彈中的應(yīng)用[J].現(xiàn)代防御技術(shù),2009，37(1)：53-57.

[2]周翔,霍鵬飛,祁克玉.MEMS陀螺/磁傳感器復(fù)合彈丸姿態(tài)測(cè)量[J].探測(cè)與控制學(xué)報(bào),2010，32(6)：10-13.

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

[4]秦永元.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社,1998.

[5]韓子鵬.外彈道學(xué)[M].西安：國(guó)防工業(yè)出版社,2000.

Projectile Roll Angle Measurement Besed on Gyroscope and Satellite

WANG Chao1, WANG Yongji2, HUO Pengfei1, ZHOU Xiang1, YANG Xiaohui1

(1. Xi’an Institute of Electromechanical Information Technology, Xi’an 710065，China;2.Huazhong University of Science and Technology, Wuhan 430074, China)

Abstract:The low spinning projectile roll angle measurement is liable to be influenced by weather conditions or environmental electromagnetic interference.A new estimation algorithm based on three axis gyro and satellite combination was proposed. We first analysised the principle of the gyro measurement roll angle, and then summed two kinds of the projectile rolling angle of the triangle function of the triaxial gyro and satellite positioning data fusion Kalman filtering equation to estimate the projectile rolling Angle. Experiments and simulation results showed that the filter could estimate roll angle accurately and filtering algorithm convergence speed, the mean error of measurement was less than 2.5 degrees, mean square error was less than 3 degrees.

Key words:two-dimensional trajectory correction; roll angle; Kalman filter; triaxial gyro

中圖分類(lèi)號(hào):TJ765.4

文獻(xiàn)標(biāo)志碼:A

文章編號(hào)：1008-1194(2016)01-0095-04

作者簡(jiǎn)介:王超(1983—)男，陜西西安人，碩士研究生，研究方向：彈道修正仿真及控制。E-mail:wangchao20001112@tom.com。

基金項(xiàng)目:國(guó)防科工局預(yù)研項(xiàng)目資助(SCKY2013208B002)

*收稿日期:2015-10-23