徐凱凱，宋利飛，史曉騫，石正坤，孫 昊

（1.武漢理工大學(xué)高性能船舶技術(shù)教育部重點實驗室，武漢 430063；2.武漢理工大學(xué)船海與能源動力工程學(xué)院，武漢 430063）

1 引言

無人艇（Unmanned Surface Vessel，USV）在航行過程中，可由船舶自動識別系統(tǒng)（Automatic Identification System，AIS）獲取周圍部分船舶的準確狀態(tài)（位置、速度和航向），為避碰等運動方式提供信息[1-2]，然而所遇到的許多障礙并不都配備AIS，位置信息一般可由無人艇的雷達直接獲得[3-4]，而速度、方向和航跡等信息需要對雷達采樣數(shù)據(jù)進行多目標跟蹤算法處理后獲得[5]。

雷達測得無人艇周圍障礙物的距離和方位信息，慣性導(dǎo)航/全球定位（Inertial Navigation System/Global Positioning System，INS/GPS）組合導(dǎo)航測得無人艇的坐標和航向信息。無人艇在實際航行一段時間的雷達探測點如圖1所示，從中可以看出存在以下問題。

圖1 雷達探測Fig.1 Radar detection

（1）雷達和INS/GPS 組合導(dǎo)航采樣數(shù)據(jù)時鐘不同步。圖中雷達采樣點較為稀疏而INS/GPS 組合導(dǎo)航采樣點較為密集；

（2）雷達和INS/GPS 組合導(dǎo)航采樣數(shù)據(jù)參考坐標系不同，前者為極坐標系，后者為直角坐標系；

（3）雷達無法分辨出哪些探測點是真正的障礙或雜波，即使分辨出真正的障礙，由于涉及時間維度，也無法直接得到障礙物的航跡和數(shù)量；

（4）雷達的采樣數(shù)據(jù)是有誤差的，且無法直接探測到機動障礙的狀態(tài)。

大多數(shù)目標跟蹤算法利用仿真完成實驗，沒有將雷達和INS/GPS 組合導(dǎo)航采樣數(shù)據(jù)時鐘不同步問題考慮在內(nèi)，但可以利用插值的方法解決采樣時間不同步的問題。常用的插值方法為多項式插值，包括線性插值、牛頓插值和埃爾米特插值等[6]。其中，線性插值只需兩個周期的數(shù)據(jù)，計算步驟簡便，在相鄰樣本數(shù)據(jù)相差較小時即可滿足計算要求，而后兩種插值方法涉及到多個周期的數(shù)據(jù)，屬于多項式插值，計算過程較為復(fù)雜。

針對雷達和INS/GPS 組合導(dǎo)航采樣數(shù)據(jù)參考坐標系不同問題，需將極坐標系下的探測點轉(zhuǎn)換成無人艇所在的直角坐標系對應(yīng)的坐標[7]。吉林大學(xué)的王鵬宇等[8]研究了傳感器載體裝載有多個傳感器時，由于各個傳感器參考的坐標系不同，無法直接探測數(shù)據(jù)進行處理而需要進行轉(zhuǎn)換的問題。郭云飛等[9]為了減少坐標系轉(zhuǎn)換帶來的誤差，在極坐標下對目標建立目標跟蹤運動模型，通過模擬數(shù)據(jù)仿真驗證了目標跟蹤效果。一般雷達的探測范圍較小，不需要考慮地球表面因素；INS/GPS 組合導(dǎo)航采樣的經(jīng)緯度在選取正確參考系的情況下，可以最大限度地減小經(jīng)緯度轉(zhuǎn)平面坐標系的誤差，在此基礎(chǔ)上進行的極坐標系轉(zhuǎn)化會產(chǎn)生累計誤差，但只要保證在一定范圍內(nèi)滿足需求即可。針對雷達無法直接得到障礙物的航跡和數(shù)量的問題，可以利用關(guān)聯(lián)門，從時間維度上對探測點進行數(shù)據(jù)關(guān)聯(lián)，剔除多余探測點，獲得每個障礙在一段周期內(nèi)的探測點集合，即每個障礙的航跡。常用的關(guān)聯(lián)門有環(huán)形關(guān)聯(lián)門、橢圓關(guān)聯(lián)門和扇形關(guān)聯(lián)門[10]。環(huán)形關(guān)聯(lián)門所關(guān)聯(lián)的范圍較大，不容易漏掉探測點，后兩種關(guān)聯(lián)門則相反，在對目標運動能力掌握較少的情況下，容易漏掉探測點。航跡探測點數(shù)據(jù)關(guān)聯(lián)常用的方法有最近鄰算法、概率數(shù)據(jù)互聯(lián)算法、聯(lián)合概率數(shù)據(jù)互聯(lián)算法和多假設(shè)法等[11]。其中，最近鄰算法最為簡便，后3 種關(guān)聯(lián)算法計算方法都較為復(fù)雜，計算量大。天津大學(xué)的魏存?zhèn)サ萚12]考慮到雷達探測過程中的隨機誤差、信號處理、設(shè)備硬件等因素，對探測點中的孤立野值和離群點進行了剔除處理。湖南大學(xué)的黃晶等[13]基于最近鄰法對航跡下一時刻的雷達探測點進行篩選，根據(jù)關(guān)聯(lián)門的性質(zhì)去匹配航跡與探測點。國防科技大學(xué)的王樹亮等[14]考慮到了交叉目標的情況，提出一種概率數(shù)據(jù)關(guān)聯(lián)算法。Bae S H[15]提到在目標跟蹤過程中需要初始化和終止航跡，并剔除重合和錯誤的航跡。雖然以上文章介紹了航跡的生成方法，但是僅局限于關(guān)聯(lián)門的選擇或數(shù)據(jù)關(guān)聯(lián)方法的介紹，并沒有建立完整的航跡管理框架。

