CN115727846B - 一种捷联惯导与卫导的多线程回溯式组合导航方法 - Google Patents

Abstract

本发明公开了一种捷联惯导与卫导的多线程回溯式组合导航方法，包括：在实时导航线程，基于惯导数据进行惯性导航的位置、速度、姿态更新计算，并将计算得到导航信息实时输出；在回溯导航线程，基于惯导数据、卫导数据与实时导航线程的状态数据进行周期性的回溯导航计算与回溯卡尔曼滤波计算，并在每个回溯解算周期结束后基于回溯结果对实时导航线程中对应的状态参量和/或误差参量进行校正。本发明应用于导航领域，在解决组合导航中滤波估算周期中断、误差校正周期较长等问题的同时，提升组合导航系统对误差的动态跟踪能力及组合导航精度，同时提升系统从组合导航状态切换到纯惯性导航状态后的长期导航精度。

Description

一种捷联惯导与卫导的多线程回溯式组合导航方法

技术领域

本发明涉及导航技术领域，具体是一种捷联惯导与卫导的多线程回溯式组合导航方法。

背景技术

捷联惯导系统可以实时输出所安装载体的位置、速度、姿态等导航信息，具有自主性、隐蔽性、可靠性等诸多优点，被广泛地应用在军事领域及民用市场。由于纯惯性导航系统的误差随时间发散，在实际应用中，捷联惯导系统常常与卫星导航系统相互组合，从而取长补短，以此获得更优的导航性能。在捷联惯导与卫导的组合导航系统中，有两种典型的工作状态：组合导航状态和纯惯性导航状态。这两种状态可以根据是否有卫星导航信息而自动切换，在能够利用到卫星导航信息的情况下，一般工作在惯导/卫导组合导航状态，在水下导航、卫星信号丢失或者其它特殊情况下，系统工作在纯惯性导航状态。

高精度的组合导航系统中，为了取得更高的导航性能，在惯导/卫导组合导航状态下，经常需要对捷联惯导系统的各种误差参量进行精确的估算和校正。这些误差参量除了位置、速度、姿态误差外，还包括陀螺漂移、加速度计漂移等误差。对于不同的误差参量，由于其可观测性不同，其滤波估算的时间也可能相差较大，例如在单轴旋转式捷联惯导与卫导的组合导航系统中，位置、速度、水平姿态、水平加速度计漂移误差几分钟的时间内即可以估算出来，水平陀螺漂移误差、航向角误差的估算则需要稍长时间，系统的天向陀螺漂移估算则至少需要6个小时（地球自转周期的1/4）以上的更长时间。

在组合导航系统实际应用中，由于卫星导航信息从可用到不可用的时间点具有不确定性，可能导致预设的滤波估算周期不能完成，从而使系统的误差参数校正精度受到影响，进而影响到纯惯性导航状态下的长期导航精度。例如在单轴旋转式捷联惯导与卫导的组合导航系统中，如果当系统充分预热后的第一次天向陀螺漂移估算即将完成时，卫星导航信息突然不可用，则由于天向陀螺漂移的补偿精度不够，很可能导致系统的纯惯性导航精度显著下降。此外，如果每6小时以上才能校正一次系统的天向陀螺漂移误差，其误差校正频率太低，无法对系统的误差变化进行较快的跟踪，其动态适应性也受到一定限制。

发明内容

针对上述现有技术中高精度捷联惯导与卫导组合导航中存在的滤波估算周期中断、误差校正周期较长导致的系统误差动态跟踪能力下降等问题，本发明提供一种捷联惯导与卫导的多线程回溯式组合导航方法，能够避免组合导航的滤波估算周期中断问题的同时，提升组合导航对系统误差的动态跟踪能力及组合导航精度，同时提升捷联惯导系统从组合导航状态切换到纯惯性导航状态后的长期导航精度。

为实现上述目的，本发明提供一种捷联惯导与卫导的多线程回溯式组合导航方法，采用实时导航线程与回溯导航线程进行组合导航，所述组合导航方法包括如下步骤：

实时采集并存储惯导数据与卫导数据；

在实时导航线程，基于实时采集的惯导数据进行惯性导航的位置、速度、姿态更新计算，并将计算得到导航信息实时输出；

在回溯导航线程，基于历史存储的惯导数据、卫导数据与实时导航线程的状态数据进行周期性的回溯导航计算与回溯卡尔曼滤波计算，并在每个回溯解算周期结束后基于回溯结果对实时导航线程中对应的状态参量和/或误差参量进行校正；

