CN112284419A - 一种双轴旋转调制初始精对准方法 - Google Patents

Abstract

本发明提供了一种双轴旋转调制初始精对准方法。首先，获得载体所在位置的纬度和高程信息，进而完成***的粗对准过程，然后基于导航计算结果进入精对准过程。本专利主要处理问题是精对准过程中，由于误差模型中存在惯性器件常值零偏不完全可观测和收敛速度慢的问题。通过引入双轴旋转调制机构可以改善误差模型满足完全可观测条件，从而保证了对准结果的精度和可靠性。通过缩短离散化时间，增加单位时间内的观测序列长度，从而获得了更丰富的观测信息，大大缩短了等效天向陀螺仪常值零偏估计序列的收敛时间，提高了初始对准的快速性。通过优化离散化误差模型的更新方法，从而降低了由于缩短滤波周期导致的计算复杂度增加问题。结合三者，本专利提供了一种工程实用性强的快速高精度的双轴旋转调制初始精对准方法。

Description

一种双轴旋转调制初始精对准方法

技术领域

本发明属于导航技术中惯性导航***初始对准领域，特别涉及一种双轴旋转调制初始精对准方法。

背景技术

导航技术是一种为不同运载体提供位置、速度、姿态和时间等相关信息的技术，与运载体完成任务期间的安全性密切相关，故受到了广泛的关注。而惯性导航技术是不同武器***平台常用的一种导航技术。由于其工作特点的要求，惯性导航***是一种基于初始位置、速度和姿态信息为基础，从惯性器件量测信息中实时获得载***置、速度和姿态等导航信息的***。

由于其可靠性、自主性和全天候等特点，惯性导航***在陆海空天各领域被广泛使用。但是由于惯性导航***是基于初始信息的航位递推***，故在进入导航模式前，需要首先获得高精度的初始信息，为后续导航递推结果提供高精度初始基准。由于惯性导航***导航误差随工作时间增长而变大，初始信息的精度关系到武器平台的长航时精度和可靠性。

但是，武器平台不仅要求导航***的精度高，且要求具备一定的机动和防摧毁能力。而惯性导航***的初始对准过程一般要求运载体处于静基座条件下，即要求运载体处于对地近似静止状态，而这一要求降低了平台的防摧毁能力，故一般又要求初始对准过程在满足精度要求的前提下，对准时间越短越好。换言之，在惯性导航***的初始对准精度满足要求的前提下，如何缩短对准时间一直是惯性导航***应用中的重点关注问题之一。

惯性导航***的初始自对准过程依据其对准的基本原理可以分为两步：粗对准和精对准过程，其中粗对准是依据一定时间内的惯性器件测量数据获得初始姿态阵的粗略结果，可以提供精对准过程的初始方向余弦矩阵等信息。精对准过程是在粗对准过程的基础上，依据惯性器件的测量信息,通过精对准过程修正初始姿态等误差信息，获得高精度的惯性导航***的初始信息过程。

在一般捷联式惯性导航***的静基座初始精对准过程中，存在惯性器件零偏状态的误差模型无法满足完全可观测条件，故高精度初始对准过程一般为多位置对准方案。但是多位置对准方案中，天向陀螺零偏的收敛时间较长，无法满足***对高精度初始化的快速性要求。换言之，在惯性导航***初始精对准中，精度和对准时间是一对矛盾的指标。一般无法在要求精度的前提下，再要求***满足对准时间短。但是两者对惯性导航***的精度和适应性都至关重要，寻找高精度的快速初始对准方法是惯性导航***工程应用中一直未得到解决的问题之一，也是当前迫切需要解决的惯性导航技术难题之一。

发明内容

本发明的目的在于提供一种双轴旋转调制初始精对准方法，以解决上述问题。

为实现上述目的，本发明采用以下技术方案：

一种双轴旋转调制初始精对准方法，包括以下步骤：

步骤1，基于陆用惯性导航***自对准的线性***模型，利用事先装订的载***置信息和惯性器件输出序列完成粗对准过程；

