CN110243362A - 一种中高空超声速靶标导航方法 - Google Patents

Abstract

本发明涉及一种中高空超声速靶标导航方法，在成熟的当地水平坐标系导航算法的基础上，通过坐标转换，得到发射坐标系的导航参数，满足制导控制系统的导航需求，容易与制导控制系统、仿真系统等进行整合。

Description

一种中高空超声速靶标导航方法

技术领域

本发明属于飞行器导航、制导与控制领域，涉及中高空超声速飞行领域，特别涉及一种中高空超声速靶标导航方法。

背景技术

靶标模拟来袭飞行器的各种特性，是考核防空飞行器系统作战效能和实战能力的一种常用设备，靶标一般是在远程制导火箭的基础上改型得到。某型远程制导火箭采 用MEMS捷联惯导系统(SINS)和全球卫星导航系统(GNSS)，将当地水平坐标系 作为导航参考坐标系。改型后靶标的制导控制系统设计中，采用发射坐标系作为制导 控制系统的参考坐标系。导航系统为了满足制导控制系统的导航参数需求，需要输出 发射坐标系的相关参数。因此，需要在现有导航算法的基础上，设计将当地水平坐标 系到发射坐标系的坐标转换方法，得到发射坐标系的导航参数，满足制导控制系统的 需求。

发明内容

本发明解决的技术问题是：为了满足制导控制系统的导航参数需求，本发明采用成熟的当地水平坐标系作为导航参考坐标系，输出当地水平坐标系的导航参数。再利 用当地水平坐标系到发射坐标系的坐标转换方法，得到发射坐标系的导航参数，满足 制导控制系统的导航需求。

本发明的技术方案是：一种中高空超声速靶标导航方法，包括以下步骤：

步骤一：捷联惯导系统初始对准，包括以下子步骤：

子步骤一：在当地水平坐标系下，中高空超声速靶标捷联惯导系统通过外部装订参 数方法进行初始对准，得到发射点经度L₀、发射点大地纬度λ₀、发射点椭球高度H₀、 发射方位角A₀以及当地水平坐标系初始航向角ψ_n0；由于是地面静止发射，初始速度 为零；

子步骤二：计算捷联惯导系统的初始俯仰角和滚转角，公式如下：

式中：θ_n0和γ_n0分别为当地水平坐标系下的初始俯仰角和滚转角；f_xm、f_ym和f_zm为右 前上三轴加速度计输出平均值；初始航向角、初始俯仰角和滚转角是姿态数值更新的初始值；

子步骤三：考虑地球自转的影响，利用初始姿态角和装订的位置信息估计三轴陀螺 仪漂移：

式中，ω_xb、ω_yb和ω_zb是三轴陀螺仪的估计漂移，ω_xm、ω_ym和ω_zm是三轴陀螺仪输出平 均值，是初始姿态矩阵，是初始位置矩阵，ω_ie是地球自转角速度，其中：

步骤二：在扣除了三轴陀螺仪漂移的基础上，采用角增量和速度增量进行数字递推以 完成姿态、速度、位置的数值更新；

步骤三：捷联惯导系统/全球卫星导航系统组合导航，包括以下子步骤：

子步骤一：当接收到全球卫星导航系统秒脉冲中断，置当前状态为状态1，并记 录当前时刻t1。

子步骤二：惯组数据为周期性数据，并进行周期性捷联惯导数值更新。更新周期中检测到当前状态为状态1时，则锁存当前的加速度计和陀螺仪数据，以及捷联惯导 的位置速度和姿态数据，作为组合导航的SINS量测量；置当前状态为状态2，并记录 当前时刻t2。

子步骤三：当接收到全球卫星导航系统数据中断，接收全球卫星导航系统数据。如果检测到当前状态为状态2，则置当前状态为状态3，并记录当前时刻t3。

子步骤四：进行组合条件的判断，判断卫星个数是否大于3；卫星模式是否处于定位状态；VDOP与HDOP值的平方和是否小于100；秒脉冲锁存时刻的三个加速度计 平方和是否小于10000；t3-t1的数值小于80ms。如果上述条件均满足，则进行组合导 航算法，并进行惯导修正；否则到子步骤五。

子步骤五：置当前状态为无效，等待秒脉冲中断，回到子步骤一；

