姚曉涵 陳 帥 楊 博 李 杰 張 敏

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

0 引言

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

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

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

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

1.1 多尺度模型描述

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

Sj+1=njSj1≤j≤N-1

(1)

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

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

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

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時(shí)刻的狀態(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個(gè)傳感器在采樣率Si下獲得的第k個(gè)觀測(cè)值，H(i,k)∈Rqi×n為觀測(cè)矩陣，觀測(cè)噪聲V(i,k)∈Rqi×1均值為0的高斯白噪聲，其協(xié)方差為R(i)，且不同采樣率下的觀測(cè)噪聲互不相關(guān)[7]。初始狀態(tài)向量X(N,0)為一隨機(jī)變量，且假設(shè)X(N,0)，W(N,k)，V(i,k)彼此間獨(dú)立[8]。

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

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

本文中聯(lián)邦卡爾曼濾波以慣性導(dǎo)航系統(tǒng)為基礎(chǔ)，其它系統(tǒng)分別與SINS構(gòu)建子濾波器。選取東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系。狀態(tài)量選用SINS慣性器件的誤差量，并加入陀螺的常值誤差、隨機(jī)誤差以及加速度計(jì)的隨機(jī)誤差，得到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子濾波器的量測(cè)方程為：

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子濾波器的量測(cè)方程為：

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子濾波器的量測(cè)方程為：

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

(10)

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

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

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

(11)

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

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

(12)

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

(13)

濾波增益：

(14)

狀態(tài)估計(jì)：

(15)

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

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

(16)

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

(17)

(18)

(19)

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

(20)

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

(21)

(22)

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

(23)

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

(24)

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

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

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

(25)

(26)

3 仿真校驗(yàn)

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

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

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

圖1 飛行軌跡

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

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

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

圖2 本文算法速度誤差

圖3 本文算法位置誤差

圖4 算法1速度誤差

圖5 算法1位置誤差

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

4 結(jié)論

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