SR-CH∞KF用于彈丸飛行姿態(tài)測量研究

張平安, 汪 偉,*, 高 敏, 王 毅

(1. 陸軍工程大學(xué)石家莊校區(qū)火炮工程系, 河北 石家莊 050084; 2. 陸軍工程大學(xué)石家莊校區(qū)導(dǎo)彈工程系, 河北 石家莊 050084)

0 引 言

彈丸飛行姿態(tài)估計的目標(biāo)是確定物體固定坐標(biāo)的方向相對于導(dǎo)航坐標(biāo)系的方向。由于姿態(tài)估計問題本質(zhì)上是一個非線性濾波問題,于是許多非線性濾波方法被提出。非線性濾波中比較具有代表性的有擴展卡爾曼濾波(extended Kalman filter,EKF)、無跡卡爾曼濾波(unscented Kalman filter,UKF)以及容積卡爾曼濾波(cubatu Kalman filter, CKF)。EKF圍繞濾波值將非線性狀態(tài)過程和量測模型展開成泰勒級數(shù)并省去二階以上的項,得到一階近似線性化模型,然后根據(jù)線性卡爾曼濾波原理對系統(tǒng)進(jìn)行濾波處理。這種方法的前提是非線性函數(shù)具有現(xiàn)實的表達(dá)式且存在偏導(dǎo)數(shù)。一階近似得到的Jacobian矩陣與實際模型之間存在誤差,在系統(tǒng)模型傳遞過程中誤差會出現(xiàn)累積現(xiàn)象,導(dǎo)致最后計算值的發(fā)散。Julierder等人摒棄了對非線性函數(shù)進(jìn)行線性化的傳統(tǒng)做法,采用無跡變換(unscented transform,UT)對非線性函數(shù)的概率密度分布近似的方法來處理均值和協(xié)方差的非線性傳遞問題,提出了UKF的算法。與UKF相似,CKF利用球面徑向規(guī)則來逼近非線性函數(shù)的概率密度分布。雖然UKF和CKF來自不同的數(shù)學(xué)觀點,但可從幾個方面進(jìn)行比較。兩者都使用確定性的Sigma點來獲取均值和協(xié)方差,UKF需要2+1個Sigma點,而CKF需要2個容積點。UKF需要附加一個與狀態(tài)維數(shù)有關(guān)的縮放參數(shù),而CKF在采樣的過程中不需要其他參數(shù)的加入,無論狀態(tài)維數(shù)為多少,始終可以保持權(quán)值為正值,使得濾波能夠順利進(jìn)行下去。因此,CKF比UKF在高維系統(tǒng)狀態(tài)估計中有更好的應(yīng)用。

標(biāo)準(zhǔn)的CKF算法經(jīng)過多次遞推,計算的舍入誤差會逐漸累積,導(dǎo)致誤差協(xié)方差矩陣失去對稱性和正定性,這種累積誤差最常出現(xiàn)在彈丸的姿態(tài)測量中。近年來,許多學(xué)者針對這一現(xiàn)象進(jìn)行了大量的研究。其中,Qiu在標(biāo)準(zhǔn)的CKF中引入了基于多重強跟蹤的自適應(yīng)Huber算法,可以有效改善CKF的濾波性能;Geng通過建立基于預(yù)測殘差構(gòu)造的自適應(yīng)因子來克服CKF模型誤差和異常干擾的影響;Tang利用矩陣的奇異值分解(orthogonal decomposition, QR)和四元數(shù)數(shù)值積分構(gòu)建新的CKF算法,有效提高了濾波的收斂速度;Huang為了提高CKF的調(diào)節(jié)能力,提出了一種穩(wěn)定的高強跟蹤CKF算法。對此,為了減少濾波運算過程的復(fù)雜度,應(yīng)用了數(shù)值穩(wěn)定性較強的QR方法,可以有效分解系統(tǒng)狀態(tài)協(xié)方差矩陣并進(jìn)行迭代,提高了算法運算過程中的數(shù)值穩(wěn)定性。H∞濾波是假設(shè)在噪聲干擾環(huán)境最壞的情況下建立的線性狀態(tài)估計方法,適用于系統(tǒng)過程噪聲和量測噪聲統(tǒng)計特性未知的狀態(tài)估計問題,同時對系統(tǒng)建模不準(zhǔn)確有一定的適應(yīng)能力。本文提出了一種全新的基于H∞濾波改進(jìn)的SR-CKF算法,將H∞濾波的魯棒性優(yōu)勢與SR-CKF在非線性系統(tǒng)上的高精度處理能力相結(jié)合,利用三軸地磁傳感器和陀螺儀組合系統(tǒng)對彈丸的飛行姿態(tài)進(jìn)行解算。

