基于馬氏距離結(jié)合自適應(yīng)濾波的協(xié)同定位方法

徐 博，王權(quán)達(dá)，李盛新，王連釗

（哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001）

我國(guó)是海洋大國(guó)，擁有四大海域，海洋面積遼闊。人類也從未停止過對(duì)海洋的探索，但是由于海洋環(huán)境復(fù)雜、條件惡劣，大約95%的水下區(qū)域都還沒有被探索。自主式水下機(jī)器人（Autonomous Underwater Vehicles, AUV）作為一種海上無人智能裝備，已廣泛應(yīng)用于海上各個(gè)領(lǐng)域，由于人類文明的深化，在海洋勘探開發(fā)活動(dòng)中，使用單AUV已經(jīng)不能滿足當(dāng)前的任務(wù)要求。因此，基于水聲通信網(wǎng)絡(luò)的多AUV的協(xié)同定位技術(shù)受到了更多的關(guān)注[1]。世界各國(guó)總體都在朝著多AUV、UUV的自主、智能協(xié)同作戰(zhàn)方向發(fā)展，但是各國(guó)偏重有所不同。美國(guó)從海洋數(shù)據(jù)搜集和海底探測(cè)向著潛艇協(xié)同作戰(zhàn)和協(xié)同反潛發(fā)展。葡萄牙近幾年也側(cè)重于反潛、跟蹤。英國(guó)和歐盟近幾年的項(xiàng)目側(cè)重于海洋采樣和數(shù)據(jù)搜集[2-5]。協(xié)同定位技術(shù)在未來會(huì)有更廣泛的應(yīng)用。

目前即使使用精度最高的慣性傳感器，導(dǎo)航參數(shù)也會(huì)產(chǎn)生漂移現(xiàn)象，從而導(dǎo)致定位誤差發(fā)散，最終引起較大的誤差。因此找到一種能夠抑制這一誤差的方法就尤為重要，如果將主從AUV水聲距離通信應(yīng)用于導(dǎo)航系統(tǒng)中，結(jié)合信息融合技術(shù)就可以抑制此類誤差，這種方法被稱為協(xié)同定位技術(shù)，這是一種水下定位技術(shù)，是針對(duì)水下航行器協(xié)同應(yīng)用而發(fā)展起來的[6,7]。在水下協(xié)同導(dǎo)航系統(tǒng)中，主AUV需裝配精度較高的導(dǎo)航系統(tǒng)，以便獲取高精度的導(dǎo)航信息，從AUV一般裝配低精度的慣性導(dǎo)航系統(tǒng)（Inertial Navigation System, INS），主AUV和從AUV通過水下聲學(xué)調(diào)制解調(diào)器進(jìn)行通信以此提高從AUV定位精度。AUV間利用水聲通信來共享彼此的導(dǎo)航信息，利用水聲測(cè)距獲取相對(duì)距離作為觀測(cè)量，然后利用數(shù)據(jù)融合技術(shù)來降低定位誤差[8]。由于多艇協(xié)同定位可以提高系統(tǒng)整體的定位能力，而且具有成本低、系統(tǒng)魯棒性好的優(yōu)點(diǎn)，可以極大地增強(qiáng)多AUV系統(tǒng)的任務(wù)執(zhí)行能力，具有重要的理論和應(yīng)用價(jià)值。水下環(huán)境是復(fù)雜且多變的，因此AUV在航行過程中會(huì)遇到很多不確定情況，導(dǎo)致量測(cè)信息出現(xiàn)野值或不更新等故障，這將會(huì)對(duì)協(xié)同導(dǎo)航系統(tǒng)定位精度帶來很大影響[9,10]。因此，設(shè)計(jì)能提高多AUV協(xié)同定位精度的故障診斷算法是至關(guān)重要的。

