一種大安裝偏差快速傳遞對準中的閉環卡爾曼濾波算法

李壽鵬，穆榮軍，崔乃剛，龍 騰

（哈爾濱工業大學 航天學院，哈爾濱 150001）

機載導彈發射前，彈載低精度子慣導以機載高精度主慣導作為位姿基準，通過動基座傳遞對準技術快速獲取準確初始狀態。由于發射架與機翼、導彈間的安裝方式以及機翼形變等原因，主子慣導間比較容易產生較大的安裝偏差，導致慣導姿態與速度誤差模型的非線性增強，如果仍采用KF 算法，則會因模型線性化誤差而導致濾波器估計精度下降。

當前解決該問題的思路基本有兩種，一類是建立非線性系統模型，采用非線性濾波算法[1]，另一類是將非線性系統轉化或近似為線性[2]，依然采用線性最優的KF 算法。從非線性系統角度考慮該問題，采用的非線性濾波算法包括UKF[3]、容積卡爾曼濾波（Cubature Kalman Filter，CKF）[4]、粒子濾波（Particle Filter，PF）[5]等，但非線性濾波器計算量較大的問題是影響實時性的主要因素，因此一些算法根據具體模型，在不損失精度的前提下對非線性濾波器進行改進或簡化，如簡化UKF[2]，簡化高階CKF[6]等。

當從線性角度考慮該問題時，研究重點則集中于如何將非線性系統轉化為線性系統以適應KF 算法框架。典型的是采用擴展卡爾曼濾波器（Extended Kalman Filter，EKF），通過泰勒展開將非線性系統線性化，但由于EKF 算法僅保留了一階項，在系統非線性較強時會產生難以忽略的高階截斷誤差，可以通過保留高階項來彌補該缺點，如二階EKF[7]。文獻[8]基于加性四元數推導出線性的姿態誤差模型，但是速度誤差模型依然無法擺脫非線性。

系統線性化的另一個思路是首先粗略估計出子慣導姿態誤差并補償，使其滿足小角度假設，然后在此基礎上采用線性傳遞對準模型和KF 完成姿態誤差的精確估計。文獻[9]采用數秒子慣導數據粗略算出水平姿態，來構造與子慣導姿態接近的虛擬主慣導，作為后續對準過程中的子慣導姿態基準；文獻[10]則首先通過線性模型估計出姿態誤差，補償后再采用相同的模型進行二次精確估計，但需要存儲數秒主子慣導數據。

針對以上問題，通過對非線性模型的線性化過程進行詳細分析，得出姿態誤差高階耦合項將導致線性濾波器的估計精度下降。采用安裝角估計值和主慣導姿態共同建立子慣導姿態基準，使子慣導姿態與基準之間的安裝關系滿足小角度假設，從而可以繼續使用KF 算法。

在傳遞對準過程中，為使系統始終保持良好的線性狀態，設計了一種反饋校正機制，即在每次量測更新后，修正主子慣導間的安裝角估計值、子慣導解算出的狀態量以及陀螺儀零偏估計值，使所建立的子慣導基準系與子慣導狀態逐漸逼近實際狀態，并采用陀螺儀零偏估計值對子慣導輸出數據進行補償，然后再進行子慣導狀態解算，以延緩其姿態發散的速度。通過仿真實驗與對比分析，表明了所設計的閉環卡爾曼濾波器（Closed Loop Kalman Filter，CLKF）在幾乎不增加計算量的前提下，有效地提高了線性濾波器的估計精度，并消除了系統非線性對傳遞對準的影響，具有一定的工程實用性。

1 坐標系定義

本文所涉及到的坐標系定義如下:地心慣性坐標系i；地心地固坐標系e；選擇“東-北-天”地理坐標系為導航系n；選擇載體的“右-前-上”坐標系為載體系b；主慣導載體系記作m；子慣導標稱載體系、計算載體系和實際載體系分別記作、s*和s；由安裝矩陣估計值和主慣導姿態矩陣構建子慣導姿態基準矩陣，其中系即為子慣導基準載體系；各載體系間的關系如圖1所示，s?系與m系間的姿態誤差定義為安裝角ψa，s系與s?系間的姿態誤差定義為撓曲變形角θf，s*系與m系間的姿態誤差定義為載體系失準角ψm，系與m系間的姿態誤差為載體系失準角估計值。

