CN107314718A - 基于磁测滚转角速率信息的高速旋转弹姿态估计方法 - Google Patents

Abstract

本发明涉及飞行器或者弹体空间姿态的测量方法，具体为一种基于磁测滚转角速率信息的高速旋转弹姿态估计方法，采用三轴地磁传感器、两轴陀螺仪和卫星接收机组成弹载姿态传感系统和采用磁测辅助的旋转弹姿态快速估计方法，能够实现初始参数未知下的弹体姿态的空中在线自行快速获取。旋转弹姿态快速估计算法利用弹体速度信息（弹体速度信息由卫星接收机测量所得）来估算弹体偏航角和俯仰角姿态，并利用其作为快速估计算法的观测信息。旋转弹姿态快速估计方法在完成弹体姿态估计的同时实现了弹载三轴地磁传感器和二轴陀螺仪传感器的误差参数的在线估计与补偿。本发明设计合理，能够快速在线估计出旋转弹的飞行姿态，具有很好的推广应用价值。

Description

基于磁测滚转角速率信息的高速旋转弹姿态估计方法

技术领域

本发明涉及飞行器或者弹体空间姿态的测量方法，具体为一种基 于磁测滚转角速率信息的高速旋转弹姿态估计方法。

背景技术

受高速旋转弹发射大过载、高速旋转、高动态和空间狭小等恶劣 测试环境的限制，现有的导弹弹载飞行姿态测量系统直接移植于应用 于高速旋转弹药姿态测量时，存在弹体初始姿态参数获取困难、姿态 测量精度较低、或者无法实现旋转弹的全程全姿态测量等问题。特别 是由于高速旋转弹药具有极高的滚转速率，现有陀螺的量程与精度无 法同时满足弹载测量的要求，从而限制了惯性姿态测量系统在其上的 移植应用。因此，高速旋转弹高精度姿态测量技术是其制导化改造的 难点所在，亟需寻求一种低成本、易移植、适用于旋转弹飞行姿态测 量技术，对解决旋转弹药制导化改造中空中飞行姿态测量难题具有重 要的理论价值与现实意义。

发明内容

本发明为了解决高速旋转弹药受到发射大过载、高速旋转和高动 态等恶劣应用环境限制而造成导航初始姿态参数获取困难、测量精度 低、或无法实现全程全姿态测量等问题，发明了一种基于磁测滚转角 速率信息的高速旋转弹姿态估计方法。

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

一种基于磁测滚转角速率信息的高速旋转弹姿态估计方法，包括 如下步骤：

(1)、弹体三维姿态组合测量方案

旋转弹弹载姿态传感器由三轴地磁传感器、两轴陀螺仪和卫星接 收机组成。

所述三轴地磁传感器捷联安装于旋转弹弹体，三轴地磁传感器的 各敏感轴Sx、Sy和Sz与弹体坐标系OX^bY^bZ^b方向完全一致，用于 测量弹体内的地磁场矢量信息。

其中，弹体坐标系(b系)：弹体坐标系是用来描述弹体空间的姿 态。弹体坐标系和运动载体固连，坐标原点位于弹体的质心，X^b轴 沿弹轴方向向前；Y^b轴沿弹体纵轴方向向上；Z^b轴与X^b、Y^b轴构成 右手坐标系。弹体坐标系随弹体的旋转而旋转。

三轴地磁传感器的测量输出关系为：

上式中，为弹体坐标系内的三轴地磁传感器测量 输出分量；是导航参考坐标系下的地磁场三分量， 由于常规弹药射程内变化很小，其在弹道射程内是常值量；为 姿态变换矩阵；若选取地面发射坐标系作为导航参考坐标系，则三轴 地磁传感器的测量输出为。

其中，地面坐标系(发射坐标系，t系)：以弹道起点作为坐标原 点，以射击面与弹道起点水平面的交线为X^t轴，射击方向为正；Y^t轴铅直向上；Z^t轴与X^t、Y^t轴构成右手坐标系。该坐标系主要用来确 定弹丸质心坐标，并作为确定弹轴和速度方向的基准。

所述两轴陀螺仪捷联安装弹体坐标系的Y^b轴和Z^b轴；滚弹轴(X^b轴)上不安装陀螺仪。

所述卫星接收机用来测量弹体的速度信息，利用测量所得速度信 息的来估算弹体偏航角和俯仰角姿态，为弹体姿态组合测量系统提供 观测信息。

利用速度信息的估算弹体偏航角和俯仰角姿态公式为：

(2)、基于地磁信息的弹体滚转角速率滤波估计方法

I、基于地磁传感器信息的滚转角测量方法

建立弹轴坐标系OXYZ，其中，弹轴坐标系(z系)：弹轴坐标 系是用来描述弹体在空间的方位。弹轴坐标系和运动载体固连，坐标 原点位于弹体的质心，X^z轴沿弹轴方向向前；Y^z轴垂直于弹轴向上 为正；Z^b轴与X^z、Y^z轴构成右手坐标系。弹轴坐标系不随弹体的旋 转而旋转。