卡爾曼濾波算法在導(dǎo)航領(lǐng)域的應(yīng)用非常廣泛，協(xié)同導(dǎo)航領(lǐng)域也不例外，由于在協(xié)同定位系統(tǒng)中，系統(tǒng)模型和量測(cè)方程都是非線性的，因此需要使用非線性卡爾曼濾波算法進(jìn)行協(xié)同定位。容積卡爾曼濾波（Cubature Kalman Filter, CKF）算法在處理非線性模型時(shí)相比擴(kuò)展卡爾曼濾波算法和無跡卡爾曼濾波算法濾波精度更高，實(shí)時(shí)性更好[11-13]。CKF利用點(diǎn)估計(jì)方法，提高了擬合精度，同時(shí)也避免了數(shù)值不穩(wěn)定現(xiàn)象。文獻(xiàn)[14]將容積卡爾曼濾波方法與無跡卡爾曼濾波進(jìn)行對(duì)比，結(jié)果表明容積卡爾曼濾波算法精度更高、適用范圍更廣尤其針對(duì)高維非線性系統(tǒng)。

馬氏距離（Mahalanobis Distance, MD）是一種距離的度量，它獨(dú)立于量測(cè)尺度，不受量綱的影響，可以有效排除變量之間的相互影響[15,16]。本文使用濾波新息的馬氏距離作為量測(cè)異常的檢驗(yàn)統(tǒng)計(jì)量，判斷水聲傳感器是否發(fā)生故障。針對(duì)協(xié)同導(dǎo)航在噪聲統(tǒng)計(jì)特性不確定和傳感器出現(xiàn)故障時(shí)量測(cè)信息不更新的問題。文獻(xiàn)[17]利用誤差開窗擬合來解決量測(cè)異常問題，但這種方法不利于對(duì)數(shù)據(jù)進(jìn)行實(shí)時(shí)處理，數(shù)據(jù)存儲(chǔ)量較大。文獻(xiàn)[18]利用兩次卡方檢測(cè)來進(jìn)行故障診斷，在標(biāo)準(zhǔn)CKF、抗差濾波和自適應(yīng)濾波中進(jìn)行選擇。但它并不能處理系統(tǒng)同時(shí)存在量測(cè)異常和噪聲不確定的狀況。本文將利用新息序列馬氏距離對(duì)量測(cè)系統(tǒng)進(jìn)行評(píng)估，再利用自適應(yīng)容積卡爾曼濾波算法實(shí)現(xiàn)協(xié)同導(dǎo)航系統(tǒng)的實(shí)時(shí)數(shù)據(jù)融合，提出了基于新息序列馬氏距離結(jié)合自適應(yīng)容積卡爾曼濾波算法（MD-ACKF）。本文的主要貢獻(xiàn)是提高了在量測(cè)異常情況下多艇協(xié)同導(dǎo)航精度，減小了異常量測(cè)對(duì)協(xié)同導(dǎo)航精度的影響。

1 AUV協(xié)同定位系統(tǒng)描述

目前水下協(xié)同定位主要分為兩類方式，并行式和主從式，考慮到在保證精度的前提下盡量減小設(shè)備成本的問題，本文主要研究基于主-從編隊(duì)協(xié)同定位的方法。在主從式協(xié)同定位方法中，也分為單領(lǐng)航和多領(lǐng)航模式，考慮到可觀測(cè)性問題，如果在單領(lǐng)航模式中，主AUV與從AUV在特殊的航行路線下，會(huì)導(dǎo)致可觀測(cè)性不足，進(jìn)而導(dǎo)致協(xié)同定位系統(tǒng)失效，定位誤差發(fā)散，因此本文所研究的系統(tǒng)為雙AUV交替領(lǐng)航系統(tǒng)，來提高系統(tǒng)可觀測(cè)性。其中主AUV配備高精度的導(dǎo)航設(shè)備，該系統(tǒng)承擔(dān)通信和導(dǎo)航設(shè)備（Communication and Navigation Aid, CNA）的作用；從AUV只依靠多普勒速度計(jì)程儀、羅經(jīng)和深度傳感器提供的信息進(jìn)行航位推算。

首先建立描述AUV協(xié)同導(dǎo)航系統(tǒng)的狀態(tài)方程和量測(cè)方程。AUV在運(yùn)動(dòng)時(shí)的俯仰角θ較小，可近似cosθ≈1，同時(shí)深度壓力傳感器可以提供水下航行器的深度信息，因此可將協(xié)同定位運(yùn)動(dòng)模型簡(jiǎn)化為二維模型。

