張淏酥，王 盛，王 濤，苗建明，龔 喜

（1.中山大學(xué)海洋工程與技術(shù)學(xué)院，廣州 510275；2.南方海洋科學(xué)與工程廣東省實(shí)驗(yàn)室(珠海)，珠海 519000；3.華中光電技術(shù)研究所?武漢光電國(guó)家研究中心，武漢 430223；4.中山大學(xué)智能工程學(xué)院，廣州 510006；5.廣東省消防科學(xué)與智能應(yīng)急技術(shù)重點(diǎn)實(shí)驗(yàn)室，廣州 510006）

1 引言

水下無(wú)人航行器（Unmanned Underwater Vehicle，UUV）最近成為一個(gè)研究的熱點(diǎn)。它在軍事偵察、水下打擊、水下搜救與打撈、水下資源勘探與開采等領(lǐng)域都有十分重要的應(yīng)用。UUV又可以分為自主式水下無(wú)人航行器（Autonomous Underwater Vehicle，AUV）、遠(yuǎn)程操控式水下無(wú)人航行器（Remotely Operated Vehicle，ROV）兩大類。ROV 由于需要一根連接到母船的電纜/光纜進(jìn)行操控和通信，所以限制了其應(yīng)用范圍。AUV 具有更高的自主性，可以完成更多更復(fù)雜的水下作業(yè)任務(wù)。AUV 的導(dǎo)航定位是AUV 總體研制技術(shù)中的關(guān)鍵技術(shù)，對(duì)于AUV 十分重要。水下可以利用的導(dǎo)航信息十分稀少，所以水下的導(dǎo)航比空中導(dǎo)航研究的難度更大，面臨更大的挑戰(zhàn)。AUV 的導(dǎo)航定位分系統(tǒng)通常由慣性導(dǎo)航裝置、多普勒計(jì)程儀和衛(wèi)星定位裝置組成。單獨(dú)一個(gè)AUV作業(yè)時(shí)，其導(dǎo)航定位技術(shù)比較成熟，精度已經(jīng)達(dá)到一個(gè)較高的水平。定位精度與導(dǎo)航系統(tǒng)的成本是一對(duì)矛盾，低成本的導(dǎo)航系統(tǒng)實(shí)現(xiàn)高精度的定位對(duì)于AUV 意義重大：可以大大地降低成本并極大地拓展AUV 的應(yīng)用范圍。但是，對(duì)于單體AUV，低成本導(dǎo)航系統(tǒng)很難實(shí)現(xiàn)高精度的定位。

對(duì)于AUV 集群，通過(guò)較優(yōu)的協(xié)同導(dǎo)航算法可以實(shí)現(xiàn)集群中所有低成本的從AUV 的導(dǎo)航定位精度接近主AUV 的水平。在這樣的集群中，主AUV 配備常規(guī)的高精度慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）+高精度多普勒計(jì)程儀（Doppler Velocity Log，DVL）+全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS），從AUV 通常配備磁羅盤（Magnetic Compass，MC）+低精度DVL+GNSS 或者低精度INS+MC+低精度DVL+GNSS。由于高精度INS 占整個(gè)導(dǎo)航系統(tǒng)成本的60%～70%左右，高精度DVL 與低精度DVL 的差價(jià)一般在10 萬(wàn)元以上，所以從AUV的導(dǎo)航定位系統(tǒng)成本將大大降低，從而可以實(shí)現(xiàn)整個(gè)較低成本的AUV 集群達(dá)到較高的導(dǎo)航定位精度。協(xié)同導(dǎo)航的核心是：利用水聲定位系統(tǒng)、INS、MC 等測(cè)量和計(jì)算得到的主從AUV 相對(duì)距離，相對(duì)方位等信息，再利用水聲通信設(shè)備交互共享這些信息并通過(guò)合理的協(xié)同導(dǎo)航模型與算法實(shí)現(xiàn)所有從AUV 的高精度定位。采用這種技術(shù)，可以讓一個(gè)制造成本較低的AUV 集群去執(zhí)行一些需要高精度導(dǎo)航定位的水下作業(yè)任務(wù)（例如：水下資源勘探、測(cè)繪、打撈與搜救、探潛與反潛、反水雷作戰(zhàn)、水下高精度的水雷布放等），從而可以完全取代傳統(tǒng)的高成本AUV 集群（集群中所有AUV 都配置高精度INS）。所以，這種協(xié)同導(dǎo)航技術(shù)將大大擴(kuò)展AUV 的應(yīng)用，有效地降低各種水下作業(yè)的成本以及提高作業(yè)效率，對(duì)于國(guó)家大力發(fā)展海洋經(jīng)濟(jì)有重要的意義。

