CN111721291A - 一种发射系下捷联惯组导航的工程算法 - Google Patents

Abstract

本发明公开了一种发射系下捷联惯组导航的工程算法,适用于飞行器导航领域。包括以下步骤：建立载体b坐标系和发射g坐标系；建立导航初始状态；更新飞行器姿态；更新本地重力加速度；更新飞行器实时速度；更新飞行器实时位置。本发明基于发射g坐标系进行导航解算，发射系与地球固联，其位置、速度和姿态导航参数是相对于地球的，与地面飞控系统需求的导航信息一致，有利于人的直观描述和理解。本发明发射g坐标系采用J2重力模型，考虑了当地水平的南北向重力影响，速度更新算法考虑了由于地球自转产生的牵引加速度和柯式加速度在飞行器自由飞行段中的影响，适合所有短程助推‑滑翔供靶飞行器的导航使用，适合与卫星导航进行组合导航。

Description

一种发射系下捷联惯组导航的工程算法

技术领域

本发明涉及飞行器导航领域，具体地说涉及一种发射系下捷联惯组导航的工程算法。

背景技术

供靶飞行器可以较真实的模拟敌方来袭飞行器的运动特性，是部队训练和武器定型时必不可少的考核手段。供靶飞行器以助推-滑翔飞行的方式飞行至预定区域(弹道定点附近)后，控制舵面偏转，使弹体产生相应的飞行攻角，弹体进入巡航或机动飞行状态，为防空武器提供一个水平飞行并有一定机动能力的待拦截目标。该靶弹采用低成本的捷联惯组进行导航，结合嵌入式飞行控制计算机，形成控制系统，在满足靶弹基本性能要求的同时，能够很大程度的降低费用。

选择合适的导航参考坐标系，有利于飞行器导航制导系统的设计和应用。在常用的地球导航定位方法中，一般都是以地球中心为原点，采用与地球相固联的坐标系作为基准进行定位，如地心固联坐标系。对于助推-滑翔式靶弹，飞行距离有限，其飞行剖面划分为助推段、自由弹道段(含滑翔供靶)和下降段等飞行阶段，自由段的射程和飞行时间占全弹道的80％～90％以上，是靶弹最重要的供靶时间，此时靶弹沿着地球表面飞行，控制算法期望用描述飞行器与地面相对关系的姿态、位置导航数据来进行飞行控制，也有利于人的直观描述和理解。在工程实际中，常使用以地面台、站中心为坐标原点建立起来的发射坐标系作为靶弹。

发明内容

本发明所要解决的技术问题是如何提供一种综合考虑当地水平的南北向重力和由于地球自转产生的牵引加速度、柯式加速度在飞行器中的影响的发射系下捷联惯组导航的工程算法。

为解决上述技术问题，本发明所采取的技术方案是：包括以下步骤：

步骤一：以飞行器为载体，建立载体b坐标系，坐标原点为飞行器的质心， x_b轴指向头部，y_b轴在飞行器的主对称面内向上为正；

以发射点为原点建立发射g坐标系，x_g轴在发射点水平面内，指向发射瞄准方向，y_g轴垂直于发射点水平面指向上方，所述发射g坐标系为前上右坐标系，设定发射g坐标系作为导航n坐标系；

步骤二：根据飞行器发射的初始信息，建立导航初始状态，包括以下子步骤：

子步骤一：计算初始四元数：设弹体的初始俯仰角为

、滚转角为γ₀、航向角为ψ₀，代入四元数方程得到载体b坐标系向发射g坐标系转换的四元数：

归一化的四元数为：

子步骤二：建立初始姿态矩阵：根据四元数与姿态矩阵的关系得到载体b 坐标系向发射g坐标系转换姿态矩阵

子步骤三：初始化本地地球参数：设发射点的纬度为B₀，经度为U₀，方位角为A₀，地球自转角速度ω_e在发射g坐标系下的投影分量为：

