改進的格網慣導系統無阻尼綜合校正方法

黃衛權，方濤，王宗義

哈爾濱工程大學 自動化學院，哈爾濱 150001

為克服指北方位慣性導航系統(Inertial Navigation System, INS)在高緯度地區定位與定向的困難，以格網坐標系為導航坐標系的格網INS力學編排被提出并經過理論驗證可作為船舶在高緯度地區導航的可靠方案[1-2]。慣性器件測量誤差、初始對準誤差等誤差源的存在導致INS導航誤差隨時間積累嚴重，其中陀螺漂移是導致誤差積累的主要因素。隨著導航時間的延長，單純依靠INS自主工作難以長時間持續為載體提供可靠的姿態、速度和位置信息[3-6]。

組合導航技術可有效抑制INS隨時間積累的誤差[7-8]，但對于在海上航行的船舶而言，長時間獲取外部導航信息是不允許的，并且將極大地降低船舶的導航自主性。從保持船舶導航自主性的角度出發，綜合校正技術作為一種有效的誤差抑制手段被廣泛應用于長航時船用INS中，該技術旨在通過有限的外部導航信息對陀螺漂移進行估計和補償來抑制INS的誤差[9-11]。當有限的外部導航信息為間斷信息時，稱為點校，如兩點校、三點校和點點校。

目前，大多數點校算法均需要在阻尼條件下進行。文獻[12]最早提出了在阻尼條件下進行的適用于平臺式INS的兩點校、三點校以及點點校方案。在此基礎上，針對捷聯式INS，文獻[13]提出了在位置信息輔助下的三點校方案，對方位陀螺漂移進行了估計和補償。文獻[14]根據全阻尼INS誤差傳播規律，在間斷的位置信息輔助下估計并補償了導致誤差發散的等效陀螺漂移。針對高緯度地區的捷聯式INS，文獻[15]延續文獻[13]的思路提出了橫坐標系INS的三點校方案。文獻[16]在阻尼條件下設計了格網INS的兩點校和三點校方案。通常阻尼需要借助外部速度進行[17-18]，而外部測速設備如多普勒計程儀(Doppler Velocity Log, DVL)是通過向外發射信號的形式測量速度[19-20]。阻尼條件下進行的綜合校正方案需要在校正期間長時間獲取外部速度輔助，是以犧牲慣導系統導航自主性和隱蔽性為代價的，該類方案在船舶需保持靜默航行狀態下無法實施，不具備普適性。為確保綜合校正在無阻尼條件下進行，文獻[20]將外部位置和速度與INS組合通過卡爾曼濾波技術估計水平姿態誤差角，進而反饋修正水平姿態誤差以保證綜合校正在無阻尼條件下進行，該方法本質上仍然屬于阻尼條件下的綜合校正。阻尼條件下的綜合校正是從抑制水平姿態誤差的角度進行綜合校正，而阻尼無法抑制加速度計零偏所造成的水平姿態誤差這一事實也就成為影響陀螺漂移估計精度的重要因素。文獻[21]將外部速度與格網INS組合,通過卡爾曼濾波技術估計水平姿態誤差角，將估計結果作為位置和航向誤差的修正量引入到綜合校正過程以保證綜合校正在無阻尼條件下進行。該方案有效地縮短了外部速度信息的使用時間，同時由于保證了舒勒調整條件，船舶航行不受運動狀態的限制。該方案中組合濾波方式仍然無法估計出加速度計零偏所造成的水平姿態誤差，因而陀螺漂移的估計精度仍受限于加速度計零偏。

文獻[22]提出了在全球定位系統(Global Positioning System, GPS)提供的速度信息輔助下的姿態和加速度計零偏的參數估計方法。文獻[23] 提出了在DVL提供的速度信息輔助下的姿態估計方法，基于此，本文提出一種改進的格網INS無阻尼兩點校方案。首先在文獻[23]基礎上，參考文獻[22]中的思路，將加速度計零偏納入被估計參數，在格網坐標系框架下推導了DVL輔助下的姿態和加速度計零偏的參數估計目標函數，進而采用牛頓迭代最優化方法對姿態和加速度計零偏進行估計。綜合校正前，在DVL輔助下估計和補償加速度計零偏以抑制由其所造成的水平姿態誤差對綜合校正中陀螺漂移估計精度的影響。其次在兩次間斷的外部位置和航向的輔助下設計了無阻尼綜合校正方案對陀螺漂移進行估計，接受校正信息時刻處所涉及到的水平姿態誤差由本文所推導的參數估計方法估計得到。仿真及實驗結果表明：所設計的方案進一步縮短了DVL使用時間，同時加速度計零偏的補償顯著地提高了陀螺漂移的估計精度，補償慣性器件測量誤差有效地抑制了格網INS隨時間積累的導航誤差。

