一種多傳感器組合導(dǎo)航系統(tǒng)的改進異步融合算法

劉麗麗，林雪原,*，陳祥光,2

1. 煙臺南山學(xué)院 電氣與電子工程系，煙臺 265713 2. 北京理工大學(xué)，北京 100081

1 引言

目前，多傳感器組合導(dǎo)航系統(tǒng)的研究較多，并且均集中于同步信息的融合算法的研究。然而在實際應(yīng)用中各子導(dǎo)航傳感器由于檢測裝置及采樣速率的不同導(dǎo)致多傳感器組合導(dǎo)航系統(tǒng)是一種典型的異步信息融合系統(tǒng)。

秦永元等[1-2]提出了典型的異步融合方法，其核心思想是利用時間更新把非同步信號變換成同步信號。周明進等[3]結(jié)合非等間隔采樣，研究了基于聯(lián)邦濾波的異步融合，并采用時間更新與量測更新分離的方法解決了非等間隔問題。Jian等[4]研究了脈沖星/星體跟蹤定位器/INS組合導(dǎo)航系統(tǒng)的異步融合方法，其核心思想是根據(jù)濾波器的速率把多傳感器組合導(dǎo)航系統(tǒng)分為姿態(tài)濾波器與速度位置濾波器兩部分。總體而言，上述算法本質(zhì)上是次優(yōu)的。

為了得到性能最優(yōu)的多傳感器組合導(dǎo)航系統(tǒng)的最優(yōu)異步濾波方法，林雪原[5]、赫飛等[6]提出了在子導(dǎo)航傳感器數(shù)據(jù)采樣速率呈有理數(shù)倍情況下的最優(yōu)異步融合方法，其核心思想是基于簡單尺度的思想，把當(dāng)前系統(tǒng)狀態(tài)向量向前遞推，并得到各尺度上的狀態(tài)方程及對應(yīng)的量測方程?；诔叨鹊乃枷?，林雪原等[7-8]研究了基于狀態(tài)數(shù)據(jù)塊與小波變換相結(jié)合的、基于尺度的多傳感器組合導(dǎo)航系統(tǒng)的同步信息融合方法。劉磊等[9]研究了采樣速率為2的整數(shù)倍這一特殊情況下的基于多尺度思想的多傳感器組合導(dǎo)航系統(tǒng)融合算法；朱倚嫻等[10]研究了子導(dǎo)航傳感器存在輸出延遲情況下的基于多尺度的多傳感器異步信息融合方法。

考慮到大多數(shù)子導(dǎo)航傳感器具有輸出數(shù)據(jù)速率穩(wěn)定、且為有理數(shù)倍采樣率[11]這一現(xiàn)實，本文先簡要介紹了基于多尺度思想的多傳感器組合導(dǎo)航模型，然后在此基礎(chǔ)上給出了在有理數(shù)倍采樣率下的多傳感器組合導(dǎo)航系統(tǒng)的改進異步最優(yōu)信息融合算法，最后將該算法應(yīng)用于天文導(dǎo)航(CNS)/全球?qū)Ш蕉ㄎ幌到y(tǒng)(GNSS)/捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)/高度表多傳感器組合導(dǎo)航系統(tǒng)，進而驗證了算法的有效性。

2 基于多尺度的組合導(dǎo)航系統(tǒng)模型

以SINS的誤差模型建立系統(tǒng)的狀態(tài)方程：

x(k+1)=x[(k+1)T0]=

A(kT0)x(kT0)+G(kT0)w(kT0)=

A(k)x(k)+G(k)w(k)

(1)

式中：T0為系統(tǒng)濾波周期，其他各參數(shù)的定義及其約束條件見文獻[7]；x為狀態(tài)向量；A為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；G為系統(tǒng)噪聲矩陣；w為系統(tǒng)噪聲，且是高斯白噪聲序列；k為離散時間指標。

將系統(tǒng)(1)表示為塊長為M(通常取M=2i，i為大于0的正整數(shù)；如果i=0則對應(yīng)于常規(guī)卡爾曼濾波形式)的狀態(tài)塊的形式：

X(m+1):=

