基于矢量對偶四元數的水下機動目標自適應跟蹤

侯翔昊，喬鋼，周建波，楊益新

(1. 哈爾濱工程大學 水聲技術重點實驗室，黑龍江 哈爾濱 150001；2. 海洋信息獲取與安全工業和信息化部重點實驗室(哈爾濱工程大學)，黑龍江 哈爾濱 150001；3.哈爾濱工程大學 水聲工程學院，黑龍江 哈爾濱 150001；4.西北工業大學 航海學院，陜西 西安 710072)

水下目標跟蹤技術是對特定海域進行實時監測的必要前提，對水下目標進行準確可靠的跟蹤是對其進行后續操作的基礎。然而傳統的水下目標跟蹤技術常常受制于目標運動模型不準確、水下環境干擾而導致的目標觀測信息失效以及跟蹤算法缺少容錯性等缺陷而不能提供良好的水下目標跟蹤精度。因此，研究高精度、高魯棒性的水下目標跟蹤算法已然成為水下目標跟蹤領域研究的熱點與難點問題[1-2]。

在復雜的水下環境中，水下目標可能進行多種機動，建立準確的水下機動目標運動學與動力學模型是對其進行跟蹤算法設計的基礎。在水下目標跟蹤領域，目前研究多集中在基于質點模型的水下目標運動學與動力學建模，即將水下目標視為質點，從而對其運動學與動力學過程進行數學描述。通常，采用不同的數學模型對水下目標的加速度進行表達，從而建立不同的水下目標運動學與動力學模型。這一類基于質點的模型主要有：勻速模型(CV模型)[3-4]、勻速轉彎模型(CT模型)以及變加速模型(Singer模型[5-7]、“當前”統計模型[8]、勻加速模型(CA模型)[9]、自適應高斯模型[10]等)等。對水下機動目標而言，為了將多種運動學與動力學模型統一描述，克服目標模型表述的不確定性，文獻[16]提出了交互式多模型對水下目標的運動學與動力學過程進行了統一描述，從而對水下機動目標進行有效跟蹤。然而，隨著目前水下機動目標運動復雜性的增強以及水下觀測精度的提升，將水下目標視為質點而對其進行描述的方法已經不能滿足對水下目標運動學與動力學準確建模的需求，將其視為質點進行建模將會忽略其質心與觀測位置之間的位移，同時也會忽略由水下環境干擾等因素引起的水下目標平動轉動耦合作用，當干擾較大時，可能會引起跟蹤發散的情況。因此，為了滿足當下對水下機動目標準確可靠跟蹤的需求，需要研究將水下目標視為剛體，同時將水下目標的加速度與角加速度作為待定跟蹤量的新型水下目標運動學與動力學建模方法。

由于水下環境復雜以及水下目標的運動學動力學模型與觀測模型具有較強的非線性，一般設計非線性濾波跟蹤算法對水下目標進行跟蹤。其中，擴展卡爾曼濾波(EKF)[11]是最為廣泛使用的非線性濾波方法。然而，由于EKF采用一階泰勒級數展開對非線性觀測模型進行線性化處理，可能會帶來較大線性化誤差。針對此不足，諸多采用不同線性化方法的貝葉斯濾波技術被提出并應用于目標跟蹤過程中，其中具有代表性的有無跡卡爾曼濾波(UKF)[12]、容積卡爾曼(CKF)濾波[13]和粒子濾波(PF)[14-15]。但不論是UKF、CKF，還是PF，理論上越強的線性化能力帶來的后果就是急劇增加的運算量與隨之而來的計算不確定性。尤其在水下環境復雜，干擾眾多的條件下，濾波算法的穩定性與可靠性需對跟蹤結果至關重要。因此，為了對水下機動目標進行準確可靠的跟蹤，設計具有自適應抗干擾能力的濾波跟蹤算法十分必要。

本文將水下機動目標視為剛體，考慮水下目標的平動轉動耦合效應，采用矢量對偶四元數對水下剛體目標進行六自由度一體化建模，避免傳統基于質點的模型對水下目標建模的局限性；同時將水下機動目標的加速度與角加速度作為狀態估計量進行跟蹤估計，從而突破IMM算法要求完備模型集的限制。利用水下三維距離觀測信息，建立目標觀測點與跟蹤平臺之間的距離觀測模型，并考慮水下復雜環境對觀測量的干擾，結合新息信息，提出用于水下剛體機動目標跟蹤的基于矢量對偶四元數的擴展卡爾曼濾波算法(DVQ-EKF)以及自適應擴展卡爾曼濾波算法(DVQ-AEKF)，以實現復雜水下環境情況下，對水下機動目標的準確可靠跟蹤。

