馮國虎，吳文啟，曾觀林

（國防科技大學 智能科學學院，長沙 410073）

對于航海導航，長航時導航精度和可靠性至關重要。慣性導航具有連續(xù)性好、自主性高、隱蔽性強的特點，是航海組合導航的重要基礎[1]。受水下環(huán)境限制，組合導航主要依賴慣導/計程儀組合。基于速度觀測的組合導航精度依賴于計程儀速度精度。當艦船在深遠海航行時，計程儀信號無法到達海底，此時計程儀測量的是對水速度而不是對地速度。由于洋流速度難以準確建模和實時測量，計程儀測速精度受限，基于速度觀測的濾波算法精度受計程儀測速精度影響大，而阻尼技術在載體勻速直航條件下可以做到高階無靜差，受計程儀測速精度影響小，因此阻尼技術成為主要手段[2,3]。然而，當計程儀測速誤差變化劇烈時，阻尼算法效果也不好。為確保導航可靠性，需切斷舒勒回路的阻尼，由阻尼狀態(tài)變成無阻尼狀態(tài)，以保持慣導導航精度。等計程儀測速誤差變化平緩后，再由無阻尼切換到阻尼。變阻尼瞬間舒勒回路容易出現(xiàn)大幅超調振蕩，影響阻尼效果[4,5]。

變阻尼是阻尼算法的研究重點[6]。傳統(tǒng)阻尼基于反饋校正實現(xiàn)，破壞了純慣導解算過程，阻尼切換不當會引起較大的導航誤差。本文針對傳統(tǒng)阻尼算法的不足，基于虛擬圓球法向量位置模型設計了容錯阻尼算法。相對傳統(tǒng)阻尼算法，本文算法同時具備慣導兩套解算結果：一套阻尼，一套無阻尼。無阻尼解算不受阻尼影響。基于北極科考導航數(shù)據(jù)的仿真驗證表明，本文算法的阻尼效果與傳統(tǒng)阻尼算法相當，在阻尼切換方面，定位精度更高。

1 基于法向量位置模型的誤差微分方程

傳統(tǒng)的慣性導航阻尼不適用于極區(qū)。當艦船出入極區(qū)時，需要將傳統(tǒng)導航算法與極區(qū)算法進行切換，切換過程會影響阻尼內部過程的連續(xù)性與一致性[7,8]，因此在全球統(tǒng)一的機械編排下實現(xiàn)阻尼算法十分必要。本文在全球適用的法向量位置模型下設計實現(xiàn)阻尼。

文獻[9]對法向量位置模型的建立和法向量的位置表示方法進行了詳細推導。如圖1 所示，由載體對應參考橢球位置的卯酉圈構造一個虛擬圓球，其球心為對應卯酉圈的圓心，圓球半徑為卯酉圈半徑ER與大地高度h之和。

圖1 虛擬圓球法向量示意圖Fig.1 Sketch of virtual sphere n-vector

法向量η=是虛擬圓球球心指向載體位置的單位矢量，是當?shù)厮矫娣ㄏ蛄吭诘厍蜃鴺讼迪碌耐队埃脕肀碚鬏d體的水平位置。大地高度h即載體位置相對地球橢球模型表面的高程，表征載體的垂直位置。在此模型下，用包含法向量的四元組代替經(jīng)緯高表示載體位置，避免了位置表示的奇異性問題?；诜ㄏ蛄课恢媚Ｐ偷膽T性導航機械編排方案形式更簡潔、實現(xiàn)更簡便并且適用于全球。

水平通道的姿態(tài)、速度以及位置誤差微分方程式為[9]：

其中，φe為姿態(tài)角誤差矢量，為水平速度誤差矢量，δη為法向量誤差矢量,為角速度誤差矢量。

純慣導系統(tǒng)的垂直通道不穩(wěn)定，需要對垂直通道進行阻尼。在靜基座的垂直通道中引入?yún)⒖几叨冗M行反饋補償,使系統(tǒng)閉環(huán)穩(wěn)定從而抑制高度發(fā)散?？梢缘玫浇?jīng)過阻尼后垂直通道的高度和速度的誤差微分方程為[10]：

式中uD1與uD2是引入的反饋，即：

其中，k1D與k2D為反饋系數(shù)。由于計深儀測量高度ha=htrue+δha，導航計算高度h=htrue+δh，因此δ h-δha等價于計算高度與參考高度之差h-ha。

2 容錯阻尼算法

容錯阻尼算法框圖如圖2 所示，有兩套慣導解算，一套用于無阻尼純慣導解算，另一套用于阻尼慣導解算。無阻尼純慣導解算，輸入是三個加速度計、三個陀螺儀采樣值，輸出是慣導解算的速度、位置和姿態(tài)。阻尼慣導解算過程中，輸入是純慣導解算的速度、位置和反饋的阻尼速度、位置，輸出是與純慣導解算頻率一致的速度、高度。輸出的速度、高度與外部參考速度、高度一并輸入阻尼慣導解算中阻尼環(huán)節(jié)，高度阻尼環(huán)節(jié)輸出阻尼高度，速度阻尼環(huán)節(jié)輸出阻尼速度和法向量，法向量和阻尼高度合成阻尼位置。

