王慧哲，曾慶化，劉建業(yè)，陳維娜

(1.南京航空航天大學(xué)導(dǎo)航研究中心，南京211106；2.衛(wèi)星通信與導(dǎo)航協(xié)同創(chuàng)新中心，南京211106)

基于因子圖的無人機(jī)全源導(dǎo)航關(guān)鍵技術(shù)研究

王慧哲1，2，曾慶化1，2，劉建業(yè)1，2，陳維娜1，2

(1.南京航空航天大學(xué)導(dǎo)航研究中心，南京211106；2.衛(wèi)星通信與導(dǎo)航協(xié)同創(chuàng)新中心，南京211106)

針對DARPA提出的全源導(dǎo)航需要快速集成并重新配置任意導(dǎo)航傳感器的要求，結(jié)合民用無人機(jī)的任務(wù)需求，通過概率圖模型相關(guān)原理，對基于因子圖的無人機(jī)全源導(dǎo)航關(guān)鍵技術(shù)開展研究工作。采用因子圖對系統(tǒng)狀態(tài)更新過程進(jìn)行表示，實現(xiàn)系統(tǒng)狀態(tài)的遞推與更新，完成傳感器信息的數(shù)據(jù)綜合。仿真結(jié)果表明，該方法能在傳感器可用性時變的情況下，將不同傳感器的數(shù)據(jù)進(jìn)行有效融合，確保系統(tǒng)導(dǎo)航定位精度，使載體滿足不斷變化的任務(wù)需求與環(huán)境變化的要求。

無人機(jī)；因子圖；全源導(dǎo)航；信息融合

0 引言

無人機(jī)作為一種無人駕駛、能執(zhí)行多種任務(wù)的航空器，曾在戰(zhàn)場中顯示出強(qiáng)大的戰(zhàn)斗能力。隨著科技的發(fā)展，民用無人機(jī)在航空攝影、地面災(zāi)害評估、航空測繪、交通監(jiān)視、消防、人工增雨等民用領(lǐng)域同樣發(fā)揮著重要作用［1］。同樣，GPS的迅猛發(fā)展使其在人們的生活中扮演著重要角色。然而，由于GPS不可避免地具有衛(wèi)星導(dǎo)航系統(tǒng)固有的脆弱性，其提供的服務(wù)與應(yīng)用受到限制。例如，衛(wèi)星導(dǎo)航信號功率低，易被敵方壓制或欺騙，在高對抗作戰(zhàn)環(huán)境下難以保障；衛(wèi)星信號難以穿透地面和建筑物等密度較大的物質(zhì)，在都市、室內(nèi)、地下環(huán)境中信號衰減現(xiàn)象非常嚴(yán)重。為了解決衛(wèi)星導(dǎo)航系統(tǒng)的制約，在各種情況下提供精確可靠的導(dǎo)航信息，亟需開展不依賴于GPS的新導(dǎo)航技術(shù)［2］。2014年，DARPA表示正在發(fā)展5項不依賴GPS的導(dǎo)航技術(shù)項目，包括定位、導(dǎo)航與授時微技術(shù)，自適應(yīng)導(dǎo)航系統(tǒng)，量子輔助傳感與讀出，超速激光科學(xué)與工程，對抗環(huán)境下的空間、時間和方位信息項目。其中，自適應(yīng)導(dǎo)航系統(tǒng)包括精確慣性導(dǎo)航系統(tǒng)和全源定位與導(dǎo)航兩個子項目［3］。

本文以民用無人機(jī)為背景，結(jié)合全源導(dǎo)航需要快速集成并重新配置任意導(dǎo)航傳感器的要求，通過研究因子圖的相關(guān)理論與成果，對全源導(dǎo)航的關(guān)鍵技術(shù)展開研究。

1 全源導(dǎo)航

全源定位導(dǎo)航技術(shù)利用先進(jìn)算法，根據(jù)不同任務(wù)和作戰(zhàn)環(huán)境，將作戰(zhàn)平臺中的導(dǎo)航系統(tǒng)與雷達(dá)、紅外、聲學(xué)等多種傳感器信息進(jìn)行融合，提供低成本、高可靠的導(dǎo)航定位系統(tǒng)，解決GPS受阻環(huán)境下精確導(dǎo)航的需求，降低對衛(wèi)星導(dǎo)航的依賴［4］。