1 基于矢量對偶四元數的水下機動目標運動學與動力學模型

1.1 矢量對偶四元數(DVQ)

四元數具有如下形式：

q=(q0,q1,q2,q3)=q0+q1i+q2j+q3k

(1)

式中：q0是標量部分；q1i+q2j+q3k是矢量部分。其共軛表示為：(q0,-q1,-q2,-q3)。

同時，四元數與實數域的映射關系表示為：[·]:Q4→R4。在這種映射定義下，四元數叉乘可以定義在實數域內：

[a×b]=[a×][b]

(2)

類似地，四元數內積可以定義為：

[ab]=[a]L[b]

(3)

其中:

并且:

由于四元數僅僅描述了目標的轉動過程，而忽略了對目標平動過程的表達，因此，單純采用四元數對目標的運動進行建模將忽略其平動過程，從而不能完成對目標的轉動平動一體化建模。為了解決這一問題，文獻[17]提出對偶四元數的概念，文獻[18]將對偶四元數用于空間目標的轉動平動耦合一體化運動學與動力學建模中。

對偶四元數有：

(4)

式中q、q′均為四元數，并且對偶運算符ε2=0，ε≠0。

矢量對偶四元數有：

(5)

1)矢量對偶四元數加法：

(6)

2)矢量對偶四元數乘法：

(7)

3)矢量對偶四元數共軛：

(8)

4)矢量對偶四元數交換：

(9)

其余有關矢量對偶四元數的運算可以參考文獻[18]。

1.2 水下機動目標模型綜述

圖1展示了水下機動目標的運動模型。{I}為慣性系，{B}為水下目標的本體系，坐標原點為目標質心。{B′}為水下目標觀測點坐標系，假設與{B}平行，之間有位移ρ。

圖1 水下機動目標模型Fig.1 Model of underwater maneuvering target

{B}相對于{I}的姿態四元數為：qB/I，{B}與{I}相對距離為rB/I，{B′}與{I}的距離為rm。同時，本文假設水下機動目標的線速度為v，角速度為ω。

1.3 基于矢量對偶四元數的水下機動目標運動學與動力學模型

由文獻[18]，描述水下機動目標的位置和姿態的矢量對偶四元數為：

(10)

描述水下機動目標的速度和角速度的矢量對偶四元數為：

(11)

基于式(10)、(11)，由文獻[18]，得到基于矢量對偶四元數的水下機動目標運動學方程為：

(12)

為了避免奇異，利用誤差四元數將式(12)改寫為：

(13)

當水下機動目標受控做機動運動時，基于矢量對偶四元數的水下機動目標的動力學方程為：

(14)

(15)

2 基于矢量對偶四元數的水下機動目標跟蹤算法：DVQ-EKF與DVQ-AEKF

2.1 觀測模型

由圖1可知，觀測點(比如潛艇殼體)與目標質心具有位移ρ，因此，觀測方程為：

(16)

式中：A(qB/I)為水下目標質心與跟蹤平臺之間的姿態轉移矩陣；v為觀測噪聲，滿足：

E(v)=0

(17)

(18)

2.2 DVQ-EKF設計

水下機動目標跟蹤系統的狀態為：

(19)

由于水下復雜環境所帶來的干擾，假設水下機動目標在運動過程中，所受干擾力矩對其加速度產的干擾為w，由水下復雜環境帶來的觀測干擾為v，并且假設w滿足：

E(w)=0

(20)

(21)

Xk+1=ΦkXk+Wk

(22)

式中Φk?I+FΔt，Δt為采樣時間。

F為狀態方程的雅可比矩陣，并且：

(23)

(24)

(25)

由式(16)，觀測方程的雅可比矩陣為：

(26)

其中：

(27)

結合式(17)、(18)、(20)～(27)，基于擴展卡爾曼濾波(EKF)的框架[19]，得到基于矢量對偶四元數的水下機動目標跟蹤算法(DVQ-EKF)。

2.3 水下機動目標跟蹤算法設計

考慮水下復雜環境可能對觀測信息造成干擾，從而影響DVQ-EKF中的測量更新精度，進一步導致跟蹤精度下降甚至發散的問題，需要設計具有自適應調節能力的水下機動目標跟蹤算法。

定義新息量為：

nk=Zk-h(Xk/k-1)

(28)

由式(28)，可得新息量的方差為：

(29)

