CN116678406B - 组合导航姿态信息确定方法、装置、终端设备及存储介质 - Google Patents

Abstract

本申请适用于导航技术领域，提供了一种组合导航姿态信息确定方法、装置、终端设备及存储介质。该方法包括：根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间；根据延迟时间、获取到的预设数量的子惯导的原始信息数据与主惯导的导航信息数据，确定相同时间下的原始信息数据与导航信息数据；构建卡尔曼滤波模型，预估主惯导与子惯导之间的最优安装角度；根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，进而根据初始姿态信息确定姿态信息。本申请能够降低子惯导与主惯导之间的延时，提高传递对准的精度以及初始姿态信息的精度，进而准确确定姿态信息，提高组合导航的精度。

Description

组合导航姿态信息确定方法、装置、终端设备及存储介质

技术领域

本申请涉及导航技术领域，具体涉及一种组合导航姿态信息确定方法、装置、终端设备及存储介质。

背景技术

全球卫星导航系统（Global Navigation Satellite System，简称GNSS）和惯性导航系统（Inertial Navigation System，简称INS）具有良好的互补性，GNSS/INS组合导航能够提供相较于单一系统更精确可靠的导航定位。

GNSS/INS组合导航进入组合导航状态时需要确定初始姿态信息。在实际导航应用中，需要对主惯导与子惯导进行传递对准来预估主惯导与子惯导之间的安装角度，进而根据该安装角度确定初始姿态信息。子惯导接收主惯导发送的导航信息数据进行传递对准，然而由于主惯导的导航解算以及数据传输耗时等原因，传递对准的精度较低。此外，在传递对准结束后，子惯导因为无法获取主惯导发送的最新的导航信息数据，导致无法精准确定初始姿态信息，进而使组合导航在后续进入组合导航状态后无法提供精确导航。

因此，急需一种组合导航姿态信息确定方法，以提高传递对准的精度以及初始姿态信息的精度，进而提高组合导航的精度。

发明内容

为克服相关技术中存在的问题，本申请实施例提供了一种组合导航姿态信息确定方法、装置、终端设备及存储介质。

本申请是通过如下技术方案实现的：

第一方面，本申请实施例提供了一种组合导航姿态信息确定方法，包括：

根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间；第一钟面时为GNSS发送的秒脉冲到达子惯导时，子惯导的钟面时，第二钟面时为GNSS发送的授时信息到达子惯导时，子惯导的钟面时；根据延迟时间，获取预设数量的子惯导的原始信息数据，根据原始信息数据确定第一时间信息，第一时间信息为原始信息数据对应的绝对时间信息；获取主惯导的导航信息数据，导航信息数据包括第二时间信息，第二时间信息为主惯导发送导航信息数据的时间信息，根据第一时间信息与第二时间信息确定相同时间下的原始信息数据与导航信息数据；构建卡尔曼滤波模型，根据卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度；根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息。

在第一方面的一种可能的实施方式中，根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间，包括：将第二钟面时与第一钟面时之差作为主惯导与子惯导的延迟时间。

在第一方面的一种可能的实施方式中，根据延迟时间，获取预设数量的子惯导的原始信息数据，根据原始信息数据确定第一时间信息，包括：根据延迟时间与子惯导的预测周期，获取预设数量的子惯导的原始信息数据；其中，预设数量与预测周期的乘积大于延迟时间；原始信息数据包括子惯导的第三钟面时；根据第三钟面时、第一钟面时以及授时信息确定第一时间信息。

在第一方面的一种可能的实施方式中，根据第一时间信息与第二时间信息确定相同时间下的原始信息数据与导航信息数据，包括：根据第二时间信息，对预设数量的子惯导的第一时间信息进行遍历，确定与第二时间信息的误差在预设误差范围内的第一时间信息，作为同步时间信息；确定第二时间信息对应的导航信息数据与同步时间信息对应的原始信息数据为相同时间下的原始信息数据与导航信息数据。

在第一方面的一种可能的实施方式中，原始信息数据包括陀螺原始数据与加速度计原始数据，以及获取陀螺原始数据与加速度计原始数据时对应的子惯导的第三钟面时；导航信息数据包括主惯导的速度数据和姿态数据，以及与速度数据和姿态数据对应的第二时间信息。

在第一方面的一种可能的实施方式中，构建卡尔曼滤波模型，根据卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度，包括：构建卡尔曼滤波的状态向量，状态向量包括主惯导与子惯导之间的安装角度误差；根据相同时间下的原始信息数据与导航信息数据，构建卡尔曼滤波状态转移方程和卡尔曼滤波量测方程；根据相同时间下的原始信息数据与导航信息数据，以及卡尔曼滤波状态转移方程和卡尔曼滤波量测方程对状态向量进行计算，确定主惯导与子惯导之间的最优安装角度。

