基于協(xié)方差變換的INS/DVL全球組合導(dǎo)航算法

梁鐘泓，謝元平，張永健，王 林，魏 國(guó)，高春峰

（國(guó)防科技大學(xué) 前沿交叉學(xué)科學(xué)院，長(zhǎng)沙 410073）

自主水下潛航器（Autonomous Underwater Vehicle,AUV）在極地的科考、海底勘探等領(lǐng)域有著重要地位，而高精度的導(dǎo)航技術(shù)是實(shí)現(xiàn)水下潛航的關(guān)鍵[1]。慣性/多普勒測(cè)速儀（Inertial Navigation System/Doppler Velocity Log, INS/DVL）組合導(dǎo)航方式憑借其完全自主、高精度等優(yōu)勢(shì)，成為AUV導(dǎo)航的主要手段[2]。但是由于極區(qū)經(jīng)線(xiàn)快速收斂，傳統(tǒng)地理坐標(biāo)系編排下的慣性導(dǎo)航方式在極區(qū)會(huì)出現(xiàn)很大的誤差[3]，需要在進(jìn)入極區(qū)時(shí)切換導(dǎo)航坐標(biāo)系才能保證導(dǎo)航的精度。常用的極區(qū)導(dǎo)航坐標(biāo)系有格網(wǎng)系[4]、橫坐標(biāo)系[5]等。其中，橫坐標(biāo)系參照地理坐標(biāo)系的構(gòu)建方法，將現(xiàn)有地理極點(diǎn)旋轉(zhuǎn)90 °后到赤道作為新的極點(diǎn)構(gòu)建而成，通過(guò)定義橫經(jīng)緯度，解決了AUV在極區(qū)的航向參考問(wèn)題。

目前關(guān)于橫坐標(biāo)系導(dǎo)航的研究多集中于慣性導(dǎo)航算法優(yōu)化方面。文獻(xiàn)[6]提出了基于參考橢球模型的橫坐標(biāo)系導(dǎo)航算法，并驗(yàn)證了橫坐標(biāo)系適用范圍，證明在中緯度地區(qū)橫坐標(biāo)系導(dǎo)航與傳統(tǒng)慣性導(dǎo)航方式有著相同的精度。文獻(xiàn)[7]從理論上重新推導(dǎo)基于橢球地球模型的極區(qū)導(dǎo)航機(jī)理，避免求解橢球半徑，優(yōu)化了算法的精度。文獻(xiàn)[8]以載體所在位置的卯酉圈半徑為半徑，構(gòu)建了虛擬圓球用于橫向?qū)Ш?，相較于橢球模型的復(fù)雜編排，該方法在保持精度的前提下簡(jiǎn)化了運(yùn)算。在極區(qū)組合導(dǎo)航方面，目前的研究主要集中在極區(qū)范圍內(nèi)組合導(dǎo)航濾波器設(shè)計(jì)。文獻(xiàn)[9]比較了格網(wǎng)系與橢球模型橫坐標(biāo)系下的組合導(dǎo)航的精度，證明慣性/衛(wèi)星組合導(dǎo)航在橢球模型橫坐標(biāo)系下具有更好的性能。文獻(xiàn)[10]設(shè)計(jì)了橫坐標(biāo)系下的INS/DVL組合導(dǎo)航算法，可以有效抑制極區(qū)內(nèi)方位失準(zhǔn)角的增大。然而以上算法并沒(méi)有考慮導(dǎo)航過(guò)程的連續(xù)性，忽視了載體進(jìn)出極區(qū)時(shí)導(dǎo)航坐標(biāo)系切換引起的濾波器超調(diào)與誤差狀態(tài)估計(jì)不連續(xù)的問(wèn)題[11]。

針對(duì)這一問(wèn)題，本文提出了基于協(xié)方差變換的INS/DVL全球組合導(dǎo)航算法，建立了橫地理坐標(biāo)系與地理坐標(biāo)系下系統(tǒng)誤差狀態(tài)及協(xié)方差矩陣的轉(zhuǎn)換關(guān)系，在載體進(jìn)入極區(qū)切換導(dǎo)航坐標(biāo)系時(shí)，同時(shí)完成導(dǎo)航參數(shù)、誤差狀態(tài)以及其協(xié)方差矩陣的轉(zhuǎn)換，解決了濾波器結(jié)構(gòu)變化導(dǎo)致的狀態(tài)估計(jì)值震蕩及收斂錯(cuò)誤的問(wèn)題，使得濾波器能夠平滑切換，實(shí)現(xiàn)全球?qū)Ш健?/p>