2010年11月，DARPA啟動了全源定位與導(dǎo)航(ASPN)項目，旨在開發(fā)一種在GPS受限情況下的廉價導(dǎo)航信息融合技術(shù)，可以與激光測距儀、相機(jī)和磁力計等多種傳感器實現(xiàn)即插即用的組合［5］。不過，值得注意的是，全源導(dǎo)航并不是要全面取代或替代衛(wèi)星導(dǎo)航系統(tǒng)，而是在無法使用衛(wèi)星導(dǎo)航系統(tǒng)的情況下，提供一種解決方案，彌補(bǔ)因衛(wèi)星導(dǎo)航系統(tǒng)固有脆弱性產(chǎn)生的服務(wù)或能力不足［6］。圖1為全源定位與導(dǎo)航的方案示意圖。

在復(fù)雜環(huán)境中，單一導(dǎo)航系統(tǒng)缺點明顯，如慣導(dǎo)系統(tǒng)誤差隨時間累積、GPS易受到敵方壓制等，不能滿足軍用領(lǐng)域?qū)煽啃浴⒎€(wěn)定性及高精度的需求。而全源導(dǎo)航可以根據(jù)環(huán)境、需求及任務(wù)要求的不同，靈活地接入或移除不同類型的傳感器，引入不同類型的量測量和特征數(shù)據(jù)庫，從而為不同環(huán)境下的用戶提供GPS受限情況下的精確定位。為了使多傳感器能夠快速融合和重新配置，本文結(jié)合因子圖理論，通過構(gòu)建系統(tǒng)框架，實現(xiàn)對任意傳感器信息的融合。

2 概率圖模型理論

概率圖模型是一種表達(dá)隨機(jī)變量聯(lián)合概率分布的圖模式，由圖和與之相關(guān)的參數(shù)兩部分組成。目前，常用的概率圖模型包括貝葉斯網(wǎng)絡(luò)(Bayesian Networks)、馬爾科夫網(wǎng)絡(luò)(Markov Networks)、因子圖(Factor Graphs)等［7］。因子圖是一個通用模型，具有很強(qiáng)的表達(dá)能力。

因子圖可以用作表達(dá)隨機(jī)變量聯(lián)合概率分布的概率圖模型，由變量節(jié)點、因子節(jié)點和連線組成。假設(shè)一個因子圖中的所有變量為x1、x2、…、xn，所有的局部函數(shù)為 f1(X1)、 f2(X2)、 …、fK(XK)。其中，Xk?｛x1，x2，…，xn｝，1≤k≤K，表示第k個局部函數(shù)fk(·)的自變量點集，K表示局部函數(shù)的總數(shù)，則因子圖所指定的聯(lián)合概率分布為：

其中，fk(Xk)(1≤k≤K)為非負(fù)有限函數(shù)，Z為歸一化因子，若聯(lián)合概率分布函數(shù)P(x1，x2，x3，x4)可表示為如下因子分解形式：

與其對應(yīng)的因子圖如圖2所示，其中空心圓形節(jié)點代表變量，實心圓形節(jié)點代表局部函數(shù)。

3 基于因子圖的多信息融合方法設(shè)計

隨著傳感器數(shù)量和種類的增加，不同的傳感器具有不同的更新頻率且在時間上存在不同步性，傳統(tǒng)的信息融合方法很難滿足系統(tǒng)日益復(fù)雜多變化的需求。本文結(jié)合因子圖的原理，提出了一種基于因子圖的多信息融合方法，構(gòu)建因子圖模型，獲取不同傳感器測量信息后，通過定義的系統(tǒng)變量節(jié)點以及因子節(jié)點，進(jìn)行因子圖的拓展，對應(yīng)不同傳感器的因子節(jié)點實現(xiàn)系統(tǒng)狀態(tài)的遞推和更新，從而完成多傳感器的信息融合。

