自適應(yīng)聯(lián)邦卡爾曼濾波的時(shí)間配準(zhǔn)方法

趙明亮，汪立新，秦偉偉

(火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，陜西 西安 710025)

0 引言

組合導(dǎo)航信息融合系統(tǒng)中存在系統(tǒng)誤差與隨機(jī)誤差，傳感器時(shí)間配準(zhǔn)問題主要是為了消除系統(tǒng)誤差[1]。

由于傳感器的采樣頻率、測量誤差、采樣起始時(shí)間的不同等，在進(jìn)行多傳感器信息融合處理前應(yīng)把傳感器數(shù)據(jù)同步至相同時(shí)刻，這個(gè)過程稱為時(shí)間配準(zhǔn)[2]。實(shí)際應(yīng)用表明不進(jìn)行時(shí)間配準(zhǔn)或配準(zhǔn)精度不高，都可能導(dǎo)致信息融合結(jié)果不理想[3]。目前常用的時(shí)間配準(zhǔn)方法有泰勒展開修正法、最小二乘法、內(nèi)插外推法、最大熵準(zhǔn)則法和濾波方法等。

文獻(xiàn)[4]應(yīng)用泰勒展開修正法來修正時(shí)間數(shù)據(jù)，但算法要求采樣間隔必須為等間隔，且算法存在近似計(jì)算，配準(zhǔn)精度不高；文獻(xiàn)[5-7]采用最小二乘規(guī)則對多傳感器數(shù)據(jù)進(jìn)行虛擬融合，但采樣點(diǎn)必須嚴(yán)格對應(yīng)、起始采樣點(diǎn)必須相同且配準(zhǔn)精度較低；文獻(xiàn)[8-9]采用曲線擬合最小二乘法進(jìn)行時(shí)間配準(zhǔn)，但當(dāng)配準(zhǔn)時(shí)刻在擬合曲線的端點(diǎn)處就存在發(fā)散情況，致使配準(zhǔn)精度在端點(diǎn)處急劇下降，影響配準(zhǔn)精度。文獻(xiàn)[10]提出的內(nèi)插外推法要求系統(tǒng)運(yùn)動(dòng)狀態(tài)確定，且算法配準(zhǔn)精度不高；文獻(xiàn)[11-12]運(yùn)用最大熵準(zhǔn)則與最小均方估計(jì)來對不同速率信號(hào)進(jìn)行配準(zhǔn)，但算法對量測信號(hào)的統(tǒng)計(jì)特性和不同量測間相關(guān)度有要求，且精度較低；文獻(xiàn)[3]采用EKF(extended Kalman filter)算法對目標(biāo)進(jìn)行時(shí)間配準(zhǔn)，但是該算法的濾波過程是在假設(shè)目標(biāo)運(yùn)動(dòng)模型已知，若運(yùn)動(dòng)模型失配，則算法的配準(zhǔn)精度急劇下降；文獻(xiàn)[13]采用H∞濾波法，但其通常僅在某種特定機(jī)動(dòng)條件下精度相對較高，而復(fù)雜機(jī)動(dòng)形式下不總是最優(yōu)的。

本文針對現(xiàn)有時(shí)間配準(zhǔn)算法存在的配準(zhǔn)精度低、只能在限定條件下使用等不足，提出了一種基于自適應(yīng)聯(lián)邦卡爾曼濾波的分段重疊采樣周期時(shí)間配準(zhǔn)算法，通過對量測信息進(jìn)行分段處理，將各個(gè)傳感器輸出的量測信息統(tǒng)一到同一時(shí)刻且不降低其精度，有效抑制了時(shí)間誤差影響。應(yīng)用于SINS/GPS組合導(dǎo)航系統(tǒng)，實(shí)驗(yàn)表明算法能有效提高濾波精度、數(shù)據(jù)平穩(wěn)性，具有工程意義。

1 組合導(dǎo)航時(shí)間誤差分析

已知慣導(dǎo)系統(tǒng)比力方程如下[14]：

(1)

不考慮誤差，由式(1)可得速度理想值為

(2)

實(shí)際系統(tǒng)中考慮誤差，實(shí)際速度為

(3)

[δKA]=diag(δKAx,δKAy,δKAz);

式中:φE,φN,φU為姿態(tài)誤差角;δKAi和δAi分別為加表刻度系數(shù)誤差和安裝誤差角。

用式(3)減去式(2)，忽略δgn的影響，略去二階小量，得

(4)

又由四元數(shù)法推導(dǎo)得出姿態(tài)誤差方程為[14]

(5)

首先分析時(shí)間誤差對速度估計(jì)的影響，建立組合導(dǎo)航系統(tǒng)量測方程，則速度觀測量為

(6)

(7)

當(dāng)系統(tǒng)作直線加速運(yùn)動(dòng)時(shí)，速度狀態(tài)變?yōu)?/p>

(8)

(9)

由式(8)知，直線加速運(yùn)動(dòng)時(shí)，時(shí)間誤差Δt影響速度估計(jì)量，但Δt一般為小量，此種情況可不計(jì)。

