基于自适应EKF的相对导航算法研究

雷创

（中国电子科技集团公司第二十研究所，西安 710068）

0 引言

编队飞机的相对导航功能将在未来战争中发挥至关重要的作用，可有效弥补绝对导航定位性能较差的缺陷。基于编队飞机间的测距测角功能设备，可在不依赖外界条件下实现平台间自主的相对导航功能。测距测角是一种非线性的测量信息，常规卡尔曼技术无法对其滤波处理。扩展卡尔曼滤波是基于一阶泰勒级数展开的近似线性化滤波技术，是工程中最常用的非线性系统滤波技术之一。扩展卡尔曼滤波中需要知道精确的系统模型和噪声统计先验知识，实际滤波中这些条件较难满足，往往使得滤波结果往往不是最优的，且不经过特殊的处理，极易导致滤波发散。实际应用中多采用自适应卡尔曼滤波方法，例如：强跟踪卡尔曼滤波、基于极大似然准则的自适应卡尔曼滤波、Sage-Husa自适应卡尔曼滤波等。

通常情况下，基于Sage-Husa的自适应时变噪声估计滤波方法具有与常规卡尔曼滤波方法相当的精度且具有计算量小、效率高等优点，得到最为广泛的研究，但其不具有对测量噪声方差阵和系统噪声方差阵进行同时在线估计的能力。因此，本文研究基于自适应扩展卡尔曼滤波的相对导航滤波算法，即在对测量数据进行递推滤波的同时，利用Sage-Husa法对测量噪声方差阵进行动态估计，实时修正测量噪声的统计特性，而系统噪声则是由编队飞机惯导特性的先验值获得，由此通过调节滤波增益参数达到提高相对定位精度且抑制滤波发散的目的。

1 系统模型

以一架长机和一架僚机组成的双机编队为例（参见图1），在不依赖绝对导航信息的情况下，长机通过相对导航功能实时获知僚机高精度的相对位置信息。此中，相对导航坐标系定义为：以长机质心o为原点，x沿长机水平面指向东向，y沿长机水平面指向真北，z沿长机当地垂线指向天向，相对导航坐标系随着长机的运动而移动。

测距测角功能设备的测量信息描述为：测量的距离ρ为两架飞机的径向连线；测量的相对方位角α为两架飞机的径向连线在长机本地水平面的投影与长机真北向的夹角；测量的相对俯仰角β为两架飞机的径向连线与长机本地水平面的夹角。相对导航的测量信息是极坐标系下的位置信息，滤波中需要对该信息进行线性化处理。

图1 相对导航坐标系与测量信息的关系示意图

本文研究中采用简化的相对导航模型，离散化的系统状态方程为：

系统状态矢量为

表示僚机在相对导航坐标系下的三维位置、速度及加速度。

相对导航的系统状态矢量通过将双机的惯导信息相差获得，即：将僚机和长机惯导提供的经纬高位置和、地理系速度和以及地理系加速度和分别转换为地心系下的状态矢量，然后相差并与转换矩阵S相乘，由此获得相对导航坐标系下的系统状态矢量。其中，转换矩阵S为：

式中，1L、1λ分别为长机的经度、纬度。状态矢量求取过程中，僚机的惯导信息是通过数据链发送至长机，研究中假设时延问题已通过时间对准手段进行了同步，测量信息和状态信息都为同一时间的传感器测量信息。

状态转移矩阵φ表示为：

Γ为系统噪声驱动阵：

系统测量方程为：

其中，Zk为测距测角设备k时刻的测量信息，表示为：Z=[ρα β]T，Vk为测量噪声且V～

阶泰勒级数展开方法对测量矩阵进行线性化，可得：

经过线性化处理的测量方程表示为：

2 自适应EKF算法