ω_ex＝ω_e cos(B₀)*cos(A₀)

ω_ey＝ω_e sin(B₀)

ω_ez＝-ω_e cos(B₀)*sin(A₀)

地球自转产生的柯式加速度a_c＝-2ω_e×f_i在发射g坐标系各轴上的分量矩阵为：

地球自转产生的牵连加速度

设自转角加速度为ω′_e，飞行器的地心矢量为

牵连加速度在发射g坐标系各轴上各分量的转换矩阵为：

地球重力加速度g在发射g坐标系中可表述为：

其中：

地球引力系数σ＝3.986005*1014，

地球长半轴a_ε＝6378140米，φ为地心纬度；发射点的地心矢量为：

步骤三：更新飞行器姿态：包括以下子步骤：

子步骤一：采用四阶毕卡增量算法更新四元素，四阶毕卡公式为：

式中：

得到更新后四元数：

做归一化处理后，代入公式(3)得到更新后的姿态矩阵；

子步骤二：利用更新后的姿态矩阵，计算三向姿态角为：

俯仰角：

偏航角：ψ′＝arctan(k₂/k₁)

其中：

滚转角：γ′＝rctan(k₂/k₁)

其中，

步骤四：更新本地重力加速度：设发射点到飞行器当前位置的矢量在发射坐标系的三分量为[x,y,z]T，则其地心矢量在发射g坐标系的分量为：

代入公式(6)得到重力加速度更新公式：

步骤五：更新飞行器实时速度，包括以下子步骤：

子步骤一：根据质点的动力学理论，建立飞行器在发射坐标系中动力学矢量公式为：

其中：

为载体的加速度矢量，

为地球引力加速度矢量，

为柯氏加速度矢量；

为牵连加速度矢量；

子步骤二：依据步骤二中建立的柯式加速度在发射坐标系各轴上的分量矩阵，得到飞行器的柯氏加速度

在发射坐标系各轴上的分量：

子步骤三：依据步骤二中建立的牵连加速度在发射坐标系各轴上各分量的转换矩阵，得到飞行器的牵连加速度

在发射坐标系各轴上的分量为：

其中，R_0i(i＝x，y，z)为发射点地心矢径在发射坐标系各轴上的分量； x、y、z为靶弹的地心矢径r在发射坐标系各轴上的分量；

子步骤四：建立实际工程中飞行器在发射系下速度的更新公式：

步骤六：更新飞行器实时位置：对步骤五中更新后的速度进行积分，即可更新发射g坐标系下的导航位置：

进一步的，还包括发射g坐标系下的惯组导航算法编排方法：建立导航初始化状态，计算初始姿态矩阵，初始化本地地球参数；测量陀螺仪相对惯性空间的角速率在载体b坐标系中的投影和加速度计相对惯性空间的比力在载体b 坐标系中的投影；通过姿态矩阵将角速率和比力转化为发射g坐标系中的物理量；在角速度积分环路中，利用测得的角速度按四元数积分方法修正载体b坐标系至发射g坐标系的坐标变换矩阵，根据所述坐标变换矩阵计算姿态角；在加速度积分环路中，利用所述坐标变换矩阵将观测量转换至发射g坐标系，并从其中补偿重力加速度g、柯式加速度a_c和牵连加速度a_e，经过积分得到速度与位置导航信息。

进一步的，还包括解算流程：导航开始后，导航计算机读取导航初始化参数，进行初始对准；计算初始四元数、初始姿态矩阵并解算地球参数，建立导航初始状态；按导航周期，循环读取补偿后的惯组数据；按导航数值更新算法依次更新四元数和姿态矩阵；计算姿态角并更新飞行器的实时速度和位置；向制导和控制系统按格式要求输出导航参数；在下个导航周期中再读取补偿后的惯组参数，进行循环。

进一步的，解算的主处理器为TMS320C6747芯片。