圖2 容錯阻尼算法框圖Fig.2 Block diagram of fault tolerant damping method

阻尼位置合成公式為：

式中下標D是阻尼damp 的縮寫。

容錯阻尼算法中的阻尼環(huán)節(jié)與傳統(tǒng)阻尼算法[10]相同，不同的是，容錯阻尼算法有慣導兩套解算，其中純慣導解算不受阻尼慣導解算影響。當外參考速度誤差變化劇烈，需要由阻尼切換至無阻尼時，可以使用純慣導解算結果。待外參考速度誤差變化平緩，由無阻尼切換至阻尼。

容錯阻尼算法的兩個特點：1.同時具備純慣導解算和阻尼慣導解算，阻尼切換方便，避免了阻尼切換過程引起的超調振蕩。2.阻尼慣導解算在純慣導解算外部完成，阻尼慣導解算過程以及結果不影響純慣導解算，增強了可靠性。

2.1 速度阻尼

相對慣導解算頻率，提供外參考速度的計程儀解算頻率較低。

有外參考速度時，阻尼速度微分方程：

式中，C是速度阻尼函數(shù)。

無阻尼純慣導解算的速度微分方程：

由于阻尼慣導解算與無阻尼純慣導解算測量同一載體運動，即兩者公式中比力項相同。阻尼速度微分方程減去無阻尼速度微分方程，得：

選取Q為相位滯后-超前串聯(lián)校正網(wǎng)絡[10]：

式中ε為阻尼比，ωs為舒勒周期頻率，該校正網(wǎng)絡的位置穩(wěn)態(tài)誤差是三階無靜差。

沒有外參考速度時，阻尼速度按純慣導解算頻率更新：

阻尼速度微分方程減去無阻尼速度微分方程，得：

方程兩邊積分，得：

采取迭代法求阻尼速度。具體步驟如下：

阻尼速度初始值計算：

阻尼位置初始值計算：

阻尼位置更新值：

2.2 阻尼切換

計程儀測速精度受使用環(huán)境限制，一方面測速數(shù)據(jù)會被環(huán)繞載體的畸變水流污染；另一方面載體運動狀態(tài)發(fā)生改變或某些海域的洋流大小和方向變化明顯，會使得外參考速度誤差變化非?？?。而當由洋流或者載體機動引起的外參考速度誤差超差時，阻尼有可能引起較大的導航參數(shù)誤差，應當切換到無阻尼模式。

為了判定外參考速度信息是否可用，設置外速度誤差變化判據(jù)G(k) ：

式中，δve（k）-δve（k-1）為k和k-1 時刻慣性系統(tǒng)速度誤差的差值，由于慣導系統(tǒng)解算具有連續(xù)性，因此相臨時刻慣導速度誤差的差值可以忽略。為k和k-1 時刻計程儀速度誤差的差值，如果G(k) 的模較大，超過設定閾值（可以選取計程儀測速誤差最大值的2 倍作為閾值，也可以根據(jù)艦艇航行的實際情況，制定適當?shù)臉藴首鳛殚撝担瑒t可以認為外參考速度誤差變化劇烈，阻尼狀態(tài)自動切換至無阻尼狀態(tài)。待G(k) 的模小于閾值，無阻尼狀態(tài)自動切換至阻尼狀態(tài)。

高度阻尼和垂直速度計算與文獻[10]相同，不再贅述。

3 基于實測數(shù)據(jù)的仿真驗證

采用北極科學考察的船載導航數(shù)據(jù)對容錯阻尼算法進行仿真驗證，并與純慣導和傳統(tǒng)外速度阻尼算法進行比較。運動軌跡如圖3 所示，數(shù)據(jù)采用GNSS 數(shù)據(jù)作為參考真值，從起始位置(56.99 °N,174.1 °E)開始，經(jīng)過72小時穿越180°經(jīng)線運動至71.37 °N,169.5 °W)。

圖3 航行軌跡及解算軌跡Fig.3 Trajectory curves of different method

垂直通道阻尼效果與文獻[10]相同，本文主要展示水平通道的速度阻尼效果。

3.1 速度阻尼驗證

速度曲線如圖4 所示，相對純慣導解算，本文方法可以抑制舒勒周期振蕩誤差。本文方法與傳統(tǒng)方法效果相當。

圖4 阻尼前后格網(wǎng)速度曲線Fig.4 Damped and undamped grid velocity curves