由于通过测距测角设备受多种因素的影响，其误差通常具有较多的不确定性，且利用EKF线性化处理带来的系统误差以及算法本身不具有在线调整测量噪声方差阵的能力，通常不能获得最优的估计结果。下面研究利用Sage-Husa自适应方法对滤波算法进行优化，以期获得最优的系统状态估计参数。

常规扩展卡尔曼滤波处理过程如下。状态一步预测：

均方误差矩阵一步预测：

滤波增益：

状态估计：

均方误差矩阵估计：

处理过程中，采用Sage-Husa自适应时变噪声统计估计方法对测量噪声方差阵kR进行动态估计，即将kR调整为：

式中，d=(1-b)/(1-bk)，b为遗忘因子，通常在k-1[0 .95,0.995]间取值。当b较小时，Rk对当前时刻测量信息依赖的权重较大，这样虽然使得Rk更快地调整到与实际相匹配的测量噪声方差阵，但也使得Rk阵频繁震荡致使滤波结果具有一定的波动性；当b较大时，Rk则较为依赖历史的测量信息，但Rk收敛时间较长且容易导致滤波发散，因此需要合理地配置b值。

3 仿真验证

下面利用数字仿真对基于自适应 EKF的相对导航算法的效果进行验证。长机和僚机编队的飞行轨迹如图 2所示，其中长机高度 8km、僚机高度6km，仿真时长约 40min。仿真中，惯导加速度计随机漂移50gμ，且双机惯导加速度计误差互不相关；测距误差 3m，相对方位角及相对俯仰角误差都为 0.2°，测量误差均为高斯白噪声，本文研究中不考虑测量设备系统误差的影响；在700s～1000s时间段内测量噪声扩大为默认值的3倍，在1700s～2000s时间段内测量噪声扩大为默认值的5倍；测量噪声方差阵的遗忘因子取0.98。

在上述条件下，将本文研究的自适应EKF算法（AEKF）和常规EKF算法（EKF）进行了比较，相对导航定位的仿真结果如图3～图5所示。

图2 编队飞机飞行轨迹

图3 相对导航东向位置误差

图4 相对导航北向位置误差

图5 相对导航天向位置误差

通过图3～图5可以看出，在测量噪声未扩大的情况下，本文研究的自适应EKF算法与常规EKF算法相比，精度相当，且相对定位结果连续平滑；当测量误差扩大至3倍时，常规EKF算法的相对定位精度为15.4m（σ1），自适应EKF相对定位精度6.5m（σ1）；当测量误差扩大至5倍时，常规EKF算法的相对定位精度为24.6m（σ1），自适应EKF相对定位精度7.2m（σ1）。由此可见，当测量误差变大时，基于自适应EKF滤波算法表现出了良好的性能，相对定位精度明显优于常规滤波算法。

4 结束语

本文研究了基于自适应扩展卡尔曼滤波的相对导航算法，即利用泰勒一阶展开法对测量矩阵进行了线性化处理，并利用Sage-Husa自适应时变噪声估计方法对测量噪声方差阵进行动态估计。通过仿真分析，本文研究的自适应相对导航算法可获得编队飞机间高精度的相对定位信息，尤其是当测量噪声发生变化时，可通过动态调整测量噪声方差阵数值，对相对定位状态参数进行有效的估计。因此，

相对于变化的、不确定的测量噪声，本文研究的基于自适应 EKF相对导航算法具有较强的鲁棒性并可获得高精度的相对定位参数。

[1] 范科, 赵伟, 刘建业. 自适应滤波算法在 SINS/GPS组合导航系统中的应用研究[J]. 航空电子技术, 2008,39(3), 11-15.

[2] 田海, 朱新岩. 一种简化的 Sage_Husa卡尔曼滤波[J].弹箭与制导学报,2011, 31(1), 75-77.

[3] 梁松, 徐晓苏, 黄永亮. Sage_Husa自适应滤波算法在捷联组合导航系统中的应用[J]. 测试技术学报, 2011,25(4), 327-331.