郭 瑜，徐 博，余志強，田鑫雨

（1.哈爾濱工程大學 智能科學與工程學院，哈爾濱 150001；2.武漢第二船舶設計研究所，武漢 430064）

以慣性導航系統(tǒng)（Inertial Navigation System,INS）為核心的組合導航技術已被廣泛用于水下導航領域[1]。由于電磁波在水中衰減嚴重，常見的全球導航衛(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)等依賴電磁波傳遞信息的導航方式均不可用。在水下場景多以聲學設備為輔助對INS 誤差進行抑制。超短基線聲學定位系統(tǒng)（Ultra-short Baseline Acoustic Positioning System,USBL）具有體積小、易部署、定位精度高等特點，將USBL 與INS 進行組合可以克服USBL 更新頻率低、導航數(shù)據(jù)不連續(xù)以及INS 誤差易發(fā)散的缺陷，成為了研究熱點[2,3]。

文獻[4]以USBL 解算的載體位置為觀測量，結合慣導系統(tǒng)和其他傳感器構建了聯(lián)邦濾波結構，提高了系統(tǒng)的容錯性。文獻[5]直接利用接收機和聲學信標的相對距離為觀測量，構建了基于擴展卡爾曼濾波器的INS/USBL 組合導航系統(tǒng)。此外針對極區(qū)導航這類特殊場景，文獻[6][7]基于網(wǎng)格坐標系構建了USBL 輔助下的組合導航系統(tǒng)。在現(xiàn)有的INS/USBL 松組合導航系統(tǒng)中，直接采用位置信息或相對位置矢量構建量測。實際上USBL 的原始測量為方位角和斜距，受安裝偏差角、姿態(tài)誤差等因素的影響，在位置信息解算的過程中容易引入確定性誤差，并且濾波參數(shù)很難根據(jù)USBL 傳感器精度準確選取[8]。為了解決這類問題，文獻[8]-[10]直接利用USBL 測量的方位角和斜距信息構建觀測量，得到了INS/USBL 緊組合導航系統(tǒng)，并采用基于Student’s t分布的魯棒濾波器處理測量野值。針對USBL 異常信息檢測的問題，文獻[11]采用自適應模糊神經(jīng)網(wǎng)絡診斷和修復異常信息，提高了INS/USBL 緊組合導航系統(tǒng)的魯棒性。相比于松組合，INS/USBL 緊組合導航系統(tǒng)已經(jīng)具有更高的精度。

上述提到的組合導航系統(tǒng)中向量誤差的定義都是計算值減去真實值。實際上，慣導系統(tǒng)解算的參考坐標系與真實的參考坐標系間存在失準角，使得解算的速度和位置矢量與真實的速度和位置矢量不處于同一坐標系。對于不同坐標系中的矢量，直接做差來定義誤差是不準確的，易產(chǎn)生空間不一致性。慣導解算的參考坐標系和真實參考坐標系間的失準角越大，傳統(tǒng)誤差定義越不準確。為了解決上述問題，近些年來，李群理論開始被引入組合導航系統(tǒng)的研究中。通過將轉動和平動定義在同一個李群空間中，能夠有效改善空間不一致性，提高導航精度[12]。李群空間上定義的誤差可以稱為不變誤差，所謂的不變是指對真實的群狀態(tài)和慣導解算的群狀態(tài)做相同的線性變換后，誤差保持不變。文獻[13]在慣性坐標系下基于不變誤差定義構建新的慣導誤差模型，并將其應用在初始對準中實現(xiàn)了粗對準和精對準的統(tǒng)一。文獻[14]基于李群非線性誤差構建了INS/GNSS/磁力計組合導航系統(tǒng)，相比于傳統(tǒng)誤差模型展示出了更好的姿態(tài)精度。文獻[15]基于左不變誤差和右不變誤差的定義推導了地球坐標系下INS/GNSS 和INS/里程計（Odometer,OD）組合導航系統(tǒng)，結果表明左不變誤差定義下的INS/GNSS組合導航系統(tǒng)和右不變誤差定義下的INS/OD 組合導航系統(tǒng)更適合在大初始姿態(tài)誤差條件下工作。文獻[16]比較了不同慣導系統(tǒng)機械編排和誤差定義對INS/多普勒計程儀（Doppler Velocity Log,DVL）組合導航系統(tǒng)的影響，結果表明在小初始姿態(tài)誤差條件下，左不變誤差定義下的INS/DVL 組合導航系統(tǒng)的精度最高。文獻[17]則將李群理論應用在了INS/USBL 松組合導航系統(tǒng)中，在右不變誤差定義下推導了松組合的觀測方程。