1 橫坐標(biāo)系的定義

定義橫地球坐標(biāo)系為e'系，定義橫地理坐標(biāo)系為t系（地理坐標(biāo)系與橫地理坐標(biāo)系的定義均為“東-北-天”）。如圖1所示，橫地球坐標(biāo)系是由地球坐標(biāo)系旋轉(zhuǎn)兩次得到的[12]，旋轉(zhuǎn)方式為：Oxe y e ze繞xe軸順時(shí)針旋轉(zhuǎn)90 °得到Ox′y′z′，Ox′y′z′繞z′軸順時(shí)針旋轉(zhuǎn)90 °得到Oxe'y e'ze'。根據(jù)坐標(biāo)系之間的變換關(guān)系可以得到：

在橫坐標(biāo)系的定義下，N'與S'分別為橫北極與橫南極，Oxe'ye'為橫赤道面，Oxe'ze'為橫本初子午面。P為地球上一點(diǎn)，OP與橫赤道面的夾角定義為P點(diǎn)所在位置的橫緯度Lt，ON'P平面與橫本初子午面的夾角定義為P點(diǎn)所在位置的橫經(jīng)度λt。南北極點(diǎn)在橫坐標(biāo)系的定義下位于橫赤道上，在極區(qū)導(dǎo)航時(shí)切換至橫坐標(biāo)系就可以解決極區(qū)附近導(dǎo)航誤差增大的問(wèn)題。

地球坐標(biāo)系e與導(dǎo)航系n之間的轉(zhuǎn)換關(guān)系可以用來(lái)表示[13]：

式中，L、λ分別為P點(diǎn)所在位置的緯度、經(jīng)度。橫坐標(biāo)系下的橫經(jīng)緯度、橫地理系定義與地理坐標(biāo)系類(lèi)似，同樣的方法可以得到橫地球坐標(biāo)系與橫地理坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。

橫經(jīng)緯度與經(jīng)緯度的之間的關(guān)系[12]：

或者，

根據(jù)式(1)-(3)，地理坐標(biāo)系與橫地理坐標(biāo)系之間的轉(zhuǎn)換關(guān)系可以表示為：

2 橫地理坐標(biāo)系下INS/DVL組合導(dǎo)航濾波器設(shè)計(jì)

2.1 系統(tǒng)狀態(tài)方程

綜合慣導(dǎo)系統(tǒng)的誤差項(xiàng)與多普勒測(cè)速儀的誤差項(xiàng)，橫坐標(biāo)系表示下的INS/DVL組合導(dǎo)航的系統(tǒng)誤差狀態(tài)選為慣導(dǎo)系統(tǒng)的姿態(tài)誤差φt、速度誤差δvt、位置誤差δ pt、陀螺常值零漂εb、加速度計(jì)常值零偏?b，同時(shí)增廣多普勒測(cè)速儀的比例因子誤差δk、俯仰安裝誤差角δθ、航向安裝誤差角δψ共18維，即：

相關(guān)的狀態(tài)方程為：

且有：

式中：

其中，R為地球半徑。

2.2 觀測(cè)方程

其中，vd為DVL測(cè)速儀理論輸出值。將姿態(tài)矩陣表示為：

式中?b=[δθ0δψ]T。將式(17)(18)帶入式(16)得到：

式中忽略了二階小量，υ為觀測(cè)噪聲。

3基于協(xié)方差變換的INS/DVL全球組合導(dǎo)航算法

AUV駛?cè)牒婉偝鰳O區(qū)時(shí)，導(dǎo)航坐標(biāo)系要在地理坐標(biāo)系與橫地理坐標(biāo)系之間切換，切換時(shí)既需要將導(dǎo)航參數(shù)完成轉(zhuǎn)換，又要保證濾波器的系統(tǒng)狀態(tài)估計(jì)值不會(huì)因?yàn)闉V波器結(jié)構(gòu)的變化而震蕩，這是實(shí)現(xiàn)全球組合導(dǎo)航的關(guān)鍵。

3.1 導(dǎo)航參數(shù)的切換

