李文敏，刘明威，高兵兵，胡高歌

（西北工业大学自动化学院，西安 710072）

1 引 言

惯性导航/全球导航卫星系统（INS/GNSS）组合已成为无人机应用中最常用的导航策略。然而，其非线性本质以及动态属性严重限制了无人机在复杂任务环境下的应用[1-2]。

信息融合是实现组合导航的关键技术[3-4]。王文益和陈晨应用线性卡尔曼滤波（KF），设计了一种基于卫星/INS 组合导航系统的无人机诱捕方法[5]。然而，由于组合导航系统的非线性本质，KF 已很难适用，特别是在复杂应用环境中。扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）是常用的非线性滤波器[5-6]。然而，由于系统模型的线性化，EKF 对于强非线性系统的估计精度较差[6]，而UKF 在无迹变换中权值会出现负值，导致滤波结果不稳定，特别是对于高维（三维以上）非线性系统，例如INS/GNSS 组合导航系统[7]。

容积卡尔曼滤波（CKF）是一种新兴的非线性高斯滤波策略，其利用球面径向容积法则逼近贝叶斯后验概率密度函数的数值积分，使得状态的后验均值和协方差至少具有二阶近似精度[3-7]。在初始误差大、可观测性弱的情况下，CKF 比EKF 具有更好的性能[6]。此外，文献[8]已经证实，对于高维滤波问题，CKF 相比UKF 具有更好的稳定性和精度。此外，CKF 的计算负担也小于UKF，因为它包含2n个容积点，而UKF 包含2n+1个Sigma 点，从而提高了系统的计算性能[9]。因此，对于系统维数至少为15 维的INS/GNSS 系统，CKF 是一个更优的滤波选择。Muhammad 研究了基于EKF 和CKF 的INS/GPS 组合导航滤波算法，表明了CKF 相对于EKF 的优越性，有效提高了导航滤波器的非线性逼近性能[10]。

然而，KF 框架中的高斯滤波器有一个共同的局限性，即它们的滤波解算严重依赖系统噪声统计参数的精确性[9,11-12]，CKF 也是如此。如果在滤波过程中使用有偏的系统噪声统计，滤波结果会导致较大的估计误差，甚至发散。因此，系统噪声的统计特性是提高INS/GNSS 组合导航定位精度的关键。在INS/GNSS 组合导航系统中，量测噪声协方差R与GNSS 接收机的导航误差有关。借助先进的卫星导航技术，使用高精度的GNSS 观测设备或大量冗余的观测数据可以保证R的精度[13]。然而，由于状态模型的噪声统计量总是与系统动力学具有强耦合的关系，所以很难获取过程噪声协方差矩阵Q的精确信息。因此，研究如何抑制有偏过程噪声方差对INS/GNSS 组合系统解算精度的影响，提高系统的自适应能力是十分必要的。

自适应滤波是处理有偏过程噪声统计影响的常用方法[14-15]。基于不同的自适应策略，如协方差匹配技术、Sage-Husa 估计、贝叶斯推断理论等，学者设计了大量的自适应滤波，以抑制有偏过程噪声方差对系统滤波解算的影响。协方差匹配技术是一种通过保持新息协方差与其理论值一致，从而在线估计过程噪声协方差的自适应策略[11]。然而，该方法在估计过程中会产生一定的稳态估计误差，导致滤波精度改善效果不佳。Sage-Husa估计器基于最大后验原理获取过程噪声的统计信息[14]。然而，其不能保证收敛到正确的过程噪声协方差，因此可能导致滤波发散[11]。贝叶斯推断理论自适应滤波器是基于时不变动态误差假设建立的，并且需要大量的计算负荷，因而不适用于INS/GNSS 组合导航系统[16]。

对于提高CKF 滤波器自适应能力的研究，Cui 等基于Sage-Husa 估计器，建立了一种用于INS/GNSS 组合导航的迭代CKF 算法，通过过程噪声统计估计来抑制过程噪声不确定性对滤波解算的影响[14]。然而，该方法中遗忘因子的选取严重依赖人工经验，改进性能不佳。通过将极大似然准则与期望最大化方法相结合，Zhou 和Liu 提出了一种自适应平方根CKF（ASCKF），用于在线估计和调整过程噪声协方差矩阵[15]。然而，该方法的计算量会随着时间的推移而无限增加，不适用于长时间的无人机导航。更重要的是，上述自适应方法不能避免过程噪声协方差估计的负定结果，需要额外的措施来确保过程噪声协方差估计值的半正定性，这在一定程度上是有效的，但在滤波精度上是折衷的[16-17]。总体而言，在INS/GNSS 组合精确导航方面，如何更好地抑制过程噪声协方差对CKF 解算的影响还缺乏有效的解决方案。

