雷達(dá)和AIS是水上交通監(jiān)控里最常用也最容易“打架”的兩類信息來源。雷達(dá)每個掃描周期都能給出目標(biāo)的位置但不知道這條船是誰AIS每隔幾秒到幾分鐘報一次船名、航速、航向但目標(biāo)一旦關(guān)機(jī)或者報文延遲監(jiān)控畫面就會出現(xiàn)一條“隱身船”。把兩者的航跡融合成一條準(zhǔn)確、連續(xù)、帶身份的目標(biāo)軌跡是這個領(lǐng)域繞不開的核心工作。我最近用一個基于MATLAB的雷達(dá)與AIS航跡融合處理項(xiàng)目把從數(shù)據(jù)預(yù)處理、時空對齊、航跡關(guān)聯(lián)到融合濾波的完整鏈路跑通了一遍這里把思路、代碼和踩過的坑一起整理出來。內(nèi)容主要面向海事雷達(dá)工程師、做水上交通信息處理的學(xué)生以及正在寫多傳感器融合代碼的朋友如果你只想快速跑通一個Demo也可以直接照著核心邏輯改。1. 融合需求剖析與整體策略1.1 雷達(dá)與AIS的互補(bǔ)關(guān)系以及為什么非融合不可雷達(dá)是主動傳感器自己發(fā)射電磁波、接收回波好處是不依賴目標(biāo)配合任何有雷達(dá)反射面的目標(biāo)都能被探測到壞處也很明顯它只給“這里有個東西”不給“這是誰”。在近岸、彎曲航道、雨雪天氣下雷達(dá)點(diǎn)跡里還混著大量海雜波和假回波距離誤差隨距離增大而增大方位誤差在遠(yuǎn)區(qū)會被放大成好幾倍的橫向偏差。AIS則完全是另一套邏輯。它通過船載設(shè)備主動廣播本船的靜態(tài)信息船名、MMSI、船長船寬和動態(tài)信息經(jīng)緯度、對地航速、對地航向定位用的是GNSS位置精度通常在幾米到十幾米遠(yuǎn)遠(yuǎn)好于雷達(dá)。但它是“合作式”系統(tǒng)船上沒裝設(shè)備、設(shè)備關(guān)機(jī)或報文被干擾時就一點(diǎn)數(shù)據(jù)都沒有。而且AIS更新率隨航行狀態(tài)變化錨泊船可能幾分鐘才發(fā)一條船多了報文還可能排隊(duì)延遲。我實(shí)際調(diào)試時遇到過這種情況監(jiān)控畫面里一個雷達(dá)回波和一個AIS目標(biāo)相距約200米看起來是兩條船實(shí)際上雷達(dá)回波的位置噪聲偏大、AIS定位又很準(zhǔn)兩者指向的其實(shí)是同一艘船。如果這時候直接拿兩個位置做平均身份和位置都會一起漂掉。所以融合系統(tǒng)的第一件事不是算平均而是先回答“這兩個測量到底是不是同一個目標(biāo)”這個問題答錯了后面所有的濾波和加權(quán)都沒有意義。1.2 融合質(zhì)量的評價指標(biāo)融合做得好不好不能靠肉眼和感覺。我自己的習(xí)慣是先把評價指標(biāo)定下來再動手寫算法。做過這個方向的同行應(yīng)該都有同感沒有量化指標(biāo)調(diào)試時很難說清楚一個改動是變好了還是變壞了。指標(biāo)工程含義怎么衡量位置誤差融合航跡與真值的偏差與高精度定位或人工標(biāo)定真值比較取均方根誤差身份保持率航跡號和MMSI綁定的穩(wěn)定程度統(tǒng)計(jì)一段時間內(nèi)身份未跳變的軌跡占比軌跡完整率目標(biāo)可連續(xù)跟蹤的程度目標(biāo)全程未斷裂、未丟失的軌跡占比航向航速跳變率融合輸出的平滑程度相鄰兩次更新中航向或航速突變超過設(shè)定閾值的次數(shù)誤關(guān)聯(lián)率關(guān)聯(lián)算法張冠李戴的比例錯誤配對點(diǎn)跡數(shù)占總關(guān)聯(lián)次數(shù)的比例這些指標(biāo)不需要做得很重一個簡單的離線統(tǒng)計(jì)腳本就夠了。每次改動算法或參數(shù)后跑同一段回放數(shù)據(jù)對比這些數(shù)字的變化比憑感覺判斷可靠得多。2. 航跡關(guān)聯(lián)前的時空對齊處理2.1 坐標(biāo)統(tǒng)一把經(jīng)緯度與極坐標(biāo)放到同一平面上雷達(dá)原始輸出通常是極坐標(biāo)距離和方位角AIS輸出是WGS84經(jīng)緯度加對地速度、對地航向。要把兩者做關(guān)聯(lián)第一步是放到同一個平面坐標(biāo)系里。工程上最常用的公共參考系是以雷達(dá)站為原點(diǎn)的本地切平面直角坐標(biāo)也就是東向x、北向y的ENU坐標(biāo)系。小范圍水域幾十公里以內(nèi)可以用等距近似轉(zhuǎn)換公式很簡單function [x, y] ll2xy(lat, lon, lat0, lon0) % 簡化等距轉(zhuǎn)換適合幾十公里內(nèi)的監(jiān)控場景 R 6371000; % 地球近似半徑單位m x (lon - lon0) * pi/180 * R * cos(lat0*pi/180); y (lat - lat0) * pi/180 * R; end這段代碼夠用但要注意兩點(diǎn)。第一參考點(diǎn)lat0、lon0一定要選在監(jiān)控區(qū)域中心偏離太遠(yuǎn)時等距近似的誤差會明顯變大。第二更嚴(yán)謹(jǐn)?shù)膱鼍敖ㄗh直接用投影轉(zhuǎn)換把經(jīng)緯度投影到UTM坐標(biāo)MATLAB里用自帶函數(shù)創(chuàng)建投影對象后做正向轉(zhuǎn)換這樣在大范圍、高緯度區(qū)域也不會出問題。雷達(dá)極坐標(biāo)轉(zhuǎn)直角坐標(biāo)也要統(tǒng)一口徑x rho * sin(theta); % theta從正北順時針單位弧度 y rho * cos(theta);不同雷達(dá)設(shè)備輸出的方位角定義可能不一樣有的是從正北順時針有的是從正東逆時針轉(zhuǎn)坐標(biāo)前必須確認(rèn)設(shè)備手冊里的角度定義否則整個關(guān)聯(lián)都會錯位。2.2 時間同步用運(yùn)動模型把航跡外推到同一時刻AIS和雷達(dá)的采樣時刻幾乎不可能完全一致。雷達(dá)掃描周期通常是2到6秒AIS更新周期從幾秒到幾分鐘不等二者在同一時刻點(diǎn)上的概率很低。時間差小于一個閾值時可以直接忽略大于閾值就必須做外推。外推用最樸素的運(yùn)動模型就夠了假設(shè)目標(biāo)在短時間內(nèi)外推窗口內(nèi)保持勻速直線運(yùn)動function [x_est, y_est] extrapolate(x0, y0, v, cog, dt) % v為對地航速單位m/scog為對地航向單位弧度dt為外推時間差單位s x_est x0 v * sin(cog) * dt; y_est y0 v * cos(cog) * dt; end這里的方向定義要跟坐標(biāo)轉(zhuǎn)換保持一致。如果目標(biāo)航速接近零比如錨泊船直接沿用舊位置不要硬外推。外推時間越長位置誤差越大所以一般建議外推窗口不超過10到30秒。AIS的SOG和COG本身就是GNSS解算出來的平滑值短時間外推的精度還可以雷達(dá)點(diǎn)跡如果也要做時間對齊它的速度估計(jì)噪聲大外推時要加約束防止位置被推到離譜的地方。時間對齊做完后每個待關(guān)聯(lián)的點(diǎn)都必須打上“測量時間戳”和“外推后的位置”后續(xù)關(guān)聯(lián)用的是外推后的位置但原始測量值要保留下來方便排查問題。3. 航跡關(guān)聯(lián)判定與融合濾波實(shí)現(xiàn)3.1 關(guān)聯(lián)門限與最近鄰分配關(guān)聯(lián)判定的核心邏輯是在公共坐標(biāo)系下計(jì)算雷達(dá)航跡點(diǎn)和AIS航跡點(diǎn)之間的距離、速度差、航向差然后判斷它們是否屬于同一個目標(biāo)。位置門限可以用誤差傳播的思路來定。設(shè)雷達(dá)測距誤差標(biāo)準(zhǔn)差為σ_RAIS定位誤差標(biāo)準(zhǔn)差為σ_A那么兩者差的方差是兩者方差之和取3倍標(biāo)準(zhǔn)差作為門限覆蓋大約99.7%的隨機(jī)誤差同時給目標(biāo)機(jī)動留一點(diǎn)余量G_p 3 * sqrt(σ_R^2 σ_A^2)舉個例子σ_R取50米σ_A取10米則G_p約為153米。實(shí)際工程里為了應(yīng)對目標(biāo)機(jī)動和航跡外推誤差我一般會把門限再放寬到300米左右再配合速度和航向門限做細(xì)篩。速度門限取Δv ≤ 5節(jié)航向門限取Δφ ≤ 30度這兩個約束能擋掉大量近距平行航行的錯誤配對。匹配策略上簡單場景用最近鄰就夠了多目標(biāo)交叉、編隊(duì)航行的場景強(qiáng)烈建議用全局最優(yōu)分配避免出現(xiàn)一個AIS目標(biāo)同時匹配兩個雷達(dá)目標(biāo)這種沖突。MATLAB里可以用匹配函數(shù)做最小代價指派代價矩陣就是距離矩陣超過門限的項(xiàng)設(shè)置成大數(shù)或者直接排除function [pairs] associate_gnn(Xr, Xa, gate) % Xr: 雷達(dá)點(diǎn)跡坐標(biāo) [Nx2]Xa: AIS點(diǎn)跡坐標(biāo) [Mx2] N size(Xr,1); M size(Xa,1); D sqrt((Xr(:,1) - Xa(:,1)).^2 (Xr(:,2) - Xa(:,2)).^2); % 超過門限的代價置為很大的數(shù)讓求解器不要選它 D(D gate) 1e9; [assignment, ~] matchpairs(D, min); % assignment列出的是有效配對的索引 pairs assignment; end沒裝優(yōu)化工具箱的讀者也別急這個最小距離分配用兩層循環(huán)加一個占位標(biāo)記也能寫性能差別在點(diǎn)跡數(shù)量不大的場景下完全體現(xiàn)不出來。提醒一句不要每幀都對全部點(diǎn)跡做一次全量關(guān)聯(lián)那樣容易造成身份抖動。推薦以AIS航跡為“錨”雷達(dá)點(diǎn)跡來配對更新AIS沒有覆蓋到的目標(biāo)雷達(dá)點(diǎn)跡可以獨(dú)立建航跡以后再嘗試關(guān)聯(lián)。3.2 位置融合從加權(quán)平均到卡爾曼濾波關(guān)聯(lián)配對成功后才輪到真正的“融合”。需要說清楚的是融合不是簡單的坐標(biāo)平均而是按誤差逆加權(quán)。雷達(dá)位置誤差σ_R遠(yuǎn)大于AIS的σ_A但AIS更新慢所以融合結(jié)果應(yīng)當(dāng)偏向“當(dāng)前時刻更可信”的信息源。加權(quán)融合公式如下x_f (σ_A^2 * x_r σ_R^2 * x_a) / (σ_R^2 σ_A^2) y_f (σ_A^2 * y_r σ_R^2 * y_a) / (σ_R^2 σ_A^2)這個公式本質(zhì)是最小方差估計(jì)。MATLAB實(shí)現(xiàn)起來就是幾行function [xf, yf] fuse_weighted(xr, yr, xa, ya, sigmaR, sigmaA) wR sigmaA^2 / (sigmaR^2 sigmaA^2); wA sigmaR^2 / (sigmaR^2 sigmaA^2); xf wR * xr wA * xa; yf wR * yr wA * ya; end想更進(jìn)一步可以用常速度模型做卡爾曼濾波狀態(tài)取[x, y, vx, vy]雷達(dá)和AIS作為兩類量測輸入。MATLAB自帶跟蹤濾波函數(shù)可以直接調(diào)用但我更建議自己把濾波核心寫一遍狀態(tài)轉(zhuǎn)移矩陣和量測更新邏輯才不會變成黑盒。量測更新部分的核心代碼大概是function [x_est, P_est] kf_update(x_pred, P_pred, z, H, R) K P_pred * H / (H * P_pred * H R); x_est x_pred K * (z - H * x_pred); P_est (eye(size(P_pred)) - K * H) * P_pred; end預(yù)測部分就是狀態(tài)轉(zhuǎn)移矩陣乘狀態(tài)、狀態(tài)協(xié)方差乘轉(zhuǎn)移矩陣轉(zhuǎn)置再加過程噪聲協(xié)方差。工程上Q取多大、R取多大跟場景強(qiáng)相關(guān)不要迷信某個固定數(shù)值要根據(jù)回放數(shù)據(jù)里的跳動幅度來調(diào)。3.3 航向航速融合與航跡身份管理位置融合之外SOG和COG的融合也有講究。AIS的SOG、COG來自GNSS解算統(tǒng)計(jì)特性比雷達(dá)點(diǎn)跡差分出來的速度穩(wěn)得多所以在AIS報文“新鮮”的時候直接用AIS的SOG和COG作為融合輸出不做額外計(jì)算。AIS報文超過一定時間后就不新鮮了比如超過30秒這時需要用雷達(dá)點(diǎn)跡的位置差分估算速度航向作為輔助。雷達(dá)差分速度噪聲大我一般會再做一次中值濾波把跳變點(diǎn)壓掉。這里的原則是優(yōu)先用可信源備用源做兜底。身份綁定是融合系統(tǒng)里容易被忽略卻很重要的環(huán)節(jié)。融合航跡號與AIS的MMSI綁定后即使AIS信號中斷航跡身份也要保持住不能等AIS重新出現(xiàn)時又當(dāng)成新目標(biāo)。身份管理的核心就是“綁定一次保持到底”直到確認(rèn)目標(biāo)離開或長時間失聯(lián)。4. MATLAB工程實(shí)現(xiàn)與調(diào)試心得4.1 數(shù)據(jù)輸入與模擬數(shù)據(jù)生成調(diào)試融合算法最怕沒有數(shù)據(jù)。項(xiàng)目早期如果拿不到真實(shí)雷達(dá)和AIS記錄可以先寫一個模擬數(shù)據(jù)生成器把傳感器的時序特征造出來。模擬器要還原三個關(guān)鍵特征雷達(dá)掃描周期短但噪聲大AIS更新周期長且不一定規(guī)律兩個傳感器的時間戳不同步。function gen_demo() t 0:1:300; % 仿真時間軸單位秒 % 目標(biāo)勻速直線運(yùn)動位置單位為米 x_true 1000 5 * t; y_true 2000 3 * t; % AIS每10秒上報一次定位噪聲小 aisT t(1:10:end); aisX x_true(1:10:end) randn(size(aisT)) * 10; aisY y_true(1:10:end) randn(size(aisT)) * 10; % 雷達(dá)每3秒掃描一次位置噪聲大 radT t(1:3:end); radX x_true(1:3:end) randn(size(radT)) * 30; radY y_true(1:3:end) randn(size(radT)) * 30; save(demo.mat, aisT,aisX,aisY,radT,radX,radY, x_true,y_true); end這個模擬器生成的數(shù)據(jù)足夠把關(guān)聯(lián)、融合、航跡管理整條鏈路跑通。建議模擬場景里再放一條近距離平行航行的干擾船專門用來驗(yàn)證關(guān)聯(lián)算法在多目標(biāo)場景下的表現(xiàn)。4.2 關(guān)鍵參數(shù)配置與調(diào)試順序我把這套融合系統(tǒng)里最關(guān)鍵的參數(shù)整理成了一張表初值是基于普通VTS監(jiān)控場景的經(jīng)驗(yàn)值實(shí)際使用時需要按具體數(shù)據(jù)調(diào)整。參數(shù)推薦初值說明坐標(biāo)參考點(diǎn)監(jiān)控區(qū)域中心影響坐標(biāo)轉(zhuǎn)換精度時間對齊窗口2秒小于窗口直接用大于窗口外推位置關(guān)聯(lián)門限300至800米按σ組合計(jì)算再留機(jī)動余量速度門限5節(jié)防止誤關(guān)聯(lián)航向門限30度防止大角度錯誤配對AIS新鮮度閾值30秒超過閾值的AIS信息不參與融合航跡保持周期5個雷達(dá)周期防止軌跡頻繁斷裂航跡起始條件連續(xù)3次關(guān)聯(lián)成功防止雜波起批外推時長上限30秒防止外推發(fā)散調(diào)試順序也很重要。我建議先從坐標(biāo)轉(zhuǎn)換入手畫一張俯視圖把雷達(dá)點(diǎn)、AIS點(diǎn)、轉(zhuǎn)換后的點(diǎn)疊在一起看確認(rèn)沒有系統(tǒng)性偏移然后調(diào)時間對齊觀察軌跡是否出現(xiàn)鋸齒接著調(diào)關(guān)聯(lián)門限確認(rèn)配對關(guān)系符合直覺最后才動濾波器參數(shù)。不要一上來就調(diào)卡爾曼的Q和R問題定位會很痛苦。4.3 可視化調(diào)試三板斧融合算法的調(diào)試可視化比看日志高效得多。我常用的有三招。第一招把所有原始點(diǎn)和融合軌跡畫在一張圖上。雷達(dá)點(diǎn)用圓圈、AIS點(diǎn)用三角、融合軌跡用連續(xù)的線不同目標(biāo)用不同顏色。一眼就能看出關(guān)聯(lián)錯誤和位置偏移比對著數(shù)字猜快太多了。第二招打印關(guān)聯(lián)矩陣。每次關(guān)聯(lián)計(jì)算后把配對雙方的航跡編號、距離、速度差、航向差輸出到命令行人工檢查有沒有一對多、跳號、距離異常等狀況。第三招畫軌跡生命周期視圖。橫軸是時間縱軸是航跡號一條目標(biāo)的生命過程就是一條水平線斷裂和跳變在這個視圖里十分直觀。目標(biāo)分裂、丟失、身份跳變的時刻都能一眼定位。這三招配合起來基本能把算法跑飛的原因鎖定到具體環(huán)節(jié)。5. 典型問題與排查實(shí)錄5.1 五個高頻問題與處理思路問題之一多目標(biāo)交叉時身份亂跳。這是關(guān)聯(lián)門限設(shè)得過大造成的。我把位置門限從3σ收緊到2.5σ同時強(qiáng)制航向門限誤關(guān)聯(lián)率明顯下降。如果場景里經(jīng)常出現(xiàn)近距離并行船可以再加一個船長門限做輔助判斷。問題之二融合軌跡來回?cái)[動。雷達(dá)點(diǎn)噪聲偏大加權(quán)系數(shù)沒有反映誤差變化。處理辦法是對雷達(dá)點(diǎn)先做一次α-β平滑再把平滑后的位置參與融合。α取0.7、β取0.1左右效果比較溫和不會把真實(shí)機(jī)動削掉。問題之三融合軌跡整體滯后。AIS報文的傳輸延遲會導(dǎo)致軌跡整體偏向歷史位置。短外推解決不了系統(tǒng)性的滯后我在項(xiàng)目里記錄AIS報文“發(fā)送時刻”與“接收時刻”的時間差估算固定延遲后做補(bǔ)償滯后問題基本消除。問題之四目標(biāo)短暫丟失后重新出現(xiàn)被當(dāng)成新目標(biāo)。這是航跡生命周期參數(shù)設(shè)置不合理。把航跡保持時間拉長在丟失階段用AIS信息或運(yùn)動外推維持航跡再用連續(xù)計(jì)數(shù)邏輯把“短暫消失”和“真正離開”區(qū)分開。問題之五近岸雜波引起的假航跡。雷達(dá)在近岸會產(chǎn)生大量假回波單點(diǎn)起批很容易把雜波當(dāng)成船。對策是航跡起始必須滿足連續(xù)3次關(guān)聯(lián)成功并且加最小航速約束靜止雜波一般過不了這個條件。5.2 問題排查速查表現(xiàn)象可能原因處理動作身份跳變位置門限過大、特征約束弱收縮門限增加航速航向門限軌跡擺動雷達(dá)噪聲大、加權(quán)失衡先平滑雷達(dá)點(diǎn)按誤差更新權(quán)重整體滯后AIS鏈路傳輸延遲估算延遲并補(bǔ)償優(yōu)先采用新鮮源軌跡分裂航跡保持周期太短加長“丟失保持”時間放寬重新關(guān)聯(lián)條件誤匹配局部最近鄰沖突改用全局最優(yōu)分配排除一對多6. 寫在最后的一些體會融合算法做出來不難難的是在真實(shí)數(shù)據(jù)環(huán)境下一直不壞。回看這個項(xiàng)目我大部分時間其實(shí)花在數(shù)據(jù)清洗和參數(shù)調(diào)試上——AIS字段缺失、雷達(dá)野值、時間戳不齊這些問題不處理干凈再漂亮的濾波公式輸出也是垃圾。另一個體會是調(diào)試時一定要保留可追溯的中間結(jié)果每次改動參數(shù)都能復(fù)現(xiàn)問題否則調(diào)參只能靠猜。這套基于MATLAB的實(shí)現(xiàn)思路覆蓋了普通VTS監(jiān)控場景的典型需求如果后續(xù)要做高動態(tài)編隊(duì)或者復(fù)雜航道多目標(biāo)還需要引入更強(qiáng)的數(shù)據(jù)關(guān)聯(lián)方法比如概率數(shù)據(jù)關(guān)聯(lián)或帶航跡管理能力的多假設(shè)跟蹤。希望這份記錄能幫正在做同類工作的朋友少踩幾個坑。