基于自適應交互多模型算法的三維空間水下動態目標跟蹤

秦洪懋，葉宏偉，崔慶佳，徐 彪，胡滿江

（1. 湖南大學 機械與運載工程學院 整車先進設計制造技術全國重點實驗室，湖南 長沙 410082；2. 湖南大學無錫智能控制研究院，江蘇 無錫 214115）

0 引言

自主水下機器人（autonomous underwater vehicle，AUV）作為高科技水下平臺，被廣泛應用于海洋資源探測、目標探測和目標偵察等民用和軍用領域，目前已成為各國競相發展的重要海洋裝備。其中，AUV 動態目標跟蹤技術是實現目標探測、目標偵察等任務的核心技術之一。AUV 利用其配置的攝像頭、聲吶等傳感器探測水中的目標，通過算法處理獲取目標物觀測信息，從而得到目標的運動狀態信息，進而針對目標物運動狀態執行相應的軌跡跟蹤、姿態調整等控制指令。而目標物運動狀態的精準預測是動態目標跟蹤控制技術的基礎，其核心是基于傳感器測量數據對目標物的位置、速度等狀態信息進行預測。

目標跟蹤的傳統方法是采用單一的運動模型，如恒定速度（constant velocity， CV）模型、恒定加速度（constant acceleration， CA）模型等，這些模型利用卡爾曼濾波、擴展卡爾曼濾波等算法進行求解。近些年來，一些更加準確的模型開始被引入，并且結合各種優化的濾波算法，從某個方向上也使跟蹤精度得到了提高。但是當跟蹤的目標發生機動行為時，使用單一模型的算法會出現模型失配情況，從而會造成目標跟蹤精度的大幅降低。針對目標發生機動運動或者系統模型不確定的問題，目前較為成熟的解決方法是采用交互式多模型（interacting multiple model， IMM）［1-2］，其最直觀的思想就是如果任意一個模型都不能描述出目標的所有運動狀態，那么就用多個模型并行對目標狀態進行估計，然后融合結果，通過馬爾可夫轉移概率矩陣實現模型間的轉換，因此具有自適應跟蹤能力。IMM算法受所使用的濾波方法的影響。無跡卡爾曼濾波器（unscented Kalman filter， UKF）［3］從統計學角度出發，采用無跡變換得到Sigma 點來對非線性系統進行近似。擴展卡爾曼濾波器（extend Kalman filter， EKF） 通過泰勒展開來完成局部線性化，與之相比UKF則擁有更高的精度和穩定性，而所需的計算量又遠遠比粒子濾波器的?。?］。因此選用UKF 構成的IMM-UKF 算法在目標跟蹤領域得到了廣泛的應用。但是標準IMMUKF算法的模型間切換存在滯后性，往往在目標發生機動后一段時間算法才能切換為匹配模型進行跟蹤［5］。如果可以根據當前系統模型信息自適應地修改轉移概率，IMM 算法的性能將得到充分的提高。許多研究人員在該領域進行了卓有成效的探索。在文獻［6］中，誤差壓縮率由基于模型的狀態估計與交互步驟之后狀態估計的比率定義。該誤差壓縮率有助于實現轉移概率的適應性，從而提高IMM 算法的估計精度。然而，這種方法僅適用于雙模型系統。文獻［7］概括了文獻［6］中得出的結論，所提出的方法不再局限于雙模型系統。然而，如果系統模型不跳躍，則會增加局部誤差。在文獻［8］中，轉移概率根據模型概率修正。然而，由于模型概率包含過去模型的信息，該算法在響應模型跳轉時存在嚴重的滯后。在文獻［9］中，轉移概率根據基于模型的似然函數的比率修正，這種方法可以實現轉移概率的自適應。然而，由于比率不受限制，轉移概率矩陣（transition probability matrix， TPM） 中的主對角元素將不再占主導地位，因此算法的性能嚴重下降。文獻［10］提出了一種基于自適應轉移概率矩陣的并行IMM算法，其將模型跳躍閾值引入轉移概率校正函數中，將過去模型信息的影響最小化，減少了模型跳躍過程中的響應時間并減小了估計誤差，保持了抗噪聲能力。

