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

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

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

0 引言

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

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

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

1 因子圖模型概述

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

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

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

(1)

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

設(shè)f(x1,x2,x3,x4,x5)是一個包含5個變量的函數(shù)，且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)可表示的因子圖結(jié)構(gòu)如圖1所示。

圖1 函數(shù)f(x1,x2,x3,x4,x5)對應(yīng)的因子圖結(jié)構(gòu)

圖中，圓形表示變量節(jié)點，正方形表示因子節(jié)點，每個因子節(jié)點和不同的變量節(jié)點產(chǎn)生關(guān)系，從而構(gòu)成不同的局部函數(shù)。

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

2.1 基于因子圖的MINS/BDS/磁羅盤/氣壓高度表信息融合結(jié)構(gòu)

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

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

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

1)MINS因子

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

(3)

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

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

(4)

此時因子節(jié)點可表示為

(5)

式中，d[·]表示相應(yīng)的代價函數(shù)。

2)BDS因子

將BDS量測方程表示為

(6)

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

(7)

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

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

2.2 組合導航系統(tǒng)數(shù)學模型

2.2.1 組合導航系統(tǒng)狀態(tài)方程

為了導航系統(tǒng)能夠快速進行數(shù)據(jù)融合，將MINS捷聯(lián)解算得到的基本導航參數(shù)誤差與慣性儀器參數(shù)誤差合并，得到系統(tǒng)的狀態(tài)向量

(8)

系統(tǒng)狀態(tài)方程為

(9)

式中：F為狀態(tài)轉(zhuǎn)移矩陣；G為噪聲轉(zhuǎn)移矩陣；W為狀態(tài)噪聲矢量。

2.2.2 組合導航系統(tǒng)量測方程

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

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

(10)

式中：LINS、λINS、hINS分別表示MINS解算得到的緯度、經(jīng)度和高度；LBDS、λBDS、hBDS分別表示BDS量測的緯度、經(jīng)度和高度；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/磁羅盤航向角量測誤差方程

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

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

(11)

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

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

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

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

(12)

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

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

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

(13)

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

(14)

(15)

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

(16)

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

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

(17)

(18)

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

(19)

(20)

(21)

M=JTR-1J

(22)

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

MΔX*=-b

(23)

則狀態(tài)變量的解為

(24)

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

3 仿真校驗

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

圖3 設(shè)備裝車效果圖

表1 試驗設(shè)備性能指標

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

圖4 位置誤差對比曲線

圖5 速度誤差對比曲線

圖6 姿態(tài)誤差對比曲線

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

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

表2 均方根誤差結(jié)果對比

4 結(jié)論

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