本文提出了一個新穎的基于H∞濾波的姿態(tài)估計算法,有效提高了算法的魯棒性和濾波精度。與文獻(xiàn)[22]相比較,本文提出了自適應(yīng)的誤差補償參數(shù)計算并降低了濾波運算的復(fù)雜度。具體而言,本文的貢獻(xiàn)在以下幾個方面:① 在線性-非線性系統(tǒng)中,將H∞濾波融入到SR-CKF中,建立了平方根容積H∞濾波(SR-CH∞KF);② 在SR-CH∞KF中采用了歐拉角測量模型,利用線性狀態(tài)方程減輕了計算量;③ 從Silbley等人的工作中提取了誤差協(xié)方差矩陣和互協(xié)方差矩陣的近似方法,將線性狀態(tài)的H∞濾波成功地轉(zhuǎn)化到了非線性系統(tǒng)。此外,受這種方法啟發(fā),詳細(xì)推導(dǎo)了誤差協(xié)方差矩陣和量測噪聲的更新表達(dá)式。

1 傳感器配置和姿態(tài)測量模型

1.1 坐標(biāo)系與傳感器配置

由于彈丸是對稱構(gòu)建,彈丸的重心在彈軸上,因此傳感器安裝在彈軸位置處,陀螺儀和地磁傳感器呈前后位置安裝,安裝示意圖如圖1所示。

圖1 傳感器的安裝方式Fig.1 Installation method of sensor

圖1中,-為載體坐標(biāo)系,-為地磁傳感器坐標(biāo)系,1-為陀螺儀坐標(biāo)系。、和在安裝時與彈軸方向保持一致,與在安裝時保持平行,和在安裝時保持平行。為了更好地去表示彈丸在飛行過程中的飛行姿態(tài),選用北天東坐標(biāo)系作為導(dǎo)航坐標(biāo)系,用、和去表示坐標(biāo)系的北軸、豎直軸和東軸。

1.2 陀螺儀模型

彈丸在飛行過程中的姿態(tài)變化用彈丸在北-天-東坐標(biāo)系的俯仰角、偏航角以及滾轉(zhuǎn)角進(jìn)行表示,其中俯仰角是彈軸與水平面的高低夾角,用表示;偏航角是彈軸在水平面的投影與北軸的夾角,用表示;滾轉(zhuǎn)角是彈丸繞著自身彈軸翻滾的角度,用表示(3個姿態(tài)角的正負(fù)都滿足右手定則)。彈丸在飛行過程中的任一姿態(tài)都可以用圖2的示意圖進(jìn)行表示。

圖2 坐標(biāo)系轉(zhuǎn)換方式Fig.2 Coordinate systems conversion mode

在圖2中,彈丸任一姿態(tài)的起初都可以看作是載體坐標(biāo)系與北天東坐標(biāo)系完全重合,然后繞著軸旋轉(zhuǎn)角,繞著軸旋轉(zhuǎn)角,繞著軸滾轉(zhuǎn)角。

陀螺儀在彈丸飛行過程中能夠測量出彈丸在載體坐標(biāo)系中的3個角速度,分別用、和表示(3個角速度的正負(fù)滿足右手定則),其中是軸和軸繞著軸旋轉(zhuǎn)的角速度,是軸和軸繞著軸旋轉(zhuǎn)的角速度,是軸和軸繞著軸旋轉(zhuǎn)的角速度。陀螺儀測得的3個角速度與飛行器的姿態(tài)角速度的關(guān)系可以通過歐拉角微分方程得到,如下所示:

(1)

(2)

(3)

(4)

對俯仰角速度、偏航角速度和滾轉(zhuǎn)角速度進(jìn)行數(shù)值積分可以得到俯仰角、偏航角以及滾轉(zhuǎn)角,用數(shù)學(xué)模型表示為

(5)

(6)

(7)

由于彈丸的滾轉(zhuǎn)速度較大,會超出陀螺儀測量的量程,因此陀螺儀的軸在彈丸飛行過程中處于斷路狀態(tài)。陀螺儀測量計算俯仰角和偏航角中所需要的前一時刻滾轉(zhuǎn)角由地磁傳感器測量獲得。

1.3 地磁傳感器模型

地磁場作為地球的基本能量場,是地球的一種重要固有資源,近地面任意地點的地磁強度由地磁要素決定,地磁要素由經(jīng)度、緯度和海拔高度組成,為地磁導(dǎo)航提供了良好的測量基準(zhǔn)。根據(jù)發(fā)射位置的經(jīng)度、維度和高度,通過世界地磁場模型可以解算得到地磁矢量在北-天-東坐標(biāo)系中的磁傾角和磁偏角,進(jìn)而得到地磁矢量在北-天-東坐標(biāo)系3軸的分量、和,如下所示:

(8)

式中:為彈丸發(fā)射處的地磁強度;和分別為磁偏角和磁傾角,磁傾角以水平面上方為正,磁偏角以北偏西為正。由圖2所示的坐標(biāo)系轉(zhuǎn)換方式可知,彈丸飛行過程中載體坐標(biāo)系與導(dǎo)航坐標(biāo)系的關(guān)系可以表示為

(9)

彈丸飛行過程中地磁矢量在載體坐標(biāo)系3軸的投影可以表示為

=coscoscos(-)+sinsin

(10)

=-cossincoscos(-)-
cossinsin(-)+sincoscos

(11)

=cossinsincos(-)-coscos·
sin(-)-sincossin

(12)

由于地磁傳感器坐標(biāo)系與載體坐標(biāo)系完全重合,即=,=,=。由和之間的比值關(guān)系,可以得到滾轉(zhuǎn)角的解析式為

(13)

式中：1=tancos-sincos(-)。

在地磁傳感器測量彈丸飛行滾轉(zhuǎn)角時,同時要利用地磁傳感器計算的彈丸滾轉(zhuǎn)圈數(shù),計算圈數(shù)在文獻(xiàn)[24]中進(jìn)行了詳細(xì)說明。由于滾轉(zhuǎn)角的范圍在0°到360°之間,而反正切求值的范圍在-180°到180°之間,因此需要引入其他變量對滾轉(zhuǎn)角的求解范圍進(jìn)行重定義。滾轉(zhuǎn)角是彈丸繞著自身彈軸滾轉(zhuǎn)的角度,可以看成是載體坐標(biāo)系的軸繞著彈軸轉(zhuǎn)過的讀數(shù),滾轉(zhuǎn)角可以用象限來進(jìn)行判斷大小,如圖3所示。

圖3 滾轉(zhuǎn)角象限示意圖Fig.3 Diagram of rolling angle quadrants

在圖3中,0和0是滾轉(zhuǎn)角為0時地磁傳感器軸和軸的位置。圖3(a)～圖3(d)分別表示了滾轉(zhuǎn)角的范圍為0°～90°、90°～180°、180°～270°和270°～360°,用數(shù)學(xué)表達(dá)式可以表示為

(1)<0,>0,=+360°;

(2)<0,<0,=+90°+360°;

(3)>0,<0,=+180°+360°;

(4)>0,>0,=+270°+360°;

式中：為實際滾轉(zhuǎn)角的大小;為測出彈丸滾轉(zhuǎn)的圈數(shù);是由式(13)直接解出,用數(shù)學(xué)模型可以表示為

(14)

2 基于線性-非線性系統(tǒng)的SR-CKF運用

