基于Kalman固定點平滑的綜合校正陀螺漂移誤差評估方法

徐英蛟

（91851部隊葫蘆島125000）

基于Kalman固定點平滑的綜合校正陀螺漂移誤差評估方法

徐英蛟

（91851部隊葫蘆島125000）

由于長航時慣導(dǎo)系統(tǒng)進(jìn)行綜合校正后陀螺漂移的補(bǔ)償效果缺乏直接評估的方法，論文提出一種利用Kalman固定點平滑的校正后陀螺漂移誤差定量評估方法。結(jié)合綜校后慣導(dǎo)系統(tǒng)的輸出信息，通過引入高精度的差分GPS和天文導(dǎo)航信息，以速度、位置、姿態(tài)構(gòu)造復(fù)合觀測量進(jìn)行匹配濾波平滑，實現(xiàn)校正后殘留陀螺常值漂移的精確定量估計。仿真結(jié)果表明，該方法可以在40min內(nèi)有效地估計出綜合校正結(jié)束時刻的陀螺漂移誤差，估計誤差在10%之內(nèi)，且在姿態(tài)重調(diào)精度較高時，估計的速度與精度均有所提升。

慣導(dǎo)系統(tǒng)；綜合校正；陀螺漂移；Kalman固定點平滑

Class NumberU666.1

1 引言

為了抑制慣導(dǎo)誤差發(fā)散，慣導(dǎo)系統(tǒng)必須依靠外界信息進(jìn)行定期的綜合校正，重調(diào)導(dǎo)航誤差參數(shù)，并對慣性器件誤差進(jìn)行估計和補(bǔ)償［1］。部分學(xué)者利用間斷獲取的外部參考位置、速度信息進(jìn)行綜合校正，計算等效陀螺漂移并進(jìn)行補(bǔ)償［2］。部分學(xué)者研究了綜合校正中誤差補(bǔ)償所引起的超調(diào)抑制方法，實現(xiàn)了綜合校正過程的平滑過渡［3］。就各種校正方法的性能而言，現(xiàn)有研究都是對導(dǎo)航參數(shù)誤差進(jìn)行間接表征［4～5］，并沒有直接評估校正性能的方法，特別是缺乏對校正結(jié)束時刻校正效果的定量分析方法。因此開展對校正完成時刻的陀螺漂移余留誤差評估方法有著重要的實際意義。

本文采用最優(yōu)估計中的平滑算法，引入差分GPS系統(tǒng)（DGPS）的速度、位置以及天文導(dǎo)航系統(tǒng)（CNS）提供的姿態(tài)信息，與慣導(dǎo)信息進(jìn)行“速度+位置+姿態(tài)”匹配濾波，并利用Kalman固定點平滑算法估計出校正完成時刻的陀螺漂移。方程為

2 慣導(dǎo)系統(tǒng)的誤差模型

選用地理坐標(biāo)系作為慣導(dǎo)的導(dǎo)航坐標(biāo)系n系，艦船載體坐標(biāo)系記為b系，地心慣性坐標(biāo)系記為i系，下面給出動基座下慣導(dǎo)系統(tǒng)誤差方程。

2.1 姿態(tài)誤差方程

結(jié)合式（1）、式（2），通過忽略二階小項，可得到慣導(dǎo)的姿態(tài)誤差方程為

2.2 速度誤差方程

已知捷聯(lián)式慣導(dǎo)系統(tǒng)的速度基本方程為

則慣導(dǎo)系統(tǒng)速度誤差δV的微分方程為

2.3 位置誤差方程

位置誤差的微分方程為

式中，φ為緯度，λ為經(jīng)度，分別用δφ、δλ表示緯度、經(jīng)度誤差；RM為子午圈曲率半徑，RN為卯酉圈曲率半徑。

考慮與濾波過程中外部位置信息觀測量統(tǒng)一，采用位置誤差的非經(jīng)緯度描述微分方程，即需要將經(jīng)緯度轉(zhuǎn)換為東向、北向位置誤差δPx、δPy，轉(zhuǎn)換

3 Kalman固定點平滑算法

依據(jù)慣導(dǎo)參數(shù)一段時間內(nèi)的觀測值，可用Kalman平滑的方法估計出初始時刻的慣導(dǎo)系統(tǒng)誤差，這也是對慣導(dǎo)誤差進(jìn)行精度評估的主要思想［6］。R-T-S固定區(qū)間平滑算法，需存儲濾波過程產(chǎn)生的4個矩陣數(shù)據(jù)，在濾波結(jié)束后再進(jìn)行平滑［7～8］。然而，高維的數(shù)學(xué)模型以及較快的解算頻率使得需快速存儲大量的數(shù)據(jù)，將有可能造成數(shù)據(jù)實時性的問題，因此本文采用可以實時遞推解算的固定點平滑算法。

設(shè)離散系統(tǒng)的狀態(tài)空間描述為