针对INS/GNSS 组合系统过程噪声方差（Q）具有不确定性，本文提出了一种具有先验状态协方差反馈控制的自适应CKF 算法，以提高无人机INS/GNSS 组合导航对过程噪声方差的自适应能力。在该算法中，将系统后验状态和误差协方差信息反馈至滤波过程中，然后基于最大似然准则，利用估计窗口内的反馈状态和误差协方差信息，建立了一种先验状态协方差在线反馈控制策略。提出的方法有效抑制了过程噪声方差不确定性对滤波解的影响，提高了CKF 用于无人机 INS/ GNSS 组合导航解算时的自适应能力。通过无人机INS/GNSS 组合导航仿真实验对提出算法的有效性进行了评估，并与经典CKF 和ASCKF 进行了比较分析。

2 基于四元数的INS/GNSS 组合导航非线性数学模型

2.1 状态模型

选择E-N-U（东北天）地理坐标系（g系）作为无人机导航坐标系（n系）。将机体坐标系表示为b系，惯性坐标系表示为i系，地球坐标系表示为e系。因此，系统的状态变量为

其中，[δq0δq1δq2δq3]T表示加性姿态误差四元数，[δvEδvNδvU]T为速度误差，[δLδλ δh]T为位置误差，[εbxεbvεbz]T为陀螺仪的常值漂移，[∇bx∇bv∇bz]T为加速度计的零偏。

采用四元数来描述姿态误差，可以将INS 姿态误差方程写成[18]

INS 的速度误差方程为

INS 位置误差的传播方程为

其中，式(2)～式(4)中参数的含义参见文献[18]。

接下来，陀螺的输出误差可描述为[15]

其中，bε为陀螺的常值漂移，gw是陀螺的白噪声。

类似地，加速度计输出误差公式为[17]

其中，b∇为加速度计零偏，aw是加速度计的白噪声。

将式(2)～式(4)、式(6)和式(8)组合在一起，可以得到INS/GNSS 组合导航系统的状态模型

其中，f(·)为连续形式的非线性系统函数，w(t)为系统过程噪声。

2.2 量测模型

INS/GNSS 组合导航量测模型可描述如下[19]

3 具有协方差反馈控制的自适应CKF 算法

3.1 经典CKF 算法

为了更清楚地说明提出的自适应CKF 算法的推导过程，首先简要回顾经典CKF 算法的计算过程。

采用改进的Euler 方法[20]对式(9)进行离散化，可以得到一般形式的非线性离散系统模型

其中，xk∈Rn和zk∈Rm表示在k时刻系统的状态向量和量测向量；kw和kv为不相关的零均值高斯白噪声序列，其方差矩阵为和为描述状态模型的非线性函数，Hk是量测矩阵。

基于容积法则，经典CKF 的计算过程总结如下。

步骤1：预测。应用容积法则近似先验状态及其误差协方差

步骤2：更新。由于量测模型为线性模型，则更新过程与卡尔曼滤波一致。

步骤3：返回步骤1，直到处理完所有样本。

由式(14)可以看出，过程噪声协方差信息Qk-1是由先验状态协方差来进行传播的，其通过式(18)～式(20)来影响滤波增益Kk，从而影响系统状态估计式(21)的结果。因此，当过程噪声协方差矩阵存在跳变或偏差时，滤波增益Kk将会失去最优性，从而导致系统状态估计精度变差，即组合导航误差变大。

3.2 先验状态协方差反馈控制

针对上述问题，本节设计一种具有先验状态协方差反馈控制的自适应CKF 算法。首先将系统后验状态和误差协方差信息反馈至滤波过程中，构成CKF 协方差传播的闭环结构；然后，基于极大似然准则，利用估计窗口内的反馈状态和误差协方差信息，建立一种先验状态协方差在线反馈控制策略，从而克服了经典CKF 的缺陷，有效抑制过程噪声方差不确定性对滤波解的影响，提高了滤波器的自适应能力。

定义滤波器的新息向量为

其中，m为量测向量的维数表示矩阵的行列式，为新息向量的协方差矩阵，可由式(18)获得。

在一个固定长度的估计窗口中，对似然函数(25)的对数求取关于的偏导数，并应用矩阵的导数理论[21]，可以得出

其中，s和t分别表示矩阵的第s行和第t列。

由式(27)可以看出，上述极大似然估计问题已被转化为计算新息协方差关于( 1,s,t=2, ,n… )的偏导数。

将式(18)代入式(27)得到

文献[21-22]表明，当估计窗口内的滤波过程达到稳态时，将趋于收敛，式(28)中的微分项将趋于1，因此可得

由式(21)和式(22)，有以下关系

因此，可以得到

3.3 算法实现

根据3.1 节和3.2 节的描述，提出的自适应CKF 的算法流程如图1 所示，其步骤可总结如下。

图1 提出的自适应CKF 算法流程图Fig.1 Flow chart of proposed adaptive CKF

步骤1: 初始化。设置滤波器的初始状态估计及其误差协方差矩阵。

步骤2：预测。计算容积点并执行经典CKF预测步骤式(13)和式(14)。

步骤3：更新。执行经典CKF 更新步骤式(17)～ 式(22)，获取系统的状态估计及其误差协方差矩阵。

通过式(34)调整先验状态协方差矩阵，然后，重新执行经典CKF 更新步骤式(17)～式(22)，获取系统的状态估计及其误差协方差矩阵。