步骤四：将当地水平坐标系参数转换到发射系算法，最终输出发射坐标系的相关参数， 包括以下子步骤：

子步骤一：位置信息转换

根据靶标当前的纬经高(L,λ,H)，可得到当前的地固系下的位置p_e＝[x_e,y_e,z_e]^T，即

其中，e是地球偏心率。

根据地固系下的位置，可得到发射系下的当前位置p_g为

式中，p_e0为地固系下靶标发射点的位置初值，为地固系到发射系的转换矩阵，即e 系旋转到g系由3次旋转获得，各次旋转描述如下：

子步骤二：姿态信息转换

发射系下的姿态矩阵由g系→e系→n系→b_n→b_g系多次转换得到：

式中，各姿态矩阵的定义为：

a)、为g系旋转到e系的旋转矩阵，前面已经描述。

b)、为n系旋转到e系的旋转矩阵，由3次旋转获得，涉及靶标经度λ、纬度L， 各次旋转描述如下：

c)、为n系旋转到b_n系的旋转矩阵，由三次旋转获得，如式(1)所示；

d)、为b_n系和b_g系两种弹体坐标系之间的旋转矩阵，

令则得到了靶标在发射系下的三个姿态角为

子步骤三：速度信息转换

根据当地水平系下的速度矢量v_enu，可得到发射系下的速度矢量v_g

式中，和前面已经描述。

子步骤四：攻角侧滑角的扩展

由靶标发射系的速度矢量v_gxyz＝[v_gx,v_gy,v_gz]^T，可得载体系速度矢量则攻角α、侧滑角β为

本发明进一步的技术方案是：所述姿态四元数更新的数字递推算法为：

其中，姿态变化前后的时刻分别为t_m-1、t_m，对应的姿态四元数分别为 是以n系为参考坐标系时b系的变换四元数，它的计算和b系相对于n系的转动 角速度有关，下面为的计算方法。

记Φ_m为b系相对于n系等效旋转矢量，则：

其中T_m＝t_m-t_m-1为捷联惯导更新周期，且记

求姿态更新前后等效旋转矢量Φ(h)的双子样算法为

其中Δθ₁和Δθ₂是[t_m-1,t_m]内角增量的两个等间隔采样值，求姿态更新前后的姿态变 化四元数q(h)

η_m的计算采用双子样算法，即由式得到η_m，进而得到Φ_m， 利用式可求得再代入完成姿态更新。

本发明进一步的技术方案是：所述速度更新的数字递推算法为：

其中，和分别为t_m-1和t_m时刻靶标速度，为t_m-1时刻的姿态矩阵，为 重力/哥式速度增量，为比力速度增量，且

其中，Δv_m为加速度计在[t_m-1,t_m]的输出速度增量

Δv_rotm为速度的旋转效应补偿项，由运载体的线运动方向在空间旋转引起

Δv_sculm为速度的划桨效应补偿项，采用双子样算法计算如下

其中Δθ₁和Δθ₂是[t_m-1,t_m]内角增量的两个等间隔采样值，Δθ_m＝Δθ₁+Δθ₂；Δv₁和Δv₂是 [t_m-1,t_m]内速度增量的两个等间隔采样值，Δv_m＝Δv₁+Δv₂；

本发明进一步的技术方案是：所述位置更新的算法为：

t_m时刻的速度已经更新，因此可以用速度的平均值进行计算，提高精 度：

其中，L为靶标纬度；λ为靶标经度；h为靶标高度；为靶标的北向速度；为靶标的东向速度；为靶标的天向速度。且下标m-1和m分别表示其在t_m-1和t_m时刻的 值。

发明效果

本发明的技术效果在于：本发明在成熟的当地水平坐标系导航算法的基础上，通过坐标转换，得到发射坐标系的导航参数，满足制导控制系统的导航需求，容易与制 导控制系统、仿真系统等进行整合。

附图说明

图1：当地水平坐标系捷联惯导算法编排

图2：组合导航状态流程图

图3：组合导航系统算法流程图

具体实施方式

参见图1—图3，中高空超声速靶标导航算法，包括SINS初始对准、SINS导航数值更新、SINS/GNSS组合导航和坐标转换四部分，其中前三个步骤在当地水平坐标系下 进行，步骤如下：

