沈 磊，劉 康

（中國計(jì)量大學(xué) 機(jī)電工程學(xué)院，浙江 杭州 310018）

0 引 言

隨著時代的發(fā)展，許多4S店期望基于毫米波雷達(dá)獲取展館內(nèi)客戶的參觀路線、熱點(diǎn)停留區(qū)域、滯留時長等信息，實(shí)現(xiàn)室內(nèi)多目標(biāo)跟蹤，提高針對性導(dǎo)購的效率。因此，本文選用多毫米波雷達(dá)融合的方式實(shí)現(xiàn)多目標(biāo)行人跟蹤[1-2]，在保護(hù)隱私的同時，且不易受天氣、光線影響。

由于低成本毫米波雷達(dá)的天線數(shù)目少且孔徑小，雷達(dá)功率低以及數(shù)字信號處理（Digital Signal Processing，DSP）和室內(nèi)復(fù)雜環(huán)境的局限，限制了雷達(dá)DOA（Direction of Arrival）估計(jì)[3]的性能，點(diǎn)云分布離散稀疏且誤差大。單人真實(shí)占地面積在0.5～1 m2，而雷達(dá)檢測的擴(kuò)展目標(biāo)的面積可達(dá)1～3 m2。多人場景中易混淆不同擴(kuò)展目標(biāo)間的點(diǎn)云，導(dǎo)致聚類環(huán)節(jié)生成錯誤的簇，增加目標(biāo)關(guān)聯(lián)時誤判的風(fēng)險(xiǎn)。過程中試圖將徑向速度和信噪比等信息融入判斷（存在諧波、多徑、微多普勒等信號干擾），但不能杜絕此問題發(fā)生。

前 人 通 過ZoomFFT算 法[4]、MIMO（Multiple-input Multiple-output）陣列超分辨到達(dá)角估計(jì)算法[5]、將多個連續(xù)相干處理間隔（CPIs）的信號向量結(jié)合得到大的虛擬孔徑[6]、引入k空間分解格式的加權(quán)核密度（WKD）估計(jì)[7]等方式實(shí)現(xiàn)分辨率方面的提升。相較上述對信號濾波處理的方案，本文從點(diǎn)云層面入手，通過結(jié)合最小二乘法曲線擬合[8]和EKF（Extended Kalman Filter）[9-10]，設(shè)計(jì)了針對低成本毫米波雷達(dá)稀疏點(diǎn)云的擴(kuò)展目標(biāo)聚合算法。該算法有效解決了點(diǎn)云離散和偏差大導(dǎo)致的擴(kuò)展目標(biāo)點(diǎn)云混疊的問題，提高了跟蹤的性能。

1 擴(kuò)展目標(biāo)點(diǎn)云聚合算法

本文使用一種低成本77 GHz毫米波雷達(dá)，視場角（Field of View，F(xiàn)OV）為150°，探測距離為13 m。由于單毫米波雷達(dá)的精度有限且點(diǎn)數(shù)不穩(wěn)定（空間頻譜上由于信號的誤差估計(jì)過大，導(dǎo)致點(diǎn)云無法通過恒虛警的檢測），可以使用多個毫米波雷達(dá)，融合分布式毫米波雷達(dá)數(shù)據(jù)來降低跟蹤誤差，彌補(bǔ)單雷達(dá)反射點(diǎn)不穩(wěn)定的問題[11]。

首先，同時驅(qū)動多個雷達(dá)（保證同步的時間戳），在本地進(jìn)行濾波后整合到同一笛卡爾坐標(biāo)系下，再經(jīng)過點(diǎn)云配準(zhǔn)擬合算法[12]，減小測量誤差和雷達(dá)標(biāo)定誤差，為使同時刻下的擴(kuò)展目標(biāo)點(diǎn)云團(tuán)盡可能完美重合。再對融合[13]后的點(diǎn)云做疊幀處理，使用滑動窗口算法[14]將多幀的點(diǎn)云數(shù)據(jù)疊加。如上操作緩解了點(diǎn)云數(shù)目不穩(wěn)定和擴(kuò)展目標(biāo)點(diǎn)云缺失的問題，但數(shù)據(jù)融合也使得單目標(biāo)的點(diǎn)云團(tuán)面積更大，對跟蹤層仍是挑戰(zhàn)，由此提出一種擴(kuò)展目標(biāo)點(diǎn)云聚合算法。先對多個毫米波雷達(dá)所探測到的點(diǎn)云進(jìn)行融合和疊幀處理，在增加點(diǎn)云數(shù)目的同時降低單一雷達(dá)丟失目標(biāo)的風(fēng)險(xiǎn)；其次通過最小二乘法曲線擬合，構(gòu)建EKF的目標(biāo)位置預(yù)測模型，在預(yù)測值處自適應(yīng)生成波門，得到假設(shè)目標(biāo)關(guān)聯(lián)位置，將此位置與預(yù)測值作加權(quán)修正處理后聚合點(diǎn)云。圖1是擴(kuò)展目標(biāo)點(diǎn)云聚合算法的流程框圖。