采用上述技术方案所产生的有益效果在于：本发明基于发射系进行导航解算，其中发射系与地球固联，其位置、速度和姿态导航参数是相对于地球的，与很多地面发射飞行器飞控系统需求的导航信息一致，有利于人的直观描述和理解。本发明中发射系采用J2重力模型，考虑了当地水平的南北向重力影响，速度更新算法考虑了由于地球自转产生的牵引加速度和柯式加速度在飞行器自由飞行段中的影响，适合所有短程助推-滑翔供靶飞行器的导航使用，适合与卫星导航进行组合导航。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1是本发明的惯组导航算法编排框图；

图2是本发明的捷联惯组导航解算流程图；

图3是本发明的靶弹飞行器飞行轨迹(无控)三维图；

图4是本发明仿真后的发射系下姿态误差；

图5是本发明仿真后的发射系下速度误差；

图6是本发明仿真后的发射系下位置误差。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

本发明涉及的坐标系如下：

1)地心地固e坐标系(地固系)，原点为地球中心，x_e、y_e轴在地球赤道平面内，x_e指向本初子午线，z_e轴为地球自转轴。

2)载体b坐标系(载体系)，坐标原点为飞行器的质心，x_b轴指向头部， y_b轴在飞行器的主对称面内，向上为正，载体b坐标系为前上右坐标系。

3)发射g坐标系(发射系)，坐标原点与发射点固连，x_g轴在发射点水平面内，指向发射瞄准方向，y_g轴垂直于发射点水平面指向上方，发射系为前上右坐标系。

4)导航n坐标系，是在导航时根据导航n坐标系统工作的需要而选取的作为导航基准的坐标系，导航坐标系又称自由方位系统。本发明设定发射坐标系为导航坐标系。

本发明以飞行器为载体，建立载体b坐标系，以坐标原点为飞行器的质心， x_b轴指向头部，y_b轴在飞行器的主对称面内向上为正。以发射点为原点建立发射g坐标系，x_g轴在发射点水平面内，指向发射瞄准方向，y_g轴垂直于发射点水平面指向上方，所述发射g坐标系为前上右坐标系，设定发射g坐标系作为导航n坐标系。

在本发明的一个实施例中，捷联惯组导航n坐标系统固连在飞行器上，所述飞行器为助推-滑翔飞行器，测量值为载体b坐标系下的物理量。陀螺仪组件输出的是飞行器相对惯性空间转动的角速率在载体b坐标系中的投影

加速度计组件输出的是飞行器相对惯性空间的比力在载体b坐标系中的投影

导航计算需要在导航n坐标系中完成，首先通过姿态矩阵

将载体b坐标系中的物理量

转化为导航n坐标系中的物理量

姿态矩阵

随时间的变化而不断变化，捷联惯组导航系统中的导航解算问题就是事实地求取姿态矩阵，计算得出载体姿态角以及变换比力，得到实时速度、实时位置信息的过程。

如图1所示为惯组导航系统的算法编排原理图。建立导航初始化状态，计算初始姿态矩阵，初始化本地地球参数；测量陀螺仪相对惯性空间的角速率在载体b坐标系中的投影和加速度计相对惯性空间的比力在载体b坐标系中的投影；通过姿态矩阵将角速率和比力转化为发射g坐标系中的物理量；在角速度积分环路中，利用测得的角速度按四元数积分方法修正载体b坐标系至发射g 坐标系的坐标变换矩阵，根据所述坐标变换矩阵计算姿态角；在加速度积分环路中，利用所述坐标变换矩阵将观测量转换至发射g坐标系，并从其中补偿重力加速度g、柯式加速度a_c和牵连加速度a_e，经过两次积分得到速度与位置导航信息。

本发明包括以下步骤：

步骤一：以飞行器为载体，建立载体b坐标系，坐标原点为飞行器的质心， x_b轴指向头部，y_b轴在飞行器的主对称面内向上为正；

