INS/GNSS/Vision組合近距空中加油相對導航算法

孫永榮，劉梓軒，曾慶化，趙科東，張 怡

（南京航空航天大學導航研究中心，南京 210016）

近年來，隨著無人機編隊技術(shù)受到廣泛的關注，相對導航技術(shù)逐漸成為研究熱點。自主空中加油技術(shù)成為了延長無人機航程與續(xù)航時間的有效手段，是空中力量的倍增器[1]。為保證受油機順利接近加油機并進行對接加油，相對導航精度的重要性毋庸置疑。

目前，常見的相對導航方法主要是利用移動基站型差分GNSS信息校正相對慣導信息，通過INS/GNSS組合進行優(yōu)勢互補以得到高精度的相對導航信息[2,3]。然而在近距對接場景下，GNSS信號易被機體遮擋而出現(xiàn)受干擾甚至失效的情況[4]，導致相對導航精度顯著下降，同時GNSS信號缺少必要的相對姿態(tài)信息[5]，因此INS/GNSS組合的相對導航系統(tǒng)無法完全滿足空中加油近距對接的導航需求。針對這一問題，考慮到近距條件下相對視線定位傳感器具有更高的導航精度與更強的抗干擾能力[6]，利用Vision導航系統(tǒng)來獲得受油機與加油機之間的相對視線信息，并與GNSS信號共同輔助校正INS誤差，融合各傳感器的測量信息進行相對導航估計，從而得到可靠性強、精度高的相對導航信息。

融合算法對組合導航的估計精度、可靠性與容錯性有著至關重要的影響[7]。其中，聯(lián)邦濾波方法具有算法靈活、計算量小、容錯性能好等優(yōu)點，在組合導航系統(tǒng)中得到了廣泛應用[8]。針對重置式聯(lián)邦濾波子系統(tǒng)故障對全局造成污染的問題，有學者開展了諸多自適應信息分配方法的研究[9,11]。然而，這些方法對于導航系統(tǒng)軟故障的檢測與隔離并沒有表現(xiàn)出顯著的優(yōu)勢，并且當無故障子系統(tǒng)具有較大的分配系數(shù)時，其對自身量測信息的利用程度也隨之減小，從而對全局的融合估計精度造成影響[12,13]。

為了進一步提高組合導航系統(tǒng)的容錯性能與估計精度，本文在常規(guī)重置式聯(lián)邦濾波器的基礎上，提出了一種新的容錯濾波結(jié)構(gòu)，并應用于INS/GNSS/Vision組合相對導航系統(tǒng)。該結(jié)構(gòu)通過滑動窗口自適應調(diào)節(jié)子濾波器的量測噪聲，并根據(jù)協(xié)方差奇異值分塊重置信息分配系數(shù)，在提高故障子濾波器估計精度的同時，一定程度上隔離了各狀態(tài)量之間的故障污染，提高了全局估計精度。

2 相對導航運動學模型

雙機之間的相對導航運動學模型由相對質(zhì)心運動方程與相對姿態(tài)方程組成，近距空中加油相對導航示意圖如圖1所示。

圖1 空中加油相對導航示意圖Fig.1 Schematic diagram of air refueling relative navigation

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

定義受油機r相對于加油機t的位置在受油機r機體系下的投影為相對位置矢量表示為：

式中：為慣性坐標系到受油機r機體坐標系的方向余弦矩陣。

對式(1)求二階導數(shù)，可得：

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

采用四元數(shù)形式表示姿態(tài)，姿態(tài)四元數(shù)由旋轉(zhuǎn)軸n與旋轉(zhuǎn)角θ表示：

式中：q0為四元數(shù)中的標量部分，q13為四元數(shù)中的矢量部分。

定義受油機r相對于加油機t的姿態(tài)四元數(shù)表示為：

式中：qr為受油機r的姿態(tài)四元數(shù)，qt為受油機t的姿態(tài)四元數(shù)，?為四元數(shù)乘法。