在中低緯度地區(qū)，INS/DVL組合導(dǎo)航濾波器在地理坐標(biāo)系構(gòu)建[14]，輸出的導(dǎo)航參數(shù)為地理坐標(biāo)系下解算得到的姿態(tài)矩陣、速度vn、位置pn；AUV航行至極區(qū)時(shí)，需要切換到橫地理坐標(biāo)系下導(dǎo)航，導(dǎo)航參數(shù)為橫地理坐標(biāo)系下表示的姿態(tài)矩陣、速度vt、位置pt。切換時(shí)根據(jù)坐標(biāo)系間的轉(zhuǎn)換關(guān)系，確定導(dǎo)航參數(shù)的轉(zhuǎn)換關(guān)系為：

同理，在AUV駛出極區(qū)時(shí)導(dǎo)航參數(shù)需要從橫地理坐標(biāo)系轉(zhuǎn)換至地理坐標(biāo)系。

位置轉(zhuǎn)換由式(5)確定。

3.2 INS/DVL組合導(dǎo)航濾波器

導(dǎo)航坐標(biāo)系切換后，組合導(dǎo)航濾波器的結(jié)構(gòu)也會(huì)隨之改變，Kalman濾波器中的系統(tǒng)誤差狀態(tài)及其協(xié)方差矩陣、狀態(tài)轉(zhuǎn)移矩陣、觀測(cè)方程均需要完成轉(zhuǎn)換。由于在兩種導(dǎo)航坐標(biāo)系下的系統(tǒng)狀態(tài)方程、觀測(cè)方程是事先建立的，所以在切換濾波器時(shí)只需要完成系統(tǒng)誤差狀態(tài)與協(xié)方差矩陣的轉(zhuǎn)換。

中低緯度下，INS/DVL組合導(dǎo)航濾波器的系統(tǒng)誤差狀態(tài)在地理坐標(biāo)系下表示為：

其系統(tǒng)狀態(tài)方程、觀測(cè)方程由式(8)-(11)、式(16)推得在此，不再贅述。對(duì)比式(7)，系統(tǒng)誤差狀態(tài)中姿態(tài)誤差、速度誤差和位置誤差與導(dǎo)航系定義相關(guān)，陀螺零漂和加速度零偏、多普勒測(cè)速儀的比例因子誤差、安裝角誤差與導(dǎo)航系定義無(wú)關(guān)，濾波器切換時(shí)無(wú)需轉(zhuǎn)換，所以系統(tǒng)誤差狀態(tài)的轉(zhuǎn)換只需要完成姿態(tài)誤差、速度誤差、位置誤差的轉(zhuǎn)換。

姿態(tài)誤差的轉(zhuǎn)換關(guān)系：

對(duì)式(2)兩側(cè)做擾動(dòng)，得到：

式中：

另一方面，對(duì)式(4)兩側(cè)微分，得到：

將式(26)帶入τt的表達(dá)式可以得到：

對(duì)式(6)兩側(cè)做擾動(dòng)，得到：

根據(jù)定義可知，

將式(24)(27)(28)帶入式(29)，以及根據(jù)δCbn的定義可以得到：

整理式(30)，得到姿態(tài)誤差φt與φn之間的轉(zhuǎn)換關(guān)系：

速度誤差的轉(zhuǎn)換關(guān)系：

與姿態(tài)誤差轉(zhuǎn)換關(guān)系類(lèi)似，將式(28)帶入δvt中即可得到速度誤差的轉(zhuǎn)換關(guān)系：

位置誤差的轉(zhuǎn)換關(guān)系：

位置誤差的轉(zhuǎn)換關(guān)系可以通過(guò)式(26)確定，高度誤差在兩個(gè)坐標(biāo)系下是一致的，無(wú)需轉(zhuǎn)換。

系統(tǒng)誤差狀態(tài)xn（t）與xt（t）之間的轉(zhuǎn)換關(guān)系可以由式(31)-(33)確定：

其中，

根據(jù)協(xié)方差的定義，確定不同導(dǎo)航坐標(biāo)系下的濾波器系統(tǒng)誤差狀態(tài)的協(xié)方差矩陣轉(zhuǎn)換關(guān)系為：

同理，AUV駛出極區(qū)時(shí)，濾波器誤差狀態(tài)及其協(xié)方差矩陣的轉(zhuǎn)換關(guān)系為：

整個(gè)算法的流程如圖2所示。

圖2 算法流程圖Fig.2 Algorithm flow chart