因子圖是一種二分圖G＝(F，X，E)，包括兩類節(jié)點——因子節(jié)點fi∈F和變量節(jié)點xj∈X，其中，eij∈E表示因子節(jié)點與變量節(jié)點之間的邊。因子圖G定義函數(shù)f(X)的因式分解為f(X)＝∏ifi(Xi)，其中，Xi是所有變量xj的集合，與因子fi由一條邊相連［8］。將因子圖的概念引入導(dǎo)航信息融合中，變量節(jié)點表示組合導(dǎo)航狀態(tài)變量，因子節(jié)點表示組合導(dǎo)航的量測更新過程，接收到來自傳感器的量測信息后，利用因子節(jié)點進(jìn)行狀態(tài)的遞推估計。

圖3為信息融合的因子圖框架，其中涉及的傳感器包括IMU、GPS、氣壓高度計、光流傳感器、磁航向傳感器、星敏感器。

圖3中，x為變量節(jié)點。設(shè)定系統(tǒng)的狀態(tài)變量為變量節(jié)點：

變量節(jié)點包括［9］平臺誤差角?E、?N、?U，速度誤差δνE、δνN、δνU，位置誤差δL、δλ、δh，陀螺常值漂移誤差εbx、εby、εbz，陀螺1階馬爾可夫漂移誤差εrx、εry、εrz和加速度計零偏▽x、▽y、▽z。

在導(dǎo)航框架中，定義量測模型h(·)可以根據(jù)給定的狀態(tài)估計來預(yù)測傳感器的量測值。因子節(jié)點獲取預(yù)測的量測信息和實際量測信息的差值，構(gòu)建相應(yīng)的指標(biāo)函數(shù)從而獲取狀態(tài)變量的估計，基于高斯白噪聲模型假設(shè)，一個量測因子節(jié)點可以用如下公式表示：

式中，hi(Xi)表示量測模型，是與狀態(tài)變量相關(guān)的函數(shù)；zi表示實際的觀測信息；d(·)表示代價函數(shù)。

接收來自IMU的觀測信息后，定義因子節(jié)點fIMU。fIMU連接不同時刻tk和tk＋1的兩個變量節(jié)點，即導(dǎo)航狀態(tài)xk和xk＋1。根據(jù)系統(tǒng)的狀態(tài)方程進(jìn)行狀態(tài)更新和變量節(jié)點擴(kuò)增：

其中，a、w分別代表加速度計和陀螺的輸出，F(xiàn)(·)表示系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)矩陣。那么因子節(jié)點IMU可以表示為：式(6)中所涉及到的未知向量都代表了因子圖中的變量節(jié)點，其中，IMU因子是連接變量的因子節(jié)點［10］。

在tl時刻接收到GPS的觀測信息后，定義因子節(jié)點fGPS。fGPS連接至變量節(jié)點xl，GPS的量測方程如下：

式中，zGPS是GPS的觀測信息，hGPS是觀測函數(shù)，vGPS是觀測噪聲。

因此，因子節(jié)點fGPS可以表示為：

選定因子節(jié)點的代價函數(shù)如下：

同理，在其他時刻接收到氣壓高度計、光流傳感器、磁航向傳感器、以及星敏感器的觀測信息后，定義因子節(jié)點fbaro、fflow、fmag、fstar拓展因子圖，根據(jù)傳感器的觀測方程以及相應(yīng)的代價函數(shù)進(jìn)行變量節(jié)點的狀態(tài)更新?；谝蜃訄D的多傳感器信息融合方法，能方便地對來自異步傳感器的數(shù)據(jù)進(jìn)行處理，接收到傳感器的輸出數(shù)據(jù)后，擴(kuò)充因子圖節(jié)點，根據(jù)系統(tǒng)的狀態(tài)方程和量測方程快速有效地進(jìn)行系統(tǒng)狀態(tài)的更新，實現(xiàn)多傳感器的數(shù)據(jù)綜合處理。

4 仿真驗證

采用仿真手段對文中提出的方法進(jìn)行分析驗證，本文以Matlab為試驗平臺，對仿真方法進(jìn)行了設(shè)計，其中包括對飛行器運行軌跡的設(shè)計、模擬不同傳感器的數(shù)據(jù)輸出以及對文中提出的算法仿真驗證。

4.1 仿真條件和方法