步骤2，基于粗对准结果和惯性器件输出，依据四子样导航解算方法和选定的16位置转停方案的姿态补偿方法进行导航更新，获得惯性导航***的速度误差，作为卡尔曼滤波器的观测信息；

步骤3，更新误差模型的状态转移矩阵；将误差模型中状态转移矩阵采用更新变化；

步骤4，基于卡尔曼滤波器，估计惯性导航***的误差，并将角误差进行反馈修正下一步导航更新过程；

步骤5，满足设定的对准结束时间，初始对准结束，获得惯性导航***的初始基准信息，并获得加速度计和陀螺仪的常值零偏估计，为双轴旋转调制惯性导航***提供初始方向余弦矩阵和惯性器件补偿参考。

进一步的，步骤1中陆用惯性导航***自对准的线性***模型为：

其中，

δV_E为东向速度误差，δV_N为北向速度误差，φ_E为俯仰角误差，φ_N为横滚角误差，φ_U为航向角误差，

为imu坐标系下x轴的加速度计常值零偏，

为imu坐标系下y轴的加速度计常值零偏，ε_x为imu坐标系下x轴的陀螺仪常值零偏，ε_y为imu坐标系下y轴的陀螺仪常值零偏，ε_z为imu坐标系下z轴的陀螺仪常值零偏；

为等效东向角速度计随机误差，

为等效北向加速度计随机误差，ε_E为等效东向陀螺仪随机误差，ε_N为等效北向陀螺仪随机误差，ε_U为等效天向陀螺仪随机误差；

ω_ie为地球自转角速率，L为载体所在位置地理纬度，g为重力加速度，R_M为载体所在位置子午圈曲率半径，R_N为载体所在位置卯酉圈曲率半径；

其中，

为imu坐标系到导航坐标系的方向余弦矩阵，

为载体坐标系到导航坐标系的方向余弦矩阵，

为imu坐标系到载体坐标系的方向余弦矩阵；

y_k为k时刻观测的速度误差，v_k为K时刻速度误差的观测噪声；k时刻观测的速度误差为第一分量为观测的东向速度误差，以及第二分量为观测的北向速度误差；

模型中，w为惯性器件的等效随机误差分量，其协方差矩阵事先获取；当惯性导航***处于静基座条件时，F₁₁矩阵为定常矩阵；

如果载体处于近似静基座条件下，惯性导航***不存在旋转机构，则

为单位矩阵，

为常值矩阵，则该误差模型不满足完全可观测条件；

如果存在转动机构，则

为可变矩阵，通过引入16位置的转停方案后，该误差模型满足完全可观测条件，通过卡尔曼滤波器获得惯性器件常值零偏估计。

进一步的，载***置信息为纬度、经度和海拔高度。

进一步的，步骤3中，误差模型的状态转移矩阵如下所示：

ω_ie为地球自转角速率，L为载体所在位置地理纬度，g为载体所在位置的重力加速度，R_M为载体所在位置的子午圈曲率半径，R_N为载体所在位置的卯酉圈曲率半径；

F₁₁为常值矩阵，其时变部分由于旋转机构的存在，F₁₂是时变矩阵；

采用将F₁₁按照泰勒展开形式进行离散化，获得离散化后的转台转移子矩阵，方法如下：

其中，n取10阶，T取0.02秒，Φ₁₁和Γ₁₁为离散后的状态转移矩阵子矩阵和输入矩阵子矩阵。

通过上述方法，可以获得离散后的状态方差如下，

x_k＝Φx_k-1+Γω_k-1

其中，

其中，0_5×5为零矩阵，I_5×5为5阶单位矩阵。

进一步的，步骤4中，基于离散后的误差模型，将速度误差作为卡尔曼滤波器的观测序列，代入标准卡尔曼滤波方法中获得实时的状态估计及估计偏差的协方差矩阵；其处理过程与标准卡尔曼滤波方法类似，分为：时间更新和观测更新两部分；