4 實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證算法的有效性，分別通過(guò)船載實(shí)驗(yàn)、半實(shí)物仿真實(shí)驗(yàn)進(jìn)行驗(yàn)證。船載實(shí)驗(yàn)在中低緯度地區(qū)進(jìn)行，半實(shí)物仿真實(shí)驗(yàn)按照文獻(xiàn)[6]中方法，保持載體相對(duì)于地面的姿態(tài)和速度不變，將實(shí)驗(yàn)所在位置的緯度轉(zhuǎn)換至高緯度地區(qū)，基于船載實(shí)驗(yàn)數(shù)據(jù)仿真產(chǎn)生高緯度地區(qū)的測(cè)試數(shù)據(jù)。

4.1 船載實(shí)驗(yàn)

實(shí)驗(yàn)船只搭載INS/DVL組合導(dǎo)航系統(tǒng)在長(zhǎng)江某段航行實(shí)驗(yàn)，系統(tǒng)中陀螺零漂穩(wěn)定性?xún)?yōu)于0.02 °/h，加速度計(jì)零偏穩(wěn)定性?xún)?yōu)于50 μg，DVL的精度為0.5%V±5 mm/s。船載實(shí)驗(yàn)的起始位置為北緯30.8581 °，東經(jīng)110.9877 °。實(shí)驗(yàn)時(shí)前900 s進(jìn)行對(duì)準(zhǔn)，對(duì)準(zhǔn)后進(jìn)入導(dǎo)航。在導(dǎo)航過(guò)程中，慣導(dǎo)系統(tǒng)的誤差狀態(tài)均進(jìn)行反饋校正，多普勒測(cè)速儀的誤差只進(jìn)行估計(jì)，未進(jìn)行反饋，前1 h使用地理坐標(biāo)系進(jìn)行導(dǎo)航，然后切換至橫地理坐標(biāo)系導(dǎo)航模擬駛?cè)霕O區(qū)，航行1 h后再切換至地理坐標(biāo)系模擬駛出極區(qū)，實(shí)驗(yàn)總時(shí)長(zhǎng)為3 h。

中低緯度地區(qū)，地理坐標(biāo)系下構(gòu)建的INS/DVL組合導(dǎo)航濾波器不存在跟緯度相關(guān)的誤差放大效應(yīng)，將其組合導(dǎo)航結(jié)果作為對(duì)照，比較有協(xié)方差變換與無(wú)協(xié)方差變換的情況下，由于導(dǎo)航坐標(biāo)系切換帶來(lái)的系統(tǒng)誤差狀態(tài)估計(jì)值的變化。由于采用速度觀測(cè)，所以導(dǎo)航坐標(biāo)系切換時(shí)速度震蕩并不明顯，除速度誤差外其他參數(shù)的相對(duì)估計(jì)誤差如圖3-7所示。

圖3 相對(duì)姿態(tài)誤差Fig.3 Relative attitude error

圖3為相對(duì)姿態(tài)誤差曲線(xiàn)，無(wú)協(xié)方差變換情況下俯仰角的誤差震蕩峰值為0.3 ′，橫滾角誤差峰值0.23 ′。由于航向角的可觀測(cè)性較差，受導(dǎo)航坐標(biāo)系切換的影響較大，誤差峰值為2.54 ′。協(xié)方差變化算法的相對(duì)姿態(tài)誤差不明顯，能夠保證濾波狀態(tài)估計(jì)值在導(dǎo)航坐標(biāo)系切換前后的一致性。

圖4為相對(duì)位置誤差曲線(xiàn)，從圖中可以看出，有協(xié)方差變換時(shí)的東向相對(duì)誤差峰值為10.38 m，北向相對(duì)誤差峰值為27.72 m。而無(wú)協(xié)方差變換時(shí)，東向相對(duì)誤差峰值為30.11 m，北向相對(duì)誤差峰值為60.57 m。雖然高度參數(shù)理論上不受導(dǎo)航坐標(biāo)系切換的影響，但是由于其他誤差的耦合效應(yīng)也發(fā)生了震蕩，震蕩峰值為2.06 m。

圖4 相對(duì)位置誤差Fig.4 Relative position error

圖5 陀螺零漂相對(duì)估計(jì)誤差Fig.5 Relative estimation error of the gyro drift

圖6 加速度計(jì)零偏相對(duì)估計(jì)誤差Fig.6 Relative estimation error of the accelerometer bias

