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

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

0 引言

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

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

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

1 因子圖模型概述

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

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

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

(1)

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

設(shè)f(x1,x2,x3,x4,x5)是一個(gè)包含5個(gè)變量的函數(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é)點(diǎn)，正方形表示因子節(jié)點(diǎn)，每個(gè)因子節(jié)點(diǎn)和不同的變量節(jié)點(diǎn)產(chǎn)生關(guān)系，從而構(gòu)成不同的局部函數(shù)。

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

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

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

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

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

1)MINS因子

無人機(jī)導(dǎo)航狀態(tài)的時(shí)間更新可用如下方程進(jìn)行抽象描述

(3)

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

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

(4)

此時(shí)因子節(jié)點(diǎn)可表示為

(5)

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

2)BDS因子

將BDS量測方程表示為

(6)

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

(7)

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

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

2.2 組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

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

為了導(dǎo)航系統(tǒng)能夠快速進(jìn)行數(shù)據(jù)融合，將MINS捷聯(lián)解算得到的基本導(dǎo)航參數(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 組合導(dǎo)航系統(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/磁羅盤/氣壓高度表信息融合算法

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

(13)

根據(jù)因子的定義可知，每個(gè)因子表示為一個(gè)獨(dú)立的項(xiàng)，因此

(14)

(15)

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

(16)

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

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

(17)

(18)

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

(19)

(20)

(21)

M=JTR-1J

(22)

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

MΔX*=-b

(23)

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

(24)

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

3 仿真校驗(yàn)

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

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

表1 試驗(yàn)設(shè)備性能指標(biāo)

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

圖4 位置誤差對比曲線

圖5 速度誤差對比曲線

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

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

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

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

4 結(jié)論

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