步骤一：中高空超声速靶标捷联惯导初始对准通过外部装订参数方法，得到发射点经度、纬度和高度，发射方位角以及弹体航向角。对初始对准阶段的陀螺仪和加速 度计数据求取均值，利用加速度计均值计算弹体初始的俯仰角和滚转角；利用姿态角 和位置信息扣除地球自转的影响，估计出三轴陀螺仪漂移。

步骤二：根据捷联惯导数值更新，采用角增量和速度增量进行数字递推以完成姿态、速度、位置的数值更新。

步骤三：SINS/GNSS组合导航，组合导航卡尔曼滤波器状态包括：平台失准角、 速度误差、位置误差、陀螺仪的常值漂移、加速度计的常值偏置，将SINS速度和位 置与GNSS速度和位置对应相减，作为卡尔曼滤波的量测量。

步骤四：当地水平坐标系参数转换到发射系算法，包括(1)位置信息转换，根据 靶标纬经高得到靶标发射系位置。(2)姿态信息转换：靶标在当地水平系下的姿态角 通过多次坐标转换得到靶标在发射系下的三个姿态角。(3)速度信息转换：根据当地 东北天水平系下的速度矢量得到发射系下的速度矢量。(4)攻角侧滑角计算：由靶标 发射坐标系的速度和姿态矩阵，计算得到攻角和侧滑角。

下面对每一步骤，具体进行解释说明：

首先定义本发明中涉及的坐标系：

1)、地球固连坐标系(简称：地固系，e系)。

2)、当地东北天水平坐标系(简称：水平系，n系)，水平系是航空飞行器的导航 参考坐标系。

3)、弹体坐标系(简称：b_n系)，此处采用右前上坐标指向。

弹体坐标系(b_n系)相对于导航坐标系(n系)的姿态角为当地水平系捷联算法使用的姿态角，分别用ψ_n、θ_n和γ_n表示，姿态矩阵如式(1)所示。

其中，R_x，R_y，R_z分别为绕x、y、z轴旋转的余弦矩阵。

4)、惯组坐标系(简称：IMU系)，本文采用右前上坐标指向。

5)、发射坐标系(简称：发射系，g系)，此处采用前上右坐标指向。

6)、弹体坐标系(简称：b_g系)，为前上右坐标指向。弹体的俯仰角为偏航角 为ψ_g，滚转角为γ_g，弹体坐标系(b_g系)相对于发射系(g系)的姿态矩阵如式(2) 所示。

下面对本发明的算法进行详细说明。

(1)SINS初始对准

中高空超声速靶标SINS初始对准通过外部装订参数方法，得到发射点经度L₀、 发射点大地纬度λ₀、发射点椭球高度H₀、发射方位角A₀以及当地水平坐标系初始航 向角ψ_n0。由于是地面静止发射，初始速度为零。初始俯仰角和滚转角计算为：取180s 陀螺仪和加速度计输出的数据求取均值，利用加速度计均值计算弹体初始俯仰角和初 始滚转角；

式中：θ_n0和γ_n0是当地水平坐标系初始俯仰角和滚转角；f_xm、f_ym和f_zm是右前上三轴 加速度计输出平均值。

利用初始姿态角和装订的位置信息扣除地球自转的影响，估计出三轴陀螺仪漂移。 公式如(4)所示。

式中，ω_xb、ω_yb和ω_zb是三轴陀螺仪的估计漂移，ω_xm、ω_ym和ω_zm是三轴陀螺仪输出平 均值，是初始姿态矩阵，是初始位置矩阵，ω_ie是地球自转角速度。

(2)SINS导航数值更新

初始对准完成后，可以转入SINS导航更新状态，SINS数值更新是在扣除出三轴 陀螺仪漂移的基础上进行的。中高空超声速靶标捷联惯导算法编排如图1所示：由获 得的陀螺仪角增量，在扣除地球自转和坐标系转动影响后，进行姿态数字递推， 得到当前的姿态四元数和姿态矩阵且由获得的加速度计速度增量， 通过比力坐标转换，得到当地水平系的速度增量fⁿ，在扣除重力gⁿ和哥氏力影响后， 进行速度数字递推；由前后拍的速度，进行位置数字递推。

①姿态更新算法

姿态四元数更新的数字递推算法为：

