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

（1. 湖南大學(xué) 機(jī)械與運(yùn)載工程學(xué)院 整車先進(jìn)設(shè)計(jì)制造技術(shù)全國重點(diǎn)實(shí)驗(yàn)室，湖南 長沙 410082；2. 湖南大學(xué)無錫智能控制研究院，江蘇 無錫 214115）

0 引言

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

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

但是目前大多數(shù)的目標(biāo)跟蹤都只是考慮了對(duì)二維平面中的機(jī)動(dòng)目標(biāo)進(jìn)行跟蹤。本文基于文獻(xiàn)［10］中的并行IMM算法進(jìn)行了改進(jìn)，且改進(jìn)算法中的模型集合選擇了CV模型、當(dāng)前統(tǒng)計(jì)（current statistical， CS）模型和自適應(yīng)轉(zhuǎn)彎速率的三維固定中心恒定速率和轉(zhuǎn)向速率（CSCTR）模型，提出了APIMM-UKF 算法，對(duì)三維空間中的機(jī)動(dòng)目標(biāo)進(jìn)行狀態(tài)預(yù)測(cè)；并通過數(shù)值仿真證明了本文所提算法兼顧了模型切換速度和預(yù)測(cè)精度。

1 目標(biāo)模型

機(jī)動(dòng)目標(biāo)模型描述了目標(biāo)狀態(tài)隨著時(shí)間變化的過程?；谀Ｐ偷念A(yù)測(cè)算法，如果模型與目標(biāo)當(dāng)前運(yùn)動(dòng)狀態(tài)越是匹配，預(yù)測(cè)的結(jié)果就越是精準(zhǔn)。因此，選擇目標(biāo)的運(yùn)動(dòng)模型十分重要。

1.1 運(yùn)動(dòng)模型

目標(biāo)運(yùn)動(dòng)模型的精度對(duì)跟蹤器的跟蹤精度至關(guān)重要，因?yàn)橐坏┘僭O(shè)模型預(yù)測(cè)的運(yùn)動(dòng)與目標(biāo)的實(shí)際運(yùn)動(dòng)不匹配，那么跟蹤器的性能就會(huì)惡化。

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

1.1.1 CV模型

CV模型的離散時(shí)間狀態(tài)方程一般式為

式中：Fk，CV——CV 模型的狀態(tài)轉(zhuǎn)移矩陣；Γk，CV——CV 模型的噪聲驅(qū)動(dòng)矩陣；ωk——過程噪聲，ωk=為x、y和z方向的過程噪聲，其是均值為零、方差為Qk的高斯白噪聲。

ωk是一個(gè)離散的白噪聲序列，其對(duì)狀態(tài)變量產(chǎn)生“最小”的影響，反映了因擾動(dòng)和其他因素導(dǎo)致不可預(yù)測(cè)的建模錯(cuò)誤。

假設(shè)采樣周期為T，則每個(gè)參數(shù)的具體值為

式中：FCV——CV 模型的狀態(tài)轉(zhuǎn)移矩陣子矩陣；ΓCV—— CV 模型的噪聲驅(qū)動(dòng)矩陣子矩陣。

1.1.2 CS模型

CS 模型本質(zhì)上是一個(gè)具有自適應(yīng)均值（非零均值）的加速度的Singer模型。其對(duì)目標(biāo)運(yùn)動(dòng)加速度的變化具有更好的敏感性，同時(shí)有更好的實(shí)時(shí)處理能力［11］。

CS模型的離散狀態(tài)方程和觀測(cè)方程如下：

式中：Γk，CS—— CS 模型的狀態(tài)轉(zhuǎn)移矩陣；Gk，CS——CS 模型的控制轉(zhuǎn)移矩陣；——控制向量平均值。

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

假設(shè)采樣周期為T，則：

式中：FCS—— CS 模型的狀態(tài)轉(zhuǎn)移矩陣子矩陣；Fk，CS—— CS 模型的狀態(tài)轉(zhuǎn)移矩陣。