在弹体径向平面YOZ，其中Y、Z分别为弹轴坐标系的Y和Z轴； Y^b、Z^b表示为弹体坐标系的Y和Z轴，而为捷联安装于弹体 Y和Z轴上的三轴地磁传感器实际测量输出；H_YOZ为地磁场强度在弹 体径向平面内YOZ内的投影分量，它与OZ轴之间存在夹角，定义 为地磁基准角，用α来表示。

根据当处于第二象限投影关系可知，捷联于弹体的磁传感器 的测量所得地磁场分量的大小即为H_YOZ在Y^b和Z^b轴上的投影值， 因此，在任意外弹道时刻t，弹体滚转角γ(t)按如下公式(4)计算得 到：

上式β(t)为H_YOZ与Z^b间夹角。

同理，整理出全象限范围内滚转角的计算公式为：

由于弹道各点磁基准角α(t)是时变的，但当采样率较高时前后采 样点时刻的地磁基准角相同，即认为α(t)≈α(t-Δt)，因此弹体滚转角 变化率通过下式计算得到：

上式(6)中，为旋转弹的理想滚转角速率；表示为测量计算 误差。

因此，利用两个正交安装的地磁传感器的测量输出值，综 合式(5)和式(6)估算出弹体滚转角和角度变化率的大小。

II、基于地磁信息的弹体滚转角速率观测模型

由欧拉方程推出弹体滚转角速率ω_x与滚转角姿态变化率之间的 关系为：

将式(7)代入式(6)，进一步整理得到弹体滚转角速率的观测方 程为：

若选取滚转角变化率作为观测量，通过滤波方法估算出弹体滚 转轴向的角速率用于替代X轴陀螺仪；由于旋转弹体高速旋转 时，且偏航变化率很小，若令则观测方程式(8)简写为：

因此，若以式(6)计算所得滚转角速率作为观测值，通过kalman 滤波估计方法估算出弹体X轴向角速率用于替代X轴陀螺仪。

(3)、旋转弹飞行姿态空中快速滤波估计方法

I、旋转弹三维姿态组合式滤波器结构

旋转弹三维姿态空中快速滤波估计器包括地磁传感器在线补偿滤 波器、弹体角速率滤波器和弹体姿态估计器；其中，地磁传感器在线 补偿滤波器实现地磁传感器的在线补偿；弹体角速率滤波器实现弹体 角速率估计与补偿；弹体姿态滤波器完成旋转弹姿态估计。

II、弹体角速率估计滤波方法

若以式(6)计算所得滚转角速率作为观测值，通过滤波方法估算 出弹体X轴向弹体角速率；根据上述式(9)所示的弹体X轴向角速 率观测方程，并结合捷联Y和Z轴陀螺传感器测量误差模型，得到 弹体三轴向角速率测量误差模型为：

上式中，分别为Y、Z轴向角速率测量值；而为 角速率估计值；s_Gy,s_Gz分别为Y和Z轴陀螺的灵敏度系数；b_i(i＝x,y,z) 各轴向陀螺常值零偏误差；k_yx、k_yz和k_zx、k_yz分别为Y、Z轴陀螺的 轴间的交叉耦合系数；n_i(i＝x,y,z)各轴向陀螺测量噪声。

选取陀螺传感器误差参数b_x,b_y,b_z,s_gy,s_gx,s_zz,k_yz,k_zx,k_zy共9个参数作为 系统的状态变量X_g＝[b_x,k_yz,s_gy,s_gx,b_y,k_zx,k_zy,s_zz,b_z]^T，并假设陀螺传感器各 误差参数是常值，因此，系统的状态方程表示为：

X_g(k)＝Φ_(k，k-1)X_g(k-1)+w_k-1……………………………………..……(13)

上式Φ_(k，k-1)＝I_9×9，w_(k-1)为系统零均值高斯白噪声。

选取弹体滚转姿态变化率Y和Z陀螺测量输出共同构 成量测变量：则弹体角速率测量误差模型(12) 即为系统的量测方程，简写为：

Z_g(t)＝h[X_g(t),t]+v(t)………………………………………..…….(14)

式中v(t)是系统的量测噪声，设为高斯白噪声，E[v(t)]＝0， E[v(t),v^T(τ)]＝R(t)δ(t-τ)；并对量测方程(14)进行线性与离散化处理： Z_g(k)＝H_g(k)X_g(k)+u_k+v_k……………………………..……………(15)

上式中：

因此，由状态方程(13)和观测方程组(15)成系统滤波方程， 采用离散kalman滤波方程进行滤波状态估计，滤波方程如下：

时间更新：

量测更新：

其中，K_g(k)表示滤波增益阵；H_k为量测矩阵；R_k为量测噪声 噪声阵；Q_k-1为系统噪声阵；P_g(k,k-1)为前一时刻系统估计方差阵； P_g(k)为当前时刻系统方差阵，通过上述滤波步骤完成对陀螺误差 参数测最优估计。