1 姿態和加速度計零偏的參數估計方法

在格網坐標系框架下，本節首先推導了DVL輔助下的加速度計零偏和姿態的參數估計目標函數，進而介紹了牛頓迭代最優化參數估計方法。本文將3次使用該參數估計方法：第1次在綜合校正前估計并補償加速度計零偏以抑制水平姿態誤差；第2次和第3次分別在接受外部校正信息的200 s時間內估計水平姿態，將估計的等效水平姿態誤差引入無阻尼兩點校過程。本文所涉及的坐標系及其定義如下：

1) 載體坐標系b。載體坐標系原點Ob位于船舶質心,zb軸垂直于船舶的甲板平面，xb軸指向船舶的右舷，yb軸指向船艏，構成右手坐標系。載體坐標系如圖1所示。

2) 慣性坐標系i。慣性坐標系原點位于地球質心,xi軸位于赤道平面內并指向某一恒星，yi軸位于赤道平面內并垂直于xi軸，zi軸與地軸重合，構成右手坐標系。

3) 地心地固坐標系e。地心地固坐標系原點位于地球質心,ze軸沿地軸方向，xe軸在赤道平面與格林威治子午面的交線上，ye軸也在赤道平面內與xe、ze軸構成右手坐標系。慣性坐標系和地心地固坐標系如圖2所示。

4) 格網坐標系G。格網坐標系原點OG位于船舶質心,yG軸位于當地水平面指向北并平行于格林威治子午面，xG軸位于當地水平面指向東并垂直于yG軸，zG軸垂直于當地水平面與yG、zG軸構成右手坐標系。格網坐標系如圖3所示，圖中格網平面平行于格林威治子午面。

圖1 載體坐標系Fig.1 Body frame

圖2 地心地固坐標系和慣性坐標系Fig.2 Earth centered-earth fixed frame and inertial frame

圖3 格網坐標系Fig.3 Grid frame

1.1 參數估計目標函數

格網坐標系下的姿態和速度微分方程為

(1)

(2)

由式(1)，速度微分方程可進一步表示為

(3)

(4)

(5)

(6)

將式(4)代入式(3)可得：

(7)

在[0,t]時間段內對式(7)兩側進行積分：

(8)

結合式(6)，左側第1個積分項可計算為

(9)

式中：Vb(0)表示載體坐標系下的初始速度。

將式(9)代入式(8)可得：

(10)

(11)

(12)

(13)

(14)

式中：δθ1、 δθ2為[tk,tk+1]時間段內第1和第2次陀螺輸出的等效角增量信息；δv1、 δv2為[tk,tk+1]時間段內第1和第2次加速度計輸出的等效速度增量信息。式(14)的詳細計算過程可參考文獻[22]。

為表示方便，令

(15)

(16)

(17)

結合式(12)和式(17)，式(10)可表示為

(18)

為表示方便，令

(19)

(20)

則參數估計目標函數為

(21)

(22)

為了表述方便，仍然使用式(21)的形式表示重構后的參數估計目標函數：

(23)

(24)

與四元數乘法相關的矩陣為

(25)

式(23)的等價形式為

(26)

式中：α、β和κΔb在此處轉換為標量為0的四元數。

進一步地，文中的參數估計問題可轉化為有約束條件的最小化參數估計問題：

(27)

1.2 牛頓迭代法

牛頓迭代法可作為解決有約束條件的最小化參數估計問題的有效手段。引入拉格朗日乘子ξ，則式(27)可通過拉格朗日算子表示為

(28)

(29)

式中：迭代增量δx和δξ的計算方式為

(30)