其中，所述实时导航线程与所述回溯导航线程并行运行。

在其中一个实施例，所述回溯导航线程的数量为两个以上，且各所述回溯导航线程的回溯解算周期、逆向数据时长均不相同；

在组合导航过程中，各所述回溯导航线程并行运行，以校正实时导航线程中不同的状态参量和/或误差参量。

在其中一个实施例，所述基于实时采集的惯导数据进行惯性导航的位置、速度、姿态更新计算，具体为：

步骤101，获取当前实时采集的惯导数据，包括从 k时刻到 k+1时刻惯性测量单元本体坐标系3个方向的角速度与比力；

步骤102，基于所述角速度与所述比力，得到从 k时刻到 k+1时刻惯性测量单元本体坐标系3个方向的角度增量与速度增量；

步骤103，基于实时导航线程 k时刻的状态数据以及所述角度增量、所述速度增量，得到实时导航线程 k+1时刻的状态数据；

步骤104，判断是否存在回溯导航线程完成回溯解算周期：

若是，则基于回溯结果校正实时导航线程 k+1时刻的状态数据中对应的状态参量和/或实时导航线程的误差参量，并将校正后的状态数据作为 k+1时刻的导航信息输出；

否则，将步骤103中得到的状态数据作为 k+1时刻的导航信息输出。

在其中一个实施例，在回溯导航线程 i中，进行回溯导航计算与回溯卡尔曼滤波计算的过程具体为：

步骤201，当存储的卫导数据连续可用时间大于回溯导航线程 i的设定的逆向数据时长时，记当前时间点为，并复制时刻实时导航线程中的惯性测量单元的位置、速度、姿态、陀螺漂移、加速度计漂移作为回溯导航线程 i中惯性导航计算的惯导状态初值；

步骤202，从时刻的惯导状态初值开始，根据存储的惯导数据，开展逆向惯性导航位置、速度、姿态更新计算，同时以存储的历史卫导数据为观测值，开展逆向卡尔曼滤波系统误差估算，直到逆向计算到历史数据时间点时刻；

步骤203，以历史数据时间点时刻惯性导航的状态值以及卡尔曼滤波状态值为初始值，根据存储的惯导数据，开展正向惯性导航位置、速度、姿态更新计算，同时以存储的历史卫导数据为观测值，开展正向卡尔曼滤波系统误差估算，直到正向计算到历史数据时间点，利用卡尔曼滤波误差估算值，校正回溯导航线程 i的对应惯性导航误差；

步骤204，以正向惯性导航计算历史数据时间点校正后的惯性导航的状态值为初值，继续开展正向惯性导航计算，一直计算到当前实时导航线程的最新时间，将回溯导航线程 i的对应惯性导航参数值复制替换实时导航线程中的对应参数，实现对实时导航线程中的对应导航误差校正；

步骤205，重新步骤201步骤至步骤204并不断循环，即能实现周期性的对实时导航线程进行误差校正。

在其中一个实施例，当回溯导航线程的数量为两个以上时，在各回溯导航线程中：

采用相同的误差状态矢量、观测方程和状态方程；或

根据各回溯导航线程针对的状态参量和/或误差参量，建立相应的误差状态矢量、观测方程和状态方程。

与现有技术相比，本发明的优点在于：

(1)、本发明的组合导航方法中，可采用不同导航计算线程对不同的导航误差进行了最优估算及校正，各种不同估算时间要求及不同类型的误差修正互不干扰，便于系统针对不同误差参数设计不同的最优卡尔曼滤波估算模型，从而提高误差估算及校正的精度；

(2)、本发明的组合导航方法中，通过回溯导航线程对同一组历史数据进行了逆向和正向计算，相当于对历史数据重复计算了2次来估算导航误差，相比于常规组合导航方法只进行了1次正向计算来说，误差估算的收敛性及稳定性更好，从而进一步提高了误差估算及校正的精度；

(3)、本发明的组合导航方法中，只要历史数据条件满足，即可以开展并完成回溯导航计算及误差校正，不存在由于卫星信号突然不可用而导致的组合导航滤波中断问题，从而自然也避免了由于组合导航滤波中断而导致的系统误差估算补偿精度下降问题，提升了组合导航系统的可靠性和稳定性；

(4)、本发明的组合导航方法中，对于每一个回溯导航线程，都只需要一个较短的回溯解算周期即可以完成常规方法中需要花费较长时间才能完成的误差参量估算及校正，即系统对误差校正频率远高于常规方法的频率，从而使系统对各种误差的动态跟踪能力明显优于常规方法；