對式(5)求一階導數(shù)，滿足如下關系式：

3 容錯聯(lián)邦濾波器設計

3.1 INS/GNSS/Vision組合聯(lián)邦濾波器

聯(lián)邦濾波是一種并行兩級結(jié)構(gòu)的分散式濾波方法，根據(jù)本文INS/GNSS/Vision組合導航系統(tǒng)設計的聯(lián)邦濾波器結(jié)構(gòu)如圖2所示，其中主濾波器僅進行最優(yōu)信息融合。INS導航系統(tǒng)作為整個濾波系統(tǒng)的公共參考系統(tǒng)，分別與GNSS、Vision導航系統(tǒng)構(gòu)成子濾波系統(tǒng)；通過主濾波器將各個子濾波器的狀態(tài)估計信息進行融合，得到全局最優(yōu)估計；最后，按照根據(jù)設計的信息分配準則將主濾波器的全局融合信息分配給各個子濾波系統(tǒng)，同時反饋修正INS公共參考系統(tǒng)的系統(tǒng)偏差。

圖2 聯(lián)邦濾波器結(jié)構(gòu)圖Fig.2 Federated filter structure diagram

設主濾波器與子濾波器具有相同的誤差狀態(tài)變量，對于狀態(tài)誤差，基于GNSS與Vision導航系統(tǒng)的誤差量測方程均為非線性模型，可得系統(tǒng)狀態(tài)方程與第個量測方程為：

式中：Fk,k1-為狀態(tài)轉(zhuǎn)移矩陣；Gk,k1-為系統(tǒng)矩陣；Wk-1的協(xié)方差為Q；的協(xié)方差為Ri。

本文聯(lián)邦濾波主要包括信息分配、時間更新、量測更新與信息融合四個部分，子濾波器采用擴展卡爾曼濾波方法進行非線性濾波估計：

(1)信息分配

(2)子濾波器時間更新

(3)子濾波器量測更新

(4)主濾波器信息融合

3.2 基于滑動窗口的時變量測噪聲估計

針對重置模式下聯(lián)邦濾波器子系統(tǒng)故障時對導航系統(tǒng)的污染問題，利用滑動窗口進行時變量測噪聲估計，取代傳統(tǒng)的故障隔離方法。該方法通過對比實際新息殘差協(xié)方差與理論殘差協(xié)方差構(gòu)造滑動窗口，利用一段窗口內(nèi)的殘差序列估計值判斷觀測量的故障程度，進而可自適應地調(diào)整各子濾波器的量測噪聲。

對于k時刻的子濾波器i，殘差向量為：

可得實際殘差序列協(xié)方差矩陣為：

同時，由式(12)可得理論殘差序列協(xié)方差矩陣為：

根據(jù)式(18)(19)可得k時刻子濾波器i量測噪聲統(tǒng)計值為：

利用一段窗口內(nèi)的量測噪聲統(tǒng)計特性的變化，可以得到時刻子濾波器的時變量測噪聲估計值為：

式中：R0為設定的初始量測噪聲協(xié)方差陣；M為滑動估計窗口長度。

通過監(jiān)測新息殘差序列，在量測傳感器出現(xiàn)故障時，可以有效地調(diào)節(jié)滑動窗口估計下的量測噪聲協(xié)方差陣。由式(12)可知，隨著時變量測的增大，降低了增益矩陣中故障量測值的信任程度，以提高故障子濾波器的估計精度。同時，通過平衡與無故障子濾波器之間的信息分配系數(shù)，保持了無故障傳感器的量測信息利用率。

3.3 基于協(xié)方差奇異值的信息分配系數(shù)分塊重置