當(dāng)系統(tǒng)作圓周運(yùn)動(dòng)時(shí)，速度狀態(tài)變?yōu)?/p>

(10)

(11)

由式(10)知，系統(tǒng)作圓周運(yùn)動(dòng)時(shí)，時(shí)間誤差與系統(tǒng)方位角有信息一致性，在進(jìn)行數(shù)據(jù)濾波時(shí)，會(huì)把時(shí)間誤差的來源歸于方位角，進(jìn)而直接造成方位角的估計(jì)錯(cuò)誤，給速度估計(jì)帶來較大誤差。由上述分析可知，載體的牽連加速度是時(shí)間誤差對速度估計(jì)的主要來源。

其次，分析時(shí)間誤差在對位置估計(jì)的影響。已知位置誤差如下：

(12)

建立組合導(dǎo)航系統(tǒng)量測方程，則位置量測為

(13)

綜上所述，時(shí)間誤差會(huì)對速度估計(jì)與位置估計(jì)產(chǎn)生影響。為提高組合導(dǎo)航系統(tǒng)精度,對時(shí)間誤差必須加以消除。

2 分段重疊時(shí)間配準(zhǔn)算法

在組合導(dǎo)航系統(tǒng)中，由于傳感器的測量數(shù)據(jù)不同步(初始采樣時(shí)刻不同、采樣頻率不同、傳輸延遲等)從而導(dǎo)致時(shí)間誤差。圖1所示在有4個(gè)傳感器A，B，C，D的組合導(dǎo)航系統(tǒng)中。A與B采樣間隔不同且起始時(shí)刻不同；A和C為異類傳感器采樣間隔不同；A和D采樣率雖然相同但由于有傳輸延遲等因素的影響造成采樣不同步。

圖1 多個(gè)傳感器時(shí)間誤差圖Fig.1 Time error schematic diagram of multi-sensor system

分段重疊配準(zhǔn)算法步驟如下：

第1步：進(jìn)行分段處理，確定分段間隔ΔT。分段間隔的選取必須保證在間隔內(nèi)每個(gè)傳感器都應(yīng)有測量值，故分段間隔ΔT須大于等于最大傳感器采樣間隔，即ΔT≥Tmax。

第2步：進(jìn)行重疊處理，確定重疊區(qū)間T′。重疊區(qū)間的選取既要考慮高的采樣率又要考慮計(jì)算機(jī)實(shí)時(shí)計(jì)算的時(shí)效性，故重疊區(qū)間T′考慮選取各個(gè)傳感器采樣周期的均值且應(yīng)排除最值情況。故T′為

(14)

式中：n為傳感器的數(shù)量；Ti為各傳感器采樣周期；Tmax為各傳感器中采樣周期的最大值；Tmin為各傳感器中采樣周期的最小值。

第3步：確定組合系統(tǒng)處理區(qū)間。配準(zhǔn)算法的目的是為了將多傳感器的量測信息分別整合到處理區(qū)間。定義系統(tǒng)接收到量測信息的時(shí)刻為配準(zhǔn)起始時(shí)刻T0，則配準(zhǔn)區(qū)間為：[T0+nΔT,T0+nΔT+T′]。

第4步：進(jìn)行量測投影，確定傳感器在處理區(qū)間的量測值。使用最小二乘法進(jìn)行估計(jì)[15]。

(15)

應(yīng)用最小二乘法可解得其最小二乘解及其相應(yīng)方差陣估計(jì)值如下：

(16)

(17)

第5步：進(jìn)行數(shù)據(jù)濾波，得到各傳感器濾波量測值。

通過步驟4便可得到各傳感器處于同一時(shí)刻且間隔區(qū)間為T′量測估計(jì)值，作為各傳感器量測值。設(shè)計(jì)聯(lián)邦卡爾曼濾波器對組合導(dǎo)航系統(tǒng)進(jìn)行估計(jì)，經(jīng)典聯(lián)邦卡爾曼濾波器在實(shí)際應(yīng)用時(shí)，由于環(huán)境等不確定因素的影響，各子傳感器和局部濾波器的濾波性能可能會(huì)發(fā)生變化[16]。故本文設(shè)計(jì)按即時(shí)性能調(diào)整的信息分配因子的自適應(yīng)聯(lián)邦卡爾曼濾波器。

由各個(gè)子傳感器的系統(tǒng)方程，得到系統(tǒng)的狀態(tài)空間方程，離散化后可得狀態(tài)空間模型如下：

(18)

自適應(yīng)聯(lián)邦卡爾曼濾波設(shè)計(jì)如下：

(1) 信息分配

(19)

式中：βi為信息分配因子，且滿足信息守恒定律：

(20)

(2) 時(shí)間更新

(21)

(22)

(3) 量測更新

(23)

(24)

(25)

(4) 信息融合

(26)

(27)

(5) 基于濾波協(xié)方差陣特征值分解，估計(jì)誤差協(xié)方差陣Pi按其特征值分解為

Pi=LΛiLT,

(28)

式中：Λi=diag{λi1,λi2,…,λin}，λi1,λi2,…,λin為Pi的特征值。