CS 模型的控制轉(zhuǎn)移矩陣子矩陣如下：

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

1.1.3 CSCTR模型

CSCTR模型［12］假設(shè)目標(biāo)以一個(gè)恒定速度在固定平面上圍繞一個(gè)固定中心旋轉(zhuǎn)，同時(shí)利用目標(biāo)的加速度和速度向量定義了一個(gè)具有恒定轉(zhuǎn)彎速率的圓弧運(yùn)動(dòng)。對(duì)于恒定速率和轉(zhuǎn)向速率的運(yùn)動(dòng)目標(biāo)，其加速度和速度向量是正交的（即a·v= 0），那么可以得到其轉(zhuǎn)彎速率ω：

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

CSCTR模型的離散狀態(tài)方程和觀測(cè)方程與CV模型的相同。不同之處在于CSCTR模型的狀態(tài)轉(zhuǎn)移矩陣和噪聲驅(qū)動(dòng)矩陣的表達(dá)式與CV 模型的是不同的。假設(shè)采樣周期為T，則：

式中：FCSCTR——CSCTR 模型的狀態(tài)轉(zhuǎn)移矩陣的子矩陣；Fk，CSCTR——CSCTR模型的狀態(tài)轉(zhuǎn)移矩陣。

式中：ΓCSCTR——CSCTR 模型的噪聲驅(qū)動(dòng)矩陣的子矩陣；Γk，CSCTR——CSCTR模型的噪聲驅(qū)動(dòng)矩陣。

1.2 量測(cè)模型

目標(biāo)的量測(cè)模型為

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

h()轉(zhuǎn)換關(guān)系為

2 自適應(yīng)并行IMM-UKF算法

2.1 標(biāo)準(zhǔn)IMM-UKF算法

標(biāo)準(zhǔn)IMM-UKF算法由輸入交互、模型條件濾波、模型概率更新和估計(jì)融合4 個(gè)部分組成，算法流程如圖1 所示。下文詳細(xì)描述包含N個(gè)子運(yùn)動(dòng)模型的標(biāo)準(zhǔn)IMM-UKF 算法從（k-1）時(shí)刻到k時(shí)刻4 個(gè)步驟的遞推過程。

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

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

2.1.1 輸入交互

對(duì)模型條件初始化，或者根據(jù)前一時(shí)刻系統(tǒng)狀態(tài)估計(jì)和協(xié)方差估計(jì)來推測(cè)當(dāng)前時(shí)刻測(cè)量值并對(duì)模型重新初始化，得出模型當(dāng)前時(shí)刻各個(gè)UKF輸入的狀態(tài)向量和協(xié)方差矩陣。

模型概率預(yù)測(cè)計(jì)算公式如下：

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

混合權(quán)重計(jì)算公式如下：

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

混合狀態(tài)估計(jì)計(jì)算公式如下：

混合協(xié)方差計(jì)算公式如下：

式中：-1|k-1，i——（k-1）時(shí)刻的混合協(xié)方差矩陣；Pk-1|k-1，i——（k-1）時(shí)刻協(xié)方差矩陣的值；-1|k-1，j——（k-1）時(shí)刻模型j的混合狀態(tài)向量估計(jì)值。

2.1.2 模型并行濾波

利用交互作用器得出的混合估計(jì)和混合協(xié)方差，分別采用不同運(yùn)動(dòng)模型的UKF濾波器進(jìn)行濾波，得到k時(shí)刻狀態(tài)估計(jì)值和協(xié)方差估計(jì)值Pk|k，i。

2.1.3 模型概率更新

通過似然函數(shù)更新各個(gè)模型的概率，計(jì)算公式如下：

式中：μk，i——k時(shí)刻模型i為匹配模型的概率；Λk，i——模型i的似然概率；z?k，i——量測(cè)殘差；Sk，i——?dú)埐顓f(xié)方差，其可通過UKF濾波獲得。