时间更新：

观测更新：

其中

为k时刻***状态预估值，P_k,k-1为其协方差矩阵；

为k时刻状态估计值，P_k为k时刻状态估计偏差的协方差矩阵；K_k为卡尔曼滤波增益；

将姿态误差估计值进行实时反馈，将误差结果补偿后的姿态值作为下一个周期姿态解算的初始值，并将滤波器中状态估计中姿态估计初始值置零。

与现有技术相比，本发明有以下技术效果：

本发明相对于一般捷联式惯性导航***的精对准方法，引入了16位置转停方案，采用了多位置对准方案，实现了惯性器件等效零偏的有效估计，提高了初始精对准结果的精度和可靠性。相比于传统的多位置和一般双轴旋转调制***精对准过程，本发明提高了数据更新率，采用了50赫兹的滤波器更新率，缩短了滤波器收敛的时间，降低了惯性导航***初始对准对时间的要求，提高了载体的机动能力和战场生存能力。

此外，从方法的实现可以看出，为了有效的解决由于提高滤波器数据更新率带来的计算复杂度增加问题，我们对状态转移矩阵的更新进行了优化设计，实现了基于矩阵乘法而非高阶离散化的状态转移矩阵的更新方法，有效降低了滤波器计算过程的计算复杂度，为方法良好的实时性提供支撑，利于惯性导航***的工程实践应用和实现。

附图说明

图1为本发明的流程图；

图2为不同初始对准方法俯仰角对准误差结果示意图；

图3为不同初始对准方法横滚角对准误差结果示意图；

图4不同初始对准方法航向角对准误差结果示意图；

图5不同初始对准方法航向角对准误差结果局部放大示意图；

图6不同初始对准方法等效东向加速度计零偏估计结果示意图；

图7不同初始对准方法等效东向加速度计零偏估计结果局部放大示意图；

图8不同初始对准方法等效北向加速度计零偏估计结果示意图；

图9不同初始对准方法等效北向加速度计零偏估计结果局部放大示意图；

图10不同初始对准方法等效东向陀螺仪零偏估计结果示意图；

图11不同初始对准方法等效北向陀螺仪零偏估计结果示意图；

图12不同初始对准方法等效天向陀螺仪零偏估计结果示意图。

具体实施方式

本方法的基本原理为：

通过引入了16位置转停方案，实现了惯性器件常值零偏的有效估计，提高了初始精对准结果的精度和可靠性。通过提高数据更新率，缩短了滤波器收敛的时间，降低了惯性导航***初始对准对时间的要求，提高了载体的机动能力和战场生存能力。为了有效解决由于提高滤波器数据更新率导致的计算复杂度增加问题，我们对状态转移矩阵的更新进行了优化设计，实现了基于矩阵乘法而非高阶离散化的状态转移矩阵的更新方法，有效降低了滤波器计算过程的计算复杂度，为方法良好的实时性提供支撑，利于惯性导航***的工程实践应用和实现。

下面结合实施例，对本发明进行详细描述。

本实施例中，为了便于描述惯性导航***自对准新方法，我们首先给出其模型和前提假设条件。

该方法所针对陆用惯性导航***自对准的线性***模型为：

其中，

δV_E为东向速度误差，δV_N为北向速度误差，φ_E为俯仰角误差，φ_N为横滚角误差，φ_U为航向角误差，

为imu坐标系下x轴的加速度计常值零偏，

为imu坐标系下y轴的加速度计常值零偏，ε_x为imu坐标系下x轴的陀螺仪常值零偏，ε_y为imu坐标系下y轴的陀螺仪常值零偏，ε_z为imu坐标系下z轴的陀螺仪常值零偏；

为等效东向角速度计随机误差，

为等效北向加速度计随机误差，ε_E为等效东向陀螺仪随机误差，ε_N为等效北向陀螺仪随机误差，ε_U为等效天向陀螺仪随机误差；

