組合導(dǎo)航系統(tǒng)中異步多傳感器信息融合算法

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

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

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

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

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

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

1.1　系統(tǒng)描述

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

Sj+1=njSj1≤j≤N-1

(1)

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

根據(jù)多尺度理論，i(i=1,2,…,N)既表示傳感器，也表示尺度.具有最低采樣率S1的第i=1個傳感器對應(yīng)于最粗尺度，具有最高采樣率SN的第i=N個傳感器對應(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時刻的狀態(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個傳感器在采樣率Si下獲得的第k個觀測值；H(i,k)∈Rqi×n為觀測矩陣；觀測噪聲v(i,k)∈Rqi×l為零均值高斯白噪聲，其協(xié)方差為R(i)，且不同采樣率下的觀測噪聲互不相關(guān).初始狀態(tài)向量x(N,0)為一隨機變量，且假設(shè)x(N,0)，w(N,k)和v(i,k)彼此間統(tǒng)計獨立.

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

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

器異步采樣示意圖.其中，橫坐標(biāo)表示時刻，縱坐標(biāo)表示傳感器(或尺度)，N=3，n1=3，n2=2.具有最高采樣率的傳感器N采樣均勻，且其采樣點與時刻一一對應(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　信息融合估計算法

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

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

(27)

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

(28)

(29)

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

(30)

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

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

3　仿真及分析

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

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

圖3　載體航行軌跡

(a) 航行速度

(b) 航向角

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

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

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

(a) 速度誤差

(b) 位置誤差

(a) 速度誤差

(b) 航向角

算法誤差項均方根最大絕對值本文算法東向速度誤差/(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)為對象，在導(dǎo)航傳感器量測具有隨機延遲的情況下，設(shè)計了一種基于多尺度數(shù)據(jù)分塊的組合導(dǎo)航信息融合算法.將所提算法用于SINS/DVL/TAN組合導(dǎo)航系統(tǒng)進(jìn)行仿真驗證.通過處理包含所有傳感器量測信息的數(shù)據(jù)塊，有效解決了量測延遲問題.

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

但該算法也存在不足，通過狀態(tài)和觀測的分塊建立多尺度系統(tǒng)模型的過程中，狀態(tài)向量和觀測向量的維數(shù)有所擴展，這將增加計算量和計算復(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] 張輝, 牛小驥, 郭文飛，等. 一種簡易通用的敲擊觸發(fā)式慣性導(dǎo)航系統(tǒng)時間同步裝置設(shè)計[J]. 傳感技術(shù)學(xué)報, 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é)報, 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é)報, 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è), 熊智, 段方. 考慮量測滯后的INS/SAR組合導(dǎo)航非等間隔濾波算法研究[J]. 宇航學(xué)報, 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.