最常用的協(xié)同導(dǎo)航方法就是基于AUV 之間的距離或者距離+方位角的觀測(cè)量在直角坐標(biāo)系（笛卡爾坐標(biāo)系）下建立擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）的模型來(lái)提升每個(gè)AUV 的導(dǎo)航定位精度[1-4]。協(xié)同導(dǎo)航的模式一般分為無(wú)領(lǐng)航者模式（去中心化模式）[4]和主從式（領(lǐng)航者模式）。主從式一般又分為單領(lǐng)航者[1-3,5]、雙領(lǐng)航者[6]和多領(lǐng)航者模式[2]。在無(wú)領(lǐng)航者模式中，所有AUV 配置同樣精度的慣導(dǎo)，通過(guò)協(xié)同導(dǎo)航來(lái)對(duì)導(dǎo)航精度進(jìn)行微弱的提升。而在有領(lǐng)航者模式中，領(lǐng)航者AUV 攜帶的慣導(dǎo)精度比跟隨者AUV 的至少高一個(gè)數(shù)量級(jí)，通過(guò)協(xié)同導(dǎo)航，將整個(gè)AUV 團(tuán)體的精度提升至與領(lǐng)航者的導(dǎo)航精度相當(dāng)?shù)乃?。主從式模式還可以用來(lái)估算未知的定常洋流信息[7]。一般AUV 之間的距離/方位角信息通過(guò)聲通機(jī)、水聽器[8]、長(zhǎng)基線水聲定位系統(tǒng)[9]等設(shè)備來(lái)測(cè)量并進(jìn)行交互。聲學(xué)設(shè)備都存在聲波通信延遲帶來(lái)的各種問(wèn)題，所以需要專門針對(duì)聲學(xué)通信延遲進(jìn)行補(bǔ)償[10-11]。對(duì)于長(zhǎng)期作曲線運(yùn)動(dòng)的AUV，運(yùn)用直角坐標(biāo)系建模將帶來(lái)較大誤差，有學(xué)者提出了基于極坐標(biāo)系的協(xié)同導(dǎo)航算法[12]。

傳統(tǒng)的AUV 協(xié)同導(dǎo)航算法都是基于直角坐標(biāo)系，對(duì)于長(zhǎng)期作直線或近似直線運(yùn)動(dòng)的AUV集群具有較高的精度。但是，大量的可觀性分析表明：想要提高AUV 集群的可觀性，必須依靠曲線運(yùn)動(dòng)等這類機(jī)動(dòng)。另外，有時(shí)受限于水下地形（例如：必須不停地拐彎進(jìn)行避障）或任務(wù)的特殊要求（例如：按某種特定的曲線運(yùn)動(dòng)進(jìn)行高效的搜索與探測(cè)），AUV 集群必須長(zhǎng)期處于機(jī)動(dòng)運(yùn)動(dòng)狀態(tài)。在這種情況下，采用常規(guī)的基于直角坐標(biāo)系的協(xié)同導(dǎo)航算法，定位精度將變差，定位誤差將有較大的增長(zhǎng)。所以，需要開發(fā)一種適用于這種運(yùn)動(dòng)情形的新協(xié)同導(dǎo)航算法來(lái)提高精度。本文提出了在極坐標(biāo)系下建立協(xié)同導(dǎo)航模型和相應(yīng)的算法來(lái)解決定位精度不高的問(wèn)題。

除了AUV，協(xié)同導(dǎo)航技術(shù)在其他各類無(wú)人裝備/機(jī)器人中也得到了廣泛的應(yīng)用。例如，有學(xué)者提出將最優(yōu)通信網(wǎng)絡(luò)模型[13]、車聯(lián)網(wǎng)[14]、概率估計(jì)算法和優(yōu)化算法[15]、協(xié)同編隊(duì)[16]、自適應(yīng)協(xié)同[17]等用于無(wú)人系統(tǒng)的協(xié)同導(dǎo)航與定位?？缬騾f(xié)同[18-19]、有人/無(wú)人裝備的協(xié)同[20-21]等也是協(xié)同導(dǎo)航領(lǐng)域未來(lái)的發(fā)展方向。

2 物理模型

2.1 直角坐標(biāo)系情形

協(xié)同導(dǎo)航主要是利用AUV 之間的距離，方位角信息，所以建模也是針對(duì)這些信息。以僅觀測(cè)距離信息的情況為例（假設(shè)AUV 的俯仰角接近零，忽略它的影響），模型如下：

其中，[xk,yk]T是k時(shí)刻AUV 在導(dǎo)航坐標(biāo)系（一般采用東北天的直角坐標(biāo)系）里的位置；Ψk是AUV 的航向角（相對(duì)于地理北向）；vk是AUV 的前向速度；uk是AUV 的橫向速度；Δt是導(dǎo)航計(jì)算的時(shí)間間隔。

需要說(shuō)明的是，由于慣導(dǎo)的高度通道是不穩(wěn)定的，一般深度信息都通過(guò)AUV 搭載的深度計(jì)獲得，所以建模時(shí)只用考慮AUV 實(shí)際位置投影到同一水平面（一般選擇水域的表面），只對(duì)水平距離建模即可。

如果需要觀測(cè)AUV 之間相對(duì)方位角信息，則模型中需要加入相對(duì)方位角的變化規(guī)律（與AUV 航向角變化速率相關(guān)）。有的情況下還可以獲得AUV 的準(zhǔn)確航速信息，航速當(dāng)作觀測(cè)量時(shí)，模型中需要加入速度相關(guān)的變化規(guī)律（與加速度相關(guān)）。另外，建立卡爾曼濾波模型時(shí)還需要考慮AUV 的IMU 中陀螺和加速度計(jì)的噪聲。由于協(xié)同導(dǎo)航中相對(duì)距離是最重要的關(guān)鍵量，所以建模時(shí)加速度計(jì)的噪聲更為重要。詳細(xì)模型方程可以參考文獻(xiàn)[1-4]。

