朱倚嫻　程向紅　周　玲,2　劉　全

(1東南大學(xué)微慣性?xún)x表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室， 南京 210096)(2運(yùn)城學(xué)院物理與電子工程系， 運(yùn)城 044000)

為提高導(dǎo)航系統(tǒng)定位精度和容錯(cuò)性能，多傳感器組合導(dǎo)航是目前最常采用的有效策略.實(shí)現(xiàn)多傳感器信息融合的Kalman濾波分為集中式和分布式2種結(jié)構(gòu).1988年Carlson[1]提出的聯(lián)邦濾波器作為最具代表性的分布式融合算法，設(shè)計(jì)靈活、計(jì)算量小、容錯(cuò)性能好，被廣泛應(yīng)用于組合導(dǎo)航系統(tǒng)中.多傳感器的同步數(shù)據(jù)采集通過(guò)時(shí)間同步裝置[2-3]實(shí)現(xiàn).在現(xiàn)有的多傳感器信息融合研究[4-6]中，大多假設(shè)各個(gè)導(dǎo)航傳感器同步地將數(shù)據(jù)傳遞給融合中心.然而，在實(shí)際系統(tǒng)中，由于信號(hào)變化速率相差較大、檢測(cè)裝置采樣周期不同等原因使各個(gè)傳感器的采樣速率不盡相同[7]，且由于傳感器固有延遲和通信延遲的不同[8]，使融合中心接收到的數(shù)據(jù)也不同步.因此，研究針對(duì)異步多傳感器的信息融合算法對(duì)提高組合導(dǎo)航系統(tǒng)定位精度具有重要意義.

赫飛等[9]針對(duì)SINS/GPS/捷聯(lián)星光跟蹤儀/高度表組合導(dǎo)航系統(tǒng)各傳感器采樣率不同的問(wèn)題，提出了基于狀態(tài)方程多尺度變換的信息融合算法，將最高采樣率下的系統(tǒng)狀態(tài)方程分解到不同的尺度上，得到各尺度上的狀態(tài)方程及對(duì)應(yīng)的量測(cè)方程.劉磊等[10]利用數(shù)據(jù)塊分析技術(shù)和小波變換技術(shù)，將狀態(tài)塊向量、量測(cè)塊向量在粗尺度上分解，并在不同尺度上進(jìn)行相應(yīng)的Kalman濾波，得到各尺度上平滑信息的最優(yōu)估計(jì)值，最終利用異步貫序?yàn)V波建立基于全局的最優(yōu)估計(jì)值，但該方法要求不同傳感器間的采樣率必須為2的整數(shù)倍.此外上述文獻(xiàn)均針對(duì)多傳感器采樣率不同的問(wèn)題提出了解決方案，但未考慮傳感器量測(cè)延遲，即數(shù)據(jù)不同步問(wèn)題.Safari等[11]針對(duì)多傳感器異步量測(cè)問(wèn)題，提出一種基于神經(jīng)網(wǎng)絡(luò)的信息融合策略，建立基于狀態(tài)分塊的系統(tǒng)模型，并利用神經(jīng)網(wǎng)絡(luò)融合各子濾波器的狀態(tài)估計(jì)值.Yang等[12]提出基于UKF的異步數(shù)據(jù)融合算法，通過(guò)檢測(cè)每個(gè)融合期內(nèi)無(wú)線(xiàn)傳感器網(wǎng)絡(luò)的量測(cè)值，建立基于時(shí)間序列的偽量測(cè)方程.Lin等[13]針對(duì)含丟包的異步量測(cè)問(wèn)題，利用射影理論推導(dǎo)線(xiàn)性最小方差意義下的最優(yōu)局部狀態(tài)估計(jì)值，并在此基礎(chǔ)上計(jì)算局部狀態(tài)估計(jì)誤差間的協(xié)方差矩陣，最終通過(guò)矩陣加權(quán)算法得到分布式最優(yōu)融合狀態(tài)估計(jì)值.劉建業(yè)等[14]考慮組合導(dǎo)航系統(tǒng)中量測(cè)不等間隔頻率輸出和量測(cè)信息滯后的問(wèn)題，設(shè)計(jì)了非等間隔Kalman濾波算法，利用數(shù)組記錄某一時(shí)刻的慣性導(dǎo)航系統(tǒng)(inertial navigation system, INS)狀態(tài)信息，當(dāng)對(duì)應(yīng)時(shí)刻的合成孔徑雷達(dá) (synthetic aperture radar, SAR)量測(cè)信息到達(dá)時(shí)，將其與數(shù)組中信息進(jìn)行融合，再利用Kalman濾波時(shí)間更新遞推得到當(dāng)前時(shí)刻的狀態(tài)估計(jì)值.

