SR-CH∞KF用于彈丸飛行姿態測量研究

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

(1. 陸軍工程大學石家莊校區火炮工程系, 河北 石家莊 050084; 2. 陸軍工程大學石家莊校區導彈工程系, 河北 石家莊 050084)

0 引 言

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

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

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

1 傳感器配置和姿態測量模型

1.1 坐標系與傳感器配置

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

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

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

1.2 陀螺儀模型

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

圖2 坐標系轉換方式Fig.2 Coordinate systems conversion mode

在圖2中,彈丸任一姿態的起初都可以看作是載體坐標系與北天東坐標系完全重合,然后繞著軸旋轉角,繞著軸旋轉角,繞著軸滾轉角。

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

(1)

(2)

(3)

(4)

對俯仰角速度、偏航角速度和滾轉角速度進行數值積分可以得到俯仰角、偏航角以及滾轉角,用數學模型表示為

(5)

(6)

(7)

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

1.3 地磁傳感器模型

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

(8)

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

(9)

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

=coscoscos(-)+sinsin

(10)

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

(11)

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

(12)

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

(13)

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

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

圖3 滾轉角象限示意圖Fig.3 Diagram of rolling angle quadrants

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

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

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

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

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

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

(14)

2 基于線性-非線性系統的SR-CKF運用

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

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

(15)

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

線性-非線性系統的SR-CKF算法如下。

(1) 系統初始化

初始俯仰角和偏航角由飛行器發射姿態決定,初始誤差協方差由實際發射條件決定;

(2) 計算點集和均值

(16)

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

(3) 時間更新

① 計算容積點:

(17)

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

② 傳播容積點,計算狀態方程后的估計值:

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

(18)

③ 預測狀態變量,計算加權后容積點估計值的和:

(19)

④ 估計誤差協方差陣平方根系數,三角分解:

(20)

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

(4) 測量更新

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

(21)

② 量測預測,容積點預測值的加權和:

(22)

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

(23)

式中:,是狀態系統噪聲的協方差矩陣經過Cholesky分解得到的矩陣。

④ 計算互協方差矩陣:

(24)

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

(25)

⑥ 計算狀態變量估計值,根據量測值更新狀態值:

(26)

⑦ 更新誤差協方差矩陣平方根系數,矩陣的正交三角分解:

(27)

3 H∞濾波與SR-CKF結合估計飛行姿態

3.1 H∞濾波基本原理

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

定義下式所示的代價函數為

(28)

a。

H∞濾波器是為了使狀態估計誤差最小,通常情況下,最優H∞濾波問題存在一個較大的缺陷是最佳解析形式的解很難獲得,因此尋求一種新的高效迭代算法,∞的邊界由、和在最壞情況下設計規定一個門限值:

sup∞<

(29)

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

(30)

在H∞濾波器中,誤差限定參數可以實現控制估計誤差,即使狀態估計在最壞情況下,的取值與系統的魯棒性成反比。同時當的取值達到一定范圍時,H∞濾波器與標準的卡爾曼濾波的濾波特性相似。常規的誤差限定參數是根據實際工程經驗設置為具體值,從而使濾波效果存在著很大的保守性,無法適應隨時發生變化的彈丸飛行環境,不能同時實現降低系統估計誤差及提高系統魯棒性。因此,需要對誤差限定參數的取值進行自適應選取。

由于新息序列的平方和在提高濾波器的性能方面與誤差限定參數成反比,為了能夠實現連續解算誤差限定參數,引入了新息序列。新息序列的解算方法為

(31)

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

(32)

進而可以得到誤差限定參數的值為

(33)

(34)

3.2 容積H∞濾波

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

(35)

(36)

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

(37)

(38)

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

(39)

更新協方差矩陣可以轉化為

(40)

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

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

(41)

3.3 SR-CH∞KF濾波

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

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

(42)

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

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

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

4 仿真校驗

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

表1 地磁參數

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

表2 仿真條件

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

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

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

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

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

圖7 滾轉角誤差對比示意圖Fig.7 Comparison diagram of roll angle error

5 結 論

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