(5)、本发明的组合导航方法比常规组合导航方法的组合导航精度更高，尤其是当采用本发明的方法进行长时间组合导航后，可以通过设定较长的逆向数据时长，很方便地对系统的长航时漂移误差参数进行估算，从而提升惯导系统从组合导航状态切换到纯惯性导航状态后的长期导航精度。

附图说明

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

图1为本发明实施例中多线程回溯式组合导航方法的流程图；

图2为本发明实施例中第i个回溯导航线程的计算流程图；

图3为本发明实施例中第i个回溯导航线程单周期计算时序图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

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

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

参考图1，本实施例公开了一种捷联惯导与卫导的多线程回溯式组合导航方法，通过搭载在载体上的捷联惯导与卫导的组合系统导航计算机同时进行多个导航解算线程，其中1个线程为实时导航线程，其余 m个线程为回溯导航线程，该组合导航方法的过程为：所述组合导航方法包括如下步骤：

实时采集陀螺仪、加速度计等惯性测量单元(IMU，Inertial Measurement Unit)惯导数据，并且实时接收外部卫导数据，同时，将采集与接收的惯导数据、卫导数据存储至导航计算机的内存环形缓冲区，其中，惯导数据包括载体上陀螺仪的角速度以及加速度计的比力，卫导数据为载体的位置与速度。

在实时导航线程，基于实时采集的惯导数据进行惯性导航的位置、速度、姿态更新计算，并将计算得到导航信息实时输出；

在回溯导航线程，基于历史存储的惯导数据、卫导数据与实时导航线程的状态数据进行周期性的回溯导航计算与回溯卡尔曼滤波计算，并在每个回溯解算周期结束后基于回溯结果对实时导航线程中对应的状态参量和/或误差参量进行校正，其中，状态参量指的是位置、速度、姿态等导航参数，误差参量指的陀螺漂移、加速度计漂移等误差参数；

其中，各回溯导航线程的回溯解算周期、逆向数据时长均不相同。在组合导航过程中，实时导航线程与各回溯导航线程并行运行，以校正实时导航线程中不同的状态参量和/或误差参量。

本实施例中，在实时导航线程基于实时采集的惯导数据进行惯性导航的位置、速度、姿态更新计算的具体实施方式为：

步骤101，获取当前实时采集的惯导数据，包括从 k时刻到 k+1时刻惯性测量单元(包括3个正交的陀螺仪与3个正交的加速度计)本体坐标系3个方向的角速度，、、与比力、、；

步骤102，基于实时采集的角速度与比力，以及3个陀螺仪和3个加速计的漂移值，计算得到从 k时刻到 k+1时刻惯性测量单元本体坐标系3个方向的角度增量与速度增量，计算过程为：

其中，、、分别为从 k时刻到 k+1时刻惯性测量单元本体坐标系3个方向的角度增量，、、分别为从 k时刻到 k+1时刻惯性测量单元本体坐标系3个方向的速度增量，、、分别为3个陀螺仪的漂移，、、分别为3个加速度计的漂移，为惯导递推计算的时间间隔。

步骤103，基于实时导航线程 k时刻的状态数据以及步骤102中得到的 k时刻到 k+1时刻惯性测量单元角度增量、速度增量，即能得到实时导航线程 k+1时刻的状态数据，包括载体在 k+1时刻的经度、纬度、高度、东向速度、北向速度、天向速度与姿态角等导航信息；

步骤104，依次检查 m个回溯导航线程，判断是否存在回溯导航线程刚好完成一次各自的回溯解算周期：

若是，则基于对应回溯导航线程的回溯结果校正实时导航线程 k+1时刻的状态数据中对应的位置、速度、姿态等状态参量和/或实时导航线程的陀螺漂移、加速度计漂移等误差参量，并将校正后的状态数据作为 k+1时刻的导航信息输出；

否则，将步骤103中得到的状态数据作为 k+1时刻的导航信息输出。

在具体应用过程中，在经过一次上述的步骤101至步骤104后即完成了一次实时惯导计算。在完成一次实时惯导计算后，等到下次实时惯导计算周期的到来，之后重新按照上述步骤101至步骤104以为计算周期不断循环。

参考图2，在回溯导航线程 i( i=1,2,…, m)中，进行回溯导航计算与回溯卡尔曼滤波计算的过程具体为：

