復雜水下環境下高精度SINS/USBL誤差標定技術

張 濤，王 健，張佳宇

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

0 引言

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

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

為了提高標定算法的適用性和實用性，本文對復雜水下環境下高精度SINS/USBL一體化系統誤差標定技術展開了研究，旨在實現對一體化系統安裝誤差角和桿臂誤差的精確估計。

1 標定誤差的定義

(1)

(2)

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

2 基于量測信息濾波估計的誤差標定技術

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

圖1的標定方案可以簡化為圖2的標定模型。

圖1 標定方案設計示意圖

圖2 標定模型示意圖

(3)

(4)

(5)

結合式(3)，考慮標定誤差后，應答器在u系下的位置可表示為

(6)

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

(7)

(8)

(10)

根據式(10)，系統的狀態方程可表示為

(11)

其中，Fusbl(t)=06×6。

3 基于Huber M估計的非線性魯棒自適應標定技術

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

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

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

構建非線性遞歸量測模型為

(12)

(13)

(14)

(15)

(16)

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

(17)

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

(18)

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

(19)

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

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

(20)

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

(21)

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

(22)

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

=Pk|k-1

(23)

(24)

另外一種解決方案是構造偽觀測值，當|ζ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)

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

3.2 基于Huber M估計的魯棒自適應濾波

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

4 仿真驗證

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

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

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

表2 USBL系統仿真參數

表3 慣性設備仿真參數

圖3 標定軌跡

表4 一體化系統的標定誤差設置

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

(29)

(30)

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

圖4 概率密度對比

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

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

圖5 縱向安裝誤差角

圖6 橫向安裝誤差角

圖7 航向安裝誤差角

圖8 安裝誤差角標定方法對比

圖9 橫向桿臂估計

圖10 縱向桿臂估計

圖11 天向桿臂估計

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

5 結論

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

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