對(duì)于在深海區(qū)航行的AUV，多普勒計(jì)程儀往往只能測(cè)出相對(duì)海流的速度，這將導(dǎo)致組合導(dǎo)航精度下降，所以洋流估計(jì)也是一個(gè)重要的研究方向。如前文分析，由于領(lǐng)航者攜帶了高精度慣導(dǎo)，所以通過(guò)協(xié)同導(dǎo)航在提高跟隨者AUV 導(dǎo)航定位精度的同時(shí)，還可以估算出定常洋流的大小。利用主從式協(xié)同導(dǎo)航方式還可以估算定常未知洋流。這種情形下的建模如下[7]：

式中，上標(biāo)S 代表從AUV，也就是跟隨者；Uxk和Uyk分別為k時(shí)刻的x和y方向的定常洋流大?。沪膖是協(xié)同定位導(dǎo)航的更新時(shí)間；N=δt/Δt。為從AUV 在ti時(shí)刻的前向航行速度；為從AUV 在ti時(shí)刻的偏航角。

利用以上這種模型就可以估算出定常洋流信息，估算出洋流以后，再補(bǔ)償?shù)蕉嗥绽沼?jì)程儀的測(cè)速信息上，就可以改進(jìn)每個(gè)AUV 自身的導(dǎo)航定位精度。

任何水聲設(shè)備都有一定的延時(shí)，為了提高協(xié)同導(dǎo)航的精度，需要對(duì)延時(shí)進(jìn)行補(bǔ)償。考慮水聲延時(shí)的模型和常規(guī)情況的模型一樣?？紤]水聲通信延遲的主從式多AUV 協(xié)同定位具體過(guò)程如下：①AUVM（主AUV）、從AUVS（從AUV）進(jìn)行時(shí)鐘同步準(zhǔn)備；②AUVM在TS時(shí)刻對(duì)AUVS進(jìn)行水聲測(cè)距；③由于水聲設(shè)備信息處理、激勵(lì)聲波信號(hào)需要一定的時(shí)間，所以AUVM延遲一段時(shí)間向AUVS廣播自身的定位信息、AUVM與AUVS的間距信息，以及信息發(fā)送時(shí)刻的時(shí)間戳；④水聲信息經(jīng)過(guò)一定時(shí)間的傳播，到達(dá)AUVS。AUVS接收到信息后，通過(guò)自身攜帶的世界標(biāo)準(zhǔn)時(shí)間系統(tǒng)計(jì)算本次接收的信息延遲時(shí)長(zhǎng)，按照信息發(fā)送時(shí)間順序，依次利用AUVM的相對(duì)定位信息實(shí)現(xiàn)對(duì)自身航推定位誤差的修正，完成AUVS的協(xié)同定位；⑤AUVM與各AUVS重復(fù)完成過(guò)程②～④，實(shí)現(xiàn)主從式多AUV 協(xié)同定位。

2.2 極坐標(biāo)系情形

對(duì)于長(zhǎng)時(shí)間作曲線運(yùn)動(dòng)的AUV，利用直角坐標(biāo)系會(huì)帶來(lái)一些不便。這時(shí)，用極坐標(biāo)系更加簡(jiǎn)便。以主從模式為例，AUVM和AUVS的兩組極坐標(biāo)與航向角分別記為ρM，φM，ΨM和ρS，φS，ΨS?？梢哉业饺缦碌年P(guān)系式[12]：

在慣性系下研究主和從AUV 的運(yùn)動(dòng)時(shí)，Ψ就是相對(duì)于慣性系y軸的方位角；在東北天的導(dǎo)航系下研究主和從AUV 的運(yùn)動(dòng)時(shí)，Ψ就是相對(duì)地理北向的方位角。

當(dāng)AUVM在AUVS前面時(shí)：

當(dāng)AUVM在AUVS后面時(shí)：

式中，R為主從AUV 之間的距離；φ為主從AUV之間的相對(duì)方位角。將ρM，φM，ΨM和ρS，φS，ΨS這6 個(gè)量作為狀態(tài)變量，就可以建立卡爾曼模型。其中，狀態(tài)轉(zhuǎn)移陣為：

式中，T為卡爾曼濾波器的離散化時(shí)的采樣周期。這種方法在極坐標(biāo)體系下描述曲線運(yùn)動(dòng)十分簡(jiǎn)便，也可以保證較高的導(dǎo)航精度。

3 導(dǎo)航算法

3.1 直角坐標(biāo)系情形

整個(gè)協(xié)同導(dǎo)航算法的核心就是模型的建立。通常可以選擇的觀測(cè)量為兩個(gè)AUV 之間的距離、兩個(gè)AUV 之間的相對(duì)方位角和兩個(gè)AUV 各自的速度等。最常用的就是距離，距離+相對(duì)方位角，距離+主和從AUV 的速度，距離+相對(duì)方位角+主和從AUV 的速度這4 種情況。建模時(shí)應(yīng)該根據(jù)觀測(cè)量的情況和所需協(xié)同導(dǎo)航精度來(lái)選擇狀態(tài)變量進(jìn)行建模。一般觀測(cè)量越多并且精度要求越高，則需要選擇的狀態(tài)變量就越多，計(jì)算也就越慢。模型的系統(tǒng)噪聲、量測(cè)噪聲、初值等也十分重要。在AUV 不會(huì)長(zhǎng)時(shí)間做曲線運(yùn)動(dòng)的大多數(shù)情況下，還是選擇直角坐標(biāo)系更加簡(jiǎn)便。

