CN109373833B - 适用于旋转弹初始姿态和速度联合测量方法 - Google Patents

Abstract

本发明公开了一种适用于旋转弹初始姿态和速度联合测量方法，包括如下步骤：（1）、弹载传感器测量方案和坐标系变换，（2）、旋转弹滚转速度解算方法，（3）、弹体滚转角速率滤波器，（4）、旋转弹炮口初始速度解算方法，（5）、旋转弹炮口初始姿态解算方法，（6）、利用速度矢量投影关系，即按如下速度投影计算旋转弹出炮口时各初始速度υ_x，υ_y和υ_z。本发明方法实现旋转弹体初始姿态参数的准确测量。

Description

适用于旋转弹初始姿态和速度联合测量方法

技术领域

本发明涉及高速旋转弹体炮口初始参数测量技术领域，具体是一种基于三轴磁传感器和两个单轴陀螺仪的旋转弹炮口初始姿态和速度联合测量方法。

背景技术

旋转弹制导化改造是国内外常规武器发展方向，旋转弹飞行过程中姿态角的实时精确测量是实现其精确控制飞行的关键前提。受旋转弹高速转转、发射大过载和空间体积小等的限制，其弹载系统必须满足抗高过载、小体积和低成本等要求。旋转弹其自旋能高达40转/秒，轴向角速率达14400°/s，而目前弹用陀螺仪的量程与精度很难同时满足这类应用需求，弹丸发射瞬间产生的大过载也容易使弹载传感器出现短时间的饱和、失真或失效等问题。这些不利因素使得现有姿态测试系统移植很难直接应用于旋转弹的导航测试。特别是弹体炮口姿态和初始速度参数的准确性将直接会影响全弹道飞行参数的惯导解算。因此，旋转弹炮口初始姿态和初始速度参数的测量是其制导化改造关键难点之一。

发明内容

本发明目的是提供一种适用于旋转弹初始姿态和速度联合测量方法，实现旋转弹体初始姿态参数的准确测量。

本发明是采用如下技术方案实现的：

一种适用于旋转弹初始姿态和速度联合测量方法，包括如下步骤：

(1)、弹载传感器测量方案和坐标系变换

弹载测量传感器由三轴地磁传感器和两个单轴MEMS陀螺仪组成，其中，三轴磁传感器与弹体捷联安装，三轴磁传感器各敏感轴方向与弹体坐标系完全一致，其中弹体坐标系(OX^bY^bZ^b)符合右手关系，X轴为弹轴，则三轴磁传感器的X轴、Y轴和Z轴敏感方向分别对应弹体坐标系的X^b、Y^b和Z^b轴，三轴磁传感器用于测量弹体内地磁场矢量信息，其测量输出分别用H_x、Hy和H_z表示；

两个单轴陀螺仪捷联安装于X₁轴和Y₁轴上，X₁轴和Y₁轴相互正交，X₁和Y₁陀螺安装轴与弹体坐标系的X^b和Y^b不重合，它们之间存在夹角，两个陀螺仪的测量输出分别用ω_X1和ω_Y1表示，即陀螺安装轴X₁轴和Y₁轴处在平面(OX^bY^b)内，OZ^b轴垂直于平面(OX^bY^b)，X₁轴和Y₁轴分别与X^b和Y^b之间的夹角均为β角；因此，两坐标系间的变换关系矩阵，用变换矩阵

或

表示：

(2)、旋转弹滚转速度解算方法

根据弹体坐标系与陀螺仪安装坐标系变换矩阵

陀螺仪安装坐标系内测量所得角速率投影到弹体坐标系角速率的变换关系表示为：

上式(3)中，ω_x,ω_y,ω_z为弹体坐标系内各轴投影角速率分量；ω_X1，ω_Y1和ω_Z1为陀螺仪安装坐标系内测量输出的角速率分量；

因此，根据公式(3)矩阵变换关系，得到弹体坐标系内弹丸滚转轴X轴的滚转速率的计算公式为：

(3)、弹体滚转角速率滤波器

由于陀螺仪测量误差的存在，按(4)式计算得到的弹体滚转角速率总是存在测量误差，其简化的误差模型为：

上式(5)中，

为实际测量滚转角速率，ω_x为理想的滚转角速率，n_x为测量噪声；

滤波器选取滚转角速率ω_x作为系统的状态变量X＝ω_x，因此，其状态方程表示为：

X_(k)＝X_(k-1)+w_(k-1) (6)

