基于魯棒濾波的無(wú)人機(jī)著陸相對(duì)導(dǎo)航方法

陳 誠(chéng)，王小剛，秦武韜，崔乃剛，許河川

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

基于魯棒濾波的無(wú)人機(jī)著陸相對(duì)導(dǎo)航方法

陳 誠(chéng)1，王小剛1，秦武韜1，崔乃剛1，許河川2

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

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

無(wú)人機(jī)；相對(duì)導(dǎo)航；視覺(jué)導(dǎo)航；非線性濾波方法；魯棒高階容積濾波

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

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

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

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

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

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

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

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

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

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

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

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

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

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

根據(jù)姿態(tài)運(yùn)動(dòng)學(xué)，無(wú)人機(jī)與移動(dòng)平臺(tái)之間的相對(duì)四元數(shù)滿足：

1.2.2 相對(duì)質(zhì)心運(yùn)動(dòng)方程

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

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

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

加速度的測(cè)量模型如下：

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

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

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

2 魯棒高階容積濾波算法

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

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

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

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

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

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

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

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

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

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

4 仿真分析

4.1 仿真初始條件

1）仿真場(chǎng)景

無(wú)人機(jī)從距離移動(dòng)平臺(tái)60 m處開(kāi)始進(jìn)行相對(duì)導(dǎo)航，引導(dǎo)無(wú)人機(jī)至移動(dòng)平臺(tái)正上方2 m處結(jié)束；移動(dòng)平臺(tái)以2 m/s的速度勻速直線運(yùn)動(dòng)。仿真場(chǎng)景如圖2所示。

圖2 仿真場(chǎng)景圖Fig.2 Simulation scene

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

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

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

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

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

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

表1 特征光點(diǎn)位置Tab.1 Location of beacons

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

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

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

圖4 相對(duì)速度估計(jì)精度對(duì)比Fig.4 Contrast on estimation accuracies of relative velocities

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

5 結(jié) 論

本文以無(wú)人機(jī)在移動(dòng)平臺(tái)上進(jìn)行自主起降時(shí)的相對(duì)導(dǎo)航問(wèn)題為研究背景，給出了無(wú)人機(jī)與移動(dòng)平臺(tái)間的相對(duì)運(yùn)動(dòng)方程及相對(duì)導(dǎo)航敏感器的測(cè)量方程，針對(duì)系統(tǒng)的量測(cè)噪聲不符合高斯分布的問(wèn)題，提出一種改進(jìn)的魯棒高階容積濾波算法，設(shè)計(jì)了相對(duì)導(dǎo)航濾波器，并進(jìn)行了數(shù)學(xué)仿真。仿真結(jié)果表明，該方法可以有效解決量測(cè)信息非高斯以及系統(tǒng)非線性的問(wèn)題，導(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] 耿明志. 無(wú)人機(jī)自動(dòng)著陸過(guò)程中的視覺(jué)導(dǎo)航技術(shù)研究[D]. 南京: 南京航空航天大學(xué), 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)高階容積卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用[J]. 航空學(xué)報(bào), 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ì)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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

國(guó)家自然科學(xué)基金（61304236）；微小型航天器技術(shù)國(guó)防重點(diǎn)學(xué)科實(shí)驗(yàn)室開(kāi)放基金（HIT.KLOF.MST.201606）

陳誠(chéng)（1986—），男，博士研究生，研究方向?yàn)闊o(wú)人機(jī)自主導(dǎo)航與控制。Email: hitsa_chencheng@163.com

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