詳細的計算可參考文獻[22]中給出的姿態和參數估計遞歸算法，本文不再贅述。

2 P方程和ψ方程

P方程和ψ方程是設計綜合校正策略的核心方程。P方程建立起位置誤差、格網航向誤差與平臺漂移角之間的關系；ψ方程建立起平臺漂移角與陀螺漂移之間的關系。針對格網INS，文獻[21] 給出了適用于無阻尼條件下進行綜合校正的P方程：

(31)

ψ方程為

(32)

3 綜合校正方案

在兩次間斷的外部位置和航向信息輔助下，本文設計了相應的兩點校策略,外部位置由GPS提供，外部航向由天文導航系統(Celestial Navigation System, CNS)提供。在綜合校正前的某一時間段內，在DVL的輔助下，通過第1節中設計的參數估計方法對加速度計零偏進行估計和補償。通常現代船舶一般都裝備有DVL，由于該參數估計方法所需時間較短，因而船舶在開始導航后即可進行加速度計零偏的估計和補償。需要指出的是，本文所設計的參數估計方法在估計航向的精度方面性能有限，因而并不將估計出的航向引入綜合校正過程。DVL、GPS和CNS引入INS的時間如圖4所示。圖中虛線中的實線和實心原點表示向INS引入外部導航信息的時間，需要強調的是本文所設計的參數估計方法在200 s時間內即可準確地估計出加速度計零偏和姿態。

圖4 3種導航系統信息引入INS的時間Fig.4 Time for introducing three types of navigation information to INS

設INS在t1和t2時刻接收外部校正信息。由式(32)可得t1～t2時間段內的ψ角增量為

(33)

由式(31)可得t1～t2時間段內ψ角增量的另一種計算形式

δψ=M-1(t2)P(t2)-M-1(t1)P(t1)

(34)

在t1時刻，根據外部提供的位置和航向對INS進行位置和航向重調，同時根據前述參數估計方法估計出的水平姿態對水平姿態進行重調。則式(34)可簡化為

δψ=M-1(t2)P(t2)

(35)

(36)

由式(36)可計算得到等效的姿態誤差φx、φy和φz，進而可計算得到P(t2)。

由式(33)和式(35)可計算得到εb：

(37)

由式(37)估計出陀螺漂移后對陀螺漂移進行補償，同時對位置、姿態和速度進行重調即可完成綜合校正過程。

4 仿真及實驗結果及分析

本文分別采用數值仿真和實驗的形式對所設計的兩點校方案進行驗證。

4.1 數值仿真結果及分析

仿真條件設置如下：仿真時長25 h；陀螺常值漂移為0.01 (°)/h，隨機漂移為白噪聲；加速度計常值零偏為100 μg，隨機零偏為白噪聲；船舶初始位置為(85°N, 18°E)；船舶以10 m/s的速度向正北方向運動；姿態設置為正弦函數變化；對準后失準角為6″、6″和6′；外部位置誤差標準差為10 m；外部航向誤差標準差為10″；輔助速度信息誤差為白噪聲。

t=3 h時，在DVL的輔助下完成加速度計零偏的估計和補償以及水平姿態的估計和重調。為確保參數估計的收斂，參數估計在t=3 h前持續200 s。加速度計零偏的估計結果如圖5所示。

由圖5可知，在200 s時間內，所設計的參數估計方法可準確地估計出加速度計零偏，在t=3 h 時補償加速度計零偏可有效地抑制由其所造成的水平姿態誤差。

t=6 h和t=8 h時，INS兩次接收外部位置和航向，同時在t=6 h和t=8 h的前200 s時間內，在DVL的輔助下估計水平姿態并引入綜合校正過程。由于在t=3 h后加速度計零偏造成的水平姿態誤差得到了有效抑制，相對于未進行加速度計零偏補償的情況，在接收校正信息前的短時間內本文所提出的方案中水平姿態的估計精度應該更高。以t=6 h前200 s為例，t=6 h前200 s水平姿態估計誤差如圖6所示：

如圖6所示，由于加速度計零偏得到了有效的補償，所提出的方案在接受校正信息前的200 s內具備更高的水平姿態估計精度。為驗證所設計的綜合校正方案在陀螺漂移估計和抑制導航誤差方面的優勢，對包含本文所設計方案在內的4種解算方案進行仿真分析，4種方案如表1所示。