上式(6)中，w_(k-1)为假设为零均值高斯白噪声，满足E[w(t)]＝0，E[w(t),w^T(τ)]＝Q(t)δ(t-τ)；

选取式(4)计算所得滚转角速率

作为滤波系统的观测量

则滤波器的观测方程表示为：

z(t)＝X(t)+v(t) (7)

上式(7)中，v(t)为系统的量测噪声，假设v(t)为高斯白噪声，并满足E[v(t)]＝0，E[v(t),v^T(τ)]＝R(t)δ(t-τ)；

因此，滤波器由状态方程式(6)和观测方程式(7)共同构建系统的滤波模型，采用kalman滤波算法对滚转角速率进行最优估计，滤波包括如下两个过程：

3.1时间更新：

3.2量测更新：

上式中，K_(k)表示滤波增益；H_k为量测值；R_k为量测噪声；Q_k-1为系统噪声；P_(k,k-1)为前一时刻系统估计方差；P_(k)为当前时刻系统方差；I为单位阵；P_(k-1)为系统估计方差；

为量测阵；k表示采样时刻；

因此，通过离散kalman滤波算法最终完成弹体滚转角速率

的最优估计

(4)、旋转弹炮口初始速度解算方法

弹丸出膛总速度的估算表示为：

上式(10)中，D为旋转弹体口径，Ω_g为膛线缠角，缠角由火炮系统型号所决定，是一固定值；

由上式(10)可知，若已知所测弹体型号，炮口的出膛初始速度与弹轴角速度存在相互对应关系，所以，利用测量弹轴X轴的角速度ω_x，通过反推计算出旋转弹在出炮口时总速度v₀；因此，利用所述测量方案中的两个正交安装陀螺测量解算所得弹体滚转角速度

通过查表得出弹丸出炮口总速度v₀的大小；

(5)、旋转弹炮口初始姿态解算方法

选取弹丸出炮口瞬间进行磁测姿态解算，因此选取发射坐标系为旋转弹导航参考坐标系(OXⁿYⁿZⁿ)，其中

为弹体偏航角，θ为弹体俯仰角，γ为弹体横滚角；

由于三轴地磁传感器直接捷联安装于弹体坐标系，则三轴磁传感器的测量输出为地磁矢量信息，其投影关系表示为：

上式(11)中，

为三轴地磁传感器测量输出；

为地磁场矢量在导航坐标系的投影；

为旋转弹姿态变换的方向余弦矩阵，方向余弦矩阵为：

假设弹体在膛内偏航角和俯仰角不变，以常值对待处理，则火炮发射前，通常火炮的射角或偏角由发射指令得知，因此，在进行炮口弹体姿态解算时，假定其偏航角已知，再利用磁测投影公式(11)得到旋转弹其余姿态角计算公式，弹体俯仰角和滚转角计算公式分别为：

因此，利用上式姿态计算公式(13)和(14)，最终实现旋转弹体炮口三个初始姿态角的测量解算；

(6)最后，利用速度矢量投影关系，即按如下速度投影式(15)计算旋转弹出炮口时各初始速度v_x,v_y和v_z

因此，按上述步骤(1)～(6)最终完成旋转弹炮口初始姿态和速度联合测量解算。

本发明方法具有如下优点：

1、本发明采用与弹体系非正交安装的陀螺仪测量方法，其适用于高过载、高自旋和高动态下的“三高”恶劣弹载测试应用环境，其测量系统具有更高的存活性和可靠性。

2、本发明所述测量方法，能够同时完成炮口三维初始速度和三维初始姿态的测量，实现了炮口初始参数的全参数的获取。

3、本发明所述测量方案具有简单、容易实施，性格比高等优点。

附图说明

图1表示弹载传感器测量方案。

图2表示坐标系变换关系。

图3表示导航参考坐标系。

具体实施方式

下面结合附图对本发明的具体实施例进行详细说明。

一种适用于旋转弹初始姿态和速度联合测量方法，包括如下步骤：

1、弹载传感器测量方案和坐标系变换

旋转弹炮口初始姿态和速度联合测量方法中，弹载测量传感器主要由三轴地磁传感器和两个单轴MEMS陀螺仪组成，弹载传感器安装方式如图1所示。三轴磁传感器与弹体捷联安装，磁传感器各敏感轴方向与弹体坐标系完全一致，弹体坐标系(OX^bY^bZ^b)规定为前上右部，符合右手关系，X轴为弹轴。即传感器的X轴、Y轴和Z轴敏感方向分别对应弹体坐标系的X^b、Y^b和Z^b轴。磁传感器用于测量弹体内地磁场矢量信息，其测量输出分别用H_x、Hy和H_z表示。

