復(fù)雜水下環(huán)境下高精度SINS/USBL誤差標(biāo)定技術(shù)

張 濤，王 健，張佳宇

(1. 東南大學(xué)儀器科學(xué)與工程學(xué)院，南京 210096；2. 北京控制與電子技術(shù)研究所，北京 100038)

0 引言

在捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System, SINS)/超短基線(Ultra-short Baseline, USBL)一體化產(chǎn)品的研制方面，水下定位的迫切需求促使水聲定位技術(shù)進一步發(fā)展，USBL定位系統(tǒng)及裝備朝著慣性/水聲一體化的方向發(fā)展。SINS/USBL一體化系統(tǒng)中，載體坐標(biāo)系和聲學(xué)坐標(biāo)系在出廠前不可避免地存在安裝誤差角和桿臂誤差，并且均為固定的常值。因此，在導(dǎo)航與定位前需要提前進行標(biāo)定并補償，精確標(biāo)定后無需重復(fù)標(biāo)定，這也是一體化系統(tǒng)的突出優(yōu)勢。影響高精度USBL定位系統(tǒng)定位精度的主要因素有系統(tǒng)自身誤差、海洋環(huán)境參數(shù)測量誤差以及SINS/USBL之間的安裝誤差。其中，聲學(xué)基陣安裝誤差是定位系統(tǒng)的主要誤差源, 從數(shù)值角度粗略分析:1°的航向安裝誤差角將會引入約1.7%斜距的水平位置誤差，因此，在實際使用前需要進行安裝誤差的標(biāo)定[1]。

國內(nèi)外眾多研究所和公司在標(biāo)定算法方面進行了研究，但是國外公開報道的文獻和技術(shù)資料并不多。2003年，D.Philip等對影響聲學(xué)基陣標(biāo)定精度的因素進行了分析，包括采集數(shù)據(jù)的質(zhì)量、USBL基陣的幾何結(jié)構(gòu)、高效的數(shù)值計算算法和觀測值的權(quán)重等[2]。R.Mcewen等在2005年通過對分立式USBL系統(tǒng)的研究發(fā)現(xiàn)，聲學(xué)傳感器與姿態(tài)傳感器之間的平均安裝誤差在2.5°左右，標(biāo)準(zhǔn)差在0.5°左右[3]。國內(nèi)對于USBL標(biāo)定技術(shù)的研究以哈爾濱工程大學(xué)、中國海洋大學(xué)和東南大學(xué)等科研機構(gòu)為主[4]。將全球定位系統(tǒng)(Global Positioning System， GPS)與水下聲學(xué)定位系統(tǒng)相結(jié)合, 中國海洋大學(xué)的唐秋華利用空間測距交會的方法確定水下應(yīng)答器的位置, 并推導(dǎo)了聲學(xué)基陣坐標(biāo)系和載體坐標(biāo)系之間的轉(zhuǎn)換關(guān)系, 通過最小二乘法求取出6個校準(zhǔn)參數(shù)，實現(xiàn)了對安裝誤差角的標(biāo)定[5]。哈爾濱工程大學(xué)的鄭翠娥等提出了一種基于最小二乘的USBL定位系統(tǒng)安裝誤差校準(zhǔn)方法，并推導(dǎo)了完整的誤差標(biāo)定模型，長江和海試結(jié)果證明了其有效性[6-8]?；趯Π惭b誤差角所導(dǎo)致的定位誤差的分析，臺灣國立中央大學(xué)的陳信宏等利用測量船繞著海底的應(yīng)答器行駛，先確定翻滾誤差角，然后確定航向誤差角，最后是縱搖誤差角，并通過仿真驗證和實際實驗證明了算法的有效性，但航向安裝誤差角和縱向安裝角的估計會出現(xiàn)振蕩[9-10]。在此基礎(chǔ)上，東南大學(xué)的童金武基于布放的2個應(yīng)答器提出了一種基于增量迭代的安裝誤差角快速標(biāo)定算法，但是該算法對應(yīng)答器的布放有要求，限制了其應(yīng)用[11-12]。同時，在實際標(biāo)定的環(huán)境中，由于海洋環(huán)境并不安靜，受波浪、船只和波浪所形成的空氣氣泡及溫度、鹽度等復(fù)雜水文環(huán)境的影響，水聲數(shù)據(jù)的量測噪聲的特性是未知的，且存在不規(guī)律的野值，這些都會影響標(biāo)定的精度[13-14]。