針對雷達無法直接探測到機動障礙狀態(tài)的問題，可以對航跡進行狀態(tài)估計，以獲得該障礙物相對準確的狀態(tài)信息[16]。運動軌跡狀態(tài)估計的方法有雙向擬合、回歸法等數(shù)學(xué)預(yù)測方法，卡爾曼濾波也較多地被用來預(yù)測運動物體的狀態(tài)信息[17]。西安電子科技大學(xué)的劉代等[18]針對雜波環(huán)境下的目標狀態(tài)估計問題，提出一種利用多普勒量測狀態(tài)估計算法，側(cè)重于目標數(shù)學(xué)運動模型的建立，將觀測目標運動模型的徑向速度線性化，提高了目標狀態(tài)精度和算法收斂速度，但該方法僅針對目標勻速情況。海軍航空工程學(xué)院的王國宏等[19]針對目標測量速度欺騙干擾情況，提出基于速度估計徑向投影和運動狀態(tài)技術(shù)延遲的狀態(tài)估計方法，在仿真實驗下有較好的效果。Tlig 等[20]基于卡爾曼濾波成功實現(xiàn)了目標跟蹤，但卡爾曼濾波需要準確選取目標運動模型，且部分參數(shù)經(jīng)驗性較強，模型和部分參數(shù)選取不當(dāng)，收斂速度大大降低。上述文章都可對目標進行狀態(tài)估計，但沒有提及狀態(tài)估計后的數(shù)據(jù)仍需一次時鐘對準，以便于后續(xù)無人艇避碰時的定時執(zhí)行。

上述相關(guān)文獻更多側(cè)重介紹數(shù)據(jù)關(guān)聯(lián)方法或狀態(tài)估計等多目標跟蹤中的部分步驟，較少對整個雷達多目標跟蹤流程中的細節(jié)進行介紹，對時鐘和空間對準、航跡管理、航跡來源和去向等步驟描述不夠詳細。本文做出的主要貢獻如下。

（1）對雷達和INS/GPS 組合導(dǎo)航的原始采樣數(shù)據(jù)進行多目標跟蹤，建立了一套完整的雷達多目標跟蹤算法處理流程。

（2）對該算法進行了雷達數(shù)字孿生仿真測試，與多項式擬合算法進行對比，驗證了本文算法的有效性和穩(wěn)定性。

2 多目標跟蹤方法

2.1 算法架構(gòu)

多目標跟蹤方法的算法架構(gòu)如圖2所示。由于硬件本身采集數(shù)據(jù)的性能、硬件之間的差異性以及系統(tǒng)各個層級之間的通信延遲等因素，需要對雷達和INS/GPS 組合導(dǎo)航的采樣數(shù)據(jù)進行數(shù)據(jù)預(yù)處理，即無效值剔除、時鐘對準和空間對準；在數(shù)據(jù)預(yù)處理后，可以在相同的時間和空間維度對障礙物數(shù)據(jù)進行航跡管理，基于關(guān)聯(lián)門對探測點進行數(shù)據(jù)關(guān)聯(lián)；在對航跡分類之后，需要對穩(wěn)定航跡進行狀態(tài)估計，以獲得機動障礙狀態(tài)，再進行二次時鐘對準，方便定時發(fā)送數(shù)據(jù)。