[xT(m+1,1),xT(m+1,2),…,xT(m+1,M)]T:=

[xT(mM+1),xT(mM+2),…,xT(mM+M)]T

(2)

式中：m=0,1,2,…；M=2i。X(m+1)中的第s個元素為：

式中：

s=1,2,…,M

w(mM+j-1)+G(mM+s-1)·

w(mM+s-1)

定義過程噪聲塊向量為：

并可表示為：

式中：B(m)的第r行第c列元素用Br,c(m)表示為

[wT(mM),wT(mM+1),…,wT(mM+M-1)]T

根據(jù)以上定義，系統(tǒng)模型(1)可表示為：

(3)

多尺度分析中的尺度算子與小波算子可對信號在平滑值信號空間和細節(jié)信號空間進行投影，建立一個涵蓋尺度算子與小波算子的矩陣算子Wx以對數(shù)據(jù)塊進行同樣的變換[7-8,12]，且Wx是一個正交矩陣，即

(4)

則式(3)可表達為：

假設(shè)有N0個子導(dǎo)航傳感器對系統(tǒng)(1)獨立地進行測量，則相應(yīng)的測量方程可表示為：

zi(nTi)=Ci(nTi)x(kTi)+vi(nTi)，

i=1,2,…,N0

(5)

式中：Ti為傳感器i的采樣周期；zi為第i個子導(dǎo)航傳感器的量測值；Ci為第i個子導(dǎo)航傳感器的量測矩陣；vi為第i個子導(dǎo)航傳感器的量測噪聲，且是高斯白噪聲序列。并假定

Ti=niT0，i=1,2,…,N0

式中：ni為大于0的正整數(shù)。

根據(jù)前面的定義式(5)，對某一時刻k，若第i個子導(dǎo)航傳感器輸出有效信息，則結(jié)合式(1)的定義，k必為ni的整數(shù)倍。同時根據(jù)式(2)，k可表示為：

k=(m-1)M+s，1≤s≤M

如果第i個子導(dǎo)航傳感器在時刻k有效，并提供量測值，則有：

zi(k)=Ci(k)x(k)+vi(k)=

Ci[(m-1)M+s]x[(m-1)M+s]+

vi[(m-1)M+s]

(6)

式中：i=1,2,…，N0；x[(m-1)M+s]為第m個狀態(tài)塊中的第s個元素。

結(jié)合式(2)的定義，式(6)可進一步表示為狀態(tài)塊的形式：

(7)

利用式(4)，式(7)可表示為

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

如前所描述，在異步融合中，各子導(dǎo)航傳感器的數(shù)據(jù)采集率不同，為此算法的首要任務(wù)是判斷在每個濾波時刻點上都有哪些子導(dǎo)航傳感器提供了量測信息。

經(jīng)常情況下系統(tǒng)的濾波周期常取T0=1，針對異步多傳感器組合導(dǎo)航系統(tǒng)，結(jié)合前面的分析，用mod(k-1,ni)是否為零來斷定傳感器i在k時刻是否有測量值(mod是取(k-1)/ni余數(shù)的函數(shù))[9,12]：

mod(k-1,ni)=

當(dāng)上述結(jié)果為零時，說明第i個子導(dǎo)航傳感器在時刻k有效并提供量測信息，否則該子導(dǎo)航傳感器在時刻k無效且不能提供量測信息。多傳感器組合導(dǎo)航系統(tǒng)的異步信息融合算法分為兩個過程：時間序貫和傳感器測量序貫。下面以第m塊為例，給出系統(tǒng)的更新過程。

假設(shè)m≠1，利用第(m-1)塊的狀態(tài)估計值對第m塊的狀態(tài)估計值進行預(yù)測，有：

(8)

(9)

完成式(8)(9)的預(yù)測后，對于第m塊內(nèi)的時刻s=1,2,…,M，依次利用Z(m,1)，Z(m,2)，…，Z(m,M)對系統(tǒng)進行信息處理，過程如下：

(1)當(dāng)s=1時

若mod[(m-1)M,ni]=0，i=2,…,N0，則第i個傳感器在該時刻有測量值zi(m,1)，則有更新方程為

增益矩陣為

