基于鲁棒滤波的无人机着陆相对导航方法

陈 诚，王小刚，秦武韬，崔乃刚，许河川

（1. 哈尔滨工业大学 航天学院，哈尔滨 150001；2. 国营第624厂，哈尔滨 150030）

基于鲁棒滤波的无人机着陆相对导航方法

陈 诚1，王小刚1，秦武韬1，崔乃刚1，许河川2

（1. 哈尔滨工业大学 航天学院，哈尔滨 150001；2. 国营第624厂，哈尔滨 150030）

针对无人机在移动平台上进行起降时的相对导航问题，提出了一种基于鲁棒高阶容积滤波的惯导/视觉相对导航方法。建立了相对导航系统模型，基于无人机与移动平台之间的相对运动给出了系统的相对惯导方程，并针对系统中传感器的量测特性给出了导航敏感器的测量方程。针对相对导航系统非线性较强且量测噪声不符合高斯分布等问题，在高阶容积滤波的基础上，结合Huber-based量测更新方程，设计了鲁棒高阶容积滤波相对导航滤波器，该方法具有较高的估计精度，且对混合高斯噪声有鲁棒性。相对姿态采用四元数表示，为保证四元数的归一化，在设计相对导航滤波器时采用修正的罗德里格斯参数表示姿态误差。仿真结果表明，该方法可以准确地给出无人机与移动平台之间的相对位置、速度和姿态信息，且估计精度高于扩展卡尔曼滤波、Huber-Based滤波以及高阶容积卡尔曼滤波。

无人机；相对导航；视觉导航；非线性滤波方法；鲁棒高阶容积滤波

目前，无人机在军事、民用的各个领域都得到了广泛的关注，其中无人机在移动平台上的起降问题逐渐成为一个研究热点，典型的例子包括无人机自主着舰等。为保证无人机在移动平台上顺利自主起降，精确的相对导航信息是必要条件。目前，相对导航方法主要有三种：①载体间的GPS测量值直接相减[1]；②载体间的相对GPS信息校正相对惯导信息[2]；③载体间的相对视线矢量信息校正相对惯导信息[3]。

方法①和②无法摆脱对GPS的依赖，且缺少必要的相对姿态信息，在无人机自主起降问题上不适用；方法③利用视觉导航设备获取无人机与移动平台之间的相对视线信息，进而校正相对惯导信息，可以同时获取无人机与移动平台之间的相对位置、速度以及姿态信息，且自主性好，可靠性强，精度高[4]，在无人机移动平台自主起降上具有较好的应用前景。

非线性滤波方法是相对导航系统的关键技术之一[5]。传统扩展卡尔曼滤波基于一阶泰勒级数展开，在系统非线性较强时存在着近似偏差较大等问题[6]。同时，基于惯导/视觉的相对导航系统雅克比矩阵求解相对复杂。高阶容积卡尔曼滤波是近年来提出的一种新型非线性滤波算法，具有估计精度高、滤波稳定性好等特点，同时避免了求解雅克比矩阵等问题，在各个领域都得到了广泛应用[7-8]。但该算法是基于L2范数最小的估计，当系统的量测噪声不符合高斯分布时，算法性能会大幅下降甚至发散。由Huber提出的Huber-Based滤波是一种混合L1/L2范数的滤波方法[9]，该方法给出了指标函数的具体形式并通过可调参数确定L1/L2范数的混合情况，对于量测噪声为受污染的高斯白噪声时具有较强的鲁棒性。因此，本文将高阶容积卡尔曼滤波的量测更新方法转化为用Huber估计方法求解线性回归问题，进而提出了一种鲁棒高阶容积滤波算法，该算法兼备高阶容积卡尔曼滤波算法的精度高、稳定性好的优点，同时对量测噪声为混合高斯噪声具有一定的鲁棒性。

