基于陀螺和卫星定位组合的滚转角测量方法

王　超，王永骥，霍鹏飞，周　翔，杨小会

(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