張亮，張濤*

（1.東南大學(xué)儀器科學(xué)與工程學(xué)院，南京 210096；2.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096）

1 引 言

自主水下航行器（Autonomous Underwater Vehicle，AUV）可以用于水下地形測量、目標(biāo)探測和水下資源開采等用途［1］，當(dāng)AUV配備用于探測、監(jiān)視、環(huán)境傳感的成套聲學(xué)和非聲學(xué)傳感器以及水下高速火箭、防御武器等時(shí)，也可用于潛艇戰(zhàn)、水雷戰(zhàn)、偵察、監(jiān)視和水下攻擊等各方面。因此，水下高精度導(dǎo)航和定位技術(shù)是解決水下作業(yè)的前提和關(guān)鍵。

慣性導(dǎo)航是一種自主的導(dǎo)航方式，它不受環(huán)境的影響，可以全天候提供姿態(tài)、速度、位置的全方位信息，擁有其他導(dǎo)航傳感器不可比擬的優(yōu)點(diǎn)，然而其定位誤差會隨時(shí)間發(fā)散［2］，因此，探索如何實(shí)現(xiàn)對慣性導(dǎo)航誤差的校正一直是導(dǎo)航領(lǐng)域研究的熱點(diǎn)問題。地磁匹配導(dǎo)航［3］、重力匹配導(dǎo)航［4］等，都是水下導(dǎo)航的常見方式，考慮到在水下環(huán)境中，聲波的傳輸距離遠(yuǎn)，信號衰減損失小，因而水聲導(dǎo)航被廣泛應(yīng)用于水下航行器的導(dǎo)航與定位中［5］。聲學(xué)導(dǎo)航按照基線的長短可以分為長基線（Long Baseline，LBL）、短基線（Short Baseline，SBL）以及超短基線（Ultrashort Baseline，USBL）。通常LBL需要在水下布放至少三個(gè)以上的信標(biāo)才能定位［6］，而USBL由于其無需事先布放基陣，具有更高的便攜性以及獨(dú)立性，使用一個(gè)信標(biāo)就可以完成水下定位，因而在水下航行器中廣泛應(yīng)用［7］。

這種單信標(biāo)的方式，只需在海底布放一個(gè)聲源，對傳統(tǒng)多應(yīng)答器方式進(jìn)行了簡化，提高了系統(tǒng)便捷性和作業(yè)效率。文獻(xiàn)［8］最先制定了單信標(biāo)定位方案，提出了利用測量的斜距以及自身的速度信息共同完成對AUV位置的計(jì)算，不僅適用于信標(biāo)安裝于海底的情況，同時(shí)適用于拖曳信標(biāo)的模式。文獻(xiàn)［9］對單信標(biāo)下的AUV定位問題進(jìn)行了可觀性分析，指出距離與偏航角的觀測能夠使得系統(tǒng)可觀測。文獻(xiàn)［10］提出一種線性迭代求解器以解決直線航跡下的單信標(biāo)定位問題。文獻(xiàn)［11］在單信標(biāo)的基礎(chǔ)上增加了徑向速度觀測量以改善單信標(biāo)導(dǎo)航算法收斂速度慢、非全局可觀測等問題。文獻(xiàn)［12］在單信標(biāo)定位模型中斜距的基礎(chǔ)上又增加了方位角觀測，可進(jìn)一步提高系統(tǒng)的可觀性?？v觀當(dāng)前的單信標(biāo)定位方法，均是基于斜距測量定位技術(shù)，其中時(shí)鐘必須嚴(yán)格同步是單信標(biāo)定位方式必須面對的難題。同時(shí)，基于斜距測量的定位技術(shù)，需要采用AUV與信標(biāo)之間的收發(fā)模式，即主動(dòng)導(dǎo)航方式，導(dǎo)致AUV容易暴露自身位置。區(qū)別于現(xiàn)有的單信標(biāo)定位方法，文獻(xiàn)［13］提出一種基于到達(dá)時(shí)間差（Time-Difference-of-Arrival，TDOA）的單聲源定位方法，該聲源只需對外發(fā)射信號即可，不需接收來自AUV的水聲信號，因此AUV可以在不對外發(fā)射信號的情況下實(shí)現(xiàn)完全自主定位，然而其定位解無法在大噪聲條件下滿足克拉美羅（Cramer-Rao Lower Bound，CRLB）下界［14］。

