基于毫米波雷達(dá)的室內(nèi)目標(biāo)檢測與跟蹤算法

常浩, 殳國華, 胡博文

(上海交通大學(xué) 電子信息與電氣工程學(xué)院,上海 200240)

0 引 言

隨著工業(yè)物聯(lián)網(wǎng)的發(fā)展,人們對于室內(nèi)監(jiān)控的需求不斷增加。近年來,有學(xué)者通過攝像頭監(jiān)控、WiFi定位和紅外線等技術(shù)實現(xiàn)了室內(nèi)人員的定位與跟蹤。采用攝像頭監(jiān)控可以清晰識別室內(nèi)人員位置,但是遇到障礙物無法工作,且過分采集人員隱私;基于WiFi的定位技術(shù)克服了這些缺點,但是很難實現(xiàn)較高的檢測精度;紅外線傳感器尺寸小,成本低,但是易受光照等環(huán)境影響。而毫米波雷達(dá)克服了這些技術(shù)的缺點,且具有更高的檢測精度[1］,采用毫米波雷達(dá)進行室內(nèi)目標(biāo)檢測與跟蹤有獨特的優(yōu)勢。

在基于毫米波雷達(dá)的目標(biāo)檢測中,通過對雷達(dá)信號的混頻與調(diào)制,對目標(biāo)的距離、速度和角度信息進行測量估計,進而對目標(biāo)運動軌跡進行跟蹤。東南大學(xué)毫米波雷達(dá)實驗室已經(jīng)完成了8 mm波段的倍頻器、混頻器等芯片的研究。李方敏等[2］提出應(yīng)用于室內(nèi)的基于雙頻連續(xù)波比相測距的局部速度差補償算法,獲取高精度的定位信息。

本文提出了一種基于毫米波雷達(dá)調(diào)頻連續(xù)波(frequency modulated continuous wave, FMCW)的目標(biāo)檢測與跟蹤算法。通過對雷達(dá)信號的預(yù)處理和快速傅里葉變換(fast Fourier transform,FFT)提取出目標(biāo)的距離、速度和角度信息,經(jīng)過二維恒虛警檢測確定目標(biāo)位置,然后基于擴展卡爾曼濾波算法跟蹤目標(biāo)的運動軌跡。

1 毫米波雷達(dá)原理

本文采用的毫米波雷達(dá)設(shè)備為德州儀器公司發(fā)布的AWR1443BOOST開發(fā)板,發(fā)射的波形為線性調(diào)頻連續(xù)波,其頻率隨著時間線性增加,可以將發(fā)射信號表示為:

sT(t)=ATcos[2π(f0t+St2/2)+φ0]

(1)

式中:AT為發(fā)射信號的幅值;f0為載波的中心頻率;S為調(diào)頻斜率,S=B/T;B為發(fā)射信號掃頻帶寬;T為信號頻率抬高周期;t為時間;φ0為初始相位。

假設(shè)雷達(dá)到目標(biāo)的距離為R,電磁波在空氣中傳播速度為c,接收天線接收到的目標(biāo)信號為:

sR(t)=ARcos{2π[f0(t-τ)+S(t-τ)2/2]+φ0}

(2)

式中:AR為接收信號幅值;τ為接收信號相對于發(fā)射信號的固定延時,τ=2R/c。將發(fā)射信號與接收信號進行混頻可以得到中頻信號,如圖1所示。

(3)

圖1 調(diào)頻連續(xù)波產(chǎn)生 中頻信號原理圖

中頻信號的頻率為:

(4)

FMCW雷達(dá)信號的頻率隨著脈沖時間的增加不斷提高。根據(jù)回波信號的頻率信息,估計目標(biāo)的距離信息:

(5)

由于雷達(dá)的中頻帶寬受ADC采樣頻率的限制,因此可以推出最大探測距離為:

(6)

式中:Fs為最高采樣頻率。目標(biāo)處于相同距離發(fā)射多個信號時回波信號的相位不同,利用相位差來計算目標(biāo)的速度。

(7)

式中:ω為回波信號角頻率;λ為回波信號波長;Tc為相鄰兩個回波信號間隔時間;v為目標(biāo)速度。由于|ω|<π,最大檢測速度為:

(8)

使用兩根雷達(dá)天線接收回波信號時,目標(biāo)與兩根天線之間的距離差會導(dǎo)致FFT峰值的相位變化,因此可以進行目標(biāo)的角度估計。

(9)

式中:θ為接收天線與目標(biāo)的夾角;d為兩根接收天線之間的距離。

2 目標(biāo)檢測流程

圖2為毫米波雷達(dá)目標(biāo)檢測流程。通過混頻得頻率穩(wěn)定的中頻信號,對中頻信號進行ADC采樣得到雷達(dá)的時域信號。接著對于每個周期的線性調(diào)頻連續(xù)波進行一維加窗與一維距離FFT處理來獲取目標(biāo)的距離信息;對于一幀內(nèi)的所有線性調(diào)頻連續(xù)波進行多普勒維FFT,以獲取目標(biāo)速度信息;再利用多根接收天線相位差估計目標(biāo)角度信息。

圖2 毫米波雷達(dá)目標(biāo)檢測流程

雷達(dá)接收的信號中含有雜波與噪聲干擾,采用二維恒虛警檢測可以濾除這些干擾[3］,設(shè)噪聲分布服從瑞利分布[4］,此時恒虛警檢測的檢測門限為:

(10)

式中:K為常數(shù);xi為檢測單元;n為樣本數(shù)量;σΛ為單元標(biāo)準(zhǔn)差σ的估計值。自適應(yīng)的檢測門限會隨著噪聲與雜波信號的大小而改變,有效濾除雜波和噪聲干擾,獲取目標(biāo)位置。

3 目標(biāo)跟蹤算法

3.1 基于卡爾曼濾波的跟蹤算法

基于卡爾曼濾波的跟蹤算法是在線性系統(tǒng)中根據(jù)物體在當(dāng)前時刻的觀測位置和前一時刻的最優(yōu)預(yù)測位置[5］,來確定物體在當(dāng)前時刻的真實位置。線性系統(tǒng)的狀態(tài)方程和觀測方程為:

X(k)=AX(k-1)+BU(k)+W(k)

(11)

Z(k)=HX(k)+V(k)

(12)

式中:X(k)、X(k-1)分別為k時刻和k-1時刻的目標(biāo)狀態(tài);A為狀態(tài)轉(zhuǎn)移矩陣;B為系統(tǒng)參數(shù)矩陣;U(k)為k時刻對系統(tǒng)的控制量;W(k)為系統(tǒng)噪聲;Z(k)為k時刻的測量值;H為觀測矩陣;V(k)為測量噪聲。

使用卡爾曼濾波進行目標(biāo)跟蹤主要基于2個時間更新方程和3個狀態(tài)更新方程。

時間更新方程為:

X(k|k-1)=AX(k-1|k-1)+BU(k)

(13)

P(k|k-1)=AP(k-1|k-1)AT+Q

(14)

式中:X(k|k-1)為利用上一次狀態(tài)預(yù)測的結(jié)果;X(k-1|k-1)為上一次狀態(tài)的最優(yōu)預(yù)測結(jié)果;P(k|k-1)為X(k|k-1)的協(xié)方差;P(k-1|k-1)為X(k-1|k-1)的協(xié)方差;AT為A的轉(zhuǎn)置;Q為系統(tǒng)噪聲的協(xié)方差。時間更新方程對系統(tǒng)當(dāng)前位置進行估計,并計算當(dāng)前時刻系統(tǒng)預(yù)測位置產(chǎn)生的協(xié)方差。

狀態(tài)更新方程為:

(15)

X(k|k)=X(k|k-1)+K(k)[Z(k)-HX(k|k-1)]

(16)

P(k|k)=[I-K(k)H]P(k|k-1)

(17)

式中:K(k)為卡爾曼增益;HT為H的轉(zhuǎn)置;R為測量噪聲V(k)的協(xié)方差;X(k|k)為當(dāng)前狀態(tài)的最優(yōu)估計值;P(k|k)為X(k|k)的協(xié)方差;I為單位矩陣。狀態(tài)更新方程計算了卡爾曼增益,并根據(jù)物體當(dāng)前時刻的估計位置和當(dāng)前時刻的觀測位置計算物體當(dāng)前時刻的最優(yōu)位置及協(xié)方差。

3.2 基于擴展卡爾曼濾波的跟蹤算法

由于室內(nèi)目標(biāo)的運動是非線性的,需要將卡爾曼濾波算法進行擴展,即將物體運動的非線性函數(shù)進行泰勒級數(shù)展開,將展開式中二階及以上項略去得到近似線性的模型。

擴展卡爾曼濾波算法的5個基本公式與卡爾曼濾波算法的公式相比,區(qū)別在于將系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣A和觀測矩陣H分別用其Jacobian矩陣代替。

狀態(tài)轉(zhuǎn)移矩陣代替為:

(18)

式中:f為非線性系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù);X為系統(tǒng)狀態(tài)。觀測矩陣代替為:

(19)

式中:h為非線性系統(tǒng)觀測函數(shù)。圖3為基于擴展卡爾曼濾波的目標(biāo)跟蹤算法流程,其主要包含兩個階段:① 預(yù)測階段,通過目標(biāo)上一時刻的位置信息和速度信息來預(yù)測目標(biāo)當(dāng)前時刻的位置;② 更新階段,通過物體當(dāng)前時刻的觀測信息對第一階段的預(yù)測結(jié)果進行修正,得到目標(biāo)當(dāng)前時刻位置的最優(yōu)值。

圖3 擴展卡爾曼濾波跟蹤算法流程

4 試驗結(jié)果與分析

4.1 試驗場景與雷達(dá)參數(shù)設(shè)置

本文試驗場所為一間長7.5 m、寬5.0 m、高3.5 m的實驗室,雷達(dá)架設(shè)高度為1.2 m,距離左、右兩邊墻各2.5 m,距離前端墻7 m。試驗設(shè)計了三種人員活動方式:① 目標(biāo)人員依次站在雷達(dá)正前方1.0 m、1.5 m、2.0 m、2.5 m、3.0 m、4.0 m、5.0 m、6.0 m處固定位置,原地做踏步動作;② 目標(biāo)人員從雷達(dá)右前方5.0 m處徑直走到雷達(dá)左前方1.0 m處,行走路線為1字型;③ 目標(biāo)人員從左前方5.0 m處先走到雷達(dá)正前方,然后轉(zhuǎn)身從雷達(dá)正前方靠近雷達(dá),行走路線為7字型。試驗中雷達(dá)系統(tǒng)參數(shù)如表1所示。

表1 雷達(dá)系統(tǒng)參數(shù)

4.2 試驗結(jié)果驗證

圖4和圖5分別為目標(biāo)人員分別位于距離雷達(dá)系統(tǒng)1.5 m和3.0 m時的目標(biāo)檢測二維散點圖,可以發(fā)現(xiàn)距離雷達(dá)較遠(yuǎn)時散點比較集中,較近時散點比較離散,可以定位人員位置。

圖4 距離1.5 m目標(biāo) 檢測散點圖

圖5 距離3.0 m目標(biāo) 檢測散點圖

為了驗證目標(biāo)位置檢測的準(zhǔn)確性,對選取的8個不同距離的目標(biāo)位置分別進行檢測。表2為檢測結(jié)果,可以發(fā)現(xiàn):當(dāng)目標(biāo)距離雷達(dá)超過2 m時定位較為精準(zhǔn);當(dāng)目標(biāo)距離雷達(dá)較近時,定位誤差增大,誤差在0.1 m以內(nèi)。

表2 雷達(dá)目標(biāo)檢測結(jié)果 單位:m

圖6和圖7表示目標(biāo)人員運動軌跡為1字型和7字型時,基于擴展卡爾曼濾波的目標(biāo)跟蹤軌跡?？梢园l(fā)現(xiàn):當(dāng)距離較遠(yuǎn)時,檢測到的運動軌跡較為平滑;當(dāng)距離較近時,運動軌跡較為粗糙,可以實現(xiàn)對目標(biāo)運動軌跡的跟蹤。

圖6 目標(biāo)1字型運動 跟蹤軌跡圖

圖7 目標(biāo)7字型運動 跟蹤軌跡圖

5 結(jié)束語

本文提出了一種基于毫米波雷達(dá)與擴展卡爾曼濾波算法的室內(nèi)目標(biāo)檢測與跟蹤算法,克服了傳統(tǒng)室內(nèi)監(jiān)控受環(huán)境影響較大等缺點。通過對FMCW毫米波雷達(dá)原理的分析和系統(tǒng)參數(shù)設(shè)計,利用AWR1443BOOST毫米波雷達(dá)開發(fā)板提取目標(biāo)距離、速度和角度信息,并設(shè)計基于擴展卡爾曼濾波的軌跡跟蹤模型,實現(xiàn)精度為0.1 m的室內(nèi)目標(biāo)的位置檢測和軌跡跟蹤。系統(tǒng)具有穩(wěn)定性好、體積小和功耗低的優(yōu)點,在智能監(jiān)控及健康檢測等領(lǐng)域具有廣闊的應(yīng)用前景。