(10)

若mod[(m-1)M,ni]≠0，此時傳感器i并無測量值，有

在該時刻，如果最后一個(即第N0個)傳感器也有測量值zN0(m,1)，則有更新方程為

該時刻的更新結(jié)束。

(2)當(dāng)s≠1時

增益矩陣為

類似地，若mod((m-1)M+s-1,ni)=0，i=2,…,N0，則第i個傳感器在該時刻有測量值zi(m,s)，則有更新方程為

若mod((m-1)M+s-1,ni)≠0，此時傳感器i并無測量值，有：

(3)當(dāng)s=M,i=N0時

應(yīng)用上述方法可判斷該數(shù)據(jù)塊的最后時刻、最后一個傳感器是否能夠提供測量信息后，進而得到狀態(tài)塊的最優(yōu)估計值如下：

根據(jù)矩陣算子WX的特性公式(4)，并應(yīng)用于上式進而得到如下的最優(yōu)估計：

4 仿真及分析

為驗證本文算法，將上述異步信息融合算法用于CNS/GNSS/SINS/高度表多傳感器組合導(dǎo)航系統(tǒng)。濾波器的狀態(tài)向量取為[8，13-14]：

x(t)=[φE,φN,φU,δvE,δvN,δvU,δL,

δλ,δh,εrx,εry,εrz]T

(11)

式中：δL,δλ,δh為緯度、經(jīng)度、高度誤差；δvE,δvN,δvU為速度誤差；φE,φN,φU為數(shù)學(xué)平臺誤差角；εrx,εry,εrz為陀螺一階馬爾可夫漂移誤差[8]。仿真中各傳感器誤差參數(shù)設(shè)置如表1所示。

表1 導(dǎo)航傳感器參數(shù)誤差

假設(shè)GNSS、CNS、高度表的數(shù)據(jù)采樣周期分別設(shè)定為1 s、2 s、3 s，而SINS的采樣周期設(shè)定為0.02 s，系統(tǒng)的濾波周期取為1 s；式(2)中的M取2，這樣選取的目的是不會導(dǎo)致系統(tǒng)的濾波時間過長進而影響系統(tǒng)的實時性。

文獻[5]中提出了一種基于有理數(shù)倍采樣率的簡易尺度異步多傳感器信息融合算法(在下面的仿真驗證中定義為算法1)，并且仿真試驗也驗證了該算法相對于文獻[1-2]所提異步融合算法具有較好的優(yōu)越性。為了便于比較基于本文改進算法(在下面的仿真驗證中定義為改進算法)與算法1的性能，對相同的高度表、GNSS、CNS、SINS仿真原始數(shù)據(jù)，本文同時進行了基于兩種算法的仿真分析，其中圖1～圖6分別為基于兩種算法的位置、速度與姿態(tài)誤差曲線。

圖1 基于改進算法的位置誤差Fig.1 Position error based on the improved algorithm

圖2 基于算法1的位置誤差Fig.2 Position error based on algorithm 1

圖3 基于改進算法的速度誤差Fig.3 Velocity error based on the improved algorithm

圖4 基于算法1的速度誤差Fig.4 Velocity error based on algorithm 1

圖5 基于改進算法的姿態(tài)誤差Fig.5 Attitude error based on the improved algorithm

圖6 基于算法1的姿態(tài)誤差Fig.6 Attitude error based on algorithm 1

圖7為飛行器在3 600 s時間段內(nèi)的飛行軌跡，該軌跡包含了平飛、俯仰、俯沖、轉(zhuǎn)彎、加速、平飛圓周運動等機動過程以方便對算法進行綜合評估。航跡初始位置為(118°, 29°, 50 m)，整個運動過程中機體最大加速度為4m/s2、且機體最大速度達到510 m/s，轉(zhuǎn)彎時航向角變化最大變化速率為5(°)/s，加速拉起時俯仰角變化最大變化速率為3(°)/s，轉(zhuǎn)彎時橫滾角變化最大變化速率為2(°)/s。

圖7 仿真飛行軌跡Fig.7 Simulated flying trajectory