两个单轴陀螺仪捷联安装于图1所示X₁轴和Y₁轴上，X₁轴和Y₁轴相互正交，因此两个陀螺仪也相互正交安装，两个陀螺仪没有捷联安装于弹体坐标系，X₁和Y₁陀螺安装轴与弹体坐标系的X^b和Y^b不重合，它们之间存在一定的夹角，两个陀螺仪的测量输出分别用ω_X1和ω_Y1表示。本发明弹载传感器测量测试方案中，陀螺仪安装坐标系(OX₁Y₁Z₁)与弹体坐标系(OX^bY^bZ^b)的坐标变换关系如图2所示，陀螺安装轴X₁轴和Y₁轴处在平面(OX^bY^b)内，OZ^b轴垂直于平面(OX^bY^b)，X₁轴和Y₁轴分别与X^b和Y^b之间的夹角均为β角。实际上，陀螺仪安装坐标系可以理解为弹体坐标系以OZ^b轴旋转β角所得。

因此，由图示坐标系变换关系可得两坐标系间的变换关系矩阵，用变换矩阵

或

来表示：

2、旋转弹滚转速度解算方法

根据前述所得弹体坐标系与陀螺仪安装坐标系变换矩阵

陀螺仪安装坐标系内测量所得角速率(即陀螺仪实测输出值)投影到弹体坐标系角速率的变换关系可表示为：

上式(3)中，ω_x,ω_y,ω_z为弹体坐标系内各轴投影角速率分量；ω_X1，ω_Y1和ω_Z1陀螺仪安装坐标系内测量输出的角速率分量。

因此，根据公式(3)矩阵变换关系，可得到弹体坐标系内弹丸滚转轴X轴的滚转速率的计算公式为：

由公式(4)可知，若合理设计陀螺仪安装轴X₁轴与X^b之间、Y₁轴与Y^b之间的夹角(均为β角)，并确保两个正交捷联安装的陀螺仪在OX^bY^b平面内。因此，利用本发明弹载传感的测量方案，可利用两个小量程、高精度陀螺仪实现了旋转弹超高滚转速率的测量。

该测量方案优点是利用两个高精度、小量程陀螺测量输出，通过矩阵投影关系，用于替代旋转弹轴向所需大量程和高精度陀螺仪，解决了高速旋转弹体弹轴角速率测量时大量程与高精度矛盾的测量难题。

3、弹体滚转角速率滤波器

由于陀螺仪测量误差的存在，按(4)式计算得到的弹体滚转角速率总是存在测量误差，其简化的误差模型可写为：

上式(5)中，

为实际测量滚转角速率，ω_x为理想的滚转角速率，n_x为测量噪声。

本发明采用滤波方法对按(4)式计算所得滚转率进行最优估计，以提高滚转角速率精度。滤波器选取滚转角速率ω_x作为系统的状态变量X＝ω_x，因此，其状态方程可以表示为：

X_(k)＝X_(k-1)+w_(k-1) (6)

上式(6)中，w_(k-1)为假设为零均值高斯白噪声，满足E[w(t)]＝0，E[w(t),w^T(τ)]＝Q(t)δ(t-τ)。

选取式(4)计算所得滚转角速率

作为滤波系统的观测量

则滤波器的观测方程可表示为：

z(t)＝X(t)+v(t) (7)

上式(7)中，v(t)为系统的量测噪声，假设v(t)为高斯白噪声，并满足E[v(t)]＝0，E[v(t),v^T(τ)]＝R(t)δ(t-τ)。

因此，滤波器由状态方程式(6)和观测方程式(7)共同构建系统的滤波模型，采用kalman滤波算法对滚转速率进行最优估计，滤波包括如下两个过程：

(1)时间更新：

(2)量测更新：

上式中，K_(k)表示滤波增益；H_k为量测值；R_k为量测噪声；Q_k-1为系统噪声；P_(k,k-1)为前一时刻系统估计方差；P_(k)为当前时刻系统方差；I为单位阵；P_(k-1)为系统估计方差；

为量测阵；k表示采样时刻。

因此，通过离散kalman滤波算法最终完成弹体滚转角速率

的最优估计

4、旋转弹炮口初始速度解算方法

