胡振濤，楊詩博，胡玉梅，周 林，，金 勇，楊琳琳

（1.河南大學人工智能學院,河南鄭州 450046；2.西北工業(yè)大學自動化學院，陜西西安 710129）

1 引言

目標跟蹤技術(shù)被廣泛地應(yīng)用于協(xié)同定位、態(tài)勢評估、視頻監(jiān)控、智能交通、海洋探測等各個領(lǐng)域［1］.近年來，伴隨著實際目標跟蹤場景中探測空間范圍擴大、探測干擾因素增多等復(fù)雜情況不斷地出現(xiàn)，已有目標跟蹤算法性能難以滿足實際工程問題需求.由于狀態(tài)估計器設(shè)計的優(yōu)劣直接決定著目標跟蹤效果的好壞，其研究近年來也因此受到國內(nèi)外相關(guān)領(lǐng)域?qū)＜液蛯W者的廣泛關(guān)注［2］.考慮到在實際目標跟蹤系統(tǒng)中單個傳感器量測精度往往難以達到系統(tǒng)濾波要求，設(shè)計依據(jù)數(shù)據(jù)融合機理的多傳感器跟蹤系統(tǒng)能夠較好地解決單傳感器跟蹤系統(tǒng)濾波精度不足問題［3］.分布式融合即上述思想的一種典型實現(xiàn)方式，它將各傳感器量測送入局部濾波器分別進行狀態(tài)估計，并將局部狀態(tài)估計結(jié)果送入融合中心進行處理，從而獲得相對于利用單傳感器量測數(shù)據(jù)更好的目標狀態(tài)估計效果，并且該方式具有計算量較小和容錯率高的優(yōu)點［4］.文獻［5］提出了一種用于分布式傳感器網(wǎng)絡(luò)的一致性容積卡爾曼濾波器；文獻［6］提出了一類用于分布式傳感器網(wǎng)絡(luò)的一致性信息濾波器.上述濾波器都采用一致性方法處理各節(jié)點的目標狀態(tài)估計，進而得到跟蹤系統(tǒng)關(guān)于目標的狀態(tài)最優(yōu)估計.一致性方法能夠在傳感器節(jié)點數(shù)量較多的分布式傳感器網(wǎng)絡(luò)中取得較好的跟蹤效果，但在傳感器數(shù)量較少的分布式融合目標跟蹤系統(tǒng)中具有精度較低和計算量較大等缺點.相比一致性方法，協(xié)方差交叉（Covariance Intersection,CI)融合策略能夠?qū)崿F(xiàn)信息的反饋調(diào)節(jié)，即將融合中心獲取目標狀態(tài)融合估計值后反饋于各局部濾波器，具有更高的穩(wěn)定性，適合于傳感器數(shù)量較少的分布式融合目標跟蹤系統(tǒng)［7］.此外，考慮到天氣、溫濕度等環(huán)境因素的影響，系統(tǒng)過程噪聲時變且其統(tǒng)計量等先驗信息未知因素，以及外界干擾、傳感器硬件故障等原因造成量測出現(xiàn)隨機異常值等情況，若仍在傳統(tǒng)高斯假設(shè)下直接使用上述錯誤的噪聲統(tǒng)計量和異常量測值等信息估計系統(tǒng)狀態(tài)，則不可避免地導(dǎo)致融合估計結(jié)果變差，甚至引起濾波器發(fā)散現(xiàn)象［8］.