本文给出了一种无人机自主起降系统，建立了相对导航系统模型，包括相对姿态运动方程和相对质心运动方程，在高阶容积卡尔曼滤波的基础上，基于Huber-Based量测更新方法，提出了一种鲁棒高阶容积滤波算法，并进行了相对导航滤波器的设计，最后进行了数学仿真和分析。

1 无人机自主起降系统及建模

1.1 无人机自主起降系统组成

在移动平台上自主起降是无人机的一种重要应用场景，其典型例子是舰载无人机的自主起降。本系统采用车辆模拟移动平台，车辆上载有惯性导航装置，车体上布置若干特征光点。无人机上同样安装有惯性导航装置，并通过视觉导航相机对车辆上的特征光点进行观测，获取相对视线矢量信息。同时，车载惯性导航系统测量数据通过数据链传到无人机数据处理中心，进而完成惯性/视觉相对导航信息融合，其系统组成如图1所示。

图1 无人机自主起降系统示意图Fig.1 Schematic of UAV automatic landing system

1.2 相对惯导方程

相对惯导方程主要由相对姿态方程和相对质心运动方程两部分组成[10]，因此本节将分两部分对相对惯导方程进行描述。

1.2.1 相对姿态方程

采用四元数作为姿态表示的参数，姿态四元数由旋转轴n和旋转角θ表示为：

式中：q13为姿态四元数的矢量部分；q4为姿态四元数的标量部分。

式中：qb为无人机的姿态四元数；qp为移动平台的姿态四元数；符号⊗表示表示四元数乘法。其定义如下：

根据姿态运动学，无人机与移动平台之间的相对四元数满足：

1.2.2 相对质心运动方程

1.3 相对导航敏感器测量方程

1.3.1 惯性导航系统测量方程

陀螺仪和加速度计是惯性导航系统的测量元件。陀螺仪的误差主要由漂移误差以及刻度误差两部分组成，其模型如下所示：

加速度的测量模型如下：

式中：aB是载体的真实视加速度，是载体视加速度的测量值，ba是加速度计的偏置，Ka是刻度系数误差矩阵，ηav和ηau是零均值高斯白噪声。在本文所进行的研究中，认为bg和ba为常值。

1.3.2 VisNav视觉导航系统测量方程

VisNav视觉导航系统主要包括两部分，其一为视觉导航相机，其二为特征光点。当其在相对导航中进行应用时需要将这两部分分别安装在无人机和移动平台上，本文采用无人机上安装视觉导航相机而移动平台上安装特征光点的办法，因此，视觉导航相机的测量值为无人机与移动平台之间的相对视线矢量，其测量原理如图1所示。特征光点在移动平台上的位置为为移动平台与无人机之间的相对位置矢量，则单位视线矢量为

2 鲁棒高阶容积滤波算法

本文采用基于高阶容积滤波算法[11]中的时间更新方法对Huber-Based滤波算法[12-13]进行改进，提出了鲁棒高阶容积滤波方法。下面给出具体的滤波算法。

3 相对导航滤波器设计

采用四元数表示姿态时，由于其误差不属于加性误差，无法保证滤波过程中四元数的归一化。因此，本文采用一种无约束的三参数罗德里格参数代表姿态误差，具体的滤波器设计步骤如下：

罗德里格参数与误差四元数的对应关系为

由误差四元数和当前估计四元数相乘得到用于传播的新四元数为

总结应用高阶容积卡尔曼滤波进行相对导航的具体步骤为：

1）给出相对姿态、位置和速度、陀螺常值漂移及加表随机常值偏置的初始估计以及相应的协方差矩阵，其中，前3项对应于姿态误差。

4）相对位置、速度以及陀螺常值漂移和加表随机常值偏置按照相应的数学模型进行直接传播，并计算传播后的状态均值和协方差。

5）根据传播后的状态均值和协方差计算用于量测更新的Cubature点，并计算量测预测值。

6）计算协方差矩阵及增益矩阵，完成量测更新，其中四元数更新按下式进行更新：

4 仿真分析

4.1 仿真初始条件

1）仿真场景

