基于因子圖的空天飛行器多源異步導航算法

聶庭宇,王 融,劉建業,熊 智

（南京航空航天大學自動化學院,南京 211106）

0 引言

作為集航空器、航天器和運載器于一體的可重復使用的新型飛行器,空天飛行器在重復使用性、發射操作費用、可維修性、周轉時間和靈活機動性等方面都有革命性的改變,是未來確保空間優勢核心能力的關鍵支柱［1-2］。由于空天飛行器多任務、多工作模式的特征,其導航系統需具有靈活性且保證可靠性和穩定性。為保證整個飛行過程中導航系統的可靠性,可用多種導航傳感器組成冗余配置的多源組合導航系統,在部分導航系統出現故障和異常情況下仍能夠連續提供準確的導航信息。在多傳感器數據融合方面,目前采用較多的方法是Kalman濾波算法。但是,隨著跨空天飛行環境的變化、不同傳感器量測周期的差異,導致多傳感器的數據融合需要面對采樣信息異步非周期和量測冗余特性隨環境變化的特性［3-5］。針對此量測動態集成的特性,導航系統不僅需要最優的導航結果,同時要求其傳感器及融合算法具有信息靈活接入處理的能力,通過優化利用導航信息源保障多任務多工作模式下穩定的導航能力。

因子圖是圖模型中的一種,已廣泛應用于人工智能、信號處理、數字通信等領域［6-9］。文獻［10］以民用無人機為背景對全源導航的關鍵技術展開研究,構建了基于因子圖的多傳感器融合框架。文獻［11］以無人機MINS/BDS/磁羅盤/氣壓高度表組合導航系統為例構建了因子圖信息融合模型,并通過車載跑車試驗采集了導航傳感器原始數據進行離線仿真。通常,多源量測信息會出現信息可用性非周期變化的情況,此時信息融合主要涉及的問題有數據異構、多源采樣頻率不同和系統穩定性等;當系統的部分信息源失效后,如何保證系統的信息準確性這就需要進行較穩定的信息融合［12］。因子圖將狀態量定義為變量節點,各傳感器測量值定義為因子節點,多傳感器組合導航便轉換為在因子圖中相應的變量節點上連接這些因子節點并以此為基礎提供了一個非常靈活的框架,具有“即插即用”的特性,可以很好地融合這多個互補的信息來源,并解決異步非周期信息源造成的信息融合難點。因此,基于因子圖模型的信息融合方法在解決數據融合中的異步問題方面具有良好的性能［13-14］,而且針對多傳感器具有很好的擴展性,可以對傳感器靈活配置。

本文依據因子圖在處理多源信息異步采樣以及信息可用性非周期變化問題方面的優勢,通過分析因子圖基本理論,設計了空天飛行器導航傳感器冗余配置方案,并研究了基于因子圖的空天飛行器多源信息組合導航算法。

1 空天飛行器組合導航系統方案

空天飛行器的飛行過程可分為上升段、在軌段、再入段以及著陸段,在不同的階段飛行環境差別較大,對導航系統的精度和性能也有不同的要求。為滿足空天飛行器飛行任務需求,導航系統必須具有高精度和自適應的特性。

根據空天飛行器的任務要求及飛行環境,考慮到慣性導航系統（Inertial Navigation System,INS）具有高動態適應性和抗干擾能力,通常將其作為貫穿整個飛行過程的組合導航核心。由于慣導誤差隨時間的推移累積特性,需引入其他傳感器系統進行數據融合和誤差修正,輔以衛星導航系統（Global Navigation Satellite System,GNSS）、天文導航系統（Celestial Navigation System,CNS）、大氣數據系統（Atmospheric Data System,ADS）、無線電導航系統（Radio Navigation System,RNS）以及微波著陸系統（Microwave Landing System,MLS）等,在飛行任務的不同階段提供輔助量測信息,用于導航誤差的估計和修正,如圖1所示。

圖1 空天飛行器組合導航系統方案圖Fig.1 Scheme diagram of integrated navigation system for aerospace vehicle

因子圖算法通過構建系統某一時間段內的圖模型,將系統狀態與導航信息相關聯,在給定所有可用量測值后,計算所有狀態的聯合概率分布函數的最大后驗概率估計實現數據的優化融合［10-11］。根據空天飛行器冗余配置的組合導航系統的特點以及所選擇的導航傳感器設備,本文構建了因子圖的組合導航框架,如圖2所示。

