本发明涉及飞行器导航技术领域，特别是涉及一种在发射坐标系下的飞行器姿态对准方法及系统。
背景技术：
捷联惯性导航系统（strap-downinertialnavigationsystem，sins）作为一种自主导航系统，具有隐蔽性好、不受外界干扰、不受地域和气候条件限制等诸多优点，目前已广泛应用于航天、航空、航海以及陆地等领域。在捷联惯导系统进入导航解算前，需要得到载体准确的初始状态，包括载体的姿态、速度和位置等。捷联惯导系统的精对准和后续导航必须在完成姿态粗对准的基础上进行，所以姿态粗对准在整个导航任务中起着至关重要的作用。
现有的粗对准技术中，大多是在当地水平坐标系或地心地固坐标系下进行的，但这两种坐标系无法满足垂直发射的要求。而现行的很多飞行器（如助推-滑翔高超声速飞行器、垂直发射的火箭等），均采用垂直发射，因此，解决初始捷联惯导初始对准问题是十分必要的。
技术实现要素：
本发明的目的是提供一种在发射坐标系下的飞行器姿态对准方法及系统，能够满足垂直发射飞行器初始姿态信息获取需求。
为实现上述目的，本发明提供了如下方案：
一种飞行器姿态对准方法，包括：
获取发射时刻飞行器的初始纬度、发射方位角、陀螺仪输出角速度、加速度计的输出和地球自转角速度；
根据所述初始纬度和所述发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵；
根据所述陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵；
根据所述加速度计的输出、所述地球自转角速度和所述载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵；
将所述地心惯性坐标系到发射坐标系的转换矩阵、所述载体惯性坐标系到地心惯性坐标系的转换矩阵与所述载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵。
可选的，所述根据所述初始纬度和所述发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵，具体包括：
根据所述初始纬度和所述发射方位角，采用如下公式计算发射坐标系到地心惯性坐标系的转换矩阵；
对所述发射坐标系到地心惯性坐标系的转换矩阵进行正交变换，得到地心惯性坐标系到发射坐标系的转换矩阵；所述地心惯性坐标系到发射坐标系的转换矩阵如下：
式中，
可选的，所述根据所述陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵，具体包括：
获取上一时刻的姿态四元数；
根据所述陀螺仪输出角速度采用如下公式计算载体坐标系相对于地心惯性坐标系的等效旋转矢量；
根据所述等效旋转矢量采用如下公式计算变换四元数；
根据所述上一时刻的姿态四元数和所述变换四元数，采用如下公式计算当前时刻的姿态四元数；
将所述当前时刻的姿态四元数转换为载体坐标系到载体惯性坐标系的转换矩阵；所述载体坐标系到载体惯性坐标系的转换矩阵如下：
其中，
式中，
可选的，所述根据所述加速度计的输出、所述地球自转角速度和所述载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵，具体包括：
根据所述加速度计的输出和所述载体坐标系到载体惯性坐标系的转换矩阵，采用如下公式计算载体惯性坐标系下的重力矢量；
式中，
根据所述地球自转角速度，采用如下公式计算地心惯性坐标系下的重力矢量；
其中，
式中，
根据所述载体惯性坐标系下的重力矢量和所述地心惯性坐标系下的重力矢量，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵。
可选的，所述根据所述载体惯性坐标系下的重力矢量和所述地心惯性坐标系下的重力矢量，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵，具体包括：
根据如下公式计算载体惯性坐标系到地心惯性坐标系的转换矩阵：
式中，
可选的，所述根据所述载体惯性坐标系下的重力矢量和所述地心惯性坐标系下的重力矢量，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵，具体包括：
对所述载体惯性坐标系下的重力矢量采用如下公式进行积分，得到积分后的载体惯性坐标系下的重力矢量；
对所述地心惯性坐标系下的重力矢量采用如下公式进行积分，得到积分后的地心惯性坐标系下的重力矢量；
根据所述积分后的载体惯性坐标系下的重力矢量和所述积分后的地心惯性坐标系下的重力矢量，采用如下公式计算载体惯性坐标系到地心惯性坐标系的转换矩阵；
式中，
可选的，所述将所述地心惯性坐标系到发射坐标系的转换矩阵、所述载体惯性坐标系到地心惯性坐标系的转换矩阵与所述载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵，具体包括：
根据如下公式确定发射坐标系下飞行器发射前的姿态矩阵：
式中，
本发明还提供一种飞行器姿态对准系统，包括：
数据获取模块，用于获取发射时刻飞行器的初始纬度、发射方位角、陀螺仪输出角速度、加速度计的输出和地球自转角速度；
第一计算模块，用于根据所述初始纬度和所述发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵；
第二计算模块，用于根据所述陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵；
第三计算模块，用于根据所述加速度计的输出、所述地球自转角速度和所述载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵；
姿态矩阵确定模块，用于将所述地心惯性坐标系到发射坐标系的转换矩阵、所述载体惯性坐标系到地心惯性坐标系的转换矩阵与所述载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵。
与现有技术相比，本发明的有益效果是：
本发明提出了一种在发射坐标系下的飞行器姿态对准方法及系统，根据初始纬度和发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵；根据陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵；根据加速度计的输出、地球自转角速度和载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵；将地心惯性坐标系到发射坐标系的转换矩阵、载体惯性坐标系到地心惯性坐标系的转换矩阵与载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵。本发明采用发射坐标系的初始姿态对准方法，能够满足垂直发射飞行器初始姿态信息获取需求，为精对准和导航任务提供姿态初始对准信息，对准方法提供的姿态信息能满足高超声速助推-滑翔飞行器的飞行控制和导航。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例中在发射坐标系下的飞行器姿态对准方法流程图；
图2为本发明实施例中发射坐标系下抗晃动粗对准方法流程框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种在发射坐标系下的飞行器姿态对准方法及系统，能够满足垂直发射飞行器初始姿态信息获取需求。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例
图1为本发明实施例中在发射坐标系下的飞行器姿态对准方法流程图，图2为本发明实施例中发射坐标系下抗晃动粗对准方法流程框图，如图1-2所示，一种在发射坐标系下的飞行器姿态对准方法，包括：
步骤101：获取发射时刻飞行器的初始纬度、发射方位角、陀螺仪输出角速度、加速度计的输出和地球自转角速度。
步骤102：根据初始纬度和发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵。
步骤102，具体包括：
根据初始纬度和发射方位角，采用如下公式计算发射坐标系到地心惯性坐标系的转换矩阵；
对发射坐标系到地心惯性坐标系的转换矩阵进行正交变换，得到地心惯性坐标系到发射坐标系的转换矩阵；地心惯性坐标系到发射坐标系的转换矩阵如下：
式中，
步骤103：根据陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵。
步骤103，具体包括：
获取上一时刻的姿态四元数；
根据陀螺仪输出角速度采用如下公式计算载体坐标系相对于地心惯性坐标系的等效旋转矢量；
若采用二子样算法，则有：
若采用三子样算法，则有：
式中，
根据等效旋转矢量采用如下公式计算变换四元数；
根据上一时刻的姿态四元数和变换四元数，采用如下公式计算当前时刻的姿态四元数；
在初始时刻，载体坐标系与载体惯性坐标系重合，可得
将当前时刻的姿态四元数转换为载体坐标系到载体惯性坐标系的转换矩阵；载体坐标系到载体惯性坐标系的转换矩阵如下：
其中，
式中，
步骤104：根据加速度计的输出、地球自转角速度和载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵。
由于在惯性坐标系下，不同时刻的重力矢量是不共线的，所以可以根据双矢量定姿原理求解转换矩阵
未考虑晃动基座对粗对准带来的影响，步骤104，具体包括：
载体惯性坐标系和地心惯性坐标系下的重力矢量分别为
分别取
根据加速度计的输出和载体坐标系到载体惯性坐标系的转换矩阵，采用如下公式计算载体惯性坐标系下的重力矢量；
式中，
具体的，
式中，
根据地球自转角速度，采用如下公式计算地心惯性坐标系下的重力矢量；
其中，
式中，
根据载体惯性坐标系下的重力矢量和地心惯性坐标系下的重力矢量，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵。
根据如下公式计算载体惯性坐标系到地心惯性坐标系的转换矩阵：
式中，
考虑晃动基座对粗对准带来的影响，步骤104，具体包括：
对
忽略
令
可得
对载体惯性坐标系下的重力矢量采用如下公式进行积分，得到积分后的载体惯性坐标系下的重力矢量；
对地心惯性坐标系下的重力矢量采用如下公式进行积分，得到积分后的地心惯性坐标系下的重力矢量；
根据积分后的载体惯性坐标系下的重力矢量和积分后的地心惯性坐标系下的重力矢量，采用如下公式计算载体惯性坐标系到地心惯性坐标系的转换矩阵；
式中，
步骤105：将地心惯性坐标系到发射坐标系的转换矩阵、载体惯性坐标系到地心惯性坐标系的转换矩阵与载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵。
步骤105，具体包括：
根据如下公式确定发射坐标系下飞行器发射前的姿态矩阵：
式中，
本发明还提供一种在发射坐标系下的飞行器姿态对准系统，包括：
数据获取模块，用于获取发射时刻飞行器的初始纬度、发射方位角、陀螺仪输出角速度、加速度计的输出和地球自转角速度。
第一计算模块，用于根据初始纬度和发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵。
第二计算模块，用于根据陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵。
第三计算模块，用于根据加速度计的输出、地球自转角速度和载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵。
姿态矩阵确定模块，用于将地心惯性坐标系到发射坐标系的转换矩阵、载体惯性坐标系到地心惯性坐标系的转换矩阵与载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵。
对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上，本说明书内容不应理解为对本发明的限制。
1.一种飞行器姿态对准方法，其特征在于，包括：
获取发射时刻飞行器的初始纬度、发射方位角、陀螺仪输出角速度、加速度计的输出和地球自转角速度；
根据所述初始纬度和所述发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵；
根据所述陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵；
根据所述加速度计的输出、所述地球自转角速度和所述载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵；
将所述地心惯性坐标系到发射坐标系的转换矩阵、所述载体惯性坐标系到地心惯性坐标系的转换矩阵与所述载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵。
2.根据权利要求1所述的飞行器姿态对准方法，其特征在于，所述根据所述初始纬度和所述发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵，具体包括：
根据所述初始纬度和所述发射方位角，采用如下公式计算发射坐标系到地心惯性坐标系的转换矩阵；
对所述发射坐标系到地心惯性坐标系的转换矩阵进行正交变换，得到地心惯性坐标系到发射坐标系的转换矩阵；所述地心惯性坐标系到发射坐标系的转换矩阵如下：
式中，
3.根据权利要求2所述的飞行器姿态对准方法，其特征在于，所述根据所述陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵，具体包括：
获取上一时刻的姿态四元数；
根据所述陀螺仪输出角速度采用如下公式计算载体坐标系相对于地心惯性坐标系的等效旋转矢量；
根据所述等效旋转矢量采用如下公式计算变换四元数；
根据所述上一时刻的姿态四元数和所述变换四元数，采用如下公式计算当前时刻的姿态四元数；
将所述当前时刻的姿态四元数转换为载体坐标系到载体惯性坐标系的转换矩阵；所述载体坐标系到载体惯性坐标系的转换矩阵如下：
其中，
式中，
4.根据权利要求3所述的飞行器姿态对准方法，其特征在于，所述根据所述加速度计的输出、所述地球自转角速度和所述载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵，具体包括：
根据所述加速度计的输出和所述载体坐标系到载体惯性坐标系的转换矩阵，采用如下公式计算载体惯性坐标系下的重力矢量；
式中，
根据所述地球自转角速度，采用如下公式计算地心惯性坐标系下的重力矢量；
其中，
式中，
根据所述载体惯性坐标系下的重力矢量和所述地心惯性坐标系下的重力矢量，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵。
5.根据权利要求4所述的飞行器姿态对准方法，其特征在于，所述根据所述载体惯性坐标系下的重力矢量和所述地心惯性坐标系下的重力矢量，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵，具体包括：
根据如下公式计算载体惯性坐标系到地心惯性坐标系的转换矩阵：
式中，
6.根据权利要求4所述的飞行器姿态对准方法，其特征在于，所述根据所述载体惯性坐标系下的重力矢量和所述地心惯性坐标系下的重力矢量，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵，具体包括：
对所述载体惯性坐标系下的重力矢量采用如下公式进行积分，得到积分后的载体惯性坐标系下的重力矢量；
对所述地心惯性坐标系下的重力矢量采用如下公式进行积分，得到积分后的地心惯性坐标系下的重力矢量；
根据所述积分后的载体惯性坐标系下的重力矢量和所述积分后的地心惯性坐标系下的重力矢量，采用如下公式计算载体惯性坐标系到地心惯性坐标系的转换矩阵；
式中，
7.根据权利要求1所述的飞行器姿态对准方法，其特征在于，所述将所述地心惯性坐标系到发射坐标系的转换矩阵、所述载体惯性坐标系到地心惯性坐标系的转换矩阵与所述载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵，具体包括：
根据如下公式确定发射坐标系下飞行器发射前的姿态矩阵：
式中，
8.一种飞行器姿态对准系统，其特征在于，包括：
数据获取模块，用于获取发射时刻飞行器的初始纬度、发射方位角、陀螺仪输出角速度、加速度计的输出和地球自转角速度；
第一计算模块，用于根据所述初始纬度和所述发射方位角，计算地心惯性坐标系到发射坐标系的转换矩阵；
第二计算模块，用于根据所述陀螺仪输出角速度，计算载体坐标系到载体惯性坐标系的转换矩阵；
第三计算模块，用于根据所述加速度计的输出、所述地球自转角速度和所述载体坐标系到载体惯性坐标系的转换矩阵，采用双矢量定姿方法计算载体惯性坐标系到地心惯性坐标系的转换矩阵；
姿态矩阵确定模块，用于将所述地心惯性坐标系到发射坐标系的转换矩阵、所述载体惯性坐标系到地心惯性坐标系的转换矩阵与所述载体坐标系到载体惯性坐标系的转换矩阵的乘积确定为发射坐标系下飞行器发射前的姿态矩阵。
技术总结