CN111707292B - 一种自适应滤波的快速传递对准方法 - Google Patents

Abstract

本发明公开了一种自适应滤波的快速传递对准方法，包括以下步骤：首先建立快速传递对准误差模型；然后建立快速传递对准系统状态方程和增量形式系统量测方程；最后设计改进的Sage‑Husa自适应增量滤波算法，估计失准角，完成主、子惯导传递对准。本发明可以适应快速传递对准模型，保证滤波器的稳定性和精确性，同时提高传递对准的速度和精度。

Description

一种自适应滤波的快速传递对准方法

技术领域

本发明涉及惯性导航系统领域，尤其涉及一种自适应滤波的快速传递对准方法。

背景技术

初始对准为导航系统提供解算初值，对准时间和精度影响惯性导航系统的性能，因此对惯性导航系统而言至关重要。初始对准可分为自对准和传递对准。传递对准是运载体上待对准的中低精度子惯导系统利用已经对准好的高精度主惯导系统完成初始对准的一种方法。相较于自对准而言，传递对准不仅可以降低对惯性器件精度的要求，而且大大缩短了对准时间。目前，传递对准被广泛应用于机载、舰载和车载等多种不同场合。

传递对准模型有平台失准角模型和量测失准角模型两种。平台失准角模型不仅可以估计出主、子惯导之间的姿态失准角，而且可以对子惯导的传感器误差进行估计以提高传递对准的精度，但需要一定的时间消耗，因此适用于传递对准时间较长的情况。量测失准角模型中量测失准角既是状态量又是量测量，该模型形式简单、计算量小，更适用于快速传递对准的场景。直升机、战斗机装备的武器导弹不允许有长时间的提前对准，需要具备很强的机动性和快速反应能力，因此基于量测失准角建立合适的快速传递对准模型对机载武器而言至关重要。

传递对准一般包括粗对准和精对准两个过程。粗对准是利用主惯导系统提供的导航信息(速度、姿态和位置等)对子惯导直接进行装订。由于主子惯导存在失准角、挠曲变形等，需要通过精对准对误差进行估计及修正。精对准过程中的误差估计通常采用kalman滤波，而kalman滤波仅适用于线性系统，并要求系统误差模型准确，过程噪声和量测噪声均值为零，且方差已知，这对处于复杂运动状态下的运载体而言很难实现。

基于此，相关文献提出的Sage-Husa自适应滤波形式简单、计算量小，利用量测数据进行递推滤波的同时，通过时变噪声统计估计器，实时估计和修正系统过程噪声和量测噪声的统计特性，从而达到降低模型误差、抑制滤波发散、提高滤波精度的目的。但是Sage-Husa自适应滤波存在对滤波器初值设定敏感的问题，只有当初值准确时，Sage-Husa滤波器才能获得较好的滤波效果。因此直接使用Sage-Husa自适应滤波将无法保证滤波器的稳定性和精确性。

发明内容

发明目的：针对以上问题，本发明提出一种自适应滤波的快速传递对准方法，以适应快速传递对准模型，保证滤波器的稳定性和精确性，同时提高传递对准的速度和精度。

技术方案：本发明提供的一种自适应滤波的快速传递对准方法，包括如下步骤：

S1，建立快速传递对准误差模型；

S2，建立快速传递对准系统状态方程和增量形式系统量测方程；

S3，设计改进的Sage-Husa自适应增量滤波算法，估计失准角，完成主、子惯导传递对准。

所述的一种自适应滤波的快速传递对准方法，步骤S1具体包括：

S1.1，建立速度误差方程：

其中：n，b_s，b_s′分别为导航坐标系、实际子惯导坐标系和计算子惯导坐标系；δVⁿ为经杆臂效应补偿后的子惯导与主惯导计算速度差在导航坐标系中的投影；

为δVⁿ对时间的导数；

为计算子惯导坐标系到导航坐标系的方向余弦矩阵；φ_m为主惯导坐标系b_m系到计算子惯导坐标系b_s′系的姿态失准角，也称为量测失准角；

为主惯导坐标系b_m系到实际子惯导坐标系b_s系的姿态误差角，即实际失准角；θ为挠曲变形角；

为子惯导敏感到的挠曲变形加速度，

S1.2，建立姿态误差方程：

其中：

为b_s系到n系下的角速率值在实际子惯导坐标系上的投影；

表示φ_m的一阶导数，

S1.3，建立挠曲变形角状态方程：

其中：θ为挠曲变形角；

和

分别是θ的二阶导数和一阶导数；β为相关系数；η为白噪声，