圖2 雷達多目標跟蹤算法流程圖Fig.2 Flow chart of radar multi-target tracking algorithm

2.2 數(shù)據(jù)預(yù)處理

無人艇在航行過程中需要在安全范圍內(nèi)提前感知到周圍障礙物以采取下一步行動。所以，只需要保留以無人艇為圓心，半徑為安全距離內(nèi)的雷達數(shù)據(jù)（如圖1 中藍色圓圈內(nèi)的探測點），并剔除采樣中的空值。

由于兩個傳感器在無人艇上放置的相對位置一般較近，忽略各自偏差，盡管各自的坐標系不同，但是可以認為其坐標系的原點是在同一個位置?？臻g轉(zhuǎn)化后的探測點坐標(x z,yz)計算如式（2）所示。

2.3 航跡管理

環(huán)形關(guān)聯(lián)門是以障礙航跡末端為中心建立一個由障礙最大和最小運動速度以及采樣時間間隔決定的360°環(huán)形區(qū)域（圖3 灰色區(qū)域），即障礙可能在下一時刻出現(xiàn)的位置。其內(nèi)徑和外徑滿足和分別為目標最小速度和最大速度，ΔT為采樣間隔。

在同一時刻下，落入關(guān)聯(lián)門的探測點往往不止1 個，可通過最近鄰法選擇下一個時刻的探測點，主要思想就是選擇目標中心點最近的探測點作為下一時刻的目標中心點。所以當(dāng)多個探測點同時落入關(guān)聯(lián)門內(nèi)時，選擇距離該末航跡點最近的一個探測點作為該機動障礙的下一時刻航跡點，如圖3所示，Z(k-1)的下一個航跡點是Z1(k)，而不是Z2(k)。

圖3 環(huán)形關(guān)聯(lián)門Fig.3 Annular association gate

為了方便航跡管理，把航跡分成起始航跡、暫時航跡和穩(wěn)定航跡3 種，其探測點數(shù)量特征、航跡來源和去向如表1所示。

表1 航跡特征、來源和去向Table 1 Track characteristics,source and destination

3 種航跡可能在一段周期的時間內(nèi)匹配不到新的探測點，這段周期稱為航跡剔除時間閾值，分別為T1、T2和T3；當(dāng)前時刻與每個航跡的末航跡點時間戳差值為ΔTE；令當(dāng)前時刻的探測點集合z(k)，上一時刻所有航跡末點集合為E。航跡管理涉及到空間和時間兩個維度、探測點數(shù)量、關(guān)聯(lián)門、數(shù)據(jù)關(guān)聯(lián)和航跡剔除時間閾值等多個變量，其流程圖如圖4所示。

圖4 航跡管理流程圖Fig.4 Flow chart of track management

2.4 雙向擬合狀態(tài)估計

基于多項式和粒子群算法（Particle Swarm Optimization，PSO）雙向擬合以消除采樣誤差，對障礙進行狀態(tài)估計。距離當(dāng)前時刻太久前的采樣數(shù)據(jù)對當(dāng)前目標狀態(tài)估計參考意義不大，探測點數(shù)量N= 4是同時獲得目標估計位置、速度和航向的最小周期數(shù)，如式（3）所示。

雷達的采樣誤差為r_error，每次采樣時間為Tp，采樣誤差在直角坐標系中體現(xiàn)為在x軸和y軸上的坐標誤差。雙向擬合誤差消除的主要思路是首先用多項式擬合的方法消除y向誤差并得到擬合曲線，然后用粒子群算法通過最優(yōu)化消除x向誤差。

2.4.1 多項式擬合消除y向誤差

本刊訊（本刊記者）日前，由湖南省期刊協(xié)會組織的“湖南省第八屆雙十佳期刊”“湖南省第二屆優(yōu)秀內(nèi)部資料”評選工作塵埃落定。本刊等16種期刊及20種內(nèi)部資料分獲殊榮。湖南省期刊協(xié)會表示，獲得“湖南省第八屆雙十佳期刊”的16種期刊，皆創(chuàng)刊3年以上，堅持正確的政治方向和出版導(dǎo)向，有較好的輿論引導(dǎo)力、市場競爭力和文化傳播力，編校質(zhì)量優(yōu)秀、裝幀設(shè)計精美、印裝質(zhì)量良好，在傳統(tǒng)媒體與新興媒體的融合發(fā)展、創(chuàng)新發(fā)展上有新思路和新成效，在切實履行“舉旗幟、聚民心、育新人、興文化、展形象” 使命任務(wù)中作出了積極貢獻。