為了提高標(biāo)定算法的適用性和實用性，本文對復(fù)雜水下環(huán)境下高精度SINS/USBL一體化系統(tǒng)誤差標(biāo)定技術(shù)展開了研究，旨在實現(xiàn)對一體化系統(tǒng)安裝誤差角和桿臂誤差的精確估計。

1 標(biāo)定誤差的定義

(1)

(2)

其中，Δx表示投影在載體坐標(biāo)系x坐標(biāo)方向的偏移量；Δy表示在y坐標(biāo)方向的偏移量；Δz表示在z坐標(biāo)方向的偏移量。

2 基于量測信息濾波估計的誤差標(biāo)定技術(shù)

進行安裝誤差標(biāo)定時，將應(yīng)答器投錨在水底，其位置可以由長基線的方式精確標(biāo)定。SINS/USBL一體化裝置通過剛性連接的壓力桿投放于水中，壓力桿和測量船之間同樣剛性連接在一起，同時，搭載高精度的RTK-GPS。安裝USBL系統(tǒng)的測量船圍繞著應(yīng)答器進行運動，在每個航跡位置，聲頭與應(yīng)答器相互應(yīng)答通信，記錄航跡上各個測量點的GPS提供的位置信息、SINS敏感的方位/姿態(tài)信息以及USBL系統(tǒng)測量的水聲信息等。聲學(xué)定位系統(tǒng)向應(yīng)答器發(fā)出詢問信號，應(yīng)答器檢測到該詢問信號后回復(fù)定位信號，標(biāo)定方案的設(shè)計如圖1所示。

圖1的標(biāo)定方案可以簡化為圖2的標(biāo)定模型。

圖1 標(biāo)定方案設(shè)計示意圖

圖2 標(biāo)定模型示意圖

(3)

(4)

(5)

結(jié)合式(3)，考慮標(biāo)定誤差后，應(yīng)答器在u系下的位置可表示為

(6)

選擇USBL原始輸出的斜距與方位角信息作為觀測量，則

(7)

(8)

(10)

根據(jù)式(10)，系統(tǒng)的狀態(tài)方程可表示為

(11)

其中，F(xiàn)usbl(t)=06×6。

3 基于Huber M估計的非線性魯棒自適應(yīng)標(biāo)定技術(shù)

在實際的水下復(fù)雜工作環(huán)境中，水下聲場是一種時變、多變和不穩(wěn)定的聲學(xué)信道環(huán)境。受水下復(fù)雜聲學(xué)環(huán)境的影響，水聲數(shù)據(jù)的量測噪聲表現(xiàn)為未知或時變的統(tǒng)計特性。同時，由于水下多徑的影響和基于寬帶擴頻的相干信號的相互干擾，估計的相關(guān)峰會產(chǎn)生錯亂，導(dǎo)致水聲定位系統(tǒng)的測量信息夾雜著沒有規(guī)律性的野值，將會影響標(biāo)定的精度，甚至導(dǎo)致無法完成標(biāo)定工作。因此，本文提出了面向水下SINS/USBL一體化標(biāo)定的新型魯棒自適應(yīng)標(biāo)定算法。在高斯近似濾波框架下，Kalman濾波包括時間更新和量測更新兩個階段。在量測更新階段，基于變分貝葉斯的自適應(yīng)濾波算法對未知的噪聲特性進行估計，采用M估計算法對聲學(xué)野值進行魯棒處理，并通過高斯牛頓迭代將M估計嵌入到變分貝葉斯框架中，使得濾波算法兼具魯棒性和自適應(yīng)性。

3.1 基于Huber M估計的非線性魯棒濾波

為了解決標(biāo)定過程中由于存在野值而導(dǎo)致濾波估計性能下降的問題，本文采用基于M估計的Kalman濾波算法。通過重構(gòu)經(jīng)典的Kalman系統(tǒng)和量測方程，使其具有歸一化的噪聲特性，引入Huber的代價核函數(shù)ρ，得到具有魯棒特性的M估計的Kalman濾波算法。本節(jié)的創(chuàng)新之處在于將M估計直接應(yīng)用于非線性的觀測模型中，避免了對其進行線性化近似，得到了一種真正意義上的非線性魯棒自適應(yīng)濾波，具體的研究思路如下[15]：

構(gòu)建非線性遞歸量測模型為

(12)

(13)

(14)

(15)

(16)

則非線性遞歸量測模型可以整合為

(17)

M估計的主要問題可轉(zhuǎn)化為對上述非線性遞歸模型的求解，而上述非線性遞歸方程又可以轉(zhuǎn)化為對如下Huber的代價函數(shù)的求解