以純慣導定位誤差最大值為1 進行歸一化處理，位置誤差歸一化曲線如圖5 所示，相對純慣導定位誤差，本文方法可以抑制周期振蕩誤差，提高慣導系統(tǒng)定位精度。比較本文方法和傳統(tǒng)阻尼方法的位置誤差曲線，兩者精度相當。

圖5 阻尼前后歸一化位置誤差曲線Fig.5 Damped and undamped normalized position error curves

傳統(tǒng)阻尼方法基于反饋校正實現(xiàn)，本文容錯阻尼算法基于輸出校正實現(xiàn)。理論上，輸出校正與反饋校正精度相同。本文容錯阻尼算法輸入是純慣導解算結果，阻尼速度計算采用迭代法近似，存在時間延誤等因素影響，定位誤差比傳統(tǒng)阻尼方法大。從定位誤差曲線來看，本文方法和傳統(tǒng)方法精度相當。雖然精度上付出了些許代價，帶來的好處是增強了算法的工程適應性，因為在實際使用中，受通信資源所限，不一定能獲得慣導的加速度計、陀螺儀原始采樣值。

3.2 阻尼切換驗證

在29.5～31 h 內計程儀速度由于加入斜坡振蕩干擾，先幅值增大，然后等幅振蕩，最后幅值減小。計程儀速度精度超差時，系統(tǒng)由阻尼狀態(tài)切換至無阻尼純慣導解算，計程儀速度精度恢復正常后，系統(tǒng)由無阻尼切換至阻尼狀態(tài)。

系統(tǒng)由阻尼切換至無阻尼狀態(tài)，傳統(tǒng)方法切換為純慣導解算，本文方法中將阻尼慣導的計程儀速度替換為純慣導速度實現(xiàn)等效的無阻尼純慣導運算。

斜坡振蕩幅值設置不同數(shù)值，振幅大、中、小三種情況對應的阻尼方法歸一化定位誤差曲線如圖6 所示。相同閾值切換條件下，計程儀速度中斜坡振蕩干擾增大快時，根據(jù)閾值切換條件判斷，會較早發(fā)現(xiàn)計程儀速度超差，較早進行阻尼切換；而當計程儀速度中斜坡振蕩干擾增大較慢時，根據(jù)閾值切換條件判斷，則會較晚發(fā)現(xiàn)計程儀速度超差，較晚進行阻尼切換，因此圖6 的三種情況中，定位誤差曲線受計程儀速度中斜坡振蕩幅值影響的情況相似。

圖6 不同振蕩幅值的歸一化定位誤差曲線Fig.6 Normalized position error curves at different oscillation amplitudes

計程儀速度中出現(xiàn)干擾，如果阻尼切換存在較大延遲，傳統(tǒng)方法定位誤差受計程儀速度誤差干擾的影響較大，如圖7 所示。比較圖6 和圖7 發(fā)現(xiàn)，無阻尼切換阻尼后剛開始一段時間內，傳統(tǒng)方法定位誤差曲線存在明顯振蕩，與文獻[11]阻尼切換振蕩現(xiàn)象相同，本文方法不存在明顯振蕩。

圖7 阻尼切換延遲的歸一化定位誤差曲線Fig.7 Normalized position error of damping switching delay

比較圖5(c)、圖6 和圖7 發(fā)現(xiàn)，相對于全程阻尼，無阻尼切換阻尼后的定位誤差曲線，傳統(tǒng)方法的曲線形狀變化較大，本文方法的曲線形狀變化較小，受阻尼切換影響較小。

設置計程儀速度中斜坡振蕩干擾不同幅值進行測試，31 小時后傳統(tǒng)方法和本文方法的誤差數(shù)據(jù)如表1所示，相對傳統(tǒng)方法，本文方法的歸一化定位誤差最大值和均方根更小。

表1 計程儀速度中斜坡振蕩干擾不同幅值的阻尼誤差Tab.1 Damping error of different amplitude of slope oscillation in DVL velocity

綜上所述，不需要阻尼切換時，本文方法與傳統(tǒng)方位在定位精度上相當。需要阻尼切換時，本文方法比傳統(tǒng)方法定位精度高。

4 結論

針對傳統(tǒng)阻尼方法在無阻尼切換阻尼后定位誤差超調振蕩的不足，本文提出基于虛擬圓球法向量位置模型的航海慣導全球容錯阻尼算法，同時具備無阻尼純慣導解算和阻尼慣導解算。外參考速度精度超差時，阻尼慣導的外參考速度替換為純慣導速度實現(xiàn)等效的無阻尼純慣導解算，克服了傳統(tǒng)阻尼方法阻尼切換的不足。

北極科考航行導航數(shù)據(jù)驗證了本文算法的有效性，表明本文算法適用于穿越對向子午線的場景，具有全球適用性。在無須阻尼切換時，阻尼效果上與傳統(tǒng)算法相當，在阻尼切換方面相對傳統(tǒng)方法具有優(yōu)勢，均方根減小超過12%。