以发射点为原点建立发射g坐标系，x_g轴在发射点水平面内，指向发射瞄准方向，y_g轴垂直于发射点水平面指向上方，所述发射g坐标系为前上右坐标系，设定发射g坐标系作为导航n坐标系；

步骤二：根据飞行器发射的初始信息，建立导航初始状态，包括以下子步骤：

子步骤一：计算初始四元数

四元数具有可以全姿态工作、计算工作量小并且计算方法成熟的特点，工程上计算姿态捷联惯组导航算法时多采用四元数法。设弹体的初始俯仰角为

、滚转角为γ₀、航向角为ψ₀，代入四元数方程得到载体b坐标系向发射g坐标系转换的四元数：

归一化的四元数为：

子步骤二：建立初始姿态矩阵

根据四元数与姿态矩阵的关系得到载体b坐标系向发射g坐标系转换的姿态矩阵

子步骤三：初始化本地地球参数

地球参数包括地球自转角速度在发射系下的投影分量、因地球自转产生的柯式加速度在发射坐标系下的投影分量、牵连加速度在发射坐标系下的投影分量和所在位置的重力加速度信息。发射g坐标系采用J2重力模型，设发射点的纬度为B₀，经度为U₀，方位角为A₀，地球自转角速度ω_e在发射g坐标系下的投影分量为：

ω_ex＝ω_e cos(B₀)*cos(A₀)

ω_ey＝ω_e sin(B₀)

ω_ez＝-ω_e cos(B₀)*sin(A₀)

由理论力学可知，由地球自转产生的柯式加速度为a_c＝-2ω_e×f_i。依据矢量外积运算，柯式加速度在发射坐标系各轴上的分量矩阵为：

同理，由理论力学可知，由地球自转产生的牵连加速度为

其中，ω′_e为自转角加速度，

为飞行器的地心矢径。牵连加速度在发射坐标系各轴上各分量的转换矩阵为：

地球重力加速度g在发射g坐标系中可表述为：

其中：：

地球引力系数σ＝3.986005*1014，

地球长半轴a_ε＝6378140米，φ为地心纬度；发射点的地心矢量为：

步骤三：更新飞行器姿态

包括以下步骤：

子步骤一：采用四阶毕卡增量算法，直接使用捷联惯组导航系统的陀螺输出角增量Δθ，避免噪声微分放大。四阶毕卡公式为：

其中：

在靶弹的实际工程实现中，为了提高计算精度，采用公式10更新四元数：

更新后的四元数做归一化处理后，代入公式(3)得到更新后的姿态矩阵；

子步骤二：利用更新后的姿态矩阵，计算三向姿态角为：

俯仰角

偏航角ψ：ψ′＝arctan(k₂/k₁)

其中：

滚转角γ：

γ′＝rctan(k₂/k₁)

其中：

步骤四：更新本地重力加速度

设发射点到飞行器当前位置的矢量在发射坐标系的三分量为[x,y,z]^T，则其地心矢量在发射g坐标系的分量为：

代入公式(6) 得到重力加速度更新公式：

步骤五：更新飞行器实时速度，包括以下步骤：

子步骤一：根据质点的动力学理论，建立飞行器在发射g坐标系中的动力学矢量方程为(忽略空气阻力的影响)：

其中，

为飞行器的加速度矢量，

为地球引力加速度矢量，

为柯式加速度矢量，

为牵连加速度矢量。

子步骤二：依据步骤二中建立的柯式加速度

在发射g坐标系各轴上的分量为：

子步骤三：依据步骤二中建立的牵连加速度在发射坐标系各轴上各分量的转换矩阵，得到飞行器的牵连加速度

在发射g坐标系各轴上的分量为：

其中，R_0i(i＝x,y,z)为发射点地心矢径r在发射坐标系各轴上的分量；x、y、 z为靶弹的地心矢径r在发射坐标系各轴上的分量。

子步骤四：建立实际工程中飞行器在发射g坐标系下速度的更新公式为：