所述的一种自适应滤波的快速传递对准方法，步骤S2具体包括：

S2.1，建立快速传递对准系统状态方程：

选取15维系统状态量：

其中，δV^T表示速度误差状态量δV的转置；φ_m ^T表示量测失准角φ_m的转置；

表示安装误差角

的转置；θ^T表示挠曲变形角θ的转置；

表示挠曲变形角速率

的转置，

传递对准系统状态方程如式(4)所示：

其中，

表示系统状态量一阶导数；A表示状态转移矩阵；W表示系统噪声阵，

状态转移矩阵如式(5)：

其中，

表示

的反对称阵；

表示

的反对称阵；

β_x表示β在x轴的分量，β_y表示β在y轴的分量、β_z表示β在z轴的分量；

S2.2，建立快速传递对准“速度+姿态”匹配方式增量形式量测方程：

快速传递对准“速度+姿态”量测方程如式(6)和式(7)：

其中，Z_v表示速度量测量；

表示子惯导解算的速度值；

表示主惯导解算的速度值；δVⁿ表示主子惯导速度误差；V_v表示速度量测噪声向量；

为

的转置矩阵；

为主惯导坐标系到导航坐标系的姿态矩阵；φ_mx，φ_my，φ_mz分别为φ_m在x轴，y轴和z轴向上的投影，

系统增量形式量测方程如式(8)：

ΔZ_k＝H_kX_k-H_k-1X_k-1+V_z (8)

其中，ΔZ_k表示当前时刻与前一时刻量测量之差；H_k和H_k-1分别为k时刻和k-1时刻的量测矩阵；X_k和X_k-1分别为k时刻和k-1时刻的状态量；V_z为系统量测噪声向量，

量测矩阵如式(9)：

其中，

表示“速度+姿态”匹配的量测阵；I₃为三维单位矩阵；0_6×9表示6×9维零矩阵。

所述的一种自适应滤波的快速传递对准方法，步骤S3具体包括：

S3.1，进行时间更新，根据式(10)计算状态一步预测值：

其中，

表示状态一步预测值；Φ_k,k-1为k-1时刻至k时刻的一步状态转移矩阵；

表示k-1时刻状态估计值；

根据式(11)计算估计误差方差阵P_k,k-1：

其中，P_k,k-1表示估计误差方差阵；

表示Φ_k,k-1的转置；P_k-1表示k-1时刻的误差方差阵；

表示k-1时刻系统噪声阵计算值，

S3.2，进行量测更新：

计算渐消因子λ_k，如式(12)：

其中：

γ_k表示增量新息，计算公式为：γ_k＝ΔZ_k-ΔZ_k-1；γ₁表示初始时刻增量新息向量；

表示γ₁的转置；R_k表示量测噪声方差阵；

N(k)表示残差序列的协方差阵；N(k-1)表示N(k)的前一时刻值；b为遗忘因子，取0.95≤b≤0.995；弱化因子l_k≥1，

l_k＝1-d_k，d_k＝(1-b)/(1-b^k)

计算增益矩阵K_k，如式(13)：

其中，K_k表示k时刻的滤波增益；P_k,k-1表示估计误差方差阵；λ_k表示渐消因子；

表示量测噪声方差阵计算值；

根据式(14)计算状态估计值：

其中，

表示状态估计值；K_k表示滤波增益；ΔZ_k表示k时刻与k-1时刻量测量之差；ΔZ_k-1表示k-1时刻与k-2时刻量测量之差；

更新误差方差阵P_k，如式(15)：

P_k＝[I-K_kH_k]P_k,k-1 (15)

返回到步骤S3.1，直至滤波结束，根据式(14)计算得到的状态估计值对子惯导进行修正，完成传递对准过程。

有益效果：与现有技术相比，本发明实施例的一种自适应滤波的快速传递对准方法更适应于快速传递对准条件，通过引入渐消因子调整一步预测误差方差阵或增益矩阵，使滤波值能够跟踪当前值的变化，抑制滤波器的发散，同时选取相邻两个时刻的量测量之差ΔZ_k作为量测量，尽可能地消除引入到量测方程中的系统误差。保证了传递对准过程中滤波器稳定性的同时，提高了传递对准的速度和精度。

附图说明

图1为本发明传递对准的流程图；

图2为本发明实施例所采用的方法与kalman滤波和Sage-Husa自适应滤波算法失准角估计误差对比曲线图，其中图2(a)、图2(b)、图2(c)分别是俯仰角、横滚角和航向角的估计误差；