因此，TDOA或TDOA/AOA（到達(dá)角度，Angleof-Arrival）定位解的精度是單聲源定位的關(guān)鍵之一。早在1994年就提出了一種高效的兩步最小二乘法對TDOA定位問題進(jìn)行求解［15］。隨著AOA技術(shù)的引入，聯(lián)合TDOA/AOA的定位技術(shù)可以提高系統(tǒng)定位精度［16-17］。然而傳統(tǒng)的AOA定位模型以方位角、高度角為基礎(chǔ)［18］，針對非高度角的AOA定位模型，文獻(xiàn)［14］推導(dǎo)了聯(lián)合TDOA/AOA定位的閉式解，并以水下傳感器進(jìn)行實(shí)驗(yàn)驗(yàn)證，對TDOA/AOA方法應(yīng)用于水下導(dǎo)航提出了設(shè)想與展望。

為了解決文獻(xiàn)［13］中TDOA/AOA定位解無法滿足CRLB性能從而影響單聲源定位精度的問題，本文提出一種基于TDOA/AOA的慣性/單聲源組合導(dǎo)航方法，給出滿足CRLB性能的TDOA/AOA單聲源定位解。通過單聲源計(jì)算的位置完成對慣性導(dǎo)航誤差的校正，實(shí)現(xiàn)AUV無需上浮水面即可完成對自身位置的校正，同時(shí)兼具定位安全性高、隱蔽性好的優(yōu)勢。

2 單聲源工作原理

單聲源定位的基本原理，即水下聲源按照固定的時(shí)間間隔周期性地發(fā)射信號，航行器圍繞聲源運(yùn)動(dòng)構(gòu)成虛擬長基線基陣，根據(jù)時(shí)間到達(dá)差解算出聲源相對于虛擬基陣的坐標(biāo)，再根據(jù)已知聲源的絕對位置逆向推算航行器位置。定位原理示意圖如圖1所示。

圖1 單聲源定位原理示意圖Fig.1 Schematic diagram of single source location principle

圖1中，航行器沿著真實(shí)的軌跡（即黑色虛線）運(yùn)動(dòng)，并在tk1，tk1，…，tkn時(shí)刻分別接收到來自水下聲源的信號，設(shè)置一個(gè)基準(zhǔn)位置，例如tkn時(shí)刻的位置，根據(jù)每個(gè)點(diǎn)對應(yīng)的時(shí)刻tki及信號發(fā)射周期T可以計(jì)算出信號從聲源到達(dá)這些位置和基準(zhǔn)位置的時(shí)間差，建立以基準(zhǔn)位置為坐標(biāo)中心的相對坐標(biāo)系，根據(jù)各點(diǎn)之間的慣性導(dǎo)航結(jié)果可以得到所有航行器位置在相對坐標(biāo)系下的位置關(guān)系。于是tk1，tk1，…，tkn之間的航行器位置就構(gòu)成了一個(gè)虛擬的長基線基陣。慣性導(dǎo)航系統(tǒng)由于其定位累積誤差的存在，慣導(dǎo)實(shí)際解算的位置如圖1中藍(lán)色實(shí)線所示，偏離了真實(shí)軌跡。我們可以根據(jù)時(shí)間到達(dá)差計(jì)算出聲源在慣性軌跡基陣下的相對位置，盡管慣導(dǎo)位置逐漸偏離了真實(shí)的位置，但是可以根據(jù)計(jì)算得到的較為準(zhǔn)確的航行器與聲源之間的相對位置關(guān)系，以真實(shí)的聲源位置為基準(zhǔn)，根據(jù)相對位置關(guān)系將慣性軌跡向真實(shí)軌跡“靠攏”，即實(shí)現(xiàn)了慣導(dǎo)位置誤差的修正，修正后的慣導(dǎo)位置如圖1中紅色實(shí)線所示。

從以上單聲源導(dǎo)航的原理中可知，慣性導(dǎo)航與單聲源之間是互相需要的，慣導(dǎo)可以提供位置與姿態(tài)信息，合成單聲源定位中所需要的虛擬基陣，而單聲源系統(tǒng)可以通過TDOA的觀測值校正慣導(dǎo)誤差，它們互為補(bǔ)充，共同完成被動(dòng)式導(dǎo)航定位的功能。

