基于聯(lián)邦濾波的異質(zhì)異步多傳感器組合導(dǎo)航算法*

姚曉涵 陳 帥 楊 博 李 杰 張 敏

1.南京理工大學(xué)自動化學(xué)院，南京210094 2.中國船舶工業(yè)綜合技術(shù)經(jīng)濟研究院，北京100081

0 引言

為了提高導(dǎo)航系統(tǒng)定位精度和容錯性能，發(fā)展多傳感器組合導(dǎo)航是目前最常采用的有效策略。在現(xiàn)有的多傳感器信息融合研究中，大多假定各個導(dǎo)航傳感器同步地將數(shù)據(jù)傳遞給融合中心[1]。然而，在實際系統(tǒng)中，由于信號變化速率相差較大、檢測裝置采樣周期不同等原因使各個傳感器的采樣速率不盡相同，且由于傳感器固有延遲和通信延遲的不同，使融合中心接收到的數(shù)據(jù)也不同步。此外，融合導(dǎo)航濾波過程中會遇到各種各樣的干擾，若動力學(xué)模型信息受到擾動，或多傳感器觀測信息含有異常誤差，那么最終的導(dǎo)航濾波解必定會和真實值有較大誤差。因此，為了提高組合導(dǎo)航系統(tǒng)定位精度，研究異質(zhì)異步多傳感器信息融合算法具有重要意義。

方崢等提出了基于子濾波器方差陣修正的多源信息非等間隔聯(lián)邦濾波算法，解決無重置聯(lián)邦濾波器在應(yīng)用于多源信息非等間隔濾波時的適應(yīng)性問題[2]。該算法有效改進了濾波器的估計性能。文獻[3]針對多傳感器非等間隔數(shù)據(jù)采樣問題,采用時間與量測更新分離的異步非等間隔聯(lián)邦濾波算法進行信息融合,并對濾波器結(jié)構(gòu)進行改進以減少算法復(fù)雜度。Narjes D等提出了一種基于變分貝葉斯逼近的多傳感器自適應(yīng)卡爾曼濾波算法[4]。該濾波算法自適應(yīng)地估計傳感器的測量噪聲方差，對傳感器的異常測量具有較強的魯棒性。文獻[5] 在常規(guī)KF基礎(chǔ)上加入了基于新息的方差匹配自適應(yīng)濾波技術(shù),有效減小了異常情況下的導(dǎo)航誤差。

本文針對組合導(dǎo)航系統(tǒng)中多個傳感器采樣頻率不同且融合導(dǎo)航濾波過程中動力學(xué)模型信息受到擾動的問題，提出一種基于聯(lián)邦Kalman濾波的多尺度自適應(yīng)組合導(dǎo)航信息融合算法。該算法把各傳感器的數(shù)據(jù)按照最低尺度進行分塊處理，通過狀態(tài)和觀測的擴維，得到多尺度系統(tǒng)模型，并引入了自適應(yīng)因子以控制量測預(yù)測值。通過仿真，驗證了本文所提信息融合算法可有效提高異步異質(zhì)多傳感器組合導(dǎo)航系統(tǒng)的定位精度。

1 多傳感器組合導(dǎo)航系統(tǒng)建模

1.1 多尺度模型描述

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

Sj+1=njSj1≤j≤N-1

(1)

式中nj為已知正整數(shù)。

i(i=1,2,…,N)既表示傳感器，也表示尺度。具有最低采樣率SN的第i=1個傳感器對應(yīng)于最粗尺度，具有最高采樣率SN的第i=N個對應(yīng)于最細尺度，傳感器i=1，2,…,N-1的采樣率介于兩者之間，依次升高[6]。

在最高采樣率下多傳感器組合導(dǎo)航系統(tǒng)狀態(tài)方程和觀測方程一般可描述為：

X(N,K+1)=F(N,k)X(N,k)+W(N,k)

(2)

Z(i,k)=H(i,k)X(i,k)+V(i,k)

(3)

式中，X(N,k)∈Rn×1是最高尺度N上k時刻的狀態(tài)量，F(xiàn)(N,k)為系統(tǒng)一步轉(zhuǎn)移矩陣，系統(tǒng)噪聲W(N,k)∈Rn×1是均值為零的高斯白噪聲，Q(N)為其協(xié)方差,Z(i,k)∈Rqi×1(qi≤n)為第i個傳感器在采樣率Si下獲得的第k個觀測值，H(i,k)∈Rqi×n為觀測矩陣，觀測噪聲V(i,k)∈Rqi×1均值為0的高斯白噪聲，其協(xié)方差為R(i)，且不同采樣率下的觀測噪聲互不相關(guān)[7]。初始狀態(tài)向量X(N,0)為一隨機變量，且假設(shè)X(N,0)，W(N,k)，V(i,k)彼此間獨立[8]。