ω_ie为地球自转角速率，L为载体所在位置地理纬度，g为重力加速度，R_M为载体所在位置子午圈曲率半径，R_N为载体所在位置卯酉圈曲率半径；

其中，

为imu坐标系到导航坐标系的方向余弦矩阵，

为载体坐标系到导航坐标系的方向余弦矩阵，

为imu坐标系到载体坐标系的方向余弦矩阵。

y_k为k时刻观测的速度误差(第一分量为观测的东向速度误差，第二分量为观测的北向速度误差)，v_k为K时刻速度误差的观测噪声；

模型中，w为惯性器件的等效随机误差分量，故其协方差矩阵可事先获取。当惯性导航***处于静基座条件时，F₁₁矩阵为定常矩阵。其速度误差的量测噪声会随着惯性器件精度的变化而不同，但如果惯性器件误差精度确定，一般可以基于统计规律获得。

如果载体处于近似静基座条件下，惯性导航***不存在旋转机构，则

为单位矩阵，

为常值矩阵，则该误差模型不满足完全可观测条件。一般认为静基座条件下加速度计的常值零偏和某一方向的陀螺仪常值零偏不可观测，这会导致对准结果中，俯仰角和横滚角包含了由于加速度计零偏导致的误差，即俯仰角和横滚角对准结果精度降低。

如果存在转动机构，则

为可变矩阵，通过引入16位置的转停方案后，该误差模型满足完全可观测条件，故通过卡尔曼滤波器可以获得可靠的惯性器件常值零偏估计，提高了对准结果的精度。但是，经典的多位置对准方案采用1赫兹的更新率，则天向陀螺的常值零偏估计值收敛时间一般不小于6小时，这大大限制了载体的战场机动能力。而本发明提高了在引入转停方案的基础上，提出了将数据更新率提高到50赫兹(惯性导航***导航结果输出为50赫兹，故50赫兹的数据更新率中速度观测不存在相关性问题)，增加了滤波器单位时间内的数据，实现了天向陀螺常值零偏估计值收敛时间不大于40分钟的效果。在保证对准精度的前提下，本方案降低了惯性导航***对准时间，大大增加了载体的战场机动能力。

针对上述惯性导航***初始对准的精对准滤波器估计序列收敛精度和速度问题，本发明的具体实施步骤如下：

步骤一、利用事先装订的载***置(纬度、经度和海拔高度)信息和惯性器件输出序列完成粗对准过程；

根据初始位置信息和惯性器件的测量序列，粗对准方案可以获得惯性导航***的粗对准结果，即存在一定误差的载体初始信息。本部分非本发明的核心部分，且前期已经存在相关的专利可以查询，故不再详述。

步骤二、基于粗对准结果和惯性器件输出，依据四子样导航解算方法和选定的16位置转停方案的姿态补偿方法进行导航更新，获得惯性导航***的速度误差，作为卡尔曼滤波器的观测信息；

基于粗对准的结果可以获得存在误差的初始信息，基于惯性器件输出和双轴旋转调制参数的补偿方法，可以获得实时的载体速度信息。但是由于载体处于近似对地静止状态，故解算的速度可以被认为是速度误差，为卡尔曼滤波器提供观测信息。

步骤三、更新误差模型的状态转移矩阵；

考虑到载体处于对地近似静止状态，以及将滤波估计其的更新频率提高后对处理器的要求，将误差模型中状态转移矩阵的更新方法不再采用定阶次的更新方法，而是更新变化的部分，从而降低状态转移矩阵更新对处理器的要求，方便方法的工程应用。

误差模型的状态转移矩阵如下所示：

ω_ie为地球自转角速率，L为载体所在位置地理纬度，g为载体所在位置的重力加速度，R_M为载体所在位置的子午圈曲率半径，R_N为载体所在位置的卯酉圈曲率半径；

从其定义我们不难发现，F₁₁为常值矩阵，其时变部分主要由于旋转机构的存在，即F₁₂是时变矩阵。

