基于自适应容积信息滤波的无人机相对导航方法

苏炳志，何权荣，曹 晞，裴文龙，张少华

（1.中国直升机设计研究所，天津 300300；2.94657部队，九江 332000；3.陆军装备部航空军代局驻天津地区航空军代室，天津 300384）

无人机采用编队方式可以协同完成多样、复杂的任务，在搜索救援、合作目标跟踪、空中表演等领域得到广泛应用[1]。准确可靠的相对位置、速度和姿态信息是安全地保持无人机队形和避免编队内碰撞的前提，因此无人机间的相对导航对于编队飞行是至关重要的。最常见的相对导航方法是在每架无人机上配备全球卫星导航系统（Global Navigation Satellite System,GNSS）和惯性导航系统（Inertial Navigation System,INS）传感器，使用相对GNSS测量来修正相对INS[2,3]。尽管基于GNSS/INS的相对导航方法在美国F-18飞机编队飞行项目中得到应用[4]，但是该方法在卫星导航信号受到干扰或丢失情况下性能急剧下降。因此，学者们发展了一种基于视觉传感器和信标点的视觉辅助惯性导航以提高相对导航系统的自主性和减少对外界的依赖性[5]。

INS/Vision相对导航精度与用于状态估计的非线性滤波算法息息相关。在过去很长一段时间，扩展卡尔曼滤波（Extended Kalman Filtering,EKF）一直是相对导航状态估计的重要方法[6]。但是EKF只适用于非线性方程能用一阶泰勒级数进行近似的弱非线性情况，并且其在状态递推估计过程需要求解非线性系统的雅克比矩阵[7]。为了克服EKF的限制，学者们采用高斯加权积分法解决系统非线性问题，提出了一系列确定性采样非线性高斯滤波。Julier和Uhlmann采用无迹变换生成一系列Sigma点来捕获高斯分布的均值和方差，提出了具有三阶泰勒级数逼近精度的无迹卡尔曼滤波（Unscented Kalman Filtering,UKF）[8]。Arasaratnam首次以公式形式详尽地推导了一种基于三阶球面-径向容积准则的确定性采样非线性高斯滤波——容积卡尔曼滤波（Cubature Kalman Filtering,CKF）[9]。与UKF相比，CKF各容积点的权重相同并且全为正数，因此其计算形式更简单、数值稳定性更好。

飞行过程中由于无人机间相对姿态和位置变化将导致部分信标点被机体/机翼遮挡，此时可见信标点数发生变化，上述EKF等卡尔曼类滤波难以处理这种情况下的状态估计问题；将每个信标点当成单独的信息源，从多传感器信息融合的角度出发对多个信标点提供的无人机间的相对视线矢量信息进行融合，王小刚采用扩展信息滤波（Extended Information Filtering,EIF）实现了INS/Vision相对导航状态估计[10]。文献[11]和[12]分别将UKF和CKF与信息滤波相结合发展了无迹信息滤波（Unscented Information Filtering,UIF）和容积信息滤波（Cubature Information Filtering,CIF）。

无论是卡尔曼滤波还是信息滤波，均需要知道量测噪声先验统计特性来设置滤波器的量测噪声协方差，并且在整个编队飞行过程中量测噪声统计特性不变，系统才能获得最优状态估计。当视觉导航系统处于实际运行环境中，由于各种不确定因素的影响，噪声的统计特性变得复杂，并具有时变特性[13]。为了解决上述问题，Sarkka和Nummenmaa将变分贝叶斯（Variational Bayesian,VB）理论和卡尔曼滤波相结合提出了变分贝叶斯自适应卡尔曼滤波[14]，在线估计和修正滤波器的量测噪声协方差矩阵，降低了状态估计误差，提高了系统精度；之后将变分贝叶斯理论推广到非线性系统中提出了变分贝叶斯自适应CKF[15]。