在實際工程應(yīng)用容積卡爾曼濾波算法時,由于計算機字長有限,可能導(dǎo)致以下結(jié)果:誤差協(xié)方差矩陣將失去對稱性和正定性,以及過濾器的數(shù)值穩(wěn)定性會受到較大影響。平方根容積卡爾曼濾波可以避免直接操作誤差協(xié)方差矩陣,采用矩陣的QR分解法,可較大提高過濾器的穩(wěn)定性,在較大程度上改善濾波在線性-非線性測量系統(tǒng)中的性能。

針對陀螺儀/地磁傳感器姿態(tài)測量系統(tǒng)中獲得的俯仰角和偏航角,利用式(5)、式(6)和式(10)建立狀態(tài)方程和量測方程:

(15)

式中:∈為×2維的系統(tǒng)狀態(tài)向量;∈為×1維的量測向量;-為輸入控制矩陣;為控制輸入矩陣;(·)為非線性函數(shù),-1和分別為狀態(tài)系統(tǒng)和觀測系統(tǒng)的均值為0,協(xié)方差分別為-1和的高斯白噪聲,且相互獨立。

線性-非線性系統(tǒng)的SR-CKF算法如下。

(1) 系統(tǒng)初始化

初始俯仰角和偏航角由飛行器發(fā)射姿態(tài)決定,初始誤差協(xié)方差由實際發(fā)射條件決定;

(2) 計算點集和均值

(16)

式中：=0,1,2,…,,表示基本容積點個數(shù),根據(jù)三階球面徑向容積準(zhǔn)則,容積點數(shù)是狀態(tài)變量維數(shù)的2倍,即=2,為狀態(tài)變量維數(shù);[]表示完整全對稱點集,由于狀態(tài)變量為俯仰角和偏航角,則=2,可以得到點集中的第個元素為

(3) 時間更新

① 計算容積點:

(17)

式中:Chol表示矩陣的Cholesky分解。

② 傳播容積點,計算狀態(tài)方程后的估計值:

,|-1=,-1+-1-1

(18)

③ 預(yù)測狀態(tài)變量,計算加權(quán)后容積點估計值的和:

(19)

④ 估計誤差協(xié)方差陣平方根系數(shù),三角分解:

(20)

式中:QR(·)表示通過矩陣的QR分解得到的下三角矩陣;,-1是狀態(tài)系統(tǒng)噪聲的協(xié)方差矩陣-1經(jīng)過Cholesky分解得到的矩陣。

(4) 測量更新

① 傳播量測容積點,計算量測方程的估計值:

(21)

② 量測預(yù)測,容積點預(yù)測值的加權(quán)和:

(22)

③ 計算新息協(xié)方差矩陣,三角分解:

(23)

式中:,是狀態(tài)系統(tǒng)噪聲的協(xié)方差矩陣經(jīng)過Cholesky分解得到的矩陣。

④ 計算互協(xié)方差矩陣:

(24)

⑤ 計算SR-CKF濾波增益值:

(25)

⑥ 計算狀態(tài)變量估計值,根據(jù)量測值更新狀態(tài)值:

(26)

⑦ 更新誤差協(xié)方差矩陣平方根系數(shù),矩陣的正交三角分解:

(27)

3 H∞濾波與SR-CKF結(jié)合估計飛行姿態(tài)

3.1 H∞濾波基本原理

通過引入H∞范數(shù)思想的方法,可以有效解決H∞濾波中的系統(tǒng)模型及噪聲統(tǒng)計特性的不確定性,同時可以保證干擾輸入到濾波輸出的過程中H∞范數(shù)的最小化,達(dá)到即使在最壞情況下系統(tǒng)的估計誤差也能保證最小化的目的。

定義下式所示的代價函數(shù)為

(28)

a。

H∞濾波器是為了使?fàn)顟B(tài)估計誤差最小,通常情況下,最優(yōu)H∞濾波問題存在一個較大的缺陷是最佳解析形式的解很難獲得,因此尋求一種新的高效迭代算法,∞的邊界由、和在最壞情況下設(shè)計規(guī)定一個門限值:

sup∞<

(29)