圖2中采用了多個慣性導航系統為主要參考源,輔以衛星導航系統、天文導航系統、大氣數據系統、無線電導航系統以及微波著陸系統,將6種導航系統量測方程抽象為6種不同的因子節點。圖2中,圓圈為狀態變量節點,圓點為因子節點。其中,x為系統的導航狀態,fIMU為來自IMU的量測信息,f GNSS、f CNS、f ADS、f RNS和f MLS分別為來自GNSS、CNS、ADS、RNS和MLS的量測信息。

圖2 基于因子圖的組合導航框架示意圖Fig.2 Schematic diagram of integrated navigation framework based on factor graph

基于因子圖構架的組合導航系統能夠有效地實現空天飛行器飛行過程中導航信息的即插即用。跨空天飛行過程中,若在導航系統中動態引入其他導航傳感器量測信息,則可構建相應的新因子節點并拓展因子圖。根據傳感器的觀測方程構建邊緣概率函數以及相應的代價函數進行的狀態更新,可作為解決空天飛行器多冗余配置的多傳感器觀測數據問題的一種有效方法。

2 基于因子圖的空天飛行器組合導航信息融合方法設計

2.1 空天飛行器多源融合導航因子模型

因子圖通過構建概率圖模型的方式來對多種傳感器信息源之間的關聯與依賴特性進行描述,利用圖論實現多傳感器的即插即用以及數據融合的過程。因子圖可以用來對各類系統進行建模,基于因子圖建模的主要目的是將復雜的系統進行因式化分解。當因子圖的節點表示隨機變量和概率分布時,因子圖也是一種概率圖模型,在導航上的應用主要為概率圖。

（1）INS因子

空天飛行器導航狀態的時間更新可用如下方程進行抽象描述

式（1）中,fb和ωb分別為加速度計和陀螺儀測量的比力和角速度。式（2）中,γn、θe和ψd為姿態失準角,δVn、δVe和δVd為北東天三個方向的速度誤差,δL、δλ和δh為緯度、經度和高度的位置誤差,εbx、εby、εbz、εrx、εry和εrz為陀螺儀隨機誤差,Δrx、Δry和Δrz為加速度計隨機誤差。將式（1）離散化,可得

（2）輔助導航系統因子

輔助導航系統中,GNSS能夠提供輔助的位置、速度及姿態信息,其量測方程為

則以GNSS為例構建的因子節點表示為

其余的輔助導航系統中,ADS能夠提供速度信息以及高度信息,CNS能夠提供位置和姿態信息,RNS能夠提供位置信息,MLS能夠提供位置和姿態信息。這些導航系統的量測因子可參考GNSS因子的表示方法。

不同時刻的空天飛行器采用不同的輔助導航系統構架方案,其部分時刻因子圖示意圖如圖3所示。

圖3 空天飛行器組合導航因子圖示意圖Fig.3 Schematic diagram of integrated navigation factors for aerospace vehicle

每個因子圖都是由節點和邊組成的,每個局部函數都可以由唯一的一個因子節點表示,每個變量由唯一的一個變量節點表示。當某一個局部函數與某一個變量相關時,相應的因子節點與變量節點通過邊相連。基于因子圖框架,變量節點到因子節點或者因子節點到變量節點是通過信息交互,通過最大后驗概率估計算法進行狀態估計。

2.2 基于因子圖的多源信息融合算法

定義tk時刻的導航狀態矢量為Xk,則其聯合概率密度函數為Pk=(Xk|Zk)。其中,Zk為該時刻得到的量測信息,則其最大后驗概率估計為

a=arg maxf(a)表示求滿足使f(a)最大的a的值因子節點。故式（8）中,a為Xj:k∈Xk,表示一個變量節點的集合,f(a)為P(Xj:k|Zj:k)。由于先驗概率一致,且各量測相互獨立,可以得到后驗概率正比于狀態遞推與觀測量的概率密度至積。k時刻的概率密度可表示為

根據最大后驗概率的公式和全概率Bayes公式,求解滿足最大后驗概率最大的狀態變為最終的估計值,則一段時間j時刻到k時刻之內的后驗概率可表示為

式（10）中,P(Xj:k|Zj:k)為j時刻到k時刻之間的觀測量對狀態量的先驗概率,P(Xi|Xi-1)為i時刻的狀態量相對于前一時刻狀態量的先驗概率。

通過式（10）可以看出,該先驗概率可以表示為多個函數相乘的形式

對式（11）取對數,從而將乘積變成和的形式

式（12）中,‖a‖=aTΛ-1a指馬氏距離,Λ為測量噪聲方差陣。

通過取對數,將求解最大后驗概率的問題轉化成求解代價函數最小的問題

2.3 基于因子圖的異步信息融合算法