圖1 不同載體系關系圖Fig.1 Diagram of different body frames

n系按照Z（ψ）→X（θ）→Y（γ）的轉序旋轉至b系，則n系與b系之間的姿態轉換矩陣為

式中:ψ為航向角；θ為俯仰角；γ為滾轉角；s、c分別為正、余弦函數。

2 傳遞對準模型

2.1 機翼撓曲變形模型

機翼撓曲變形通常采用白噪聲驅動的二階馬爾可夫過程來描述，具體為[11]

式中:θfi為撓曲變形角，相應方差為σi；ηi為高斯白噪聲，相應的功率譜密度為Qηi；βi為常數；σi，Qηi和βi間的關系為Qηi=4βi3σi2；隨機過程相關時間τi與βi間的關系為βi=2.146τi。

2.1 非線性模型

常用快速傳遞對準狀態模型有兩種:導航系失準角模型[12]和載體系失準角模型[13]，在小角度假設下，文獻[14]詳細地證明了兩種模型的等價性，且基于載體系失準角的傳遞對準模型的量測向量構造較為簡單，量測方程為線性的，因此本文采用載體系失準角模型。

根據文獻[6]的相關推導，并考慮慣性器件輸出的角速度和加速度包含誤差，其中僅考慮陀螺儀和加速度計的常值零偏，并忽略動態桿臂誤差，則可以獲得捷聯慣導姿態與速度誤差非線性模型為

式中:為子慣導陀螺儀輸出；為子慣導姿態矩陣的轉置，即

2.1.1 狀態模型

由于加速度計零偏可觀測度較低，在短時間內很難完成估計，因此在傳遞對準過程中不對該量進行估計。綜合考慮“速度+姿態”匹配模型的可觀測性和慣性器件常值誤差，分別選取子慣導載體系失準角ψm，子慣導速度誤差δVn，子慣導陀螺儀常值零偏εs，安裝角ψa，撓曲變形角θf以及變形角速度ωf作為狀態向量X，即

在主子慣導間安裝偏差較大的情況下，載體系失準角ψm以及安裝角ψa不滿足小角度假設，且載體系失準角矩陣、安裝角矩陣與的轉換順序類似，撓曲變形角θf滿足小角度假設，則滿足，其中向量ψ的反對稱矩陣ψ×的具體形式為

在對準期間認為陀螺儀零偏和安裝角保持不變，根據式(2)(3)(4)(5)，且忽略小量以及，則可得傳遞對準非線性狀態模型的矢量形式為

2.1.2 量測模型

選取子慣導載體系失準角和主子慣導間的速度之差作為量測向量Z，即

式中:為主慣導姿態矩陣。

由此可得傳遞對準觀測方程為

式中:V為量測噪聲；H為量測矩陣，具體形式為

式中: I6×6為6 階單位矩陣； 06×6為6 階零矩陣。

2.2 線性模型

2.2.1 狀態模型

線性模型采用與非線性模型相同的狀態向量，如式(6)所示，且在安裝偏差較小的情況下，載體系失準角和安裝角滿足小角度假設，即和，且根據式(4)可得的近似形式為

將上述近似關系代入傳遞對準非線性模型式(8)中，且忽略二階及以上小量，可獲得傳遞對準線性狀態模型矢量形式如下

2.2.2 量測模型

線性模型采用與非線性模型相同的量測向量，如式(9)所示，但求取載體系失準角量測值的方式略有不同，具體為:

2.3 模型線性化誤差分析

“速度+姿態”匹配快速傳遞對準一般在勻速平飛階段通過速度觀測完成水平姿態的估計，而在滾轉機動階段通過姿態觀測完成航向姿態的估計。因此，以下分別就勻速平飛條件下的速度通道和滾轉機動條件下的姿態通道展開分析。