式中，Xk為系統(tǒng)狀態(tài)變量，Φk，k-1為系統(tǒng)一步轉(zhuǎn)移矩陣，Wk-1為系統(tǒng)噪聲，Zk為量測量，Hk為量測矩陣，Vk為量測噪聲。

基于Kalman濾波方程，利用擴(kuò)充狀態(tài)變量的方法，將擴(kuò)充與濾波狀態(tài)變量一一對應(yīng)的平滑狀態(tài)量，可推得固定點平滑方程［9］。Kalman固定點平滑算法的解算步驟為

1）完成濾波值與平滑值的初始化：

式中，m表示狀態(tài)變量的維數(shù)。

2）依據(jù)狀態(tài)方程進(jìn)行一步預(yù)估計，并計算一步預(yù)估計協(xié)方差陣：

3）計算濾波增益矩陣，并結(jié)合量測量修正一步預(yù)估計值，得到濾波值：

4）計算平滑增益矩陣，并結(jié)合量測量修正上一步平滑值，得到固定點平滑值：

5）計算濾波協(xié)方差陣與平滑協(xié)方差陣：

4 濾波器設(shè)計

4.1 狀態(tài)變量

設(shè)置待估計的13維狀態(tài)變量X為

4.2 狀態(tài)方程

考慮對校正后陀螺漂移量的高精度平滑，選用較為精確的慣導(dǎo)數(shù)學(xué)模型描述，其狀態(tài)方程為［10，11］

4.3 量測方程

作為評估用的慣導(dǎo)外部參考基準(zhǔn)源，其精度必須比相應(yīng)的慣導(dǎo)信息高一個或幾個數(shù)量級。本文選擇能夠提供精確位置、速度信息的DGPS，及能提供精度達(dá)到10″量級姿態(tài)信息的CNS，構(gòu)建組合觀測基準(zhǔn)［12］。量測方程可表示為

5 仿真條件設(shè)置

5.1 仿真時間設(shè)置

仿真時長為2400s，仿真步長為0.1s。

5.2 誤差參數(shù)設(shè)置

陀螺常值漂移為0.004°/h，陀螺隨機(jī)噪聲為0.001°/h；加速度計零偏為10-4g，加速度計隨機(jī)噪聲為0.5×10-4g；初始速度誤差0.01m s，初始北向位置誤差為5m、初始東向位置誤差為8m； DGPS系統(tǒng)速度量測噪聲為0.01m s，天文導(dǎo)航系統(tǒng)姿態(tài)量測噪聲10″。

5.3 載體運(yùn)動設(shè)置

初始緯度45.7796°，初始經(jīng)度126.6705°。運(yùn)動方式：沿北偏東45°航向作勻速直線運(yùn)動，速度為4節(jié)。搖擺方式：

縱搖：周期3s，幅值3°，初值0°；橫搖：周期5s，幅值5°，初值0°；航向：周期7s，幅值2°，初值45°。

5.4 濾波器初始值設(shè)置

6 仿真結(jié)果及分析

以上述仿真條件，分別在初始姿態(tài)誤差為0°、2″及10″時，分別進(jìn)行10組仿真后平均結(jié)果如表1～表3所示。

表1 初始姿態(tài)誤差0°為時的仿真結(jié)果（單位：10-3°/h）

陀螺漂移的固定點平滑仿真結(jié)果如圖1所示。

圖1陀螺漂移的固定點平滑仿真曲線

陀螺漂移的固定點平滑仿真結(jié)果如圖2所示。陀螺漂移的固定點平滑仿真結(jié)果如圖3所示。

表2 初始姿態(tài)誤差為1″時的仿真結(jié)果（單位：10-3°/h）

表3 初始姿態(tài)誤差為10″時的仿真結(jié)果（單位：10-3°/h）

由表1～表3可知，經(jīng)過約40min的匹配濾波可以精確平滑出校正結(jié)束時刻的陀螺漂移，平滑誤差均在10%之內(nèi)，只有表1中天向陀螺漂移的平滑精度在10%左右。而陀螺漂移的平滑精度受平滑開始時刻的系統(tǒng)初始姿態(tài)誤差影響較大，對比圖2、圖3可知其特征表現(xiàn)為：當(dāng)初始姿態(tài)誤差較大時，東向、北向陀螺漂移的平滑精度下降，收斂速度變慢，圖3在30min時已基本收斂，而圖2約需40min；而天向的陀螺漂移則與之相反，在初始姿態(tài)誤差較大時，可獲得較好的平滑精度，這主要是由于天向陀螺漂移所激勵的導(dǎo)航信息誤差受初始姿態(tài)誤差的影響，更大程度地在慣導(dǎo)誤差中得到了體現(xiàn)。然而，過大的初始姿態(tài)誤差也將使導(dǎo)航信息過快的發(fā)散，成為產(chǎn)生導(dǎo)航參數(shù)誤差的主要誤差源，且表現(xiàn)為明顯的振蕩，此時若進(jìn)行長時間的匹配濾波平滑沒有多大的實際意義。因此，為了在有效觀測時間段內(nèi)有效地平滑出慣導(dǎo)系統(tǒng)的陀螺漂移，應(yīng)在綜合校正環(huán)節(jié)盡可能的實現(xiàn)高精度的姿態(tài)重調(diào)。