空天飛行器多源導航系統各子系統的量測信息可用性受傳感器采樣時間或工作特性影響,將隨時間或環境變化而改變。針對子系統傳感器可用性反復變化的特性,信息融合算法需要具有處理量測信息異步的能力。

在處理異步非周期信息時,常規Kalman濾波算法是在數據融合前消除時間不同步的問題,其信息融合周期如圖4（a）所示。其中,GNSS、ADS、CNS、RNS、MLS的量測周期分別為TG、TA、TC、TR、TM,Kalman濾波的周期為Tk。實心圓為在此時刻導航傳感器獲取的量測信息,此時不進行信息融合而是保存量測信息,僅在到達濾波周期時進行信息融合,而虛線圓為在此時刻該傳感器采用之前存儲的量測信息進行信息融合。而因子圖信息融合方法是異步量測信息異步融合的方式,其信息融合周期如圖4（b）所示。其中,因子圖信息融合的周期為Tf。

圖4 Kalman濾波及因子圖信息融合周期示意圖Fig.4 Period diagram of Kalman filter and factor graph information fusion

由圖4可知,在處理異步量測信息時,常規Kalman濾波算法是對量測信息進行存儲,并將高頻量測值融合到低頻量測時刻進行同步;而因子圖算法是采用各量測周期的最大公約數作為信息融合周期,實時改變信息融合結構。兩者相較之下,采用因子圖算法能夠更高效地利用異步傳感器的量測信息。

3 基于因子圖的空天飛行器組合導航系統多源信息融合仿真和分析

3.1 仿真環境配置

根據空天飛行器的飛行任務,其飛行過程可分為上升段、在軌段、再入段以及著陸段。結合各個飛行階段的飛行環境,對空天飛行器航跡進行仿真,仿真時間為4500s。仿真的三維航跡如圖5所示。

圖5 空天飛行器航跡三維示意圖Fig.5 Three-dimensional diagram of aerospace vehicle flight track

本次仿真以慣性導航系統為核心組件,其余導航系統為輔助導航,形成多種組合導航方式,并建立組合導航方案庫。

自適應導航系統配置了2套慣性導航系統,其參數如表1所示。

表1 INS參數Table1 Parameters of INS

自適應導航系統另外配置了1套全球衛星導航系統（GNSS）、1套天文導航系統（CNS）、1套大氣數據系統（ADS）、1套地基無線電導航系統（RNS）和1套微波著陸系統（MLS）,各導航子系統的參數如表2所示。

表2 自適應導航系統子系統參數Table2 Subsystem parameters of adaptive navigation system

3.2 基于因子圖的空天飛行器組合導航系統仿真

在空天飛行器飛行過程中,導航系統會因各類故障造成一定程度上的失效,本文的仿真對此進行了設定,各輔助導航系統實際正常工作情況如圖6所示。

圖6 輔助導航系統工作狀態圖Fig.6 Working state diagram of auxiliary navigation system

在此基礎上,本文進行了基于非異步導航信息源的Kalman濾波算法和因子圖信息融合算法兩種方式的組合導航系統仿真。Kalman濾波算法以多個導航信息源的最大量測周期作為其信息融合的周期,而因子圖信息融合算法可以在導航信息源變化時實時地修改因子圖結構,達成“即插即用”的功能,兩種算法的誤差曲線如圖7所示。

圖7 組合導航系統誤差曲線Fig.7 Error curves of integrated navigation system

通過數據統計,可計算出因子圖算法及Kalman濾波算法的標準差,如表3所示。比較圖7和表3,可以明顯看出,相較于Kalman濾波算法,在組合導航系統結構變化的情況下,因子圖算法獲得的導航信息更加平滑且能夠得到更高精度的導航結果,誤差降低約20%～40%。

表3 組合導航系統誤差統計表Table3 Error statistics of integrated navigation system

4 結論

空天飛行器具有多任務、長航時、高機動性的特點,這對其冗余配置的多源組合導航系統提出了高精度、高可靠性的要求。本文通過仿真建模的方式,在冗余配置的多源信息組合導航系統存在輔助導航采樣時斷時續的情況下,對比了因子圖和傳統Kalman濾波信息融合的精度和穩定性。因子圖由于其數學特性以及圖模型構建和狀態估計算法的優越性,在組合導航系統工作過程中獲得了比傳統Kalman濾波更好的信息融合效果,因子圖能夠在面對不同導航系統特性的情況下設計出更具針對性的導航信息融合濾波算法。未來,因子圖信息融合算法將是全源導航系統算法的重要研究方向。