當安裝偏差較大時，與導航系至載體系的轉動過程類似，姿態誤差歐拉角按照Z→X→Y的轉序，根據式(1)可以得到子慣導計算載體系s*與主慣導載體系m之間的轉換關系，如式(17)所示。采用泰勒公式將式(17)中的正、余弦函數展開，并保留二階及以下項，則可以得到近似關系為sinψ≈ψ和，利用該近似關系將姿態誤差矩陣展開，并忽略二階以上小量（不包括二階），可得近似關系如式(18)所示。

標稱載體系與主慣導載體系m之間的轉換關系及其近似關系分別與式(17)(18)類似，只需將其中失準角的下標m換作a即可，不再贅述。將和的近似關系代入式(8)中的速度誤差非線性模型，忽略二階以上小量（不包括二階）以及次要項，整理可得

式中:δC為由載體失準角矩陣和安裝角矩陣線性化引起的姿態誤差矩陣，具體形式為

當載機處于勻速平飛狀態時，子慣導水平兩軸加速度計與垂直軸相比所敏感的比力屬于小量，垂直軸加速度計的比力接近重力加速度，則加速度計輸出可近似為

式中:g為重力加速度。

因為載機水平姿態角為小量，可近似認為姿態中僅存在航向角，則子慣導姿態矩陣可近似為:

式中:ψs*為子慣導航向角。

將式（7）（20）（21）（22）代入式（19）中，并忽略小量（ψa-ψm）×，展開可得如下關系

從式(23)前兩分量可以看出，在水平速度誤差模型中，航向與水平姿態誤差相互耦合的二階項，經過重力加速度的放大而引起相對較大的輸入偏差，且認為短時間內加速度計零偏較難估計，則該偏差量與加速度計零偏在水平軸上的等效分量共同影響水平兩軸安裝角的估計精度。

由式（24）可以看出，姿態矩陣線性化誤差與陀螺儀零偏相互耦合而產生輸入偏差，但由于εs為小量，使得該耦合偏差較小，因此對εs的估計精度影響有限。

當載體處于姿態機動的狀態時，其載體系相對導航系的旋轉角速度滿足，將和的近似關系代入式(8)中的姿態誤差非線性模型，為突出線性化誤差對姿態誤差估計的影響，此處忽略陀螺儀零偏相關項，則可獲得如下關系

在“速度+姿態”匹配傳遞對準模型中，載體一般采用搖翼機動的方式來激勵航向通道快速收斂，且由于導航系相對慣性系的旋轉角速度為小量，可近似認為載體系相對導航系的旋轉角速度僅沿滾轉軸方向存在分量，則陀螺儀輸出可近似為

將式(7)(20)(26)代入式(25)中并展開，整理可得式(27)所示的關系從該式可以看出，依賴小角度假設的線性化方式，會忽略姿態誤差角的高階耦合項，該誤差項與載機滾轉角速度相耦合而引起載體失準角模型的輸入偏差，該偏差量會對俯仰軸與航向軸安裝角的估計精度產生影響。

2.4 線性化誤差對卡爾曼濾波器的影響

由2.3 節的分析可以看出，當主子慣導之間的安裝偏差較大時，仍然按照小角度假設來獲得傳遞對準線性化系統，將使狀態模型產生輸入偏差，而量測模型始終為線性，則不存在線性化誤差。傳遞對準過程中卡爾曼濾波器需根據系統離散模型不斷遞推完成相應狀態估計，且根據之前的分析，則建立僅狀態模型含有輸入偏差的離散系統模型，具體如下:

式中:Ak-1為狀態轉移矩陣，Uk-1為輸入偏差，Wk、Vk分別為過程噪聲和量測噪聲，滿足零均值高斯分布，即

式中:Qk為過程噪聲方差陣；Rk為量測噪聲方差陣。

定義先驗與后驗估計誤差為:

式中:、分別為先驗估計及其誤差；、分別為后驗估計及其誤差；Xk為狀態量實際值。

根據卡爾曼濾波方程和系統模型，可以獲得先驗和后驗估計誤差傳播方程為:

式中:Kk為卡爾曼濾波增益矩陣。

由上式可以獲得估計誤差均值的傳播方程為

可以看出，系統的輸入偏差Uk-1在每次濾波器時間更新階段會產生額外的累積偏差-Uk-1，在每次濾波器量測更新階段會產生額外的累積偏差- （I-KkHk）Uk-1，從而使得卡爾曼濾波器的后驗估計有偏。

3 快速傳遞對準閉環卡爾曼濾波器設計

本文所設計的基于CLKF 的傳遞對準算法，其主要思想是不直接以主慣導姿態作為子慣導姿態基準，而是尋找一個更接近子慣導姿態的基準，可以保持子慣導姿態與該基準間的姿態誤差始終滿足小角度假設，此時KF 的估計仍為線性最優，從而避免了使用計算量較大的非線性濾波器，并采用閉環反饋方式來維護系統的線性狀態，使子慣導初始姿態逐漸逼近實際值，傳遞對準流程如圖2所示。

圖2 基于CLKF 的快速傳遞對準流程圖Fig.2 Flow chart of rapid transfer alignment based on CLKF

具體描述如下:

第1 步:系統初始化

系統初始化主要包括子慣導解算和傳遞對準濾波器初始化，采用主慣導數據直接對子慣導進行初始狀態裝訂，濾波器狀態向量及其協方差陣初始化X0、P0，一般情況下X0=0，即將安裝角矩陣和載體系失準角矩陣初始化為

第2 步:子慣導狀態解算

當k時刻子慣導IMU 數據更新后，采用k- 1時刻陀螺儀零偏估計值εk-1對角速度測量值進行補償，采用零偏補償后的IMU 數據進行子慣導位姿解算，由此獲得k時刻解算出的子慣導姿態和速度；

第3 步:濾波器時間更新

采用零偏補償后的IMU 數據和子慣導解算出的姿態和速度進行濾波器時間更新，即

當主慣導數據未更新時，則返回第2 步繼續進行子慣導位姿解算和濾波器時間更新，此時不采用濾波器狀態對子慣導位姿和陀螺儀零偏進行修正；

第4 步:濾波器量測更新

當k時刻主慣導姿態和速度更新后，則進行濾波器量測更新，由k- 1時刻安裝矩陣估計值來補償，以構造子慣導姿態基準矩陣即為構造的子慣導基準載體系，然后采用構造濾波器量測向量Zk中的失準角部分，即

量測向量的速度誤差部分仍按照式（9）的方式構造，即取子慣導與主慣導間的速度之差。

在此基礎上完成量測更新，即

第5 步:子慣導狀態修正

該反饋校正算法在濾波器量測更新后，將對估計出的各項誤差量進行修正，由此可以得出，每次濾波器的更新量都是在之前估計值基礎上的增量，而非該量的實際估計值。

k時刻由濾波器量測更新所獲得的狀態向量，包括載體系失準角增量Δψm(k)、安裝角增量Δψa(k)、速度誤差增量和陀螺儀零偏增量Δεk。由Δψm(k)、Δψa(k)分別構造k時刻子慣導標稱載體系、計算載體系與基準載體系之間的矩陣估計值，并采用與Δεk對當前子慣導解算出的姿態與速度、陀螺儀零偏估計值εk-1以及安裝矩陣估計值進行修正，具體為

在量測更新與修正步驟完成后，將濾波器狀態向量中ψm、δVn、εs、ψa置零，并以修正后的子慣導狀態作為初值進行慣導解算。

4 仿真驗證與分析

4.1 仿真條件