考慮到傳統(tǒng)誤差定義的缺陷和INS/USBL 松組合的局限性，本文采用了左不變/右不變誤差定義下的誤差微分方程[15]，根據(jù)左不變和右不變誤差的定義推導緊組合量測方程，構建了不變誤差定義下的INS/USBL 緊組合導航系統(tǒng)。對不同的INS/USBL 組合導航算法進行了比較，結果表明：所提出的基于右不變誤差定義的INS/USBL 緊組合導航系統(tǒng)比松組合具有更高的定位精度，與傳統(tǒng)誤差定義下的INS/USBL 緊組合導航系統(tǒng)相比，對初始姿態(tài)精度的需求更低，能夠在初始對準效果不好的條件下工作。

1 USBL 工作原理

USBL 主要由聲學信標和聲學基陣構成。聲學信標通常放置在海底，準確位置已知，聲學基陣則安裝在航行器上。USBL 利用聲學信號到達聲學基陣陣元的相位差解算聲學信標與基陣中心的相對方位角，利用聲學信號從聲學信標到基陣中心的往返時延解算斜距，USBL 工作原理如圖1 所示。

圖1 USBL 工作原理Fig.1 USBL working principle

假設USBL 測量的方位角信息為α和β，斜距信息為r，則聲學信標和基陣之間的相對位置矢量可以表示為：

2 基于李群誤差的INS/USBL 緊組合導航算法

本文提出的算法主要包括純慣性解算、組合導航模型構建、誤差估計與反饋校正。首先，INS 實時解算導航參數(shù)，并構建導航誤差狀態(tài)方程。當USBL 數(shù)據(jù)更新時，利用USBL 輸出的方位角和斜距信息構建量測方程并采用卡爾曼濾波器對誤差狀態(tài)進行估計，得到誤差估計結果后對INS 進行反饋校正。組合導航系統(tǒng)結構圖如圖2 所示。

圖2 INS/USBL 組合導航系統(tǒng)結構圖Fig.2 Structure of INS/USBL integrated navigation system

2.1 INS 機械編排

與之前INS/USBL 松組合的研究工作類似[17]，本文同樣采用地球系下的INS 機械編排方式。

INS 根據(jù)初始的姿態(tài)陣、速度矢量和位置矢量，結合式(2)就可以實時解算導航參數(shù)。式(2)所示的地球坐標系下INS 機械編排，相比于文獻[8]中東-北-天坐標系下的機械編排，計算量更小，且在南北極點處沒有計算奇異點，具備全球導航能力[18]。

更為重要的是，式(2)所示的INS 機械編排滿足李群中“群仿射”性質，即對于式(3)(4)所示的群狀態(tài)χ和動力學模型

其中，χA和χB分別表示不同的群狀態(tài)。

則將群狀態(tài)誤差對數(shù)投影到李代數(shù)向量空間上后，李代數(shù)向量空間上誤差模型獨立于導航參數(shù)[19]?！叭悍律洹毙再|使得這類誤差模型適用于較大的初始姿態(tài)誤差條件。

2.2 INS/USBL 緊組合導航系統(tǒng)誤差狀態(tài)方程

在傳統(tǒng)誤差模型中將姿態(tài)誤差定義在特殊正交群空間，將速度和位置誤差定義在歐幾里得空間，而李群理論將姿態(tài)矩陣、速度矢量和位置矢量構建在同一李群空間(如式(3)所示)，定義導航誤差時不存在空間不一致問題。根據(jù)李群中左不變誤差和右不變誤差的定義分別推導兩種誤差定義下的慣導/超短基線緊組合導航算法。首先給出不同誤差定義下的狀態(tài)方程[15]。

2.2.1 左不變誤差狀態(tài)方程

式(3)所示群狀態(tài)的左不變誤差形式為[15]：

其中，ηl表示左不變誤差群狀態(tài)；表示由慣導解算的導航參數(shù)構成的群狀態(tài)；χ表示由真實的導航參數(shù)構成的群狀態(tài)分別為慣導解算的地球系到載體系的姿態(tài)變換矩陣、速度矢量和位置矢量。