但是目前大多數的目標跟蹤都只是考慮了對二維平面中的機動目標進行跟蹤。本文基于文獻［10］中的并行IMM算法進行了改進，且改進算法中的模型集合選擇了CV模型、當前統計（current statistical， CS）模型和自適應轉彎速率的三維固定中心恒定速率和轉向速率（CSCTR）模型，提出了APIMM-UKF 算法，對三維空間中的機動目標進行狀態預測；并通過數值仿真證明了本文所提算法兼顧了模型切換速度和預測精度。

1 目標模型

機動目標模型描述了目標狀態隨著時間變化的過程?；谀Ｐ偷念A測算法，如果模型與目標當前運動狀態越是匹配，預測的結果就越是精準。因此，選擇目標的運動模型十分重要。

1.1 運動模型

目標運動模型的精度對跟蹤器的跟蹤精度至關重要，因為一旦假設模型預測的運動與目標的實際運動不匹配，那么跟蹤器的性能就會惡化。

在三維空間對目標運動過程進行仿真，k時刻的目標狀態為X(k) =[xk，vxk，axk，yk，vyk，ayk，zk，vzk，azk]T，其分別包含了目標在x、y和z三個方向上的位置（xk，yk，zk）、速度（vxk，vyk，vzk）和加速度（axk，ayk，azk）信息?？紤]水下運動目標的特點，本文選擇CV 模型、CS模型和CSCTR模型對目標運動狀態進行建模。

1.1.1 CV模型

CV模型的離散時間狀態方程一般式為

式中：Fk，CV——CV 模型的狀態轉移矩陣；Γk，CV——CV 模型的噪聲驅動矩陣；ωk——過程噪聲，ωk=為x、y和z方向的過程噪聲，其是均值為零、方差為Qk的高斯白噪聲。

ωk是一個離散的白噪聲序列，其對狀態變量產生“最小”的影響，反映了因擾動和其他因素導致不可預測的建模錯誤。

假設采樣周期為T，則每個參數的具體值為

式中：FCV——CV 模型的狀態轉移矩陣子矩陣；ΓCV—— CV 模型的噪聲驅動矩陣子矩陣。

1.1.2 CS模型

CS 模型本質上是一個具有自適應均值（非零均值）的加速度的Singer模型。其對目標運動加速度的變化具有更好的敏感性，同時有更好的實時處理能力［11］。

CS模型的離散狀態方程和觀測方程如下：

式中：Γk，CS—— CS 模型的狀態轉移矩陣；Gk，CS——CS 模型的控制轉移矩陣；——控制向量平均值。

CS 模型的觀測方程與CV 模型的觀測方程相同。不同之處在于其將Gk，CS和添加到了狀態方程中。與CV模型相比，狀態轉移矩陣和過程噪聲的表達式也不一致。該模型的關鍵假設是當前加速度均值等于估計加速度向量， 即從觀測向量Z(k)到時刻k獲得的被視為。目標的機動頻率通常被設置為固定值α，目標最大和最小機動加速度通常被設置為固定值amax和a-max，當前加速度的方差為。

假設采樣周期為T，則：

式中：FCS—— CS 模型的狀態轉移矩陣子矩陣；Fk，CS—— CS 模型的狀態轉移矩陣。

CS 模型的控制轉移矩陣子矩陣如下：

CS 模型的過程噪聲協方差矩陣如下：

1.1.3 CSCTR模型

CSCTR模型［12］假設目標以一個恒定速度在固定平面上圍繞一個固定中心旋轉，同時利用目標的加速度和速度向量定義了一個具有恒定轉彎速率的圓弧運動。對于恒定速率和轉向速率的運動目標，其加速度和速度向量是正交的（即a·v= 0），那么可以得到其轉彎速率ω：

式中：a——加速度向量，a=[ax，ay，az]T；v——速度向量，v=[vx，vy，vz]T。

CSCTR模型的離散狀態方程和觀測方程與CV模型的相同。不同之處在于CSCTR模型的狀態轉移矩陣和噪聲驅動矩陣的表達式與CV 模型的是不同的。假設采樣周期為T，則：

