法向量位置模型下的水下長航時慣性導航阻尼算法

劉 潺，吳文啟，馮國虎，王茂松

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

長時間水下航行是潛航器執行任務重要的應用需求。慣性導航具有連續性好、自主性高、隱蔽性強的特點，是水下導航最基本的有效手段[1]。純慣性導航系統的垂直通道具有不穩定性，水平通道具有舒勒振蕩特性。由于水下環境的限制,潛航器能夠獲取的外部信息主要有來自計深儀的參考高度信息和來自計程儀的參考速度信息，因此垂直通道和水平通道的阻尼技術就成為了水下航行器提高長航時導航精度的主要手段[2]。傳統的慣性導航阻尼技術是在傳統當地水平地理坐標系下編排的，并不適用于極區[3]。另外當潛航器出入極區時，需要將傳統導航算法與極區算法進行切換，切換過程會影響阻尼內部過程的連續性與一致性，因此在全球統一的力學編排下實現阻尼算法十分必要[4]。

立足現代控制理論，應用卡爾曼濾波技術實現阻尼[5-8]的研究較多。然而卡爾曼濾波技術通常用于觀測噪聲為零均值的情況，而計程儀外參考速度信息可能存在未知常值誤差，水下的“慣性/速度”組合模式按傳統方法處理會使導航精度成為計程儀速度量級。而采用卡爾曼濾波增益得到常數項變阻尼參數的方法設計阻尼網絡，其位置穩態誤差通常不是無靜差的。

本文針對傳統算法的問題，基于法向量位置模型分別在垂直通道和水平通道中設計實現了阻尼網絡，水平通道阻尼后的導航參數為三階無靜差。基于北極實際導航數據的仿真實驗結果表明，本文提出的阻尼算法能夠有效抑制舒勒周期振蕩誤差，提高了導航性能。

1 基于法向量位置模型的力學編排與誤差微分方程

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

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

基于法向量位置模型的力學編排方案采用地球坐標系進行導航解算。載體垂直方向上的微分方程為：

結合水平通道和垂直通道，位置微分方程為：

式中KR為尺度系數矩陣，kR為尺度變換系數，RE為卯酉圈半徑，RN為子午圈半徑，即：

地球坐標系下的速度微分方程為：

地球坐標系下的姿態微分方程為：

因此，靜基座下垂直通道的速度、高度誤差微分方程為[9]：

其中，δvh為垂直通道速度誤差，δh為垂直通道高度誤差，ωs為舒勒周期角頻率，δha為外部參考高度誤差，δfb為比力誤差矢量。

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

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

2 垂直通道阻尼算法

當系統無外部參考高度輸入時，則式(5)的解析解在δha=δh的條件下包含隨時間增長的指數項因此，純慣性導航系統的垂直通道是不穩定的，需要在垂直通道中引入參考高度進行反饋補償，使系統閉環穩定從而抑制高度發散。由式(5)可以得到經過阻尼后垂直通道高度和垂直速度的誤差微分方程為：

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

引入反饋補償后，傳統垂直通道阻尼的原理圖如圖2所示。

圖2 垂直通道阻尼框圖Fig.2 Block diagram of vertical damping

將式(7)變換為矩陣形式：

則阻尼系統的特征方程為：

只需要配置合理的阻尼系數k1D與k2D，就能夠使系統的兩個極點都位于左半平面，也就是通過阻尼使得垂直通道變成一個漸近穩定的回路。

只考慮外參考高度誤差，由圖2可以得到傳遞函數為：

圖3 垂直通道阻尼系統的波特圖Fig.3 Bode diagram of vertical damping

計深儀指示高度信息受高頻的水面波浪影響較大。由圖3可得，垂直通道阻尼系統具有低通濾波器的特性，使計深儀指示高度信息的高頻誤差得到了衰減。

設外參考高度誤差為常值誤差，即：

考慮到ω0遠大于ωs，由拉式變換終值定理可得垂直通道速度的穩態誤差為：

其中δvhss為垂直速度的穩態誤差，由于式(13)中的系數可得外參考高度常值誤差對垂直速度的穩態誤差的影響非常小。由于水下潛航器的垂直通道解算在很多應用場景下都是不能忽略的，因此本文的阻尼方法具有較強的現實意義。

3 水平通道阻尼算法

在法向量位置模型下水平通道誤差微分方程的基礎上，通過引入計程儀的外參考速度進行反饋補償，將原本臨界穩定的系統轉換為漸近穩定的系統，建立阻尼模型的系統方程為：

其中

令尺度變換系數kR近似為1，則特征方程為：

當三個頻率函數都為0時：

觀察特征方程根的情況，除三個零根外，系統的其他6個特征根全部為純虛根，分別對應地球周期、舒勒周期和傅科周期的角頻率，此時特征方程退化為純慣導的形式。

為了方便觀察阻尼狀態下的特征方程，令三個方向上水平速度阻尼的頻率函數相等，即頻率函數H=H1=H2=H3，則原特征方程可以簡化為：

令頻率函數Q滿足：

則可以得到：