本文針對(duì)組合導(dǎo)航系統(tǒng)中多個(gè)傳感器采樣頻率不同且存在量測(cè)滯后的問(wèn)題，提出了一種基于多尺度數(shù)據(jù)分塊的組合導(dǎo)航信息融合算法.建立最高采樣率下(即最細(xì)尺度上)的系統(tǒng)模型，通過(guò)狀態(tài)和觀(guān)測(cè)的分塊與擴(kuò)維，得到多尺度系統(tǒng)模型.利用各尺度上的觀(guān)測(cè)信息分別進(jìn)行Kalman濾波，經(jīng)數(shù)據(jù)融合最終得到基于全局的狀態(tài)估計(jì)值.通過(guò)仿真，驗(yàn)證了本文所提信息融合算法可有效提高異步多傳感器組合導(dǎo)航系統(tǒng)的定位精度.

1　異步多傳感器組合導(dǎo)航系統(tǒng)

1.1　系統(tǒng)描述

設(shè)有N個(gè)導(dǎo)航傳感器(i=1,2,…,N)以不同采樣率Si進(jìn)行觀(guān)測(cè)，且彼此間的采樣是異步的，采樣率間的關(guān)系滿(mǎn)足

Sj+1=njSj1≤j≤N-1

(1)

式中，nj為已知正整數(shù).

根據(jù)多尺度理論，i(i=1,2,…,N)既表示傳感器，也表示尺度.具有最低采樣率S1的第i=1個(gè)傳感器對(duì)應(yīng)于最粗尺度，具有最高采樣率SN的第i=N個(gè)傳感器對(duì)應(yīng)于最細(xì)尺度，傳感器i=2,3,…,N-1的采樣率介于兩者之間，依次升高.在最高采樣率下(即最細(xì)尺度上)建立狀態(tài)方程，多傳感器組合導(dǎo)航系統(tǒng)可描述為

x(N,k+1)=Φ(N,k)x(N,k)+w(N,k)

(2)

z(i,k)=H(i,k)x(i,k)+v(i,k)
i=1,2,…,N-1

(3)

式中，x(N,k)∈Rn×l為最細(xì)尺度上k時(shí)刻的狀態(tài)變量；Φ(N,k)為系統(tǒng)一步轉(zhuǎn)移矩陣；系統(tǒng)噪聲w(N,k)∈Rn×l為零均值高斯白噪聲,Q(N) 為其協(xié)方差；z(i,k)∈Rqi×l(qi≤n)為第i個(gè)傳感器在采樣率Si下獲得的第k個(gè)觀(guān)測(cè)值；H(i,k)∈Rqi×n為觀(guān)測(cè)矩陣；觀(guān)測(cè)噪聲v(i,k)∈Rqi×l為零均值高斯白噪聲，其協(xié)方差為R(i)，且不同采樣率下的觀(guān)測(cè)噪聲互不相關(guān).初始狀態(tài)向量x(N,0)為一隨機(jī)變量，且假設(shè)x(N,0)，w(N,k)和v(i,k)彼此間統(tǒng)計(jì)獨(dú)立.

為更清晰地表達(dá)各傳感器、尺度和時(shí)間的對(duì)應(yīng)關(guān)系，圖1給出了具有不同采樣率的3個(gè)導(dǎo)航傳感

圖1　多導(dǎo)航傳感器異步采樣示意圖

器異步采樣示意圖.其中，橫坐標(biāo)表示時(shí)刻，縱坐標(biāo)表示傳感器(或尺度)，N=3，n1=3，n2=2.具有最高采樣率的傳感器N采樣均勻，且其采樣點(diǎn)與時(shí)刻一一對(duì)應(yīng)，傳感器i(i=1,2,…,N-1)的采樣可以是非均勻的，即各傳感器以多速率進(jìn)行異步采樣.

