CN105300405B - 一种主基准速度时间延迟估计和补偿方法 - Google Patents

Abstract

本发明是一种主基准速度时间延迟估计和补偿方法。包括如下步骤：步骤一：建立带时间延迟状态量τ_V的系统误差模型；步骤二：观测噪声自适应卡尔曼滤波估计；步骤三：利用加速度微量积分对速度时间延迟修正补偿；步骤3.1：计算速度时间延迟估计累计值；步骤3.2：速度时间延迟带来的速度误差补偿。该方法通过一种观测噪声自适应卡尔曼滤波对主基准速度时间延迟进行实时估计，并通过加速度微量积分方法求取准确速度时间延迟量，然后进行补偿，从而保证传递对准精度和导航输出精度。

Description

一种主基准速度时间延迟估计和补偿方法

技术领域

本发明属于惯性导航技术领域，涉及到一种主基准速度时间延迟估计和补偿方法。

技术背景

传递对准是惯导系统利用精度较高的主基准信息进行自身的航姿估计和相关误差估计，主基准信息的精度对传递对准的估计精度至关重要。在传递对准前，主基准导航信息的精度一般经过专门的测试考核，均能够满足使用指标要求。但影响传递对准的精度不光受主基准导航精度影响，传递过来的主基准信息时间特性对传递对准精度同样重要。

目前，子惯导利用主惯导信息的传输通道包括数字串口、以太网以及模拟量等。模拟量传递一般能够保证信息传递的实时性，而数字串口和以太网实时性较差，特别是以太网传输，往往由于其他信息报文或者系统分发处理的非实时性，导致传递过来的导航信息时间延迟过大，甚至能够达到秒级以上。这样的主基准信息将严重影响子惯导传递对准精度，甚至对准失败。

针对该问题，目前常采取的做法是对主基准信息的时间延迟进行预先测量或者在线估计。对于时间延迟较为固定的情况，可以进行预先测量，但若时间延迟随时间波动较大，则无法预先获得一个准确的 时间延迟信息，也就无法准确补偿。目前采用在线估计方法一般是在系统误差模型中建立时间延迟状态量，利用普通卡尔曼滤波器进行在线估计，然后对时间延迟进行补偿。由于时间延迟的估计本身依赖载体机动进行激励，但载体机动时往往伴随着观测噪声特性发生变化，这种噪声会减弱甚至淹没时间延迟引起的误差观测量，从而导致这种普通的卡尔曼滤波器将无法准确估计出时间延迟，甚至时间延迟不收敛。另外，传统的时间延迟补偿往往利用当前的运动信息(角速度信息和加速度信息)一步计算时间延迟带来的姿态误差和速度误差，这种情况下，若载体运动参数变化较快，主基准信息时间延迟较大，则因误差补偿模型不准确，导致时间延迟补偿失效，影响传递对准精度和导航输出精度。

为解决当主基准速度时间延迟过大且波动变化时，在子惯导传递对准中利用普通卡尔曼滤波估计和常规补偿时导致估计和补偿不准确的问题，有必要提出一种主基准速度时间延迟估计与补偿方法，该方法通过一种观测噪声自适应卡尔曼滤波对主基准速度时间延迟进行实时估计，并通过加速度微量积分方法求取准确速度时间延迟量，然后进行补偿，从而保证传递对准精度和导航输出精度。

发明内容

发明目的

本发明的目的在于，要解决的技术问题为：当主基准速度时间延迟过大且波动变化时，在子惯导传递对准中利用普通卡尔曼滤波器无 法准确估计出速度时间延迟量；另外对速度时间延迟补偿而言，若载体运动参数变化较快，主基准速度时间延迟较大，传统的时间延迟补偿往往利用当前的加速度一步计算时间延迟带来的速度误差，则因误差补偿模型不准确，导致时间延迟补偿失效，影响传递对准精度和导航输出精度。

技术方案

本发明是一种主基准速度时间延迟估计和补偿方法，其中，包括如下步骤：

步骤一：建立带时间延迟状态量τ_V的系统误差模型；

步骤二：观测噪声自适应卡尔曼滤波估计；

步骤三：利用加速度微量积分对速度时间延迟修正补偿；

步骤3.1：计算速度时间延迟估计累计值；

δT_delay(t_k)＝δT_delay(t_k-1)+τ_V

δT_delay(t_k)为t_k时刻速度时间延迟的估计累计值，即τ_V的累计值；

步骤3.2：速度时间延迟带来的速度误差补偿；

首先，对当前时刻的速度时间延迟δT_delay＝δT_delay(t_k)，除以导航周期T_n取整数，即

NUM_delay＝|δT_delay/T_n|_取整

然后，得到残余时间延迟为

δT_surplus＝δT_delay-NUM_delay·T_n