整個(gè)模型建立好以后，開始選擇合適的濾波算法。由于協(xié)同導(dǎo)航的卡爾曼模型是一個(gè)典型的非線性模型，只能使用非線性的卡爾曼濾波算法，最常見的是EKF（擴(kuò)展卡爾曼）算法。由于采用的都是標(biāo)準(zhǔn)的EKF 計(jì)算流程，這里不再贅述。對(duì)于需要補(bǔ)償聲學(xué)設(shè)備的延時(shí)的情況，可以使用延時(shí)EKF 算法（DEKF）[10]。DEKF 是在水聲通信延遲EKF 方法的基礎(chǔ)上，利用延遲濾波狀態(tài)存儲(chǔ)器保存的延遲過(guò)程中各時(shí)刻的濾波狀態(tài)值，基于狀態(tài)估計(jì)均方誤差最小對(duì)量測(cè)信息延遲產(chǎn)生的協(xié)同定位誤差進(jìn)行修正。

除了EKF，還可以采用更先進(jìn)的自適應(yīng)濾波（AF）[3]、粒子濾波（PF）[22-23]、因子圖算法[24]、M 估計(jì)算法[25]、無(wú)跡卡爾曼濾波（UKF）算法[26]等。一般情況下，越先進(jìn)的算法意味著更高的精度和更大的計(jì)算量。需要根據(jù)AUV 搭載的運(yùn)算硬件資源來(lái)選擇合適的濾波算法。另外，在設(shè)計(jì)濾波算法時(shí)，如果在使用距離和速度等量測(cè)量之前對(duì)它們進(jìn)行濾波，剔除掉其中的野值[27-28]，則可以進(jìn)一步提升協(xié)同導(dǎo)航的精度和可靠性。

3.2 極坐標(biāo)系情形

在極坐標(biāo)情形下，同樣是選擇合適的觀測(cè)量，建立極坐標(biāo)下的卡爾曼模型，然后推導(dǎo)濾波算法。以選擇相對(duì)距離+相對(duì)方位角為例，可以得到AUVM在AUVS前方時(shí)的量測(cè)矩陣為：

AUVM在AUVS后方時(shí)的量測(cè)矩陣與式(8)幾乎相同，只是第二行的元素全部取反。建立完模型以后就可以采用常規(guī)的EKF 算法或自適應(yīng)濾波等進(jìn)行解算。

4 可觀性分析

對(duì)于一個(gè)卡爾曼系統(tǒng)，其可觀性分析十分重要。選擇不同的觀測(cè)量，系統(tǒng)就有不同的可觀性。先分析直角坐標(biāo)系情形下的可觀測(cè)性問(wèn)題。建立的坐標(biāo)系關(guān)系如圖1所示。

圖1 中，ΣI、ΣM和ΣS分別是慣性坐標(biāo)系、主AUV 坐標(biāo)系和從AUV 坐標(biāo)系。注意圖1 中的航向角Ψ為相對(duì)于主AUV 的X軸向（主AUV 的前進(jìn)方向）的相對(duì)方位角?；谝苿?dòng)矢徑單領(lǐng)航AUV 協(xié)同導(dǎo)航定位示意圖見圖2。根據(jù)圖2 可以得到表示式(9)。

圖1 單領(lǐng)航者AUV 協(xié)同導(dǎo)航各個(gè)坐標(biāo)系關(guān)系Fig.1 Schematic diagram of multi-AUV cooperative navigation and positioning system for single leader

圖2 基于移動(dòng)矢徑單領(lǐng)航AUV 協(xié)同導(dǎo)航定位示意圖Fig.2 Schematic diagram of cooperative navigation of multiple AUVs based on the MRV(Moving Radius Vector)

如果記ΣI中主AUV 和從AUV 的狀態(tài)變量分別為XM=[xM,yM,ΨM]T和XS=[xS,yS,ΨS]T，則有式(10)所示的關(guān)系。

式中，ωM(t)和ωS(t)是航向角變化的角速率。注意此處的航向角都是相對(duì)于XI軸的方位角。如果把ΣI中的運(yùn)動(dòng)狀態(tài)XS=[xS,yS,ΨS]T轉(zhuǎn)換到ΣM中，記為[x,y,Ψ]T?？紤]到極坐標(biāo)下運(yùn)算更加便捷，轉(zhuǎn)換為極坐標(biāo)后記為[ρ,φ,Ψ]T，則有式(11)所示的表達(dá)式。

一個(gè)典型的非線性系統(tǒng)如式(12)所示。

式中，x=[x1,…,xn]T為該系統(tǒng)的狀態(tài)向量；u=[u1,…,ul]T為該系統(tǒng)的輸入向量；y=[y1,…,ym]T為該系統(tǒng)的輸出向量，其中，yk=hk(x)，k=1,…,m。

非線性函數(shù)hk(x)關(guān)于fi(x)的n階Lie 導(dǎo)數(shù)為：

式中，fi(x)=[fi1(x),…,fin(x)]T；?是梯度運(yùn)算符，?是向量的內(nèi)積運(yùn)算符。