在第一方面的一种可能的实施方式中，根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息，包括：根据最优安装角度确定子惯导的方向余弦矩阵；根据子惯导的方向余弦矩阵确定姿态角信息，对姿态角信息和相同时间下的原始信息数据与导航信息数据进行连续的机械编排，确定子惯导的初始姿态信息；检测到GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息。

第二方面，本申请实施例提供了一种组合导航姿态信息确定装置，包括：

第一确定模块，用于根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间；第一钟面时为GNSS发送的秒脉冲到达子惯导时，子惯导的钟面时，第二钟面时为GNSS发送的授时信息到达子惯导时，子惯导的钟面时。

第二确定模块，用于根据延迟时间，获取预设数量的子惯导的原始信息数据，根据原始信息数据确定第一时间信息，第一时间信息为原始信息数据对应的绝对时间信息。

第三确定模块，用于获取主惯导的导航信息数据，导航信息数据包括第二时间信息，第二时间信息为主惯导发送导航信息数据的时间信息，根据第一时间信息与第二时间信息确定相同时间下的原始信息数据与导航信息数据。

预估模块，用于构建卡尔曼滤波模型，根据卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度。

第四确定模块，用于根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息。

第三方面，本申请实施例提供了一种终端设备，包括存储器和处理器，上述存储器中存储有可在处理器上运行的计算机程序，上述处理器执行计算机程序时实现如第一方面任一项所述的组合导航姿态信息确定方法。

第四方面，本申请实施例提供了一种计算机可读存储介质，上述计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现如第一方面任一项所述的组合导航姿态信息确定方法。

第五方面，本申请实施例提供了一种计算机程序产品，当计算机程序产品在终端设备上运行时，使得终端设备执行上述第一方面中任一项所述的组合导航姿态信息确定方法。

可以理解的是，上述第二方面至第五方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。

本申请实施例提供的组合导航姿态信息确定方法、装置、终端设备及存储介质，通过根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间，根据延迟时间、获取到的预设数量的子惯导的原始信息数据与主惯导的导航信息数据，确定相同时间下的原始信息数据与导航信息数据，并构建卡尔曼滤波模型，根据该卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据，预估主惯导与子惯导之间的最优安装角度，根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，进而根据初始姿态信息确定姿态信息。本申请能够降低子惯导与主惯导之间的延时，提高传递对准的精度以及初始姿态信息的精度，进而准确确定姿态信息，提高组合导航的精度。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本说明书。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本申请一实施例提供的组合导航姿态信息确定方法的流程示意图；

图2是本申请另一实施例提供的组合导航姿态信息确定方法的流程示意图；

图3是本申请又一实施例提供的组合导航姿态信息确定方法的流程示意图；

图4是本申请再一实施例提供的组合导航姿态信息确定方法的流程示意图；

图5是本申请还一实施例提供的组合导航姿态信息确定方法的流程示意图；

图6是本申请一实施例提供的组合导航姿态信息确定方法的数据流示意图；

图7是本申请一实施例提供的组合导航姿态信息确定装置的结构示意图；

图8是本申请一实施例提供的终端设备的结构示意图。

具体实施方式

下面结合具体实施例对本申请进行更清楚的说明。以下实施例将有助于本领域的技术人员进一步理解本申请的作用，但不以任何形式限制本申请。应当指出的是，对本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进。这些都属于本申请的保护范围。

应当理解，当在本申请说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

在本申请说明书和所附权利要求书的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

在本申请说明书中描述的参考“一个实施例”或“一些实施例”等意味着在本申请的一个或多个实施例中包括结合该实施例描述的特定特征、结构或特点。由此，在本说明书中的不同之处出现的语句“在一个实施例中”、“在一些实施例中”、“在其他一些实施例中”、“在另外一些实施例中”等不是必然都参考相同的实施例，而是意味着“一个或多个但不是所有的实施例”，除非是以其他方式另外特别强调。术语“包括”、“包含”、“具有”及它们的变形都意味着“包括但不限于”，除非是以其他方式另外特别强调。

此外，本申请实施例中提到的“多个”应当被解释为两个或两个以上。

全球卫星导航系统和惯性导航系统具有良好的互补性，GNSS/INS组合导航能够提供相较于单一系统更精确可靠的导航定位。目前GNSS/INS组合导航已广泛应用于导弹制导等领域。