圖1 算法流程圖

1.1 最小二乘法擬合

首先將各個目標(biāo)過往質(zhì)心的x，y分量分別對T通過最小二乘法作二階擬合，分別得到x，y與時間T的二次函數(shù)f(x,t)，f(y,t)，對T求二次導(dǎo)，即可以得到x，y方向上的加速度，以此加速度分量構(gòu)建EKF加速度模型。

1.2 擴(kuò)展卡爾曼濾波

卡爾曼濾波器（Kalman Filter，KF）是最小均方誤差線性濾波器，但通常狀態(tài)方程或觀測方程為非線性關(guān)系。當(dāng)非線性函數(shù)在工作點(diǎn)附近存在各階導(dǎo)數(shù)或偏導(dǎo)數(shù)，且迭代時工作點(diǎn)僅在小范圍內(nèi)變化時，一種解決非線性的辦法就是在狀態(tài)估計(jì)值處利用泰勒（Taylor）級數(shù)展開，忽略其二階及以上的部分，得到近似線性的模型，這種方法就是擴(kuò)展卡爾曼濾波[15]。毫米波雷達(dá)的觀測量與運(yùn)動狀態(tài)量呈非線性關(guān)系，因此選用擴(kuò)展卡爾曼濾波。

將擬合求得的加速度分量用于構(gòu)建加速度模型的擴(kuò)展卡爾曼濾波。對穩(wěn)定跟蹤的目標(biāo)（滿足生命周期的目標(biāo)，排除虛影目標(biāo)）進(jìn)行擴(kuò)展卡爾曼濾波迭代，將最終預(yù)測的目標(biāo)位置輸出，以下是EKF設(shè)計(jì)。

狀態(tài)量：

狀態(tài)方程為：

這邊將加速度分量作為Bu輸入量輸入，而不是將ax，ay也作為狀態(tài)量，好處在于：從6維狀態(tài)轉(zhuǎn)移矩陣變?yōu)?維，降低了維度，提高了運(yùn)行效率；在多人運(yùn)動時，存在交錯、交匯、急停等各種非勻變速情況，運(yùn)動狀態(tài)復(fù)雜多變。此外，由于跟蹤目標(biāo)采樣數(shù)目有限（幀周期長），外部輸入能更快速反映測量值，使模型更快地收斂。

觀測方程：

觀測量：

觀測量與狀態(tài)量非線性轉(zhuǎn)換：

雅可比矩陣：

以EKF預(yù)測位置為質(zhì)心，設(shè)計(jì)distance波門函數(shù)（主要通過歐氏距離度量），將滿足波門半徑范圍內(nèi)的點(diǎn)云都納入進(jìn)來（若同時滿足兩個目標(biāo)波門，取近的目標(biāo)波門），并且將這些點(diǎn)云標(biāo)記到該簇下。如果場景中存在多個真實(shí)目標(biāo)，需要遍歷每個目標(biāo)，重復(fù)上述操作。

然后計(jì)算每個簇內(nèi)所有點(diǎn)云的x，y均值，作為假設(shè)的目標(biāo)關(guān)聯(lián)位置，再用此假設(shè)的目標(biāo)關(guān)聯(lián)位置和EKF得到的預(yù)測值作加權(quán)修正處理，分配兩個質(zhì)心之間的加權(quán)系數(shù)w。令修正質(zhì)心為F_p，預(yù)測質(zhì)心為EKF_p，假設(shè)目標(biāo)關(guān)聯(lián)位置為S_p。

做加權(quán)修正的原因：由于雷達(dá)自身檢測的點(diǎn)云偏動較大，若不經(jīng)處理，軌跡十分曲折。本算法為了保證點(diǎn)云聚合效率的同時，兼顧運(yùn)動軌跡的順滑性。

但僅通過EKF參數(shù)設(shè)定無法同時兼顧兩個指標(biāo)：

1）調(diào)大Q過程噪聲，使模型更相信預(yù)測值，雖能緩解雷達(dá)測量值的偏動，軌跡更平緩且更接近真實(shí)行走軌跡，但是喪失了觀測軌跡的特征，無法在此預(yù)測值周圍準(zhǔn)確地捕獲下一時刻的目標(biāo)點(diǎn)云，降低了擴(kuò)展目標(biāo)點(diǎn)云聚合的效率。

