基于虛擬圓球法向量位置模型的航海慣導(dǎo)全球容錯(cuò)阻尼算法

馮國(guó)虎，吳文啟，曾觀林

（國(guó)防科技大學(xué) 智能科學(xué)學(xué)院，長(zhǎng)沙 410073）

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

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

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

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

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

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

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

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

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

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

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

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

2 容錯(cuò)阻尼算法

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

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

阻尼位置合成公式為：

式中下標(biāo)D是阻尼damp 的縮寫。

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

容錯(cuò)阻尼算法的兩個(gè)特點(diǎn)：1.同時(shí)具備純慣導(dǎo)解算和阻尼慣導(dǎo)解算，阻尼切換方便，避免了阻尼切換過程引起的超調(diào)振蕩。2.阻尼慣導(dǎo)解算在純慣導(dǎo)解算外部完成，阻尼慣導(dǎo)解算過程以及結(jié)果不影響純慣導(dǎo)解算，增強(qiáng)了可靠性。

2.1 速度阻尼

相對(duì)慣導(dǎo)解算頻率，提供外參考速度的計(jì)程儀解算頻率較低。

有外參考速度時(shí)，阻尼速度微分方程：

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

無(wú)阻尼純慣導(dǎo)解算的速度微分方程：

由于阻尼慣導(dǎo)解算與無(wú)阻尼純慣導(dǎo)解算測(cè)量同一載體運(yùn)動(dòng)，即兩者公式中比力項(xiàng)相同。阻尼速度微分方程減去無(wú)阻尼速度微分方程，得：

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

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

沒有外參考速度時(shí)，阻尼速度按純慣導(dǎo)解算頻率更新：

阻尼速度微分方程減去無(wú)阻尼速度微分方程，得：

方程兩邊積分，得：

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

阻尼速度初始值計(jì)算：

阻尼位置初始值計(jì)算：

阻尼位置更新值：

2.2 阻尼切換

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

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

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

高度阻尼和垂直速度計(jì)算與文獻(xiàn)[10]相同，不再贅述。

3 基于實(shí)測(cè)數(shù)據(jù)的仿真驗(yàn)證

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

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

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

3.1 速度阻尼驗(yàn)證

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

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

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

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

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

3.2 阻尼切換驗(yàn)證

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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

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