圖5 加速度計零偏估計值Fig.5 Estimation results of accelerometer biases

圖6 200 s內水平姿態估計誤差Fig.6 Estimation errors of level attitudes in 200 s

表1中方案1即為常規的無阻尼INS解算。方案3與文獻[21]中設計的無阻尼兩點校方案相同，區別在于本文所設計的方案采用第1節中提到的參數估計方法對水平姿態進行估計，而文獻[21] 則采用卡爾曼濾波技術估計水平姿態誤差。方案4即為本文所設計的綜合校正方案。方案3、方案4和文獻[21]中方案的陀螺漂移估計值如表2所示。

由表2可知，方案3取得了與文獻[21]中方案相當的陀螺漂移估計精度。文獻[21]中濾波器單次需要20 min以確保水平姿態誤差估計穩定收斂，兩點校總共需要40 min的DVL輔助，而本文所設計的參數估計方法單次僅需要200 s的DVL輔助即可保證水平姿態估計穩定收斂，3次參數估計總共需要10 min的DVL輔助，因而采用該方法進一步地縮短了DVL的輔助時間。方案4取得了更高的陀螺漂移估計精度，原因分析如下：方案3中的參數估計方法并不能估計出加速度計零偏所造成的等效水平姿態誤差。由于方案4在綜合校正前補償了加速度計零偏，在綜合校正階段相對于方案3具備更高的水平姿態估計精度，因此方案4取得了更高的陀螺漂移估計精度。

表1 4種解算方案Table 1 Four types of solution schemes

表2 3軸陀螺漂移估計值Table 2 Estimation results of three-axis gyroscope drifts

表1中4種方案的格網姿態誤差、格網水平速度誤差以及位置誤差如圖7所示。

t=8 h后4種方案導航誤差的均方根統計值如表3所示。

圖7 姿態、速度和位置誤差Fig.7 Attitude, velocity and position errors

表3 t=8 h后4種方案導航誤差的均方根統計值

由圖7和表3可知，方案2由于在t=3 h后補償了加速度計零偏，相較于方案1，穩態水平姿態誤差得到了有效抑制(振蕩更靠近X軸)，同時由仿真結果可以看到，僅補償加速度計零偏無法抑制航向、速度及位置誤差。方案4由于補償了陀螺漂移，格網INS隨時間積累的誤差在校正點后得到了有效的抑制。相對于方案3，方案4的陀螺漂移估計精度更高。這樣由于陀螺漂移補償更徹底，加速度計零偏同時也得到了有效的補償，方案4更好地抑制了積累的導航誤差。綜上所述，所設計的綜合校正方案進一步地縮短了綜合校正過程中DVL的輔助時間，有效地解決了加速度計零偏所造成的水平姿態誤差影響陀螺漂移估計精度這一問題，更好地抑制了格網INS隨時間積累的導航誤差。

4.2 實驗結果及分析

本文采用3軸轉臺搖擺實驗對所設計的綜合校正方案進行驗證。實驗場景如圖8所示。

激光慣導系統安裝于3軸轉臺上，安裝誤差已補償，轉臺3軸進行等幅正弦搖擺，轉臺位置為(45.779 6°N, 126.670 5°E)。激光慣導系統在室溫條件下熱開機后完成初始對準，導航時長11 h，在t=4 h和t=6 h接入位置和航向輔助信息，位置輔助信息由轉臺位置疊加誤差仿真得到，誤差標準差設置為10 m；航向輔助信息由轉臺同步輸出航向疊加誤差仿真得到，誤差標準差設置為10″。慣導系統在t=1,4,6 h的前200 s時間內接入DVL信息估計加速度計零偏和水平姿態，DVL測速由零速疊加真實海試實驗測速誤差得到。所給出的海試實驗測速統計結果給出了DVL和GPS的對地速度，以GPS測速為參考，某一段200 s時間內DVL在載體縱軸方向上的測速誤差如圖9所示。