步骤六：更新飞行器实时位置

对步骤五中的速度进行积分，即可更新发射g坐标系下的导航位置：

在本发明的一个实施例中，捷联惯组算法的工程实现采用TI公司的浮点型 DSP芯片TMS320C6747作为导航解算的主处理器，如图2所示为捷联惯组导航解算流程：导航开始后，导航计算机读取导航初始化参数，进行初始对准；计算初始四元数、初始姿态矩阵并解算地球参数，建立导航初始状态；按导航周期，循环读取补偿后的惯组数据；按导航数值更新算法依次更新四元数和姿态矩阵；计算姿态角并更新飞行器的实时速度和位置；向制导和控制系统按格式要求输出导航参数；在下个导航周期中再读取补偿后的惯组参数，进行循环。

采用真实的靶弹飞行器飞行轨迹和数据对本发明进行了数字仿真。靶弹无控弹道轨迹的初始状态为：初速为V_0x＝15.502m/s、V_0y＝13.958m/s、V_0z＝0，方位角A₀＝0.00062805(弧度)、纬度B₀＝0.7985026(弧度)、经度U₀＝2.141020974 (弧度)，俯仰角

，高度h＝0，仿真步长为dt＝0.01s。飞行轨迹如图3 所示。

如图4～图6所示，捷联惯组导航在67.7秒仿真时间内三个姿态角误差基本能收敛到0.02°以内(弹道轨迹中偏航角设置为零，该方向数值可以不考虑)；三个方向的速度误差能收敛到1m/s以内；Y轴方向的位置误差能收敛到20m以内，X轴方向的位置误差在80米，呈现发散趋势。从仿真结果看，本发明提出的算法计算精度能够满足供靶飞行器的需求，算法复杂度和实现难度较低，满足了工程上的需求。

本发明中发射g坐标系与地球固联，其位置、速度和姿态导航参数是相对于地球的，与很多地面发射飞行器飞控系统需求的导航信息一致，有利于人的直观描述和理解。发射g坐标系采用J2重力模型，考虑了当地水平的南北向重力影响，速度更新算法考虑了由于地球自转产生的牵引加速度和柯式加速度在飞行器自由飞行段中的影响，适合所有短程助推-滑翔供靶飞行器的导航使用，适合与卫星导航进行组合导航。

Claims (5)

1.一种发射系下捷联惯组导航的工程算法，其特征在于：包括以下步骤：

步骤一：以飞行器为载体，建立载体b坐标系，坐标原点为飞行器的质心，x_b轴指向头部，y_b轴在飞行器的主对称面内向上为正；

以发射点为原点建立发射g坐标系，x_g轴在发射点水平面内，指向发射瞄准方向，y_g轴垂直于发射点水平面指向上方，所述发射g坐标系为前上右坐标系，设定发射g坐标系作为导航n坐标系；

步骤二：根据飞行器发射的初始信息，建立导航初始状态，包括以下子步骤：

子步骤一：计算初始四元数：设弹体的初始俯仰角为

滚转角为γ₀、航向角为ψ₀，代入四元数方程得到载体b坐标系向发射g坐标系转换的四元数：

归一化的四元数为：

子步骤二：建立初始姿态矩阵：根据四元数与姿态矩阵的关系得到载体b坐标系向发射g坐标系转换姿态矩阵

子步骤三：初始化本地地球参数：设发射点的纬度为B₀，经度为U₀，方位角为A₀，地球自转角速度ω_e在发射g坐标系下的投影分量为：

ω_ex＝ω_ecos(B₀)*cos(A₀)

ω_ey＝ω_esin(B₀)

ω_ez＝-ω_ecos(B₀)*sin(A₀)

地球自转产生的柯式加速度a_c＝-2ω_e×f_i在发射g坐标系各轴上的分量矩阵为：

地球自转产生的牵连加速度

设自转角加速度为ω′_e，飞行器的地心矢量为