首先利用多項式對采樣點(xi,yi)進行擬合，表示為

圖5 坐標系轉(zhuǎn)換Fig.5 Coordinate system conversion

采用式（6）將采樣點從坐標系xOy轉(zhuǎn)化為坐標系x′Oy′。

采用式（7）將采樣點從坐標系x′Oy′擬合后的曲線轉(zhuǎn)化為坐標系xOy。

2.4.2 粒子群算法修正擬合點消除x向誤差

通過多項式擬合，得到擬合之后4 個點，假定首尾采樣點x(k-3Tp)和x(k)的位置是正確的，對中間采樣點x(k-2Tp)和x(k-Tp)進行修正再次減小誤差，滿足以下條件：

（1）修正后的x(k-Tp)和x(k-2Tp)與修正前同樣滿足式（4），即保證目標軌跡是光滑的曲線。

（2）修正后的x(k-Tp)和x(k-2Tp)距修正前的距離小于r_error，即認為估計位置滿足雷達的誤差精度要求。

（3）修正后的x(k-Tp)和x(k-2Tp)應(yīng)滿足式（3）中加速度改變量 Δa(k)盡量小。

基于此，采用粒子群算法對x(k-Tp)和x(k-2Tp)的位置進行優(yōu)化求解。如圖6所示，將x(k-Tp)和x(k-2Tp)的橫坐標值作為二維粒子x(k-Tp)和x(k-2Tp)，并帶入對應(yīng)多項式曲線擬合公式得到y(tǒng)(k-Tp)和y(k-2Tp)，此時兩點分別為(xk-Tp,yk-Tp)和(xk-2Tp,yk-2Tp)，與真實值的距離集合為di={d k-1,dk-2}，則該距離對目標函數(shù)的影響可表示為罰函數(shù)H

圖6 粒子群算法消除x 向誤差Fig.6 PSO eliminate x-direction error

式中，i=t- 1,t- 2。

適應(yīng)度函數(shù)表示為使修正后的4 個采樣點的Δa最小，表示為

雙向擬合估計的計算流程如下：

Step2：按照式（6）對坐標系xOy的進行坐標旋轉(zhuǎn)，得到坐標系x′Oy′下的坐標，對進行多項式曲線擬合，得到擬合后的曲線函數(shù)，將′中的橫坐標代入，得到坐標，多項式曲線擬合前后的橫坐標是相同的。

Step3：在坐標系x′Oy′下，將兩個中間時刻的采樣點橫坐標作為粒子，式（9）為適應(yīng)度函數(shù)，采用 PSO 求解最優(yōu)，帶入得到，得到坐標系x′Oy′下的估計位置，并按照式（7）轉(zhuǎn)為坐標系xOy下的坐標，得到估計位置。此外，估計位置需要按照式（1）進行第二次時鐘對準。

3 仿真結(jié)果及分析

3.1 本文目標跟蹤算法仿真

以安裝雷達和INS/GPS 組合導(dǎo)航的無人艇所采集到數(shù)據(jù)集進行數(shù)字孿生仿真測試。以無人艇初始位置為原點，將所有時刻的障礙物相對位置轉(zhuǎn)化到對應(yīng)的直角坐標系內(nèi)。通過關(guān)聯(lián)門對探測點進行航跡分類。t=40.66s 時，如圖7（a）所示，有4 個孤立探測點，為起始航跡；3 條航跡的探測點個數(shù)還不到5 個，為暫時航跡；2 條航跡的探測點個數(shù)已超過5 個，為穩(wěn)定航跡。t=98.71s時，如圖7（b）所示，起始航跡、暫時航跡和部分穩(wěn)定航跡已經(jīng)被剔除，環(huán)境中的雜波已經(jīng)完全過濾，形成了5 條穩(wěn)定航跡。

圖7 航跡管理Fig.7 Track management

