多傳感器組合導航系統(tǒng)的改進多尺度濾波算法

林雪原，孫玉梅，董云云，喬玉新，陳祥光,2

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

多傳感器組合導航系統(tǒng)有兩項重要指標，即可靠性與精度。為了提高系統(tǒng)的可靠性，可以從子導航傳感器[1-2]及系統(tǒng)融合[3]加以考慮。解決多傳感器系統(tǒng)的信息融合問題，進而提高系統(tǒng)的導航精度，目前有兩種途徑：一種是集中式卡爾曼濾波，另一種是分散化卡爾曼濾波。集中式卡爾曼濾波具有計算負擔重以及容錯性能差的缺點，而分散化濾波在多傳感器組合導航系統(tǒng)中得到了廣泛應用[4-5]，其中Carlson提出的聯(lián)邦濾波器(下面簡稱算法1)受到了重視[6]。

以上算法都是基于系統(tǒng)在時域的動態(tài)模型和觀測模型上建立起來的，并沒有考慮到系統(tǒng)的多尺度特性。隨著多尺度系統(tǒng)理論的不斷發(fā)展，將小波分析與卡爾曼濾波技術相結合形成多尺度卡爾曼濾波算法得到發(fā)展[7-9]，并在目標跟蹤領域及圖像處理領域得到驗證[10]。其工作的基本思想大都是先將系統(tǒng)的動態(tài)方程和觀測方程改寫成以數(shù)據(jù)塊形式描述的狀態(tài)方程和觀測方程(即狀態(tài)方程是塊狀態(tài)向量與塊狀態(tài)向量的關系，而量測方程是塊觀測向量與塊狀態(tài)向量的關系)，從而再套用經典卡爾曼濾波算法與小波分析相結合對相關的塊狀態(tài)向量進行處理， 試驗表明該方法相對于現(xiàn)有狀態(tài)估計方法可以以更高的精度來估計系統(tǒng)狀態(tài)。組合導航系統(tǒng)濾波模型相對于目標跟蹤模型而言，具有狀態(tài)方程復雜且維數(shù)較高、濾波耗時高等特點，文獻[11-12]成功地將多傳感器組合導航系統(tǒng)與多尺度卡爾曼濾波相結合，進而得到精度更高的濾波精度。然而上述方法存在如下問題：塊狀態(tài)向量間各向量的噪聲是相關的，濾波器的實時性得不到保證[8]。

為了克服上述問題、優(yōu)化多傳感器組合導航系統(tǒng)的多尺度濾波算法，本文首先將系統(tǒng)原始狀態(tài)方程表達為塊狀態(tài)向量的形式，然后將實時獲得的量測向量表達為塊狀態(tài)向量的方程進而解決估計器的實時性問題，并描述了濾波算法的詳細步驟，最后以SST(天文導航)/GPS(全球導航定位系統(tǒng))/SINS(捷聯(lián)慣性導航系統(tǒng))組合導航系統(tǒng)為例對本文算法進行了仿真與驗證。

1 系統(tǒng)描述

給定一類有單個動態(tài)模型和多個測量模型組成的動態(tài)系統(tǒng)：

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

(1)

zi(k)=Hi(k)x(k)+vi(k)，i=1,2,…,N0

(2)

式中：k為離散時間指標；x(k)∈Rn×1為目標狀態(tài)向量；A(k)∈Rn×n為系統(tǒng)狀態(tài)轉移矩陣；G(k)∈Rn×m為系統(tǒng)噪聲矩陣；w(k)∈Rm×1為系統(tǒng)噪聲，它是一個高斯白噪聲序列，并滿足：

E{w(k)}=0，E{w(k)wT(j)}=Q(k)δkj

系統(tǒng)(1)的初始狀態(tài)x(0)是一個隨機向量，并具有如下統(tǒng)計特性：

(3)

假定x(0)、w(k)、vi(k)之間是相互統(tǒng)計獨立的。

1.1 分塊描述

將系統(tǒng)(1)表示為狀態(tài)塊的形式，以便對系統(tǒng)進行多尺度分析，定義塊長為M的狀態(tài)塊：