整個(gè)過程中，航行器不需要向外發(fā)送詢問信號，因此，這是一種低成本、高效、被動(dòng)的定位方式。同時(shí)，基于時(shí)間到達(dá)差的定位方法無需傳感器之間的信號同步，可適用于多用戶自主定位校正。水下航行器在聲源作用范圍內(nèi)只要接收到廣播信號，在已知聲源位置以及信號廣播周期的基礎(chǔ)上即可自主完成位置校正。

從以上分析過程可以看出，單聲源定位需要航行器繞著聲源做規(guī)律性的繞行運(yùn)動(dòng)，并且在文獻(xiàn)［13］中也證明了這種運(yùn)動(dòng)軌跡有利于TDOA定位解的計(jì)算。因此，單聲源定位適用于對水下大范圍、長航時(shí)下的慣性導(dǎo)航誤差進(jìn)行校正，尤其是僅依靠慣性導(dǎo)航情況，對定位的隱蔽性有特殊要求時(shí)，可以在聲源作用范圍內(nèi)，依靠周期運(yùn)動(dòng)以及接收信號的到達(dá)時(shí)間差，實(shí)現(xiàn)AUV無需上浮水面即可完成對自身位置的校正，從而繼續(xù)完成水下AUV的導(dǎo)航任務(wù)。這種隱蔽性的要求是針對水下AUV而言的，即在不暴露自身位置情況下完成定位，而水下聲源由于需要發(fā)送信號，對其隱蔽性不作要求。

3 基于TDOA/AOA的聲源位置定位模型

假設(shè)聲源在ts1時(shí)刻發(fā)射了一段信號，在tk1時(shí)刻被水下航行器接收到，在已知聲速c的情況下，那么聲源與航行器接收到信號時(shí)刻之間的距離為

如圖2所示，聲源按照固定周期T向外發(fā)射信號，那么第M-1 個(gè)周期后發(fā)射第M個(gè)信號的時(shí)刻為ts1+(M-1)T，此時(shí)航行器收到來自聲源的第M個(gè)信號時(shí)刻為tkM。

圖2 聲源信號發(fā)射及接收示意圖Fig.2 Schematic diagram of sound source signal transmission and reception

因此，tkM時(shí)刻聲源與航行器之間的距離為

航行器在行駛過程中記錄下M個(gè)時(shí)間區(qū)間內(nèi)的位置，以tkM時(shí)刻位置為參考基元，可以獲得區(qū)間內(nèi)每個(gè)時(shí)刻相對于參考基元的斜距差，即

在獲得M個(gè)時(shí)間區(qū)間內(nèi)的各基元位置基礎(chǔ)上，水下聲源定位問題可以描述為如圖3所示。

圖3 水下聲源定位示意圖Fig.3 Schematic diagram of underwater source localization

式（3）中，uT0表示聲源的位置，si表示第i個(gè)周期后的船的坐標(biāo)。si處接收到的方位角模型如圖3所示，可以表示為

式中，α0i和β0i表示方位角的真值，εi和δi分別表示方位角的測量噪聲，服從高斯分布。

根據(jù)文獻(xiàn)［14］對TDOA定位模型的分析可知，在獲得一組斜距差觀測值的基礎(chǔ)上，可以得到如下定位方程

式中，n表示TDOA的量測噪聲。

在單聲源定位中，如果信號接收裝置具備AOA測量的能力，可以將AOA作為輔助手段與TDOA共同完成定位。聯(lián)合TDOA/AOA的定位模型為［14］

以上TDOA/AOA定位解模型可以在一定噪聲條件下滿足CRLB性能，具體證明過程見文獻(xiàn)［14］。

當(dāng)系統(tǒng)配備深度計(jì)時(shí)，可以結(jié)合深度計(jì)的觀測值完成對聲源的定位，由深度計(jì)觀測值構(gòu)成的定位方程為

式中，h3=ht-hs，G3=[0 0 1 0]，B3=-1，n3為深度計(jì)噪聲，ht為應(yīng)答器真實(shí)的深度，hs為由深度計(jì)測得航行器的深度值。