穩(wěn)定航跡形成時，便可運用雙向擬合算法對穩(wěn)定航跡進行狀態(tài)估計，以求得機動障礙相對準確的位置、速度和航向信息。從圖8（a）可以看出，原始航跡的采樣點比較曲折，航跡連線并不平滑，經(jīng)過雙向擬合算法后，其估計航跡連線平滑了，更符合機動障礙的運動規(guī)律。

圖8（b）和（c）分別是利用雙向擬合算法對原始航跡處理所得到的估計速度和航向。可以看出，速度估計趨勢較為平緩，而前若干個探測點的航向變化較大，原因在于，機動障礙初始速度較小，位置變化較小，待障礙速度提升之后，雙向擬合算法可以進行相對準確的估計，機動障礙的速度和航向在若干采樣周期之后，已經(jīng)趨于穩(wěn)定。

圖8 狀態(tài)估計Fig.8 State estimation

3.2 對比仿真分析

以航跡1 為例，將真實航跡、穩(wěn)定航跡、本文目標跟蹤航跡和多項式擬合航跡作對比，如圖9所示?？梢钥闯觯瑑煞N算法的軌跡都較為平滑，本文目標跟蹤航跡與真實航跡的重合度較高；多項式擬合算法與穩(wěn)定航跡的重合度較高，且易產(chǎn)生過擬合和欠擬合的情況。其原因在用本文目標跟蹤方法在狀態(tài)估計時，第一步擬合的采樣點較少，且第二步經(jīng)過一次粒子群算法修正。

圖9 航跡1 對比Fig.9 Track1 comparison

將本文目標跟蹤算法與多項式擬合算法進行性能對比，性能指標采用障礙空間位置均方根誤差（Root Mean Square Error,RMSE）和速度RMSE，計算公式如式（10）所示。

圖10 和圖11 分別是兩種估計算法的位置RMSE和速度RMSE變化曲線，可以看出本文目標跟蹤算法的RSME在前期有些不穩(wěn)定且高于多項式擬合算法，經(jīng)過5 個周期之后便逐漸下降并趨于平穩(wěn)；多項式擬合算法整體處于平穩(wěn)，但局部波動較大。通過對比，驗證了本文所提算法的有效性和穩(wěn)定性。

圖10 位置RMSE 曲線Fig.10 RMSE curve of position

圖11 速度RMSE 曲線Fig.11 RMSE curve of velocity

兩種算法的時間復(fù)雜度差別主要在于狀態(tài)估計所采用的方法不同，只考慮狀態(tài)估計步驟，忽略O(shè)(1)、矩陣轉(zhuǎn)置等較小的復(fù)雜度，本文目標跟蹤算法時間復(fù)雜度為O(8m+mn+glm2)，其中，m為觀測數(shù)據(jù)維數(shù)；n為狀態(tài)維數(shù)；g為粒子群算法中的種群規(guī)模；l為迭代次數(shù)。

多項式擬合算法的時間復(fù)雜度為O(8m+mn)。理論上本文目標跟蹤算法花費的時間更多，但仍需以實際時間為準。

如表2所示，與多項式擬合算法相比，本文目標跟蹤算法的位置RMSE 均值和速度RMSE 均值更小，而本文目標跟蹤算法平均每步運算時間與多項式擬合算法平均每步運算時間之間的差別很小，說明本文目標跟蹤算法在跟蹤精度方面具有一定的優(yōu)勢。

表2 兩種算法數(shù)據(jù)對比表Table 2 Data comparison table of two algorithms

4 結(jié)論

本文研究了基于雷達數(shù)字孿生的水上多目標跟蹤方法。以雷達和INS/GPS 組合導(dǎo)航采樣的數(shù)據(jù)為樣本，進行了預(yù)處理，使兩種傳感器的數(shù)據(jù)可在相同的時間和空間維度進行計算，并通過環(huán)形關(guān)聯(lián)門和最近鄰數(shù)據(jù)關(guān)聯(lián)實現(xiàn)了對航跡的分類，建立了航跡管理流程，最后運用雙向擬合算法對機動障礙的位置、速度和航向進行數(shù)字孿生仿真測試，結(jié)果優(yōu)于多項式擬合算法。

未來可從數(shù)據(jù)關(guān)聯(lián)、航跡管理和狀態(tài)估計等多方面對多目標跟蹤算法進行優(yōu)化，以應(yīng)對更加復(fù)雜的情況，比如交叉航跡分類，更快地識別障礙的出現(xiàn)和消失時間點，更加精確地估計機動障礙位置、速度和航向等。