圖5-6為陀螺零漂、加速度計(jì)零偏的相對(duì)估計(jì)誤差，有協(xié)方差變換時(shí)幾乎沒(méi)有發(fā)生震蕩，無(wú)協(xié)方差變換時(shí)，三軸陀螺零漂的估計(jì)震蕩峰值分別為0.0045 °/h、0.0058 °/h、0.049 °/h，三軸加速度計(jì)零偏的估計(jì)震蕩峰值分別為45.21 μg、97.13 μg、3.11 μg。陀螺零漂、加速度計(jì)零偏在導(dǎo)航坐標(biāo)系切換前后出現(xiàn)了持續(xù)震蕩，甚至出現(xiàn)了收斂錯(cuò)誤。由于方位陀螺(z軸)零漂的可觀性弱于水平陀螺零漂，水平加速度計(jì)（x、y軸）零偏的可觀性弱于垂向加速度計(jì)零偏，相應(yīng)的狀態(tài)估計(jì)震蕩更為劇烈。

圖7為DVL比例因子及安裝參數(shù)的相對(duì)估計(jì)誤差曲線(xiàn)。無(wú)協(xié)方差變換時(shí)，AUV進(jìn)出極區(qū)過(guò)程中導(dǎo)航坐標(biāo)系切換會(huì)使得其誤差參數(shù)估計(jì)值產(chǎn)生震蕩，并且出現(xiàn)了估計(jì)值收斂錯(cuò)誤。

圖7 DVL誤差參數(shù)相對(duì)估計(jì)誤差Fig.7 Relative estimation error of DVL error parameters

從圖3-7可以看出，當(dāng)導(dǎo)航坐標(biāo)系切換時(shí)，協(xié)方差變換算法可以實(shí)現(xiàn)組合導(dǎo)航濾波器的平滑過(guò)渡，狀態(tài)估計(jì)值震蕩不明顯。但如果忽略濾波器協(xié)方差矩陣的轉(zhuǎn)換，狀態(tài)估計(jì)值會(huì)出現(xiàn)不同程度的震蕩，部分參數(shù)甚至收斂至錯(cuò)誤值。

為進(jìn)一步驗(yàn)證算法有效性，進(jìn)行了六組實(shí)驗(yàn)，模擬AUV進(jìn)出極區(qū)時(shí)導(dǎo)航坐標(biāo)系的切換過(guò)程。無(wú)協(xié)方差變換時(shí)狀態(tài)估計(jì)相對(duì)誤差震蕩峰值見(jiàn)表1，有協(xié)方差變換時(shí)狀態(tài)估計(jì)相對(duì)誤差震蕩峰值見(jiàn)表2。

表1 無(wú)協(xié)方差變換的狀態(tài)估計(jì)相對(duì)誤差Tab.1 Relative error of state estimation with non-covariance transformation

表2 協(xié)方差變換算法的狀態(tài)估計(jì)相對(duì)誤差Tab.2 Relative error of state estimation with covariance transformation

對(duì)比表1-2的數(shù)據(jù)可以發(fā)現(xiàn)，有協(xié)方差變換時(shí)姿態(tài)誤差、陀螺零漂、加速度計(jì)零偏的相對(duì)估計(jì)誤差相較無(wú)協(xié)方差變換減少一個(gè)數(shù)量級(jí)。由于橫地理坐標(biāo)系下的INS/DVL組合導(dǎo)航濾波器是基于地球圓球模型設(shè)計(jì)的，存在模型誤差[6]。使得在導(dǎo)航坐標(biāo)系切換過(guò)程中，無(wú)論有無(wú)協(xié)方差變換均出現(xiàn)了一定程度的位置誤差震蕩，但采用協(xié)方差變換算法后位置誤差仍減小50%以上，證明了該算法的有效性。

4.2 半實(shí)物仿真實(shí)驗(yàn)

為檢驗(yàn)算法在高緯度地區(qū)的適用性，采用半實(shí)物仿真方法，將原數(shù)據(jù)轉(zhuǎn)換至高緯度地區(qū)，轉(zhuǎn)換后的起始位置為北緯79.95 °，AUV在5500 s時(shí)航行至北緯80 °進(jìn)入極區(qū)，此時(shí)導(dǎo)航坐標(biāo)系從地理系切換至橫地理坐標(biāo)系，圖8為高緯度下AUV的行進(jìn)軌跡。

在高緯度地區(qū)，橫地理坐標(biāo)系下重新定義了航向參考與橫經(jīng)緯度，在此坐標(biāo)系下設(shè)計(jì)的INS/DVL組合導(dǎo)航濾波器不存在誤差方法效應(yīng)。因此，可以將橫地理坐標(biāo)系下的導(dǎo)航結(jié)果作為參考，比較協(xié)方差變換算法與無(wú)協(xié)方差變換的導(dǎo)航結(jié)果，如圖8-12所示。