无人机从距离移动平台60 m处开始进行相对导航，引导无人机至移动平台正上方2 m处结束；移动平台以2 m/s的速度匀速直线运动。仿真场景如图2所示。

图2 仿真场景图Fig.2 Simulation scene

2）惯导设备参数

仿真中，陀螺的常值漂移为0.1 (°)/h，随机游走为0.1 (°)/h1/2；加速度计的常值偏置为200 μg，噪声密度为50 μg/Hz1/2，陀螺仪刻度系数误差阵Kg=0.01I3×3，加速度计刻度系数误差阵Ka=0.005I3×3。

3）视觉导航系统参数

视觉导航系统量测噪声是一种受污染的高斯白噪声，式(63)给出了高斯白噪声受到污染后的概率密度表达式，在本文的仿真中认为污染分布也符合高斯分布，而且方差是原高斯分布的5倍，即

式中：为污染率，在仿真中取0.1；σ1和σ2分别为视觉导航系统测量噪声方差和污染噪声方差。

由可观测性分析可知，至少3个特征光点才能满足相对导航系统需求，本文中采用8个特征光点。与3个特征光点方案相比，该方案可以提高相对导航的精度。表1给出了特征光点在移动平台上的位置分布。

表1 特征光点位置Tab.1 Location of beacons

4.2 仿真结果及分析

图3～5给出了扩展卡尔曼滤波（EKF）、高阶容积卡尔曼滤波（HCKF）、Huber-Based滤波（HF）以及鲁棒高阶容积滤波（HHCF）四种方法估计相对位置、相对速度和相对姿态的估计精度，结果表明当系统噪声为受污染的高斯噪声时，鲁棒高阶容积滤波估计精度最高，其次为Huber-Based滤波，再次为高阶容积卡尔曼滤波，精度最低的为扩展卡尔曼滤波。因此，鲁棒高阶容积滤波在未显著提高计算量的情况下，提高了相对导航估计精度。

图3 相对位置估计精度对比Fig.3 Contrast on estimation accuracies of relative positions

图4 相对速度估计精度对比Fig.4 Contrast on estimation accuracies of relative velocities

图5 相对姿态估计精度对比Fig.5 Contrast on estimation accuracies of relative attitudes

5 结 论

本文以无人机在移动平台上进行自主起降时的相对导航问题为研究背景，给出了无人机与移动平台间的相对运动方程及相对导航敏感器的测量方程，针对系统的量测噪声不符合高斯分布的问题，提出一种改进的鲁棒高阶容积滤波算法，设计了相对导航滤波器，并进行了数学仿真。仿真结果表明，该方法可以有效解决量测信息非高斯以及系统非线性的问题，导航精度高于EKF、HF和HCKF。

（References）：

[1] Tancredi U, Renga A, Grassi M. Carrier-based differential GPS for autonomous relative navigation in LEO[C]//AIAA Guidance, Navigation, and Control Conference.2012: 1-12.

[2] Lee J Y, Kim H S, Choi K H, et al. Adaptive GPS/INS integration for relative navigation[J]. GPS Solutions,2016, 20(1): 63-75.

[3] Fourie D, Tweddle B, Ulrich S, et al. Vision-based relative navigation and control for autonomous spacecraft inspection of an unknown object[C]//AIAA Guidance,Navigation, and Control Conference. 2013: 1-17.