步骤201，当环形缓冲区存储的卫导数据连续可用时间大于回溯导航线程 i的设定的逆向数据时长时，记当前时间点为，并复制时刻实时导航线程中的惯性测量单元的位置、速度、姿态、陀螺漂移、加速度计漂移等值作为回溯导航线程 i中惯性导航计算的惯导状态初值；

步骤202，在回溯导航线程 i中，从时刻的惯导状态初值开始，根据环形缓冲区存储的IMU角速度、、和比力、、，开展逆向惯性导航位置、速度、姿态更新计算，同时在此过程中，以环形缓冲区存储的历史卫导数据为观测值，开展逆向卡尔曼滤波系统误差估算，直到逆向计算到历史数据时间点时刻，停止逆向惯性导航计算和逆向卡尔曼滤波系统误差估算，并保存当前所有的惯性导航及卡尔曼滤波状态值，此步骤回溯导航线程 i计算完成所需要的真实时间为秒；

步骤203，接着在回溯导航线程 i中，以历史数据时间点时刻惯性导航的状态值以及卡尔曼滤波状态值为初始值，根据环形缓冲区存储的IMU角速度、、和比力、、，开展正向惯性导航位置、速度、姿态更新计算，同时在此过程中，以环形缓冲区存储的历史卫导数据为观测值，开展正向卡尔曼滤波系统误差估算，直到正向计算到历史数据时间点，停止正向惯性导航计算和正向卡尔曼滤波系统误差估算，同时利用卡尔曼滤波误差估算值，校正回溯导航线程 i的对应惯性导航误差，此步骤回溯导航线程 i计算完成所需要的真实时间为秒；

步骤204，接着在回溯导航线程 i中，以步骤203中正向惯性导航计算历史数据时间点校正后的惯性导航的状态值为初值，继续开展正向惯性导航计算，一直计算到当前实时导航线程的最新时间，达到与实时导航线程的时间同步后，在实时导航线程的步骤104中，将回溯导航线程 i的对应惯性导航参数值复制替换实时导航线程中的对应参数，从而实现对实时导航线程中的对应导航误差校正，此步骤回溯导航线程 i计算完成所需要的真实时间为秒；

步骤205，最后在回溯导航线程 i中，重新步骤201步骤至步骤204开始回溯运算并不断循环，从而实现以为回溯解算周期对实时导航线程进行周期性的误差校正。

回溯解算周期由导航计算机对每个回溯导航线程的计算能力决定，具体有如下关系：

第 i个回溯导航线程进行1个周期的回溯导航计算的时序如图3所示。实时导航线程是按照固定频率采集陀螺仪及加速度计数据并进行导航计算的，回溯导航线程则是连续计算内存中所有限定时间内的历史数据，当回溯导航线程完成逆向导航计算及滤波、正向导航计算及滤波后，重新回到历史数据时间点时，实时导航线程则已经达到了时间点，因此回溯导航线程必须继续正向导航计算秒，才可以追上实时导航线程从而达到时间同步。

例如：第 i个回溯导航线程的回溯解算周期为20秒，其中，秒，当回溯导航线程 i完成逆向导航计算及滤波、正向导航计算及滤波后，重新回到历史数据时间点时，实时导航线程则已经达到了秒时间点，因此回溯导航线程 i最后用1秒的时间完成正向数据时长20秒的正向惯性导航计算，即能实现与实时导航线程的时间同步。

在具体应用过程中，若导航坐标系采用东北天地理坐标系，则步骤202中逆向惯性导航位置、速度与姿态更新计算的过程为：

逆向惯性导航的位置更新计算的过程为：

其中，、、分别为 k时刻的经度、纬度、高度，、、分别为 k+1时刻的经度、纬度、高度，、、分别为 k+1时刻的东向速度、北向速度、天向速度，、分别为 k+1时刻所在位置地球子午面及其垂直平面内的曲率半径，为惯导递推计算的时间间隔。

逆向惯性导航的速度更新计算的过程为：

其中，、、分别为 k时刻的东向速度、北向速度、天向速度；、、分别为 k+1时刻的东向速度、北向速度、天向速度；为地球自转角速度，为 k+1时刻重力加速度值大小；、、分别为从 k时刻到 k+1时刻惯性测量单元东北天三个方向的速度增量，其计算公式为：

其中，为 k+1时刻的从惯性测量单元本体坐标系 b到导航坐标系 n的变换矩阵，、、为环形缓冲区中存储的惯性测量单元从 k时刻到 k+1时刻本体坐标系三个坐标轴方向上的比力大小。

逆向惯性导航姿态更新计算的过程为：