III、旋转弹三维姿态估计滤波算法

根据捷联惯性导航理论，旋转弹体姿态动力学方程由欧拉方程进 行描述：

上式中，为经过滤波估计后的弹体角速率，ψ,θ,γ分别 为弹体的偏航角、俯仰角和滚转角。

选取旋转弹三维姿态角ψ,θ,γ作为系统的状态变量X_a＝[ψ,θ,γ]^T； 并以旋转弹体姿态欧拉方程(18)作为系统状态方程，则其简写为如 下形式：

上式中w(t)是系统过程噪声，假设为零均值高斯白色噪声，并 满足E[w(t)]＝0，E[w(t),w^T(τ)]＝Q(t)δ(t-τ)；

捷联弹体的三轴地磁传感器测量输出和由卫星接收 机测量所得到弹体速度估算所得的弹体偏航角和俯仰角(ψ_m,θ_m)作 为系统的观测信息，则系统的观测变量：

由于三轴地磁传感器捷联安装于旋转弹弹体坐标系，则地磁传感 器的测量输出为：

上式中，为弹体坐标系内的理想测量输出矢量；是导航参考坐标系下的地磁场矢量，在弹道射程内是常值变量；为 姿态变换矩阵。

因此，综合上述磁传感器的测量输出模型式(2)和偏航角及俯仰 角的估计公式(3)，共同构成了系统的观测方程：

上式中，c_i,s_i(i＝ψ,θ,γ)是三角函数cosi和sini的简写形式；v(t)是系 统的量测噪声，假设为零均值高斯白噪声，并满足E[v(t)]＝0， E[v(t),v^T(τ)]＝R(t)δ(t-τ)。

由状态方程(18)和观测方程(21)共同构成初始姿态角未知下 旋转弹姿态估计的滤波模型，利用滤波方法估计出弹体三维姿态参数， 但由于状态方程和观测方程都是非线性连续系统，其简写为如下一般 形式：

上式中f(·)和h(·)是关于自变量X_a(t)的非线性函数，w(t)和v(t)分别 是系统过程噪声和量测噪声。

根据所建立的滤波模型，首先按上述方法进行滤波模型的离散化 及线性处理，因此在给定滤波初值P₀情况下，基于EKF的弹体 姿态滤波算法包括如下时间和量测更新过程：

给定初始滤波参数P₀：

时间更新：

量测更新：

P_a(k)＝(I-K_a(k)H_k)P_a(k,k-1)(I-K_a(k)H_k)^T+K_a(k)R_k(K_a(k))^T………………..(30)

上式中Φ_k,k-1为状态转移矩阵，H_k为量测矩阵，分别是状态方程中 f(·)和量测方程中h(·)的雅可比矩阵；K_a(k)表示滤波增益阵；R_k为量测 噪声阵；Q_k-1为系统噪声阵；P_a(k，k-1)为前一时刻系统估计方差阵；P_a(k)为当前时刻系统方差阵。

因此，通过上述的滤波初值选取、状态预测与量测更新三个滤波 算法步骤，最终实现在初始参数未知状态下的弹体三维飞行姿态的空 中快速估计。

1、该方法在实时完成弹体三维姿态估计的同时，完成了弹载传感 器误差参数估计与数据补偿，进一步提高了弹载传感器的测量精度。

2、快速估计方法能实现初始参数未知状态下的弹体三维姿态快速 获取，有利于实现初始参数射前装定向空中自行快速获取技术的转变， 能够提高了武器弹药的快速反应时间与机动性能。

上述磁测滚转角速率信息的旋转弹姿态快速估计方法采用三轴地 磁传感器、两轴陀螺仪和卫星接收机组成弹载姿态传感系统。三轴地 磁传感器测量输出信息具有两种不同使用方式：其一是以两个正交的 Y和Z轴磁测输出信息估算得到的弹丸滚转角速率作为滤波器观测 信息，然后利用kalman滤波估算到弹体角速率信息替代测量方法， 用于解决弹丸高自旋而造成陀螺无法测量的问题；其二是利用地磁传 感器与微惯性测量器件构建磁测辅助信息的弹体姿态组合测量系统 的观测信息。采用磁测辅助的旋转弹姿态快速估计方法，能够实现初 始参数未知下的弹体姿态的空中在线自行快速获取。旋转弹姿态快速 滤波算法具体为选取旋转弹三维姿态角ψ,θ,γ作为系统的状态变量 X_a＝[ψ,θ,γ]^T；以旋转弹体姿态欧拉方程(18)作为系统状态方程； 并以捷联地三轴磁传感器的测量输出模型式(2)和偏航角及俯仰角 的估计公式(3)，共同构成了系统的观测方程。旋转弹姿态快速估计 算法利用弹体速度信息(弹体速度信息由卫星接收机测量所得)来估 算弹体偏航角和俯仰角姿态，并利用其作为快速估计算法的观测信息。 旋转弹姿态快速估计方法在完成弹体姿态估计的同时实现了弹载三 轴地磁传感器和二轴陀螺仪传感器的误差参数的在线估计与补偿。