X(m+1)=[xT(m+1,1),xT(m+1,2),…,

(4)

式中：m=0,1,2,…；M=2i(i=1,2,3,…)。

而狀態(tài)塊X(m+1)中的第s(s=1,2,…,M)個元素為：

x(m+1,s)=x(mM+s)=

A(mM+s-1)x(mM+s-1)+

G(mM+s-1)w(mM+s-1)=…=

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

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

相應的過程噪聲塊向量為：

并可表示為：

B(m)的第r行第c列元素用Br,c(m)表示，則B(m)的各元素表示為：

根據(jù)以上定義和符號，狀態(tài)向量模型(1)可重新描述為：

(5)

其中:

(6)

式中：

測量模型(2)可進一步表示為：

(7)

因此，式(7)就建立了每個時刻的測量值zi(m,s)與狀態(tài)塊X(m)之間的關系，這樣就為算法的實時性提供了可能，并將能克服已有算法中存在半實時性的不足。

1.2 離散小波變換

多尺度分析是將待處理信號用小波變換的方法在不同的尺度上進行分解，分別得到粗尺度上的平滑信號與細尺度上的細節(jié)信號[10]。

如果將尺度i上向量序列x(i,k)∈Rn×1(k∈Z)表達成數(shù)據(jù)塊的形式：

Xi,V(m)=Xi(m)=

[xT(i,(m-1)Mi+1),

xT(i,(m-1)Mi+2),…,

xT(i,(m-1)Mi+Mi)]T

式中：Mi=2i-1。則小波變換的分析算子與綜合變換算子表達如下：

Xi,V(m)=Hi·Xi+1,V(m)，L≤i≤N-1

(8)

Xi,D(m)=Gi·Xi+1,V(m)，L≤i≤N-1

(9)

對應的多尺度重構變換為:

(10)

“*”表示復矩陣的共軛轉置，若記

γ(m)=[(XN-1,D(m))T,(XN-2,D(m))T,…,

則與式(8)(9)相對應的多尺度分解可進一步表示為:

γ(m)=WX·X(m)

(11)

式中：WX為矩陣算子，

容易驗證WX是一個正交矩陣，即：

(12)

而式(10)可表示為:

(13)

2 分塊系統(tǒng)的多尺度濾波方法

首先，將小波算子WX同時作用于方程(5)兩邊：

我院2016年5月--2017年9月收治的64例行口腔修復患者,按照隨機數(shù)字表法分為實驗組和對照組,每組患者32例。實驗組:男16例,女16例,年齡42-76歲,平均年齡(58.5±14.5)歲,病程1-5周,平均病程(2.5±0.5)周;對照組:男17例,女15例,年齡43-78歲,平均年齡(60.5±15.5)歲,病程1.5-5周,平均病程(3.25±0.75)周。將兩組患者基本資料進行精細對比分析,差異不明顯(P>0.05),具有臨床可比性。所有患者均自愿入組并簽署知情同意書。

(14)

再利用式(11)(12)，式(14)就又改寫成：

(15)

利用式(12)，觀測向量模型(7)可以等價描述為：

zi(m,s)=ψi,w(m,s)γ(m)+vi(m,s)

(16)

下面將從初始條件(3)出發(fā)，基于多尺度模型(15)(16)，建立具有同步多傳感器系統(tǒng)多尺度序貫卡爾曼濾波器。為此，先引入來自所有傳感器的1～m塊的測量值集合[7]：

其中:

Z(l)={Z(l,1),Z(l,2),…,Z(l,M)}=

{Z1(l),Z2(l),…,ZN0(l)}，l=1,2,…

l=1,2,…，s=1,2,…,M

Zi(l)={zi(l,1),zi(l,2),…,zi(l,M)}，

i=1,2,…,N0

下面以第m個狀態(tài)塊為例，從兩個層次上來說明對狀態(tài)塊進行估計的過程。

假設已經完成下列工作：

3)在第m塊中的第s個點處，已完成了基于z1(m,s)，z2(m,s)，…，zi-1(m,s)對γ(m)的估計，即：

Z(m,s-1),z1(m,s),…,zi-1(m,s)}

利用逆小波變換，可以得到目標狀態(tài)塊X(m)的估計值及其誤差協(xié)方差矩陣：

m=1,2,…，s=0,1,2,…,M，i=1,2,…,N0

