基于因子圖的船用導航系統信息融合算法

張崇猛，鄧福建，楊 濤，蘭曉陽

（1.中國船舶航海保障技術實驗室，天津 300131；2.天津航海儀器研究所，天津 300131）

艦船導航系統通常包括捷聯慣性導航系統(SINS)、北斗(BD)、多普勒(DVL)和天文導航(CNS)等，其中任何單一導航設備都具有一定的局限性，而相互之間輸出信息又具有冗余性和互補性。為滿足艦船長期航行中高精度和高可靠性的要求，有必要進行多源融合組合多導航設備的量測信息，克服單一導航設備存在的不足，提高導航系統的可靠性。對于多源導航信息融合算法，目前常用的有聯邦濾波和因子圖等[1]。國內外學者對聯邦濾波的研究起步較早，已取得豐碩的成果，如文獻[2]利用混合粒子聯邦濾波方法，解決具有非線性系統的組合導航信息融合問題，文獻[3]設計一種自適應聯邦濾波算法，并基于支持向量機設計一種故障檢測隔離算法，提高組合導航的穩定性。聯邦濾波算法通常要求各導航設備連續工作且具有相同的信息更新頻率。然而艦船眾多導航設備通常具有不同的信息更新頻率，且各導航設備的可用性隨環境動態改變，如艦船在深水域航行時多普勒計程儀(DVL)無法測得絕對速度，北斗導航系統(BD)和天文導航系統(CNS)的可觀測星數目受環境影響等，這些情況使得聯邦濾波需要頻繁調整融合周期和架構，增加組合導航系統設計的復雜性，使組合導航系統的靈活性和可擴展性降低。

文獻[4]針對自主水下航行器(AUV)異步傳感器的信息融合問題，提出一種基于因子圖的多源信息組合導航算法，實現連續穩定的輸出導航結果。文獻[5]針對地面車輛在GNSS 拒止環境下精確導航定位的需求，提出基于因子圖的即插即用融合算法，快速組合可用量測信息獲得健壯的導航性能。文獻[6]提出一種基于因子圖的異步信息融合算法，應用于無人定翼機種，滿足各種條件下多源信息融合的需要，獲得精確可靠的定位結果。文獻[7]采用衛導輸出的偽距、偽距率構建慣導/衛導因子圖組合模型，利用信息矩陣的稀疏性進行優化求解以滿足實時性的要求。文獻[8]針對微型無人機中多導航設備信息更新頻率不同和導航設備非線性問題，提出基于因子圖的信息融合算法，并搭建試驗系統驗證算法的有效性。文獻[9]采用貝葉斯樹的增量推理因子圖算法，自適應判斷需要迭代優化的節點數量，提高計算效率。因子圖通過把導航設備量測信息抽象為因子節點，用圖模型描述融合問題，提供一個統一的導航設備接口和融合算法以適應各種復雜環境。目前關于因子圖組合算法的研究正在逐步深入，在因子圖組合算法中當某個導航設備出現故障時如何進行容錯設計還缺乏研究。

綜上，針對船載導航設備信息更新頻率不同和可用性隨環境動態改變問題，本文設計一種基于因子圖的船用導航系統信息融合及容錯算法，實現不同類型導航設備的即插即用和異步信息融合；采用增量平滑算法，利用先前步驟的計算結果，減少計算量；設計基于因子圖的容錯算法，并用于導航設備的故障檢測，避免故障信息污染組合導航系統。最后開展數值仿真和半實物車載試驗驗證所提算法的有效性。

1 船用導航系統信息融合因子圖架構

基于捷聯慣性導航系統(SINS)、北斗(BD)、多普勒計程儀(DVL)和天文導航(CNS)的船用導航系統因子圖融合架構如圖1所示。圖中慣性測量單元因子節點fIMU連接相鄰時刻的導航狀態變量節點，北斗因子節點fBD、多普勒計程儀因子節點fDVL和天文導航因子節點fCNS分別經故障檢測與隔離模塊(FDI)與相應時刻導航狀態變量節點和系統誤差變量節點連接，同時，系統誤差因子節點ferr連接相鄰時刻的系統誤差變量節點，因子圖模型隨量測信息的不斷接入而拓展。