所謂的左不變是指，對和χ左乘一個相同的矩陣后，ηl保持不變。利用李群和李代數(shù)間的對數(shù)映射關系得到左不變誤差定義下的誤差向量為[15]：

其中，φl表示左不變誤差定義下的姿態(tài)失準角，φl=?la，a是單位旋轉矢量；dvl和dpl分別表示左不變誤差定義下的速度誤差矢量和位置誤差矢量。

由式(7)可以看出，最終得到的速度誤差向量和位置誤差向量中均考慮了姿態(tài)失準角φl的影響。因此李群理論的引入使得導航誤差的定義更為準確。

從式(14)中可以看出，狀態(tài)轉移矩陣中只包含慣性器件的輸出。對于中高精度的INS，陀螺漂移和加速度計零偏所引起的誤差與載體角運動和線運動信息相差多個數(shù)量級。整個狀態(tài)轉移矩陣很準確，即使初始姿態(tài)誤差很大，不準確的姿態(tài)陣也不會對狀態(tài)轉移矩陣的準確性產(chǎn)生影響，濾波性能容易被保證。

2.2.2 右不變誤差狀態(tài)方程

式(4)所示群狀態(tài)的右不變誤差形式為[15]：

其中，ηr為右不變誤差群狀態(tài)。所謂的右不變是指對和χ右乘一個相同的矩陣后，ηr保持不變。類似于左不變誤差微分方程，利用李群和李代數(shù)間的對數(shù)映射關系可以得到右不變誤差定義下的誤差向量和其微分方程[15]：

其中，φr表示右不變誤差定義下的姿態(tài)失準角；dvr和dpr分別表示右不變誤差定義下的速度誤差矢量和位置誤差矢量。

選取誤差向量φr、dvr、dpr、和 ?b為狀態(tài)量，得到右不變誤差下的組合導航誤差狀態(tài)方程。

從式(21)(22)中可以看出，如果忽略慣性器件誤差，誤差狀態(tài)的傳播只與有關，不會受到含有誤差的導航參數(shù)的影響。短時間內(nèi)大初始導航誤差對導航系統(tǒng)的影響比慣性器件誤差的影響大得多。如果算法能在短時間內(nèi)快速準確地收斂，那么不準確的慣性器件誤差系數(shù)項對系統(tǒng)的影響就很小，幾乎可以忽略。因此，這類右不變誤差模型同樣適合初始姿態(tài)誤差較大的場合。

2.3 INS/USBL 松組合導航系統(tǒng)量測噪聲分析

松組合導航系統(tǒng)將USBL 相對于聲學信標的位置矢量與INS 相對于聲學信標的位置矢量做差作為量測向量[17]。假設USBL 測量的方位角信息和斜距信息含有零均值高斯噪聲，則USBL 測量的方位角信息和斜距信息表示為：

其中，α、β和r表示真實值；wα、wβ和wr表示測量噪聲。由式(1)可知的計算涉及到非線性變換，這使得含有隨機噪聲的位置矢量相對于真實位置矢量不是一個無偏量[20]。例如：

即使USBL 測量的方位角和斜距信息的噪聲是零均值高斯噪聲，松組合的量測噪聲也不再符合零均值假設，進而影響了松組合的濾波精度。因此，本文在不變誤差定義下推導新的緊組合導航模型，直接使用方位角和斜距信息，以獲得比文獻[17]松組合模型更高的導航精度。

2.4 INS/USBL 緊組合導航系統(tǒng)量測方程

首先推導左不變誤差下的量測方程，在左不變誤差定義下，式(25)可以重寫為：

對式(31)進行整理，得到左不變誤差定義下的緊組合量測方程：

接下來推導右不變誤差定義下的量測方程，與式(29)的推導過程類似，在右不變誤差定義下，式(25)可以重寫為：

同理可得到右不變誤差量測向量為：

量測方程經(jīng)過整理得：

從左不變誤差定義下的量測方程中可以看出，量測矩陣Hl包含有姿態(tài)陣，當初始姿態(tài)誤差很小時，含有誤差的姿態(tài)陣并不會對量測矩陣Hl的準確性產(chǎn)生明顯影響，結合完全獨立于導航參數(shù)的左不變誤差狀態(tài)方程，使得左不變誤差定義下的緊組合導航系統(tǒng)在較小的初始姿態(tài)誤差條件下有著良好的性能。當姿態(tài)解算誤差很大時，不準確的使整個量測矩陣的準確度受到負面影響，因此，在初始姿態(tài)誤差較大時，左不變誤差下的誤差模型會面臨收斂速度和收斂精度下降的問題。