本发明设计合理，在实现初始参数未知状态下，能够快速在线估 计出旋转弹的三维空中飞行姿态，具有很好的推广应用价值。

附图说明

图1表示弹载传感器安装几何关系示意图。

图2表示偏航角和俯仰角估算原理示意图。

图3-1表示弹体径向平面截取示意图。

图3-2表示弹体径向平面内处于第二象限投影关系示意图。

图3-3表示弹体径向平面内处于第一象限投影关系示意图。

图3-4表示弹体径向平面内处于第四象限投影关系示意图。

图3-5表示弹体径向平面内处于第三象限投影关系示意图。

图4表示旋转弹三维姿态空中快速滤波估计器结构示意图。

图中，1-旋转弹弹体，2-三轴地磁传感器，3-两轴陀螺仪，4-卫星 接收机。

具体实施方式

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

一种基于磁测滚转角速率信息的高速旋转弹姿态估计方法，主要 包括弹体姿态组合测量方案、基于地磁信息的弹体角速率滤波估计方 法和旋转弹三维姿态空中快速滤波估计方法三部分内容。

1、弹体三维姿态组合测量方案

旋转弹弹载姿态传感器由三轴地磁传感器2、两轴陀螺仪3和卫 星接收机4组成，各弹载传感器捷联安装几何关系如图1所示。

图1中所标示Mx、My和Mz表示三轴地磁传感器，其捷联安装 于旋转弹弹体1，地磁传感器的各敏感轴(Sx、Sy和Sz)与弹体坐 标系(OX^bY^bZ^b)方向完全一致，用于测量弹体内的地磁场矢量信息。 三轴地磁传感器的测量输出关系为：

上式中，为弹体坐标系内的三轴地磁传感器测量 输出分量；是导航参考坐标系下的地磁场三分量， 由于常规弹药射程内变化很小，通常认为其在弹道射程内是常值 量；为姿态变换矩阵。若选取地面发射坐标系作为导航参考坐标系， 则三轴地磁传感器的测量输出为：

需要说明的是地磁传感器测量输出信息有两个作用：一是利用两 个正交的Y和Z轴磁测输出信息估算弹丸滚转角速率，用于解决弹 丸高自旋而造成陀螺无法测量的问题；另一作用是利用地磁传感器与 微惯性测量器件构建磁测辅助信息的。

图1中标示Gy和Gz传感器表示两轴MEMS陀螺仪，其分别捷 联安装弹体Y^b轴和Z^b轴，而X^b轴向并没有安装陀螺仪，正如前所 述，弹体滚转角速率是通过地磁传感器测量信息估计得到。另外，图 1中卫星接收机4用来测量弹体的速度和位置信息，为弹体姿态组合 测量系统提供观测信息。

由于旋转弹的外弹道飞行特殊性，即便是弹体受控飞行时，旋转 弹的攻角通常也很小，在不考虑弹体的攻角情况下，完全可以利用卫 星导航系统所测量的弹体速度信息来估算弹体的偏航角和俯仰角，用 于代替偏航角不变假设，其估算原理如图2所示：

因此，利用速度信息的估算弹体偏航角和俯仰角姿态公式为：

2、基于地磁信息的弹体滚转角速率滤波估计方法

所述组合测量方案中，由于弹体坐标系X轴向没有安装陀螺仪， 需要其它辅助测量方法获得X轴角速率。因此，本发明采用以地磁 矢量作为测量基准，通过地磁传感器在弹体内的适当布阵，首先利 用两个正交的Y和Z轴磁测输出信息估算弹丸滚转角速率的测量，然后利用滤波估计方法完成弹体角速率滤波估计，用于解决弹丸高 自旋而造成陀螺无法测量的问题。

(1)基于地磁传感器信息的滚转角测量方法

基于地磁传感器信息的滚转角测量方法示意如图3-2所示，图中 圆为弹体径向平面YOZ，其中Y、Z分别为弹轴坐标系(z系)的Y 和Z轴；Y^b、Z^b表示为弹体坐标系(b系)的Y和Z轴，而为捷联安装于弹体Y和Z轴上地磁传感器实际测量输出；H_YOZ为地 磁场强度在弹体径向平面内YOZ内的投影分量，它与OZ轴间存在 一定的夹角，定义为地磁基准角，用α来表示。

按图3-2所示，处于第二象限投影关系可知，捷联于弹体的磁传 感器的测量所得地磁场分量的大小即为H_YOZ在Y^b和Z^b轴上的 投影值。因此，在任意外弹道时刻t，弹体滚转角γ(t)可按如下公式(4) 计算得到：

上式β(t)为H_YOZ与Z^b间夹角。相似的情况处于其余象限投影关 系如图3-3、3-4、3-5所示：