GNSS/INS组合导航进入组合导航状态时需要确定初始姿态信息。然而由于GNSS/INS组合导航的安装平台通常包括动态平台，例如，弹载发射平台通常包括飞机、舰船等动态平台，其姿态信息无法通过自身获取得到，需要依赖外部信息通过传递对准的方式获得。也就是说，在实际导航应用中，需要对主惯导与子惯导进行传递对准来预估主惯导与子惯导之间的安装角度，进而根据该安装角度确定初始姿态信息。子惯导接收主惯导发送的导航信息数据进行传递对准，然而由于主惯导的导航解算以及数据传输耗时等原因，传递对准的精度较低。例如，当载体以的角速度做机动时，可能会导致1.8°的航向延迟。此外，在传递对准结束后，子惯导因为无法获取主惯导发送的最新的导航信息数据，导致无法精准确定初始姿态信息，进而使组合导航在后续进入组合导航状态后无法提供精确导航。因此，急需一种组合导航姿态信息确定方法，以提高传递对准的精度以及初始姿态信息的精度，进而提高组合导航的精度。

基于上述问题，发明人经研究发现，可以利用GNSS秒脉冲与授时信息到达子惯导时的钟面时确定延迟时间，进而根据延迟时间进行主惯导与子惯导的时间同步，减小延时，进行精准的传递对准，并在传递对准结束后，根据确定的最优安装角度和相同时间下主惯导的导航信息数据与子惯导的原始信息数据，确定初始姿态信息。

图1是本申请一实施例提供的组合导航姿态信息确定方法的流程示意图。如图1所示，本申请实施例中的方法，可以包括：

步骤101、根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间。

可选的，执行根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间的步骤，具体可以包括：将第二钟面时与第一钟面时之差作为主惯导与子惯导的延迟时间。

其中，第一钟面时为GNSS发送的秒脉冲到达子惯导时，子惯导的钟面时，第二钟面时为GNSS发送的授时信息到达子惯导时，子惯导的钟面时。其中，上述子惯导的钟面时为子惯导的系统时间，信息到达子惯导时子惯导的钟面时，即为信息到达子惯导时子惯导的系统时间。

示例性的，GNSS可以为全球定位系统（GPS）、北斗卫星导航系统（BDS）或伽利略卫星导航系统（GALILEO）等，惯导可以为低等级的微惯性导航系统（MEMS惯导）等，相应的，子惯导可以为 MEMS子惯导，主惯导可以为MEMS主惯导。

具体来说，GNSS的接收机向子惯导的处理器发送秒脉冲，子惯导的处理器通过检测上述秒脉冲的上升沿确定秒脉冲的到达时刻，并在该到达时刻记录子惯导的处理器的系统时间作为第一钟面时。

可选的，GNSS的接收机向子惯导的处理器发送授时信息，实际为通过数据信息接口向子惯导的处理器发送授时信息数据帧，其中，数据信息接口可以为RS422接口，在此不做具体限定，子惯导的处理器在接收到上述授时信息数据帧时，记录子惯导的处理器的系统时间作为第二钟面时，并对上述授时信息数据帧进行解析，得到授时信息。

示例性的，将第二钟面时与第一钟面时之差作为主惯导与子惯导的延迟时间，延迟时间公式为：

式中，为延迟时间，/>为第二钟面时，/>为第一钟面时。

步骤102、根据延迟时间，获取预设数量的子惯导的原始信息数据，根据原始信息数据确定第一时间信息。

在一种可能的实施方式中，参见图2，步骤102中，具体可以包括：

步骤1021、根据延迟时间与子惯导的预测周期，获取预设数量的子惯导的原始信息数据；原始信息数据包括子惯导的第三钟面时。

步骤1022、根据第三钟面时、第一钟面时以及授时信息确定第一时间信息。

其中，预设数量与预测周期的乘积大于延迟时间。原始信息数据包括陀螺原始数据与加速度计原始数据，以及获取陀螺原始数据与加速度计原始数据时对应的子惯导的第三钟面时。

具体的，获取子惯导的原始信息数据帧，对上述原始信息数据帧进行解析可以得到原始信息数据。陀螺原始数据具体为角速度，加速度计原始数据具体为加速度。

示例性的，子惯导的预测周期为子惯导获取陀螺原始数据与加速度计原始数据的周期，例如，预测周期可以为5ms，即每隔5ms获取一组陀螺原始数据与加速度计原始数据。根据延迟时间与子惯导的预测周期设置预设数量，以使预设数量与预测周期的乘积大于延迟时间，例如，预测周期为5ms，根据前述实施例确定延迟时间为30ms，则可以确定预设数量为20，即在100ms中获取20个原始信息数据。原始信息数据可以表示为[]，i表示获取的原始信息数据的序列，/>，也就是说，/>、/>和/>分别为第i个原始信息数据中陀螺x轴、y轴和z轴的数据，/>、/>和分别为第i个原始信息数据中加速度计x轴、y轴和z轴的数据。

