技术摘要：
本发明公开了一种发射系下捷联惯组导航的工程算法,适用于飞行器导航领域。包括以下步骤：建立载体b坐标系和发射g坐标系；建立导航初始状态；更新飞行器姿态；更新本地重力加速度；更新飞行器实时速度；更新飞行器实时位置。本发明基于发射g坐标系进行导航解算，发射系  全部
背景技术：
供靶飞行器可以较真实的模拟敌方来袭飞行器的运动特性，是部队训练和武器定 型时必不可少的考核手段。供靶飞行器以助推-滑翔飞行的方式飞行至预定区域(弹道定点 附近)后，控制舵面偏转，使弹体产生相应的飞行攻角，弹体进入巡航或机动飞行状态，为防 空武器提供一个水平飞行并有一定机动能力的待拦截目标。该靶弹采用低成本的捷联惯组 进行导航，结合嵌入式飞行控制计算机，形成控制系统，在满足靶弹基本性能要求的同时， 能够很大程度的降低费用。 选择合适的导航参考坐标系，有利于飞行器导航制导系统的设计和应用。在常用 的地球导航定位方法中，一般都是以地球中心为原点，采用与地球相固联的坐标系作为基 准进行定位，如地心固联坐标系。对于助推-滑翔式靶弹，飞行距离有限，其飞行剖面划分为 助推段、自由弹道段(含滑翔供靶)和下降段等飞行阶段，自由段的射程和飞行时间占全弹 道的80％～90％以上，是靶弹最重要的供靶时间，此时靶弹沿着地球表面飞行，控制算法期 望用描述飞行器与地面相对关系的姿态、位置导航数据来进行飞行控制，也有利于人的直 观描述和理解。在工程实际中，常使用以地面台、站中心为坐标原点建立起来的发射坐标系 作为靶弹。
技术实现要素：
本发明所要解决的技术问题是如何提供一种综合考虑当地水平的南北向重力和 由于地球自转产生的牵引加速度、柯式加速度在飞行器中的影响的发射系下捷联惯组导航 的工程算法。 为解决上述技术问题，本发明所采取的技术方案是：包括以下步骤： 步骤一：以飞行器为载体，建立载体b坐标系，坐标原点为飞行器的质心，  xb轴指 向头部，yb轴在飞行器的主对称面内向上为正； 以发射点为原点建立发射g坐标系，xg轴在发射点水平面内，指向发射瞄准方向，yg 轴垂直于发射点水平面指向上方，所述发射g坐标系为前上右坐标系，设定发射g坐标系作 为导航n坐标系； 步骤二：根据飞行器发射的初始信息，建立导航初始状态，包括以下子步骤： 子步骤一：计算初始四元数：设弹体的初始俯仰角为 、滚转角为γ0、航向角为 ψ0，代入四元数方程得到载体b坐标系向发射g坐标系转换的四元数： 6 CN 111721291 A 说　明　书 2/10 页 归一化的四元数为： 子步骤二：建立初始姿态矩阵：根据四元数与姿态矩阵的关系得到载体b  坐标系 向发射g坐标系转换姿态矩阵 子步骤三：初始化本地地球参数：设发射点的纬度为B0，经度为U0，方位角为A0，地 球自转角速度ωe在发射g坐标系下的投影分量为： ωex＝ωe  cos(B0)*cos(A0) ωey＝ωe  sin(B0) ωez＝-ωe  cos(B0)*sin(A0) 地球自转产生的柯式加速度ac＝-2ωe×fi在发射g坐标系各轴上的分量矩阵为： 地球自转产生的牵连加速度 设自转角加速度为ω′e，飞行器的 地心矢量为 牵连加速度在发射g坐标系各轴上各分量的转换矩阵为： 地球重力加速度g在发射g坐标系中可表述为： 7 CN 111721291 A 说　明　书 3/10 页 其中： 地球引力系数σ＝ 3.986005*1014， 地球长半轴aε＝6378140米，φ为地心纬度；发射点的 地心矢量为： 步骤三：更新飞行器姿态：包括以下子步骤： 子步骤一：采用四阶毕卡增量算法更新四元素，四阶毕卡公式为： 式中： 得到更新后四元数： 做归一化处理后，代入公式(3)得到更新后的姿态矩阵； 子步骤二：利用更新后的姿态矩阵，计算三向姿态角为： 俯仰角： 偏航角：ψ′＝arctan(k2/k1) 其中： 滚转角：γ′＝rctan(k2/k1) 8 CN 111721291 A 说　明　书 4/10 页 其中， 步骤四：更新本地重力加速度：设发射点到飞行器当前位置的矢量在发射坐标系 的 三 分 量 为 [ x , y , z ] T ，则 其 地 心 矢 量 在 发 射 g 坐 标 系 的 分 量 为 ： 代入公式(6)得到重力加速度更新公式： 步骤五：更新飞行器实时速度，包括以下子步骤： 子步骤一：根据质点的动力学理论，建立飞行器在发射坐标系中动力学矢量公式 为： 其中： 为载体的加速度矢量， 为地球引力加速度矢量， 为柯氏加速度矢量； 为牵连加速度矢量； 子步骤二：依据步骤二中建立的柯式加速度在发射坐标系各轴上的分量矩阵，得 到飞行器的柯氏加速度 在发射坐标系各轴上的分量： 子步骤三：依据步骤二中建立的牵连加速度在发射坐标系各轴上各分量的转换矩 阵，得到飞行器的牵连加速度 在发射坐标系各轴上的分量为： 其中，R0i(i＝x，y，z)为发射点地心矢径在发射坐标系各轴上的分量；  x、y、z为靶 弹的地心矢径r在发射坐标系各轴上的分量； 子步骤四：建立实际工程中飞行器在发射系下速度的更新公式： 步骤六：更新飞行器实时位置：对步骤五中更新后的速度进行积分，即可更新发射 9 CN 111721291 A 说　明　书 5/10 页 g坐标系下的导航位置： 进一步的，还包括发射g坐标系下的惯组导航算法编排方法：建立导航初始化状 态，计算初始姿态矩阵，初始化本地地球参数；测量陀螺仪相对惯性空间的角速率在载体b 坐标系中的投影和加速度计相对惯性空间的比力在载体b  坐标系中的投影；通过姿态矩阵 将角速率和比力转化为发射g坐标系中的物理量；在角速度积分环路中，利用测得的角速度 按四元数积分方法修正载体b坐标系至发射g坐标系的坐标变换矩阵，根据所述坐标变换矩 阵计算姿态角；在加速度积分环路中，利用所述坐标变换矩阵将观测量转换至发射g坐标 系，并从其中补偿重力加速度g、柯式加速度ac和牵连加速度ae，经过积分得到速度与位置导 航信息。 进一步的，还包括解算流程：导航开始后，导航计算机读取导航初始化参数，进行 初始对准；计算初始四元数、初始姿态矩阵并解算地球参数，建立导航初始状态；按导航周 期，循环读取补偿后的惯组数据；按导航数值更新算法依次更新四元数和姿态矩阵；计算姿 态角并更新飞行器的实时速度和位置；向制导和控制系统按格式要求输出导航参数；在下 个导航周期中再读取补偿后的惯组参数，进行循环。 进一步的，解算的主处理器为TMS320C6747芯片。 采用上述技术方案所产生的有益效果在于：本发明基于发射系进行导航解算，其 中发射系与地球固联，其位置、速度和姿态导航参数是相对于地球的，与很多地面发射飞行 器飞控系统需求的导航信息一致，有利于人的直观描述和理解。本发明中发射系采用J2重 力模型，考虑了当地水平的南北向重力影响，速度更新算法考虑了由于地球自转产生的牵 引加速度和柯式加速度在飞行器自由飞行段中的影响，适合所有短程助推-滑翔供靶飞行 器的导航使用，适合与卫星导航进行组合导航。 附图说明 下面结合附图和