图3为本发明实施例所采用的方法与kalman滤波和Sage-Husa自适应滤波算法安装误差角估计误差对比曲线图，其中图3(a)、图3(b)、图3(c)分别是x轴、y轴和z轴安装误差角估计误差；

图4为本发明实施例所采用的方法与kalman滤波和Sage-Husa自适应滤波算法失准角估计误差标准差对比曲线图，其中图4(a)、图4(b)、图4(c)分别是俯仰角、横滚角和航向角的估计误差标准差；

图5为本发明实施例所采用的方法与kalman滤波和Sage-Husa自适应滤波算法安装误差角估计误差标准差对比曲线图，其中图5(a)、图5(b)、图5(c)分别是x轴、y轴和z轴安装误差角估计误差标准差。

具体实施方式

下面结合附图和实施例对本发明作进一步详细说明。

如图1所示，本发明提供的一种自适应滤波的快速传递对准方法，包括如下步骤：

S1，建立快速传递对准误差模型；

S2，建立快速传递对准系统状态方程和增量形式系统量测方程；

S3，设计改进的Sage-Husa自适应增量滤波算法，估计失准角，完成主、子惯导传递对准。

所述的一种自适应滤波的快速传递对准方法，步骤S1具体包括：

S1.1，建立速度误差方程：

其中：n，b_s，b_s′分别为导航坐标系、实际子惯导坐标系和计算子惯导坐标系；δVⁿ为经杆臂效应补偿后的子惯导与主惯导计算速度差在导航坐标系中的投影；

为δVⁿ对时间的导数；

为计算子惯导坐标系到导航坐标系的方向余弦矩阵；φ_m为主惯导坐标系b_m系到计算子惯导坐标系b_s′系的姿态失准角，也称为量测失准角；

为主惯导坐标系b_m系到实际子惯导坐标系b_s系的姿态误差角，即实际失准角；θ为挠曲变形角；

为子惯导敏感到的挠曲变形加速度，

S1.2，建立姿态误差方程：

其中：

为b_s系到n系下的角速率值在实际子惯导坐标系上的投影；

表示φ_m的一阶导数，

S1.3，建立挠曲变形角状态方程：

其中：θ为挠曲变形角；

和

分别是θ的二阶导数和一阶导数；β为相关系数；η为白噪声，

所述的一种自适应滤波的快速传递对准方法，步骤S2具体包括：

S2.1，建立快速传递对准系统状态方程：

选取15维系统状态量：

其中，δV^T表示速度误差状态量δV的转置；φ_m ^T示量测失准角φ_m的转置；

表示安装误差角

的转置；θ^T表示挠曲变形角θ的转置；

表示挠曲变形角速率

的转置，

传递对准系统状态方程如式(4)所示：

其中，

表示系统状态量一阶导数；A表示状态转移矩阵；W表示系统噪声阵，

状态转移矩阵如式(5)：

其中，

表示

的反对称阵；

表示

的反对称阵；

β_x表示β在x轴的分量，β_y表示β在y轴的分量、β_z表示β在z轴的分量；

S2.2，建立快速传递对准“速度+姿态”匹配方式增量形式量测方程：

快速传递对准“速度+姿态”量测方程如式(6)和式(7)：

其中，Z_v表示速度量测量；

表示子惯导解算的速度值；

表示主惯导解算的速度值；δVⁿ表示主子惯导速度误差；V_v表示速度量测噪声向量；

为

的转置矩阵；

为主惯导坐标系到导航坐标系的姿态矩阵；

分别为φ_m在x轴，y轴和z轴向上的投影，

系统增量形式量测方程如式(8)：

ΔZ_k＝H_kX_k-H_k-1X_k-1+V_z (8)

其中，ΔZ_k表示当前时刻与前一时刻量测量之差；H_k和H_k-1分别为k时刻和k-1时刻的量测矩阵；X_k和X_k-1分别为k时刻和k-1时刻的状态量；V_z为系统量测噪声向量，

量测矩阵如式(9)：

其中，

表示“速度+姿态”匹配的量测阵；I₃为三维单位矩阵；0_6×9表示6×9维零矩阵。

所述的一种自适应滤波的快速传递对准方法，步骤S3具体包括：

S3.1，进行时间更新，根据式(10)计算状态一步预测值：

其中，

表示状态一步预测值；Φ_k,k-1为k-1时刻至k时刻的一步状态转移矩阵；

表示k-1时刻状态估计值；

根据式(11)计算估计误差方差阵P_k,k-1：

其中，P_k,k-1表示估计误差方差阵；

表示Φ_k,k-1的转置；P_k-1表示k-1时刻的误差方差阵；

表示k-1时刻系统噪声阵计算值，

