基于陀螺和衛星定位組合的滾轉角測量方法

王　超，王永驥，霍鵬飛，周　翔，楊小會

(1.西安機電信息技術研究所，陜西 西安 710065;2.華中科技大學自動化學院，湖北 武漢 430074)

?

基于陀螺和衛星定位組合的滾轉角測量方法

王超1，王永驥2，霍鵬飛1，周翔1，楊小會1

(1.西安機電信息技術研究所，陜西 西安 710065;2.華中科技大學自動化學院，湖北 武漢 430074)

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

關鍵詞:二維彈道修正；滾轉角；卡爾曼濾波；三軸陀螺

0引言

目前國際上局部戰爭結果表明，精確打擊彈藥在未來戰爭中起到重要的作用。然而，精確打擊彈藥成本太高，大量裝備部隊目前還不太現實。因而，常規兵器的制導化，制導武器的小型化成為未來武器系統的發展趨勢[1]。

二維彈道修正彈藥是在常規彈藥基礎上，將傳統引信更換為二維彈道修正引信就可以實現彈丸的靈巧化改造，具有很高的效費比，是目前各國爭相研究的熱點方向，在二維彈道修正引信研究中，彈丸的滾轉角精確測量是其中一項關鍵技術。

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

1微旋彈滾轉角測量原理

彈丸在飛行過程中，由于自身重力作用，彈道向下彎曲；在彈丸自身旋轉情況下，安裝在彈丸中徑向陀螺的輸出為彈丸自轉角速度對彈道彎曲角速度的調制信號，若僅采用陀螺測量無法得到準確的彈丸滾轉角,因此采用陀螺與衛星定位組合,通過衛星定位信息解算得到彈道彎曲角速度,結合陀螺測量信息,通過濾波估計得到彈丸滾轉角。陀螺在彈丸中的相對位置關系如圖1所示。

圖1　三軸陀螺在彈丸中的相對位置關系Fig.1　Triaxial gyro in the relative position of the projectile

根據圖1可知，三個單軸陀螺在彈丸中的是互相正交的組合關系，其中，軸向安裝的陀螺測量彈丸滾轉角速度，徑向安裝的兩個陀螺測量彈道俯仰角速度在彈丸徑向的投影。

在彈丸發射后，衛星定位接收機開始測量彈丸的速度信息，衛星定位測量的速度信息可轉化到地面坐標系下三個方向的速度Vx、Vy、Vz，根據衛星定位測量的速度信息，可以解算出準彈體坐標系下的俯仰角?和偏航角ψ。

(1)

通過式(1)分別求導得到彈丸俯仰角速度和偏航角速度，結合三個陀螺測量的角速度信息，通過卡爾曼濾波算法估計出彈丸的滾轉角，圖2為測量流程圖。

自2007年開始，先后組織4次入河排污口聯合執法檢查行動，整改企業違法違規行為，效果顯著。2011年，黃河流域在全國率先啟動流域入河排污口全面核查，并已取得成果，為入河排污口監管提供了準確可靠的基礎依據。

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

2基于卡爾曼濾波的彈丸滾轉角估計

2.1卡爾曼濾波原理

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

卡爾曼濾波系統方程和量測方程如下[4]：

(2)

其中，Wk為系統噪聲，Vk為量測噪聲，均為高斯白噪聲。

采用卡爾曼濾波迭代計算過程如下：

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表示濾波狀態變量，Φk,k-1表示狀態方程一步轉移陣，Pk-1表示濾波估計均方誤差，Qk表示濾波噪聲陣，Rk為系統噪聲方差陣，Hk表示濾波量測陣， Zk表示量測值，Kk表示濾波增益陣。

2.2卡爾曼濾波方程建立

根據外彈道學理論可知[5]，固連坐標系下的Y軸和Z軸角速度輸出為：

(8)

(9)

(10)

根據X軸陀螺輸出的角速度，彈體滾轉角的正弦函數還可表示為：

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

(11)

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

對于卡爾曼濾波來說，濾波量測方程中選取的測量值越多，濾波估計越準確，由式(10)和式(11)可以看出，彈丸滾轉角三角函數兩種不同的表達方式與X軸陀螺、Y軸和Z軸陀螺測量數據和衛星測量數據都有關系，因此將兩個公式求和，可以將更多的測量信息用于濾波量測方程的建立，其滾轉角三角函數中的正弦函數求和后為：

(12)

即：

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

(13)

同理可知，彈丸滾轉角三角函數中的余弦函數求和后為：

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

(14)

因此，根據上式推導，得到卡爾曼濾波狀態變量與三軸陀螺和衛星定位測量數據的三角函數關系，建立卡爾曼濾波量測方程為：

(15)

建立卡爾曼濾波系統狀態方程為：

(16)

將建立的濾波狀態變量，濾波量測方程和狀態方程代入到卡爾曼濾波迭代方程中，估計出彈丸滾轉角正弦和余弦函數值，對函數值求反正切值，結合滾轉角正弦和余弦函數值的正負號進行象限判斷，可得到彈丸的滾轉角。

(17)

3仿真和測試驗證

3.1仿真驗證

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

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

圖4　濾波估計滾轉角誤差曲線Fig.4　Filter estimate roll Angle error curve

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

3.2實驗室測試驗證

為了進一步驗證濾波算法的有效性，在實驗室采用MEMS傳感器三軸標定轉臺近似模擬彈丸飛行環境，驗證濾波算法的正確性，該轉臺可以三軸同時轉動，并且可以輸出實時的三軸姿態角信息。

具體驗證方法是將三軸陀螺測量樣機固定在轉臺上，控制轉臺模擬彈丸運動過程，通過三軸陀螺測量角速度數據，采用轉臺輸出的俯仰角速度和偏航角速度信息模擬衛星定位測量信息，其中轉臺輸出的滾轉角信息作為彈丸滾轉角的真實值，將三軸陀螺測量數據和模擬的衛星定位測量數據輸入到卡爾曼濾波算法中估計彈丸滾轉角，通過比較濾波估計的滾轉角與轉臺輸出滾轉角的誤差大小，評價濾波估計滾轉角的有效性，具體過程如下：

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

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

圖6　濾波估計滾轉角與轉臺輸出滾轉角曲線Fig.6　Filtered estimate roll Angle with the turntable output curve

圖7　濾波估計滾轉角相對轉臺輸出滾轉角誤差曲線Fig.7　Filtered estimate roll Angle relative to the output angular roll Angle error of curve

通過實驗室MEMS傳感器三軸標定轉臺測試驗證，可以看出滾轉角測量誤差均值小于2.5°，均方差小于3°，且濾波收斂速度快、濾波估計滾轉角精度高，進一步在實驗室下，證明了本文提出的卡爾曼濾波方法的正確性。

4結論

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

參考文獻:

[1]馬芮.MIMU/GNSS組合制導技術在制導炮彈中的應用[J].現代防御技術,2009，37(1)：53-57.

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

[3]李冰,霍鵬飛,祁克玉.基于雙軸磁強計及GPS的滾轉角測量方法[J].現代電子技術,2012，35(11)：58-60.

[4]秦永元.卡爾曼濾波與組合導航原理[M].西安：西北工業大學出版社,1998.

[5]韓子鵬.外彈道學[M].西安：國防工業出版社,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

中圖分類號:TJ765.4

文獻標志碼:A

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

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

基金項目:國防科工局預研項目資助(SCKY2013208B002)

*收稿日期:2015-10-23