1.2　多尺度系統(tǒng)模型

x(N,kM+1)=Φ(N,kM)x(N,kM)+w(N,kM)

(4)

x(N,kM+2)=Φ(N,kM+1)Φ(N,kM)x(N,kM)+

Φ(N,kM+1)w(N,kM)+w(N,kM+1)

(5)

由數(shù)據(jù)歸納法，可得

(6)

由式(4)～(6)，有

XN(k+1)=ΦN(k)XN(k)+WN(k)

(7)

(8)

(9)

(10)

(11)

根據(jù)1.1節(jié)所述w(N,k)均值為零、協(xié)方差為Q(N)，以及式(10)、(11)，可得

E{WN(k)}=0

(12)

(13)

(14)

現(xiàn)假設(shè)x(i,k)是最細(xì)尺度上的狀態(tài)x(N,k)在尺度i上的平滑信息[15]，即

(15)

根據(jù)式(15)，有

x(i,(k-1)Mi+1)=

(16)

x(i,(k-1)Mi+j)=

(17)

根據(jù)式(3)、(16)和(17)，有

(18)

Zi(k)=Hi(k)XN(k)+Vi(k)

(19)

(20)

(21)

(22)

根據(jù)上節(jié)所述v(i,k) 均值為零、協(xié)方差為R(i)，且與w(N,l)不相關(guān)，以及式(10)、(22)，得

E{Vi(k)}=0

(23)

(24)

(25)

Ri(k)=diag{R(i,(k-1)Mi+1),

R(i,(k-1)Mi+2),…,R(i,kMi)}

(26)

綜上，尺度i(i=1,2,…,N-1)上的系統(tǒng)模型為式(7)和(19).

2　信息融合估計(jì)算法

通常組合導(dǎo)航系統(tǒng)中具有最高采樣率且采樣均勻的傳感器為捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)，即最細(xì)尺度N上的傳感器為SINS.其余各導(dǎo)航子傳感器可為多普勒測(cè)速儀(DVL)、里程計(jì)(OD)、全球定位系統(tǒng)(GPS)、地形輔助導(dǎo)航系統(tǒng)(TAN)等.

采用分布式的聯(lián)邦濾波實(shí)現(xiàn)各導(dǎo)航傳感器信息融合，即最細(xì)尺度N上的SINS分別與尺度i(i=1,2,…,N-1)上的各導(dǎo)航子傳感器構(gòu)成局部濾波器，則式(19)中第i個(gè)局部濾波器(即尺度i上)觀(guān)測(cè)矩陣Zi(k)的第l個(gè)元素為

(27)

式中，1≤l≤Mi；D(i,*)為尺度i上導(dǎo)航傳感器的量測(cè)信息；D(N,*)為相應(yīng)的SINS解算信息.

(28)

(29)

相應(yīng)的估計(jì)誤差協(xié)方差矩陣為

(30)

綜上，基于多尺度數(shù)據(jù)分塊的組合導(dǎo)航信息融合算法流程圖如圖2所示.

圖2　基于多尺度數(shù)據(jù)分塊的組合導(dǎo)航信息融合算法流程圖

3　仿真及分析

為驗(yàn)證本文所提信息融合算法的有效性，將所提方法用于SINS/DVL/TAN組合導(dǎo)航系統(tǒng)，利用Matlab進(jìn)行相應(yīng)的仿真試驗(yàn).仿真中各傳感器誤差參數(shù)設(shè)置如表1所示.仿真時(shí)間為25 min，圖3給出了仿真中載體的航行軌跡.載體從A點(diǎn)航行至B點(diǎn)，航行過(guò)程中包括左拐彎、右拐彎、加速、減速、勻速行駛等機(jī)動(dòng).載體在水平面航行，圖4給出了仿真中載體航行速度和航向角.

表1　傳感器誤差參數(shù)

圖3　載體航行軌跡

(a) 航行速度

(b) 航向角

為模擬多傳感器多采樣率情況，設(shè)置SINS解算更新周期為0.02 s，DVL量測(cè)更新周期為0.5 s，TAN量測(cè)更新周期為1 s.各局部濾波器和主濾波器濾波更新周期均為1 s，主濾波器將局部濾波器輸出信息進(jìn)行最優(yōu)融合.假設(shè)具有最高采樣率的SINS采樣均勻，為模擬傳感器量測(cè)延遲情況，對(duì)DVL和TAN量測(cè)信息加入隨機(jī)延時(shí).