在式（7）的基礎(chǔ)上，將式（8）進(jìn)行擴(kuò)維，即可得到聯(lián)合TDOA/AOA/深度計(jì)的定位方程。

在聯(lián)合TDOA/AOA定位的情況下，式（7）可根據(jù)最小二乘準(zhǔn)則進(jìn)行求解

式中，加權(quán)矩陣W=(BQBT)-1，Q=E(ηηT)。

以上模型實(shí)現(xiàn)了M個(gè)信號周期內(nèi)對聲源位置的定位。將每一個(gè)信號接收時(shí)刻的位置作為一個(gè)基元，基元數(shù)越多，由基元構(gòu)成的基線長度越長，因此定位精度相對較高。但是，由于位置的累積誤差，基線長度越長，每個(gè)基元的位置誤差則相應(yīng)越大。因此，需要合理選擇合適的基元數(shù)目。對基元數(shù)量設(shè)定上限，即設(shè)置一個(gè)窗口，采用滑動(dòng)窗的形式，如圖4所示。

圖4 滑動(dòng)窗示意圖Fig.4 Diagram of sliding window

設(shè)置滑動(dòng)窗窗口大小為M，當(dāng)出現(xiàn)新的基元并且窗口內(nèi)基元數(shù)量超過上限時(shí)，窗口滑動(dòng)，剔除舊的基元，加入新基元。一個(gè)窗口中的所有基元合成一個(gè)基陣，窗口的不斷滑動(dòng)使得基陣也在不停更新，每個(gè)窗口內(nèi)所有M個(gè)基元即可按照式（7）得到相應(yīng)的聲源位置。

4 基于位置匹配的組合導(dǎo)航方法

第3節(jié)根據(jù)建立的虛擬合成基陣，給出了TDOA/AOA定位模型，通過TDOA/AOA算法，可以得到聲源在慣導(dǎo)軌跡下的相對位置u0，聲源真實(shí)位置已知。因此，根據(jù)相對位置u0以及已知的絕對位置pnt可以得到修正后的航行器位置為他們之間有如下關(guān)系，

式中，RM和RN分別為地球子午圈和卯酉圈曲率半徑。

根據(jù)得到的修正的航行器位置，可以采用較為簡單的位置匹配組合模式對慣性導(dǎo)航誤差進(jìn)行校正，單聲源/慣性組合導(dǎo)航原理示意圖如圖5所示。在圖5中，選取系統(tǒng)狀態(tài)量如下：

圖5 單聲源/慣性組合導(dǎo)航原理示意圖Fig.5 Schematic diagram of single Source/INS integrated navigation system

式中，[?E?N?U]T表示姿態(tài)的失準(zhǔn)角誤差，[δVEδVNδVU]T表示速度誤差，[δL δλ δh]T表示位置誤差，[εxεyεz]T表示陀螺儀的零偏，[?x?y?z]T表示加速度計(jì)的零偏。

以傳統(tǒng)捷聯(lián)慣性導(dǎo)航的誤差微分方程建立濾波器的狀態(tài)方程為

式中，F(xiàn)(t)為狀態(tài)轉(zhuǎn)移矩陣，W(t)為系統(tǒng)噪聲。F(t)矩陣的具體表達(dá)式見文獻(xiàn)［19］。

根據(jù)式（10）得到修正的AUV位置作為觀測值，則松組合系統(tǒng)的觀測方程為

式中，Z(t)表示觀測量，表示慣導(dǎo)解算得到的導(dǎo)航系下的位置，pv表示經(jīng)單聲源修正后的位置信息，V(t)表示量測誤差矩陣。H表示觀測矩陣為

對組合導(dǎo)航系統(tǒng)模型的狀態(tài)方程式（12）和量測方程式（13）進(jìn)行離散化，定義第k時(shí)刻的狀態(tài)變量xnk=X(tk)，過程噪聲wnk=W(tk)，量測矢量znk=Z(tk)，量測噪聲vnk=V(tk)，則松組合系統(tǒng)模型可以簡化為如下的離散形式

式中，狀態(tài)轉(zhuǎn)移矩陣=I3×3+Fn?ts，?ts表示離散間隔。

