張 濤，王 健，張佳宇

(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ù)進(jìn)一步發(fā)展，USBL定位系統(tǒng)及裝備朝著慣性/水聲一體化的方向發(fā)展。SINS/USBL一體化系統(tǒng)中，載體坐標(biāo)系和聲學(xué)坐標(biāo)系在出廠前不可避免地存在安裝誤差角和桿臂誤差，并且均為固定的常值。因此，在導(dǎo)航與定位前需要提前進(jìn)行標(biāo)定并補(bǔ)償，精確標(biāo)定后無(wú)需重復(fù)標(biāo)定，這也是一體化系統(tǒng)的突出優(yōu)勢(shì)。影響高精度USBL定位系統(tǒng)定位精度的主要因素有系統(tǒng)自身誤差、海洋環(huán)境參數(shù)測(cè)量誤差以及SINS/USBL之間的安裝誤差。其中，聲學(xué)基陣安裝誤差是定位系統(tǒng)的主要誤差源, 從數(shù)值角度粗略分析:1°的航向安裝誤差角將會(huì)引入約1.7%斜距的水平位置誤差，因此，在實(shí)際使用前需要進(jìn)行安裝誤差的標(biāo)定[1]。

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

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

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

(1)

(2)

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

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

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

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

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

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

(3)

(4)

(5)

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

(6)

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

(7)

(8)

(10)

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

(11)

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

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

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

3.1 基于Huber M估計(jì)的非線性魯棒濾波

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

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

(12)

(13)

(14)

(15)

(16)

則非線性遞歸量測(cè)模型可以整合為

(17)

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

(18)

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

(19)

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

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

(20)

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

(21)

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

(22)

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

=Pk|k-1

(23)

(24)

另外一種解決方案是構(gòu)造偽觀測(cè)值，當(dāng)|ζi,k|≥γ時(shí)，采用γ/|ζ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)造信息,新方法無(wú)需對(duì)觀測(cè)方程進(jìn)行線性化近似，保證了非線性濾波的精度。

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

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

4 仿真驗(yàn)證

根據(jù)設(shè)計(jì)的標(biāo)定方案，利用仿真數(shù)據(jù)驗(yàn)證算法的有效性。在仿真實(shí)驗(yàn)中，首先設(shè)置搭載船航行的軌跡和狀態(tài)，生成慣性設(shè)備實(shí)時(shí)的姿態(tài)信息和搭載船的位置信息，而應(yīng)答器位置已知，根據(jù)船和應(yīng)答器的位置可以推算出水聲設(shè)備輸出的斜距和方位角的真值，根據(jù)表2所示的傳感器參數(shù)模擬復(fù)雜水下聲場(chǎng)環(huán)境下的水聲數(shù)據(jù)。仿真實(shí)驗(yàn)中涉及的主要傳感器有高精度全球衛(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)答器相對(duì)船起始點(diǎn)的相對(duì)坐標(biāo)為[-100,200,-100]m。不失一般性，SINS/USBL一體化系統(tǒng)的標(biāo)定誤差設(shè)置如表4所示。

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

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

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

圖3 標(biāo)定軌跡

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

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

(29)

(30)

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

圖4 概率密度對(duì)比

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

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

圖5 縱向安裝誤差角

圖6 橫向安裝誤差角

圖7 航向安裝誤差角

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

圖9 橫向桿臂估計(jì)

圖10 縱向桿臂估計(jì)

圖11 天向桿臂估計(jì)

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

5 結(jié)論

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

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