基于馬氏距離結合自適應濾波的協同定位方法

徐 博，王權達，李盛新，王連釗

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

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

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

卡爾曼濾波算法在導航領域的應用非常廣泛，協同導航領域也不例外，由于在協同定位系統中，系統模型和量測方程都是非線性的，因此需要使用非線性卡爾曼濾波算法進行協同定位。容積卡爾曼濾波（Cubature Kalman Filter, CKF）算法在處理非線性模型時相比擴展卡爾曼濾波算法和無跡卡爾曼濾波算法濾波精度更高，實時性更好[11-13]。CKF利用點估計方法，提高了擬合精度，同時也避免了數值不穩定現象。文獻[14]將容積卡爾曼濾波方法與無跡卡爾曼濾波進行對比，結果表明容積卡爾曼濾波算法精度更高、適用范圍更廣尤其針對高維非線性系統。

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

1 AUV協同定位系統描述

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

首先建立描述AUV協同導航系統的狀態方程和量測方程。AUV在運動時的俯仰角θ較小，可近似cosθ≈1，同時深度壓力傳感器可以提供水下航行器的深度信息，因此可將協同定位運動模型簡化為二維模型。

定義AUV在tk時刻的平面運動狀態為可以得到狀態空間模型為：

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

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

主AUV與從AUV都安裝了水聲調制解調器，CNA周期性的向從AUV發送自身的位置信息。CNA與從AUV之間的距離可以通過基于到達時間（TOA）定位方法來測量，定義CNA在tk時刻的運動狀態為CNA與從AUV之間的相對距離為：

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

由于深度信息可由壓力深度傳感器獲取，因此距離量測方程可簡化為：

式中，δk為量測噪聲。

基于式(1)和(9)建立多AUV協同定位系統的離散時間狀態空間模型：

式中，f(Xk-1,vk,ωk,θk)和分別為狀態轉移函數和量測函數，定義為：

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

2 CKF算法

2.1 CKF算法

CKF濾波算法的基礎是Gaussian濾波。對于一般的非線性離散系統可以用以下方程表示：

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

對于非線性系統(13)，容積卡爾曼濾波算法的計算步驟如下：預測步驟為：

Step 1: 對協方差矩陣Pk-1/k-1采用Cholesky分解。

Step 2: 計算容積點(i= 1,2…m)。

其中m=2n。

Step 3: 計算預測狀態容積點。

Step 4: 估計預測狀態。

Step 5: 估計預測協方差。

更新步驟為：

Step 1: 預測協方差矩陣進行Cholesky分解。

Step 2: 計算容積點(i= 1,2…m)。

Step 3: 計算預測量測容積點。

Step 4: 估計預測量測。

Step 5: 估計新息協方差。

Step 6: 估計互協方差矩陣。

Step 7: 計算卡爾曼濾波增益。

Step 8: 估計狀態更新。

Step 9: 估計狀態協方差。

2.2 自適應CKF算法

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

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

由式(30)，當量測異常或系統出現突變現象時，zk和之間的絕對誤差會增大，導致Δe變大，由于矩陣之間不能直接進行大小的比較，因此我們用矩陣的跡來衡量矩陣的大小，當Δe的跡大于跡，就需要對預測誤差方差陣進行修正來保證濾波結果的準確性。

自協方差矩陣修正：

互協方差矩陣修正：

至此完成k時刻的濾波修正過程。

3 基于新息序列馬氏距離輔助自適應CKF

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

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

因此量測新息的馬氏距離為：

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

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

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

①μz=0

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

因此造如下卡方檢驗統計量：

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

若量測出現異常或存在系統突變時，量測新息的馬氏距離的平方就不再服從卡方分布，因此可以通過這種方法，利用假設檢驗原理對系統進行評估。

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

若Mk＞（n），判定系統有故障；

若Mk≤n），判定系統無故障。