定義AUV在tk時(shí)刻的平面運(yùn)動(dòng)狀態(tài)為可以得到狀態(tài)空間模型為：

式中，λk表示tk時(shí)刻的經(jīng)度；Lk分別表示tk時(shí)刻的緯度；Δt為狀態(tài)采樣時(shí)間；vk為前向速度；ωk右舷速度；θk為AUV航向角，航向角為AUV的前向與北向的夾角；hk為從AUV的深度信息；為過程噪聲向量；RM,k和RN,k分別為子午線的曲率半徑和卯酉線的曲率半徑，定義為：

式中，Re=6378137 m，e=1/298.257。

主AUV與從AUV都安裝了水聲調(diào)制解調(diào)器，CNA周期性的向從AUV發(fā)送自身的位置信息。CNA與從AUV之間的距離可以通過基于到達(dá)時(shí)間（TOA）定位方法來測(cè)量，定義CNA在tk時(shí)刻的運(yùn)動(dòng)狀態(tài)為CNA與從AUV之間的相對(duì)距離為：

式中，rk為CNA與從AUV之間的相對(duì)距離；為CNA的深度；ak、bk、、定義如下：

由于深度信息可由壓力深度傳感器獲取，因此距離量測(cè)方程可簡(jiǎn)化為：

式中，δk為量測(cè)噪聲。

基于式(1)和(9)建立多AUV協(xié)同定位系統(tǒng)的離散時(shí)間狀態(tài)空間模型：

式中，f(Xk-1,vk,ωk,θk)和分別為狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)，定義為：

式中，wk-1～N(0,Qk-1)和是互不相關(guān)的高斯白噪聲，Qk-1和分別為過程噪聲協(xié)方差矩陣和測(cè)量噪聲方差。

2 CKF算法

2.1 CKF算法

CKF濾波算法的基礎(chǔ)是Gaussian濾波。對(duì)于一般的非線性離散系統(tǒng)可以用以下方程表示：

式中，xk∈Rn為n×1維狀態(tài)向量；zk∈Rm為mz×1維量測(cè)向量；f（·）和h（·）為已知函數(shù)，系統(tǒng)噪聲wk和測(cè)量噪聲vk均服從Gaussian分布N(0,Q),并且能由均值和方差完全表征。

對(duì)于非線性系統(tǒng)(13)，容積卡爾曼濾波算法的計(jì)算步驟如下：預(yù)測(cè)步驟為：

Step 1: 對(duì)協(xié)方差矩陣Pk-1/k-1采用Cholesky分解。

Step 2: 計(jì)算容積點(diǎn)(i= 1,2…m)。

其中m=2n。

Step 3: 計(jì)算預(yù)測(cè)狀態(tài)容積點(diǎn)。

Step 4: 估計(jì)預(yù)測(cè)狀態(tài)。

Step 5: 估計(jì)預(yù)測(cè)協(xié)方差。

更新步驟為：

Step 1: 預(yù)測(cè)協(xié)方差矩陣進(jìn)行Cholesky分解。

Step 2: 計(jì)算容積點(diǎn)(i= 1,2…m)。

Step 3: 計(jì)算預(yù)測(cè)量測(cè)容積點(diǎn)。

Step 4: 估計(jì)預(yù)測(cè)量測(cè)。

Step 5: 估計(jì)新息協(xié)方差。

Step 6: 估計(jì)互協(xié)方差矩陣。

Step 7: 計(jì)算卡爾曼濾波增益。

Step 8: 估計(jì)狀態(tài)更新。

Step 9: 估計(jì)狀態(tài)協(xié)方差。

2.2 自適應(yīng)CKF算法

為進(jìn)一步提升CKF算法濾波性能以應(yīng)對(duì)系統(tǒng)波動(dòng)較大等情況，提高算法的自適應(yīng)與魯棒性，將CKF算法進(jìn)行改進(jìn)。由CKF算法原理可知，影響濾波精度的參數(shù)主要是卡爾曼增益Kk+1，而協(xié)方差矩陣與互協(xié)方差矩陣可以決定Kk+1的大小。因此本文構(gòu)造了可以對(duì)協(xié)方差矩陣進(jìn)行實(shí)時(shí)修正的自適應(yīng)因子，實(shí)現(xiàn)方法如下：