其中，为 k时刻的从惯性测量单元本体坐标系 b到导航坐标系 n的变换矩阵，为导航坐标系 n从 k+1时刻到 k时刻的变换矩阵，是本体坐标系 b从 k时刻到 k+1时刻的变换矩阵。

在东北天地理坐标系中，可以表示为：

其中，导航坐标系的转动角速度、、可表示为：

矩阵可以用存储的惯性测量单元角速度表示，为：

其中，、、为环形缓冲区中存储的惯性测量单元从 k时刻到 k+1时刻本体坐标系三个坐标轴方向上的角速度。

在具体实施过程中，逆向惯性导航开始时刻的惯性导航位置、速度、姿态、陀螺漂移、加速度计漂移参数值直接采用实时导航线程中的惯性参数，然后通过上述递推公式，只要给定了当前 k+1时刻惯性导航位置、速度、姿态的参数值，就可以根据环形缓冲区中存储的角速度值和比力值，逆向递推出前面 k时刻惯性导航的位置、速度、姿态参数值，不断循环进行，从而实现了逆向惯性导航算法。

在步骤203与步骤204中，开展正向惯性导航计算的具体实施过程与前述步骤101至步骤103相同，区别仅在于步骤101至步骤103采用的是实时采集的惯性数据，步骤203与步骤204中采用的历史存储的惯性数据，因此本实施例不再对步骤203与步骤204中开展正向惯性导航计算的过程进行追溯。

步骤202中，逆向惯性导航的卡尔曼滤波递推计算与常规的正向惯性导航卡尔曼滤波计算流程一样，只是逆向导航的状态矩阵不同。对于逆向导航，相当于时间的反演。因此步骤202中逆向卡尔曼滤波系统误差估算的误差状态方程和误差观测方程为：

步骤203中正向卡尔曼滤波系统误差估算的误差状态方程和误差观测方程为：

其中， X为系统的状态误差参量， A为系统的状态矩阵， Z为系统的误差观测矢量， H为系统的观测矩阵。

由此可见，在常规的正向导航卡尔曼滤波中，只需要将状态矩阵 A取负号，就可以得到逆向导航的卡尔曼滤波状态矩阵，进一步可以得到逆向导航的状态转移矩阵，其余的卡尔曼滤波计算方法及流程与常规的正向导航卡尔曼滤波方法一致。

为了开展逆向卡尔曼滤波递推计算，需要根据惯导系统的误差方程给出上述系统状态矩阵 A的具体表达式。

以东北天地理坐标系为导航坐标系，不考虑惯导系统的高度通道，可以建立系统的状态矢量为：

上述状态误差参量 X中各个分分量分别为系统的经度误差、纬度误差、东速误差、北速误差、 x方向姿态角误差、 y方向姿态角误差、 z方向姿态角误差、陀螺 x漂移、陀螺 y漂移、陀螺 z漂移、加速度计 x漂移、加速度计 y漂移、加速度计 z漂移。

当导航解算采用东北天地理坐标系的指北方位系统时，状态矩阵 A可以表示为：

其中，表示阶零矩阵，其它矩阵元的表达式为：

，

其中，为地球自转角速度，、分别为当前位置地球子午面及其垂直平面内的曲率半径，、、分别为当前位置地理坐标系中东北天三个方向的比力大小。、、、、、分别为当前经度、纬度、高度、东向速度、北向速度、天向速度。

以卫星导航的经度、纬度为基准参考值，可得位置误差观测矢量如下：

其中，，，、分别为惯性导航解算得到经度、纬度，、分别为卫星导航得到经度、纬度。

根据误差观测矢量，观测矩阵 H则取为下列矩阵：，其中 H矩阵为阶矩阵，由阶单位矩阵和阶零矩阵构成。

在步骤203中，正向惯性惯性导航计算和正向卡尔曼滤波估算与常规的惯性导航计算和卡尔曼滤波估算完全一致，也与实时导航线程中的惯性导航计算方法一致，只是其计算数据来源于环形缓冲区中存储的历史数据。在步骤203中，可以得到第 i个回溯导航线程估算的系统状态矢量最优估计值，为：

其中，、、、为经度、纬度、东向速度、北向速度误差参量最优估计值，、、为姿态角度误差参量最优估计值，、、为陀螺漂移误差参量最优估计值，、、为加速度计漂移误差参量最优估计值。

利用状态矢量最优估计值即可以对第 i个回溯导航线程的导航误差和漂移进行校正，其中经度、纬度、东向速度、北向速度的修正计算方法为：