可选的，根据第三钟面时、第一钟面时以及授时信息确定第一时间信息，该第一时间信息为原始信息数据对应的绝对时间信息，该绝对时间信息通过GNSS和GNSS周内秒表达，且时间起点为1980年1月6日。第一时间信息公式为：

式中，为第一时间信息，/>为授时信息，/>为第三钟面时，/>为第一钟面时。

步骤103、获取主惯导的导航信息数据，导航信息数据包括第二时间信息，根据第一时间信息与第二时间信息确定相同时间下的原始信息数据与导航信息数据。

在一种可能的实施方式中，参见图3，步骤103中，具体可以包括：

步骤1031、获取主惯导的导航信息数据，导航信息数据包括第二时间信息。

步骤1032、根据第二时间信息，对预设数量的子惯导的第一时间信息进行遍历，确定与第二时间信息的误差在预设误差范围内的第一时间信息，作为同步时间信息，确定第二时间信息对应的导航信息数据与同步时间信息对应的原始信息数据为相同时间下的原始信息数据与导航信息数据。

其中，导航信息数据包括主惯导的速度数据和姿态数据，以及与速度数据和姿态数据对应的第二时间信息。第二时间信息为主惯导发送导航信息数据的时间信息。具体的，获取主惯导的导航信息数据帧，对上述导航信息数据帧进行解析可以得到导航信息数据。

可选的，预设误差范围可以根据需要设置。一个简单的示例是，设置预设误差范围为0~1ms，根据第二时间信息，对获取到的20个原始信息数据中的第一时间信息进行遍历，确定第4个原始信息数据中的第一时间信息与第二时间信息的误差在预设误差范围内，则确定该第一时间信息为同步时间信息，第4个原始信息数据与导航信息数据为相同时间下的数据。

重复步骤102~103，获取多组相同时间下的原始信息数据与导航信息数据。其中，获取的组数根据需求设置。

一个简单的示例是，设置预测周期为5ms，根据前述实施例确定延迟时间为30ms，确定预设数量为20，即在100ms中获取20个原始信息数据，同时，在该100ms中获取一个导航信息数据，进而确定相同时间下的原始信息数据与导航信息数据。在下一个100ms中获取20个原始信息数据和一个导航信息数据，并确定相同时间下的原始信息数据与导航信息数据，重复上述过程，直至获取多组相同时间下的原始信息数据与导航信息数据，以进行后续传递对准。

步骤104、构建卡尔曼滤波模型，根据卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度。

在一种可能的实施方式中，参见图4，步骤104中，具体可以包括：

步骤1041、构建卡尔曼滤波的状态向量，状态向量包括主惯导与子惯导之间的安装角度误差。

步骤1042、根据相同时间下的原始信息数据与导航信息数据，构建卡尔曼滤波状态转移方程和卡尔曼滤波量测方程。

步骤1043、根据相同时间下的原始信息数据与导航信息数据，以及卡尔曼滤波状态转移方程和卡尔曼滤波量测方程对状态向量进行计算，确定主惯导与子惯导之间的最优安装角度。

可选的，构建卡尔曼滤波的状态向量，该状态向量具体可以包括：姿态角误差、速度误差、陀螺零偏误差、加速度计零偏误差与安装角度误差5个状态量。

示例性的，建立传递对准的卡尔曼滤波状态转移方程：

其中，传递对准的卡尔曼滤波预测过程：

式中，为/>时刻的状态转移矩阵，/>为/>的转置矩阵，由多组相同时间下的原始信息数据与导航信息数据构成，/>与/>分别为/>时刻与/>时刻的状态向量，/>与分别为/>时刻与/>时刻的状态向量方差矩阵，/>为/>时刻的状态量噪声矩阵。

其中，状态量噪声矩阵为：

状态向量方差矩阵为：

式中，、/>、/>、/>与/>分别为角速度、加速度、陀螺零偏、加速度计零偏以及安装角度对应的噪声信息，/>、/>、/>、/>与/>分别为角速度、加速度、陀螺零偏、加速度计零偏以及安装角度的最优估计的方差。

建立传递对准的卡尔曼滤波量测方程：

式中，为量测信息矩阵，/>是量测噪声项，其噪声方差矩阵用R表示，/>为量测方程系数矩阵，/>可表示为：

式中，为单位矩阵，/>为姿态矩阵，由多组相同时间下的原始信息数据与导航信息数据构成。

传递对准的卡尔曼滤波更新过程：