牵连加速度在发射g坐标系各轴上各分量的转换矩阵为：

地球重力加速度g在发射g坐标系中可表述为：

其中：

地球引力系数σ＝3.986005*1014，

地球长半轴a_ε＝6378140米，φ为地心纬度；发射点的地心矢量为：

步骤三：更新飞行器姿态：包括以下子步骤：

子步骤一：采用四阶毕卡增量算法更新四元素，四阶毕卡公式为：

其中：

得到更新后四元数：

做归一化处理后，代入公式(3)得到更新后的姿态矩阵；

子步骤二：利用更新后的姿态矩阵，计算三向姿态角为：

俯仰角

偏航角ψ：ψ′＝arctan(k₂/k₁)

其中：

滚转角γ：γ′＝rctan(k₂/k₁)

其中：

步骤四：更新本地重力加速度：设发射点到飞行器当前位置的矢量在发射坐标系的三分量为[x,y,z]T，则其地心矢量在发射g坐标系的分量为：

代入公式(6)得到重力加速度更新公式：

步骤五：更新飞行器实时速度，包括以下子步骤：

子步骤一：根据质点的动力学理论，建立飞行器在发射坐标系中动力学矢量公式为：

其中：

为载体的加速度矢量，

为地球引力加速度矢量，

为柯氏加速度矢量，

为牵连加速度矢量；

子步骤二：依据步骤二中建立的柯式加速度在发射坐标系各轴上的分量矩阵，得到飞行器的柯氏加速度

在发射坐标系各轴上的分量：

子步骤三：依据步骤二中建立的牵连加速度在发射坐标系各轴上各分量的转换矩阵，得到飞行器的牵连加速度

在发射坐标系各轴上的分量为：

其中，R_0i(i＝x，y，z)为发射点地心矢径在发射坐标系各轴上的分量；x、y、z为靶弹的地心矢径r在发射坐标系各轴上的分量；

子步骤四：建立实际工程中飞行器在发射系下速度的更新公式：

步骤六：更新飞行器实时位置：对步骤五中更新后的速度进行积分，即可更新发射g坐标系下的导航位置：

2.根据权利要求1所述的一种发射系下捷联惯组导航的工程算法，其特征在于：还包括发射g坐标系下的惯组导航算法编排方法：建立导航初始化状态，计算初始姿态矩阵，初始化本地地球参数；测量陀螺仪相对惯性空间的角速率在载体b坐标系中的投影和加速度计相对惯性空间的比力在载体b坐标系中的投影；通过姿态矩阵将角速率和比力转化为发射g坐标系中的物理量；在角速度积分环路中，利用测得的角速度按四元数积分方法修正载体b坐标系至发射g坐标系的坐标变换矩阵，根据所述坐标变换矩阵计算姿态角；在加速度积分环路中，利用所述坐标变换矩阵将观测量转换至发射g坐标系，并从其中补偿重力加速度g、柯式加速度a_c和牵连加速度a_e，经过积分得到速度与位置导航信息。

3.根据权利要求1所述的一种发射系下捷联惯组导航的工程算法，其特征在于：还包括解算流程：导航开始后，导航计算机读取导航初始化参数，进行初始对准；计算初始四元数、初始姿态矩阵并解算地球参数，建立导航初始状态；按导航周期，循环读取补偿后的惯组数据；按导航数值更新算法依次更新四元数和姿态矩阵；计算姿态角并更新飞行器的实时速度和位置；向制导和控制系统按格式要求输出导航参数；在下个导航周期中再读取补偿后的惯组参数，进行循环。

4.根据权利要求3所述的一种发射系下捷联惯组导航的工程算法，其特征在于：所述惯组数据包括重力加速度g、柯式加速度a_c和牵连加速度a_e。

5.根据权利要求1所述的一种发射系下捷联惯组导航的工程算法，其特征在于：解算的主处理器为TMS320C6747芯片。

Images