圖8 高緯度下AUV的軌跡Fig.8 AUV trajectory at high latitude

圖9給出了相對(duì)姿態(tài)誤差的對(duì)比結(jié)果，其中無(wú)協(xié)方差變換時(shí)，導(dǎo)航坐標(biāo)系切換帶來(lái)的航向角誤差峰值為26.05 ′，俯仰角與橫滾角誤差也達(dá)到了0.8 ′。

圖9 高緯度下相對(duì)姿態(tài)誤差Fig.9 Relative attitude error at high latitude

從圖10可以看出，導(dǎo)航坐標(biāo)系切換后協(xié)方差變換算法可以保證位置誤差估計(jì)的一致性，與參考結(jié)果一致，但無(wú)協(xié)方差變換情況下位置誤差出現(xiàn)了震蕩、發(fā)散，東向誤差峰值為113 m，北向誤差峰值為84 m。圖11-12顯示，切換導(dǎo)航坐標(biāo)系后，無(wú)協(xié)方差變換時(shí)方位陀螺(z軸)零漂、水平軸(x、y軸)加速度計(jì)零偏在高緯度地區(qū)的相對(duì)估計(jì)誤差明顯增大并且收斂至錯(cuò)誤值。x、y軸陀螺零漂、z軸加速度計(jì)零偏震蕩幅度相對(duì)較小，但陀螺零漂收斂較差。

圖10 高緯度下相對(duì)位置誤差Fig.10 Relative position error at high latitude

圖11 高緯度下陀螺零漂相對(duì)估計(jì)誤差Fig.11 Relative estimation error of the gyro bias at high latitude

圖12 高緯度下加速度計(jì)零偏相對(duì)估計(jì)誤差Fig.12 Relative estimation error of the accelerometer bias at high latitude

圖13為DVL誤差參數(shù)的相對(duì)估計(jì)誤差，無(wú)協(xié)方差變換時(shí)導(dǎo)航坐標(biāo)系切換引起的跳變幅值增大，震蕩后發(fā)散。

圖13 高緯度下DVL誤差參數(shù)相對(duì)估計(jì)誤差Fig.13 Relative estimation error of DVL error parameters at high latitude

從仿真結(jié)果可以得到如下結(jié)論，隨著緯度升高，地球自轉(zhuǎn)角速度在水平面的投影分量減少，并且載船的機(jī)動(dòng)性較弱，相關(guān)誤差狀態(tài)的可觀性減弱，導(dǎo)航坐標(biāo)系切換對(duì)于組合導(dǎo)航濾波器狀態(tài)估計(jì)值一致性的影響變大，無(wú)協(xié)方差變換時(shí)誤差狀態(tài)估計(jì)值的震蕩變大，而協(xié)方差變換算法能夠保證濾波器的平滑過(guò)渡，狀態(tài)估計(jì)精度得以保證。

5 結(jié) 論

為解決AUV進(jìn)出極區(qū)導(dǎo)航坐標(biāo)系切換引起的系統(tǒng)誤差狀態(tài)估計(jì)不一致導(dǎo)致的濾波超調(diào)問(wèn)題，實(shí)現(xiàn)全緯度導(dǎo)航，本文提出了一種基于協(xié)方差變換的INS/DVL組合導(dǎo)航濾波算法，該算法以橫地理坐標(biāo)系與地理坐標(biāo)系為例，建立了系統(tǒng)誤差狀態(tài)及其協(xié)方差矩陣在兩種坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，解決了坐標(biāo)系切換帶來(lái)的誤差狀態(tài)估計(jì)不連續(xù)的問(wèn)題。本文分別通過(guò)船載實(shí)驗(yàn)與半實(shí)物仿真實(shí)驗(yàn)驗(yàn)證了該算法的有效性，由此證明協(xié)方差變換算法全緯度適用。相較于無(wú)協(xié)方差變換，該算法能有效減少導(dǎo)航坐標(biāo)系切換帶來(lái)的導(dǎo)航參數(shù)誤差，保持導(dǎo)航的連續(xù)性。此外，該算法只需在現(xiàn)有實(shí)現(xiàn)方案的基礎(chǔ)上進(jìn)行少量的調(diào)整，有利于慣導(dǎo)系統(tǒng)的升級(jí)。未來(lái)的研究工作將探索協(xié)方差變換算法在卡爾曼濾波阻尼切換等場(chǎng)景下的應(yīng)用。