構建緊組合導航系統(tǒng)的誤差模型后可以采用線性卡爾曼濾波器對導航誤差進行估計，根據(jù)誤差估計結果對INS 進行反饋校正，抑制系統(tǒng)誤差發(fā)散。

3 算法驗證

3.1 數(shù)值仿真驗證

在數(shù)值仿真中，將本文所提出的左不變/右不變誤差定義下的慣導/超短基線緊組合導航算法（Left Invariant Error-based Tight couple Kalman Filter/ Right Invariant Error-based Tight couple Kalman Filter,LIETKF/ RIETKF）與右不變誤差定義下的慣導/超短基線松組合導航算法（Right Invariant Error-based loose couple Kalman Filter,RIELKF）[17]以及傳統(tǒng)誤差定義下的慣導/超短基線緊組合導航算法（Tight couple Kalman Filter,TKF）進行比較。

基于式(2)中地球系機械編排的TKF 模型為：

仿真參數(shù)如表1 所示，所有噪聲方差陣均按照表1中參數(shù)設置。仿真軌跡如圖3 所示，初始姿態(tài)為[0 °,0 °,45 ° ]，初始速度為2m s，初始位置為[45.7796 °,126.6705 °,0 m]。采用三角函數(shù)模擬海浪、海風等外界擾動導致的載體角運動和線運動，俯仰、橫搖和航向的搖擺幅值分別設置為[2 °,4 °,2 ° ]，搖擺周期分別為12 s、10 s 和8 s；橫蕩、縱蕩和垂蕩加速度幅值均設為0.2m s2，周期均設置為20 s。設置多個信標有助于提高系統(tǒng)可觀測性，兩個信標經(jīng)緯度分別為[45.79 °,126.677 ° ]和[45.786 °,126.695 °]，深度100 m。

表1 仿真參數(shù)Tab.1 Simulation parameters

圖3 仿真軌跡Fig.3 Simulation trajectory

首先對比小初始姿態(tài)誤差條件下 LIETKF、RIETKF 以及傳統(tǒng)誤差定義下的TKF 的性能。初始水平姿態(tài)誤差角從[0 °,0.2 °] 區(qū)間內(nèi)隨機選取，初始航向誤差角從[0 °,1 °] 區(qū)間內(nèi)隨機選取。進行50 次蒙特卡洛仿真，采用均方根誤差（Root Mean Square Error,RMSE）衡量算法的精度，蒙特卡洛仿真中RMSE 統(tǒng)計方式為：

其中，k表示時刻，j表示第j次蒙特卡洛仿真表示k時刻第j次蒙特卡洛仿真的導航誤差；M為總的仿真次數(shù)，本文M=50。

仿真統(tǒng)計了三種算法的姿態(tài)角和位置的RMSE，如圖4-5 所示。

圖4 小初始姿態(tài)誤差下三種緊組合算法姿態(tài)RMSEFig.4 Attitude RMSE of three tightly integrated navigation algorithms with small initial attitude error

圖5 小初始姿態(tài)誤差下三種緊組合算法位置RMSEFig.5 Position RMSE of three tightly integrated navigation algorithms with small initial attitude error

從圖4-5 可以看出，即使初始姿態(tài)誤差較小，三種算法仍有較明顯區(qū)別。對于姿態(tài)中最為重要的航向角，TKF 的誤差最大，并且TKF 的收斂速度明顯比LIETKF 和RIETKF 慢，這是由于傳統(tǒng)誤差模型中速度和位置誤差定義未考慮計算參考坐標系與真實參考坐標系間的失準角，誤差模型的準確性不如不變誤差定義，影響了TKF 的濾波收斂速度和收斂精度。

對于兩種不變誤差定義下的緊組合導航算法，從圖4 可以看出，與LIETKF 相比，RIETKF 的航向精度略高、收斂速度更快。為了更細致地體現(xiàn)不同算法的精度，表2 統(tǒng)計了運行400 s 后三種算法的平均RMSE（Average RMSE,ARMSE），可以看出RIETKF和LIETKF 的定位精度和姿態(tài)精度非常接近，但RIETKF 略好，這是因為RIETKF 量測矩陣的精度完全不會受到導航誤差的影響，而LIETKF 量測矩陣中會引入姿態(tài)誤差和位置誤差。因此，相比于TKF 和LIETKF，在小初始姿態(tài)誤差條件下RIETKF 的導航精度略高。