基于式（15）的狀態(tài)和量測方程，采用卡爾曼濾波技術(shù)，即可實(shí)現(xiàn)對慣性導(dǎo)航的誤差校正，完成水下AUV的長航時(shí)、遠(yuǎn)航程被動(dòng)導(dǎo)航定位。

5 仿真結(jié)果及分析

針對本文所提出的位置匹配組合導(dǎo)航算法，采用仿真實(shí)驗(yàn)驗(yàn)證所設(shè)計(jì)方法的有效性。仿真實(shí)驗(yàn)中的傳感器誤差參數(shù)設(shè)置如下。

對聲源信號的測向誤差為0.2°，測時(shí)誤差：1 ms；

設(shè)定水下聲速1500 m/s，聲源廣播周期2 s。聲源位置相對水下航行器起始點(diǎn)的坐標(biāo)為[-1550500-20]Tm。仿真實(shí)驗(yàn)中水下航行器的運(yùn)動(dòng)軌跡設(shè)置如圖6所示。

圖6 航行器圍繞聲源運(yùn)動(dòng)軌跡示意圖Fig.6 Diagram of the trajectory of the vehicle around the sound source

航行器出發(fā)后，先是直線運(yùn)動(dòng)，接著繞圈，讓慣性導(dǎo)航累計(jì)足夠多的誤差。大約10 min后，慣性導(dǎo)航誤差達(dá)到2 km左右時(shí)到達(dá)單聲源作用區(qū)域，如圖6所示。在單聲源作用區(qū)域，航行器開始繞著聲源繞圈，運(yùn)行速度10 m/s，繞圈半徑為200 m，繞圈過程中進(jìn)行單聲源導(dǎo)航定位。根據(jù)文獻(xiàn)［13］中已有的分析，不同窗口數(shù)對定位精度有著不同的影響，本文不考慮窗口數(shù)目對定位的影響，僅比較不同方式下的組合導(dǎo)航精度。因此基元觸發(fā)數(shù)目以及窗口數(shù)都設(shè)置為定值?；|發(fā)數(shù)目為6，窗口為10，持續(xù)時(shí)間約15 min。在此相同的條件下比較TDOA以及TDOA/AOA方式下的導(dǎo)航定位精度。

在單聲源作用下，組合導(dǎo)航運(yùn)動(dòng)軌跡與純慣性軌跡對比如圖7所示。其中，組合導(dǎo)航的量測只考慮TDOA模型的情況。

圖7 TDOA方法下的運(yùn)動(dòng)軌跡對比Fig.7 Comparison of motion trajectories under the TDOA method

從圖中可以看出，慣性導(dǎo)航的誤差發(fā)散較快，接近10 km，而在聲源的作用下，航行器的位置誤差可以最終被校正到一定誤差范圍內(nèi)。為了定量分析組合導(dǎo)航的定位效果，給出在單聲源作用下，從700～1900 s之間的位置誤差以及姿態(tài)誤差曲線如圖8、圖9所示。

圖8 TDOA方式下組合導(dǎo)航位置誤差Fig.8 Position error of the integrated navigation system under the TDOA method

圖9 TDOA方式下組合導(dǎo)航姿態(tài)誤差Fig.9 Attitude error of the integrated navigation system under the TDOA method

從圖8中可以看出，在深度計(jì)的作用下，高度方向可以保持較高的精度。水平誤差方面，東向與北向的位置誤差最終可以收斂到50 m以內(nèi)。從圖9中可以看出，水平姿態(tài)誤差可以收斂到0.02°以內(nèi)，航向誤差最終收斂到0.2°以內(nèi)。因此，基于TDOA的定位方法，可以對長時(shí)間形成的慣性導(dǎo)航誤差進(jìn)行修正。

此外，考慮到結(jié)合AOA具有提高定位精度的功能，在圖6的軌跡且相同實(shí)驗(yàn)條件下，增加AOA觀測，組合導(dǎo)航運(yùn)動(dòng)軌跡與純慣性軌跡對比如圖10所示。其中，組合導(dǎo)航的量測為聯(lián)合TDOA/AOA模型的情況。

圖10 TDOA/AOA方法下的運(yùn)動(dòng)軌跡對比Fig.10 Comparison of motion trajectories under the TDOA/AOA method