將故障點標記為1，正常點標記為0，在處理正常點時，ACKF正常進行時間和量測更新。當檢測到傳感器故障時ACKF只進行時間更新不進行狀態更新，來提高濾波算法的精度。

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

步驟1 根據式(1)和式(4)建立多AUV協同定位狀態空間模型；

步驟2 用CKF進行時間更新和量測更新；

步驟3 根據式(33)構造故障檢測函數，利用新息序列馬氏距離故障檢測法對系統模型進行評估，并設定門限值；

步驟 4 根據式(30)構造自適應因子kη對協方差矩陣進行在線修正，提高CKF魯棒性；

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

步驟6 當量測正常時，ACKF正常進行時間和量測更新。當檢測到量測故障時，ACKF只進行時間更新不進行狀態更新。

4 實驗驗證

4.1 實驗情況描述

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

圖1 湖試試驗設備Fig.1 Lake test equipment

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

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

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

為了對比不同通信聲吶測距誤差下的協同定位性能，在本次湖試試驗的基礎上，增加了四種測距誤差下的協同定位性能，還對比了出現野值時對CKF算法定位性能的影響。

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

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

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

由圖3和表2可以得到，測距聲吶精度越低，協同定位性能越差，其中野值噪聲對協同定位性能影響尤其大，多AUV協同導航系統在航行時由于海洋復雜的環境，不可避免地會產生野值噪聲。因此解決野值噪聲對協同定位的影響是非常必要的。

在本次湖試試驗中主AUV裝配的聲學設備每5 s發送一次信號，每次最多只有一個主AUV與從AUV進行通信。從AUV與主AUV-1和主AUV-2之間的量測信息如圖4所示。

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

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

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

因此本文引入卡方檢驗來檢測系統的不確定程度。將置信概率為99%對應的卡方檢驗值作為閾值。經計算，本文的卡方檢驗值設為6.635 m。若量測誤差大于此閾值，則判定該量測為故障量測，并將其標記為故障點。若量測誤差小于此閾值，則判定該量測有效，并將其標記為正常點。本文將故障點賦值為1，正常點賦值為0，方便后續算法的實現。卡方故障檢驗閾值及結果如圖6所示。

圖6 卡方故障檢驗閾值及結果Fig.6 Chi-square fault test threshold and results

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

本文所設計的基于馬氏距離結合自適應容積卡爾曼濾波算法（MD-ACKF），利用故障檢測出來的結果，在處理正常點時，ACKF正常進行時間和量測更新。當處理故障點時，ACKF只進行時間更新不進行狀態更新，來提高濾波算法的精度，在短時間內ACKF只使用時間更新其誤差不會發散太大，當量測信息正常后，其定位誤差會繼續收斂。

在聲學測距異常條件下，基于馬氏距離輔助自適應容積卡爾曼濾波算法結果如圖7和圖8所示，圖7為不同定位方法估計得到的跟隨AUV軌跡對比圖。圖8為不同定位方法估計位置與基準位置的距離誤差。

圖7 從艇軌跡對比圖Fig.7 Comparison diagram from boat track

圖8 定位誤差對比圖Fig.8 Comparison diagram of positioning error

由圖7和圖8可以看出，只利用航位推算算法得到的從艇軌跡與真實軌跡誤差較大，且隨時間發散，這是因為從艇的航位推算是由低精度的多普勒計程儀和船用羅經提供的新息解算的，導致定位誤差會隨著時間累計，航位推算軌跡會隨著時間逐漸偏離從艇的真實位置，如果長時間只利用航位推算算法來進行導航定位，會導致軌跡漂移，產生嚴重后果。

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

5 結 論

本文針對雙艇交替領航協同導航系統傳感器出現故障的情況，提出了一種基于馬氏距離的自適應卡爾曼濾波算法，并通過湖試數據進行了仿真驗證。主要內容如下：

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

(2)在協同定位系統傳感器故障診斷基礎上，對故障進行了有效處理，并通過湖試數據進行了仿真實驗驗證。

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