基于因子圖的無人機多源組合導航算法研究*

李 杰 陳 帥 王 琮 姚曉涵 朱曉晗

1.南京理工大學，南京210094 2.中國電子科技集團第三十二研究所，上海201808

0 引言

無人機飛行器具有低風險、體積小、長續航和環境適應能力強等優點，在軍事和商業領域中得到了廣泛的應用[1]。由于無人機工作環境的復雜性和不定性，如何持續為無人機飛行器提供精確的定位導航信息是當前的一大難題。以微慣性導航系統(Micro-inertial Navigation System,MINS)作為參考系統，與北斗衛星導航系統(BeiDou Navigation Satellite System,BDS)、磁羅盤、氣壓高度表共同構成的多源組合導航系統彌補了傳統的MINS/BDS組合導航系統的不足，使得導航系統工作的可靠性大大提升，可以實時為無人機進行導航。

目前，對多源組合導航算法研究多以卡爾曼濾波為主，但由于各個傳感器數據傳輸頻率的不同，傳統聯邦卡爾曼濾波器需要全部量測信息到來之后才能進行濾波，使得在處理異步信息時能力有所不足，并且在使用外推內插法將量測信息同步時，將誤差引入了濾波器，增加了計算量[2-3]。文獻[4]利用因子圖模型良好的擴展性和靈活性，在該模型中處理INS/GPS組合導航系統的信息滯后問題，快速地融合異步量測信息，從而保證了導航系統的精度和實時性，計算量大大減少。

本文以無人機飛行器MINS/BDS/磁羅盤/氣壓高度表多源組合導航系統為例，通過構建因子圖模型，研究了基于因子圖的數據融合方法，通過半物理仿真試驗驗證了該方法在無人機導航系統中的可行性。

1 因子圖模型概述

因子圖作為一種概率圖模型，除解決數學概率問題外，還廣泛應用于人工智能、信號處理、數字通信等領域[5-7]。

因子圖表示的是將一個多變量的全局函數因子分解成局部函數的積而形成的二分圖模型G=(F,X,E)，可用于表示導航的估計問題。該二分圖模型由變量節點(xi∈X)、因子節點(fj∈F)和連線(eij∈E)構成，當變量節點和因子節點相關時，它們中間會產生一條連線。

因子圖G將函數f(X)的因式分解為

(1)

式中：Xi為{x1,x2,…,xn}的子集，fi為局部函數。

設f(x1,x2,x3,x4,x5)是一個包含5個變量的函數，且f可表示為

f(x1,x2,x3,x4,x5)=
f1(x1)f2(x2,x3)f3(x1,x3,x4)f4(x4,x5)f5(x2,x5)

(2)

式中：X1={x1}，X2={x2,x3}，X3={x1,x3,x4}，X4={x4,x5}，X5={x2,x5}。則式(2)可表示的因子圖結構如圖1所示。

圖1 函數f(x1,x2,x3,x4,x5)對應的因子圖結構

圖中，圓形表示變量節點，正方形表示因子節點，每個因子節點和不同的變量節點產生關系，從而構成不同的局部函數。

2 基于因子圖的MINS/BDS/磁羅盤/氣壓高度表信息融合方法設計

2.1 基于因子圖的MINS/BDS/磁羅盤/氣壓高度表信息融合結構

無人機飛行器MINS/BDS/磁羅盤/氣壓高度表多源組合導航系統，由BDS提供位置和速度信息、磁羅盤提供航向角信息、氣壓高度表提供高度信息， 將MINS捷聯解算得到的姿態、位置、速度信息作為參考信息與上述傳感器提供的數據基于因子圖模型進行融合[8]。將MINS、BDS、磁羅盤和氣壓高度表抽象為4個因子節點，分別用fMINS、fBDS、fCompass和fBaro表示。基于因子圖的MINS/GNSS/磁羅盤/氣壓高度表信息融合框架如圖2所示。

圖2 基于因子圖的MINS/BDS/磁羅盤/氣壓高度表信息融合框架

圖2中，xk代表系統的導航狀態，fMINS表示來自MINS的量測信息，與時刻tk和時刻tk+1的導航狀態相關；fBDS、fCompass和fBaro分別是來自于BDS、磁羅盤和氣壓高度表的量測信息。

1)MINS因子

無人機導航狀態的時間更新可用如下方程進行抽象描述

(3)

式中：fb和ωb分別表示加速度計和陀螺儀測量的比力和角速度。

將MINS的量測表示為zk?{fb,ωb}，其與連接的兩個導航狀態xk和xk+1相關，將式(3)離散化，可得

(4)

此時因子節點可表示為

(5)

式中，d[·]表示相應的代價函數。

2)BDS因子

將BDS量測方程表示為

(6)

式中：nBDS和hBDS分別為BDS量測函數和量測噪聲。由上述方程可將新加入的因子節點fBDS表示為

(7)

磁羅盤和氣壓高度表的量測因子可參考BDS因子表示方法，本文不再贅述。

基于因子圖架構的多源組合導航系統，能夠有效實現導航傳感器的即插即用，將不同頻率、不同誤差的傳感器合并起來，增強了導航系統的冗余性，使得導航系統能夠長時間為無人機進行導航。

2.2 組合導航系統數學模型

2.2.1 組合導航系統狀態方程

為了導航系統能夠快速進行數據融合，將MINS捷聯解算得到的基本導航參數誤差與慣性儀器參數誤差合并，得到系統的狀態向量

(8)

系統狀態方程為

(9)

式中：F為狀態轉移矩陣；G為噪聲轉移矩陣；W為狀態噪聲矢量。

2.2.2 組合導航系統量測方程

1)MINS/BDS位置、速度量測誤差方程

由于BDS能提供位置和速度的量測信息，則構造量測誤差方程為