為解決過程噪聲時變且其統(tǒng)計量等先驗信息未知的問題，文獻［9］提出了一種基于變分貝葉斯（Variational Bayes，VB）方法的自適應(yīng)線性卡爾曼濾波器，利用Inverse-Wishart（IW）分布作為狀態(tài)一步預(yù)測協(xié)方差矩陣的先驗分布，通過定點迭代方法更新系統(tǒng)狀態(tài)和噪聲參數(shù)的近似后驗分布，自適應(yīng)地估計系統(tǒng)狀態(tài)和噪聲.針對量測噪聲時變未知問題，文獻［8］首次在變分貝葉斯框架下結(jié)合Inverse-Gamma 分布實現(xiàn)狀態(tài)空間模型系統(tǒng)中量測噪聲分布參數(shù)和系統(tǒng)狀態(tài)的聯(lián)合估計，并在此基礎(chǔ)上將其推廣應(yīng)用至非線性系統(tǒng)噪聲未知問題［10］.另外，考慮量測噪聲異常值出現(xiàn)以及環(huán)境越來越復(fù)雜等導(dǎo)致的量測噪聲具有非高斯重尾特性，造成傳統(tǒng)濾波器的估計性能變差的具體情況［11］.利用Student’s t 分布和Skew-t 分布等對量測異常值具有很好魯棒性［12］，文獻［13］針對線性系統(tǒng)出現(xiàn)的重尾系統(tǒng)噪聲和量測噪聲，利用Student’s t分布對狀態(tài)預(yù)測和量測似然概率密度函數(shù)建模，通過變分迭代更新目標狀態(tài)和系統(tǒng)噪聲參數(shù)的后驗分布.隨后在文獻［14］中提出了一種基于Gaussian-Student’s t 分布混合的線性濾波器，通過自適應(yīng)調(diào)節(jié)兩種分布的混合參數(shù)實現(xiàn)線性系統(tǒng)的魯棒估計.文獻［6］針對分布式傳感器網(wǎng)絡(luò)提出了非線性一致性信息濾波器，分別采用Student’s t分布和IW 分布對過程噪聲和量測噪聲建模，并采用高斯近似（Gaussian Approximation,GA）的方法對非線性后驗分布進行近似，但是高斯近似方法需要對多個分布進行加權(quán)擬合，特別是在傳感器網(wǎng)絡(luò)節(jié)點較多的情況下易導(dǎo)致計算復(fù)雜度的急劇增加.針對具有系統(tǒng)非線性、過程噪聲時變以及量測噪聲異常等特性的多傳感器目標跟蹤系統(tǒng)的狀態(tài)估計與融合問題，本文提出一種基于變分貝葉斯的分布式融合無跡卡爾曼魯棒濾波（Distributed Fusion Unscented Kalman Filter Based on Variational Bayes,DFUKF-VB）算法.

2 問題描述

考慮如下多傳感器非線性離散時間動態(tài)系統(tǒng)，其狀態(tài)空間模型與量測模型如下：

k表示離散時刻，xk為k時刻系統(tǒng)的狀態(tài)向量，zk，l為k時刻第l個傳感器的量測向量，f(·)表示狀態(tài)轉(zhuǎn)移方程，hl(·)表示第l個傳感器的非線性量測方程；wk為零均值時變的過程噪聲，其真實協(xié)方差矩陣Qk未知且時變；vk，l為第l個傳感器具有隨機異常值的量測噪聲，其期望協(xié)方差矩陣為Rk，l；假設(shè)初始狀態(tài)x0和任意時刻wk與vk，l均互不相關(guān).

針對上述多傳感器非線性系統(tǒng)狀態(tài)估計問題，選用無跡卡爾曼濾波（Unscented Kalman Filter,UKF）來處理系統(tǒng)非線性估計問題，不同于擴展卡爾曼濾波算法（Extended Kalman Filter,EKF）［15］，UKF 通過無跡變換處理均值和協(xié)方差矩陣的非線性傳遞，避免對非線性方程進行線性化處理，具有更高的估計精度［16］；接下來，每一個局部平臺選取UKF 濾波框架，結(jié)合VB 方法處理時變過程噪聲和異常量測噪聲，進而求得局部狀態(tài)估計值；最后，利用分布式融合架構(gòu)在融合中心對每一個局部狀態(tài)估計值進行融合處理得到最終目標狀態(tài)估計結(jié)果，并反饋其結(jié)果給各局部平臺.算法實現(xiàn)結(jié)構(gòu)如圖1所示.