有了以上Lie 導(dǎo)數(shù)定義，就可以建立非線性系統(tǒng)的觀測(cè)性矩陣：

如果這個(gè)矩陣是滿秩的，則該系統(tǒng)是局部弱能觀的。下面分析僅觀測(cè)距離的非線性系統(tǒng)的能觀性。為了簡(jiǎn)化計(jì)算，把主從AUV 之間距離的測(cè)量值設(shè)為h(x)=(1/2)?d2(t)。把式(11)寫成式(12)的形式，則有：

式中，γ=Ψ?φ。設(shè)該系統(tǒng)的 4 個(gè)輸入分別為：u1=vM，u2=ωM，u3=vS，u4=ωS，則可得到如下觀測(cè)性矩陣O：

如果只考慮u1和u3，則可得到如下觀測(cè)性矩陣Ov：

類似地，只考慮u2和u4，則可得到觀測(cè)性矩陣Oω。對(duì)O陣或者Ov+Oω陣進(jìn)行分析，可以得到如下結(jié)論。在如下兩種情況下系統(tǒng)是可觀的[29]：①vM≠0，vS≠0，ωM=ωS=0，同時(shí)兩個(gè)AUV 的運(yùn)動(dòng)軌跡不共線。②vM≠0，vS≠0，而且ωM和ωS中至少一個(gè)不為0。當(dāng)在某一段時(shí)間內(nèi)，兩個(gè)AUV的航跡平行且運(yùn)動(dòng)速度相同，或者兩個(gè)AUV 的航跡共線，則此段時(shí)間內(nèi)系統(tǒng)是不可觀測(cè)的，所以應(yīng)該盡量避免這兩種運(yùn)動(dòng)情況?？偨Y(jié)以上情況，當(dāng)主從AUV 之間距離為0（主和從AUV 位置重合）或不變時(shí)，該系統(tǒng)是不可觀的。

下面，以極坐標(biāo)系下的非線性系統(tǒng)為研究對(duì)象，推導(dǎo)該系統(tǒng)以距離和航向/方位角作為觀測(cè)量時(shí)的可觀測(cè)性。極坐標(biāo)系下的非線性系統(tǒng)如式(18)和(19)所示。

當(dāng)AUVM在AUVS前面時(shí)，相對(duì)距離和相對(duì)方位角的表達(dá)式h1和h2如下面所示：

注意，此處假定是在導(dǎo)航系中建立模型，所以ΨS是相對(duì)于地理北向的航向角，與式(10)和(11)中ΨS含義不同。主和從AUV 之間的相對(duì)方位角如式(20)中的h2所示?；谝陨侠碚摽梢越⑷缡?21)所示的觀測(cè)性矩陣。

式中，相關(guān)變量的表達(dá)式為：a1=ρS?ρM?cos(φS?φM)；a2=ρS?ρM?sin(φS?φM)；b1=a2/ρS；b2=?a1?ρS；c1=(h1)2?f1?(a1)2?f1?a1?a2?f2；c2=(h1)2?(rS)2?f2?a1?a2?f1?(a2)2?f2；c3=(h1)2?(b2?f2?b1?f1)；d1=(h1)2?rS?f2?2a1?b1?f1?2a2?b2?f2；d2=?(h1)2?rS?f1?2a2?b1?f1?2a2?b2?f2；d3=(h1)2?(a2?f2+a1?f1)，f1是矢量f1(x)中的第1 個(gè)分量，f2是矢量f2(x)中的第2 個(gè)分量；OR和Oφ分別是相對(duì)距離和相對(duì)方位角的子塊觀測(cè)矩陣。

類似地，當(dāng)AUVM在AUVS后面時(shí)，相對(duì)距離和相對(duì)方位角的表達(dá)式h1和h2如下所示：

式中，相關(guān)變量的表達(dá)式為：a1=ρS?ρM?cos(φS?φM)；a2=ρS?ρM?sin(φS?φM)；b1=a2/ρS；b2=a1?ρS；c1=(h1)2?f1?(a1)2?f1?a1?a2?f2；c2=(h1)2?(rS)2?f2?a1?a2?f1?(a2)2?f2；c3=(h1)2?(b2?f2?b1?f1)；d1=?(h1)2?rS?f2?2a1?b1?f1?2a2?b2?f2；d2=(h1)2?rS?f1?2a2?b1?f1?2a2?b2?f2；d3=?(h1)2?(a2?f2+a1?f1)。

通過(guò)對(duì)式(21)和(23)分析可知：①當(dāng)主和子AUV 之間的相對(duì)距離不變時(shí)，相對(duì)距離觀測(cè)子塊不可觀。②當(dāng)主和子之間的相對(duì)方位角不變時(shí),相對(duì)方位角觀測(cè)子塊不可觀。③當(dāng)主和子AUV的位置重合時(shí)，系統(tǒng)整體不可觀（兩個(gè)子塊都不可觀）。④當(dāng)相對(duì)距離和相對(duì)方位角都不變時(shí)，系統(tǒng)整體不可觀。該結(jié)論與前面直角坐標(biāo)系情形下的分析是一致的。被觀測(cè)量只有在變化時(shí)，可觀測(cè)性才比較強(qiáng)。所以，協(xié)同導(dǎo)航中應(yīng)該盡量避免相對(duì)距離或相對(duì)方位角保持為恒定的情況。