(10)

式中：LINS、λINS、hINS分別表示MINS解算得到的緯度、經度和高度；LBDS、λBDS、hBDS分別表示BDS量測的緯度、經度和高度；VINS_E、VINS_N、VINS_U分別表示MINS解算得到的東向、北向和天向速度；VBDS_E、VBDS_N、VBDS_U分別表示BDS量測的東向、北向和天向速度；δLBDS、δλBDS、δhBDS和δVBDS_E、δVBDS_N、δVBDS_U分別表示位置和速度量測誤差；HBDS和VBDS分別表示BDS量測矩陣和噪聲矩陣。

2)MINS/磁羅盤航向角量測誤差方程

由于磁羅盤能提供航向角的量測信息，則構造量測誤差方程為

ZCompass=[ψINS-ψCompass]=[φU+δψCompass]=
HCompassX+VCompass

(11)

式中：ψINS表示MINS解算得到的航向角；ψCompass表示磁羅盤量測的航向角；δψCompass為航向角量測誤差；HCompass和VCompass分別表示磁羅盤量測矩陣和噪聲矩陣。

3)MINS/氣壓高度表高度量測誤差方程

由于氣壓高度表能提供高度的量測信息，則構造量測誤差方程為

ZBaro=[hINS-hBaro]=[δh+δhBaro]=
HBaroX+VBaro

(12)

式中：hBaro表示氣壓高度表量測的高度值；δhBaro為高度量測噪聲；HBaro和VBaro分別表示氣壓高度表量測矩陣和噪聲矩陣。

2.3 基于因子圖的MINS/BDS/磁羅盤/氣壓高度表信息融合算法

定義導航狀態集為Xk，當前時刻為tk，則由此定義的聯合概率密度函數為P(Xk|Zk)，其中Zk為時刻tk得到的量測信息，則最大后驗概率估計為

(13)

根據因子的定義可知，每個因子表示為一個獨立的項，因此

(14)

(15)

對于Gauss噪聲分布，式(15)可寫成[9]

(16)

式中，h(·)為與狀態變量相關的量測函數，zi為各類傳感器得到的實際量測值，且zi=hi(xk)+vi，vi為量測誤差。

根據上述推導得到矢量形式的表達式：

(17)

(18)

式中，J為誤差函數在初始值附近的雅克比矩陣。由式(17)和式(18)可知

(19)

(20)

(21)

M=JTR-1J

(22)

為使式(19)的值最小，即使其一階導數為0，可得：

MΔX*=-b

(23)

則狀態變量的解為

(24)

利用高斯——牛頓迭代法求最優解，通過不斷迭代使ΔX*=0，獲得增量ΔX[10]。

3 仿真校驗

為了驗證本文所述的基于因子圖的飛行器多源組合導航算法在實際工程應用中的可靠性和有效性，設計了半物理車載實驗。由NovAtel公司的SPAN-KVH1750高精度組合導航系統提供基準數據，采集BDS衛星接收機、MEMS慣導、磁羅盤和氣壓高度表的原始數據進行離線仿真，設備裝車效果如圖3所示。上述設備的主要性能指標如表1所示。

圖3 設備裝車效果圖

表1 試驗設備性能指標

跑車時長400s，數據通過上位機監控存入計算機，然后分別進行因子圖算法和傳統分散式聯邦濾波算法數據融合離線仿真，通過與高精度基準對比得出誤差，兩者的誤差對比曲線如圖4～6所示。

圖4 位置誤差對比曲線

圖5 速度誤差對比曲線

圖6 姿態誤差對比曲線

通過位置、速度和姿態誤差曲線對比可知，使用因子圖融合算法和分散式聯邦濾波融合算法的位置和速度精度相當，并且部分導航數據優于聯邦濾波。姿態中，使用分散式聯邦濾波算法時，MINS/BDS子濾波器能夠根據姿態失準角誤差方程估計出東向和北向失準角，以此修正俯仰角和滾轉角；而因子圖算法根據導航信息源進行數據估計，由于缺少輸出俯仰角和滾轉角信息的傳感器，其俯仰角和滾轉角均處于發散狀態。均方根誤差統計結果對比如表2所示。

上述結果表明，基于因子圖的多源組合導航算法能夠解決以MEMS慣導、BDS接收機、磁羅盤、氣壓高度表作為導航信息源的無人機飛行器的定位問題，由于因子圖算法能夠很好地利用不同輸出頻率的導航信息，因此其信息融合的效果更好，高度定位和航向角測姿精度較傳統分散式聯邦濾波高。另外因子圖融合算法能夠有效地實現導航傳感器的即插即用，對于外擴導航信息源具有良好的靈活性，因此可以通過外擴其它導航信息源如視覺導航傳感器等進行因子圖數據融合解決無人機飛行器俯仰角和滾轉角的測姿問題。

表2 均方根誤差結果對比

4 結論

本文詳細介紹了因子圖模型和信息融合設計方法，以無人機飛行器MINS/BDS/磁羅盤/氣壓高度表多源組合導航系統作為研究對象，通過車載采集導航信息進行離線仿真，將因子圖融合效果與傳統分散式聯邦濾波算法進行對比。實驗結果表明，經因子圖算法進行數據融合后，定位精度與聯邦濾波算法相當，并且部分導航信息精度優于聯邦濾波算法，驗證了因子圖融合算法應用于無人機多源導航系統的可行性。由于本文研究的對象缺少輸出俯仰角和滾轉角的導航傳感器，因此在無人機測姿問題中還有所欠缺，下一階段可利用因子圖算法的即插即用功能對導航傳感器進行擴展，增減傳感器只需相應增減方程中的量測值，借此提高無人機定位、測姿的完整性，可為未來實現無人機全源導航打下基礎。