因此，按上述图示关系可以整理出全象限范围内滚转角的计算公 式为：

由于弹道各点磁基准角α(t)是时变的，但当采样率较高时前后采 样点时刻的地磁基准角相同，即可以认为α(t)≈α(t-Δt)，因此弹体滚 转角变化率可通过下式计算得到：

上式(6)中，为旋转弹的理想滚转角速率；表示为测量计算 误差。

因此，利用两个正交安装的地磁传感器的测量输出值，综 合式(5)和式(6)可以估算出弹体滚转角和角度变化率的大小。需 要说明的是上述滚转角变化率解算也同样无需磁场的先验知识、只 需要标量运算等优点。

(2)、基于地磁信息的弹体滚转角速率观测模型

由欧拉方程可得推出弹体滚转角速率ω_x与滚转角姿态变化率之 间的关系为：

将式(7)代入式(6)，可进一步整理得到弹体滚转角速率的观测 方程为：

若选取滚转角变化率作为观测量，通过滤波方法可以估算出弹 体滚转轴向的角速率用于替代X轴陀螺仪。由于旋转弹体高速 旋转时，且偏航变化率很小，若令 则观测方程式(8)可简写为：

因此，若以式(6)计算所得滚转角速率作为观测值，通过kalman 滤波估计方法可以估算出弹体X轴向角速率用于替代X轴陀螺 仪。

3、旋转弹飞行姿态空中快速滤波估计方法

(1)、旋转弹三维姿态组合式滤波器结构

旋转弹三维姿态空中快速滤波估计器结构如图4所示。组合滤波 器主要由地磁传感器在线补偿滤波器、弹体角速率滤波器和弹体姿态 估计器三个滤波器组成。其中，地磁传感器在线补偿滤波器实现地磁 传感器的在线补偿；弹体角速率滤波器实现弹体角速率估计与补偿； 弹体姿态滤波器完成旋转弹姿态估计。

采用“组合式滤波结构”，而不是采用集中式结构对所有状态参数 的进行滤波估计。主要考虑到如下几个因素：其一是弹体的姿态更新 率最少要比其它两个滤波器的更新频率要高一个数量级；其二是由前 面的可观测性分析可知，当旋转弹接近地磁方向飞行时，地磁在线补 偿滤波器可观测性很差，若采用该组合结构可以很方便于关断该滤波 器以提高姿态全局滤波精度；另外是旋转弹载导航传感器数据更新频 率不同，卫星导航系统量测更新率1～10Hz，而惯性器件和地磁传感 器的数据采样更新率最少要高二个数量级，最为重要的是采用这种滤 波结构，可以减少系统的维数，有利于提高整个系统的实时性能。

(2)、弹体角速率估计滤波方法(对应于附图4中的KF)

弹体角速率滤波器用于完成弹体角速率估计与误差补偿。由式(9) 可以推导得到弹体X轴向角速率估计模型式：

又由于旋转弹体高速旋转时，其若令则由弹体X轴向角速率估计模型式(10)整理 得到其观测方程为：

注：公式(11)是公式(9)的重复引用。

若以式(6)计算所得滚转角速率作为观测值，通过滤波方法可以 估算出弹体X轴向弹体角速率。根据上述式(11)所示的弹体X轴 向角速率观测方程，并结合捷联Y和Z轴陀螺传感器测量误差模型， 可以得到弹体三轴向角速率测量误差模型为：

上式中，分别为Y、Z轴向角速率测量值；而为 角速率估计值；s_Gy,s_Gz分别为Y和Z轴陀螺的灵敏度系数；b_i(i＝x,y,z) 各轴向陀螺常值零偏误差；k_yx、k_yz和k_zx、k_yz分别为Y、Z轴陀螺的 轴间的交叉耦合系数；n_i(i＝x,y,z)各轴向陀螺测量噪声。

选取陀螺传感器误差参数b_x,b_y,b_z,s_gy,s_gx,s_zz,k_yz,k_zx,k_zy共9个参数作为 系统的状态变量X_g＝[b_x,k_yz,s_gy,s_gx,b_y,k_zx,k_zy,s_zz,b_z]^T。并假设陀螺传感器各 误差参数是常值，因此，滤波系统的状态方程可表示为：

X_g(k)＝Φ_(k,k-1)X_g(k-1)+w_k-1…………………………………………..(13)

上式Φ_(k，k-1)＝I_9×9，w_(k-1)为系统零均值高斯白噪声。

选取弹体滚转姿态变化率Y和Z陀螺测量输出共同构 成量测变量：则弹体三轴向角速率测量误差模型 (12)即可作为滤波系统的量测方程，其简写为：

Z_g(t)＝h[X_g(t),t]+v(t)………………………………………………..(14)

式中v(t)是系统的量测噪声，设为高斯白噪声，E[v(t)]＝0， E[v(t),v^T(τ)]＝R(t)δ(t-τ)。并对量测方程(14)进行线性与离散化处理：

