組合導航系統中異步多傳感器信息融合算法

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

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

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

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

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

1　異步多傳感器組合導航系統

1.1　系統描述

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

Sj+1=njSj1≤j≤N-1

(1)

式中，nj為已知正整數.

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

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為最細尺度上k時刻的狀態變量；Φ(N,k)為系統一步轉移矩陣；系統噪聲w(N,k)∈Rn×l為零均值高斯白噪聲,Q(N) 為其協方差；z(i,k)∈Rqi×l(qi≤n)為第i個傳感器在采樣率Si下獲得的第k個觀測值；H(i,k)∈Rqi×n為觀測矩陣；觀測噪聲v(i,k)∈Rqi×l為零均值高斯白噪聲，其協方差為R(i)，且不同采樣率下的觀測噪聲互不相關.初始狀態向量x(N,0)為一隨機變量，且假設x(N,0)，w(N,k)和v(i,k)彼此間統計獨立.

為更清晰地表達各傳感器、尺度和時間的對應關系，圖1給出了具有不同采樣率的3個導航傳感

圖1　多導航傳感器異步采樣示意圖

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

1.2　多尺度系統模型

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)

由數據歸納法，可得

(6)

由式(4)～(6)，有

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

(7)

(8)

(9)

(10)

(11)

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

E{WN(k)}=0

(12)

(13)

(14)

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

(15)

根據式(15)，有

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

(16)

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

(17)

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

(18)

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

(19)

(20)

(21)

(22)

根據上節所述v(i,k) 均值為零、協方差為R(i)，且與w(N,l)不相關，以及式(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)上的系統模型為式(7)和(19).

2　信息融合估計算法

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

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

(27)

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

(28)

(29)

相應的估計誤差協方差矩陣為

(30)

綜上，基于多尺度數據分塊的組合導航信息融合算法流程圖如圖2所示.

圖2　基于多尺度數據分塊的組合導航信息融合算法流程圖

3　仿真及分析

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

表1　傳感器誤差參數

圖3　載體航行軌跡

(a) 航行速度

(b) 航向角

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

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

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

(a) 速度誤差

(b) 位置誤差

(a) 速度誤差

(b) 航向角

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

4　結論

1) 以多傳感器異步采樣組合導航系統為對象，在導航傳感器量測具有隨機延遲的情況下，設計了一種基于多尺度數據分塊的組合導航信息融合算法.將所提算法用于SINS/DVL/TAN組合導航系統進行仿真驗證.通過處理包含所有傳感器量測信息的數據塊，有效解決了量測延遲問題.

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

但該算法也存在不足，通過狀態和觀測的分塊建立多尺度系統模型的過程中，狀態向量和觀測向量的維數有所擴展，這將增加計算量和計算復雜度，因此如何進一步優化算法有待研究.

參考文獻(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] 張輝, 牛小驥, 郭文飛，等. 一種簡易通用的敲擊觸發式慣性導航系統時間同步裝置設計[J]. 傳感技術學報, 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/高度表組合導航系統異步融合算法[J]. 海軍航空工程學院學報, 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] 劉磊, 林雪原, 周旻. 多傳感器組合導航系統的多尺度異步信息融合算法[J]. 海軍航空工程學院學報, 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] 劉建業, 熊智, 段方. 考慮量測滯后的INS/SAR組合導航非等間隔濾波算法研究[J]. 宇航學報, 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.