圖1 DFUKF-VB算法結(jié)構(gòu)圖

3 狀態(tài)預(yù)測協(xié)方差矩陣和量測似然建模

3.1 狀態(tài)預(yù)測及其協(xié)方差矩陣的先驗建模

對式（1）和式（2）描述的多傳感器非線性系統(tǒng)，假設(shè)局部濾波器狀態(tài)一步預(yù)測概率密度函數(shù)服從高斯分布：

N(·；μ，Σ)表示均值為μ，方差為Σ的高斯分布.分別表示k時刻第l個局部濾波器的狀態(tài)一步預(yù)測以及對應(yīng)的協(xié)方差矩陣.基于UKF 濾波框架計算，考慮到過程噪聲協(xié)方差矩陣Qk未知且時變，因此需要求Pk|k-1，l后驗概率分布，而選取合適的共軛先驗分布可以保證后驗分布與先驗分布具有相同的函數(shù)形式.這里選擇IW 分布作為Pk|k-1，l的共軛先驗分布，服從IW 分布的n×n維隨機對稱正定矩陣A的概率密度函數(shù)可表示為

t為自由度參數(shù)，T為n×n維逆尺度矩陣，tr(·)表示矩陣的跡，Γn(·)表示一個n維的Gamma 函數(shù).狀態(tài)一步預(yù)測協(xié)方差矩陣的先驗分布可表示為

將式（7）代入式（6）得到

其中，τl≥0 為調(diào)諧參數(shù)和τl的選取依據(jù)實際應(yīng)用場景而定.綜上所述，由式（3）和式（5）可以求解狀態(tài)預(yù)測及其協(xié)方差矩陣的先驗分布，由式（7）和式（8）可求解狀態(tài)預(yù)測協(xié)方差矩陣的IW先驗分布參數(shù).

3.2 量測似然建模

考慮到在式（2）描述的量測方程中量測噪聲具有隨機異常值，呈現(xiàn)出非高斯重尾特性，選取Student’s t分布對各傳感器的量測噪聲建模：

St(vk，l；0，Rk，l，vl)表示均值為0，尺度矩陣為Rk，l，自由度參數(shù)為vl的Student’s t分布.由于在分布式融合目標跟蹤系統(tǒng)中各局部濾波器相互獨立地對目標狀態(tài)進行估計，所以在第l個局部濾波器中，量測似然概率密度函數(shù)滿足p(zk，l|xk)≈p(zk，l|xk，l)，結(jié)合式（2）和式（9），k時刻第l個局部濾波器的量測似然概率密度函數(shù)表示為

為處理Student’s t 分布的概率密度函數(shù)的閉合解難以求解問題，引入輔助隨機變量λk，l，量測似然概率密度函數(shù)可寫為如下積分形式：

G(·；α，β)表示Gamma分布的概率密度函數(shù)，α和β分別為形狀參數(shù)和逆尺度參數(shù).根據(jù)式（11），條件量測似然概率密度函數(shù)p(zk，l|xk，l，λk，l)最終可表示為如下形式：

綜上所述，由式（12）和式（13）可以得到量測似然概率密度函數(shù).

4 待估計變量的后驗分布迭代更新

4.1 聯(lián)合后驗概率密度函數(shù)的變分近似

為在各局部濾波器中估計系統(tǒng)狀態(tài)和噪聲參數(shù)，需要求解狀態(tài)和噪聲參數(shù)的聯(lián)合后驗概率密度函數(shù)p(xk，l，Pk|k-1，l，λk，l|z1：k，l).由于聯(lián)合后驗概率密度函數(shù)形式十分復(fù)雜，并且無法直接求得其解析解，因此論文采用平均場VB 近似的方法實現(xiàn)聯(lián)合后驗概率密度函數(shù)的近似解耦：

q(·)是后驗概率密度函數(shù)p(·)的近似分布.于是求解聯(lián)合后驗概率密度函數(shù)問題就轉(zhuǎn)化為尋找待估計變量的近似后驗概率密度函數(shù)q(xk，l)q(Pk|k-1，l)q(λk，l).