圖1 基于因子圖的船用導航系統融合架構Fig.1 Integrated architecture of ship navigation system based on factor graph

定義ti時刻的導航狀態變量為xi和各導航設備的系統誤差變量為ci，其中導航狀態變量xi包括載體的姿態四元數、速度和位置，即xi=［qiViPi］T，系統誤差變量包括陀螺常漂εn和加速度計常偏 ?n、北斗接收機鐘差δtu和鐘漂δtf、多普勒計程儀刻度系數誤差δk。定義從初始時刻t0到當前時刻tk的導航狀態變量和系統誤差變量的集合；定義從初始時刻t0到當前時刻tk的量測信息集合為其中zi為ti時刻接收到的量測信息集合，記分別表示慣性測量單元因子節點fIMU、北斗因子節點fBD、多普勒計程儀因子節點fDVL、天文導航因子節點fCNS在ti時刻的輸出，則zi為集合的子集，其中北斗導航系統的量測為偽距、偽距率，多普勒計程儀的量測為速度，天文導航的量測為高度角。

慣性測量單元因子節點可表示為：

式(1)中，fIMU表示導航狀態運動方程，導航狀態之差只是形式上的表式，對于姿態四元數，具有乘性性質。表示馬氏距離，表示慣性測量單元的測量噪聲誤差協方差矩陣，表示歸一化系數。

系統誤差因子節點可表示為：

式(2)中fc表示系統誤差傳遞規律方程，Ric表示系統誤差的估計誤差協方差矩陣。

根據載體連續時間運動微分方程：

式中，C bn為b系到n系姿態轉換陣，ω ie為地球自轉角速度，R M、R N分別為艦船位置點處子午圈、卯酉圈曲率半徑，h為當地高度，L為當地緯度。

［ ˙］xyz表示只取四元數的虛部組成的三維向量，φ表示姿態角誤差。

陀螺和加表隨機漂移可建模如下：

式(5)中，εw和?w分別為陀螺和加表的測量白噪聲，εbn和 ?bn為陀螺和加表隨時間緩慢變化的偏置誤差，滿足下式：

εbw和?bw分別為陀螺和加表偏置誤差的驅動噪聲。北斗接收機鐘差鐘漂和DVL 刻度系數誤差微分方程如下：

定義ti時刻誤差向量

將式(3)離散化，可得載體離散時間運動方程

假設根據載體離散時間運動方程推算ti時刻導航狀態估計，ti時刻導航狀態估計，則IMU 因子節點殘差定義為：

北斗、DVL 和天文導航因子節點具有如下形式：

式中，r分別表示衛星無線電導航系統(RNSS)，DVL和CNS；fr分別表示北斗、DVL 和天文導航的量測方程，將式(10)在導航狀態變量估計值和系統誤差變量估計值處作一階泰勒展開，定義變量節點ui=［x ici］，則各因子節點可表示如下：

北斗第j顆衛星的偽距和偽距率量測方程為：

式中，xc、yc、zc和分別表示艦船在地心地固坐標系中的位置和速度，和分別表示第j顆衛星在地心地固坐標系中的位置和速度，ej1、ej2和ej3分別表示第j顆衛星到艦船的方向余弦向量。

北斗第j顆衛星的量測矩陣為：

式中，各符號定義為：

DVL 速度量測方程為：

式中，δk為刻度系數誤差。

DVL 量測矩陣為：

為簡化三角函數運算量，以高度角的正弦值作為間接觀測量。天文導航量測方程為：

式中，α為可觀測星的天球赤經，δ為天球赤緯，tG為本初子午線赤經。對于第j顆可觀測星，量測矩陣如下：

式中，

綜上，基于所有時刻量測信息Zk的聯合概率密度函數為：

根據最大后驗概率估計，各時刻導航狀態和系統誤差變量的最優估計值如下：