Z_g(k)＝H_g(k)X_g(k)+u_k+v_k………………………………….…………(15)

上式中：

因此，由状态方程(13)和观测方程(15)共同构成系统的滤波 模型，采用离散kalman滤波算法进行状态参数的滤波估计，其滤波 过程包括时间和量测更新：

时间更新：

量测更新：

其中，K_g(k)表示滤波增益阵；H_k为量测矩阵；R_k为量测噪声 噪声阵；Q_k-1为系统噪声阵；P_g(k,k-1)为前一时刻系统估计方差阵； P_g(k)为当前时刻系统方差阵，通过上述滤波步骤完成对陀螺误差 参数测最优估计。

(3)、旋转弹姿态估计滤波算法(对应于附图4中的EKF)

根据捷联惯性导航理论，旋转弹体姿态动力学方程可由欧拉方程 进行描述：

上式中，为经过滤波估计后的弹体角速率，ψ,θ,γ分别 为弹体的偏航角、俯仰角和滚转角。

选取旋转弹三维姿态角ψ,θ,γ作为系统的状态变量X_a＝[ψ,θ,γ]^T； 并以旋转弹体姿态欧拉方程(18)作为系统状态方程，则其可简写为 如下形式：

上式中w(t)是系统过程噪声，假设为零均值高斯白色噪声，并

满足E[w(t)]＝0，E[w(t),w^T(τ)]＝Q(t)δ(t-τ)。

在图1姿态组合滤波方案中，捷联弹体的三轴地磁传感器测量输 出和由卫星接收机测量所得到弹体速度估算所得的弹体 偏航角和俯仰角(ψ_m,θ_m)作为系统的观测信息，则系统的观测变量：

由于三轴地磁传感器捷联安装于旋转弹弹体坐标系，则地磁传感 器的测量输出为：

上式中，为弹体坐标系内的理想测量输出矢量；是导航参考坐标系下的地磁场矢量，通常认为在弹道射程内是常值变 量；为姿态变换矩阵。

因此，综合上述磁传感器的测量输出模型式(2)和偏航角及俯仰 角的估计公式(3)，共同构成了系统的观测方程：

上式中，c_i,s_i(i＝ψ，θ，γ)是三角函数cosi和sini的简写形式；v(t)是系 统的量测噪声，假设为零均值高斯白噪声，并满足E[v(t)]＝0， E[v(t),v^T(τ)]＝R(t)δ(t-τ)。

由状态方程(18)和观测方程(21)共同构成初始姿态角未知下 旋转弹姿态估计的滤波模型，利用滤波方法可以估计出弹体三维姿态 参数。但由于状态方程和观测方程都是非线性连续系统，其简写为如 下一般形式：

上式中f(·)和h(·)是关于自变量X_a(t)的非线性函数，w(t)和v(t)分别 是系统过程噪声和量测噪声。

根据所建立的滤波模型，首先按上述方法进行滤波模型的离散化 及线性处理。因此在给定滤波初值P₀情况下，基于EKF的弹体 姿态滤波算法主要包括如下时间和量测更新过程：

给定初始滤波参数P₀：

时间更新：

量测更新：

P_a(k)＝(I-K_a(k)H_k)P_a(k,k-1)(I-K_a(k)H_k)^T+K_a(k)R_k(K_a(k))^T……………….(30)

上式中Φ_k,k-1为状态转移矩阵，H_k为量测矩阵，分别是状态方程中 f(·)和量测方程中h(·)的雅可比矩阵；K_a(k)表示滤波增益阵；R_k为量测 噪声阵；Q_k-1为系统噪声阵；P_a(k,k-1)为前一时刻系统估计方差阵；P_a(k)为当前时刻系统方差阵。

因此，通过上述的滤波初值选取、状态预测与量测更新三个滤波 算法步骤，最终完成弹载三维姿态参数空中快速估计。

本实施例上述方法的主要特征及优点如下：

1、采用三轴地磁传感器、两轴陀螺仪和卫星接收机组成姿态测量 方案

2、采用基于地磁信息的弹体滚转角速率估计方法，替代滚转轴上 陀螺仪，解决旋转弹应用环境带来的姿态传感器的限制性问题。

3、采用一种新的基于“组合式”的滤波器结构，实现弹体三维姿 态估计的同时完成了弹载传感器的误差参数估计，方法提高了滤波系 统的实时性和可靠性。

4、采用快速姿态估计算法实现了在初始参数未知状态下的弹体三 维飞行姿态的空中快速估计。有利于实现初始参数射前装定向空中自 行快速获取技术的转变，能够提高了武器弹药的快速反应时间与机动 性能。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而 非限制，尽管参照实施例本发明进行了详细说明，本领域的普通技术 人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱 离本发明的技术方案的精神和范围，其均应涵盖本发明的权利要求保 护范围中。

Claims (1)

1.一种基于磁测滚转角速率信息的高速旋转弹姿态估计方法，其特征在于：包括如下步骤：