5 基于實(shí)航數(shù)據(jù)的仿真測(cè)試

基于以上模型和理論，利用主/從AUV 實(shí)航的試驗(yàn)數(shù)據(jù)進(jìn)行了協(xié)同導(dǎo)航的離線仿真，分析了不同模型和觀測(cè)量對(duì)協(xié)同導(dǎo)航精度的影響。試驗(yàn)在某個(gè)湖中進(jìn)行，主AUV 的口徑為324 mm，搭載高精度光纖INS（其中光纖陀螺的零偏穩(wěn)定性ε=0.008°/h，石英撓性加速度計(jì)的零偏穩(wěn)定性?=10 μg）、測(cè)速精度為0.4%±4 mm/s 的DVL（多普勒計(jì)程儀）、定位誤差≤3 m 的全球定位系統(tǒng)（Global Positioning System，GPS）接收機(jī)。從AUV 的口徑為200 mm，搭載低精度光纖慣導(dǎo)（光纖陀螺的零偏ε=0.1°/h，石英撓性加速度計(jì)的零偏?=500 μg）、測(cè)速精度為2%±150 mm/s 的DVL（多普勒計(jì)程儀）、定位誤差≤3 m 的GPS、航向角誤差≤0.2°的磁羅盤。從AUV 搭載磁羅盤是為了獲得自身較準(zhǔn)確的航向角信息，從而可以得到相對(duì)方位角觀測(cè)量。用于測(cè)量主/從AUV 相對(duì)距離的水聲設(shè)備的主要指標(biāo)為：測(cè)距誤差≤0.1 m、量程≥4000 m、聲波波束角≥200°。考慮到AUV在水面航行時(shí)水聲定位測(cè)距系統(tǒng)的測(cè)量精度較差，所以在水面時(shí)使用無(wú)線電定位測(cè)距系統(tǒng)。由主AUV 的組合導(dǎo)航推算以及GPS 量測(cè)得到的航跡如圖3所示。該試驗(yàn)中主和從AUV 的GPS 位置信息誤差較小，可以認(rèn)為是準(zhǔn)確位置，在本研究中作為Benchmark 數(shù)據(jù)。本研究中，將從AUV自身進(jìn)行航位推算、從AUV 自身進(jìn)行組合導(dǎo)航、基于距離信息的傳統(tǒng)（基于直角坐標(biāo)系）協(xié)同導(dǎo)航算法和基于距離+方位角信息的傳統(tǒng)協(xié)同導(dǎo)航算法這4 種算法作為對(duì)比對(duì)象，重點(diǎn)研究了基于距離信息（基于極坐標(biāo)系）的新式協(xié)同導(dǎo)航算法和基于距離+方位角信息的新式協(xié)同導(dǎo)航算法這兩種算法的定位精度。為了能夠得到Benchmark數(shù)據(jù)，本研究總是在整個(gè)AUV 集群都處于水面能夠接收到GPS 信息時(shí)才統(tǒng)計(jì)定位誤差。這種對(duì)比研究和衡量誤差的方法是協(xié)同導(dǎo)航研究中最常用的方法。

圖3 主AUV 的航跡圖Fig.3 Tracks of the master AUV

整個(gè)航行試驗(yàn)的時(shí)間持續(xù)了約25620 s（約7.11 h）。圖3 是出發(fā)后約1000 s 時(shí)間內(nèi)的一段航跡。當(dāng)在水面航行時(shí)，主AUV 處于INS/DVL/GPS組合導(dǎo)航狀態(tài)，在水下航行時(shí)，處于INS/DVL 組合導(dǎo)航模式。試驗(yàn)中，從AUV 始終在主AUV 附近1 km 左右跟隨前進(jìn)，主AUV 水下航行時(shí)，從AUV 也跟隨進(jìn)行水下航行。主AUV 和從AUV的巡航速度都盡量保持在2.5 kn 左右。圖中有一段沒(méi)有顯示GPS 軌跡，就是因?yàn)橹鰽UV 在水下航行。主AUV 的在某段時(shí)間內(nèi)的姿態(tài)信息如圖4（俯仰和橫滾角）和圖5（航向角）所示。

該信息為截取的一段長(zhǎng)度為350s 的姿態(tài)角數(shù)據(jù)。圖4 表明AUV 在上浮或下潛時(shí)俯仰角變化幅度較大，而橫滾角變化不大，始終在0°附近波動(dòng)。為了保持較好的可觀性，主AUV 的航向角一直在變化，如圖5所示。圖5 中，0°與360°之間的躍變是由航向角的0°～360°的周期性引起的。該航向角依靠INS/DVL/GPS 組合導(dǎo)航（水面）或INS/DVL 組合導(dǎo)航（水下）計(jì)算得到，精度比較高。

圖4 主AUV 的俯仰和橫滾角Fig.4 Roll and pitch angles of the master AUV

圖5 主AUV 的航向角Fig.5 Yaw angle of the master AUV