在H∞濾波器中,為了使濾波器的問題有解,在每一時刻必須滿足下式所示的不等式,即Riccati不等式:

(30)

在H∞濾波器中,誤差限定參數(shù)可以實現(xiàn)控制估計誤差,即使?fàn)顟B(tài)估計在最壞情況下,的取值與系統(tǒng)的魯棒性成反比。同時當(dāng)?shù)娜≈颠_(dá)到一定范圍時,H∞濾波器與標(biāo)準(zhǔn)的卡爾曼濾波的濾波特性相似。常規(guī)的誤差限定參數(shù)是根據(jù)實際工程經(jīng)驗設(shè)置為具體值,從而使濾波效果存在著很大的保守性,無法適應(yīng)隨時發(fā)生變化的彈丸飛行環(huán)境,不能同時實現(xiàn)降低系統(tǒng)估計誤差及提高系統(tǒng)魯棒性。因此,需要對誤差限定參數(shù)的取值進(jìn)行自適應(yīng)選取。

由于新息序列的平方和在提高濾波器的性能方面與誤差限定參數(shù)成反比,為了能夠?qū)崿F(xiàn)連續(xù)解算誤差限定參數(shù),引入了新息序列。新息序列的解算方法為

(31)

根據(jù)矩陣不等式相關(guān)理論可以引入一個定理:設(shè)和為2個階的Hermite矩陣,>0,≥0,則>?(<1)。這里()表示的最大特征值。根據(jù)定理由Riccati不等式可以得到:

(32)

進(jìn)而可以得到誤差限定參數(shù)的值為

(33)

(34)

3.2 容積H∞濾波

將H∞濾波融入容積卡爾曼濾波,形成容積卡爾曼H∞濾波器(CH∞F)。在CH∞F過程中,時間預(yù)測階段與CKF類似,包括誤差輔助矩陣的因子分解、估計容積和容積點傳播以及狀態(tài)預(yù)測和誤差協(xié)方差矩陣的預(yù)測。由于量測方程為非線性方程,則需要借鑒EKF的方法,利用雅可比矩陣將非線性方程近似轉(zhuǎn)化為線性方程進(jìn)行求解,H∞濾波的量測更新階段的更新狀態(tài)和量測更新協(xié)方差矩陣可表示為

(35)

(36)

Sibley等人利用雅可比矩陣將非線性轉(zhuǎn)化為線性的方法,提出了基于線性誤差傳播特性的誤差協(xié)方差和互協(xié)方差近似方法:

(37)

(38)

利用式(36)和式(37),對式(35)中部分進(jìn)行變化:

(39)

更新協(xié)方差矩陣可以轉(zhuǎn)化為

(40)

將式(37)和式(38)代入式(25),可以得到增益值的更新方程為

∞,=,|-1[,|-1+]

(41)

3.3 SR-CH∞KF濾波

將上述的增益值更新、狀態(tài)值估計和誤差協(xié)方差估計帶入到平方根容積卡爾曼濾波的濾波步驟中可以得到平方根容積H∞濾波。

在進(jìn)行判斷之前,首先按照式(37)將噪聲誤差估計值進(jìn)行解算,然后根據(jù)式(38)和式(40)對式(30)中的Riccati不等式進(jìn)行轉(zhuǎn)化,得到:

(42)

根據(jù)實際情況選取合適的誤差限定參數(shù)后,進(jìn)行測量更新計算。計算SR-CH∞F濾波增益值的解析式如式(41)所示,狀態(tài)更新值計算可以參照式(35)進(jìn)行計算,誤差協(xié)方差矩陣更新可以參照式(39)和式(40)進(jìn)行解算。

SR-CKF算法中的和,可以用H∞濾波系統(tǒng)中的濾波增益值和誤差協(xié)方差矩陣遞推式進(jìn)行替代,并進(jìn)行量測噪聲的不斷更新,構(gòu)成了基于H∞濾波的SR-CKF算法,流程圖如圖4所示。

圖4 SR-CH∞KF流程圖Fig.4 SR-CH∞KF flow chart

4 仿真校驗