式中，为/>时刻的增益矩阵，/>为/>时刻的量测方程系数矩阵，/>为/>的转置矩阵，/>为/>时刻的噪声方差矩阵，/>为单位矩阵，/>为/>时刻的量测信息矩阵。

传递对准的卡尔曼滤波预测过程与卡尔曼滤波更新过程完成后得到传递对准后的状态向量，根据上述状态向量确定安装角度误差，进而根据安装角度误差确定最优安装角度。

在一中可能的实施方式中，还可以根据上述状态向量中的陀螺零偏误差和速度误差对原始信息数据中的陀螺原始数据和导航信息数据中的速度数据进行反馈补偿，以不断对陀螺原始数据与速度数据进行反馈修正，提高数据准确度。

反馈零偏误差为：

反馈速度误差为：

式中，为补偿后的角速度，/>为补偿前的角速度，/>为陀螺零偏误差，/>为补偿后的速度，/>为补偿前的速度，/>为速度误差。

步骤105、根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息。

在一种可能的实施方式中，参见图5，步骤105中，具体可以包括：

步骤1051、根据最优安装角度确定子惯导的方向余弦矩阵。

步骤1052、根据子惯导的方向余弦矩阵确定姿态角信息，对姿态角信息和相同时间下的原始信息数据与导航信息数据进行连续的机械编排，确定子惯导的初始姿态信息。

步骤1053、检测到GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息。

可选的，根据最优安装角度确定传递对准中最后一个历元时刻的子惯导的方向余弦矩阵，子惯导的方向余弦矩阵为：

式中，为单位矩阵，/>为子惯导的方向余弦矩阵，/>为主惯导的方向余弦矩阵，该主惯导的方向余弦矩阵可以根据姿态角误差与安装角度误差得到，/>为最优安装角度的叉乘形式。

示例性的，根据子惯导的方向余弦矩阵确定传递对准中最后一个历元时刻的子惯导的姿态角信息，该姿态角信息具体包括横滚角、俯仰角与航向角。姿态角信息为：

式中，、/>与/>分别为俯仰角、横滚角与航向角，/>代表子惯导的方向余弦矩阵的第g行、第l列的元素，其中/>，/>。

确定姿态角信息后，根据姿态角信息和多组相同时间下的原始信息数据与导航信息数据进行连续的机械编排，确定子惯导的最新的初始姿态信息，以使在检测到GNSS/INS组合导航进入组合导航状态时，根据上述最新的初始姿态信息确定姿态信息。若未检测到GNSS/INS组合导航进入组合导航状态，则重复步骤102~1052，持续获取多组相同时间下的原始信息数据与导航信息数据，并根据上述多组相同时间下的原始信息数据与导航信息数据，持续确定最新的初始姿态信息。

在一种可能的实施方式中，步骤1053中，具体可以包括S1至S5。

S1、根据初始姿态信息与最优安装角度确定初始姿态角，以使GNSS/INS组合导航基于初始姿态角进行初始化。

S2、构建GNSS/INS组合导航的卡尔曼滤波的第一状态向量。

可选的，第一状态向量具体可以包括：位置误差、速度误差、姿态角误差、陀螺零偏误差、加速度计零偏误差15个误差状态量。第一状态向量可表示为：

式中，、/>与/>为北东地方向的位置误差，/>、/>与/>为北东地方向的速度误差，/>、/>与/>分别为横滚角误差、俯仰角误差与航向角误差，/>、/>与分别为陀螺x轴、y轴与z轴的零偏误差，/>、/>与/>分别为加速度计x轴、y轴与z轴的零偏误差。

具体的，通常来说选取的状态量越多，越能准确反映运动模型。此外，状态量的选取也取决于惯导的等级，例如对于低等级的MEMS惯导，其比例因子和交轴耦合误差较大，而随着状态量的增多计算的复杂度也会增大，因此在选取状态量时需要进行综合考虑。

S3、构建GNSS/INS组合导航的卡尔曼滤波的第一状态转移方程。

式中，与/>分别为/>时刻与/>时刻的第一状态向量，/>为/>时刻的系统噪声驱动矩阵，/>为/>时刻的第一状态噪声向量，/>为离散后的第一状态转移矩阵，/>可表示为：

其中：

，

，

，/>

，

，

，

，/>

式中，、/>和/>分别为载体东向速度、北向速度以及天向速度，/>为地球的自传角速度，M、N和h分别为地球卯酉圈半径、子午圈半径以及载体的大地高，/>为载体纬度，/>为当地正常重力值，/>为采样间隔，/>为惯导噪声的相关时间，/>与/>分别为传感器输出在载体坐标系以及导航坐标系的投影。