表2 400 s 后三種緊組合算法的ARMSETab.2 ARMSE of three tightly integrated navigation algorithms after 400 s

在較大的初始姿態(tài)誤差條件下，比較TKF、LIETKF 和RIETKF 三種算法的性能。初始水平姿態(tài)誤差角從[0 °,10 °] 區(qū)間內(nèi)隨機選取，初始航向誤差角從[0 °,60 ° ]區(qū)間內(nèi)隨機選取，進行50 次蒙特卡洛仿真，得到LITEKF、RIETKF 和TKF 的姿態(tài)誤差圖，如圖6 所示。

圖6 三種緊組合算法姿態(tài)誤差蒙特卡洛仿真結果Fig.6 Monte Carlo simulation results of attitude errors for three tightly integrated navigation algorithms

從圖6 可以看出，在這種較大初始姿態(tài)誤差條件下，傳統(tǒng)的TKF 容易發(fā)散，這是因為傳統(tǒng)的線性誤差模型并不能描述大姿態(tài)誤差下非線性的誤差傳播。從式(42)可以看出，TKF 的狀態(tài)轉移矩陣中包含有加速度計輸出在地球系下的投影，在進行濾波估計時，所采用的坐標系變換矩陣是INS 解算的在初始姿態(tài)誤差較大的條件下，不準確的使得TKF的狀態(tài)轉移矩陣存在較大誤差，嚴重影響了濾波估計精度，甚至無法收斂。

LIETKF 的收斂情況比TKF 好得多，但在部分條件下，收斂精度較差，存在較大的姿態(tài)誤差，而RIETKF 始終能夠實現(xiàn)很高的收斂精度。導致LIETKF和RIETKF 如此大區(qū)別的主要原因在于LIETKF 的量測矩陣中存在姿態(tài)陣(見式(33))，而RIETKF 的量測矩陣可以轉化為只與聲學信標位置有關。與不準確的對TKF 的影響類似，大初始姿態(tài)誤差使得濾波初始時刻LIETKF 的量測矩陣極不準確，嚴重影響了濾波精度。綜合考慮小初始姿態(tài)誤差和大初始姿態(tài)誤差下的仿真結果，右不變誤差定義更適合慣導/超短基線緊組合導航系統(tǒng)，RIETKF 使得緊組合導航系統(tǒng)對于初始對準精度的需求降低。

為了證明緊組合導航系統(tǒng)相比于松組合導航系統(tǒng)的精度優(yōu)勢，比較右不變誤差定義RIELKF 和RIETKF的導航性能。同樣進行50 次蒙特卡洛仿真，初始水平姿態(tài)誤差角從[0 °,5 °] 區(qū)間內(nèi)隨機選取，初始航向誤差角從[0 °,30 °] 區(qū)間內(nèi)隨機選取，得到 RIELKF 和RIETKF 的姿態(tài)和位置RMSE 圖，如圖7-8 所示。從圖7 可以看出，隨著時間的增長，松組合和緊組合姿態(tài)誤差的區(qū)別逐漸減小。圖8 統(tǒng)計了RIETKF 和RIELKF 三個位置方向的RMSE，可以看出兩種算法在東向和天向上的位置誤差區(qū)別最為明顯。

圖8 RIELKF 和RIETKF 的位置RMSEFig.8 Position RMSE of RIELKF and RIETKF

圖9 航行軌跡Fig.9 Test trajectory

圖10 參考速度Fig.10 Reference velocity

表3 統(tǒng)計了400 s 后兩種算法的姿態(tài)和位置ARMSE。對于姿態(tài)中最重要的航向指標，兩者的ARMSE 幾乎完全相同，差別僅有0.0001°，因此，單從姿態(tài)精度方面難以表明緊組合導航系統(tǒng)和松組合導航系統(tǒng)孰優(yōu)孰劣。然而，RIELKF 的位置ARMSE 比RIETKF 大，這是因為松組合以USBL 解算的相對于聲學信標的位置矢量為觀測，在解算相對位置矢量過程中方位角和斜距經(jīng)過了一系列非線性變換，使得松組合量測噪聲難以保證零均值高斯特性，噪聲方差難以準確設置，影響了導航定位精度。相比于松組合導航系統(tǒng)，緊組合導航系統(tǒng)在東向、北向和天向上位置精度分別提升了15.79%、2.61%和22.96%。綜上所述，對于右不變誤差定義下的慣導/超短基線組合導航系統(tǒng)，從定位精度方面考慮應該首選緊組合工作方式。