其中，姿态变化前后的时刻分别为t_m-1、t_m，对应的姿态四元数分别为 是以n系为参考坐标系时，b系的变换四元数，它的计算和b系相对于n系的转 动角速度有关，下面为的计算方法：

记Φ_m为b系相对于n系等效旋转矢量，则：

其中T_m＝t_m-t_m-1为捷联惯导更新周期，且记

求姿态更新前后等效旋转矢量Φ(h)的双子样算法为

其中Δθ₁和Δθ₂是[t_m-1,t_m]内角增量的两个等间隔采样值，求姿态更新前后的姿态变化 四元数q(h)

η_m的计算采用双子样算法，即由式(10)得到η_m，进而得到Φ_m，利用式(11)可求得再代入(7)完成姿态更新。

②速度更新算法

速度更新的数字递推算法为：

其中，和分别为t_m-1和t_m时刻靶标速度，为t_m-1时刻的姿态矩阵，为 重力/哥式速度增量，为比力速度增量，且

其中，Δv_m为加速度计在[t_m-1,t_m]的输出速度增量

Δv_rotm为速度的旋转效应补偿项，由运载体的线运动方向在空间旋转引起

Δv_sculm为速度的划桨效应补偿项，当运载体同时做线振动和角振动时存在，采用双子样 算法计算如下

其中Δθ₁和Δθ₂是[t_m-1,t_m]内角增量的两个等间隔采样值，Δθ_m＝Δθ₁+Δθ₂；Δv₁和Δv₂是 [t_m-1,t_m]内速度增量的两个等间隔采样值，Δv_m＝Δv₁+Δv₂。

通过(12)～(17)就可以完成速度更新计算。

③位置更新算法

位置更新时，t_m时刻的速度已经更新，因此可以用速度的平均值进行 计算，提高精度：

其中，L为靶标纬度；λ为靶标经度；h为靶标高度；为靶标的北向速度；为靶标的东向速度；为靶标的天向速度。且下标m-1和m分别表示其在t_m-1和t_m时刻的 值。

(3)SINS/GNSS组合导航

SINS/GNSS组合导航包括组合导航状态流程和组合导航算法。

①组合导航状态流程图如图2所示，详细流程如下：

a.当接收到GNSS秒脉冲(PPS)中断，置当前状态为状态1，并记录当前时刻 t1。

b.惯组数据为周期性数据，并进行周期性SINS导航数值更新。更新周期中检测 到当前状态为状态1时，则锁存当前的加速度计和陀螺仪数据，以及惯性导 航的位置速度和姿态数据，作为组合导航的SINS量测量；置当前状态为状态 2，并记录当前时刻t2。

c.当接收到GNSS数据中断，接收GNSS数据(如卫星个数、卫星模式、DOP 值等)。如果检测到当前状态为状态2，则置当前状态为状态3，并记录当前 时刻t3。

d.进行组合条件的判断，判断卫星个数是否大于3；卫星模式是否处于定位状态；VDOP与HDOP值的平方和是否小于100；秒脉冲锁存时刻的三个加速度计 平方和是否小于10000；t3-t1的数值小于80ms。如果上述条件均满足，则进 行组合导航算法，并进行惯导修正；否则到步骤e。

e.置当前状态为无效，等待秒脉冲中断，回到步骤a。

②组合导航算法

组合导航算法流程如图3所示。其中，P_I,V_I,A_I分别为捷联惯导输出的靶标位置、速度、姿态，P_p,V_p分别为GNSS输出的靶标位置、速度。选取水平系为导航坐 标系，采用卡尔曼滤波进行组合导航的信息融合。

选取水平系为导航坐标系，采用卡尔曼滤波进行组合导航的信息融合。

组合导航的状态方程为：

式中X是系统状态向量；F是状态转移矩阵；G是系统噪声驱动矩阵；W是系统噪声矩阵。状态向量X为

其中，为导航坐标系下的三个平台失准角，δv_E、δv_N、δv_U为三个方向的速度误差，δL、δλ、δh为三个位置误差，ε_bx、ε_by、ε_bz为陀螺仪的三个方向常 值漂移，为加速度计的三个方向的常值偏置。

状态转移矩阵F为

为n系相对于i系的旋转角速度，后缀×表示求反对称矩阵，且F₀～F₄分别为：