在聯(lián)邦濾波結(jié)構(gòu)中，主濾波器通過信息分配系數(shù)對各個子濾波器進行導航狀態(tài)反饋是保證子濾波器精度和魯棒性以及全局估計精度的關鍵。為了提高全局最優(yōu)估計融合精度，需要分配給狀態(tài)誤差較大的子濾波器以較低的信息分配系數(shù)，通過減小該子濾波器在融合中的輸出占比，從而提高全局最優(yōu)估計精度，一般通過子濾波器輸出協(xié)方差的跡進行計算：

然而在實際應用中，當各個狀態(tài)量之間誤差量級不一致時，顯然這種對誤差信息進行整體分配的方法無法具體反映每個狀態(tài)變量的變化特性。尤其當各個子濾波器的量測信息不一致時，對狀態(tài)誤差的修正側(cè)重不盡相同，導致對于不同的狀態(tài)誤差變量，每個子濾波器的信任程度有偏差，輸出的估計精度不一致。

針對這種情況，本文通過求解協(xié)方差的奇異值矩陣，對信息分配系數(shù)進行分塊重置，實時、精細地反映各個子濾波系統(tǒng)每個狀態(tài)分量的變化趨勢與估計精度。同時通過分塊重置，僅保留了協(xié)方差中每個狀態(tài)變量自身的方差，在不影響全局融合估計精度的基礎上也可以隔離各個狀態(tài)量之間的故障污染。基于協(xié)方差奇異值的信息分配系數(shù)分塊重置計算原理如下：

根據(jù)式(22)，可得分塊重置形式下的分配系數(shù)與信息分配矩陣為：

式中：表示第m個狀態(tài)矢量對應的分配系數(shù)；維數(shù)與狀態(tài)矢量維數(shù)一致。

此時，式(9)的信息分配規(guī)則變換為：

4 基于聯(lián)邦濾波的組合相對導航系統(tǒng)模型

4.1 狀態(tài)誤差傳播方程

相對導航問題中，一般認為長機配置有高精度的慣性導航傳感器或可以進行慣性導航噪聲自修正[14]，在本文中即認為加油機t的機載慣性導航傳感器的輸出已經(jīng)過自修正，且已知加油機t的位置、速度與姿態(tài)。

受油機r的加速度計與陀螺儀的測量模型定義為：

狀態(tài)變量選取相對慣性導航系統(tǒng)、陀螺儀和加速度的誤差共計15維，系統(tǒng)的狀態(tài)遞推方程為：

誤差狀態(tài)變量為：

根據(jù)式(3)與式(6)可推導出基于慣性導航系統(tǒng)的運動學誤差方程，可得狀態(tài)轉(zhuǎn)移矩陣F與系統(tǒng)矩陣G為：

其中，系統(tǒng)噪聲為：

4.2 相對量測誤差方程

(1)INS/GNSS子濾波器量測方程

GNSS量測信息由無基準站差分GNSS獲得，由于已知加油機t的位置與姿態(tài)信息，將相對位置、速度量測投影到加油機t的機體坐標系，可得：

選取INS與差分GNSS輸出的相對位置、速度之差作為INS/GNSS子濾波器的觀測量，量測方程可表示為：

忽略二階誤差小量，可求得雅克比矩陣為：

(2)INS/Vision子濾波器量測方程

Vision導航系統(tǒng)由兩部分組成：一部分為安裝在受油機r上的視覺傳感器；另一部分為按一定的幾何分布布置在加油機t上用于視覺識別定位的信標，如圖3所示。當受油機r接近加油機t進行加油對接時，通過視覺傳感器采集圖像信息，對信標特征點進行位姿解算，得出信標在相機坐標系中的位置狀態(tài)量。由于視覺傳感器固連在受油機r上，且已知其在受油機r機載坐標系上的位姿，因此本文假設機體坐標系與相機坐標系重合，可得信標相對于受油機r的位置為：

圖3 Vision導航系統(tǒng)示意圖Fig.3 Schematic diagram of Vision navigation system

式中：為第i個信標Bi與受油機r相對位置的真值；為對應矢量的Vision量測值；vVision為零均值白噪聲。