采用無約束非線性最小二乘優化理論求得式(19)的最優解，即可獲得各時刻導航狀態量的最優值Xk*。

2 基于因子圖的信息融合算法

2.1 增量平滑融合算法

對于SINS/BD/DVL/CNS 的融合問題，每插入一個新的量測因子節點，就相當于在式(19)線性化建立的雅克比矩陣中增加一行，隨著導航時間的增加，雅克比矩陣的維數也將越來越大，式(19)求解的計算量也逐漸增大。由于只有部分變量節點受新因子節點的影響[10]，因此在計算過程中可重復利用先前步驟的計算結果進行增量更新。

將式(19)中馬氏距離轉化為2-范數，即：

將式(19)轉化為標準最小二乘的優化問題，并線性化建立雅克比矩陣J，雅克比矩陣中每一行對應因子圖中的一個因子節點，如式(21)所示。

對應的右側向量為：

采用對聯合概率密度函數進行變量消元的方式實現對雅克比矩陣J的更新，將聯合概率密度函數分解為如下條件概率密度形式：

其中Si是與變量xi相關的分離器，選定變量消元順序后，逐步對變量和其分離器采用部分QR 分解算法依次進行消元，直至將雅克比矩陣J分解為上三角形矩陣R和正交矩陣Q。當插入新的因子節點后，識別受新因子節點影響的變量節點，然后只對受影響的變量節點及其分離器進行部分QR 分解，完成受影響變量節點的消元過程。消元步驟完成后，反向回代計算受影響的變量節點的估計值。然后在更新后的狀態變量估計值處重新線性化，迭代計算導航狀態變量的最優值，實現導航狀態變量的更新。增量更新算法如下：

1) 根據初始信息建立先驗因子節點P（u0），設置變量節點初值；

2) 插入因子節點：接收到BD、DVL 或CNS量測信息時，建立相應因子節點并連接相關變量節點，接收到IMU 量測信息時建立IMU因子節點和當前時刻變量節點，并與前一時刻變量節點連接；

3) 識別受新因子節點影響的變量節點集合；

4) 根據IMU 測量信息計算新增變量節點初始估計值，并在狀態變量估計值處線性化建立雅克比矩陣J和右側向量b；

5) 采用變量消元方式求解狀態變量誤差估計值δui，并更新變量節點，令

2.2 容錯算法

全局雅克比矩陣J經QR 分解后得到上三角形矩陣R，在矩陣R中包含有導航狀態和系統誤差的誤差協方差信息，記∑為所有時刻導航狀態和系統誤差的誤差協方差矩陣[11]，則：

記tk-1時刻導航狀態和系統誤差的誤差協方差矩陣為Pk-1，在R的維數較大時為減少計算量可采用回代算法計算Pk-1。假設Pk-1的維數為n，定義，則關于X的方程RTRX=B的解的最后一個n×n矩陣塊即為Pk-1。定義：

其中Y為中間過渡變量，因R是上三角矩陣，采用回代算法可快速求解未知矩陣X。

設tk時刻接收到量測信息zkr后，根據tk-1時刻的變量節點最優估計和IMU 量測進行預測，得tk時刻的導航狀態變量節點預測值和系統誤差變量節點預測值，可知不受量測信息zkr的影響，的估計誤差協方差矩陣:

定義殘差:

則系統無故障時殘差應為零均值的高斯白噪聲，殘差rk的協方差矩陣為：

定義統計量:

則λk服從χ2分布，由奈曼-皮爾遜準則可知，當限定誤警率P f=α時，可由解出使得漏檢率達到最小的門限TD。則判定準則為：

故障檢測與隔離模塊(FDI)插入在量測因子節點和變量因子節點之間，通過式(29)計算的統計量λk和式(30)的判定準則判斷新接收到的量測信息是否有故障，最后決定是否將該因子節點插入到相應的變量節點上。

3 仿真試驗與分析

