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

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

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

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

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

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

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

2 相對導航運動學模型

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

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

2.1 相對質心運動方程

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

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

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

2.2 相對姿態方程

采用四元數形式表示姿態，姿態四元數由旋轉軸n與旋轉角θ表示：

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

定義受油機r相對于加油機t的姿態四元數表示為：

式中：qr為受油機r的姿態四元數，qt為受油機t的姿態四元數，?為四元數乘法。

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

3 容錯聯邦濾波器設計

3.1 INS/GNSS/Vision組合聯邦濾波器

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

圖2 聯邦濾波器結構圖Fig.2 Federated filter structure diagram

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

式中：Fk,k1-為狀態轉移矩陣；Gk,k1-為系統矩陣；Wk-1的協方差為Q；的協方差為Ri。

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

(1)信息分配

(2)子濾波器時間更新

(3)子濾波器量測更新

(4)主濾波器信息融合

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

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

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

可得實際殘差序列協方差矩陣為：

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

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

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

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

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

3.3 基于協方差奇異值的信息分配系數分塊重置

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

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

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

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

式中：表示第m個狀態矢量對應的分配系數；維數與狀態矢量維數一致。

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

4 基于聯邦濾波的組合相對導航系統模型

4.1 狀態誤差傳播方程

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

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

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

誤差狀態變量為：

根據式(3)與式(6)可推導出基于慣性導航系統的運動學誤差方程，可得狀態轉移矩陣F與系統矩陣G為：

其中，系統噪聲為：

4.2 相對量測誤差方程

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

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

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

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

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

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

圖3 Vision導航系統示意圖Fig.3 Schematic diagram of Vision navigation system

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

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

5 仿真結果分析

5.1 仿真條件

(1)仿真行場景

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

(2)傳感器仿真參數

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

表1 仿真參數設置Tab.1 Simulation parameter setting

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

表2 信標位置Tab.2 Beacon position

5.2 仿真結果分析

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

（1）仿真試驗1

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

表3 IG/IV/IGV系統導航誤差Tab.3 IG/IV/IGV system navigation error

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

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

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

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

圖7 試驗1相對姿態誤差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相對姿態估計精度Fig.10 Test 1 relative attitude estimation accuracy

(2)仿真試驗2

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

方案Ⅰ：INS/GNSS組合導航系統；

方案Ⅱ：基于常規故障隔離聯邦濾波器的INS/GNSS/Vision組合相對導航系統；

方案Ⅲ：基于本文容錯聯邦濾波結構的INS/GNSS/Vision組合相對導航系統。

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

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

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

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

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

圖13 試驗2相對姿態誤差Fig.13 Test 2 relative attitude error

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

6 結 論

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

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