针对该问题，本发明采用将F₁₁按照泰勒展开形式进行离散化，从而获得离散化后的转台转移子矩阵，方法如下：

其中，n取10阶，T取0.02秒，Φ₁₁和Γ₁₁为离散后的状态转移矩阵子矩阵和输入矩阵子矩阵。

通过上述方法，可以获得离散后的状态方差如下，

x_k＝Φx_k-1+Γω_k-1

其中，

其中，0_5×5为零矩阵，I_5×5为5阶单位矩阵。

从上述方法，我们不难发现在更新状态方差过程中，由于载体处于静止状态，不需要在每个周期对状态转移矩阵进行泰勒展开求解，只需要根据16位置转停方案对F₁₂进行更新，进而通过矩阵乘法更新离散化的状态方差，大大降低了由于离散化而导致的计算量增大问题，提高了运算效率。

步骤四、基于卡尔曼滤波器，估计惯性导航***的误差，并将角误差进行反馈修正下一步导航更新过程；

基于离散后的误差模型，将速度误差作为卡尔曼滤波器的观测序列，代入标准卡尔曼滤波方法中获得实时的状态估计及估计偏差的协方差矩阵。其处理过程与标准卡尔曼滤波方法类似，可以分为：时间更新和观测更新两部分。

时间更新：

观测更新：

其中

为k时刻***状态预估值，P_k,k-1为其协方差矩阵；

为k时刻状态估计值，P_k为k时刻状态估计偏差的协方差矩阵；K_k为卡尔曼滤波增益。

为了防止由于误差增大而导致惯性导航***的误差传播模型不满足线性化假设条件，在滤波过程中，我们将姿态误差估计值进行实时反馈，即将误差结果补偿后的姿态值作为下一个周期姿态解算的初始值，并将滤波器中状态估计中姿态估计初始值置零。

步骤五、满足设定的对准结束时间，初始对准结束。

当对准过程满足40分钟对准时间要求后，我们可以获得惯性导航***的初始姿态和航向信息，并获得加速度计和陀螺仪的常值零偏估计，为双轴旋转调制惯性导航***提供初始方向余弦矩阵和惯性器件补偿参考，进一步达到抑制惯性导航***导航结果误差的目标。

本发明首先，获得载体所在位置的纬度和高程信息，进而完成***的粗对准过程，然后基于导航计算结果进入精对准过程。本专利主要处理问题是精对准过程中，由于误差模型中存在惯性器件常值零偏不完全可观测和收敛速度慢的问题。通过引入双轴旋转调制机构可以改善误差模型满足完全可观测条件，从而保证了对准结果的精度和可靠性。通过缩短离散化时间，增加单位时间内的观测序列长度，从而获得了更丰富的观测信息，大大缩短了等效天向陀螺仪常值零偏估计序列的收敛时间，提高了初始对准的快速性。通过优化离散化误差模型的更新方法，从而降低了由于缩短滤波周期导致的计算复杂度增加问题。结合三者，本发明提供了一种工程实用性强的快速高精度的双轴旋转调制初始精对准方法。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种双轴旋转调制初始精对准方法，其特征在于，包括以下步骤：

步骤1，基于陆用惯性导航***自对准的线性***模型，利用事先装订的载***置信息和惯性器件输出序列完成粗对准过程；

步骤2，基于粗对准结果和惯性器件输出，依据四子样导航解算方法和选定的16位置转停方案的姿态补偿方法进行导航更新，获得惯性导航***的速度误差，作为卡尔曼滤波器的观测信息；

步骤3，更新误差模型的状态转移矩阵；将误差模型中状态转移矩阵采用更新变化；

步骤4，基于卡尔曼滤波器，估计惯性导航***的误差，并将角误差进行反馈修正下一步导航更新过程；

步骤5，满足设定的对准结束时间，初始对准结束，获得惯性导航***的初始基准信息，并获得加速度计和陀螺仪的常值零偏估计，为双轴旋转调制惯性导航***提供初始方向余弦矩阵和惯性器件补偿参考。