2）EKF得到的目標(biāo)的預(yù)測值暫未經(jīng)過觀測值修正，若將此預(yù)測值與假設(shè)的目標(biāo)關(guān)聯(lián)位置加權(quán)修正后，可以更好地捕捉擴(kuò)展目標(biāo)點(diǎn)云，提高點(diǎn)云聚合的效率，兼顧了軌跡的順滑性和點(diǎn)云聚合的精準(zhǔn)性。

得到了最終的聚合質(zhì)心p(ax,ay)后，令各簇內(nèi)的點(diǎn)云成員都朝各自的預(yù)測質(zhì)心比例聚合。將點(diǎn)云與聚合質(zhì)心x，y分量的差值記為Δx，Δy，聚合后點(diǎn)的位置記為p(nx,ny)，聚合系數(shù)為d。

2 實(shí)驗(yàn)結(jié)果與分析

2.1 擬合效果對比與分析

擬合過程對比了最小二乘法一次擬合、二次擬合、三次擬合的效果，如圖2所示。

圖2 各階次最小二乘法擬合效果對比圖

對20個過往時刻的位置的x，y分量分別對T作不同階次的最小二乘法擬合之后，再整合繪制成軌跡。由圖2可見，坐標(biāo)軸刻度偏動在5 cm之間，幾乎可忽略。此外，虛線是實(shí)際運(yùn)動直線，點(diǎn)折線是觀測值的原始軌跡，圓圈直線作為一階勻速模型擬合線丟失了軌跡的特征信息，無法反映趨勢；菱形曲線反映的是三階擬合后的效果，大部分情況下更能貼合原折線，并且在8～20個位置采樣點(diǎn)的情況下（大概經(jīng)歷1～2 s的運(yùn)動時間），哪怕是有兩個凹凸性也能記錄原軌跡的特性，但不難看出，還原出的曲線過度相信觀測值，與實(shí)際行駛的軌跡相比無法兼顧軌跡的順滑性；相對于二次擬合曲線，綜合了一次和三次擬合的特性，既能反映軌跡特性的同時，更接近實(shí)際行走路線，軌跡順滑性好，并且考慮到本身偏差在厘米級，實(shí)際跟蹤效果并無明顯差異，因此選用二次擬合，降低模型復(fù)雜度的同時也減輕了計(jì)算負(fù)擔(dān)，提高了運(yùn)算效率。

另外，還測試了RANSAC擬合函數(shù)效果，迭代次數(shù)少時擬合曲線非?；靵y，暫不考慮。

2.2 效果分析與展示

系統(tǒng)參數(shù)設(shè)定如表1所示。

表1 系統(tǒng)參數(shù)設(shè)定

圖3是測試場景及兩個雷達(dá)安裝情況（一個雷達(dá)前照，一個雷達(dá)側(cè)照），兩人沿著相隔0.8 m的直線并排行走的實(shí)況圖（兩人實(shí)際肩距在0.5 m左右）。

圖3 場景布局及測試

圖4a）展示的是兩個人直線行走的點(diǎn)云聚合前后的效果，正方形是原始點(diǎn)云，小點(diǎn)是聚合后的點(diǎn)云，五角星是擴(kuò)展目標(biāo)聚合質(zhì)心?？梢钥闯?，在算法前兩團(tuán)點(diǎn)云占地面積較大，且靠近時有混疊現(xiàn)象。此外，從理論上看，兩團(tuán)點(diǎn)云具有相似的SNR和Doppler信息，難以區(qū)分。在算法后的點(diǎn)云團(tuán)占地面積明顯減小且擴(kuò)展目標(biāo)數(shù)目清晰分明。圖4b）展示了算法后的雙人并排直線行走（肩距0.5 m）的跟蹤及軌跡效果。

圖4 雙人并排行走點(diǎn)云及跟蹤效果

圖5是上述雙人并排行走場景的點(diǎn)云積累圖，可以看出點(diǎn)云在聚合前和聚合后有較明顯的區(qū)別，占地面積更小、點(diǎn)云更緊湊集中、目標(biāo)更加分明。多人在場景下擴(kuò)展目標(biāo)點(diǎn)云也互不干擾，減輕了目標(biāo)匹配的負(fù)擔(dān)。

圖5 雙人并排行走點(diǎn)云積累效果圖

圖6、圖7展示的分別是兩個人間隔0.5～0.8 m并排行走的算法處理前后的跟蹤軌跡效果。從圖6可以看出，兩個人靠近時，目標(biāo)軌跡有間斷和目標(biāo)ID切換。這是因?yàn)辄c(diǎn)云混疊，DBSCAN聚類環(huán)節(jié)把此時刻的點(diǎn)云聚成了一個簇（兩團(tuán)點(diǎn)云具有相似的SNR和Doppler信息，難以區(qū)分），在目標(biāo)關(guān)聯(lián)時，兩個目標(biāo)都關(guān)聯(lián)到同一個簇，因此有一個目標(biāo)丟失，后來出現(xiàn)的黑色軌跡其實(shí)是作為新目標(biāo)出現(xiàn)。在算法后兩個目標(biāo)靠近時亦能完美跟蹤兩個目標(biāo)，軌跡清晰、效果良好，如圖7所示。