(18)

其中，n為狀態(tài)維數(shù)，d為量測維數(shù)，這種代價函數(shù)結(jié)合了l1和l2范數(shù)的性質(zhì)，具有較好的魯棒性。定義殘差向量ζk=zk-g(xk)，ζi,k是向量ζk的第i個元素，p(·)是正定對稱函數(shù)且p(0)=0，可表示為

(19)

其中，γ是調(diào)節(jié)因子，一般取值為γ=1.345。純高斯分布條件下，該代價函數(shù)的估計效率是基于l2范數(shù)估計的95%。

定義φ(ζi,k)=p′(ζi,k)，令式(18)的代價函數(shù)取最小值時，則

(20)

定義ψ(ζi,k)=φ(ζi,k)/ζi,k，則有

(21)

一種解決方案是構(gòu)造偽觀測噪聲協(xié)方差，定義權(quán)重矩陣為Ψ=diag[ψ(ζi,k)]，用來修正量測噪聲協(xié)方差，即

(22)

=Λk|k(d+1:d+n,d+1:d+n)

=Pk|k-1

(23)

(24)

另外一種解決方案是構(gòu)造偽觀測值，當(dāng)|ζi,k|≥γ時，采用γ/|ζi,k|替代ζi,k，定義修正后的殘差為

(25)

(26)

(27)

=(Λk|k)1/2(1:d,1:d)[g(xk)(1:d)]+

Ψ(1:d,1:d)ζk(1:d)

(28)

上述兩種處理方法的效果是相同的，可以采用上述任何一種方法重新構(gòu)造信息,新方法無需對觀測方程進行線性化近似，保證了非線性濾波的精度。

3.2 基于Huber M估計的魯棒自適應(yīng)濾波

針對SINS/USBL標(biāo)定的誤差模型和濾波方程，本文采用非線性濾波估計的方法，旨在實現(xiàn)對安裝誤差角和桿臂誤差的準(zhǔn)確估計?？紤]到外部量測信息噪聲特性未知且夾雜著大量野值的情況，本文提出了基于Huber M估計的非線性變分貝葉斯濾波算法，并將其應(yīng)用于SINS/USBL的誤差標(biāo)定中。在變分貝葉斯的自適應(yīng)濾波的基礎(chǔ)上，高斯牛頓迭代將M估計嵌入到變分貝葉斯的濾波框架中。同時，M估計可直接應(yīng)用于非線性的觀測模型，避免了對其進行線性化近似，得到了一種真正意義上的非線性魯棒自適應(yīng)濾波，提高了濾波的精度、魯棒性和自適應(yīng)性。

4 仿真驗證

根據(jù)設(shè)計的標(biāo)定方案，利用仿真數(shù)據(jù)驗證算法的有效性。在仿真實驗中，首先設(shè)置搭載船航行的軌跡和狀態(tài)，生成慣性設(shè)備實時的姿態(tài)信息和搭載船的位置信息，而應(yīng)答器位置已知，根據(jù)船和應(yīng)答器的位置可以推算出水聲設(shè)備輸出的斜距和方位角的真值，根據(jù)表2所示的傳感器參數(shù)模擬復(fù)雜水下聲場環(huán)境下的水聲數(shù)據(jù)。仿真實驗中涉及的主要傳感器有高精度全球衛(wèi)星定位系統(tǒng)(RTK GPS)、USBL定位系統(tǒng)及其IMU，各傳感器誤差設(shè)置如表3所示。

標(biāo)定的軌跡如圖3所示，搭載船的起始位置設(shè)置為(118.7718°E, 32.1459°N, 32m)，應(yīng)答器相對船起始點的相對坐標(biāo)為[-100,200,-100]m。不失一般性，SINS/USBL一體化系統(tǒng)的標(biāo)定誤差設(shè)置如表4所示。

表1 基于Huber M估計的非線性魯棒自適應(yīng)偽代碼

表2 USBL系統(tǒng)仿真參數(shù)

表3 慣性設(shè)備仿真參數(shù)

圖3 標(biāo)定軌跡

表4 一體化系統(tǒng)的標(biāo)定誤差設(shè)置

在實際復(fù)雜的水文環(huán)境下，USBL輸出水聲信息的噪聲特性未知且存在野值，為了模擬這種情況，根據(jù)如下方式產(chǎn)生野值干擾的量測噪聲

(29)

(30)