選取INS與Vision輸出的信標相對于受油機r的位置之差作為INS/Vision子濾波器的觀測量，量測方程可表示為：

5 仿真結(jié)果分析

5.1 仿真條件

(1)仿真行場景

圖4 加油機軌跡示意圖Fig.4 Schematic diagram of tanker trajectory

(2)傳感器仿真參數(shù)

表1 為傳感器仿真誤差參數(shù)，其中INS輸出頻率為20 Hz；GNSS輸出頻率為1 Hz；Vision輸出頻率為1 Hz。

表1 仿真參數(shù)設置Tab.1 Simulation parameter setting

如表2所示，仿真信標在加油機上的位置。

表2 信標位置Tab.2 Beacon position

5.2 仿真結(jié)果分析

為驗證本文方法的有效性，分別采用2組對比試驗進行仿真。

（1）仿真試驗1

針對空中加油近距相對導航任務，分別對INS/GNSS(IG)、INS/Vision(IV)、INS/GNSS/Vision(IGV)相對導航系統(tǒng)進行仿真試驗，如表3所示為IG/IV/IGV相對導航系統(tǒng)仿真誤差。

表3 IG/IV/IGV系統(tǒng)導航誤差Tab.3 IG/IV/IGV system navigation error

圖5 -7給出了三個組合系統(tǒng)估計相對位置、相對速度和相對姿態(tài)時的估計誤差。從圖中可以看出：INS/GNSS/Vision相對導航系統(tǒng)對相對導航狀態(tài)的誤差估計是最可靠的。相對位置誤差從INS/GNSS系統(tǒng)最高超過0.6 m的誤差降低至0.3 m以內(nèi)；相對速度從INS/Vision系統(tǒng)最高超過0.5 m/s降低至0.15 m/s以內(nèi)；在相對姿態(tài)誤差估計方面，INS/GNSS/Vision相對導航系統(tǒng)具有更精確、更迅速的收斂性能，這一點在對于航向角的估計上尤為顯著。通過融合INS、GNSS與Vision導航系統(tǒng)信息，取長補短，從而提高了整個導航系統(tǒng)的濾波估計精度。

圖5 試驗1相對位置誤差Fig.5 Test 1 relative position error

為了更直觀地對比估計精度，采用蒙特卡洛仿真進行了100次仿真驗證，圖8-10給出了三個組合導航系統(tǒng)對于相對位置、相對速度以及相對姿態(tài)的RMSE估計精度。從圖中可以看出：在相對位置、相對速度以及相對姿態(tài)估計精度上，INS/GNSS/Vision相對導航系統(tǒng)都具有顯著的優(yōu)勢。并且在100~200 s、300~400 s以及400~500 s飛機進行大幅度轉(zhuǎn)彎機動時，INS/GNSS與INS/Vision導航系統(tǒng)在進行相對位置與相對速度的誤差估計上都出現(xiàn)了不同程度的估計精度波動，而INS/GNSS/Vision相對導航系統(tǒng)在整體的濾波估計上都展現(xiàn)了較好的穩(wěn)定性與誤差估計精度，能夠在一定程度上滿足高機動環(huán)境下的相對導航估計要求。

圖6 試驗1相對速度誤差Fig.6 Test 1 relative velocity error

圖7 試驗1相對姿態(tài)誤差Fig.7 Test 1 relative attitude error

圖8 試驗1相對位置估計精度Fig.8 Test 1 relative position estimation accuracy

圖9 試驗1相對速度估計精度Fig.9 Test 1 relative velocity estimation accuracy

圖10 試驗1相對姿態(tài)估計精度Fig.10 Test 1 relative attitude estimation accuracy

(2)仿真試驗2

通過三個相對導航系統(tǒng)方案進行對比，分別為：

方案Ⅰ：INS/GNSS組合導航系統(tǒng)；

方案Ⅱ：基于常規(guī)故障隔離聯(lián)邦濾波器的INS/GNSS/Vision組合相對導航系統(tǒng)；