容積卡爾曼濾波基礎(chǔ)算法不做改變，通過定義新息向量式(28)以及新息向量矩陣式(29)，判斷Δek與自協(xié)方差矩陣跡的大小來構(gòu)造自適應(yīng)因子。具體實(shí)現(xiàn)如下：

由式(30)，當(dāng)量測(cè)異?；蛳到y(tǒng)出現(xiàn)突變現(xiàn)象時(shí)，zk和之間的絕對(duì)誤差會(huì)增大，導(dǎo)致Δe變大，由于矩陣之間不能直接進(jìn)行大小的比較，因此我們用矩陣的跡來衡量矩陣的大小，當(dāng)Δe的跡大于跡，就需要對(duì)預(yù)測(cè)誤差方差陣進(jìn)行修正來保證濾波結(jié)果的準(zhǔn)確性。

自協(xié)方差矩陣修正：

互協(xié)方差矩陣修正：

至此完成k時(shí)刻的濾波修正過程。

3 基于新息序列馬氏距離輔助自適應(yīng)CKF

馬氏距離是一種很有效的計(jì)算樣本集相似度的方法，它由數(shù)據(jù)的各種特性決定，而不是只取決于單一的距離尺度。因此馬氏距離相對(duì)于歐式距離在某些數(shù)據(jù)的處理上有很大的優(yōu)勢(shì)。對(duì)于一個(gè)均值為μ=（μ1，μ2，μ3…μn）T，協(xié)方差矩陣為∑的多變量向量x=（x1，x2，x3…xn）T，其馬氏距離為：

本文中卡爾曼濾波器的量測(cè)新息可以表示為：

因此量測(cè)新息的馬氏距離為：

其中δzd為量測(cè)新息，μz為新息序列均值，為量測(cè)新息方差。

在本文中卡爾曼濾波器的量測(cè)為AUV間的相對(duì)距離，因此新息dzδ為一維向量，可以推出因此式(35)可以簡(jiǎn)化為：

卡爾曼濾波的正交性是指系統(tǒng)模型確定時(shí)，如果不存在系統(tǒng)模型誤差，卡爾曼濾波所輸出的量測(cè)新息序列為零均值高斯白噪聲。根據(jù)卡爾曼濾波的正交性原理可知：

①μz=0

由①和②可以推出量測(cè)新息的馬氏距離DM(δzd) ～N（ 0,1）。

因此造如下卡方檢驗(yàn)統(tǒng)計(jì)量：

式中Mk服從自由度為1的卡方分布。

若量測(cè)出現(xiàn)異?；虼嬖谙到y(tǒng)突變時(shí)，量測(cè)新息的馬氏距離的平方就不再服從卡方分布，因此可以通過這種方法，利用假設(shè)檢驗(yàn)原理對(duì)系統(tǒng)進(jìn)行評(píng)估。

判斷過程為：通過設(shè)置卡方檢驗(yàn)顯著性水平α，可得顯著性水平α下的馬氏距離的置信概率為P(Mk≤（n）) =1-α。此時(shí)系統(tǒng)故障檢測(cè)過程為：

若Mk＞（n），判定系統(tǒng)有故障；

若Mk≤n），判定系統(tǒng)無故障。

將故障點(diǎn)標(biāo)記為1，正常點(diǎn)標(biāo)記為0，在處理正常點(diǎn)時(shí)，ACKF正常進(jìn)行時(shí)間和量測(cè)更新。當(dāng)檢測(cè)到傳感器故障時(shí)ACKF只進(jìn)行時(shí)間更新不進(jìn)行狀態(tài)更新，來提高濾波算法的精度。

對(duì)于式(1)的非線性系統(tǒng)，基于馬氏距離輔助自適應(yīng)CKF算法主要計(jì)算過程如下：

步驟1 根據(jù)式(1)和式(4)建立多AUV協(xié)同定位狀態(tài)空間模型；