S3.2，进行量测更新：

计算渐消因子λ_k，如式(12)：

其中：

γ_k表示增量新息，计算公式为：γ_k＝ΔZ_k-ΔZ_k-1；γ₁表示初始时刻增量新息向量；

表示γ₁的转置；R_k表示量测噪声方差阵；

N(k)表示残差序列的协方差阵；N(k-1)表示N(k)的前一时刻值；b为遗忘因子，取0.95≤b≤0.995；弱化因子l_k≥1，

l_k＝1-d_k，d_k＝(1-b)/(1-b^k)

计算增益矩阵K_k，如式(13)：

其中，K_k表示k时刻的滤波增益；P_k,k-1表示估计误差方差阵；λ_k表示渐消因子；

表示量测噪声方差阵计算值；

根据式(14)计算状态估计值：

其中，

表示状态估计值；K_k表示滤波增益；ΔZ_k表示k时刻与k-1时刻量测量之差；ΔZ_k-1表示k-1时刻与k-2时刻量测量之差；

更新误差方差阵P_k，如式(15)：

P_k＝[I-K_kH_k]P_k,k-1 (15)

返回到步骤S3.1，直至滤波结束，根据式(14)计算得到的状态估计值对子惯导进行修正，完成传递对准过程。

本发明实施例的基于自适应滤波的快速传递对准方法更适应于快速传递对准条件，通过引入渐消因子调整一步预测误差方差阵或增益矩阵，使滤波值能够跟踪当前值的变化，抑制滤波器的发散，同时选取相邻两个时刻的量测量之差ΔZ_k作为量测量，尽可能地消除引入到量测方程中的系统误差。保证了传递对准过程中滤波器稳定性的同时，提高了传递对准的速度和精度。

为验证本发明方法的有效性，通过仿真试验加以验证，试验参数设置如下：

飞机匀速直航且处于三轴摇摆状态中，且不考虑挠曲变形和杆臂效应的影响。初始纬度为32°，初始经度为118.8°；飞行速度为100m/s。摇摆参数为：航向摇摆角幅值为2°，周期为3s；纵摇角幅值为2°，周期为4s；横摇角幅值为5°，周期为5s。初始航向角为30°，初始俯仰角和初始横滚角都为零；三个轴的安装误差角分别为4′、5′和6′；载体挠曲变形的相关时间均为4s。

主惯导陀螺仪随机常值漂移为0.01(°)/h，随机漂移为

主惯导加速度计常值偏置为0.1mg，随机噪声为

子惯导陀螺仪常值漂移为1(°)/h，随机漂移为

子惯导加速度计常值偏置为0.5mg，随机噪声为

滤波器初值设置如下：

P(0)＝diag{(0.1m/s)²，(0.1m/s)²，(0.1m/s)²，(0.2°)²，(0.2°)²，(0.2°)²，(0.1°)²，(0.1°)²，(0.1°)²，(0.5°)²，(0.5°)²，(0.5°)²，(0.01°/s)²，(0.01°/s)²，(0.01°/s)²}

R＝diag{(0.1m/s)²，(0.1m/s)²，(0.1m/s)²，(0.01°)²，(0.01°)²，(0.01°)²}

按照本实施例的方法，图1为本发明传递对准的流程图；图2为本发明实施例下与kalman滤波和Sage-Husa自适应滤波算法失准角估计误差对比曲线图；图3为本发明实施例下与kalman滤波和Sage-Husa自适应滤波算法安装误差角估计误差对比曲线图；图4为本发明实施例下与kalman滤波和Sage-Husa自适应滤波算法失准角估计误差标准差对比曲线图；图5为本发明实施例下与kalman滤波和Sage-Husa自适应滤波算法安装误差角估计误差标准差对比曲线图。从图2-图5可以看出，本发明的方法相对于kalman滤波和Sage-Husa自适应滤波算法失准角收敛速度更快且对准精度更高，同时具有相对较高的稳定性。

以上显示和描述了本发明的基本原理、主要特征和优点。本领域的技术人员应该了解，本发明不受上述具体实施例的限制，上述具体实施例和说明书中的描述只是为了进一步说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护的范围由权利要求书及其等效物界定。

Claims (1)

1.一种自适应滤波的快速传递对准方法，其特征在于，包括如下步骤：

S1，建立快速传递对准误差模型；

S2，建立快速传递对准系统状态方程和增量形式系统量测方程；

S3，设计改进的Sage-Husa自适应增量滤波算法，估计失准角，完成主、子惯导传递对准；

步骤S1具体包括：

S1.1，建立速度误差方程：