方案Ⅲ：基于本文容錯聯(lián)邦濾波結(jié)構(gòu)的INS/GNSS/Vision組合相對導航系統(tǒng)。

考慮到GNSS在對接過程中存在受遮擋出現(xiàn)故障的情況，假設GNSS導航系統(tǒng)相對位置輸出在100s開始以0.1 m/s的速度開始出現(xiàn)緩變故障，持續(xù)時間為150 s，如表4所示為GNSS導航系統(tǒng)故障時三種相對導航系統(tǒng)方案仿真誤差。

表4 方案Ⅰ/Ⅱ/Ⅲ相對導航誤差Tab.4 Scheme Ⅰ/Ⅱ/Ⅲ relative navigation error

圖11 -13給出了三種方案在該故障模式下100~300 s的相對導航狀態(tài)估計誤差（其中藍線為方案I，紅線為方案II，綠線為方案III）。從圖中可以看出：當GNSS導航系統(tǒng)開始出現(xiàn)緩變故障時，由于故障值較小，各方案的估計精度都較為穩(wěn)定，但隨著時間推移，故障量測對濾波估計結(jié)果的影響逐漸增長。

圖11 試驗2相對位置誤差Fig.11 Test 2 relative position error

圖12 試驗2相對速度誤差Fig.12 Test 2 relative velocity error

圖13 試驗2相對姿態(tài)誤差Fig.13 Test 2 relative attitude error

方案Ⅰ的組合導航系統(tǒng)對相對導航狀態(tài)的估計都出現(xiàn)了較大的偏移，相對位置誤差接近5 m，相對速度誤差超過0.2 m/s，相對姿態(tài)誤差接近1 °，無法滿足近距相對導航的要求；通過引入Vision系統(tǒng)對于狀態(tài)的估計誤差有了較大的改善，但由于方案Ⅱ的常規(guī)INS/GNSS/Vision組合相對導航系統(tǒng)無法對緩變故障進行有效及時地隔離，導致相對位置估計精度下降，誤差超過1 m，并逐漸對其他狀態(tài)量估計造成了污染；而本文基于容錯濾波結(jié)構(gòu)的方案Ⅲ，通過時變量測噪聲取代故障隔離模塊，進一步降低了故障信息對于狀態(tài)估計的影響，并通過分塊重置分配系數(shù)一定程度上隔離故障量測對其他狀態(tài)量估計的污染，降低子系統(tǒng)故障對于全局估計精度的影響，實現(xiàn)對緩變故障的有效處理。位置精度相對方案II提高70%以上。

6 結(jié) 論

為滿足近距空中加油相對導航的高精度、高可靠性問題，在常規(guī)INS/GNSS相對導航系統(tǒng)中引入Vision相對導航傳感器，研究了一種INS/GNSS/Vision組合的相對導航算法，融合三種傳感器的導航信息，取長補短，對相對導航狀態(tài)進行估計。該算法通過滑動窗口對時變量測噪聲進行自適應調(diào)節(jié)，來提高出現(xiàn)故障時系統(tǒng)估計的魯棒性；同時，根據(jù)協(xié)方差奇異值對分配系數(shù)進行分塊重置，一定程度上隔離各狀態(tài)量之間的故障污染，進一步提高了組合導航系統(tǒng)容錯性能。

仿真結(jié)果表明：在濾波穩(wěn)定性與估計誤差精度方面，本文的INS/GNSS/Vision組合相對導航系統(tǒng)明顯優(yōu)于INS/GNSS和INS/Vision組合導航系統(tǒng)，并且在GNSS導航系統(tǒng)出現(xiàn)緩變故障時，能夠有效保障全局估計精度，有較強的魯棒性與較好的容錯性。通過對仿真結(jié)果的分析，驗證了本文基于容錯濾波結(jié)構(gòu)INS/GNSS/Vision組合近距空中加油相對導航算法的有效性。