步驟2 用CKF進(jìn)行時(shí)間更新和量測(cè)更新；

步驟3 根據(jù)式(33)構(gòu)造故障檢測(cè)函數(shù)，利用新息序列馬氏距離故障檢測(cè)法對(duì)系統(tǒng)模型進(jìn)行評(píng)估，并設(shè)定門限值；

步驟 4 根據(jù)式(30)構(gòu)造自適應(yīng)因子kη對(duì)協(xié)方差矩陣進(jìn)行在線修正，提高CKF魯棒性；

步驟 5 根據(jù)第三部分介紹的故障檢測(cè)方法判斷量測(cè)是否異常。若量測(cè)異常則標(biāo)記為1，若量測(cè)正常則標(biāo)記為0；

步驟6 當(dāng)量測(cè)正常時(shí)，ACKF正常進(jìn)行時(shí)間和量測(cè)更新。當(dāng)檢測(cè)到量測(cè)故障時(shí)，ACKF只進(jìn)行時(shí)間更新不進(jìn)行狀態(tài)更新。

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)情況描述

為了驗(yàn)證基于馬氏距離輔助自適應(yīng)CKF協(xié)同定位方法的有效性，用湖上試驗(yàn)數(shù)據(jù)進(jìn)行仿真驗(yàn)證。實(shí)驗(yàn)使用三艘勘測(cè)船，其中兩艘船安裝高精度導(dǎo)航設(shè)備作為主AUV，另一艘安裝低精度導(dǎo)航設(shè)備作為從AUV。主AUV和從AUV都配備了聲學(xué)調(diào)制解調(diào)器ATM-885、多普勒速度計(jì)程儀和羅經(jīng)；此外，從AUV還配備了全球定位系統(tǒng)（Global Positioning System,GPS），來提供參考位置，湖試設(shè)備如圖1所示，所攜帶傳感器信息如表1所示。

圖1 湖試試驗(yàn)設(shè)備Fig.1 Lake test equipment

表1 傳感器的性能參數(shù)Tab.1 Performance parameters of sensors

主AUV和從AUV的航行軌跡如圖2所示。試驗(yàn)總時(shí)間為1700 s，采樣時(shí)間1 s，GPS提供系統(tǒng)初始狀態(tài)。在每一時(shí)刻，從AUV只與兩艘主AUV中的一艘進(jìn)行通信，從AUV通信周期為5 s，每個(gè)主AUV與從AUV的通信周期為10 s。以羅經(jīng)提供的從AUV航向信息為基準(zhǔn)航向，設(shè)置輸入航向漂移誤差為15°/h。初始狀態(tài)估計(jì)誤差協(xié)方差矩陣設(shè)為P0=diag[1m,1m]2，過程噪聲協(xié)方差矩陣為Qk=diag[1m,1 m]2，測(cè)量噪聲協(xié)方差為= [5m]2。

圖2 主與從AUV真實(shí)軌跡Fig.2 The trajectory of piloting and following AUV

為了對(duì)比不同通信聲吶測(cè)距誤差下的協(xié)同定位性能，在本次湖試試驗(yàn)的基礎(chǔ)上，增加了四種測(cè)距誤差下的協(xié)同定位性能，還對(duì)比了出現(xiàn)野值時(shí)對(duì)CKF算法定位性能的影響。

由于本次湖試試驗(yàn)主AUV-1、主AUV-2和從AUV都配備有GPS作為基準(zhǔn)，因此根據(jù)三個(gè)AUV的GPS數(shù)據(jù)所解算出來的距離，設(shè)置通信聲吶距離精度分別為3 m、5 m、10 m、20 m時(shí)的量測(cè)數(shù)據(jù)，同時(shí)在通信聲吶距離精度為5 m的量測(cè)數(shù)據(jù)上，增加本次湖試試驗(yàn)提取出來的野值點(diǎn)，并用CKF算法進(jìn)行仿真結(jié)果如圖3和表2所示。

表2 不同精度通信聲吶協(xié)同定位性能Tab.2 Cooperative positioning performance of communication sonar with different precision