其中，vr和vdir分別表示斜距和方位角噪聲;w.p.表示以概率，也就是說方差為原來100倍的量測噪聲以10%概率出現(xiàn)在原來的高斯分布中，用以模擬野值的干擾。為了分析USBL有無野值的區(qū)別，圖 4給出了其有無野值時的概率密度對比曲線。

圖4 概率密度對比

由圖4可知，由于量測野值的干擾，量測噪聲均具有厚尾分布，從而使高斯分布不再匹配系統(tǒng)的量測噪聲分布，表現(xiàn)為“矮胖”，拖著長長的“尾巴”，這是因為野值也以一定的概率充當(dāng)了“尾巴”。由于野值的存在，基于遞歸量測更新的高斯近似濾波算法對非線性標(biāo)定模型的精度提升有限。因此，在本節(jié)中高斯近似濾波算法采用容積Kalman濾波(Cubature Kalman Filter, CKF)即可。將推導(dǎo)的基于Huber M估計的非線性魯棒標(biāo)定技術(shù)簡稱為C-HCKF，將推導(dǎo)的基于Huber M估計的魯棒自適應(yīng)標(biāo)定技術(shù)簡稱為C-HVBCKF。SINS/USBL一體化系統(tǒng)安裝誤差角的標(biāo)定結(jié)果如圖5～圖7所示，安裝誤差角各標(biāo)定方法的統(tǒng)計結(jié)果(最后100s)如圖8所示。

針對USBL輸出的水聲數(shù)據(jù)出現(xiàn)的噪聲特性未知且夾雜著野值的情況，由圖5～圖7的標(biāo)定結(jié)果可以看出，本文提出的基于Huber M估計的非線性變分貝葉斯濾波方法C-HVBCKF實現(xiàn)了對安裝誤差角的有效估計，這是因為Huber M估計結(jié)合了l1/l2兩種范數(shù)構(gòu)建代價函數(shù)，在變分貝葉斯的濾波框架內(nèi)，通過加權(quán)矩陣對觀測野值進行限制，而變分貝葉斯又可以實現(xiàn)對時變量測噪聲的估計。結(jié)合圖8定量的統(tǒng)計結(jié)果可知，提出的C-HVBCKF算法均方根誤差(Root Mean Squared Error, RMSE)的估計精度可穩(wěn)定在0.01°數(shù)量級。安裝桿臂的估計結(jié)果如圖9～圖11所示，安裝桿臂各標(biāo)定方法的統(tǒng)計結(jié)果(最后100s)如圖12所示。

圖5 縱向安裝誤差角

圖6 橫向安裝誤差角

圖7 航向安裝誤差角

圖8 安裝誤差角標(biāo)定方法對比

圖9 橫向桿臂估計

圖10 縱向桿臂估計

圖11 天向桿臂估計

由圖9～圖11可知，本文提出的基于Huber M估計的非線性變分貝葉斯濾波方法C-HVBCKF同樣實現(xiàn)了對安裝桿臂的有效估計，分析其原因同安裝誤差角估計類似。由安裝桿臂估計的對比曲線也可以看出，基于一般CKF標(biāo)定算法無法完成對安裝桿臂的估計，因為其并未對未知的量測噪聲進行估計，也未對聲學(xué)野值進行處理，導(dǎo)致濾波估計精度下降甚至發(fā)散。結(jié)合圖12定量的統(tǒng)計結(jié)果可知，本文提出算法的RMSE估計精度相對其他算法有大幅提升，具有更好的濾波性能，表現(xiàn)為高精度的狀態(tài)估計和快速的收斂速度，安裝桿臂誤差(RMSE)在0.1m以內(nèi)。

5 結(jié)論

針對SINS/USBL一體化組合樣機系統(tǒng)需要提前進行精確標(biāo)定的問題，提出了基于量測信息濾波估計的誤差標(biāo)定方法，精確校正后無需重復(fù)標(biāo)定。

與其他的標(biāo)定方法相比，該算法表現(xiàn)出更好的濾波性能，具體表現(xiàn)為更快的收斂速度、更高的估計精度和更平滑的濾波曲線，證明了提出算法的有效性和優(yōu)越性。雖然基于變分貝葉斯的自適應(yīng)標(biāo)定算法C-VBCKF和基于M估計的魯棒標(biāo)定算法C-HCKF也表現(xiàn)出不錯的濾波性能，但是，由于兩個算法均無法同時兼具對時變噪聲的估計和野值的抑制，因此，在一定程度上影響了濾波中狀態(tài)估計的精度，而普通的CKF標(biāo)定算法幾乎無法完成標(biāo)定任務(wù)。