保存当前时刻的前NUM_delay-1个导航计算得到的惯导在地理系中 的北、天、东向加 速度值f_n(t-i·T_n)、f_u(t-i·T_n)、f_e(t-i·T_n)，其中i＝1～(NUM_delay-1)，将当前时刻的f_n (t)、f_u(t)、f_e(t)和保存的之前(NUM_delay-1)个f_n(t-i·T_n)、f_u(t-i·T_n)、f_e(t-i·T_n)对应 累加求和得到 于是可得速度 时间延迟带来的速度误差补偿值为：

如上所述的一种主基准速度时间延迟估计和补偿方法，其中：

在步骤3.2中，利用得到的速度误差对主基准延迟速度进行补偿，补偿方法如下：

延迟补偿后的主基准北天东速度；

延迟补偿前的主基准北天东速度。

有益效果

当主基准速度时间延迟过大且波动变化时，在子惯导传递对准中利用普通卡尔曼滤波器无法准确估计出速度时间延迟量；另外对速度 时间延迟补偿而言，若载体运动参数变化较快，主基准速度时间延迟较大，传统的时间延迟补偿往往利用当前的加速度一步计算时间延迟带来的速度误差，则因误差补偿模型不准确，导致时间延迟补偿失效，影响传递对准精度和导航输出精度。本发明提出一种主基准速度时间延迟估计与补偿方法，该方法通过一种观测噪声自适应卡尔曼滤波对主基准速度时间延迟进行实时估计，并通过加速度微量积分方法求取准确速度时间延迟量，然后进行补偿，从而保证传递对准精度和导航输出精度。该方法能够满足目前主子惯导舰传递对准时速度延迟的估计和补偿需求，提高传递对准精度。

具体实施方式

以下，结合具体实施方式，对本发明做进一步的说明。

当主基准速度时间延迟过大且波动变化时，在子惯导传递对准中利用普通卡尔曼滤波器无法准确估计出速度时间延迟量；另外对速度时间延迟补偿而言，若载体运动参数变化较快，主基准信息时间延迟较大，传统的时间延迟补偿往往利用当前的加速度一步计算时间延迟带来的速度误差，则因误差补偿模型不准确，导致时间延迟补偿失效，影响传递对准精度和导航输出精度。

本发明提出一种主基准速度时间延迟估计与补偿方法，该方法通过一种观测噪声自适应卡尔曼滤波对主基准速度时间延迟进行实时估计，并通过加速度微量积分方法求取准确速度时间延迟量，然后进行补偿，从而保证传递对准精度和导航输出精度。

该方法具体步骤如下：

步骤一：建立带时间延迟状态量的系统误差模型；

首先建立子惯导速度匹配惯导系统模型

其中：

系统状态量选取

X＝[δV_n δV_u δV_e φ_n φ_u φ_e ε_x ε_y ε_z ▽_x ▽_y ▽_z τ_V]^T(2)

系统观测量选取

表示惯导的北天东速度；

表示主基准的北天东速度；

观测矩阵为

系统矩阵A为常规系统矩阵，属于公知技术，这里不赘述。

上述公式中：

δV_n、δV_u、δV_e为捷联惯导系统的速度误差；

φ_n、φ_u、φ_e为捷联惯导系统的失准角；

ε_x、ε_y、ε_z为捷联惯导系统的陀螺零偏；

▽_x、▽_y、▽_z为捷联惯导系统的加速度计零偏；

τ_V为主基准速度时间延迟；

f_n、f_u、f_e为捷联惯导系统的计算的北、天和东向加速度值。

步骤二：观测噪声自适应卡尔曼滤波估计；

利用系统误差模型式(1)进行离散化得到离散系统模型

然后利用下面观测噪声自适应闭环卡尔曼滤波对误差状态进行估计，过程如下：

状态一步预测

一步预测均方误差

P_k,k-1＝Φ_k,k-1P_k-,k-1Φ^T _k,k-1+Q_k； (7)

新息计算

量测噪声方差

R_k＝diag(r₁₁(k),r₂₂(k),r₃₃(k)) (9)

其中i＝1、2、3，r_ii为给定的观测噪声方差阵对角线元素，C_i为给定的自适应参数，一般取C_i＝(0.5～1.5)。

滤波增益

估计均方误差

P_k＝[I-K_kH_k]P_k； (12)

状态估计

按照式(6)～式(13)完成误差状态的估计，其中包括时间延迟τ_V的估计。

此过程为卡尔曼滤波的常规过程。

步骤三：利用加速度微量积分对速度时间延迟修正补偿

a)速度时间延迟闭环修正过程

速度时间延迟估计累计值

δT_delay(t_k)＝δT_delay(t_k-1)+τ_V (14)

δT_delay(t_k)为t_k时刻速度时间延迟T_delay的估计值；

同时令

是指主基准时间延迟τ_V对应位置的向量中的一个值；