(1)、弹体三维姿态组合测量方案

旋转弹弹载姿态传感器由三轴地磁传感器(2)、两轴陀螺仪(3)和卫星接收机(4)组成；

所述三轴地磁传感器(2)捷联安装于旋转弹弹体(1)，三轴地磁传感器(2)的各敏感轴Sx、Sy和Sz与弹体坐标系OX^bY^bZ^b方向完全一致，用于测量弹体内的地磁场矢量信息；

其中，弹体坐标系OX^bY^bZ^b和运动载体固连，坐标原点位于弹体的质心，X^b轴沿弹轴方向向前；Y^b轴沿弹体纵轴方向向上；Z^b轴与X^b、Y^b轴构成右手坐标系；

三轴地磁传感器的测量输出关系为：

上式中，为弹体坐标系内的三轴地磁传感器测量输出分量；是导航参考坐标系下的地磁场三分量，由于常规弹药射程内变化很小，其在弹道射程内是常值量；为姿态变换矩阵；若选取地面发射坐标系作为导航参考坐标系，则三轴地磁传感器的测量输出为：

所述两轴陀螺仪捷联安装弹体坐标系的Y^b轴和Z^b轴；

所述卫星接收机(4)用来测量弹体的速度信息，利用测量所得速度信息的来估算弹体偏航角和俯仰角姿态，为弹体姿态组合测量系统提供观测信息；

利用速度信息的估算弹体偏航角和俯仰角姿态公式为：

(2)、基于地磁信息的弹体滚转角速率滤波估计方法

I、基于地磁传感器信息的滚转角测量方法

建立弹轴坐标系OXYZ，其中，弹轴坐标系和运动载体固连，坐标原点位于弹体的质心，X^z轴沿弹轴方向向前；Y^z轴垂直于弹轴向上为正；Z^b轴与X^z、Y^z轴构成右手坐标系；

在弹体径向平面YOZ，其中Y、Z分别为弹轴坐标系的Y和Z轴；Y^b、Z^b表示为弹体坐标系的Y和Z轴，而为捷联安装于弹体Y和Z轴上的三轴地磁传感器实际测量输出；H_YOZ为地磁场强度在弹体径向平面内YOZ内的投影分量，它与OZ轴之间存在夹角，定义为地磁基准角，用α来表示；

根据当处于第二象限投影关系可知，捷联于弹体的磁传感器 的测量所得地磁场分量的大小即为H_YOZ在Y^b和Z^b轴上的投影值，因此，在任意外弹道时刻t，弹体滚转角γ(t)按如下公式(4)计算得到：

上式β(t)为H_YOZ与Z^b间夹角；

同理，整理出全象限范围内滚转角的计算公式为：

由于弹道各点磁基准角α(t)是时变的，但当采样率较高时前后采样点时刻的地磁基准角相同，即认为α(t)≈α(t-Δt)，因此弹体滚转角变化率通过下式计算得到：

上式(6)中，为旋转弹的理想滚转角速率；表示为测量计算误差；

因此，利用两个正交安装的地磁传感器的测量输出值，综合式(5)和式(6)估算出弹体滚转角和角度变化率的大小；

II、基于地磁信息的弹体滚转角速率观测模型

由欧拉方程推出弹体滚转角速率ω_x与滚转角姿态变化率之间的关系为：

将式(7)代入式(6)，进一步整理得到弹体滚转角速率的观测方程为：

若选取滚转角变化率作为观测量，通过滤波方法估算出弹体滚转轴向的角速率用于替代X轴陀螺仪；由于旋转弹体高速旋转时，且偏航变化率很小，若令则观测方程式(8)简写为：

因此，若以式(6)计算所得滚转角速率作为观测值，通过kalman滤波估计方法估算出弹体X轴向角速率用于替代X轴陀螺仪；

(3)、旋转弹飞行姿态空中快速滤波估计方法

I、旋转弹三维姿态组合式滤波器结构

旋转弹三维姿态滤波器采用“组合式”快速滤波结构，旋转弹三维姿态空中快速滤波估计器包括地磁传感器在线补偿滤波器、弹体角速率滤波器和弹体姿态滤波估计器；其中，地磁传感器在线补偿滤波器实现地磁传感器的在线补偿；弹体角速率滤波器实现弹体角速率估计与补偿；弹体姿态滤波估计器完成旋转弹姿态估计。

II、弹体角速率估计滤波方法

若以式(6)计算所得滚转角速率作为观测值，通过滤波方法估算出弹体X轴向弹体角速率；根据上述式(9)所示的弹体X轴向角速率观测方程，并结合捷联Y和Z轴陀螺传感器测量误差模型，得到弹体三轴向角速率测量误差模型为：

上式中，分别为Y、Z轴向角速率测量值；而为角速率估计值；s_Gy,s_Gz分别为Y和Z轴陀螺的灵敏度系数；b_i(i＝x,y,z)各轴向陀螺常值零偏误差；k_yx、k_yz和k_zx、k_yz分别为Y、Z轴陀螺的轴间的交叉耦合系数；n_i(i＝x,y,z)各轴向陀螺测量噪声；