從AUV 的航向角由磁羅盤量測(cè)得到。本研究通過(guò)事后處理，綜合低精度光纖慣導(dǎo)與磁羅盤量測(cè)信息，對(duì)磁羅盤的野值進(jìn)行了識(shí)別。航向角一旦被判定為野值，就認(rèn)為此時(shí)的量測(cè)值無(wú)效，卡爾曼濾波只進(jìn)行時(shí)間更新而不進(jìn)行量測(cè)更新。這樣處理可以避免野值造成卡爾曼估算誤差的快速增長(zhǎng)。針對(duì)野值的判別與剔除，本研究采用統(tǒng)計(jì)方法(“3σ準(zhǔn)則”)。根據(jù)高斯誤差的理論，當(dāng)測(cè)量值服從正態(tài)分布時(shí)，殘差落在[?3σ,3σ]區(qū)間的概率≥99.7%，落在此區(qū)間外的概率<0.3%。因此，可認(rèn)為殘差落于該區(qū)間之外的測(cè)量值就是野值（即：異常值），這就是“3σ準(zhǔn)則”。設(shè)Xi(i=1,2,…,n)為等精度獨(dú)立測(cè)量的數(shù)值（可以是DVL 測(cè)量的速度、磁羅盤測(cè)量的角度、水聲定位系統(tǒng)測(cè)量的距離和方位等信息），平均之后得均值，標(biāo)準(zhǔn)差σ可以按照貝塞爾公式進(jìn)行計(jì)算。3σ準(zhǔn)則定義為：凡是殘差大于3 倍標(biāo)準(zhǔn)偏差的測(cè)量值判定為野值，應(yīng)進(jìn)行剔除。表達(dá)如下：

σ的計(jì)算公式（貝塞爾公式）如下：

大量的測(cè)試和數(shù)值分析表明：“3σ準(zhǔn)則”可以成功地剔除大部分野值點(diǎn)。

相對(duì)方位角的觀測(cè)信息（參考圖1 和式(15)中的γ）就是從AUV 的艏向與主從AUV 連線矢量（主AUV 質(zhì)心指向從AUV 質(zhì)心的矢量）之間的夾角。有2 種計(jì)算方法：①由從AUV 的航向角減去主從AUV 連線矢量與地理北向的夾角（該夾角由測(cè)距定位系統(tǒng)得到）；②由從AUV 與主AUV 的航向角之差、主從AUV 連線矢量與主AUV 艏向的夾角（由測(cè)距定位系統(tǒng)得到）這兩個(gè)角相減計(jì)算得到。本研究選用更加簡(jiǎn)便的方法①進(jìn)行計(jì)算。正如前面的分析，本算法對(duì)磁羅盤、GPS、水聲定位設(shè)備、水聲通信設(shè)備等的輸出進(jìn)行野值判識(shí)。當(dāng)傳感器/設(shè)備的輸出被判定為野值時(shí)，EKF 只進(jìn)行時(shí)間更新而不進(jìn)行量測(cè)更新。

主AUV 的DVL 測(cè)量的離底高度如圖6所示。

圖6 主AUV 的DVL 測(cè)量的離底高度Fig.6 Height from the lakebed measured by the DVL of the master AUV

圖6 中，301s 處存在一個(gè)野值，此時(shí)的測(cè)速信息也為一個(gè)野值，在主AUV 的INS 的組合導(dǎo)航算法里、離線仿真中都剔除了這個(gè)野值。DVL出現(xiàn)測(cè)速野值時(shí)，主AUV 的組合導(dǎo)航濾波器只進(jìn)行時(shí)間更新，不進(jìn)行量測(cè)更新。除了為數(shù)不多的幾個(gè)野值，DVL 的測(cè)速信息（AUV 在水面航行時(shí)）與GPS 的測(cè)速信息基本吻合，最大誤差不超過(guò)0.12m/s，從而保證了INS/DVL 較高的組合導(dǎo)航精度。

圖7 是從AUV 搭載的某款國(guó)產(chǎn)磁羅盤產(chǎn)品的實(shí)物照片，其核心指標(biāo)如下：

圖7 某款磁羅盤的照片F(xiàn)ig.7 Photo of a magnetic compass

（1）方位角：測(cè)量范圍?180°～+180°，分辨率0.01°，精度0.2°；

（2）俯仰角：測(cè)量范圍?90°～+90°（0°～+180°），分辨率0.01°，精度±0.1°；

（3）橫滾角：測(cè)量范圍?180°～+180°，分辨率0.01°，精度±0.1°。

在實(shí)航試驗(yàn)中，主和從AUV 的時(shí)間系統(tǒng)都依靠各自慣導(dǎo)中的高精度晶振（兩個(gè)晶振的精度相同），同時(shí)在水面時(shí)還利用了GPS 發(fā)送的時(shí)間信息和秒脈沖等進(jìn)行校準(zhǔn)，所以保證了較高的時(shí)間同步性。試驗(yàn)中，INS 的慣性測(cè)量單元（IMU）的數(shù)據(jù)更新周期為5 ms，GPS 和DVL 和磁羅盤為1s，定位測(cè)距系統(tǒng)為240s。主/從AUV 內(nèi)INS的卡爾曼濾波器的時(shí)間更新周期和量測(cè)更新周期分別為5 ms 和1s。協(xié)同導(dǎo)航的EKF 濾波器的時(shí)間更新周期和量測(cè)更新周期分別為1s 和240s。當(dāng)GPS 等傳感器的量測(cè)信息丟失或出現(xiàn)野值時(shí)，卡爾曼濾波器只進(jìn)行時(shí)間更新而不進(jìn)行量測(cè)更新。從圖3 可知，主AUV 的組合導(dǎo)航精度較高，軌跡幾乎與GPS 點(diǎn)航跡重合。而從AUV 的慣導(dǎo)中慣性器件和DVL 的精度比主AUV 都低1～2 個(gè)數(shù)量級(jí)，所以不依靠協(xié)同導(dǎo)航時(shí)，從AUV 的組合導(dǎo)航定位精度較差，如表1所示。