圖3 不同精度通信聲吶下，定位誤差對(duì)比Fig.3 Comparison of position error under different precision communication sonar

由圖3和表2可以得到，測(cè)距聲吶精度越低，協(xié)同定位性能越差，其中野值噪聲對(duì)協(xié)同定位性能影響尤其大，多AUV協(xié)同導(dǎo)航系統(tǒng)在航行時(shí)由于海洋復(fù)雜的環(huán)境，不可避免地會(huì)產(chǎn)生野值噪聲。因此解決野值噪聲對(duì)協(xié)同定位的影響是非常必要的。

在本次湖試試驗(yàn)中主AUV裝配的聲學(xué)設(shè)備每5 s發(fā)送一次信號(hào)，每次最多只有一個(gè)主AUV與從AUV進(jìn)行通信。從AUV與主AUV-1和主AUV-2之間的量測(cè)信息如圖4所示。

圖4 主AUV與從AUV之間的量測(cè)距離Fig.4 Measurement distance between master AUV and slave AUV

由圖4可以注意到，主AUV與從AUV之間的量測(cè)信息含有大量野值噪聲，如果直接使用原始數(shù)據(jù)作為濾波的量測(cè)信息，相當(dāng)于濾波器用錯(cuò)誤的量測(cè)信息進(jìn)行估計(jì)，對(duì)協(xié)同定位精度的影響很大，可能會(huì)導(dǎo)致位置估計(jì)誤差發(fā)散。主AUV-1與從AUV之間的量測(cè)誤差如圖5所示。在接下來的實(shí)驗(yàn)中，本文將使用上述數(shù)據(jù)進(jìn)行算法驗(yàn)證。通過圖5可以發(fā)現(xiàn)，野值噪聲往往偏差都很大，相比正常的量測(cè)數(shù)據(jù)，誤差要大幾十甚至上百倍，如果濾波器使用這種量測(cè)進(jìn)行量測(cè)更新，會(huì)對(duì)估計(jì)誤差產(chǎn)生很大影響。

圖5 主AUV與從AUV量測(cè)誤差及其概率分布圖Fig.5 Master AUV and slave AUV measurement errors and their probability distribution

因此本文引入卡方檢驗(yàn)來檢測(cè)系統(tǒng)的不確定程度。將置信概率為99%對(duì)應(yīng)的卡方檢驗(yàn)值作為閾值。經(jīng)計(jì)算，本文的卡方檢驗(yàn)值設(shè)為6.635 m。若量測(cè)誤差大于此閾值，則判定該量測(cè)為故障量測(cè)，并將其標(biāo)記為故障點(diǎn)。若量測(cè)誤差小于此閾值，則判定該量測(cè)有效，并將其標(biāo)記為正常點(diǎn)。本文將故障點(diǎn)賦值為1，正常點(diǎn)賦值為0，方便后續(xù)算法的實(shí)現(xiàn)?？ǚ焦收蠙z驗(yàn)閾值及結(jié)果如圖6所示。

圖6 卡方故障檢驗(yàn)閾值及結(jié)果Fig.6 Chi-square fault test threshold and results

本文將基于上述量測(cè)數(shù)據(jù)和卡方故檢測(cè)結(jié)果進(jìn)行相關(guān)算法的驗(yàn)證。首先在不進(jìn)行故障檢測(cè)時(shí)，只使用容積卡爾曼濾波算法與自適應(yīng)容積卡爾曼濾波算法對(duì)從AUV的位置進(jìn)行估計(jì)，從AUV軌跡和定位誤差如圖7和圖8所示，在野值噪聲較復(fù)雜的情況下，ACKF算法相比于CKF算法定位誤差更小，也符合理論。但是在量測(cè)信息出現(xiàn)不更新等緩變故障時(shí)，例如100～200 s、700～800 s、1580～1615 s時(shí)，ACKF的定位誤差也出現(xiàn)了明顯增大。