S4、构建GNSS/INS组合导航的卡尔曼滤波的第一量测方程。

式中，为第一量测信息矩阵，/>是第一量测噪声项，其对应的第一噪声方差矩阵用/>表示，/>为第一量测方程系数矩阵，/>可表示为：

/>

第一量测噪声项的第一噪声方差矩阵表示为：

式中，为单位矩阵，/>、/>、/>、/>、/>和/>分别为GNSS观测信息东向位置方差值、北向位置方差值、天向位置方差值、东向速度方差值、北向速度方差值以及天向速度方差值。

S5、设定GNSS/INS组合导航的卡尔曼滤波的第一状态量噪声矩阵与第一状态向量方差矩阵。

其中，第一状态量噪声矩阵为：

式中，代表第一状态量噪声矩阵，/>代表惯性传感器的速度随机游走、/>代表惯性传感器的角度随机游走，/>和/>依次代表惯性传感器的输出噪声。

第一状态向量方差矩阵为：

式中，代表第一状态向量方差矩阵，/>、/>和/>分别代表东向、北向、天向的初始位置不确定性，/>、/>和/>分别代表东向、北向、天向速度的不确定性，/>、/>和/>分别代表横滚角、俯仰角以及航向角的初始不确定性，/>、/>和/>分别代表陀螺x轴、y轴和z轴的零偏初始不确定性，/>、/>和/>分别代表加速度计x轴、y轴和z轴的零偏初始不确定性。

GNSS/INS组合导航的卡尔曼滤波预测过程：

式中，为/>时刻的第一状态转移矩阵，/>为/>的转置矩阵，/>与/>分别为/>时刻与/>时刻的第一状态向量，/>与/>分别为/>时刻与/>时刻的第一状态向量方差矩阵，/>为/>时刻的第一状态量噪声矩阵。

GNSS/INS组合导航的卡尔曼滤波更新过程：

式中，为/>时刻的第一增益矩阵，/>为/>时刻的第一量测方程系数矩阵，/>为/>的装置矩阵，/>为/>时刻的第一噪声方差矩阵，/>为单位矩阵，/>为/>时刻的第一量测信息矩阵。

GNSS/INS组合导航的卡尔曼滤波预测过程与卡尔曼滤波更新过程完成后得到第一状态向量，根据上述第一状态向量确定最新的姿态信息。

示例性的，组合导航姿态信息确定方法的数据流示意图可如图6所示。

本申请实施例通过根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间，根据延迟时间、获取到的预设数量的子惯导的原始信息数据与主惯导的导航信息数据，确定相同时间下的原始信息数据与导航信息数据，并构建卡尔曼滤波模型，根据该卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据，预估主惯导与子惯导之间的最优安装角度，根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，进而根据初始姿态信息确定姿态信息。本申请能够降低子惯导与主惯导之间的延时，提高传递对准的精度以及初始姿态信息的精度，进而准确确定姿态信息，提高组合导航的精度。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。

图7是本申请一实施例提供的组合导航姿态信息确定装置的结构示意图。如图7所示，本实施例提供的组合导航姿态信息确定装置，可以包括：第一确定模块301、第二确定模块302、第三确定模块303、预估模块304和第四确定模块305。

其中，第一确定模块301，用于根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间；第一钟面时为GNSS发送的秒脉冲到达子惯导时，子惯导的钟面时，第二钟面时为GNSS发送的授时信息到达子惯导时，子惯导的钟面时。

第二确定模块302，用于根据延迟时间，获取预设数量的子惯导的原始信息数据，根据原始信息数据确定第一时间信息，第一时间信息为原始信息数据对应的绝对时间信息。

第三确定模块303，用于获取主惯导的导航信息数据，导航信息数据包括第二时间信息，第二时间信息为主惯导发送导航信息数据的时间信息，根据第一时间信息与第二时间信息确定相同时间下的原始信息数据与导航信息数据。

预估模块304，用于构建卡尔曼滤波模型，根据卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度。

第四确定模块305，用于根据最优安装角度和相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息。

可选的，第一确定模块301具体用于：将第二钟面时与第一钟面时之差作为主惯导与子惯导的延迟时间。

可选的，第二确定模块302具体用于：根据延迟时间与子惯导的预测周期，获取预设数量的子惯导的原始信息数据；其中，预设数量与预测周期的乘积大于延迟时间；原始信息数据包括子惯导的第三钟面时；根据第三钟面时、第一钟面时以及授时信息确定第一时间信息。