为了解决基于视觉的无人机相对导航系统可见信标点数随时间变化和视觉量测噪声统计特性未知且时变问题，本文在前人研究成果的基础上对CIF进行改进，在CIF中融入变分贝叶斯理论，将量测噪声协方差建模为逆威舍特分布，提出了基于逆威舍特分布的变分贝叶斯自适应容积信息滤波（Variational Bayesian Adaptive Cubature Information Filtering,VBACIF）。建立基于视觉辅助的相对导航系统模型，设计基于VBACIF的无人机编队相对导航滤波器，参考多传感器信息融合原理将每个信标点当作一个单独信息源，采用变分贝叶斯理论和三阶球面-径向容积准则对每个信标点的相对视线量测噪声协方差阵和无人机间的相对导航状态进行联合估计。最后，通过蒙特卡洛打靶仿真对比分析验证了本文算法的有效性和优越性。

1 相对导航系统模型

对于长机-僚机编队模式，僚机需要在编队机动前后以长机为参考进行相对位置保持。用于提供相对信息的相对导航系统对于编队飞行控制是至关重要的。在僚机本体系下，以长机和僚机之间的相对运动关系建立双机间相对导航运动学模型；基于光学传感器和一系列信标点的视觉导航系统辅助估计相对状态。

1.1 相对导航运动学模型

长机和僚机之间的相对导航运动学模型由相对质心运动方程和相对姿态运动方程共同构成。

1.1.1 相对质心运动方程

1.1.2 相对姿态运动方程

用于表示姿态的四元数由欧拉旋转轴e和旋转角θ表示为：

1.2 惯性和视觉传感器测量模型

惯性导航系统的加速度计和陀螺仪测量模型分别为：

图1 视觉传感器量测Fig.1 Measurement of visual sensor

2 变分贝叶斯自适应容积信息滤波

考虑如下所示的离散系统：

2.1 容积信息滤波

2.1.1 预测

2.1.2 更新

2.2 变分贝叶斯自适应容积信息滤波

在变分贝叶斯容积信息滤波中，将系统状态和量测噪声协方差矩阵当作随机变量，采用变分变叶斯近似理论和容积准则进行联合估计；VBACIF通过在线跟踪量测噪声统计特性并自适应调节滤波器的量测噪声协方差阵以适应系统中量测噪声未知或时变等情况。

2.2.1 预测

系统状态kx和量测噪声协方差是相互独立的，根据先验知识，认为它们分别服从高斯分布和逆威舍特分布[15]。因此1k-时刻系统状态和第i个信息源的量测噪声协方差可表示成高斯-逆威舍特分布形式：

2.2.2 更新

联合先验分布和预测分布均是高斯-逆威舍特分布形式，那么联合后验分布的函数形式将保持不变（共轭性），即：

3 基于VBACIF的相对导航滤波器

本节设计基于VBACIF的相对导航滤波器。在滤波器设计中，直接使用具有模为1约束的姿态四元数作为状态参数进行滤波，将导致状态协方差阵奇异；因此，采用无约束三维矢量——罗德里格斯误差矢量来表示姿态四元数误差，罗德里格斯误差矢量δ s与姿态四元数偏差δq有如下的关系：

如图2所示将每个信标点当作独立的信息源，采用VBACIF算法融合视觉量测估计得到状态如式(60)所示：

图2 视觉量测信息融合架构Fig.2 The fusion architecture of visual measurement information

相对位置和速度可以直接从式经量测修正后的状态向量中获得，相对姿态更新按下式计算：

4 仿真分析

为了验证VBACIF算法的有效性和优越性，开展数值仿真验证，将VBACIF与EIF和CIF进行比较。

4.1 仿真初始条件

加速度计初始漂移、随机游走和噪声分别为0.2mg、0.002mg/s1/2和0.02mg·s1/2，陀螺仪初始漂移、随机游走和噪声分别为0.1°/h、0.06°/h3/2和0.01°/h1/2。信标点在僚机上的布局如表1所示，第7个信标点在0≤t≤10s和90s≤t≤100s时间段内被机体/机翼遮挡不可见，其他信标点在仿真时间段内均可见。参照文献[3]的飞行轨迹开展编队飞行相对导航仿真。