2.根据权利要求1所述的一种双轴旋转调制初始精对准方法，其特征在于，步骤1中陆用惯性导航***自对准的线性***模型为：

其中，

δV_E为东向速度误差，δV_N为北向速度误差，φ_E为俯仰角误差，φ_N为横滚角误差，φ_U为航向角误差，

为imu坐标系下x轴的加速度计常值零偏，

为imu坐标系下y轴的加速度计常值零偏，ε_x为imu坐标系下x轴的陀螺仪常值零偏，ε_y为imu坐标系下y轴的陀螺仪常值零偏，ε_z为imu坐标系下z轴的陀螺仪常值零偏；

为等效东向角速度计随机误差，

为等效北向加速度计随机误差，ε_E为等效东向陀螺仪随机误差，ε_N为等效北向陀螺仪随机误差，ε_U为等效天向陀螺仪随机误差；

ω_ie为地球自转角速率，L为载体所在位置地理纬度，g为重力加速度，R_M为载体所在位置子午圈曲率半径，R_N为载体所在位置卯酉圈曲率半径；

其中，

为imu坐标系到导航坐标系的方向余弦矩阵，

为载体坐标系到导航坐标系的方向余弦矩阵，

为imu坐标系到载体坐标系的方向余弦矩阵；

y_k为k时刻观测的速度误差，v_k为K时刻速度误差的观测噪声；k时刻观测的速度误差为第一分量为观测的东向速度误差，以及第二分量为观测的北向速度误差；

模型中，w为惯性器件的等效随机误差分量，其协方差矩阵事先获取；当惯性导航***处于静基座条件时，F₁₁矩阵为定常矩阵；

如果载体处于近似静基座条件下，惯性导航***不存在旋转机构，则

为单位矩阵，

为常值矩阵，则该误差模型不满足完全可观测条件；

如果存在转动机构，则

为可变矩阵，通过引入16位置的转停方案后，该误差模型满足完全可观测条件，通过卡尔曼滤波器获得惯性器件常值零偏估计。

3.根据权利要求1所述的一种双轴旋转调制初始精对准方法，其特征在于，载***置信息为纬度、经度和海拔高度。

4.根据权利要求1所述的一种双轴旋转调制初始精对准方法，其特征在于，步骤3中，误差模型的状态转移矩阵如下所示：

ω_ie为地球自转角速率，L为载体所在位置地理纬度，g为载体所在位置的重力加速度，R_M为载体所在位置的子午圈曲率半径，R_N为载体所在位置的卯酉圈曲率半径；

F₁₁为常值矩阵，其时变部分由于旋转机构的存在，F₁₂是时变矩阵；

采用将F₁₁按照泰勒展开形式进行离散化，获得离散化后的转台转移子矩阵，方法如下：

其中，n取10阶，T取0.02秒，Φ₁₁和Γ₁₁为离散后的状态转移矩阵子矩阵和输入矩阵子矩阵；

通过上述方法，可以获得离散后的状态方差如下，

x_k＝Φx_k-1+Γω_k-1

其中，

其中，0_5×5为零矩阵，I_5×5为5阶单位矩阵。

5.根据权利要求1所述的一种双轴旋转调制初始精对准方法，其特征在于，步骤4中，基于离散后的误差模型，将速度误差作为卡尔曼滤波器的观测序列，代入标准卡尔曼滤波方法中获得实时的状态估计及估计偏差的协方差矩阵；其处理过程与标准卡尔曼滤波方法类似，分为：时间更新和观测更新两部分；

时间更新：

观测更新：

其中

为k时刻***状态预估值，P_k,k-1为其协方差矩阵；

为k时刻状态估计值，P_k为k时刻状态估计偏差的协方差矩阵；K_k为卡尔曼滤波增益；

将姿态误差估计值进行实时反馈，将误差结果补偿后的姿态值作为下一个周期姿态解算的初始值，并将滤波器中状态估计中姿态估计初始值置零。

Images