陳 誠，王小剛，秦武韜，崔乃剛，許河川

（1. 哈爾濱工業(yè)大學 航天學院，哈爾濱 150001；2. 國營第624廠，哈爾濱 150030）

基于魯棒濾波的無人機著陸相對導(dǎo)航方法

陳 誠1，王小剛1，秦武韜1，崔乃剛1，許河川2

（1. 哈爾濱工業(yè)大學 航天學院，哈爾濱 150001；2. 國營第624廠，哈爾濱 150030）

針對無人機在移動平臺上進行起降時的相對導(dǎo)航問題，提出了一種基于魯棒高階容積濾波的慣導(dǎo)/視覺相對導(dǎo)航方法。建立了相對導(dǎo)航系統(tǒng)模型，基于無人機與移動平臺之間的相對運動給出了系統(tǒng)的相對慣導(dǎo)方程，并針對系統(tǒng)中傳感器的量測特性給出了導(dǎo)航敏感器的測量方程。針對相對導(dǎo)航系統(tǒng)非線性較強且量測噪聲不符合高斯分布等問題，在高階容積濾波的基礎(chǔ)上，結(jié)合Huber-based量測更新方程，設(shè)計了魯棒高階容積濾波相對導(dǎo)航濾波器，該方法具有較高的估計精度，且對混合高斯噪聲有魯棒性。相對姿態(tài)采用四元數(shù)表示，為保證四元數(shù)的歸一化，在設(shè)計相對導(dǎo)航濾波器時采用修正的羅德里格斯參數(shù)表示姿態(tài)誤差。仿真結(jié)果表明，該方法可以準確地給出無人機與移動平臺之間的相對位置、速度和姿態(tài)信息，且估計精度高于擴展卡爾曼濾波、Huber-Based濾波以及高階容積卡爾曼濾波。

無人機；相對導(dǎo)航；視覺導(dǎo)航；非線性濾波方法；魯棒高階容積濾波

目前，無人機在軍事、民用的各個領(lǐng)域都得到了廣泛的關(guān)注，其中無人機在移動平臺上的起降問題逐漸成為一個研究熱點，典型的例子包括無人機自主著艦等。為保證無人機在移動平臺上順利自主起降，精確的相對導(dǎo)航信息是必要條件。目前，相對導(dǎo)航方法主要有三種：①載體間的GPS測量值直接相減[1]；②載體間的相對GPS信息校正相對慣導(dǎo)信息[2]；③載體間的相對視線矢量信息校正相對慣導(dǎo)信息[3]。

方法①和②無法擺脫對GPS的依賴，且缺少必要的相對姿態(tài)信息，在無人機自主起降問題上不適用；方法③利用視覺導(dǎo)航設(shè)備獲取無人機與移動平臺之間的相對視線信息，進而校正相對慣導(dǎo)信息，可以同時獲取無人機與移動平臺之間的相對位置、速度以及姿態(tài)信息，且自主性好，可靠性強，精度高[4]，在無人機移動平臺自主起降上具有較好的應(yīng)用前景。

非線性濾波方法是相對導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù)之一[5]。傳統(tǒng)擴展卡爾曼濾波基于一階泰勒級數(shù)展開，在系統(tǒng)非線性較強時存在著近似偏差較大等問題[6]。同時，基于慣導(dǎo)/視覺的相對導(dǎo)航系統(tǒng)雅克比矩陣求解相對復(fù)雜。高階容積卡爾曼濾波是近年來提出的一種新型非線性濾波算法，具有估計精度高、濾波穩(wěn)定性好等特點，同時避免了求解雅克比矩陣等問題，在各個領(lǐng)域都得到了廣泛應(yīng)用[7-8]。但該算法是基于L2范數(shù)最小的估計，當系統(tǒng)的量測噪聲不符合高斯分布時，算法性能會大幅下降甚至發(fā)散。由Huber提出的Huber-Based濾波是一種混合L1/L2范數(shù)的濾波方法[9]，該方法給出了指標函數(shù)的具體形式并通過可調(diào)參數(shù)確定L1/L2范數(shù)的混合情況，對于量測噪聲為受污染的高斯白噪聲時具有較強的魯棒性。因此，本文將高階容積卡爾曼濾波的量測更新方法轉(zhuǎn)化為用Huber估計方法求解線性回歸問題，進而提出了一種魯棒高階容積濾波算法，該算法兼?zhèn)涓唠A容積卡爾曼濾波算法的精度高、穩(wěn)定性好的優(yōu)點，同時對量測噪聲為混合高斯噪聲具有一定的魯棒性。