选取陀螺传感器误差参数b_x,b_y,b_z,s_gy,s_gx,s_zz,k_yz,k_zx,k_zy共9个参数作为系统的状态变量X_g＝[b_x,k_yz,s_gy,s_gx,b_y,k_zx,k_zy,s_zz,b_z]^T，并假设陀螺传感器各误差参数是常值，因此，系统的状态方程表示为：

X_g(k)＝Φ_(k,k-1)X_g(k-1)+w_k-1……………………………………..……(13)

上式Φ_(k,k-1)＝I_9×9，w_(k-1)为系统零均值高斯白噪声；

选取弹体滚转姿态变化率Y和Z陀螺测量输出共同构成量测变量：则弹体角速率测量误差模型(12)即为系统的量测方程，简写为：

Z_g(t)＝h[X_g(t),t]+v(t)………………………………………………(14)

式中v(t)是系统的量测噪声，设为高斯白噪声，E[v(t)]＝0，E[v(t),v^T(τ)]＝R(t)δ(t-τ)；并对量测方程(14)进行线性与离散化处理：

Z_g(k)＝H_g(k)X_g(k)+u_k+v_k……………………………..……………(15)

上式中：

因此，由状态方程(13)和观测方程组(15)成系统滤波方程，采用离散kalman滤波方程进行滤波状态估计，滤波方程如下：

时间更新：

量测更新：

其中，K_g(k)表示滤波增益阵；H_k为量测矩阵；R_k为量测噪声噪声阵；Q_k-1为系统噪声阵；P_g(k,k-1)为前一时刻系统估计方差阵；P_g(k)为当前时刻系统方差阵，通过上述滤波步骤完成对陀螺误差参数测最优估计；

III、旋转弹三维姿态估计滤波算法

根据捷联惯性导航理论，旋转弹体姿态动力学方程由欧拉方程进行描述：

上式中，为经过滤波估计后的弹体角速率，ψ,θ,γ分别为弹体的偏航角、俯仰角和滚转角；

选取旋转弹三维姿态角ψ,θ,γ作为系统的状态变量X_a＝[ψ,θ,γ]^T；并以旋转弹体姿态欧拉方程(18)作为系统状态方程，则其简写为如下形式：

上式中w(t)是系统过程噪声，假设为零均值高斯白色噪声，并满足E[w(t)]＝0，E[w(t),w^T(τ)]＝Q(t)δ(t-τ)；

捷联弹体的三轴地磁传感器测量输出和由卫星接收机测量所得到弹体速度估算所得的弹体偏航角和俯仰角(ψ_m,θ_m)作为系统的观测信息，则系统的观测变量：

由于三轴地磁传感器捷联安装于旋转弹弹体坐标系，则地磁传感器的测量输出为：

上式中，为弹体坐标系内的理想测量输出矢量；是导航参考坐标系下的地磁场矢量，在弹道射程内是常值变量；为姿态变换矩阵；

因此，综合上述磁传感器的测量输出模型式(2)和偏航角及俯仰角的估计公式(3)，共同构成了系统的观测方程：

上式中，c_i,s_i(i＝ψ,θ,γ)是三角函数cosi和sini的简写形式；v(t)是系统的量测噪声，假设为零均值高斯白噪声，并满足E[v(t)]＝0，E[v(t),v^T(τ)]＝R(t)δ(t-τ)；

由状态方程(18)和观测方程(21)共同构成初始姿态角未知下旋转弹姿态估计的滤波模型，利用滤波方法估计出弹体三维姿态参数，但由于状态方程和观测方程都是非线性连续系统，其简写为如下一般形式：

上式中f(·)和h(·)是关于自变量X_a(t)的非线性函数，w(t)和v(t)分别是系统过程噪声和量测噪声；

根据所建立的滤波模型，首先按上述方法进行滤波模型的离散化及线性处理，因此在给定滤波初值P₀情况下，基于EKF的弹体姿态滤波算法包括如下时间和量测更新过程：

给定初始滤波参数P₀：

时间更新：

量测更新：

P_a(k)＝(I-K_a(k)H_k)P_a(k,k-1)(I-K_a(k)H_k)^T+K_a(k)R_k(K_a(k))^T………………(30)

上式中Φ_k,k-1为状态转移矩阵，H_k为量测矩阵，分别是状态方程中f(·)和量测方程中h(·)的雅可比矩阵；K_a(k)表示滤波增益阵；R_k为量测噪声阵；Q_k-1为系统噪声阵；P_a(k,k-1)为前一时刻系统估计方差阵；P_a(k)为当前时刻系统方差阵；

因此，通过上述的滤波初值选取、状态预测与量测更新三个滤波算法步骤，最终实现在初始参数未知状态下的弹体三维飞行姿态的空中快速估计。