當水下機動目標跟蹤系統正常工作，即跟蹤量滿足最優估計時，DVQ-EKF的濾波增益為：

(30)

為了對受擾觀測進行處理，需要通過計算nk，從而調節Kk對水下機動目標進行準確跟蹤。將系統觀測受擾的影響反映在觀測噪聲Rk上，由式(29)可得：

(31)

其中γk為觀測噪聲的比例因子矩陣，可表示為：

(32)

在[0,k]時刻中，有：

(33)

將式(33)代入式(32)可得：

(34)

假設觀測干擾均由水下環境波動導致，為了使γk的對角元素均大于等于1，定義修正的比例因子矩陣的第ii個對角元素為：

(35)

由式(35)可得，修正的觀測噪聲矩陣為：

(36)

修正后的卡爾曼濾波增益為：

(37)

可見，通過式(37)對DVQ-EKF的濾波增益進行修正，可以得到在干擾下較為準確的跟蹤量?；跀U展卡爾曼濾波(EKF)的框架[19]，將其中的卡爾曼增益與觀測噪聲矩陣替換為式(36)與式(37)，即可得到基于矢量對偶四元數的水下機動目標自適應跟蹤算法(DVQ-AEKF)。

2.4 觀測干擾分析

為了節約計算資源，提升跟蹤系統的跟蹤效率，對水下機動目標進行跟蹤時，需要在DVQ-EKF以及DVQ-AEKF兩者間進行合理切換。

定義系統的觀測失效判別量為：

采用假設檢驗的思想，設定2個假設條件如下：

H0：跟蹤系統不存在觀測受強干擾擾情況，跟蹤過程正常；

H1：跟蹤系統存在觀測受強干擾擾情況，跟蹤過程異常；

基于上述2個條件，觀測干擾分析步驟為：

1)給定臨界概率ξ(0<ξ<1)，即滿足：

為了更直觀的說明本文所設計的水下機動目標跟蹤算法的工作過程，其算法流程圖如圖2所示。

圖2 水下機動目標跟蹤系統算法流程Fig.2 Flow chart of the proposed tracking algorithm

3 實驗仿真與分析

并假設在[200,225] s，水下復雜環境對距離觀測量有隨機附加擾動，其強度為：

基于上述條件，對DVQ-EKF于DVQ-AEKF的性能進行仿真測試，對水下機動目標的質心位移，質心速度和質心位置的跟蹤結果如圖3～8所示。

表1 水下機動目標初始參數Table 1 Initial values of the target

圖3 DVQ-EKF與DVQ-AEKF跟蹤下的質心位移誤差Fig.3 Error of tracking distance by DVQ-EKF and DVQ-AEKF

圖4 DVQ-EKF和DVQ-AEKF跟蹤下的線速度誤差Fig.4 Error of tracking velocity by DVQ-AEKF and DVQ-EKF

通過圖3～8，可以看到采用DVQ-EKF或DVQ-AEKF均能在設定仿真環境下對水下機動目標進行跟蹤。在水下目標進行機動，并在觀測收到一定強度的擾動時，DVQ-EKF的質心位移的跟蹤精度小于25 m，DVQ-AEKF的質心位移的跟蹤精度小于10 m；DVQ-EKF的線速度估計誤差小于0.7 m/s，DVQ-AEKF的線速度估計誤差小于0.4 m/s。并且，通過本文提出的基于DVQ的建模方法，能夠對水下目標的相關尺寸(即目標質心與觀測點之間的位移，本文稱為質心位置)做出估計，從而彌補了傳統質點建模方法對該參數估計的盲區。通過仿真發現，DVQ-EKF的目標質心位置估計誤差在3 m左右;DVQ-AEKF的目標質心位置估計誤差在1 m左右。

圖5 DVQ-EKF和DVQ-AEKF跟蹤下的質心位置誤差Fig.5 Error of tracking location of the mass point by DVQ-EKF and DVQ-AEKF

4 結論

1)本文將用于航空航天領域的矢量對偶四元數建模方法用于水下機動目標建模過程，克服了傳統水下目標跟蹤過程中采用質點模型描述目標運動學動力學過程的誤差，并將CV與CA模型進行了一體化描述。

2)結合了自適應濾波的技術手段，通過對受擾觀測進行更新權重調整，從而對水下機動目標進行了有效跟蹤。

3)由于DVQ-AEKF采用了自適應調整跟蹤濾波算法增益的技術手段，使其能夠應對距離觀測量受擾較大的情況，從而具有更高的跟蹤精度與跟蹤穩定性。