根據(jù)VB近似思想，通過最小化q(xk，l)q(Pk|k-1，l)q(λk，l)與p(xk，l，Pk|k-1，l，λk，l|z1：k，l)之間的Kullback-Leibler 散度（Kullback-Leibler Divergence，KLD）來尋找后驗概率密度函數(shù)的最優(yōu)近似：

依據(jù)VB準則，式（15）最優(yōu)近似解滿足如下方程［8］：

γl表示第l個局部濾波器待估計的變量集合，E[·]表示對括號內(nèi)的函數(shù)或變量求期望，log(·)表示對括號內(nèi)的函數(shù)或變量求對數(shù)，θl表示集合γl中的任意一個元素表示除θl外，集合γl中所有元素表示與θl有關(guān)的常數(shù)項.

4.2 狀態(tài)和噪聲參數(shù)的后驗分布迭代更新

根據(jù)式（16），利用定點迭代方法逐個更新待估計變量的近似后驗分布.根據(jù)狀態(tài)空間模型的條件獨立性，第l個局部濾波器變量的聯(lián)合概率密度函數(shù)p(γl，z1：k，l)表示為

將式（3）、式（5）、式（12）、式（13）和式（18）代入式（16）對各待估計變量進行迭代更新.更新步驟如下：

令θl=Pk|k-1，l，將θl和式（18）代入式（16），更新為IW分布：

5 分布式狀態(tài)估計的協(xié)方差交叉融合

對應(yīng)于圖1 中的融合中心模塊，基于CI 融合準則對各局部濾波器的目標狀態(tài)估計進行融合處理，并將融合結(jié)果反饋給局部濾波器.其步驟如下：

首先，融合中心接收到各局部濾波器的狀態(tài)估計結(jié)果后，按照CI融合準則計算得到狀態(tài)估計結(jié)果

式中：v是清潔機器人的直線行走速度；n是履帶驅(qū)動輪的轉(zhuǎn)速；r是履帶驅(qū)動輪的半徑；n電是驅(qū)動電動機的轉(zhuǎn)速；i是減速齒輪箱的減速比。

αk，l為反饋權(quán)重系數(shù)，它們隨著局部狀態(tài)估計協(xié)方差矩陣的變化而變化，且滿足：

其中，‖ · ‖F(xiàn)表示Frobenius范數(shù).

6 DFUKF-VB算法實現(xiàn)

綜合本文第3 節(jié)到第5 節(jié)的內(nèi)容，DFUKF-VB 算法實現(xiàn)步驟見算法1.

7 仿真結(jié)果與分析

仿真實驗場景設(shè)定為典型二維平面目標跟蹤，并與基于變分貝葉斯一致性無跡卡爾曼濾波器（Consistent Unscented Kalman Filter based on Variational Bayes,CUKF-VB）［6］，基于變分貝葉斯無跡卡爾曼濾波器（Unscented Kalman Filter based on Variational Bayes，UKF-VB）［13］，基于變分貝葉斯分布式融合擴展卡爾曼濾波器（Distributed Fusion Extended Kalman Filter based on Variational Bayes，DFEKF-VB）［15］，具有固定不變提議過程噪聲和量測噪聲的分布式融合無跡卡爾曼濾波器（Distributed Fusion Unscented Kalman Filter based on Fixed Noise，DFUKF-FN）以及具有正確噪聲統(tǒng)計信息的分布式融合無跡卡爾曼濾波器（Distributed Fusion Unscented Kalman Filter based on True Noise，DFUKF-TN）等5 種濾波方法結(jié)果進行比較，通過對比多個指標來驗證本文提出算法相較于現(xiàn)有方法的可行性與有效性.值得注意的是，由于DFUKF-TN 使用了正確的噪聲統(tǒng)計信息，所以它的濾波結(jié)果是最好的，將它看作標準參考算法，這里認為性能更接近于該濾波器性能的濾波器是一個更好的濾波器.算法仿真實驗采用MATLAB R2018a 軟件，計算機核心硬件具體配置：CPU Intel（R）Core（TM）i5-5200U，4 核，主頻3.20 GHz，最大睿頻2.7 GHz，8 GB內(nèi)存.