為了驗證SR-CH∞KF在線性-非線性系統(tǒng)估計問題的性能,我們采取了仿真實驗的方式,利用彈丸實際飛行軌跡數(shù)據(jù)作為理論真值,用理論真值加上量測噪聲作為地磁傳感器X軸的量測輸出值。在仿真實驗之前,必須要知道仿真發(fā)射地點的經(jīng)度、緯度以及海拔高度,以成都某地為例,經(jīng)度為30.67°,緯度為104.07°以及海拔高度為523.87 m,利用基于國際地磁學(xué)和高空物理協(xié)會(International Association of Geomagnetism and Aeronomy,IAGA)給出的最新國際地磁參考場(International Geomagnetic Reference Field,IGRF),得到如表1所示的地磁參數(shù)。

表1 地磁參數(shù)

彈丸需要對發(fā)射速度和發(fā)射姿態(tài)進(jìn)行確定,同時在姿態(tài)測量中,陀螺儀也需要輸入初始姿態(tài)才能進(jìn)行后續(xù)姿態(tài)解算。因此在仿真實驗中,也需要對初始條件進(jìn)行首要確定,仿真實驗的初始條件如表2所示。

表2 仿真條件

為了更加直觀了解SR-CH∞KF的精確性,我們分別計算了容積卡爾曼濾波和平方根容積卡爾曼濾波算法的性能作為對比。圖5中藍(lán)色代表經(jīng)過CKF后的俯仰角誤差變化曲線,黑色代表經(jīng)過SR-CKF后的俯仰角誤差變化曲線,紅色代表經(jīng)過SR-CH∞KF后的俯仰角誤差變化曲線。經(jīng)過觀察3種濾波后的俯仰角誤差范圍,可以知道CKF的濾波效果在3種濾波中是最差的; SR-CKF誤差變化曲線波動較大,但是比CKF的精度高;3者之中,SR-CH∞KF關(guān)于俯仰角的濾波精度最高。

圖5 俯仰角誤差對比示意圖Fig.5 Schematic diagram of pitch angle error comparison

圖6中的曲線顏色代表的濾波種類和圖5相同,通過觀察偏航角誤差對比示意圖,可以發(fā)現(xiàn)CKF的濾波效果最差,SR-CKF的誤差變化曲線在前期波動較大,后期的誤差曲線較為平穩(wěn);SR-CH∞KF的濾波曲線較為平穩(wěn),并且相比較于其他兩種濾波,濾波精度較高。

圖6 偏航角誤差對比示意圖Fig.6 Comparison diagram of yaw angle error

圖7中的曲線顏色代表的濾波種類和圖5中相同,在60～80 s內(nèi)由于收到地磁盲區(qū)的影響,使?jié)L轉(zhuǎn)角的誤差較大,但是從圖6可以看出,SR-CH∞KF可以有效地降低磁測量盲區(qū)的滾轉(zhuǎn)角測量誤差。在非盲區(qū)范圍內(nèi), SR-CH∞KF的濾波精度相對較高。從整體的滾轉(zhuǎn)角測量誤差變化來看,可以知道SR-CH∞KF對于滾轉(zhuǎn)角的濾波精度較高。

圖7 滾轉(zhuǎn)角誤差對比示意圖Fig.7 Comparison diagram of roll angle error

5 結(jié) 論

本文提出了一種基于SR-CH∞KF算法的高旋飛行器姿態(tài)測量方法。該方法將H∞濾波與SR-CKF算法相結(jié)合,提出了適用于線性-非線性系統(tǒng)的詳細(xì)濾波算法。與眾不同的是,該算法能夠適用于量側(cè)噪聲不確定的情況下,利用更新新息序列來不斷計算誤差限定參數(shù)并不斷修正量測噪聲,可以有效降低量測噪聲對實驗結(jié)果的影響和提高測量系統(tǒng)的魯棒性和濾波效率。通過仿真實驗表明,SR-CH∞KF算法相比較于CKF和SR-CKF,可以有效地提高高旋飛行器的姿態(tài)解算精度,為后續(xù)飛行導(dǎo)航和控制奠定了良好的技術(shù)基礎(chǔ)。