對(duì)于相同的SINS，DVL，TAN仿真數(shù)據(jù)，采用本文所提算法與非等間隔Kalman濾波法[14](簡(jiǎn)稱(chēng)算法1)進(jìn)行信息融合比較.仿真中，本文所提算法將數(shù)據(jù)進(jìn)行分塊，且每個(gè)數(shù)據(jù)塊均包含有3種導(dǎo)航傳感器的量測(cè)信息，以數(shù)據(jù)塊為單位進(jìn)行濾波融合.算法1將與DVL，TAN量測(cè)對(duì)應(yīng)的SINS解算結(jié)果存儲(chǔ)，當(dāng)延遲的DVL或TAN量測(cè)信息到達(dá)時(shí)，將其與存儲(chǔ)的SINS解算結(jié)果相減，并將其作為觀(guān)測(cè)量進(jìn)行濾波，得到狀態(tài)估計(jì)，并將此狀態(tài)估計(jì)通過(guò)Kalman時(shí)間更新遞推，得到當(dāng)前時(shí)刻的狀態(tài)估計(jì)值.仿真結(jié)果分別如圖5和圖6所示.表2給出了2種融合算法導(dǎo)航結(jié)果的數(shù)據(jù)統(tǒng)計(jì)值.

綜合分析圖5、圖6和表2可得，本文算法與算法1相比，北向速度最大誤差減小24.1%，緯度最大誤差減小23.8%，東向速度最大誤差和經(jīng)度最大誤差均略有減小.在多導(dǎo)航傳感器異步采樣的情形下，能有效提高濾波器的估計(jì)精度，從而有利于提高導(dǎo)航系統(tǒng)的定位精度.

(a) 速度誤差

(b) 位置誤差

(a) 速度誤差

(b) 航向角

算法誤差項(xiàng)均方根最大絕對(duì)值本文算法東向速度誤差/(m·s-1)0．18780．6487北向速度誤差/(m·s-1)0．19970．6406緯度誤差/m4．565514．21經(jīng)度誤差/m4．979916．21算法1東向速度誤差/(m·s-1)0．19770．6552北向速度誤差/(m·s-1)0．22570．8439緯度誤差/m6．586121．27經(jīng)度誤差/m5．323716．53

4　結(jié)論

1) 以多傳感器異步采樣組合導(dǎo)航系統(tǒng)為對(duì)象，在導(dǎo)航傳感器量測(cè)具有隨機(jī)延遲的情況下，設(shè)計(jì)了一種基于多尺度數(shù)據(jù)分塊的組合導(dǎo)航信息融合算法.將所提算法用于SINS/DVL/TAN組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗(yàn)證.通過(guò)處理包含所有傳感器量測(cè)信息的數(shù)據(jù)塊，有效解決了量測(cè)延遲問(wèn)題.

2) 所提算法與非等間隔Kalman濾波算法相比，進(jìn)一步減小了速度誤差和位置誤差，從而有利于提高系統(tǒng)導(dǎo)航精度.

但該算法也存在不足，通過(guò)狀態(tài)和觀(guān)測(cè)的分塊建立多尺度系統(tǒng)模型的過(guò)程中，狀態(tài)向量和觀(guān)測(cè)向量的維數(shù)有所擴(kuò)展，這將增加計(jì)算量和計(jì)算復(fù)雜度，因此如何進(jìn)一步優(yōu)化算法有待研究.

參考文獻(xiàn)(References)

[1] Carlson N A. Federated filter for fault-tolerant integrated navigation systems[C]//IEEEPositionLocationandNavigationSymposium. New York, USA, 1988: 110-119. DOI:10.1109/plans.1988.195473.

[2] 張輝, 牛小驥, 郭文飛，等. 一種簡(jiǎn)易通用的敲擊觸發(fā)式慣性導(dǎo)航系統(tǒng)時(shí)間同步裝置設(shè)計(jì)[J]. 傳感技術(shù)學(xué)報(bào), 2015, 28(1): 13-18.