其中，、、、为校正后的经度、纬度、东向速度、北向速度，、、、为校正前的经度、纬度、东向速度、北向速度。

姿态的修正方法是根据、、对惯性测量单元姿态矩阵进行修正，为：

其中，为修正后的系统姿态矩阵，为修正前的系统姿态矩阵。

陀螺漂移、加速度计漂移的修正方法如下：

其中、、为修正后的陀螺漂移，、、为修正前的陀螺漂移，、、为修正后的加速度计漂移，、、为修正后的加速度计漂移。

需要说明的是，系统状态矢量的各个分量中，不同误差参量的可观测性及滤波时间长短不同，因此通过对不同回溯导航线程逆向数据时长的设定，即可以实现对其中部分误差参量的最优估算，并且在每个回溯导航线程利用其得到的状态矢量估计值校正误差时，也只需要校正其可以估算出来的对应误差参量。一般来说，如果某一个回溯导航线程的逆向数据时长较短，则适合校正位置、速度误差，逆向数据时长较长，则适合校正航向、陀螺漂移等误差。另外，由于不同的回溯导航线程的逆向数据时长不同，适合不同的误差参量校正，在回溯导航线程 i对实时导航线程进行误差修正时，不需要修正全部的误差参量，只需要修正此回溯导航线程对应的观测性最好的误差参量。最终所有回溯导航线程的综合作用，即保证了系统处于组合导航状态下的高精度导航信息输出，也同时保证了系统转为纯惯性导航状态后的导航精度尽可能达到惯性器件的极限精度。当回溯导航线程的数量为两个以上时，在各回溯导航线程中：采用相同的误差状态矢量、观测方程和状态方程；或针对所需要估算及校正的不同导航误差，结合所使用捷联惯导系统的本身特性，设计不同的误差状态矢量、观测方程和状态方程，从而达到最优的组合导航效果。

例如，若共设置有3个回溯导航线程，1号回溯导航线程的逆向数据时长较短，2号回溯导航线程的逆向数据时长适中，3号回溯导航线程的逆向数据时长较短较长。因此可以选择1号回溯导航线程的回溯结果校正位置、速度误差，选择2号回溯导航线程的回溯结果校正姿态误差，选择3号回溯导航线程的回溯结果校正陀螺漂移误差。

需要注意的是，在具体应用过程中并不局限于一定设置多个回溯导航线程，也可以仅设置一个或两个回溯导航线程对系统的所有状态参量和误差参量进行校正。

下面以一个单轴旋转式捷联惯导与卫导的组合导航示例对本实施例中的组合导航方法作出进一步的说明。

在此示例中，单轴旋转式捷联惯导系统转轴位于竖直方向，旋转调制周期设定为600秒。在导航计算机中，按照本实施例中的组合导航方法建立1个实时导航线程，进行IMU角速度数据、IMU比力数据、卫导数据在环形缓冲区中的实时存储，并开展实时惯性导航计算。同时导航计算机另外建立3个回溯导航线程，按照组合导航方法开展回溯导航计算及滤波，并对实时导航线程进行误差校正，具体每一个回溯导航线程的设置及误差校正情况如下。

(1)回溯导航线程1：回溯解算周期为1秒，逆向数据时长为60秒。

由于误差参量的可观测性所限，在60秒的逆向数据时长内，回溯导航线程1能够估算的系统误差参数有限，只有位置误差、速度误差等参量。

在回溯导航线程1计算了逆向60秒历史数据和正向60秒历史数据后，得到了系统的误差最优估计值。利用中的分量、、、分别修正回溯导航线程1的经度、纬度、东向速度、北向速度。

接着当回溯导航线程1正向惯性导航计算历史数据，到达与实时导航线程时间同步状态后，将实时导航线程中的经度、纬度、东向速度、北向速度值用回溯导航线程1中的对应经度、纬度、东向速度、北向速度值替代，然后实时导航线程更新与经度、纬度、东向速度、北向速度值有关的所有中间变量及状态量。

(2)回溯导航线程2：回溯解算周期为20秒，逆向数据时长为1200秒。

由于单轴旋转式捷联系统的旋转调制作用，在1200秒的逆向数据时长内，位置误差、速度误差、姿态误差、水平陀螺漂移、水平加速度计漂移等误差参数均可以在此时间内估算出来。