式中：FCSCTR——CSCTR 模型的狀態轉移矩陣的子矩陣；Fk，CSCTR——CSCTR模型的狀態轉移矩陣。

式中：ΓCSCTR——CSCTR 模型的噪聲驅動矩陣的子矩陣；Γk，CSCTR——CSCTR模型的噪聲驅動矩陣。

1.2 量測模型

目標的量測模型為

式中：Z(k)——k時刻目標狀態的觀測向量，Z(k) =其中，分別為方位角、俯仰角、距離和距離變化率；νk——觀測噪聲，其是均值為零、方差為Rk的高斯白噪聲；h()——狀態向量X(k)和觀測值Z(k)之間的轉換方程。

h()轉換關系為

2 自適應并行IMM-UKF算法

2.1 標準IMM-UKF算法

標準IMM-UKF算法由輸入交互、模型條件濾波、模型概率更新和估計融合4 個部分組成，算法流程如圖1 所示。下文詳細描述包含N個子運動模型的標準IMM-UKF 算法從（k-1）時刻到k時刻4 個步驟的遞推過程。

圖1 IMM-UKF 算法流程Fig. 1 Flow of IMM-UKF algorithm

圖2 APIMM-UKF 算法流程Fig. 2 Flow of APIMM-UKF algorithm

2.1.1 輸入交互

對模型條件初始化，或者根據前一時刻系統狀態估計和協方差估計來推測當前時刻測量值并對模型重新初始化，得出模型當前時刻各個UKF輸入的狀態向量和協方差矩陣。

模型概率預測計算公式如下：

式中：μk|k-1，i——（k-1）時刻預測k時刻模型i為匹配模型的概率；kji——轉移概率矩陣的元素，表示從模型j切換到模型i的概率；μ(k-1)，j——（k-1）時刻模型j為匹配模型的概率。

混合權重計算公式如下：

式中：μ(k-1)，j|i——（k-1）時刻模型j轉移到模型i的混合轉移權重。

混合狀態估計計算公式如下：

混合協方差計算公式如下：

式中：-1|k-1，i——（k-1）時刻的混合協方差矩陣；Pk-1|k-1，i——（k-1）時刻協方差矩陣的值；-1|k-1，j——（k-1）時刻模型j的混合狀態向量估計值。

2.1.2 模型并行濾波

利用交互作用器得出的混合估計和混合協方差，分別采用不同運動模型的UKF濾波器進行濾波，得到k時刻狀態估計值和協方差估計值Pk|k，i。

2.1.3 模型概率更新

通過似然函數更新各個模型的概率，計算公式如下：

式中：μk，i——k時刻模型i為匹配模型的概率；Λk，i——模型i的似然概率；z?k，i——量測殘差；Sk，i——殘差協方差，其可通過UKF濾波獲得。

2.1.4 估計融合

將模型并行濾波得出的k時刻目標狀態向量的估計值和協方差矩陣與更新后的模型概率結合，得出總的融合估計。具體過程為

式中：——k時刻目標狀態向量估計值。

式中：Pk|k——k時刻協方差估計值。

2.2 APIMM-UKF算法

模型交互是IMM 算法的標志性步驟，其中，根據TPM的指導，將基于模型的狀態估計及其協方差進行重組。這種重組使得IMM算法在不增加并行濾波器數量的情況下考慮了更多的模型跳轉信息，有效地提高了算法的估計精度。然而標準IMM 算法的TPM 是固定的，這使得算法的性能難以滿足需求。因此，利用系統當前信息來實現轉移概率的自適應調整，就能在目標發生機動后快速切換到相應的模型，從而提高算法精度［13］。

文獻［10］提出了一種自適應轉移概率矩陣的并行交互多模型（ATPM-PIMM，APIMM）算法，其由一個標準IMM（common IMM， CIMM）算法和一個自適應轉移概率的IMM并行組成，模型跳躍閾值Th用于識別模型是否跳躍，然后使用不同形式的校正函數對轉移概率進行校正。因此，APIMM-UKF 算法既能平滑噪聲，又能快速響應模型跳躍，最大限度地減小了狀態估計的誤差。APIMM-UKF算法流程如圖 2所示。