本文方案在綜合校正前完成了加速度計零偏的補償，4.1節中的仿真測試是建立在加速度計零偏不變的前提條件下。事實上，慣性器件測量誤差會隨著時間發生變化，因而后續加速度計零偏的變化會直接影響水平姿態誤差的抑制效果，進而影響陀螺漂移的估計精度。幸運的是，針對船舶導航所用的高精度加速度計而言，即使在綜合校正期間加速度計零偏發成了變化，由于仍有部分加速度計零偏被補償掉，其對綜合校正仍將產生積極效果。t=1 h和t=6 h處3軸加速度計零偏的估計值如圖10所示。

圖8 實驗場景Fig.8 Experiment scenario

由圖10可知，X軸和Z軸加速度計零偏在2個時刻處的估計值基本一致，因而在t=1 h處補償X軸和Z軸加速度計零偏對綜合校正方案是有益的。Y軸加速度計零偏的估計值由13 μg變化為39 μg，顯然在t=1 h處補償后的加速度計輸出仍將對綜合校正方案產生積極效果。以轉臺的位置和同步輸出的姿態信息為參考，表1中4種方案的姿態誤差、速度誤差和位置誤差如圖11所示。

圖9 海試實驗中DVL測速誤差Fig.9 DVL measurement error in sea test

圖10 t=1 h和t=6 h處3軸加速度計零偏估計值Fig.10 Estimations of three-axis accelerometer biases at t=1 h and t=6 h

t=6 h后4種方案導航誤差的均方根統計值如表4所示。

圖11和表4取得了與前述仿真測試一致的結果。方案2中僅補償加速度計零偏僅能抑制由其造成的穩態誤差，同時由方案2可以看到，僅補償加速度計零偏無法抑制航向、速度及位置誤差。相對于方案3，方案4由于在綜合校正前補償了加速度計零偏，取得了更高的陀螺漂移估計精度，因而最終得到了更好的導航誤差抑制效果。方案4在10 min的DVL信息和兩次間斷的外部位置和航向信息輔助下，有效地估計并補償了陀螺漂移，有效地提高了長航時格網慣導系統的導航精度。

圖11 地心地固坐標系下的位置誤差Fig.11 Position errors in earth centered-earth fixed frame

表4 t=6 h后4種方案導航誤差的均方根統計值

針對本文所設計的綜合校正方案，補充以下3點：① 針對適用于中低緯度地區的指北方位慣導系統，若能推導得到DVL輔助下的地理坐標系框架下的加速度計零偏和姿態的估計目標函數和適用于無阻尼綜合校正的P方程，延續本文的綜合校正方案設計思路，同樣可以設計出適用于指北方位慣導系統的無阻尼兩點式綜合校正策略；② 為確保參數估計方法所估計出的加速度計零偏和姿態的準確性，本文所提出的綜合校正方案需要保證DVL測速的準確性。在以下情況不采用DVL輔助估計相關參數：船舶進出港口靠離岸時、船舶轉彎時、復雜惡劣海況條件下；③ 實際船用慣導系統在初始對準階段可完成水平方向陀螺漂移的估計和補償[24]。本文為凸顯綜合校正中陀螺漂移估計精度和導航誤差抑制效果，并未在初始對準階段實現陀螺漂移的估計和補償。由于本文綜合校正方案可準確估計出天向陀螺漂移，因而本文的綜合校正方案在陀螺漂移估計和導航誤差抑制方面仍然具備積極作用。

5 結 論

1) 采用本文所設計的格網坐標系框架下的參數估計方法在200 s時間內即可準確地估計出加速度計零偏和水平姿態，補償加速度計零偏可有效地抑制格網INS中由加速度計零偏所造成的水平姿態誤差。執行綜合校正方案僅需10 min 的DVL輔助時間，相對于已有的研究，進一步地縮短了DVL的使用時間，提高了綜合校正方案在實際應用中的普適性。

2) 加速度計零偏的補償提高了陀螺漂移的估計精度。由于慣性器件測量誤差補償更加徹底，本文所設計的綜合校正方案在抑制格網INS積累的導航誤差方面效果更明顯。實測條件下，相對于已有的綜合校正方案，格網姿態誤差分別減小75.2%、47.0%和36.0%；格網水平速度誤差分別減小45.4%和20.7%；地心地固坐標系下位置誤差分別減小25.8%、33.0%和22.0%。