7.1 仿真模型和參數(shù)

假設(shè)目標在二維水平面內(nèi)做轉(zhuǎn)彎率未知且恒定的轉(zhuǎn)彎運動，目標的動力學模型表示為

采用位于水平面內(nèi)不同位置的三個雷達對目標進行量測，得到目標與雷達的距離rk，l和方位角θk，l

(xk，yk)表示目標在k時刻所在位置坐標，(xl，yl)表示第l個雷達所在位置坐標，仿真實驗中三個雷達的位置選取為(0 m，0 m)、(500 m，0 m)以及(0 m，500 m).量測噪聲vk，l具有隨機異常值，且量測異常概率均為10%時，量測噪聲異常值通過如下方法產(chǎn)生：

其中，Rk，l表示k時刻第l個傳感器的期望量測噪聲協(xié)方差矩陣，pro.0.9 表示有90%的概率不會出現(xiàn)異常量測噪聲，pro.0.1表示有10%的概率會出現(xiàn)異常量測噪聲.三個雷達具有不同大小的量測噪聲，其量測噪聲協(xié)方差矩陣Rk，l選取如下：

表1 仿真參數(shù)選取

7.2 評價指標

7.3 仿真結(jié)果和分析

圖2 展示了單次仿真各濾波器目標跟蹤軌跡.從圖2 中可以看到DFUKF-FN 的跟蹤軌跡多次較大幅度偏離真實軌跡，而UKF-VB、CUKF-VB、DFEKF-VB、DFUKF-TN 以及DFUKF-VB 跟蹤軌跡未發(fā)生大幅度偏離真實軌跡的情況.這是由于DFUKF-FN 未使用Student’s t 分布對量測似然概率密度函數(shù)建模，因此該濾波器對隨機出現(xiàn)的量測異常值十分敏感.而其他四種算法由于采用Student’s t 分布對量測似然概率密度函數(shù)建模，因此對隨機出現(xiàn)的量測異常值具有魯棒性.

圖2 單次仿真各濾波器估計軌跡

圖3給出了6種濾波算法對目標位置估計RMSE對比.由圖3可以看出，在仿真時刻達到第20 s時，所有濾波器都能夠穩(wěn)定收斂.為了更直觀地對比算法的精度，計算第20 s 到第100 s 時間內(nèi)6 種濾波算法對目標位置估計的ARMSE，通過計算得到DFEKF-VB 相比DFUKF-VB高出49.59%，這是由于EKF只適用于弱非線性白噪聲系統(tǒng)，造成DFEKF-VB 的濾波精度較低；UKF-VB 相比DFUKF-VB 高出18.60%，其原因在于DFUKF-VB采用CI融合策略對局部濾波器的狀態(tài)估計進行了加權(quán)融合；CUKF-VB相比DFUKF-VB高出9.54%，說明相比于一致性算法，CI 融合結(jié)構(gòu)更適于傳感器數(shù)量較少的非線性目標跟蹤系統(tǒng).

圖3 位置估計RMSE

單次仿真條件下各濾波器的平均運行時長如表2所示.

表2 單次仿真條件下各濾波器的平均運行時長