本文給出了一種無人機自主起降系統(tǒng)，建立了相對導(dǎo)航系統(tǒng)模型，包括相對姿態(tài)運動方程和相對質(zhì)心運動方程，在高階容積卡爾曼濾波的基礎(chǔ)上，基于Huber-Based量測更新方法，提出了一種魯棒高階容積濾波算法，并進行了相對導(dǎo)航濾波器的設(shè)計，最后進行了數(shù)學仿真和分析。

1 無人機自主起降系統(tǒng)及建模

1.1 無人機自主起降系統(tǒng)組成

在移動平臺上自主起降是無人機的一種重要應(yīng)用場景，其典型例子是艦載無人機的自主起降。本系統(tǒng)采用車輛模擬移動平臺，車輛上載有慣性導(dǎo)航裝置，車體上布置若干特征光點。無人機上同樣安裝有慣性導(dǎo)航裝置，并通過視覺導(dǎo)航相機對車輛上的特征光點進行觀測，獲取相對視線矢量信息。同時，車載慣性導(dǎo)航系統(tǒng)測量數(shù)據(jù)通過數(shù)據(jù)鏈傳到無人機數(shù)據(jù)處理中心，進而完成慣性/視覺相對導(dǎo)航信息融合，其系統(tǒng)組成如圖1所示。

圖1 無人機自主起降系統(tǒng)示意圖Fig.1 Schematic of UAV automatic landing system

1.2 相對慣導(dǎo)方程

相對慣導(dǎo)方程主要由相對姿態(tài)方程和相對質(zhì)心運動方程兩部分組成[10]，因此本節(jié)將分兩部分對相對慣導(dǎo)方程進行描述。

1.2.1 相對姿態(tài)方程

采用四元數(shù)作為姿態(tài)表示的參數(shù)，姿態(tài)四元數(shù)由旋轉(zhuǎn)軸n和旋轉(zhuǎn)角θ表示為：

式中：q13為姿態(tài)四元數(shù)的矢量部分；q4為姿態(tài)四元數(shù)的標量部分。

式中：qb為無人機的姿態(tài)四元數(shù)；qp為移動平臺的姿態(tài)四元數(shù)；符號?表示表示四元數(shù)乘法。其定義如下：

根據(jù)姿態(tài)運動學，無人機與移動平臺之間的相對四元數(shù)滿足：

1.2.2 相對質(zhì)心運動方程

1.3 相對導(dǎo)航敏感器測量方程

1.3.1 慣性導(dǎo)航系統(tǒng)測量方程

陀螺儀和加速度計是慣性導(dǎo)航系統(tǒng)的測量元件。陀螺儀的誤差主要由漂移誤差以及刻度誤差兩部分組成，其模型如下所示：

加速度的測量模型如下：

式中：aB是載體的真實視加速度，是載體視加速度的測量值，ba是加速度計的偏置，Ka是刻度系數(shù)誤差矩陣，ηav和ηau是零均值高斯白噪聲。在本文所進行的研究中，認為bg和ba為常值。

1.3.2 VisNav視覺導(dǎo)航系統(tǒng)測量方程

VisNav視覺導(dǎo)航系統(tǒng)主要包括兩部分，其一為視覺導(dǎo)航相機，其二為特征光點。當其在相對導(dǎo)航中進行應(yīng)用時需要將這兩部分分別安裝在無人機和移動平臺上，本文采用無人機上安裝視覺導(dǎo)航相機而移動平臺上安裝特征光點的辦法，因此，視覺導(dǎo)航相機的測量值為無人機與移動平臺之間的相對視線矢量，其測量原理如圖1所示。特征光點在移動平臺上的位置為為移動平臺與無人機之間的相對位置矢量，則單位視線矢量為

2 魯棒高階容積濾波算法

本文采用基于高階容積濾波算法[11]中的時間更新方法對Huber-Based濾波算法[12-13]進行改進，提出了魯棒高階容積濾波方法。下面給出具體的濾波算法。

3 相對導(dǎo)航濾波器設(shè)計

采用四元數(shù)表示姿態(tài)時，由于其誤差不屬于加性誤差，無法保證濾波過程中四元數(shù)的歸一化。因此，本文采用一種無約束的三參數(shù)羅德里格參數(shù)代表姿態(tài)誤差，具體的濾波器設(shè)計步驟如下：

羅德里格參數(shù)與誤差四元數(shù)的對應(yīng)關(guān)系為

由誤差四元數(shù)和當前估計四元數(shù)相乘得到用于傳播的新四元數(shù)為

總結(jié)應(yīng)用高階容積卡爾曼濾波進行相對導(dǎo)航的具體步驟為：