可选的，第三确定模块303具体用于：根据第二时间信息，对预设数量的子惯导的第一时间信息进行遍历，确定与第二时间信息的误差在预设误差范围内的第一时间信息，作为同步时间信息；确定第二时间信息对应的导航信息数据与同步时间信息对应的原始信息数据为相同时间下的原始信息数据与导航信息数据。

可选的，预估模块304具体用于：构建卡尔曼滤波的状态向量，状态向量包括主惯导与子惯导之间的安装角度误差；根据相同时间下的原始信息数据与导航信息数据，构建卡尔曼滤波状态转移方程和卡尔曼滤波量测方程；根据相同时间下的原始信息数据与导航信息数据，以及卡尔曼滤波状态转移方程和卡尔曼滤波量测方程对状态向量进行计算，确定主惯导与子惯导之间的最优安装角度。

可选的，第四确定模块305具体用于：根据最优安装角度确定子惯导的方向余弦矩阵；根据子惯导的方向余弦矩阵确定姿态角信息，对姿态角信息和相同时间下的原始信息数据与导航信息数据进行连续的机械编排，确定子惯导的初始姿态信息；检测到GNSS/INS组合导航进入组合导航状态时，根据初始姿态信息确定姿态信息。

需要说明的是，上述装置/单元之间的信息交互、执行过程等内容，由于与本申请方法实施例基于同一构思，其具体功能及带来的技术效果，具体可参见方法实施例部分，此处不再赘述。

图8是本申请一实施例提供的终端设备的结构示意图。如图8所示，该实施例的终端设备400包括：处理器410、存储器420，上述存储器420中存储有可在处理器410上运行的计算机程序421。处理器410执行计算机程序421时实现上述任意各个方法实施例中的步骤，例如图1所示的步骤101至105。或者，处理器410执行计算机程序421时实现上述各装置实施例中各模块/单元的功能，例如图7所示模块301至305的功能。

可选的，终端设备400可以为子惯导，相应的，处理器410为子惯导的处理器，存储器420为子惯导的存储器，子惯导的处理器执行计算机程序421时实现上述任意各个方法实施例中的步骤，例如图1所示的步骤101至105。或者，子惯导的处理器执行计算机程序421时实现上述各装置实施例中各模块/单元的功能，例如图7所示模块301至305的功能。

示例性的，计算机程序421可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器420中，并由处理器410执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序421在终端设备400中的执行过程。

本领域技术人员可以理解，图8仅仅是终端设备的示例，并不构成对终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如输入输出设备、网络接入设备、总线等。