仿真時間為45 s，載機飛行軌跡可劃分為三個階段，分別為:首先勻速平飛10 s，巡航速度200 m/s，然后搖翼機動10 s，滾轉角速度5 °/s，最后采用相同的速度勻速平飛25 s。主子慣導載體系間預設安裝角為ψa=[8 °,8 °,8 °]，機翼撓曲變形角的標準差分別為σ=[15′,20′,5′]，二階馬爾可夫過程的相關時間分別為τ=[5s,5s,10s]。主子慣導器件指標分別為:光纖主慣導，更新頻率100Hz，陀螺儀零偏0.01 ° h，隨機游走系數，加速度計零偏0.05mg，隨機游走系數；MEMS 子慣導，更新頻率200Hz，陀螺儀零偏0.02 ° s，隨機游走系數，加速度計零偏3mg，隨機游走系數

4.2 不同算法對比仿真與分析

采用不同算法在相同的條件下進行仿真，將KF、UKF 和CLKF 算法進行對比分析，不同濾波算法的安裝角和陀螺儀零偏估計誤差曲線如圖3與圖4所示。

圖3 不同算法的安裝角估計誤差Fig.3 Estimation errors of physical misalignment angles from different algorithms

表1 安裝角與陀螺儀零偏估計誤差Tab.1 Estimation errors of physical misalignment angles and gyro biases

相應的估計誤差值如表1所示，撓曲變形角和角速度估計值曲線如圖5與圖6所示，相應的估計誤差值如表2所示。

圖5 不同算法的撓曲變形角估計值Fig.5 Estimation values of flexure deformation angles from different algorithms

表2 撓曲變形角與角速度估計誤差Tab.2 Estimation errors of flexure deformation angles and angular velocities

根據上述仿真結果，由圖3和表1可以看出，CLKF 算法在水平安裝角估計精度上與UKF 算法相當，在航向安裝誤差角的估計精度上稍優于UKF 算法，且明顯優于KF 算法，表明CLKF 算法可以有效地補償航向與水平姿態誤差角的高階耦合項對線性卡爾曼濾波器估計精度的影響，在安裝角估計收斂速度方面，水平通道基本在載機平飛階段即可收斂，而航向通道則在開始搖翼機動后的5 s 時收斂，算法快速性可以滿足要求；由圖4和表1可以看出，不同算法在陀螺儀常值零偏的估計精度與收斂速度方面基本相當，表明模型線性化誤差對該項的估計影響較??；由圖5、圖6和表2可以看出，在對機翼撓曲變形角與角速度的估計方面，不同算法都可以實現機翼撓曲變形角與角速度的跟蹤，且估計精度與收斂速度也基本相當，水平通道在10 s 內收斂，而航向通道在15 s 內收斂。

為對比不同算法的計算量，統計不同算法在整個傳遞對準周期內，調用濾波器和整個程序的計算耗時，如表3所示，其中，仿真軟件采用Matlab2016b，計算機處理器為Intel Core i7-4790。從算法耗時的統計結果上可以看出:CLKF 算法的濾波器耗時與KF 較為接近且約為0.5 s，而UKF 算法的濾波器耗時則約為17.4 s，可見CLKF 的計算量與KF 相當，僅為UKF的3%，所設計的算法在保持估計精度的同時，可以有效地降低計算量。

表3 不同濾波器與總程序耗時Tab.3 Elapsed time of different filters and entire programs

5 結 論

通過對傳遞對準非線性模型線性化誤差的分析以及不同濾波算法的對比仿真，可以得出以下結論:

1)航向與水平姿態誤差角的高階耦合項經重力加速度的放大，對水平安裝角的估計精度產生較大影響，而姿態誤差角的高階耦合項經搖翼機動角速度的放大，對航向安裝角的估計精度產生影響，該耦合誤差與小量零偏項耦合對陀螺儀零偏的估計精度影響較弱；

2)CLKF 算法可以有效地補償模型線性化誤差對線性KF 的影響，其估計精度優于KF 且與UKF 相當；

3)在濾波器收斂速度方面，CLKF 算法與KF、UKF 相當，可以滿足傳遞對準對算法快速性的要求；

4)CLKF 算法在保持估計精度的同時，計算量與KF 相當，僅為UKF 的3%，有效地降低了計算量。