1）給出相對姿態(tài)、位置和速度、陀螺常值漂移及加表隨機常值偏置的初始估計以及相應(yīng)的協(xié)方差矩陣，其中，前3項對應(yīng)于姿態(tài)誤差。

4）相對位置、速度以及陀螺常值漂移和加表隨機常值偏置按照相應(yīng)的數(shù)學模型進行直接傳播，并計算傳播后的狀態(tài)均值和協(xié)方差。

5）根據(jù)傳播后的狀態(tài)均值和協(xié)方差計算用于量測更新的Cubature點，并計算量測預(yù)測值。

6）計算協(xié)方差矩陣及增益矩陣，完成量測更新，其中四元數(shù)更新按下式進行更新：

4 仿真分析

4.1 仿真初始條件

1）仿真場景

無人機從距離移動平臺60 m處開始進行相對導(dǎo)航，引導(dǎo)無人機至移動平臺正上方2 m處結(jié)束；移動平臺以2 m/s的速度勻速直線運動。仿真場景如圖2所示。

圖2 仿真場景圖Fig.2 Simulation scene

2）慣導(dǎo)設(shè)備參數(shù)

仿真中，陀螺的常值漂移為0.1 (°)/h，隨機游走為0.1 (°)/h1/2；加速度計的常值偏置為200 μg，噪聲密度為50 μg/Hz1/2，陀螺儀刻度系數(shù)誤差陣Kg=0.01I3×3，加速度計刻度系數(shù)誤差陣Ka=0.005I3×3。

3）視覺導(dǎo)航系統(tǒng)參數(shù)

視覺導(dǎo)航系統(tǒng)量測噪聲是一種受污染的高斯白噪聲，式(63)給出了高斯白噪聲受到污染后的概率密度表達式，在本文的仿真中認為污染分布也符合高斯分布，而且方差是原高斯分布的5倍，即

式中：為污染率，在仿真中取0.1；σ1和σ2分別為視覺導(dǎo)航系統(tǒng)測量噪聲方差和污染噪聲方差。

由可觀測性分析可知，至少3個特征光點才能滿足相對導(dǎo)航系統(tǒng)需求，本文中采用8個特征光點。與3個特征光點方案相比，該方案可以提高相對導(dǎo)航的精度。表1給出了特征光點在移動平臺上的位置分布。

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

4.2 仿真結(jié)果及分析

圖3～5給出了擴展卡爾曼濾波（EKF）、高階容積卡爾曼濾波（HCKF）、Huber-Based濾波（HF）以及魯棒高階容積濾波（HHCF）四種方法估計相對位置、相對速度和相對姿態(tài)的估計精度，結(jié)果表明當系統(tǒng)噪聲為受污染的高斯噪聲時，魯棒高階容積濾波估計精度最高，其次為Huber-Based濾波，再次為高階容積卡爾曼濾波，精度最低的為擴展卡爾曼濾波。因此，魯棒高階容積濾波在未顯著提高計算量的情況下，提高了相對導(dǎo)航估計精度。

圖3 相對位置估計精度對比Fig.3 Contrast on estimation accuracies of relative positions

圖4 相對速度估計精度對比Fig.4 Contrast on estimation accuracies of relative velocities

圖5 相對姿態(tài)估計精度對比Fig.5 Contrast on estimation accuracies of relative attitudes

5 結(jié) 論

本文以無人機在移動平臺上進行自主起降時的相對導(dǎo)航問題為研究背景，給出了無人機與移動平臺間的相對運動方程及相對導(dǎo)航敏感器的測量方程，針對系統(tǒng)的量測噪聲不符合高斯分布的問題，提出一種改進的魯棒高階容積濾波算法，設(shè)計了相對導(dǎo)航濾波器，并進行了數(shù)學仿真。仿真結(jié)果表明，該方法可以有效解決量測信息非高斯以及系統(tǒng)非線性的問題，導(dǎo)航精度高于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ǎo)航技術(shù)研究[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] 崔乃剛, 張龍, 王小剛, 等. 自適應(yīng)高階容積卡爾曼濾波在目標跟蹤中的應(yīng)用[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] 王小剛, 胡智勇, 于洋, 等. 基于魯棒濾波的捷聯(lián)導(dǎo)引頭視線角速度估計方法[J]. 中國慣性技術(shù)學報, 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）；微小型航天器技術(shù)國防重點學科實驗室開放基金（HIT.KLOF.MST.201606）

陳誠（1986—），男，博士研究生，研究方向為無人機自主導(dǎo)航與控制。Email: hitsa_chencheng@163.com

聯(lián) 系 人：王小剛（1980—），男，副教授，研究方向為非線性濾波理論及應(yīng)用。E-mail: wangxiaogang@hit.edu.cn