因此只要選擇適當的阻尼頻率函數Q，就能使特征方程的根具有負實部，即實現了水平阻尼。水平速度阻尼選擇頻率函數的過程中，應當同時考慮慣性儀表的輸出高頻噪聲，外參考速度的高頻測量噪聲和低頻的海流速度影響。若則外參考速度的常數誤差就不會影響阻尼速度。若則舒勒阻尼回路具有二階以上的高頻衰減特性，能有效過濾外參考速度和慣性儀表輸出的高頻干擾信號。

如果H為常數項頻率函數H=2ξωs[10]，即：

只考慮外速度參考誤差，則法向量位置穩態誤差為：

由式(21)可得，常系數反饋校正網絡的法向量位置穩態誤差是有差的，其大小與外參考速度誤差成正比。文獻[6][7][8]均應用卡爾曼濾波技術設計阻尼網絡，本質上都是變阻尼系數為常數項的比例反饋，雖然具有良好的高頻特性，變阻尼系數也能夠有效控制超調，但是在外參考速度誤差較大的情況下并不適用。

如果選擇H為相位超前串聯校正網絡[10],即：

可以得到由外速度參考誤差引起的法向量位置穩態誤差為：

是對時間的一階導數，即相位超前串聯校正網絡的法向量位置穩態誤差為一階無靜差。

如果選擇H為相位滯后-超前串聯校正網絡[10],即：

則可以得到由外速度參考誤差引起的法向量位置穩態誤差為：

是對時間的三階導數，即相位滯后-超前串聯校正網絡的法向量位置穩態誤差為三階無靜差。因此，從位置穩態誤差的角度考慮，本文選取相位滯后-超前串聯校正網設計阻尼。

經過垂直阻尼后，得到阻尼后的高度記為hDamp，垂直速度記為vhDamp。經過水平阻尼后，得到阻尼后的法向量記為ηDamp，水平速度記為。

綜上，法向量位置模型下的阻尼系統框圖如圖4所示。

圖4 法向量位置模型下的阻尼系統框圖Fig.4 Bode diagram of damping based on n-vector

4 仿真與實際航行數據半實物仿真試驗

4.1 垂直通道阻尼仿真

為了驗證該阻尼算法的適用性和有效性，采用文獻[11]中的數據轉換方法將1 h靜態數據（200 Hz）轉換至北緯85 °，航行器起始高度設為水下30 m，用于阻尼的參考高度設為10 m的常值偏差+均方差為2 m的隨機噪聲誤差，然后進行法向量位置模型下的純慣導（高度通道不置0）和垂直通道阻尼的仿真。

圖5為基于法向量位置模型的純慣導和高度通道阻尼的導航結果（垂直高度、垂直速度、橫向位置）的對比，圖中藍色虛線為純慣導的結果，紅色實線為高度通道阻尼方法的導航結果。由圖5可以看出該阻尼算法適用于極區，并且有效抑制了高度通道的發散。

圖5 阻尼前后導航結果對比Fig.5 Comparison on navigation results between damped and undamped

4.2 水平通道與垂直通道實際航行數據阻尼試驗

采用北極科學考察的一段船載實際慣性導航數據和計程儀數據進行法向量位置模型下的半實物仿真試驗。本段數據采用GNSS衛星導航數據作為參考真值，航行器從起始位置（56.99° N ,174.1°E）開始，經過72 h穿越180 °經線運動至（71.37° N ,169.5°W）。垂直通道解算以文獻[12]中的“慣性/衛星”組合導航方法為基準，進行阻尼時消除了初始高度的常值偏差；水平通道解算的位置誤差做了歸一化處理。經緯度對比、橫經度橫緯度對比、垂直通道和水平通道的導航解算結果對比如圖6-8。

圖6 經緯度對比和橫經度橫緯度對比Fig.6 Comparison on latitudes and longitudes and comparison on transversal latitudes and longitudes

經緯度對比和橫經度橫緯度對比如圖6所示，從圖中可以看到該阻尼算法適用于穿越極區、穿越對向子午線的場景，具有全球適用性。

觀察垂直通道導航參數對比曲線圖7可得，垂直通道阻尼算法能夠抑制垂直通道的發散誤差，阻尼后的高度和垂直速度與參考的慣性/衛星組合導航方法結果基本一致，高度誤差不超過1.5 m，證明了算法的有效性。

圖7 垂直通道導航參數對比Fig.7 Comparison on navigation results of vertical channel

觀察水平速度對比曲線圖8(a)(b)可得，阻尼后的水平速度相比于純慣導解算的水平速度，周期性振蕩誤差得到了明顯抑制。觀察位置誤差對比曲線圖8(c)(d)(e)可得，相比純慣導系統，阻尼算法有效抑制了位置誤差的周期振蕩，定位的歸一化誤差最大值大約減小了47%，提高了導航性能。

圖8 水平通道導航參數對比Fig.8 Comparison on navigation results of horizontal channel

5 結 論

本文以水下長航時慣性導航為背景，基于全球統一編排的法向量位置模型，分別在垂直通道和水平通道中設計實現了阻尼算法。試驗結果表明，該阻尼算法不需要出入極區的算法切換，避免了切換帶來的阻尼過程不連續的問題，具有全球適用性，有效抑制了垂直通道的發散誤差和水平通道的舒勒周期振蕩誤差，提升了導航性能。下一步將針對惡劣海況、航行器大機動等條件對阻尼切換、變阻尼算法等內容進行深入研究。