步骤4：返回步骤2，直到处理完所有样本。

4 仿真验证与分析

将提出的自适应CKF 算法应用于INS/GNSS组合导航系统，并通过蒙特卡洛仿真评估提出算法的有效性。

仿真验证模拟固定翼无人机飞行，其主要参数如表1 所示以及运动模型见文献[23]。根据无人机的基本运动状态，设计一条包括平飞、匀速、加速、转弯、爬升、下降等运动状态的飞行轨迹，对无人机的飞行过程进行模拟。假设无人机的初始姿态为(0 ,0 ,0 )° ° ° ，初始速度为（0 m/s, 150 m/s, 0 m/s），初始位置为东经108.997°、北纬34.246°、高度5000 m；轨迹仿真时间为1000 s，模拟的飞行轨迹如图2 所示。

图2 无人机飞行轨迹Fig.2 UAV flight trajectory

表1 无人机主要参数Table 1 Main parameters of UAV

仿真计算中，模拟过程噪声统计存在不确定性的情形，以验证提出的自适应CKF 算法在过程噪声统计存在不确定性时的滤波效果。在(300 s, 700 s)内，滤波器使用的过程噪声方差突变为25×Qk-1，即

导航系统仿真参数设置如表2 所示，滤波周期1 s，蒙特卡洛次数20 次。

表2 仿真参数设置Table 2 Simulation parameters setting

图3 和图 4 分别给出了采用经典CKF、ASCKF 和本文提出的自适应CKF 算法得到的无人机位置和姿态的均方根误差(RMSE)曲线。分析图3 和图4 可以看出：

图3 经典CKF、ASCKF 和本文提出的自适应CKF 得到的无人机位置RMSE 曲线图Fig.3 RMSEs of UAV position obtained by classical CKF、ASCKF and proposed adaptive CKF

图4 经典CKF、ASCKF 和本文提出的自适应CKF 得到的无人机姿态RMSE 曲线图Fig.4 RMSEs of UAV attitude obtained by classical CKF、ASCKF and proposed adaptive CKF

（1）在(300 s, 700 s)内，滤波器中使用的过程噪声方差突变，即过程噪声统计存在不确定性。经典CKF 由于自适应能力较差，因而估计的导航参数误差最大，明显大于过程噪声统计精确已知的时间段；而ASCKF 能够通过噪声统计的在线估计弱化过程噪声统计突变的影响，并提高经典CKF 的滤波精度，其估计误差明显小于经典CKF；与上述两种算法相比，本文提出的自适应CKF 通过先验状态协方差的自适应反馈控制，可

以获得比ASCKF 更高的导航精度。这是由于提出的方法有效避免了对过程噪声协方差负定估计结果的额外处理，保证了先验状态协方差的正定性，从而提高了无人机导航系统的自适应能力和导航精度。

（2）在(0 s, 300 s)和(700 s, 1000 s)内，由于过程噪声协方差未出现突变，即过程噪声统计特性精确已知，经典CKF 能够获得最优的估计结果。因而相比于ASCKF 和提出的自适应CKF 算法，经典CKF 具有最小的估计误差；而ASCKF和本文提出的自适应CKF 在提高系统自适应能力的同时会牺牲部分过程噪声统计特性精确已知情形的估计精度，在该情形下具有一定的次优性，其原因是重新修正后的先验协方差矩阵失去了最优性。此外，进一步可以看出本文提出的自适应CKF 在最大限度地提高系统自适应能力的同时，能够尽可能小地牺牲过程噪声统计特性精确已知情形的估计精度。因而，提出的自适应CKF 在复杂环境下具有相比于ASCKF 更优的性能。

图5 和图6 描绘了经典CKF、ASCKF 和提出的自适应CKF 算法分别在(300 s, 700 s)内得到的无人机位置和姿态的RMSE 均值的直观比较。图5 和图6 的统计结果同样表明，本文提出的自适应CKF 显著改善了经典CKF 的自适应性，从而减小了INS/GNSS 组合导航系统的导航误差，提高了无人机导航系统的自适应能力。

图5 无人机在(300s, 700s)内的位置RMSE 均值比较Fig.5 Comparison of mean RMSE of UAV position in (300s, 700s)

图6 无人机在(300s, 700s)内的姿态RMSE 均值比较Fig.6 Comparison of mean RMSE of UAV attitude in (300s, 700s)

5 结 论

本文提出了一种具有先验状态协方差反馈控制的自适应CKF 算法，以提高无人机INS/GNSS组合导航对过程噪声方差的自适应能力。该算法是一种利用系统后验状态和误差协方差信息，对先验状态协方差进行反馈调整控制的方法，有效地抑制过程噪声方差不确定性对滤波解的影响，并保证了调整过程中先验状态协方差的正定性，从而提高了CKF 用于无人机INS/GNSS 组合导航解算时的自适应能力。仿真实验结果表明：在过程噪声方差具有不确定性的情况下，提出方法的性能明显优于经典CKF 和ASCKF，显著改善了CKF 滤波器的自适应性，提高了无人机 INS/ GNSS 组合系统的导航精度。