在回溯导航线程2计算了逆向1200秒历史数据和正向1200秒历史数据后，得到了系统的误差最优估计值。利用中的分量、、、分别修正回溯导航线程2的经度、纬度、东向速度、北向速度，利用中的分量、、修正回溯导航线程2的姿态误差，利用中的分量、修正回溯导航线程2中的水平方向上两个加速度计漂移，利用中的分量、修正回溯导航线程2中的水平方向上两个陀螺漂移。

接着当回溯导航线程2正向惯性导航计算历史数据，到达与实时导航线程时间同步状态后，将实时导航线程中的姿态矩阵、水平加速度计漂移、水平陀螺漂移值用回溯导航线程2中的姿态矩阵、水平加速度计漂移、水平陀螺漂移值替代，然后实时导航线程更新与姿态矩阵、水平加速度计漂移、水平陀螺漂移值有关的所有中间变量及状态量。在这里不需要再对实时导航线程的位置、速度误差进行修正，是因为在回溯导航线程1中已经以更快的频率进行修正了。

(3)回溯导航线程3：回溯解算周期为360秒，逆向数据时长为21600秒。

由于单轴旋转式捷联系统的旋转调制作用，对于长时间的导航，天向陀螺漂移的影响最大。在21600秒的逆向数据时长内，地球转动了1/4周，天向陀螺漂移是可以估算出来的。

在回溯导航线程3计算了逆向21600秒历史数据和正向21600秒历史数据后，得到了系统的误差最优估计值。利用中的分量、、、分别修正回溯导航线程3的经度、纬度、东向速度、北向速度，利用中的分量、、修正回溯导航线程3的姿态误差，利用中的分量修正回溯导航线程3中的天向陀螺漂移。

接着当回溯导航线程3正向惯性导航计算历史数据，到达与实时导航线程时间同步状态后，将实时导航线程中的天向陀螺漂移值用回溯导航线程3中天向陀螺漂移值替代，然后实时导航线程更新天向陀螺漂移值有关的所有中间变量及状态量。在这里不需要再对实时导航线程的其它误差进行修正，是因为其它误差在回溯导航线程1和回溯导航线程2中已经以更快的频率进行修正了，而且对于其它误差，长时间的卡尔曼滤波还可能出现结果不收敛、动态响应差等情况，其估算值可能反而不如回溯导航线程1和回溯导航线程2的估算精度高。

在此回溯导航线程3中，上述的流程与步骤203和步骤204保持了一致，实际上，由于此示例中的回溯导航线程3只用来校正天向陀螺漂移误差，针对此回溯导航线程3上述步骤203和步骤204可以合并简化为：在回溯导航线程3计算了逆向21600秒历史数据和正向21600秒历史数据后，直接用估算出来的修正实时导航线程中的对应天向陀螺漂移误差。

在此示例中，每一个回溯导航线程的设置及功能如下表所示：

在此示例中，回溯导航线程1的回溯解算周期和逆向数据时长最短，用于快速校正位置、速度误差；回溯导航线程2的回溯解算周期和逆向数据时长稍长，用于校正姿态、部分惯性器件误差；回溯导航线程3的回溯解算周期和逆向数据时长最长，用于校正天向陀螺漂移误差。其中回溯导航线程1、2保证了系统在组合导航状态下的导航精度，回溯导航线程3保证了系统转为纯惯性导航状态后的导航精度。

需要说明的是，在上述实施方式及示例中，不同的回溯导航线程采用了相同的误差状态矢量、观测方程和状态方程，只是逆向数据时长不同。实际上，为了取得更好的误差校正及组合导航效果，不同的回溯导航线程可以设计不同的误差状态矢量、观测方程和状态方程。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (6)

1.一种捷联惯导与卫导的多线程回溯式组合导航方法，其特征在于，采用实时导航线程与回溯导航线程进行组合导航，所述组合导航方法包括如下步骤：

实时采集并存储惯导数据与卫导数据；

在实时导航线程，基于实时采集的惯导数据进行惯性导航的位置、速度、姿态更新计算，并将计算得到导航信息实时输出；

在回溯导航线程，基于历史存储的惯导数据、卫导数据与实时导航线程的状态数据进行周期性的回溯导航计算与回溯卡尔曼滤波计算，并在每个回溯解算周期结束后基于回溯结果对实时导航线程中对应的状态参量和/或误差参量进行校正；

其中，所述实时导航线程与所述回溯导航线程并行运行；

所述回溯导航线程的数量为两个以上，且各所述回溯导航线程的回溯解算周期、逆向数据时长均不相同；