仿真中模擬裝備多種傳感器的無人飛行器的飛行過程，設(shè)定飛行器的運行軌跡。模擬飛行器搭載的傳感器，包括以下幾種：100Hz的MEMS、1Hz的GPS、5Hz的氣壓高度計、10Hz的光流傳感器、10Hz的磁航向傳感器、8Hz的星敏感器。根據(jù)上節(jié)提出的方法對這6種不同更新率的傳感器進(jìn)行因子圖構(gòu)造，對輸出數(shù)據(jù)進(jìn)行綜合處理，從而實現(xiàn)信息融合。

飛行器的初始位置坐標(biāo)設(shè)置為東經(jīng)106.491°、北緯29.528°、高度300m。給定的飛行器初始姿態(tài)角為：橫滾角＝0°、俯仰角＝0°、航向角＝90°。設(shè)置陀螺和加速度計的誤差分別為：陀螺常值漂移誤差為0.03(°)／h帶有隨機(jī)白噪聲、加速度計的常值偏移量為0.1mg帶有隨機(jī)白噪聲，慣導(dǎo)系統(tǒng)的解算周期為0.02s，仿真總時長為1800s。

考慮要驗證所提方法在傳感器可用性時變情況下的可靠性，對載體飛行過程中的傳感器可用性，即故障情況進(jìn)行設(shè)置。由于全源導(dǎo)航的初衷是開發(fā)一種在GPS受限情況下的定位導(dǎo)航系統(tǒng)，因此，假設(shè)GPS受阻，設(shè)置其在500s～1000s時間段內(nèi)不可用；星敏感器使用條件也較為苛刻，所以設(shè)置其在800s～1200s不可用；光流傳感器設(shè)置為1300s～1400s不可用。通過上述故障情況，測試因子圖方法的穩(wěn)定性、可靠性。

4.2 仿真結(jié)果與分析

本次仿真中設(shè)計的飛行器運行軌跡如圖4所示，運行軌跡仿真通過抬頭、爬升、平飛、傾斜、轉(zhuǎn)彎、俯沖等機(jī)動動作，以及在經(jīng)度、緯度、高度的大幅度變化，模擬飛行器真實的飛行狀況。

以上述運行軌跡及3.1節(jié)中的初始條件為基準(zhǔn)進(jìn)行仿真，分別得到組合導(dǎo)航系統(tǒng)的位置誤差曲線、速度誤差曲線和姿態(tài)誤差曲線，如圖5所示。

由圖5可知，飛行器經(jīng)過1800s飛行，對于位置誤差而言，經(jīng)度誤差在5m以內(nèi)，緯度誤差在10m以內(nèi)，高度誤差在5m以內(nèi)；東向速度誤差在0.2m／s以內(nèi)，北向速度誤差在0.2m／s以內(nèi)，天向速度誤差在0.5m／s以內(nèi)；對于姿態(tài)誤差而言，橫滾角誤差在0.005°以內(nèi)，俯仰角誤差在0.002°以內(nèi)，航向角在0.005°以內(nèi)?？傮w來說，在飛行器機(jī)動過程中，可用傳感器變更的情況下，導(dǎo)航性能可以得到有效保障。因此，本文提出的方法能很好地適應(yīng)傳感器的可用性變化，根據(jù)實時獲取的測量數(shù)據(jù)，快速有效地進(jìn)行信息融合，從而確保載體的定位導(dǎo)航精度。

5 結(jié)論

針對載體多傳感器信息融合的應(yīng)用需求，為了實現(xiàn)多傳感器數(shù)據(jù)的快速集成和組合架構(gòu)配置，本文結(jié)合概率圖模型理論，提出了一種基于因子圖的多信息融合方法，可用于全源導(dǎo)航的相關(guān)領(lǐng)域。采用因子圖節(jié)點描述系統(tǒng)的狀態(tài)遞推和更新過程，基于因子圖模型實現(xiàn)傳感器信息的即插即用，綜合來自不同傳感器的數(shù)據(jù)，最后進(jìn)行仿真分析。仿真結(jié)果表明，本文所提方法能對異步異構(gòu)傳感器的數(shù)據(jù)進(jìn)行有效融合，并適應(yīng)傳感器在運行過程中可用性發(fā)生變化的情況，使載體滿足不斷變化的任務(wù)需求與環(huán)境變化的要求。