[4] 耿明志. 无人机自动着陆过程中的视觉导航技术研究[D]. 南京: 南京航空航天大学, 2007.Geng Ming-zhi. The application of visual navigation in auto-landing of unmanned aerial vehicle[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2007.

[5] Jia B, Xin M. Vision-based spacecraft relative navigation using sparse-grid quadrature filter[J]. IEEE Transactions on Control Systems Technology, 2013, 21(5): 1595-1606.

[6] Jia B, Xin M, Cheng Y. High-degree cubature Kalman filter[J]. Automatica, 2013, 49(2): 510-518.

[7] Zhao Y. Cubature + Extended hybrid Kalman filtering method and its application in PPP/IMU tightly coupled navigation systems[J]. IEEE Sensors Journal, 2015,15(12): 6973-6985.

[8] Shi Y, Che L, Ge Q, et al. Adaptive high-degree cubature Kalman filter with unknown noise statistics[J]. Journal of Information & Computational Science, 2014, 11(18):6703-6712.

[9] Huber P J. Robust estimation of a location parameter[J].Annals of Mathematical Statistics, 1964, 35(1): 73-101.

[10] Wang X G, Cui N G, Guo J F. INS/VisNav/GPS relative navigation system for UAV[J]. Aerospace Science &Technology, 2013, 28(1): 242-248.

[11] 崔乃刚, 张龙, 王小刚, 等. 自适应高阶容积卡尔曼滤波在目标跟踪中的应用[J]. 航空学报, 2015, 36(12):3885-3895.Cui Nai-gang, Zhang Long, Wang Xiao-gang, et al. Application of adaptive high-degree cubature Kalman filter in target tracking[J]. Acta Aeronautica et Astronautica Sinica,2015, 36(12): 3885-3895.

[12] 王小刚, 胡智勇, 于洋, 等. 基于鲁棒滤波的捷联导引头视线角速度估计方法[J]. 中国惯性技术学报, 2016,24(2): 251-256.Wang Xiao-gang, Hu Zhi-yong, Yu Yang, et al. Line-ofsight angular-rate estimation of strapdown seeker based on robust filter[J]. Journal of Chinese Inertial Technology,2016, 24(2): 251-256.

[13] Wang X G, Qin W T, Bai Y L, et al. Trajectory estimation for ballistic missile in boost stage using robust filtering[J].IET Radar, Sonar & Navigation, 2017, 11(3): 513-519.

Unmanned aerial vehicle landing navigation algorithm based on robust filter

CHEN Cheng1, WANG Xiao-gang1, QIN Wu-tao1, CUI Nai-gang1, XU He-chuan2
(1. Department of Astronautics, Harbin Institute of Technology, Harbin 150001, China;2. State-owned No.624 Factory, Harbin 150030, China)

To cope with the problems of the relative navigation when unmanned aerial vehicle (UAV) is automatically landing on mobile platform, an INS/VisNav relative navigation method is proposed based on the robust high-degree cubature filter. The relative navigation system model is established, and the relative inertial navigation equations are derived based on the relative moving between the UAV and the mobile platform. In addition, the measurement equations of navigation sensors are given according to the measuring characteristics. In order to deal with the strong-nonlinearity and non-Gaussian measurement noise of the relative navigation system, a robust high-degree cubature filter is designed based on the combination of high-degree cubature filter and Huber-based measurement update equations. The novel filter has higher estimated accuracy and is robust to the mixed Gaussian noise. The relative attitude is denoted by quaternions,and in order to ensure the normalization of the quaternions, the modified Rodrigues parameters are utilized to denote the attitude errors in designing the relative navigation filter. Simulation results indicate that the proposed method could accurately provide the relative position, velocity and attitude information between the UAV and the mobile platform, and the estimation accuracies are higher than those of the extended Kalman filter, Huber-based filter and high-degree cubature Kalman filter.

unmanned aerial vehicle; relative navigation; VisNav navigation; nonlinear filter; high-degree cubature Huber-based filter

V249.32

：A

1005-6734(2017)03-0415-06

10.13695/j.cnki.12-1222/o3.2017.03.024

2017-02-12；

：2017-05-12

国家自然科学基金（61304236）；微小型航天器技术国防重点学科实验室开放基金（HIT.KLOF.MST.201606）

陈诚（1986—），男，博士研究生，研究方向为无人机自主导航与控制。Email: hitsa_chencheng@163.com

联 系 人：王小刚（1980—），男，副教授，研究方向为非线性滤波理论及应用。E-mail: wangxiaogang@hit.edu.cn