在组合导航过程中，各所述回溯导航线程并行运行，以校正实时导航线程中不同的状态参量和/或误差参量；

在回溯导航线程i中，进行回溯导航计算与回溯卡尔曼滤波计算的过程具体为：

步骤201，当存储的卫导数据连续可用时间大于回溯导航线程i的设定的逆向数据时长时，记当前时间点为，并复制时刻实时导航线程中的惯性测量单元的位置、速度、姿态、陀螺漂移、加速度计漂移作为回溯导航线程i中惯性导航计算的惯导状态初值；

步骤202，从时刻的惯导状态初值开始，根据存储的惯导数据，开展逆向惯性导航位置、速度、姿态更新计算，同时以存储的历史卫导数据为观测值，开展逆向卡尔曼滤波系统误差估算，直到逆向计算到历史数据时间点时刻；

步骤203，以历史数据时间点时刻惯性导航的状态值以及卡尔曼滤波状态值为初始值，根据存储的惯导数据，开展正向惯性导航位置、速度、姿态更新计算，同时以存储的历史卫导数据为观测值，开展正向卡尔曼滤波系统误差估算，直到正向计算到历史数据时间点，利用卡尔曼滤波误差估算值，校正回溯导航线程i的对应惯性导航误差；

步骤204，以正向惯性导航计算历史数据时间点校正后的惯性导航的状态值为初值，继续开展正向惯性导航计算，一直计算到当前实时导航线程的最新时间，将回溯导航线程i的对应惯性导航参数值复制替换实时导航线程中的对应参数，实现对实时导航线程中的对应导航误差校正；

步骤205，重复步骤201步骤至步骤204并不断循环，即能实现周期性的对实时导航线程进行误差校正。

2.根据权利要求1所述的捷联惯导与卫导的多线程回溯式组合导航方法，其特征在于，所述基于实时采集的惯导数据进行惯性导航的位置、速度、姿态更新计算，具体为：

步骤101，获取当前实时采集的惯导数据，包括从k时刻到k+1时刻惯性测量单元本体坐标系3个方向的角速度与比力；

步骤102，基于所述角速度与所述比力，得到从k时刻到k+1时刻惯性测量单元本体坐标系3个方向的角度增量与速度增量；

步骤103，基于实时导航线程k时刻的状态数据以及所述角度增量、所述速度增量，得到实时导航线程k+1时刻的状态数据；

步骤104，判断是否存在回溯导航线程完成回溯解算周期：

若是，则基于回溯结果校正实时导航线程k+1时刻的状态数据中对应的状态参量和/或实时导航线程的误差参量，并将校正后的状态数据作为k+1时刻的导航信息输出；

否则，将步骤103中得到的状态数据作为k+1时刻的导航信息输出。

3.根据权利要求2所述的捷联惯导与卫导的多线程回溯式组合导航方法，其特征在于，步骤102中，所述角度增量、所述速度增量的计算过程为：

，

，

，

，

，

，

其中，、、分别为从k时刻到k+1时刻惯性测量单元本体坐标系3个方向的角度增量，、、分别为从k时刻到k+1时刻惯性测量单元本体坐标系3个方向的速度增量，、、分别为从k时刻到k+1时刻惯性测量单元本体坐标系3个方向的角速度，、、分别为从k时刻到k+1时刻惯性测量单元本体坐标系3个方向的比力，、、分别为3个陀螺仪的漂移，、、分别为3个加速度计的漂移，为惯导递推计算的时间间隔。

4.根据权利要求1或2或3所述的捷联惯导与卫导的多线程回溯式组合导航方法，其特征在于，步骤204中，正向惯性导航计算的数据时长等于回溯导航线程i的回溯解算周期时长。

5.根据权利要求1或2或3所述的捷联惯导与卫导的多线程回溯式组合导航方法，其特征在于，步骤202中，逆向卡尔曼滤波系统误差估算的误差状态方程和误差观测方程为：

，

，

步骤203中，正向卡尔曼滤波系统误差估算的误差状态方程和误差观测方程为：

，

，

其中，X为系统的状态误差参量，A为系统的状态矩阵，Z为系统的误差观测矢量，H为系统的观测矩阵。

6.根据权利要求5所述的捷联惯导与卫导的多线程回溯式组合导航方法，其特征在于，当回溯导航线程的数量为两个以上时，在各回溯导航线程中：

采用相同的误差状态矢量、观测方程和状态方程；或

根据各回溯导航线程针对的状态参量和/或误差参量，建立相应的误差状态矢量、观测方程和状态方程。

Images