為了驗證本文提出的基于因子圖的船用導航系統信息融合及容錯算法的性能，分別進行了仿真驗證和半實物車載試驗驗證；同時采用有重置的聯邦濾波算法與所設計的因子圖算法進行對比。

3.1 仿真試驗

設置仿真時長3600 s；IMU 更新頻率為200 Hz，陀螺儀常值漂移誤差0.01 °/h，角度隨機游走系數；加速度計常值漂移誤差100 μg，速度隨機游走系數；北斗更新頻率1 Hz，偽距測量噪聲均方根值2 m，接收機鐘差等效測距誤差300 m，偽距率測量噪聲均方根值0.1 m/s；DVL 更新頻率10 Hz，刻度系數誤差0.001，速度測量噪聲誤差均方根值0.1 m/s；天文導航更新頻率2 Hz，高度角測量噪聲均方根值10 "。由于在復雜導航環境下，DVL因工作環境限制易發生間斷性失效，北斗和天文導航易受環境和天氣影響；捷聯慣導是一種自主導航系統，且慣導本身可以采用余度技術提高可靠性，因此設置各導航設備工作狀況如表1。分別采用因子圖算法和有重置聯邦濾波算法對SINS/BD/DVL/CNS 進行信息融合，仿真結果如圖2～4，導航參數誤差以及定位誤差（徑向誤差）的均方根值（RMS）如表2所示。

表1 各導航設備工作狀況Tab.1 Working condition of each sensor

圖2 因子圖與聯邦濾波的姿態誤差角對比曲線Fig.2 Comparison on attitude errors between factor graph and federated filter

圖3 因子圖與聯邦濾波的速度誤差對比曲線Fig.3 Comparison on velocity errors between factor graph and federated filter

圖4 因子圖與聯邦濾波的位置誤差對比曲線Fig.4 Comparison on position errors between factor graph and federated filter

表2 因子圖和聯邦濾波算法定位性能比較Tab.2 Comparison of location performance between factor graph and federated filter algorithm

由圖2～4 的誤差曲線可以看出，因子圖與聯邦濾波都能保持較高的定位精度，位置誤差收斂在5 m 以內，速度誤差收斂在0.1 m/s 以內，姿態誤差收斂在1?以內，具有較好的融合精度。

船用導航系統通常采用高精度導航設備，可近似視為線性系統。由于因子圖算法是一種最小殘差優化算法，對于線性系統，聯邦濾波是一種最小化狀態誤差協方差矩陣的估計算法，在理論上因子圖算法和聯邦濾波算法具有相當的導航精度。對比表2中導航參數誤差的均方根值，因子圖姿態估計精度稍優于聯邦濾波，因子圖的定位誤差為0.74 m，聯邦濾波的定位誤差為0.88 m，速度誤差均為0.01 m/s，可知因子圖與聯邦濾波導航定位精度相當。

聯邦濾波由于需要調整融合周期而舍棄部分量測信息，而因子圖可將全部量測信息作為因子節點插入到圖中，因為姿態為間接觀測量，誤差估計較慢，因子圖通過引入更多速度量測信息而具有稍高的姿態估計精度，速度和位置為直接觀測量，誤差能夠快速校正，更高頻率的速度更新對精度影響不大，仿真驗證結果和理論分析一致。

聯邦濾波需要通過主濾波器對各子濾波器的信息進行融合，然后再根據信息分配原則將主濾波器的全局估計重新分配給各子濾波器，使得聯邦濾波的融合周期為各子濾波器融合周期的最小公倍數，當導航設備可用性動態改變時，聯邦濾波需要根據當前可用的導航設備進行系統重構。因子圖算法則是在任一時刻接收到導航設備的量測信息時建立相應的量測因子節點，并將該量測因子節點插入到相應時刻的變量節點上，同時對更新后的因子圖進行優化求解，實現多導航源的信息融合。因此，因子圖算法具有異步信息融合和導航設備可用性動態改變時的即插即用能力，可根據導航設備的可用性增減相應因子節點，在保證與聯邦濾波導航精度相當的前提下提高系統的靈活性和擴展性，滿足復雜環境下的導航信息融合。