其中：n，b_s，b_s′分别为导航坐标系、实际子惯导坐标系和计算子惯导坐标系；δVⁿ为经杆臂效应补偿后的子惯导与主惯导计算速度差在导航坐标系中的投影；

为δVⁿ对时间的导数；

为计算子惯导坐标系到导航坐标系的方向余弦矩阵；φ_m为主惯导坐标系b_m系到计算子惯导坐标系b_s′系的姿态失准角，也称为量测失准角；

为主惯导坐标系b_m系到实际子惯导坐标系b_s系的姿态误差角，即实际失准角；θ为挠曲变形角；

为子惯导敏感到的挠曲变形加速度，

S1.2，建立姿态误差方程：

其中：

为b_s系到n系下的角速率值在实际子惯导坐标系上的投影；

表示φ_m的一阶导数，

S1.3，建立挠曲变形角状态方程：

其中：θ为挠曲变形角；

和

分别是θ的二阶导数和一阶导数；β为相关系数；η为白噪声；

步骤S2具体包括：

S2.1，建立快速传递对准系统状态方程：

选取15维系统状态量：

其中，δV^T表示速度误差状态量δV的转置；φ_m ^T表示量测失准角φ_m的转置；

表示安装误差角

的转置；θ^T表示挠曲变形角θ的转置；

表示挠曲变形角速率

的转置，

传递对准系统状态方程如式(4)所示：

其中，

表示系统状态量一阶导数；A表示状态转移矩阵；W表示系统噪声阵，

状态转移矩阵如式(5)：

其中，

表示

的反对称阵；

表示

的反对称阵；

β_x表示β在x轴的分量，β_y表示β在y轴的分量、β_z表示β在z轴的分量；

S2.2，建立快速传递对准“速度+姿态”匹配方式增量形式量测方程：

快速传递对准“速度+姿态”量测方程如式(6)和式(7)：

其中，Z_v表示速度量测量；

表示子惯导解算的速度值；

表示主惯导解算的速度值；δVⁿ表示主子惯导速度误差；V_v表示速度量测噪声向量；

为

的转置矩阵；

为主惯导坐标系到导航坐标系的姿态矩阵；

分别为φ_m在x轴，y轴和z轴向上的投影，

系统增量形式量测方程如式(8)：

ΔZ_k＝H_kX_k-H_k-1X_k-1+V_z (8)

其中，ΔZ_k表示当前时刻与前一时刻量测量之差；H_k和H_k-1分别为k时刻和k-1时刻的量测矩阵；X_k和X_k-1分别为k时刻和k-1时刻的状态量；V_z为系统量测噪声向量，

量测矩阵如式(9)：

其中，

表示“速度+姿态”匹配的量测阵；I₃为三维单位矩阵；0_6×9表示6×9维零矩阵；

步骤S3具体包括：

S3.1，进行时间更新，根据式(10)计算状态一步预测值：

其中，

表示状态一步预测值；Φ_k,k-1为k-1时刻至k时刻的一步状态转移矩阵；

表示k-1时刻状态估计值；

根据式(11)计算估计误差方差阵P_k,k-1：

其中，P_k,k-1表示估计误差方差阵；

表示Φ_k,k-1的转置；P_k-1表示k-1时刻的误差方差阵；

表示k-1时刻系统噪声阵计算值，

S3.2，进行量测更新：

计算渐消因子λ_k，如式(12)：

其中：

γ_k表示增量新息，计算公式为：γ_k＝ΔZ_k-ΔZ_k-1；γ₁表示初始时刻增量新息向量；

表示γ₁的转置；R_k表示量测噪声方差阵；

N(k)表示残差序列的协方差阵；N(k-1)表示N(k)的前一时刻值；b为遗忘因子，取0.95≤b≤0.995；弱化因子l_k≥1，

l_k＝1-d_k，d_k＝(1-b)/(1-b^k)

计算增益矩阵K_k，如式(13)：

其中，K_k表示k时刻的滤波增益；P_k,k-1表示估计误差方差阵；λ_k表示渐消因子；

表示量测噪声方差阵计算值；

根据式(14)计算状态估计值：

其中，

表示状态估计值；K_k表示滤波增益；ΔZ_k表示k时刻与k-1时刻量测量之差；ΔZ_k-1表示k-1时刻与k-2时刻量测量之差；

更新误差方差阵P_k，如式(15)：

P_k＝[I-K_kH_k]P_k,k-1 (15)

返回到步骤S3.1，直至滤波结束，根据式(14)计算得到的状态估计值对子惯导进行修正，完成传递对准过程。

Images