由表2 可以看出，單次仿真條件下CUKF-VB 平均運行時長比DFUKF-VB 多56.07%，結(jié)合圖3，說明在傳感器數(shù)量較少的多傳感器非線性目標跟蹤系統(tǒng)中，DFUKF-VB的估計精度和計算實時性均優(yōu)于CUKF-VB；DFUKF-VB 的平均運行時長比DFEKF-VB 和UKF-VB的平均運行時長分別多了37.81%和166.7%，結(jié)合圖3可以得到，相較于DFEKF-VB和UKF-VB，DFUKF-VB雖然在計算復(fù)雜度有所增加，但其估計精度明顯優(yōu)于前兩種算法.圖4 展示了不同VB 迭代次數(shù)下各濾波器對目標位置估計的ARMSE 變化情況.從圖4 中可以看出當VB 迭代次數(shù)Nm達到6 時，所有的濾波器都能夠穩(wěn)定收斂，可以根據(jù)這個結(jié)果來選擇合適的變分迭代次數(shù)，從而節(jié)省算法運行時間.在目標位置估計的ARMSE 曲線穩(wěn)定收斂之后，相同VB迭代次數(shù)情況下，DFUKF-VB對目標位置估計的ARMSE 更接近于DFUKF-TN，說明DFUKF-VB 估計精度優(yōu)于除了理想DFUKF-TN 以外的其他4種濾波算法.為了使對比結(jié)果更具有說服力，計算圖4 中Nm取6 到20 到之間所有濾波器對目標位置估計的ARMSE平均值，通過計算得到DFEKF-VB、UKF-VB以及CUKF-VB 相比DFUKF-VB 分別高出39.85%，13.04%和10.15%.

圖4 不同變分迭代次數(shù)下位置估計ARMSE

圖5 給出了不同調(diào)諧參數(shù)下本文提出濾波器和CUKF-VB對目標位置估計的RMSE對比圖.從圖5中可以看出，當τl取值變化時，DFUKF-VB對目標位置估計的RMSE 在第20 s 時均能夠穩(wěn)定收斂，說明其具有較高的自適應(yīng)性，在調(diào)諧參數(shù)變化時仍能快速穩(wěn)定收斂.計算圖5 中第20 s 到第100 s 時間內(nèi)兩個濾波器對目標位置估計的ARMSE，通過計算得到：當τl取10，15，25 和50時，CUKF-VB 相比DFUKF-VB 分別分別高出9.54%，10.16%，10.41%和10.65%.

圖5 不同調(diào)諧參數(shù)下位置估計RMSE

圖6給出傳感器量測異常概率p對DFUKF-VB位置估計的RMSE影響.由圖中結(jié)果可看出當傳感器量測異常概率從0%增加到20%時，RMSE變化不明顯，其原因在于采用Student’s t 分布對量測似然概率密度函數(shù)建模，有效改善了量測異常對狀態(tài)估計精度不利影響；但當傳感器量測異常概率逐漸增加到30%時，RMSE增加明顯增加，這是因為量測異常值出現(xiàn)的更頻繁，尤其當量測異常值連續(xù)出現(xiàn)時，將對濾波器造成較大的影響.

圖6 不同異常概率位置估計RMSE

8 結(jié)論

本文提出了一種基于變分貝葉斯的分布式融合目標跟蹤算法.與傳統(tǒng)的分布式濾波算法相比，具有以下特點：首先，引入反饋式CI 融合策略，利用多傳感器量測分別實現(xiàn)局部濾波器的狀態(tài)估計，進而利用CI 融合方式得到狀態(tài)估計值，并將其反饋于所有局部濾波器，實現(xiàn)對局部濾波結(jié)果的優(yōu)化.有效解決因單傳感器量測估計精度不足的問題，并且避免一致性算法的計算量大的問題.其次，在UKF 框架中引入VB 方法，采用參數(shù)化IW 分布作為狀態(tài)預(yù)測協(xié)方差矩陣的共軛先驗分布和Student’s t分布對量測似然概率密度函數(shù)建模，改善非線性系統(tǒng)估計中出現(xiàn)的過程噪聲時變和量測噪聲隨機異常值的影響，并在VB 框架下實現(xiàn)了狀態(tài)與噪聲統(tǒng)計特性的聯(lián)合估計.再有，針對相互耦合待估計變量的聯(lián)合概率密度函數(shù)采用平均場VB 進行近似解耦，并對解耦后的各邊緣變分分布參數(shù)采用定點迭代的方法進行更新，同時通過控制迭代次數(shù)平衡估計濾波精度和實時性.