為驗證所設計容錯算法的性能，對各導航設備設置如表3所示故障，進行仿真驗證。仿真結果如圖5～6所示。

表3 各導航設備故障設置Tab.3 fault settings of each sensor

圖5 χ 2統計量曲線Fig.5 χ 2statistics curve

圖6 因子圖位置誤差曲線（導航設備有故障）Fig.6 Position errors based on factor graph(multi-sensor with fault)

由圖5可知，當某個導航設備發生故障后，χ2統計量迅速增大，本文所設計的基于因子圖的容錯算法能夠有效識別出發生故障的導航設備，進而將故障導航設備進行隔離。從圖6可以看出，帶有故障的量測信息已被隔離，組合導航系統不受故障導航設備的污染，使位置誤差保持在5 m 以內。

3.2 半實物車載試驗

為進一步驗證所設計的基于因子圖的融合與容錯算法的性能，開展跑車試驗。因受跑車試驗條件限制，開展慣導、白光測速儀和北斗的融合進行試驗驗證。試驗以諾瓦泰差分接收機輸出的速度位置作為參考信息，輸出頻率為1 Hz；并同步采集慣導、北斗和白光測速儀的數據，其中慣導輸出頻率100 Hz，白光測速儀輸出頻率10 Hz，北斗輸出頻率1 Hz。設置在1200-2400 s 白光測速儀不可用，運用因子圖算法和聯邦濾波算法分別對慣導/北斗/白光測速儀進行信息融合，得融合結果如圖7、圖8和表4所示。

圖7 因子圖與聯邦濾波的速度誤差對比曲線Fig.7 Comparison on velocity errors between factor graph and federated filter

圖8 因子圖與聯邦濾波的位置誤差對比曲線Fig.8 Comparison on position errors between factor graph and federated filter

從圖7、圖8可以看出，因子圖和聯邦濾波水平定位誤差在5 m 以內，速度誤差在0.1 m/s 以內；由表4可知，因子圖算法的定位誤差為0.79 m，聯邦濾波定位誤差0.80 m，可知因子圖與聯邦濾波導航精度相當，半實物車載試驗結果與上節仿真驗證結果一致。但在導航設備工作狀態發生改變時，聯邦濾波需要重構，因子圖算法可通過增減相應導航設備的因子節點實現，具有更好的異步信息融合和即插即用能力，組合系統具有更強的靈活性和擴展性。

表4 因子圖、聯邦濾波算法定位性能比較Tab.4 Comparison of location performance between factor graph and federated filter algorithm

為驗證所提因子圖容錯算法，在900-1200 s 北斗注入定位誤差 200 m，速度測量誤差 0.3 m/s，2100-2400 s 白光測速儀注入測量定位誤差3 m/s，驗證因子圖容錯算法的性能。結果如圖9和圖10 所示。

圖9 因子圖位置誤差曲線（導航設備有故障）Fig.9 Position errors based on factor graph(multi-sensor with fault)

圖10 χ 2統計量曲線Fig.10 χ 2statistics curve

從圖9和圖10 可知，當導航設備發生故障后，所設計的容錯算法能夠有效識別和隔離故障導航設備信息，使組合系統不受污染，并利用無故障導航設備信息進行定位，保持高精度的導航定位。

4 結 論

本文針對艦船導航系統的不同導航設備之間異步的信息更新頻率以及可用性隨環境動態改變的多源融合問題，提出一種基于因子圖的融合與容錯組合導航方案。設計基于SINS/BD/DVL/CNS 因子圖融合與容錯模型，并與有重置聯邦濾波器的導航性能進行對比。該方法與有重置聯邦濾波的導航精度相當，但卻可以靈活組合異步量測信息，以及在導航設備隨環境發生可用性動態改變時實現即插即用，有效實現船用導航系統的信息融合；同時實現對導航設備進行故障檢測和隔離，保證組合導航系統不受故障導航設備的影響。最后，通過開展數值仿真和半實物車載試驗，驗證本文所提算法的有效性。