s=0,1,2,…,M，i=1,2,…,N0

應該注意到：

1) 當s=0，i=0時，

2)當s=M，i=N0時，就得到式(15)中γ(m)的最終估計值及誤差協(xié)方差矩陣：

(17)

(18)

利用逆小波對式(17)和式(18)作變換，進而得到狀態(tài)塊X(m)的估計值及其誤差協(xié)方差矩陣：

3 仿真及分析

為驗證本文所提的改進多尺度濾波算法的有效性，將算法用于GPS/SST/SINS組合導航系統(tǒng)。濾波器的誤差方程經降階后，狀態(tài)向量取為[14-15]：

x(t)=[φE,φN,φU,δvE,δvN,δL,δλ,εrx,εry,εrz]T

式中：下標E、N、U代表地理坐標系的東、北、天方向；δL,δλ為緯度、經度誤差；δvE,δvN為速度誤差；φE,φN,φU為數(shù)學平臺誤差角；εrx,εry,εrz為陀螺一階馬爾科夫漂移誤差，量測方程及方程其他參數(shù)詳見文獻[11-12，14]。仿真中各傳感器誤差參數(shù)設置如表1所示。

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

下面的仿真試驗中，GPS、SST的采樣周期均設定為1 s，SINS的采樣周期設定為0.02 s；綜合考慮濾波精度與濾波實時性因素，式(4)中取M=2，所采用的小波為Haar二進制小波。為了便于對基于本文改進算法及基于算法1的濾波精度進行比較，對相同的SINS、GPS、SST仿真數(shù)據(jù)，本文同時進行了基于兩者算法的濾波分析，其中圖1～圖6分別給出了位置、速度與姿態(tài)誤差對比曲線、圖7給出了系統(tǒng)仿真軌跡、圖8給出了兩種算法的計算量對比。

表2及表3給出了基于上述兩種濾波算法的導航結果數(shù)據(jù)統(tǒng)計值。

圖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

在Intel Core 2 Duo P7350 2.0G 45 nm處理器的環(huán)境下，從表3可以看出改進算法的濾波平均時間約為0.07 s，算法1的濾波平均時間約為0.033 s；在相同的環(huán)境下、對相同的多傳感器組合導航系統(tǒng)模型，文獻[12]算法的濾波平均時間約為0.094 s，而改進算法與文獻[13]算法的濾波精度相同。

在式(4)所示的模型中，適當增加塊向量內向量的個數(shù)時，即M=4,8,…時，導航參數(shù)的精度將會進一步提高，但濾波所有的時間將會大幅增加，因此，M=2時即能夠保證濾波的精度又能夠保證濾波的實時性。

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

圖8 本文算法與算法1的計算量對比Fig.8 Comparison of computation burden between this paper′s algorithm and algorithm 1

4 結束語

將系統(tǒng)原始狀態(tài)方程表達為塊狀態(tài)向量的形式，并將實時獲得的量測向量表達為塊狀態(tài)向量的函數(shù)，采用SST/GPS/SINS組合導航系統(tǒng)為例對本文算法進行了仿真與驗證，得出如下結論：

1)相對于算法1，本文算法對位置精度可提升一倍以上，速度精度可提升10%，姿態(tài)精度略有提升。產生的原因是組合導航系統(tǒng)是一個典型的非線性系統(tǒng)，而經過濾波后的誤差修正采用的是反饋校正方法，即對位置、速度采用線性修正、而對姿態(tài)采用非線性修正方法。本文所提的改進算法優(yōu)于算法1的原因是：改進算法采用分塊估計與小波變換相結合的方法，塊狀態(tài)向量的估計值中包含了狀態(tài)的平滑值。

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

表3 兩種算法的計算量對比

2)本文算法的計算量與算法1的計算量在同一個數(shù)量級。特別是隨著計算機性能的不斷提高，該計算量更顯得微不足道。

故基于本文改進算法的多傳感器組合導航系統(tǒng)不僅濾波精度較高，而且計算量較低。

本文僅對同步的多傳感器組合導航系統(tǒng)進行了研究，而實際情況下經常出現(xiàn)的是異步采樣多傳感器組合導航系統(tǒng)，這也是本文作者日后研究的工作重點。