Zhang Hui, Niu Xiaoji, Guo Wenfei,et al. A universal INS time synchronization solution based on punch triggering[J].ChineseJournalofSensorsandActuator, 2015,28(1): 13-18. (in Chinese)

[3] Gabrlik P, Jelinek A, Janata P. Precise multi-sensor georeferencing system for micro UAVs[C]//14thIFACConferenceonProgrammableDevicesandEmbeddedSystems. Brno, Czech Republic, 2016: 170-175.

[4] Ning X L, Gui M Z, Xu Y Z,et al. INS/VNS/CNS integrated navigation method for planetary rovers[J].AerospaceScienceandTechnology, 2016,48: 102-114. DOI:10.1016/j.ast.2015.11.002.

[5] Yang C S, Wang Q. Low cost integrated navigation system for unmanned vessel[J].PolishMaritimeResearch, 2017,24(3): 110-115. DOI:10.1515/pomr-2017-0112.

[6] Li Z K, Wang J, Li B H, et al. GPS/INS/odometer integrated system using fuzzy neural network for land vehicle navigation applications[J].JournalofNavigaiton, 2014,67(6): 967-983. DOI:10.1017/s0373463314000307.

[7] Ni Z F, Sunderrajan S, Rahimi A, et al. Distributed particle filter tracking with online multiple instance learning in a camera sensor network[C]//IEEEInternationalConferenceonImageProcessing. Hong Kong, China, 2010: 37-40.

[8] Nilsson J O, Handel P. Time synchronization and temporal ordering of asynchronous sensor measurements of a multi-sensor navigation system[C]//IEEEPosition,LocationandNavigationSymposium. Indian Wells, CA, USA, 2010: 897-902. DOI:10.1109/plans.2010.5507193.

[9] 赫飛, 丁姚, 張永偉. SINS/GPS/SST/高度表組合導(dǎo)航系統(tǒng)異步融合算法[J]. 海軍航空工程學(xué)院學(xué)報(bào), 2015, 30(1): 17-22.

He Fei, Ding Yao, Zhang Yongwei. Fusion-algorithm of asynchronous in the SINS/GPS/SST/altimeter integrated navigation system[J].JournalofNavalAeronauticalandAstronauticalUniversity, 2015,30(1): 17-22. (in Chinese)

[10] 劉磊, 林雪原, 周旻. 多傳感器組合導(dǎo)航系統(tǒng)的多尺度異步信息融合算法[J]. 海軍航空工程學(xué)院學(xué)報(bào), 2012, 27(3): 276-280.

Liu Lei, Lin Xueyuan, Zhou Min. Multi-scale asynchronous information fusion algorithm of multi-sensor integrated navigation system[J].JournalofNavalAeronauticalandAstronauticalUniversity, 2012,27(3): 276-280. (in Chinese)

[11] Safari S, Shabani F, Simon D. Multirate multisensor data fusion for linear systems using Kalman filters and a neural network[J].AerospaceScienceandTechnology, 2014,39: 465-471. DOI:10.1016/j.ast.2014.06.005.

[12] Yang H, Li W, Luo C M. Integrated SINS/WSN positioning system for indoor mobile target using novel asynchronous data fusion method[J].JournalofSensors, 2017，2017: 7879198-1-7879198-12. DOI:10.1155/2017/7879198.

[13] Lin H L, Sun S L. Distributed fusion estimator for multi-sensor asynchronous sampling systems with missing measurements[J].IETSignalProcessing, 2016,10(7): 724-731. DOI:10.1049/iet-spr.2016.0017.

[14] 劉建業(yè), 熊智, 段方. 考慮量測(cè)滯后的INS/SAR組合導(dǎo)航非等間隔濾波算法研究[J]. 宇航學(xué)報(bào), 2004, 25(6): 626-631.

Liu Jianye, Xiong Zhi, Duan Fang. Processing the measurement delay INS/SAR integrated navigation in-coordinate interval filtering algorithm study[J].JournalofAstronautics, 2004,25(6): 626-631. (in Chinese)

[15] Yan L P, Liu B S, Zhou D H. The modeling and estimation of asynchronous multirate multisensor dynamic systems[J].AerospaceScienceandTechnology, 2006,10(1): 63-71. DOI:10.1016/j.ast.2005.09.001.