多傳感器組合導航系統的改進多尺度濾波算法

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

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

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

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

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

1 系統描述

給定一類有單個動態模型和多個測量模型組成的動態系統：

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為目標狀態向量；A(k)∈Rn×n為系統狀態轉移矩陣；G(k)∈Rn×m為系統噪聲矩陣；w(k)∈Rm×1為系統噪聲，它是一個高斯白噪聲序列，并滿足：

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

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

(3)

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

1.1 分塊描述

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

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

(4)

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

而狀態塊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)的各元素表示為：

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

(5)

其中:

(6)

式中：

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

(7)

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

1.2 離散小波變換

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

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

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 分塊系統的多尺度濾波方法

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

我院2016年5月--2017年9月收治的64例行口腔修復患者,按照隨機數字表法分為實驗組和對照組,每組患者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)出發，基于多尺度模型(15)(16)，建立具有同步多傳感器系統多尺度序貫卡爾曼濾波器。為此，先引入來自所有傳感器的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個狀態塊為例，從兩個層次上來說明對狀態塊進行估計的過程。

假設已經完成下列工作：

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)}

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

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)的最終估計值及誤差協方差矩陣：

(17)

(18)

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

3 仿真及分析

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

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

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

表1 導航傳感器誤差參數

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

表2及表3給出了基于上述兩種濾波算法的導航結果數據統計值。

圖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 基于改進算法的姿態誤差Fig.5 Attitude error based on the improved algorithm

圖6 基于算法1的姿態誤差Fig.6 Attitude error based on algorithm 1

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

在式(4)所示的模型中，適當增加塊向量內向量的個數時，即M=4,8,…時，導航參數的精度將會進一步提高，但濾波所有的時間將會大幅增加，因此，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 結束語

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

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

表2 試驗結果數據統計對比

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

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

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

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