例如，如式(2)所示，τ_V位于13位，则，式(15)中

其他误差状态量的修正按照常规闭环卡尔曼滤波处理方法进行，这里不赘述。

b)速度时间延迟带来的速度误差补偿

具体补偿方式为：

首先，对当前时刻的速度时间延迟δT_delay(t_k)除以导航周期T_n取整数，即

NUM_delay＝|δT_delay(t_k)/T_n|_取整 (16)

得到在本次延迟之前，已经经历的导航周期个数，每个周期产生一次延迟，即得到了已经经历的延迟的次数。

然后，得到残余时间延迟为

δT_surplus＝δT_delay-NUM_delay·T_n (17)

保存当前时刻前(NUM_delay-1)个导航计算得到的惯导在地理系中的北、天、东向加 速度值f_n(t-i·T_n)、f_u(t-i·T_n)、f_e(t-i·T_n)，其中i＝1～(NUM_delay-1)，将当前时刻的f_n (t)、f_u(t)、f_e(t)和保存的之前(NUM_delay-1)个f_n(t-i·T_n)、f_u(t-i·T_n)、f_e(t-i·T_n)对应 累加求和得到 于是可得速度 时间延迟带来的速度误差补偿值为

利用式(18)得到的速度误差对主基准延迟速度进行补偿，补偿方法如下：

延迟补偿后的主基准北天东速度；

延迟补偿前的主基准北天东速度。

至此，完成主基准速度时间延迟的估计和补偿。

实施例

以某舰局部基准航行试验数据为例，该局部基准利用舰上某平台惯导通过以太网输出的速度信息作为基准信息，进行传递对准和校准。利用本发明的方法对主基准速度时间延迟进行估计和补偿，方法如下：

步骤一：建立带时间延迟状态量的系统误差模型；

步骤二：进行观测噪声自适应卡尔曼滤波估计，其中量测噪声方差取R_k＝diag(r₁₁(k),r₂₂(k),r₃₃(k))＝diag[(0.1m/s)²,(0.1m/s)²,(0.1m/s)²]，自适应参数取C₁＝C₂＝C₃＝1.2，按照式(6)～式(13)完成误差状态的估计；

步骤三：步骤二中速度时间延迟估计累计值在航行300s后即收敛，收敛后稳定值δT_delay＝1.2s；

局部基准导航周期为T_n＝0.005s，所以需要保存当前时刻前个导航计算得到的惯导在地理系中的北、天、东向加速度值f_n(t-i·T_n)、f_u(t-i·T_n)、f_e(t-i·T_n)，其中i＝1～239；将当前时刻的f_n(t)、f_u(t)、f_e(t)和保存的之前239个f_n(t-i·T_n)、f_u(t-i·T_n)、f_e(t-i·T_n)对应累加求和得到 于是可得速度时间延迟带来的速度误差补偿值为

利用上式得到的速度误差按照式(19)对主基准延迟速度进行补偿，从而完成主基准速度时间延迟补偿。

虽然通过上述实施例对本发明所述的一种主基准速度时间延迟估计和补偿方法进行了详细的说明，但是上述说明并不是对本发明的限定，在不脱离本发明的主旨的范围内，可以进行各种变形和变更，例如，最优化的方法可以在现有技术的各种方法中选择。

Claims (2)

1.一种主基准速度时间延迟估计和补偿方法，其特征在于，包括如下步骤：

步骤一：建立带时间延迟状态量τ_V的系统误差模型；

步骤二：观测噪声自适应卡尔曼滤波估计；

步骤三：利用加速度微量积分对速度时间延迟修正补偿；

步骤3.1：计算速度时间延迟估计累计值；

δT_delay(t_k)＝δT_delay(t_k-1)+τ_V

δT_delay(t_k)为t_k时刻速度时间延迟的估计累计值，即τ_V的累计值；

步骤3.2：速度时间延迟带来的速度误差补偿；

首先，对当前时刻的速度时间延迟δT_delay＝δT_delay(t_k)，除以导航周期T_n取整数，即

NUM_delay＝|δT_delay/T_n|_取整

然后，得到残余时间延迟为

δT_surplus＝δT_delay-NUM_delay·T_n

保存当前时刻的前NUM_delay-1个导航计算得到的惯导在地理系中的北、天、东向加速度值、、，其中i=1~(NUM_delay-1)，将当前时刻的f_n(t)、f_u(t)、f_e(t)和保存的之前(NUM_delay-1)个、、对应累加求和得到于是可得速度时间延迟带来的速度误差补偿值为：

2.如权利要求1所述的一种主基准速度时间延迟估计和补偿方法，其特征在于：

在步骤3.2中，利用得到的速度误差对主基准延迟速度进行补偿，补偿方法如下：

延迟补偿后的主基准北天东速度；

延迟补偿前的主基准北天东速度。