[1] Xiang H，Tian L.Development of a low?cost agricultural remote sensing system based on an autonomous unmanned aerial vehicle(UAV)[J].Biosystems Engineering，2011，108(2):174?190.

[2] Williams P，Crump M.All?source navigation for enhancing UAV operations in GPS?denied environments[EB／OL].ht?tp:／／www.icas.org／ICAS＿ARCHIVE／ICAS2012／PA?PERS／130.PDF，2012.

[3] Wang Y，Meng Y S，Su Z，et al.Study on space?based all source navigation technology[C].China Satellite Navigation Conference(CSNC)2015 Proceedings:Volume II.Springer Berlin Heidelberg，2015:113?123.

[4] 劉鈍，甄衛(wèi)民，張風(fēng)國，等.PNT系統(tǒng)體系結(jié)構(gòu)與PNT新技術(shù)發(fā)展研究[J].全球定位系統(tǒng)，2015，40(2):48?52. LIU Dun，ZHEN Wei?min，ZHANG Feng?guo，et al.PNT system architecture and PNT technology development re?search[J].GNSS World of China，2015，40(2):48?52.

[5] Fisher K A，Raquet J F.Precision position，navigation，and timing without the global positioning system.[J].Air ＆Space Power Journal，2011，25(2):24?30.

[6] 文蘇麗，張國慶.美國GPS受限條件下導(dǎo)航定位技術(shù)的新發(fā)展[J].戰(zhàn)術(shù)導(dǎo)彈技術(shù)，2014(6):81?87. WEN Su?li，ZHANG Guo?qing.The technology progress of PNT in GPS limited conditions[J].Tactical Missile Technology，2014(6):81?87.

[7] Loeliger H A，Dauwels J，Hu J，et al.The factor graph ap?proach to model?based signal processing[C].Proceedings of the IEEE，2007，95(6):1295?1322.

[8] Ta D N，Kobilarov M，DellaertF. A factorgraph approach to estimation and model predictive control on un?manned aerial vehicles[C].International Conference on Unmanned Aircraft Systems，2014:181?188.

[9] 柳敏，賴際舟，黃凱，等.基于加權(quán)奇偶矢量的機(jī)載自主完好性監(jiān)測算法[J].中國慣性技術(shù)學(xué)報，2015，23 (1):43?48. LIU Min，LAI Ji?zhou，HUANG Kai，et al.Aircraft au?tonomous integrity monitoring algorithm based on weighted parity vector[J].Journal of Chinese Inertial Technology，2015，23(1):43?48.

[10] Indelman V，Williams S，Kaess M，et al.Information fu?sion in navigation systems via factor graph based incre?mental smoothing[J].Robotics＆Autonomous Systems，2013，61(8):721?738.

Research on the Key Technology of UAV of All Source Position Navigation Based on Factor Graph

WANG Hui?zhe1，2，ZENG Qing?hua1，2，LIU Jian?ye1，2，CHEN Wei?na1，2
(1.Navigation Research Center，Nanjing University of Aeronautics＆Astronautics，Nanjing 211106; 2.Satellite Communication and Navigation Collaborative Innovation Center，Nanjing 211106)

The all source position navigation proposed by DARPA needs to integrate and reconfigure any navigation sensors quickly.According to the theory of probability graph model and the mission requirements of the civil unmanned aer?ial vehicle，the research on the key technology of all source position navigation based on factor graph was proposed.Factor nodes are used to express system state and measurement update procedure，and system state recursion and updates can be realized，and plug and play sensor information data synthesis can be completed.The simulation results show that the meth?od can fuse the data of different sensors effectively，and make sure that the system navigation and positioning accuracy is guaranteed，so that the vehicle can satisfy changing task needs and environmental changes.

unmanned aerial vehicle(UAV);factor graph;all source position navigation;information fusion

V249.32

A

1674?5558(2017)01?01284

10.3969／j.issn.1674?5558.2017.02.001

王慧哲，女，碩士，研究方向為慣性技術(shù)與多信息融合組合導(dǎo)航技術(shù)。

2016?06?17

國家自然科學(xué)基金(編號：61533008，61328301)