由Pi的定義可知以λi1,λi2,…,λin與估計(jì)狀態(tài)的估計(jì)誤差方差相對應(yīng)。故使

(29)

式中：trΛi表示矩陣的跡，在數(shù)學(xué)上定義為Λi的特征值的和。

3 實(shí)驗(yàn)驗(yàn)證

設(shè)計(jì)實(shí)際的組合導(dǎo)航系統(tǒng)為實(shí)驗(yàn)對象進(jìn)行推車實(shí)驗(yàn)，組合導(dǎo)航系統(tǒng)采用中航618所生產(chǎn)的某型激光捷聯(lián)慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)，如圖2所示。

圖2 實(shí)驗(yàn)設(shè)備圖Fig.2 Experimental equipment diagram

組合導(dǎo)航系統(tǒng)模式選取為“緊組合”，采樣輸出周期為10 s，實(shí)驗(yàn)總共進(jìn)行1 970 s。由于試驗(yàn)在地面進(jìn)行，且行進(jìn)距離及速度有限，故忽略天向速度誤差及高度誤差。

實(shí)驗(yàn)開始前先進(jìn)行10 min慣組加溫和8 min左右的羅經(jīng)對準(zhǔn)(受實(shí)際環(huán)境影響)。推車初始位置為東經(jīng)109.126 2°，北緯34.312 6°，高度463.3 m；初始速度為0 m/s，初始航向角與北向夾角45°；激光陀螺初始位置誤差5 m，初始速度誤差為0.1 m/s，初始姿態(tài)誤差為10′；GPS接收機(jī)的偽距白噪聲為5 m，偽距率白噪聲為0.01 m/s。SINS解算周期為0.02 s，GPS采樣率為1 pps，采樣周期為1 s。代入配準(zhǔn)算法中，可得ΔT=1 s，T′=0.51 s。

實(shí)驗(yàn)行進(jìn)軌跡如圖3所示。速度信息與姿態(tài)角信息如圖4,5所示。

系統(tǒng)的狀態(tài)方程表示為

(30)

式中：

X(t)=(φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,

(31)

圖3 運(yùn)動(dòng)軌跡圖Fig.3 Path of particle

圖4 速度信息Fig.4 Speed information

圖5 姿態(tài)角信息Fig.5 Attitude angel information

狀態(tài)轉(zhuǎn)移陣可表示為如下形式：

(32)

噪聲驅(qū)動(dòng)陣G(t)可表示為如下形式：

(33)

噪聲陣W(t)可表示為

W(t)=(ωgx,ωgy,ωgz,ωax,ωay,ωaz,ωtu,ωtru)，

(34)

式中：ωgx，ωgy，ωgz為陀螺量測白噪聲；ωax，ωay，ωaz為加表量測白噪聲；ωtu和ωtru分別為鐘差白噪聲和鐘漂白噪聲。

偽距差量測方程如下：

Zp(t)=Hp(t)X(t)+Vp(t),

(35)

偽距率差量測方程如下：

Zv(t)=Hv(t)X(t)+Vv(t)，

(36)

應(yīng)用配準(zhǔn)算法前后組合導(dǎo)航系統(tǒng)的位置誤差曲線與速度誤差曲線如圖6，7所示，各誤差項(xiàng)RMSE(root mean square error)如表1所示。由圖示誤差曲線可知，未進(jìn)行配準(zhǔn)時(shí)，估計(jì)誤差隨時(shí)間都有較大且無確定規(guī)律波動(dòng)，由第1節(jié)時(shí)間誤差分析可知，這種不規(guī)律的波動(dòng)與時(shí)間誤差的不規(guī)律性是一致的；而在應(yīng)用配準(zhǔn)算法后，緯度誤差RMSE減小53.29%，經(jīng)度誤差RMSE減小55.54%，東向速度誤差RMSE減小47.79%，北向速度誤差RMSE減小27.71%，有效抑制了時(shí)間誤差，估計(jì)誤差顯著減小。

圖6 配準(zhǔn)前后位置誤差曲線圖Fig.6 Position error curves before and after time registration

圖7 配準(zhǔn)前后速度誤差曲線圖Fig.7 Velocity error curves before and after time registration

誤差項(xiàng)目配準(zhǔn)前配準(zhǔn)后緯度誤差/m0.707 20.330 3經(jīng)度誤差/m1.085 70.482 7東向速度誤差/(m·s-1)0.330 00.172 3北向速度誤差/(m·s-1)0.250 40.132 1

4 結(jié)束語

本文提出一種基于自適應(yīng)聯(lián)邦卡爾曼濾波的重疊分段時(shí)間配準(zhǔn)算法。通過對傳感器量測信息進(jìn)行重疊分區(qū)處理，將不同量測量統(tǒng)一至同一時(shí)刻處理。組合導(dǎo)航試驗(yàn)表明，算法有效提高了采樣率與配準(zhǔn)精度，抑制了不規(guī)則的時(shí)間誤差，顯著減小了系統(tǒng)估計(jì)誤差，提升了系統(tǒng)平穩(wěn)性。且算法簡單易實(shí)現(xiàn)，有良好的應(yīng)用前景。