在濾波前先確定融合的周期，根據(jù)這個周期，把各傳感器的數(shù)據(jù)按照最低尺度進行分塊處理。對于每一塊內(nèi)的數(shù)據(jù)，計算各采樣率高于這個融合周期的傳感器在這個融合尺度上的平滑信息。

1.2 基于聯(lián)邦卡爾曼濾波的組合導(dǎo)航模型

本文中聯(lián)邦卡爾曼濾波以慣性導(dǎo)航系統(tǒng)為基礎(chǔ)，其它系統(tǒng)分別與SINS構(gòu)建子濾波器。選取東北天地理坐標系作為導(dǎo)航坐標系。狀態(tài)量選用SINS慣性器件的誤差量，并加入陀螺的常值誤差、隨機誤差以及加速度計的隨機誤差，得到18 維的狀態(tài)矢量X：

X=[φEφNφUδVEδVNδVUδLδλδH

(4)

1)GNSS/SINS子濾波器

根據(jù)式(2)，GNSS/SINS子濾波器的狀態(tài)方程可表示為：

(5)

GNSS/SINS子濾波器的量測方程為：

ZGNSS(t)=HGNSS(t)XGNSS(t)+VGNSS(t)

(6)

2)ALTM/SINS子濾波器

ALTM/SINS的組合模型也以18維的SINS誤差量作為狀態(tài)量，根據(jù)式(2)，其子濾波器的狀態(tài)方程可表示為：

(7)

ALTM/SINS子濾波器的量測方程為：

ZALTM(t)=HALTM(t)XALTM(t)+VALTM(t)

(8)

3)AHRS/SINS子濾波器

AHRS/ SINS的組合模型也以18 維的SINS 誤差量作為狀態(tài)量，根據(jù)式(2)，其子濾波器的狀態(tài)方程可表示為：

(9)

AHRS/ SINS子濾波器的量測方程為：

ZAHRS(t)=HAHRS(t)XAHRS(t)+VAHRS(t)

(10)

2 異質(zhì)傳感器信息融合

2.1 自適應(yīng)狀態(tài)估計求解

卡爾曼濾波是一種線性最小方差估計，可以實時估計系統(tǒng)的誤差參數(shù)，主要通過遞推計算，用先驗值和最新觀測數(shù)據(jù)中得到的新值的加權(quán)平均來更新狀態(tài)估計[9]。離散化后的系統(tǒng)模型具體為：

(11)

自適應(yīng)濾波在濾波計算過程中可以抑制濾波器的發(fā)散。它通過觀測數(shù)據(jù)實時得到預(yù)測修正值，對未知的或者不確定的系統(tǒng)模型和噪聲參數(shù)進行估計和修正。下面推導(dǎo)一種基于新息的自適應(yīng)卡爾曼濾波算法，通過對新息方差的極大似然估計，解決外部量測噪聲強度不確定時系統(tǒng)的濾波穩(wěn)定問題。具體如下：

狀態(tài)一步預(yù)測：

(12)

一步預(yù)測誤差協(xié)方差：

(13)

濾波增益：

(14)

狀態(tài)估計：

(15)

估計誤差協(xié)方差：

Pk=(I-KkHk)Pk,k-1

(16)

濾波器的新息序列rk狀態(tài)和其新息方差Crk的表達式為：

(17)

(18)

(19)

則量測信息的協(xié)方差陣為：

(20)

為了控制狀態(tài)模型異常和觀測信息異常對系統(tǒng)狀態(tài)參數(shù)估計的影響，利用抗差估計原理構(gòu)造如下極值函數(shù)：

(21)

(22)

對式(21)求極值后則有：

(23)

將信息向量方程和狀態(tài)預(yù)測信息的誤差方程帶入式(23)，可得狀態(tài)參數(shù)的抗差自適應(yīng)濾波解為：

(24)

2.2 信息分配因子構(gòu)造

