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

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

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

1 引言

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

秦永元等[1-2]提出了典型的異步融合方法，其核心思想是利用時(shí)間更新把非同步信號(hào)變換成同步信號(hào)。周明進(jìn)等[3]結(jié)合非等間隔采樣，研究了基于聯(lián)邦濾波的異步融合，并采用時(shí)間更新與量測(cè)更新分離的方法解決了非等間隔問題。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)方程及對(duì)應(yīng)的量測(cè)方程?；诔叨鹊乃枷?，林雪原等[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)實(shí)，本文先簡要介紹了基于多尺度思想的多傳感器組合導(dǎo)航模型，然后在此基礎(chǔ)上給出了在有理數(shù)倍采樣率下的多傳感器組合導(dǎo)航系統(tǒng)的改進(jìn)異步最優(yōu)信息融合算法，最后將該算法應(yīng)用于天文導(dǎo)航(CNS)/全球?qū)Ш蕉ㄎ幌到y(tǒng)(GNSS)/捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)/高度表多傳感器組合導(dǎo)航系統(tǒng)，進(jìn)而驗(yàn)證了算法的有效性。

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ù)的定義及其約束條件見文獻(xiàn)[7]；x為狀態(tài)向量；A為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；G為系統(tǒng)噪聲矩陣；w為系統(tǒng)噪聲，且是高斯白噪聲序列；k為離散時(shí)間指標(biāo)。

將系統(tǒng)(1)表示為塊長為M(通常取M=2i，i為大于0的正整數(shù)；如果i=0則對(duì)應(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個(gè)元素為：

式中：

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)

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

(4)

則式(3)可表達(dá)為：

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

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

i=1,2,…,N0

(5)

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

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

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

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

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

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

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個(gè)狀態(tài)塊中的第s個(gè)元素。

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

(7)

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

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

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

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

mod(k-1,ni)=

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

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

(8)

(9)

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

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

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

增益矩陣為

(10)

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

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

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

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

增益矩陣為

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

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

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

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

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

4 仿真及分析

為驗(yàn)證本文算法，將上述異步信息融合算法用于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é)平臺(tái)誤差角；ε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，這樣選取的目的是不會(huì)導(dǎo)致系統(tǒng)的濾波時(shí)間過長進(jìn)而影響系統(tǒng)的實(shí)時(shí)性。

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

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

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

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

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

圖5 基于改進(jìn)算法的姿態(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時(shí)間段內(nèi)的飛行軌跡，該軌跡包含了平飛、俯仰、俯沖、轉(zhuǎn)彎、加速、平飛圓周運(yùn)動(dòng)等機(jī)動(dòng)過程以方便對(duì)算法進(jìn)行綜合評(píng)估。航跡初始位置為(118°, 29°, 50 m)，整個(gè)運(yùn)動(dòng)過程中機(jī)體最大加速度為4m/s2、且機(jī)體最大速度達(dá)到510 m/s，轉(zhuǎn)彎時(shí)航向角變化最大變化速率為5(°)/s，加速拉起時(shí)俯仰角變化最大變化速率為3(°)/s，轉(zhuǎn)彎時(shí)橫滾角變化最大變化速率為2(°)/s。

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

表達(dá)方便起見，假設(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表示仿真時(shí)間。

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

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

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

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

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

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

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

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

5 結(jié)論

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

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

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

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