圖中固定點平滑曲線與濾波估計曲線幾乎重合，這主要是由于較小的陀螺漂移使一步預(yù)測誤差協(xié)方差陣Pk，k-1趨近于奇異陣，導(dǎo)致固定點平滑增益矩陣與濾波增益矩陣Kk幾乎相等，從而使兩者的曲線趨于重合。

7 結(jié)語

本文針對艦船慣導(dǎo)綜合校正性能的定量評估問題，提出了一種利用Kalman固定點平滑算法的陀螺漂移精度評估方法。仿真結(jié)果表明：該方法可實現(xiàn)對綜合校正后的慣導(dǎo)系統(tǒng)陀螺漂移的精確平滑，結(jié)合綜校中的陀螺漂移補(bǔ)償量，可以定量地評估綜合校正的性能，并重點分析了綜校重調(diào)后初始姿態(tài)誤差對陀螺漂移平滑精度的影響，指出提高姿態(tài)重調(diào)精度是提升陀螺漂移平滑性能的有效措施。

［1］高鐘毓.慣性導(dǎo)航系統(tǒng)技術(shù)［M］.北京：清華大學(xué)出版社，2012：401.

［2］馮培德，李魁，王瑋等.一種新的艦載慣導(dǎo)系統(tǒng)綜合校正方法［J］.中國慣性技術(shù)學(xué)報，2011，19（2）：427-435.

［3］程建華，郝燕玲，孫楓等.自動補(bǔ)償技術(shù)在平臺式慣導(dǎo)系統(tǒng)綜合校正中的應(yīng)用研究［J］.哈爾濱工程大學(xué)學(xué)報，2008，29（2）：40-44.

［4］Grewal M S.Application of Kalman filtering to the calibration and alignment of inertial navigation systems［J］. IEEE Trans.Automat Control（S0191-2216），1991，6（11）：3325-3334.

［5］Z F Syed，P Aggarwal，C Goodall，et al.A new multi-position calibration method for MEMS inertial navigation systems［J］.Meas.Sci.Technol，2007（18）：1897-1907.

［6］Kain J.E，Cloutier J.R．Rapid Transfer Alignment for Tactical Weapon Applications［C］//Proceedings of the AIAA Guidance Navigation and Control Conference，Boston，1989：1290-1300.

［7］Watanabe K．A new forward-pass fixed-interval smoother using the UD information matrix factorization［J］.Auto-matica，1986，22（4）：465-475.

［8］宮曉琳，張蓉等.固定區(qū)間平滑算法及其在組合導(dǎo)航系統(tǒng)中的應(yīng)用［J］.中國慣性技術(shù)學(xué)報，2012，20（6）：687-693.

［9］劉建業(yè)，袁信.GPS/INS組合導(dǎo)航系統(tǒng)的平滑濾波應(yīng)用研究［J］.航天控制，1995（4）：36-42.

［10］徐景碩，秦永元，顧冬晴.慣導(dǎo)系統(tǒng)動基座對準(zhǔn)精度評估方法［J］.火力與指揮控制，2005，30（4）：20-23.

［11］楊艷娟，金志華，田蔚風(fēng)，等.R-T-S平滑算法在捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)精度事后評估中的應(yīng)用［J］.上海交通大學(xué)學(xué)報，2004，38（10）：1744-1747.

［12］Titterton D H，Wseton J L.Strapdown Inertial Navigation Technology［M］.Second Edition.United Kingdom：Institution of Electrical Engineerings，2004：387-393.

Evaluation Method of Integrated Calibration Gyroscope Drift Error Based on Kalman Fixed Point Smoothing

XU Yingjiao
（No.91851 Troops of PLA，Huludao125000）

In order to directly evaluate the compensation effect of gyroscope drift in the comprehension calibration of long-term shipborne inertial navigation system（INS），a quantitative evaluation method based on Kalman fixed-point smooth algorithm is presented.In this paper，with the output of INS after calibration，high precision information of differential GPS and celestial navigation system are introduced.Accordingly，the velocity，location and attitude errors of INS are involved as the muti-observations which are used in matched filtering and smoothing，achieving the accurately quantitative evaluation of gyroscope drift which is residual after calibration.The simulation result shows that by this method residual gyroscope drift after calibration can be estimated within 40 minutes and the estimation error is less than 10%.Moreover，if the attitude could have been well re-adjusted，the performance of evaluation method will be advanced both in the estimated rapidity and precision.

INS，comprehensive calibration，gyroscope drift，Kalman fixed-point smooth

U666.1

10.3969/j.issn.1672-9730.2017.07.024

2017年1月17日，

2017年2月20日

徐英蛟，男，碩士研究生，助理工程師，研究方向：組合導(dǎo)航。