表3 400 s后右不變誤差定義下緊組合和松組合算法的ARMSETab.3 ARMSE of RIELKF and RIETKF after 400 s

3.2 半實物仿真驗證

由于缺乏USBL 測量的方位角和斜距數(shù)據(jù)，因此采用半實物仿真對本文算法進行驗證。慣導系統(tǒng)數(shù)據(jù)來源于一次海試實驗中的自研光纖捷聯(lián)慣導系統(tǒng)，航行軌跡和速度分別如圖 9-10 所示。初始位置為[18.23927 °,109.39400 °]，采用法國PHINS 慣導系統(tǒng)和GPS 組合提供參考信息。所設置的信標位置分別為[18.244 °,109.41 °] 和[18.260 °,109.407 °]，深度100 m。根據(jù)設置的信標位置和實驗船參考位置模擬解算相對方位角和斜距。所采用的設備參數(shù)如表4 所示。

表4 設備性能Tab.4 Equipment performance

模擬的超短基線斜距刻度系數(shù)誤差0.2%，斜距噪聲標準差2 m，方位角噪聲標準差0.2°，更新周期3 s。將左不變/右不變誤差定義下的慣導/超短基線緊組合導航算法（LIETKF/RIETKF）和右不變誤差定義下的慣導/超短基線松組合導航算法（RIETKF）的性能進行比較，為了保證算法的可重復性，同樣進行了50次蒙特卡洛仿真。初始水平姿態(tài)誤差角從[0 °,10 °] 區(qū)間內(nèi)隨機選取，初始航向誤差角從[0 °,60 °] 區(qū)間內(nèi)隨機選取。圖11-12 展示了RIELKF、LIETKF 和RIETKF三種算法的姿態(tài)和位置RMSE。

圖11 三種算法姿態(tài)RMSEFig.11 Attitude RMSE of three algorithms

從圖11 可以看出，三種算法最終的收斂精度幾乎相同，但是收斂速度有著較大的區(qū)別。對于姿態(tài)中最重要的航向角，RIETKF 的航向誤差收斂速度最快，RIELKF 和LIETKF 的收斂速度則慢得多。在RIETKF收斂很長時間后，RIELKF 和LIETKF 的航向誤差仍比RIETKF 大。從圖12 可以看出，對于東向位置和北向位置，緊組合模式RIETKF 和LIETKF 的定位誤差要比松組合模式RIELKF 的定位誤差?。粚τ谔煜蛭恢谜`差，緊組合算法與松組合算法的區(qū)別更加明顯。

圖12 三種算法位置RMSEFig.12 Position RMSE of three algorithms

表5 統(tǒng)計了2000 s 后姿態(tài)和位置的ARMSE。對于重要的航向角精度，RIETKF 相比于LIETKF 和RIELKF 分別提高了8.46%和16.55%。對于定位精度，RIETKF 和LIETKF 較為接近，在東向和天向位置上，RIETKF 與LIETKF 相比，精度分別提高了2.72%和10.91%，僅僅在北向定位精度上比LIETKF 低0.52%，0.52%的差別幾乎可以忽略。此外RIETKF 的定位性能更是全面優(yōu)于RIELKF，在東向、北向和天向上，相比于 RIELKF 分別提高了 14.65%、30.26%和73.91%。從算法的收斂速度和定位精度考慮，RIETKF的性能最好，這與數(shù)值仿真得到的結論相同。

表5 2000 s 后三種算法的ARMSETab.5 ARMSE of three algorithms after 2000 s

4 結論

本文針對現(xiàn)有INS/USBL 緊組合導航算法誤差定義不準確，影響導航精度的問題，引入李群誤差理論，提出基于不變誤差定義的INS/USBL 緊組合導航算法。仿真結果表明：1）傳統(tǒng)誤差定義下的INS/USBL緊組合導航算法收斂速度和收斂精度均不如不變誤差定義下的緊組合導航算法，且在較大的初始姿態(tài)誤差條件下更容易發(fā)散；2）不變誤差定義下，緊組合算法的定位精度高于松組合算法；3）右不變誤差定義更適合INS/USBL 緊組合導航系統(tǒng)，相比于左不變誤差定義能夠應對更大的初始姿態(tài)誤差。