2.1.4 估計(jì)融合

將模型并行濾波得出的k時(shí)刻目標(biāo)狀態(tài)向量的估計(jì)值和協(xié)方差矩陣與更新后的模型概率結(jié)合，得出總的融合估計(jì)。具體過程為

式中：——k時(shí)刻目標(biāo)狀態(tài)向量估計(jì)值。

式中：Pk|k——k時(shí)刻協(xié)方差估計(jì)值。

2.2 APIMM-UKF算法

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

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

轉(zhuǎn)移概率校正函數(shù)fj為

其中，

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

2.3 改進(jìn)的APIMM-UKF算法

為了更好地利用前時(shí)刻的目標(biāo)狀態(tài)信息，采用當(dāng)前時(shí)刻與上一時(shí)刻該模型的概率之比對(duì)算法中模型的轉(zhuǎn)移概率進(jìn)行實(shí)時(shí)修正，這樣轉(zhuǎn)移概率校正函數(shù)如下：

當(dāng)k時(shí)刻模型j的概率比上一時(shí)刻大，校正函數(shù)fj就會(huì)大于1，從而k時(shí)刻模型j的轉(zhuǎn)移概率就隨之增大；反之，fj小于1，k時(shí)刻模型j的轉(zhuǎn)移概率就隨之減小。

3 仿真實(shí)驗(yàn)

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

3 個(gè)模型初始概率均為1/3，CIMM 算法初始轉(zhuǎn)移概率矩陣為

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

測(cè)量噪聲標(biāo)準(zhǔn)差為[1 1 8 0.1]T，采樣時(shí)間為1 s，聲吶平臺(tái)位置為（0，0），目標(biāo)初始狀態(tài)向量為[30 0 0 -60 0 0 0 0 0]T。在CS 模型中機(jī)動(dòng)頻率最大和最小機(jī)動(dòng)加速度為a±m(xù)ax=±0.5 m/s2。分別以本文改進(jìn)的APIMM-UKF 算法和標(biāo)準(zhǔn)IMM-UKF 算法進(jìn)行200次蒙特卡羅仿真，得到圖 3～圖6。

圖3為不同時(shí)刻2種算法的距離均方根誤差，幾乎所有時(shí)刻本文的算法的誤差都比標(biāo)準(zhǔn)IMM算法的小。圖 4是2種算法預(yù)測(cè)的目標(biāo)在不同時(shí)刻的位置和真實(shí)值的比較，可以看出，本文算法的預(yù)測(cè)精度遠(yuǎn)遠(yuǎn)高于標(biāo)準(zhǔn)算法的。圖 5 和圖 6 分別是基于標(biāo)準(zhǔn)算法和本文算法的每個(gè)運(yùn)動(dòng)模型的概率變化曲線，對(duì)比這兩個(gè)圖可以看出，標(biāo)準(zhǔn)算法各模型概率都穩(wěn)定平緩，對(duì)目標(biāo)機(jī)動(dòng)反應(yīng)變化不夠明顯，這是由于標(biāo)準(zhǔn)算法沒有利用狀態(tài)估計(jì)的后驗(yàn)信息對(duì)概率轉(zhuǎn)移矩陣進(jìn)行修正。圖5 中，CV 模型的概率一直較高，在90%左右，而另外2 個(gè)模型只有5%左右，基本沒有實(shí)現(xiàn)很好的模型切換。而從圖6 中可以看出，本文算法在跟蹤目標(biāo)發(fā)生機(jī)動(dòng)運(yùn)動(dòng)狀態(tài)改變后，可以比較快地作出反應(yīng)，相應(yīng)地提高了與目標(biāo)運(yùn)動(dòng)狀態(tài)匹配的模型概率，同時(shí)抑制了其他不匹配模型的概率；并且當(dāng)目標(biāo)穩(wěn)定在一個(gè)運(yùn)動(dòng)狀態(tài)時(shí)，匹配模型的概率達(dá)到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 結(jié)束語

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