表1 信标位置列表Tab.1 List of beacon locations

视觉量测噪声服从高斯分布N(vi,k;0,Ri,vis)，并且量测噪声是时变的，即方差Ri,vis变化如式(64)所示：

4.2 仿真结果与分析

将以三维相对位置、速度和姿态误差来描述相对导航滤波器的估计精度，其中相对位置和速度误差可直接由相对导航滤波器估计的值与标称值作差值得到，而三维相对姿态误差δϑ、δφ和δγ计算如下：

图3 VBACIF的相对位置估计误差Fig.3 Relative position estimated error using the VBACIF

图4 VBACIF的相对速度估计误差Fig.4 Relative velocity estimated error using the VBACIF

图5 VBACIF的相对姿态估计误差Fig.5 Relative attitude estimated error using the VBACIF

图7 VBACIF的R7,k(1,1)估计曲线Fig.7 Estimation curve of R7,k(1,1)using the VBACIF

如图6-7所示，VBACIF能够精确估计和快速跟踪视觉量测噪声统计特性的变化；此外，在VBACIF的算法中，不同信标点的噪声统计特性估计是相互独立，这使得部分信标点不可见的情况下其他信标点（表1中第1~6个信标点）噪声统计特性估计能正常进行，而第7个信标点的量测噪声协方差能够在可见时间段10s~90s内进行估计。

图6 VBACIF的R1,k(1,1)估计曲线Fig.6 Estimation curve of R1,k(1,1)using the VBACIF

图8-10是基于EIF、CIF和VBACIF的相对导航100次蒙特卡洛打靶仿真三维相对位置、速度和姿态均方根误差曲线的比较结果，表2是相对位置、速度和姿态平均均方根误差的汇总。打靶仿真结果表明，EIF的估计精度最差并且估计曲线初始阶段存在振荡现象，这是由于基于一阶泰勒展开的EIF具有较大截断误差并且缺乏协方差自适应估计及调节能力；VBACIF的估计精度比CIF高，且估计稳定性不受量测噪声统计特性变化的影响，这是由于VBACIF在进行状态估计的同时对量测噪声统计特性进行了估计，并用其在线自适应调节滤波器的量测噪声协方差矩阵。VBACIF算法的相对位置、速度和姿态精度较CIF算法分别提高了24.85%、9.41%和45.52%。

图8 三种滤波算法的相对位置估计精度比较Fig.8 Comparison of relative position estimated accuracy for three filtering algorithms

图9 三种滤波算法的相对速度估计精度比较Fig.9 Comparison of relative velocity estimated accuracy for three filtering algorithms

图10 三种滤波算法的相对姿态估计精度比较Fig.10 Comparison of relative attitude estimated accuracy for three filtering algorithms

表2 三种滤波算法的相对导航精度对比Tab.2 Comparison of relative navigation accuracy of three algorithms

5 结论

本文针对无人机编队基于视觉辅助的相对导航系统信标点数时变和视觉量测噪声协方差不确定问题，在信息滤波框架下提出了一种包含量测噪声自适应估计的相对导航方法，主要工作总结如下：

1)单次仿真结果表明，在量测噪声统计特性先验信息不足且时变和可见信标点数变化的情况下，基于VBACIF的视觉辅助相对导航滤波器能够对各信标点相对视线矢量量测噪声协方差进行估计并实时跟踪其变化情况，同时能够精确地给出无人机之间的相对位置、速度和姿态信息，具有良好的自适应能力。

2)不同滤波算法蒙特卡洛打靶仿真的位置、速度和姿态估计均方根误差比较结果表明，基于VBACIF的相对导航精度高于基于EIF和CIF的，验证了本文滤波算法VBACIF的有效性和优越性。

3)基于VBACIF算法提出的包含量测噪声协方差自适应估计的相对导航方法在有人/无人机空中加油、航天器交会对接和卫星编队等航空航天多飞行器系统中同样具有潜在应用前景。