從式(23)的抗差自適應(yīng)解的表達式中可以看出，利用抗差估計對觀測信息進行檢測，如果觀測信息有異常，則相應(yīng)的等價權(quán)矩陣就會減小，從而可以控制觀測信息異常的影響。并且利用自適應(yīng)濾波方法根據(jù)觀測信息對動力學(xué)模型進行檢測，如果相應(yīng)的動力學(xué)模型有異常，則相應(yīng)的自適應(yīng)因子αk就減小，從而可以控制狀態(tài)模型預(yù)測信息異常的影響[10-11]。如果系統(tǒng)的動力學(xué)模型和觀測信息均正常時，整個系統(tǒng)就可以正常利用動力學(xué)模型和觀測信息進行最優(yōu)估計，此時的抗差自適應(yīng)濾波就是標準的Kalman濾波[12]。

由于預(yù)測殘差信息可以較好地反應(yīng)擾動影響，因此可以采用預(yù)測殘差構(gòu)造自適應(yīng)因子αk，采用三段函數(shù)法可以表示如下：

(25)

(26)

3 仿真校驗

為了驗證本文所提多尺度自適應(yīng)組合導(dǎo)航信息融合算法的有效性，將所提方法用于SINS/GNSS/ALTM/AHRS組合導(dǎo)航系統(tǒng)，利用Matlab進行相應(yīng)的仿真試驗。仿真中各傳感器誤差參數(shù)設(shè)置如表1所示。

表1 傳感器誤差參數(shù)設(shè)置

仿真時間為10min，圖1給出了仿真中載體的飛行軌跡。飛行過程中包括左拐彎、右拐彎、上升、下降、加速、減速、平飛等任務(wù)。

圖1 飛行軌跡

設(shè)置SINS解算更新周期為0.02s，AHRS量測更新周期為 0.1s，GNSS量測更新周期為1s，ALTM量測更新周期為1s，模擬傳感器多采樣率情況。主濾波器將局部濾波器的輸出信息進行最優(yōu)融合，更新周期為1s。假設(shè)具有最高采樣率的SINS采樣均勻，為模擬傳感器量測延遲情況，對GNSS、ALTM、AHRS的量測信息加入隨機延時。為模擬傳感器量測異常和動力學(xué)模型擾動情況，在100～200s不使用AHRS的量測信息，在400～500s不使用ALTM的信息。

對于相同的SINS、GNSS、ALTM、AHRS仿真數(shù)據(jù)，分別采用本文算法與文獻[5]中所提抗差算法(后面簡稱算法1)進行信息融合，然后比較兩者的均方根誤差。仿真中，本文所提算法將數(shù)據(jù)進行分塊，且每個數(shù)據(jù)塊均包含有4種導(dǎo)航傳感器的量測信息，以數(shù)據(jù)塊為單位進行濾波。仿真結(jié)果如圖2～3所示。算法1將與 GNSS、ALTM、AHRS量測對應(yīng)的SINS解算結(jié)果存儲，當延遲的量測信息到達時，將其與存儲的SINS解算結(jié)果之差作為觀測量進行濾波，得到狀態(tài)估計，并將此狀態(tài)估計通過時間更新，遞推得到當前時刻的狀態(tài)估計。仿真結(jié)果如圖4～5所示。表2給出了2種融合算法導(dǎo)航結(jié)果的數(shù)據(jù)統(tǒng)計值。

綜合分析圖2～5和表2可得，本文算法與算法1融合結(jié)果相比，位置與速度精度均有提高。其中，天向速度和高度的誤差減小幅度分別達18%和23%，在異質(zhì)多傳感器異步采樣的情形下，能有效提高濾波器的估計精度，從而有利于提高導(dǎo)航系統(tǒng)的定位精度。

圖2 本文算法速度誤差

圖3 本文算法位置誤差

圖4 算法1速度誤差

圖5 算法1位置誤差

表2 均方根誤差結(jié)果對比

4 結(jié)論

本文詳細介紹了多尺度系統(tǒng)模型和自適應(yīng)信息融合設(shè)計方法，以飛行器上SINS/GNSS/ALTM/AHRS多源組合導(dǎo)航系統(tǒng)作為研究對象，通過MATLAB仿真，將本文所提算法融合效果與前人研究的抗差濾波算法進行對比。實驗結(jié)果表明，經(jīng)異質(zhì)異步多傳感器組合導(dǎo)航算法進行數(shù)據(jù)融合后，位置精度和速度精度均有一定的提高。在計算復(fù)雜度允許的情況下，本文算法能解決一定范圍內(nèi)的量測延遲，并且能處理試驗中比較常見的量測異常和動力學(xué)模型擾動的問題。