法向量位置模型下的水下長航時慣性導(dǎo)航阻尼算法

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

（國防科技大學(xué) 智能科學(xué)學(xué)院，長沙 410073）

長時間水下航行是潛航器執(zhí)行任務(wù)重要的應(yīng)用需求。慣性導(dǎo)航具有連續(xù)性好、自主性高、隱蔽性強的特點，是水下導(dǎo)航最基本的有效手段[1]。純慣性導(dǎo)航系統(tǒng)的垂直通道具有不穩(wěn)定性，水平通道具有舒勒振蕩特性。由于水下環(huán)境的限制,潛航器能夠獲取的外部信息主要有來自計深儀的參考高度信息和來自計程儀的參考速度信息，因此垂直通道和水平通道的阻尼技術(shù)就成為了水下航行器提高長航時導(dǎo)航精度的主要手段[2]。傳統(tǒng)的慣性導(dǎo)航阻尼技術(shù)是在傳統(tǒng)當?shù)厮降乩碜鴺讼迪戮幣诺?，并不適用于極區(qū)[3]。另外當潛航器出入極區(qū)時，需要將傳統(tǒng)導(dǎo)航算法與極區(qū)算法進行切換，切換過程會影響阻尼內(nèi)部過程的連續(xù)性與一致性，因此在全球統(tǒng)一的力學(xué)編排下實現(xiàn)阻尼算法十分必要[4]。

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

本文針對傳統(tǒng)算法的問題，基于法向量位置模型分別在垂直通道和水平通道中設(shè)計實現(xiàn)了阻尼網(wǎng)絡(luò)，水平通道阻尼后的導(dǎo)航參數(shù)為三階無靜差?；诒睒O實際導(dǎo)航數(shù)據(jù)的仿真實驗結(jié)果表明，本文提出的阻尼算法能夠有效抑制舒勒周期振蕩誤差，提高了導(dǎo)航性能。

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

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

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

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

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

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

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

地球坐標系下的姿態(tài)微分方程為：

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

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

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

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

2 垂直通道阻尼算法

當系統(tǒng)無外部參考高度輸入時，則式(5)的解析解在δha=δh的條件下包含隨時間增長的指數(shù)項因此，純慣性導(dǎo)航系統(tǒng)的垂直通道是不穩(wěn)定的，需要在垂直通道中引入?yún)⒖几叨冗M行反饋補償，使系統(tǒng)閉環(huán)穩(wěn)定從而抑制高度發(fā)散。由式(5)可以得到經(jīng)過阻尼后垂直通道高度和垂直速度的誤差微分方程為：

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

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

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

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

則阻尼系統(tǒng)的特征方程為：

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

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

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

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

設(shè)外參考高度誤差為常值誤差，即：

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

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

3 水平通道阻尼算法

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

其中

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

當三個頻率函數(shù)都為0時：

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

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

令頻率函數(shù)Q滿足：

則可以得到：

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

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

只考慮外速度參考誤差，則法向量位置穩(wěn)態(tài)誤差為：

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

如果選擇H為相位超前串聯(lián)校正網(wǎng)絡(luò)[10],即：

可以得到由外速度參考誤差引起的法向量位置穩(wěn)態(tài)誤差為：

是對時間的一階導(dǎo)數(shù)，即相位超前串聯(lián)校正網(wǎng)絡(luò)的法向量位置穩(wěn)態(tài)誤差為一階無靜差。

如果選擇H為相位滯后-超前串聯(lián)校正網(wǎng)絡(luò)[10],即：

則可以得到由外速度參考誤差引起的法向量位置穩(wěn)態(tài)誤差為：

是對時間的三階導(dǎo)數(shù)，即相位滯后-超前串聯(lián)校正網(wǎng)絡(luò)的法向量位置穩(wěn)態(tài)誤差為三階無靜差。因此，從位置穩(wěn)態(tài)誤差的角度考慮，本文選取相位滯后-超前串聯(lián)校正網(wǎng)設(shè)計阻尼。

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

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

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

4 仿真與實際航行數(shù)據(jù)半實物仿真試驗

4.1 垂直通道阻尼仿真

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

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

圖5 阻尼前后導(dǎo)航結(jié)果對比Fig.5 Comparison on navigation results between damped and undamped

4.2 水平通道與垂直通道實際航行數(shù)據(jù)阻尼試驗

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

圖6 經(jīng)緯度對比和橫經(jīng)度橫緯度對比Fig.6 Comparison on latitudes and longitudes and comparison on transversal latitudes and longitudes

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

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

圖7 垂直通道導(dǎo)航參數(shù)對比Fig.7 Comparison on navigation results of vertical channel

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

圖8 水平通道導(dǎo)航參數(shù)對比Fig.8 Comparison on navigation results of horizontal channel

5 結(jié) 論

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