其中R_N为卯酉圈半径、R_M为子午圈半径。

系统噪声驱动矩阵为

组合导航的量测方程为

Z_PV＝HX+V_PV (25)

其中H为量测矩阵，V_PV为量测噪声，将SINS输出的靶标速度和位置与卫星导航系统输出的速度和位置对应相减作为量测量Z_PV，即

Z_PV＝[L_I-L_p λ_I-λ_p h_I-h_p v_EI-v_Ep v_NI-v_Np v_UI-v_Up]^T(26)

结合前面所选取的状态向量X，可以列写出设计的组合导航系统的速度、位置误差量测方程

(4)当地水平坐标系参数转换到发射系算法

①位置信息转换

根据靶标当前的纬经高(L,λ,H)，可得到当前的地固系下的位置p_e＝[x_e,y_e,z_e]^T，即

其中，e是地球偏心率。

根据地固系下的位置，可得到发射系下的实际位置p_g为

式(29)中，p_e0为地固系下靶标发射点的位置初值，为地固系到发射系的转换矩阵， 即e系旋转到g系由3次旋转获得，涉及靶标初始经度L₀、纬度λ₀和发射方位角A₀，各 次旋转描述如下：

②姿态信息转换

从n系到b_n系的姿态转换如式(1)所示。根据坐标转换理论，由g系→e系→n系→b_n→b_g系多次转换得到发射系下的姿态矩阵如式(31)所示。

式(31)中，各姿态矩阵的定义为：

a)、为g系旋转到e系的旋转矩阵，前面已经描述。

b)、为n系旋转到e系的旋转矩阵，由3次旋转获得，涉及靶标经度λ、纬度L， 各次旋转描述如下：

c)为n系旋转到b_n系的旋转矩阵，由三次旋转获得，如式(1)所示；

d)、为b_n系和b_g系两种弹体坐标系之间的旋转矩阵，

令则得到了靶标在发射系下的三个姿态角为

②速度信息转换

根据当地水平系下的速度矢量v_enu，可得到发射系下的速度矢量v_g

式(34)中，和前面已经描述。

④攻角侧滑角的扩展

发射坐标系的速度矢量v_gxyz＝[v_gx,v_gy,v_gz]^T，由靶标发射坐标系的速度可得载体系速 度则攻角α、侧滑角β为

Claims (4)

1.一种中高空超声速靶标导航方法，其特征在于，包括以下步骤：

步骤一：捷联惯导系统初始对准，包括以下子步骤：

子步骤一：在当地水平坐标系下，中高空超声速靶标捷联惯导系统通过外部装订参数方法进行初始对准，得到发射点经度L₀、发射点大地纬度λ₀、发射点椭球高度H₀、发射方位角A₀以及当地水平坐标系初始航向角ψ_n0；由于是地面静止发射，初始速度为零；

子步骤二：计算捷联惯导系统的初始俯仰角和滚转角，公式如下：

式中：θ_n0和γ_n0分别为当地水平坐标系下的初始俯仰角和滚转角；f_xm、f_ym和f_zm为右前上三轴加速度计输出平均值；

子步骤三：考虑地球自转的影响，利用初始姿态角和装订的位置信息估计三轴陀螺仪漂移：

式中，ω_xb、ω_yb和ω_zb是三轴陀螺仪的估计漂移，ω_xm、ω_ym和ω_zm是三轴陀螺仪输出平均值，是初始姿态矩阵，是初始位置矩阵，ω_ie是地球自转角速度，其中：

步骤二：在扣除了三轴陀螺仪漂移的基础上，采用角增量和速度增量进行数字递推以完成姿态、速度、位置的数值更新；

步骤三：捷联惯导系统/全球卫星导航系统组合导航，包括以下子步骤：

子步骤一：当接收到全球卫星导航系统秒脉冲中断，置当前状态为状态1，并记录当前时刻t1。

子步骤二：惯组数据为周期性数据，并进行周期性捷联惯导数值更新。更新周期中检测到当前状态为状态1时，则锁存当前的加速度计和陀螺仪数据，以及捷联惯导的位置速度和姿态数据，作为组合导航的SINS量测量；置当前状态为状态2，并记录当前时刻t2。