由于旋转弹出炮口后依靠惯性飞行，弹体出炮口的初始速度的直接决定了弹体飞行距离或射程。但弹体在膛内自旋一圈所对应的飞行距离跟初速度无关，其主要由火炮系统型号、火炮内膛线的缠角和弹丸口径等所决定的。弹体出膛总速度的估算可表示为：

上式(10)中，D为旋转弹体口径，Ω_g为膛线缠角，缠角由火炮系统型号所决定，是一固定值。

由上式(10)可知，若已知所测弹体型号，炮口的出膛初始速度与弹轴角速度存在相互对应关系，所以，可利用测量弹轴X轴的角速度ω_x，通过反推计算出旋转弹在出炮口时总速度v₀。因此，本发明利用所述测量方案中的两个正交安装陀螺测量解算所得弹体滚转角速度

(滤波后的滚转速率为

)，通过查表得出弹体出炮口总速度v₀的大小，其准确的对应关系由火炮生产企业提供，并事前存储于弹载测量系统中。

5、旋转弹炮口初始姿态解算方法

考虑到火炮发射膛内的外磁场干扰比较严重，若在膛内进行磁测姿态解算，所得弹体姿态失真严重、存在较大的测量误差。因此，本发明选取弹丸出炮口瞬间进行磁测姿态解算，不采用在膛内磁测解算，以提高初始姿态解算精度。本发明选取发射坐标系为旋转弹导航参考坐标系(OXⁿYⁿZⁿ)，如图3所示，

为弹体偏航角，θ为弹体俯仰角，γ为弹体横滚角。

由于三轴地磁传感器直接捷联安装于弹体坐标系，则三轴磁传感器的测量输出为地磁矢量信息，其投影关系可表示为：

上式(11)中，

为三轴地磁传感器测量输出；

为地磁场矢量在导航坐标系的投影；

为旋转弹姿态变换的方向余弦矩阵。方向余弦矩阵为：

考虑到弹体在火炮膛内运动时是高速旋转、直线运动的特殊性，通常可认为旋转弹出炮口前其弹体偏航角和俯仰角均不变。基于此，本发明假设弹体在膛内偏航角和俯仰角不变，以常值对待处理。火炮发射前，通常火炮的射角或偏角由发射指令得知。因此，在进行炮口弹体姿态解算时，假定其偏航角(或俯仰角)已知，再利用磁测投影公式(11)可推导得到旋转弹其余姿态角计算公式，弹体俯仰角和滚转角计算公式分别为：

因此，利用上式姿态计算公式(13)和(14)，最终实现旋转弹体炮口三个初始姿态角(偏航角

俯仰角θ和滚转角γ)的测量解算。

6、炮口初始姿态和速度联合测量解算方法

本发明所述旋转弹炮口初始姿态和速度联合测量解算步骤主要包括如下：

首先，根据弹载陀螺仪测量所得角速ω_X1和ω_Y1，利用滚转速率的计算公式(4)计算出弹轴X轴的滚转角速度ω_x，再利用滤波器对其进行滤波，得到最优滚转角速度

然后，把上述滤波所得

代入炮口速度估计公式(10)，计算出旋转弹的炮口初始速度v₀。

接着，假定弹丸膛内偏航角为零且不变，利用磁测解算公式(13)和(14)计算得到旋转弹初始偏航角、俯仰角和滚转角。

最后，利用速度矢量投影关系，即按如下速度投影式(15)计算旋转弹出炮口时各初始速度v_x,v_y和v_z。

7、按上述步骤(1)～(6)最终完成旋转弹炮口初始姿态和速度联合测量解算。

应当指出，对于本技术领域的一般技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和应用，这些改进和应用也视为本发明的保护范围。

Claims (1)

1.一种适用于旋转弹初始姿态和速度联合测量方法，其特征在于：包括如下步骤：

(1)、弹载传感器测量方案和坐标系变换

弹载测量传感器由三轴地磁传感器和两个单轴MEMS陀螺仪组成，其中，三轴磁传感器与弹体捷联安装，三轴磁传感器各敏感轴方向与弹体坐标系完全一致，其中弹体坐标系OX^bY^bZ^b符合右手关系，X轴为弹轴，则三轴磁传感器的X轴、Y轴和Z轴敏感方向分别对应弹体坐标系的X^b、Y^b和Z^b轴，三轴磁传感器用于测量弹体内地磁场矢量信息，其测量输出分别用H_x、Hy和H_z表示；