從AUV 的導(dǎo)航定位有6 種方法：（1）從AUV進(jìn)行自主航位推算。利用磁羅盤的航向角和DVL的測(cè)速信息進(jìn)行航位推算。（2）從AUV 進(jìn)行自主INS/DVL/磁羅盤組合導(dǎo)航。注意從AUV 即使在水面接收到GPS 信息，也只是存儲(chǔ)當(dāng)作準(zhǔn)確位置，而不進(jìn)行參與組合導(dǎo)航，以模擬真實(shí)的從AUV 長(zhǎng)期水下航行無(wú)GPS 信息的情況。（3）在從AUV 組合導(dǎo)航的基礎(chǔ)上，利用笛卡爾坐標(biāo)系下的EKF 模型和相對(duì)距離觀測(cè)量進(jìn)行協(xié)同導(dǎo)航，記為C-EKF1。（4）利用笛卡爾坐標(biāo)系下的EKF模型以及相對(duì)距離+相對(duì)方位角觀測(cè)量進(jìn)行協(xié)同導(dǎo)航，記為C-EKF2。（5）利用極坐標(biāo)系下的EKF模型和相對(duì)距離觀測(cè)量進(jìn)行協(xié)同導(dǎo)航，記為P-EKF1。（6）利用極坐標(biāo)系下的EKF 模型以及相對(duì)距離+相對(duì)方位角觀測(cè)量進(jìn)行協(xié)同導(dǎo)航，記為P-EKF2。離線仿真的結(jié)果見表1。

表1 仿真結(jié)果Table 1 Simulation results

從AUV 的準(zhǔn)確位置選定為從AUV 的GPS軌跡點(diǎn)。所以，只有從AUV 在水面運(yùn)動(dòng)時(shí)才能夠計(jì)算定位誤差大小。表1 中選取的4 個(gè)點(diǎn)都是從AUV 在水面航行的時(shí)刻。表1 中的從AUV 總航程由水面GPS 軌跡點(diǎn)+水下P-EKF2 協(xié)同導(dǎo)航算法推算得到的。由表1 的結(jié)果可知：航位推算和組合導(dǎo)航算法都有誤差隨時(shí)間增大的特點(diǎn)。C-EKF1 算法依然服從這個(gè)誤差隨時(shí)間增大的規(guī)律，只是誤差大小得到了一定的抑制。C-EKF2、P-EKF1 和P-EKF2 這3 種算法的誤差都不再隨時(shí)間單調(diào)增加，而是波動(dòng)或保持大致恒定。特別是P-EKF2 算法可以大大地減小誤差。

以上離線仿真測(cè)試表明：當(dāng)主和從AUV 頻繁做曲線運(yùn)動(dòng)等各種機(jī)動(dòng)時(shí)，與基于相對(duì)距離的直角坐標(biāo)系下的EKF 算法（C-EKF1）相比，基于相對(duì)距離+相對(duì)方位角觀測(cè)量和曲線坐標(biāo)系下協(xié)同導(dǎo)航算法（P-EKF2）可將從AUV 的導(dǎo)航定位誤差減小至20m 以內(nèi)。

6 結(jié)論

以基于單領(lǐng)航者的主從式AUV 集群為研究對(duì)象，利用AUV 的運(yùn)動(dòng)學(xué)方程和量測(cè)方程建立了極坐標(biāo)系下的協(xié)同導(dǎo)航模型，設(shè)計(jì)了基于 EKF的導(dǎo)航濾波算法。該算法適用于長(zhǎng)時(shí)間機(jī)動(dòng)航行的AUV 集群。與常規(guī)的航位推算方法、低精度慣導(dǎo)的組合導(dǎo)航方法、基于距離信息的傳統(tǒng)（基于直角坐標(biāo)系）協(xié)同導(dǎo)航算法、基于距離+方位角信息的傳統(tǒng)協(xié)同導(dǎo)航算法進(jìn)行對(duì)比，研究了基于距離信息的新式（基于極坐標(biāo)系）協(xié)同導(dǎo)航算法、基于距離+方位角信息的新式協(xié)同導(dǎo)航算法。另外，為了提高本算法的魯棒性，本文模型考慮了各個(gè)傳感器輸出存在野值問(wèn)題。當(dāng)某個(gè)輸出被判定為野值時(shí)，EKF 只進(jìn)行時(shí)間更新而不進(jìn)行量測(cè)更新。這種技術(shù)有效克服了水聲設(shè)備、磁羅盤等的異常量測(cè)噪聲對(duì)于協(xié)同導(dǎo)航系統(tǒng)的影響。數(shù)值仿真驗(yàn)證了本文算法的有效性，相對(duì)傳統(tǒng)的從AUV 自身進(jìn)行航位推算方法，本文提出的算法可以將誤差減小至1.27%（15.9m/ 1251.3m），而基于距離信息的常規(guī)協(xié)同導(dǎo)航算法只能將誤差減小至49.6%（621.1m/1251.3m）。