圖6 兩人靠近后并排行走跟蹤效果（算法前）

圖7 兩人靠近后并排行走跟蹤效果（算法后）

2.3 SIAP度量器定量驗(yàn)證分析

單一集成空情圖（Single Integrated Air Picture，SIAP）通過融合多種傳感器數(shù)據(jù)得到監(jiān)視空中區(qū)域內(nèi)的所有目標(biāo)的軌跡[14]。雖多被用于航空、軍事的評估，也可用來量度多目標(biāo)跟蹤算法的性能。本文選取其中的完備度和狀態(tài)準(zhǔn)確度兩個指標(biāo)展開驗(yàn)證。

2.3.1 完備度

完備度[14]指的是跟蹤器中的軌跡數(shù)量與實(shí)際的目標(biāo)數(shù)目之比，當(dāng)所有的目標(biāo)都被正確跟蹤時，則稱該跟蹤器是完備的。

在k時刻的完備度如式（11）所示，JTm(k)是k時刻跟蹤器中的軌跡數(shù)量，J(k)是k時刻的真實(shí)目標(biāo)數(shù)量。當(dāng)真實(shí)目標(biāo)被完美跟蹤時，完備性Ck為1。若完備度大于1，不排除在跟蹤過程中多目標(biāo)交互時存在目標(biāo)分裂或目標(biāo)混疊后，有一個假死目標(biāo)停留在原地的可能性；如果完備度小于1，可能是兩個目標(biāo)交互后由于混疊目標(biāo)丟失。

圖8、圖9分別是圖6、圖7實(shí)況的跟蹤性能測試圖，代表點(diǎn)云聚合前后完備度指標(biāo)。由圖8可知，兩個目標(biāo)在足夠靠近時，點(diǎn)云混疊，兩個目標(biāo)聚類成一個簇，兩目標(biāo)關(guān)聯(lián)到同一個簇，導(dǎo)致其中一個目標(biāo)缺失，后來又產(chǎn)生，其實(shí)此時已經(jīng)更換了ID，作為一個新目標(biāo)出現(xiàn)。在算法后，兩目標(biāo)行走路線清晰分明，完備度測試無誤。

圖8 雙人并排行走完備度測試（算法前）

圖9 雙人并排行走完備度測試（算法后）

2.3.2 狀態(tài)準(zhǔn)確度

狀態(tài)準(zhǔn)確度[14]主要評估跟蹤器中軌跡的位置精度和速度精度，本文主要評估跟蹤的位置精度。選取一個人沿直線行走的實(shí)況，測試算法前后軌跡的位置精度。式（12）是位置精度量度公式：

式中：D(k)是k時刻軌跡的集合；PAn(k)是k時刻第n條軌跡到真實(shí)值的歐氏距離；NA(k)是k時刻分配給真實(shí)目標(biāo)的軌跡數(shù)。

由于測試人員沿著x=-0.2直線行走，將公式簡化成單一軌跡到x=-0.2的垂直距離。單人直線行走位置誤差如圖10所示。

圖10 單人直線行走位置誤差

經(jīng)過多組實(shí)驗(yàn)數(shù)據(jù)的總結(jié)，發(fā)現(xiàn)位置誤差峰值減小且誤差跨度減小，誤差從30 cm的偏差修正到15 cm的偏差，多目標(biāo)間干擾減小，軌跡的曲折程度降低，對軌跡的順滑性及跟蹤性能有一定的提升。

3 結(jié)語

為了解決低成本雷達(dá)自身缺陷導(dǎo)致的DOA估計(jì)誤差大的問題，本文提出了一種擴(kuò)展目標(biāo)點(diǎn)云聚合算法。通過最小二乘法、擴(kuò)展卡爾曼濾波、加權(quán)修正等方法得到擴(kuò)展目標(biāo)點(diǎn)云團(tuán)聚合的質(zhì)心，向此質(zhì)心比例聚合。通過實(shí)驗(yàn)前后的效果對比證明，此設(shè)計(jì)提高了擴(kuò)展目標(biāo)點(diǎn)云的辨識度，接近的擴(kuò)展目標(biāo)點(diǎn)云團(tuán)分布緊湊且清晰，為跟蹤層的目標(biāo)關(guān)聯(lián)奠定了良好的基礎(chǔ)。