轉移概率校正函數fj為

其中，

式中：μj，C——模型j在CIMM 算法中的概率；μj，A——自適應IMM 算法中模型j的模型概率，這是由模型的過去信息糾正的；Λr——匹配模型的可能性函數；Λl——其他模型的可能性函數。

2.3 改進的APIMM-UKF算法

為了更好地利用前時刻的目標狀態信息，采用當前時刻與上一時刻該模型的概率之比對算法中模型的轉移概率進行實時修正，這樣轉移概率校正函數如下：

當k時刻模型j的概率比上一時刻大，校正函數fj就會大于1，從而k時刻模型j的轉移概率就隨之增大；反之，fj小于1，k時刻模型j的轉移概率就隨之減小。

3 仿真實驗

本文選用CV、CS和CSCTR這3個模型對在三維空間中運動的單目標跟蹤進行仿真：第一階段（0～20 s），做勻加速直線運動；第二階段（20～40 s），做勻速直線運動；第三階段（40～85 s），做角速度為的勻速右轉彎運動；第四階段（85～130 s），做勻速直線運動；第五階段（130～145 s），做垂直面向上勻速轉彎運動；第六階段（145～210 s），做勻速直線運動。

3 個模型初始概率均為1/3，CIMM 算法初始轉移概率矩陣為

APIMM 算法的模型跳躍閾值選擇0.8，初始轉移概率矩陣為

測量噪聲標準差為[1 1 8 0.1]T，采樣時間為1 s，聲吶平臺位置為（0，0），目標初始狀態向量為[30 0 0 -60 0 0 0 0 0]T。在CS 模型中機動頻率最大和最小機動加速度為a±max=±0.5 m/s2。分別以本文改進的APIMM-UKF 算法和標準IMM-UKF 算法進行200次蒙特卡羅仿真，得到圖 3～圖6。

圖3為不同時刻2種算法的距離均方根誤差，幾乎所有時刻本文的算法的誤差都比標準IMM算法的小。圖 4是2種算法預測的目標在不同時刻的位置和真實值的比較，可以看出，本文算法的預測精度遠遠高于標準算法的。圖 5 和圖 6 分別是基于標準算法和本文算法的每個運動模型的概率變化曲線，對比這兩個圖可以看出，標準算法各模型概率都穩定平緩，對目標機動反應變化不夠明顯，這是由于標準算法沒有利用狀態估計的后驗信息對概率轉移矩陣進行修正。圖5 中，CV 模型的概率一直較高，在90%左右，而另外2 個模型只有5%左右，基本沒有實現很好的模型切換。而從圖6 中可以看出，本文算法在跟蹤目標發生機動運動狀態改變后，可以比較快地作出反應，相應地提高了與目標運動狀態匹配的模型概率，同時抑制了其他不匹配模型的概率；并且當目標穩定在一個運動狀態時，匹配模型的概率達到90%以上。

圖3 距離均方根誤差Fig. 3 Distance root mean square error

圖4 跟蹤軌跡Fig. 4 Tracking trajectory

圖5 IMM-UKF 算法下各模型概率Fig. 5 Probability of models under the IMM-UKF algorithm

圖6 APIMM-UKF 算法下各模型概率Fig. 6 Probability of models under the APIMM-UKF algorithm

4 結束語

本文首先將現有的用于二維平面目標跟蹤的APIMM算法進行了改進；然后，結合三維的CV、CS和自適應轉彎速率CSCTR模型，用于三維空間中的機動目標跟蹤；最后，對水下三維空間運動目標進行跟蹤仿真實驗。結果顯示，本文改進的跟蹤算法能有效地提高與目標運動狀態相匹配的運動模型的概率，抑制了其他非匹配運動模型對跟蹤結果的影響，既能平滑噪聲，又能快速響應模型跳躍，減小了對目標狀態預測的誤差，在水下機動目標的跟蹤上具有很好的應用價值。在未來工作中，還可以對使用的目標運動模型進行改進，自適應參數使其更加匹配目標的運動狀態。