表達方便起見，假設(shè)系統(tǒng)經(jīng)過相應(yīng)的濾波處理后，仿真圖中用Δλ,ΔL,Δh分別代表組合導(dǎo)航系統(tǒng)輸出的經(jīng)度誤差、緯度誤差、高度誤差；ΔvE,ΔvN,ΔvU分別代表組合導(dǎo)航系統(tǒng)輸出的東向速度誤差、北向速度誤差、天向速度誤差；Δγ,Δφ,Δθ分別代表組合導(dǎo)航系統(tǒng)輸出的橫滾角誤差、俯仰角誤差、航向角誤差；t表示仿真時間。

在Intel Core i5-7200U CPU 2.7 GHz處理器環(huán)境下，本文算法的濾波時間變化曲線如圖8所示。

為了更直觀地表述上述結(jié)果，表2給出了與圖1～圖6對應(yīng)的導(dǎo)航結(jié)果數(shù)據(jù)統(tǒng)計值。

綜合分析圖1～圖6和表2可得,本文算法與算法1相比，本文算法得到的各導(dǎo)航參數(shù)誤差不僅具有較小的最大、最小值，同時誤差的方差也大幅降低，例如位置精度采用本算法后相對于算法1可提高約20%，速度精度提高約15%，姿態(tài)精度提高約10%。

表2 試驗結(jié)果數(shù)據(jù)統(tǒng)計對比

從圖8可以看出，濾波時間最大值小于0.018 s，且平均濾波時間為0.001 7 s，而SINS的采樣周期為0.02 s，因而在本文的算法及仿真實驗中，當(dāng)M=2時可保證算法的實時性。

假設(shè)常規(guī)卡爾曼濾波器的階數(shù)為n，則其計算量與n3成正比[8]。假設(shè)與式(11)相對應(yīng)的n=12。根據(jù)式(3)(6)的特點(即本文算法的特點)，當(dāng)M=2時本文算法的計算量將與(n38n3)之間的某個數(shù)成正比；而當(dāng)M=4時本文算法的量將與(8n364n3)之中的某個數(shù)成正比。而且當(dāng)M=4及M=2時，兩種情況下濾波的計算量之比近似為8；故當(dāng)M=4時，對比圖8，可知系統(tǒng)的平均濾波時間大幅增加，同時最大濾波時間將遠大于SINS的采樣周期，因而無法保證系統(tǒng)的實時性。

圖8 本文算法的濾波時間Fig.8 Filtering time of the improved algorithm

文獻[7, 9]對M的取值與濾波精度間的關(guān)系進行定性探討，即適當(dāng)增加M的大小，即M=4,8,…時，將會進一步提高導(dǎo)航參數(shù)的精度，但精度提高的幅度會隨著M的增加而降低。為此，對M的取值需綜合考慮濾波精度與系統(tǒng)實時性兩方面因素。

5 結(jié)論

基于系統(tǒng)的原始狀態(tài)方程與量測方程，經(jīng)變換得到狀態(tài)數(shù)據(jù)塊向量與當(dāng)前狀態(tài)向量之間的關(guān)系并構(gòu)成新的狀態(tài)方程，將原始量測方程變換為與狀態(tài)數(shù)據(jù)塊向量之間的關(guān)系并構(gòu)成新的量測方程。以此為基礎(chǔ)，利用矩陣算子的尺度與小波特性，在子導(dǎo)航傳感器采樣率為有理數(shù)倍的條件下建立了多傳感器組合導(dǎo)航系統(tǒng)的異步信息融合算法。

對本文算法進行仿真分析后，結(jié)論如下：

1)相對于傳統(tǒng)異步融合算法，本文所提算法可提高各導(dǎo)航參數(shù)的精度。原因是，改進算法采用數(shù)據(jù)分塊處理與小波處理相結(jié)合的技術(shù)，進而把相鄰時刻狀態(tài)向量間的噪聲信息剔除、而保留更多的平滑信息[7]。

2)對M值的選擇需綜合考慮系統(tǒng)的濾波精度與系統(tǒng)實時性兩方面因素，在保證系統(tǒng)實時性的前提下，適當(dāng)增加M的取值可以提高系統(tǒng)的濾波精度。