從圖10中可以看出，慣性導(dǎo)航的誤差發(fā)散較快，接近10 km，而在聲源的作用下，航行器的位置誤差可以最終被校正到一定誤差范圍內(nèi)。為了定量分析組合導(dǎo)航的定位效果，給出在單聲源作用下，從700～1900 s之間的位置誤差以及姿態(tài)誤差曲線如圖11、圖12所示。

圖11 TDOA/AOA方法下組合導(dǎo)航位置誤差Fig.11 Position error of the integrated navigation system under the TDOA/AOA method

圖12 TDOA/AOA方法下組合導(dǎo)航姿態(tài)誤差Fig.12 Attitude error of the integrated navigation system under the TDOA/AOA method

從圖11中可以看出，在深度計(jì)的作用下，高度方向可以保持較高的精度。水平誤差方面，東向與北向的位置誤差最終可以收斂到10 m以內(nèi)。從圖12中可以看出，水平姿態(tài)誤差可以收斂到0.02°以內(nèi)，航向誤差最終收斂到0.1°以內(nèi)。

對軌跡最后200 s數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分析，采用RMSE以及水平誤差作為性能評價(jià)指標(biāo)，對不同校正方式下的精度進(jìn)行對比，結(jié)果如表1所示。

表1 位置均方根誤差統(tǒng)計(jì)Table 1 RMSE of position error

其中，水平誤差（Average horizontal error，AHE）的計(jì)算公式為

從表1中可以看出，在1700 s以后，TDOA/AOA的方式，可以將水平位置誤差修正到10 m以內(nèi)。TDOA的方式，可以將水平誤差修正到40 m左右。在增加AOA觀測后，可以提高單聲源的導(dǎo)航精度。兩種方式都可以完成對航行器位置誤差的修正。

以上實(shí)驗(yàn)驗(yàn)證了，只要AUV可以獲得聲源發(fā)送的信號到達(dá)時(shí)間差，并結(jié)合慣性導(dǎo)航的位置信息，即可完成水下AUV的導(dǎo)航定位功能，具有可行性的理論基礎(chǔ)。因此，單聲源導(dǎo)航定位方式可以為水下航行器在聲源作用范圍內(nèi)提供位置誤差校正的功能，并且其具有隱蔽性好、無需信號同步以及與外界信息交互的優(yōu)勢。

6 結(jié) 論

本文針對傳統(tǒng)單信標(biāo)導(dǎo)航模式下時(shí)鐘必須嚴(yán)格同步以及基于斜距測量的定位技術(shù)需要AUV與信標(biāo)進(jìn)行信息交互從而導(dǎo)致AUV容易暴露自身位置的問題，提出一種基于單聲源輔助慣性的被動(dòng)導(dǎo)航技術(shù)，推導(dǎo)了方位角下基于TDOA/AOA的定位模型，設(shè)計(jì)了基于位置匹配的慣性/單聲源組合導(dǎo)航方法，以TDOA/AOA計(jì)算得到的AUV位置修正慣性導(dǎo)航的位置誤差。仿真實(shí)驗(yàn)結(jié)果表明，本文的方法可以將慣性導(dǎo)航的誤差修正到10 m以內(nèi)，證實(shí)了單聲源導(dǎo)航定位方式可以為水下航行器在聲源作用范圍內(nèi)提供位置誤差校正的功能，并且具備不需要時(shí)鐘同步、不與外界進(jìn)行信息交互的優(yōu)勢，可以為水下無人平臺提供長航時(shí)、遠(yuǎn)距離的導(dǎo)航定位功能。

本文提出的TDOA/AOA閉式解模型可以用于單聲源的組合導(dǎo)航定位，但是本文是在常聲速模型下進(jìn)行的理論分析，水下環(huán)境的復(fù)雜性、聲場的不均勻分布等，均會影響TDOA的測量精度。此外要實(shí)現(xiàn)單聲源定位的普及應(yīng)用，其功耗問題也是將來面臨的瓶頸問題之一。未來將進(jìn)一步考慮聲速變化以及大范圍場景下TDOA測量的時(shí)延誤差等情況下的單聲源定位問題，以及解決長時(shí)間水下聲源工作的能耗問題，既降低成本，也實(shí)現(xiàn)對水下AUV長時(shí)間導(dǎo)航定位的支持。