两个单轴陀螺仪捷联安装于X₁轴和Y₁轴上，X₁轴和Y₁轴相互正交，X₁和Y₁陀螺安装轴与弹体坐标系的X^b和Y^b不重合，它们之间存在夹角，两个陀螺仪的测量输出分别用ω_X1和ω_Y1表示，即陀螺安装轴X₁轴和Y₁轴处在平面OX^bY^b内，OZ^b轴垂直于平面OX^bY^b，X₁轴和Y₁轴分别与X^b和Y^b之间的夹角均为β角；因此，两坐标系间的变换关系矩阵，用变换矩阵

或

表示：

(2)、旋转弹滚转速度解算方法

根据弹体坐标系与陀螺仪安装坐标系变换矩阵

陀螺仪安装坐标系内测量所得角速率投影到弹体坐标系角速率的变换关系表示为：

上式(3)中，ω_x,ω_y,ω_z为弹体坐标系内各轴投影角速率分量；ω_X1，ω_Y1和ω_Z1为陀螺仪安装坐标系内测量输出的角速率分量；

因此，根据公式(3)矩阵变换关系，得到弹体坐标系内弹体滚转轴X轴的滚转速率的计算公式为：

(3)、弹体滚转角速率滤波器

由于陀螺仪测量误差的存在，按(4)式计算得到的弹体滚转角速率存在测量误差，其简化的误差模型为：

上式(5)中，

为实际测量滚转角速率，ω_x为理想的滚转角速率，n_x为测量噪声；

滤波器选取滚转角速率ω_x作为系统的状态变量X＝ω_x，因此，其状态方程表示为：

X_(k)＝X_(k-1)+w_(k-1) (6)

上式(6)中，w_(k-1)为假设为零均值高斯白噪声；

选取式(4)计算所得滚转角速率

作为滤波系统的观测量

则滤波器的观测方程表示为：

z(t)＝X(t)+v(t) (7)

上式(7)中，v(t)为系统的量测噪声；

因此，滤波器由状态方程式(6)和观测方程式(7)共同构建系统的滤波模型，采用kalman滤波算法对滚转速率进行最优估计，滤波包括如下两个过程：

3.1时间更新：

3.2量测更新：

上式中，K_(k)表示滤波增益；H_k为量测值；R_k为量测噪声；Q_k-1为系统噪声；P_(k/k-1)为前一时刻系统估计方差；P_(k)为当前时刻系统方差；I为单位阵；P_(k-1)为系统估计方差；

为量测阵；

因此，通过离散kalman滤波算法最终完成弹体滚转角速率

的最优估计

(4)、旋转弹炮口初始速度解算方法

弹丸出膛总速度的估算表示为：

上式(10)中，D为旋转弹体口径，Ω_g为膛线缠角，缠角由火炮系统型号所决定，是一固定值；

由上式(10)可知，若已知所测弹体型号，炮口的出膛初始速度与弹轴角速度存在相互对应关系，所以，利用测量弹轴X轴的角速度ω_x，通过反推计算出旋转弹在出炮口时总速度v₀；因此，利用所述测量方案中的两个正交安装陀螺测量解算所得弹体滚转角速度

通过查表得出弹丸出炮口总速度v₀的大小；

(5)、旋转弹炮口初始姿态解算方法

选取弹丸出炮口瞬间进行磁测姿态解算，因此选取发射坐标系为旋转弹导航参考坐标系OXⁿYⁿZⁿ，其中

为弹体偏航角，θ为弹体俯仰角，γ为弹体横滚角；

由于三轴地磁传感器直接捷联安装于弹体坐标系，则三轴磁传感器的测量输出为地磁矢量信息，其投影关系表示为：

上式(11)中，

为三轴地磁传感器测量输出；

为地磁场矢量在导航坐标系的投影；

为旋转弹姿态变换的方向余弦矩阵，方向余弦矩阵为：

假设弹丸在膛内偏航角和俯仰角不变，以常值对待处理，则火炮发射前，通常火炮的射角或偏角由发射指令得知，因此，在进行炮口弹体姿态解算时，假定其偏航角已知，再利用磁测投影公式(11)得到旋转弹其余姿态角计算公式，弹体俯仰角和滚转角计算公式分别为：

因此，利用上式姿态计算公式(13)和(14)，最终实现旋转弹体炮口三个初始姿态角的测量解算；

(6)最后，利用速度矢量投影关系，即按如下速度投影式(15)计算旋转弹出炮口时各初始速度v_x,v_y和v_z

因此，按上述步骤(1)～(6)最终完成旋转弹炮口初始姿态和速度联合测量解算。

Images