处理器410可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器 (Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器420可以是终端设备的内部存储单元，例如终端设备的硬盘或内存，也可以是终端设备的外部存储设备，例如终端设备上配备的插接式硬盘，智能存储卡（SmartMedia Card，SMC），安全数字（Secure Digital，SD）卡，闪存卡（Flash Card）等。上述存储器420还可以既包括终端设备的内部存储单元也包括外部存储设备。上述存储器420用于存储计算机程序以及终端设备所需的其他程序和数据。存储器420还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、电载波信号、电信信号以及软件分发介质等。

以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (6)

1.一种组合导航姿态信息确定方法，其特征在于，包括：

根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间；所述第一钟面时为GNSS发送的秒脉冲到达子惯导时，子惯导的钟面时，所述第二钟面时为GNSS发送的授时信息到达子惯导时，子惯导的钟面时；其中，所述授时信息为GNSS通过数据信息接口向子惯导发送的授时信息数据帧；

根据所述延迟时间，获取预设数量的子惯导的原始信息数据，根据所述原始信息数据确定第一时间信息，所述第一时间信息为所述原始信息数据对应的绝对时间信息；

获取主惯导的导航信息数据，所述导航信息数据包括第二时间信息，所述第二时间信息为主惯导发送导航信息数据的时间信息，根据所述第一时间信息与所述第二时间信息确定相同时间下的原始信息数据与导航信息数据；

构建卡尔曼滤波模型，根据所述卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度；

根据所述最优安装角度和所述相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据所述初始姿态信息确定姿态信息；

其中，所述根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间，包括：

将所述第二钟面时与第一钟面时之差作为主惯导与子惯导的延迟时间；

所述根据所述延迟时间，获取预设数量的子惯导的原始信息数据，根据所述原始信息数据确定第一时间信息，包括：

根据所述延迟时间与子惯导的预测周期，获取预设数量的子惯导的原始信息数据；其中，所述预设数量与所述预测周期的乘积大于延迟时间；所述原始信息数据包括子惯导的第三钟面时；

根据所述第三钟面时、所述第一钟面时以及所述授时信息确定所述第一时间信息；

所述根据所述第一时间信息与所述第二时间信息确定相同时间下的原始信息数据与导航信息数据，包括：

根据所述第二时间信息，对所述预设数量的子惯导的第一时间信息进行遍历，确定与所述第二时间信息的误差在预设误差范围内的第一时间信息，作为同步时间信息；

确定所述第二时间信息对应的导航信息数据与所述同步时间信息对应的原始信息数据为所述相同时间下的原始信息数据与导航信息数据；

所述原始信息数据包括陀螺原始数据与加速度计原始数据，以及获取所述陀螺原始数据与加速度计原始数据时对应的子惯导的第三钟面时；所述导航信息数据包括主惯导的速度数据和姿态数据，以及与所述速度数据和姿态数据对应的第二时间信息。

2.根据权利要求1所述的组合导航姿态信息确定方法，其特征在于，所述构建卡尔曼滤波模型，根据所述卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度，包括：

构建卡尔曼滤波的状态向量，所述状态向量包括主惯导与子惯导之间的安装角度误差；

根据所述相同时间下的原始信息数据与导航信息数据，构建卡尔曼滤波状态转移方程和卡尔曼滤波量测方程；

根据所述相同时间下的原始信息数据与导航信息数据，以及所述卡尔曼滤波状态转移方程和所述卡尔曼滤波量测方程对所述状态向量进行计算，确定主惯导与子惯导之间的最优安装角度。

3.根据权利要求1所述的组合导航姿态信息确定方法，其特征在于，所述根据所述最优安装角度和所述相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据所述初始姿态信息确定姿态信息，包括：

根据所述最优安装角度确定子惯导的方向余弦矩阵；

根据所述子惯导的方向余弦矩阵确定姿态角信息，对所述姿态角信息和所述相同时间下的原始信息数据与导航信息数据进行连续的机械编排，确定子惯导的初始姿态信息；

检测到GNSS/INS组合导航进入组合导航状态时，根据所述初始姿态信息确定姿态信息。

4.一种组合导航姿态信息确定装置，其特征在于，包括：

第一确定模块，用于根据子惯导的第一钟面时和第二钟面时，确定子惯导与主惯导的延迟时间；所述第一钟面时为GNSS发送的秒脉冲到达子惯导时，子惯导的钟面时，所述第二钟面时为GNSS发送的授时信息到达子惯导时，子惯导的钟面时；其中，所述授时信息为GNSS通过数据信息接口向子惯导发送的授时信息数据帧；

第二确定模块，用于根据所述延迟时间，获取预设数量的子惯导的原始信息数据，根据所述原始信息数据确定第一时间信息，所述第一时间信息为所述原始信息数据对应的绝对时间信息；

第三确定模块，用于获取主惯导的导航信息数据，所述导航信息数据包括第二时间信息，所述第二时间信息为主惯导发送导航信息数据的时间信息，根据所述第一时间信息与所述第二时间信息确定相同时间下的原始信息数据与导航信息数据；

预估模块，用于构建卡尔曼滤波模型，根据所述卡尔曼滤波模型和相同时间下的原始信息数据与导航信息数据进行传递对准，预估主惯导与子惯导之间的最优安装角度；

第四确定模块，用于根据所述最优安装角度和所述相同时间下的原始信息数据与导航信息数据确定子惯导的初始姿态信息，并在GNSS/INS组合导航进入组合导航状态时，根据所述初始姿态信息确定姿态信息；

所述第一确定模块具体用于将所述第二钟面时与第一钟面时之差作为主惯导与子惯导的延迟时间；

所述第二确定模块具体用于根据所述延迟时间与子惯导的预测周期，获取预设数量的子惯导的原始信息数据；其中，所述预设数量与所述预测周期的乘积大于延迟时间；所述原始信息数据包括子惯导的第三钟面时；根据所述第三钟面时、所述第一钟面时以及所述授时信息确定所述第一时间信息；

所述第三确定模块具体用于根据所述第二时间信息，对所述预设数量的子惯导的第一时间信息进行遍历，确定与所述第二时间信息的误差在预设误差范围内的第一时间信息，作为同步时间信息；确定所述第二时间信息对应的导航信息数据与所述同步时间信息对应的原始信息数据为所述相同时间下的原始信息数据与导航信息数据；

所述原始信息数据包括陀螺原始数据与加速度计原始数据，以及获取所述陀螺原始数据与加速度计原始数据时对应的子惯导的第三钟面时；所述导航信息数据包括主惯导的速度数据和姿态数据，以及与所述速度数据和姿态数据对应的第二时间信息。

5.一种终端设备，包括存储器和处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至3任一项所述的组合导航姿态信息确定方法。

6.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至3任一项所述的组合导航姿态信息确定方法。