子步骤三：当接收到全球卫星导航系统数据中断，接收全球卫星导航系统数据。如果检测到当前状态为状态2，则置当前状态为状态3，并记录当前时刻t3。

子步骤四：进行组合条件的判断，判断卫星个数是否大于3；卫星模式是否处于定位状态；VDOP与HDOP值的平方和是否小于100；秒脉冲锁存时刻的三个加速度计平方和是否小于10000；t3-t1的数值小于80ms。如果上述条件均满足，则进行组合导航算法，并进行惯导修正；否则到子步骤五。

子步骤五：置当前状态为无效，等待秒脉冲中断，回到子步骤一；

步骤四：将当地水平坐标系参数转换到发射系算法，最终输出发射坐标系的相关参数，包括以下子步骤：

子步骤一：位置信息转换

根据靶标当前的纬经高(L,λ,H)，可得到当前的地固系下的位置p_e＝[x_e,y_e,z_e]^T，即

其中，e是地球偏心率。

根据地固系下的位置，可得到发射系下的当前位置p_g为

式中，p_e0为地固系下靶标发射点的位置初值，为地固系到发射系的转换矩阵，即e系旋转到g系由3次旋转获得，各次旋转描述如下：

子步骤二：姿态信息转换

发射系下的姿态矩阵由g系→e系→n系→b_n→b_g系多次转换得到：

式中，各姿态矩阵的定义为：

a)、为g系旋转到e系的旋转矩阵，前面已经描述。

b)、为n系旋转到e系的旋转矩阵，由3次旋转获得，涉及靶标经度λ、纬度L，各次旋转描述如下：

c)、为n系旋转到b_n系的旋转矩阵，由三次旋转获得；

d)、为b_n系和b_g系两种弹体坐标系之间的旋转矩阵，

令则得到了靶标在发射系下的三个姿态角为

子步骤三：速度信息转换

根据当地水平系下的速度矢量v_enu，可得到发射系下的速度矢量v_g

式中，和前面已经描述。

子步骤四：攻角侧滑角的扩展

由靶标发射系的速度矢量v_gxyz＝[v_gx,v_gy,v_gz]^T，可得载体系速度矢量则攻角α、侧滑角β为

2.如权利要求1所述的一种中高空超声速靶标导航方法，其特征在于，所述姿态四元数更新的数字递推算法为：

其中，姿态变化前后的时刻分别为t_m-1、t_m，对应的姿态四元数分别为 是以n系为参考坐标系时b系的变换四元数，它的计算和b系相对于n系的转动角速度有关，下面为的计算方法。

记Φ_m为b系相对于n系等效旋转矢量，则：

其中T_m＝t_m-t_m-1为捷联惯导更新周期，且记

求姿态更新前后等效旋转矢量Φ(h)的双子样算法为

其中Δθ₁和Δθ₂是[t_m-1,t_m]内角增量的两个等间隔采样值，求姿态更新前后的姿态变化四元数q(h)

η_m的计算采用双子样算法，即由式得到η_m，进而得到Φ_m，利用式可求得再代入完成姿态更新。

3.如权利要求1所述的一种中高空超声速靶标导航方法，其特征在于，所述速度更新的数字递推算法为：

其中，和分别为t_m-1和t_m时刻靶标速度，为t_m-1时刻的姿态矩阵，为重力/哥式速度增量，为比力速度增量，且

其中，Δv_m为加速度计在[t_m-1,t_m]的输出速度增量

Δv_rotm为速度的旋转效应补偿项，由运载体的线运动方向在空间旋转引起

Δv_sculm为速度的划桨效应补偿项，采用双子样算法计算如下

其中Δθ₁和Δθ₂是[t_m-1,t_m]内角增量的两个等间隔采样值，Δθ_m＝Δθ₁+Δθ₂；Δv₁和Δv₂是[t_m-1,t_m]内速度增量的两个等间隔采样值，Δv_m＝Δv₁+Δv₂。

4.如权利要求1所述的一种中高空超声速靶标导航方法，其特征在于，所述位置更新的算法为：

t_m时刻的速度已经更新，因此可以用速度的平均值进行计算，提高精度：

其中，L为靶标纬度；λ为靶标经度；h为靶标高度；为靶标的北向速度；为靶标的东向速度；为靶标的天向速度。且下标m-1和m分别表示其在t_m-1和t_m时刻的值。