本文所設(shè)計(jì)的基于馬氏距離結(jié)合自適應(yīng)容積卡爾曼濾波算法（MD-ACKF），利用故障檢測(cè)出來的結(jié)果，在處理正常點(diǎn)時(shí)，ACKF正常進(jìn)行時(shí)間和量測(cè)更新。當(dāng)處理故障點(diǎn)時(shí)，ACKF只進(jìn)行時(shí)間更新不進(jìn)行狀態(tài)更新，來提高濾波算法的精度，在短時(shí)間內(nèi)ACKF只使用時(shí)間更新其誤差不會(huì)發(fā)散太大，當(dāng)量測(cè)信息正常后，其定位誤差會(huì)繼續(xù)收斂。

在聲學(xué)測(cè)距異常條件下，基于馬氏距離輔助自適應(yīng)容積卡爾曼濾波算法結(jié)果如圖7和圖8所示，圖7為不同定位方法估計(jì)得到的跟隨AUV軌跡對(duì)比圖。圖8為不同定位方法估計(jì)位置與基準(zhǔn)位置的距離誤差。

圖7 從艇軌跡對(duì)比圖Fig.7 Comparison diagram from boat track

圖8 定位誤差對(duì)比圖Fig.8 Comparison diagram of positioning error

由圖7和圖8可以看出，只利用航位推算算法得到的從艇軌跡與真實(shí)軌跡誤差較大，且隨時(shí)間發(fā)散，這是因?yàn)閺耐У暮轿煌扑闶怯傻途鹊亩嗥绽沼?jì)程儀和船用羅經(jīng)提供的新息解算的，導(dǎo)致定位誤差會(huì)隨著時(shí)間累計(jì)，航位推算軌跡會(huì)隨著時(shí)間逐漸偏離從艇的真實(shí)位置，如果長(zhǎng)時(shí)間只利用航位推算算法來進(jìn)行導(dǎo)航定位，會(huì)導(dǎo)致軌跡漂移，產(chǎn)生嚴(yán)重后果。

本文分別使用了CKF、ACKF、殘差卡方檢測(cè)、馬氏距離輔助容積卡爾曼濾波算法（MD-CKF）和馬氏距離輔助自適應(yīng)容積卡爾曼濾波算法（MD-ACKF）進(jìn)行對(duì)比。由圖7和圖8可以看到，在100～200 s、700～800 s和1580～1615 s時(shí)間段內(nèi)，量測(cè)出現(xiàn)嚴(yán)重異常時(shí)，如果不進(jìn)行處理，其定位誤差會(huì)明顯增大，導(dǎo)致整個(gè)定位系統(tǒng)發(fā)散，甚至嚴(yán)重后果。本文所使用的基于馬氏距離的自適應(yīng)卡爾曼濾波算法可以有效解決由于量測(cè)異常導(dǎo)致的定位誤差增大問題，對(duì)比多個(gè)算法結(jié)果表明，本文所提出的基于馬氏距離的自適應(yīng)卡爾曼濾波算法更穩(wěn)定，精度更高，定位誤差最大不超過15 m。

5 結(jié) 論

本文針對(duì)雙艇交替領(lǐng)航協(xié)同導(dǎo)航系統(tǒng)傳感器出現(xiàn)故障的情況，提出了一種基于馬氏距離的自適應(yīng)卡爾曼濾波算法，并通過湖試數(shù)據(jù)進(jìn)行了仿真驗(yàn)證。主要內(nèi)容如下：

(1)建立了多AUV協(xié)同定位算法的系統(tǒng)定位數(shù)學(xué)模型，提出了一種基于馬氏距離的自適應(yīng)卡爾曼濾波算法進(jìn)行傳感器故障診斷與處理，給出了該算法的詳細(xì)過程。

(2)在協(xié)同定位系統(tǒng)傳感器故障診斷基礎(chǔ)上，對(duì)故障進(jìn)行了有效處理，并通過湖試數(shù)據(jù)進(jìn)行了仿真實(shí)驗(yàn)驗(yàn)證。

本文提出的基于馬氏距離輔助的自適應(yīng)容積卡爾曼濾波算法，將該算法故障診斷與容錯(cuò)結(jié)合，可以保證在水聲設(shè)備出現(xiàn)故障情況下的多AUV協(xié)同定位精度，為多AUV協(xié)同定位系統(tǒng)的研究提供了思路。