Radar target generation and detection - Software

本文的代碼均在MATLAB上運行, MATLAB在傳統輔助駕駛系統的驗證和模擬上相對于其他編程語言具有很強的優勢.

Github: https://github.com/williamhyin/SFND_Radar_Target_Detection

Email: williamhyin@outlook.com

知乎專欄: 自動駕駛全棧工程師

Range and velocity estimation

在這篇博客中, 首先我們需要學會如何使用多普勒傅里葉變換技術估計距離和速度. 雷達分辨目標的能力對于準確的感知是非常關鍵的. 在開始之前, 讓我們先來看一下有關啁啾和雷達分辨率的三個主要測量維度.

啁啾

啁啾(Chirp)是指頻率隨時間而改變（增加或減少）的信號, 這一術語可以與掃頻信號(Sweep signal)互換使用. 它通常用于聲納、雷達和激光.

圖片引用自此

距離分辨率(Range Resolution)

雷達需要具備區分兩個距離非常近的目標的能力. 比如, 當雷達的距離分辨率為4米時, 它就不能區分相距1m的行人和汽車. 距離分辨率完全取決于啁啾的帶寬B_sweep.
$$d_{res}=\frac{c}{2B_{sweep}}$$

速度分辨率(Velocity Resolution)

如果兩個目標有相同的距離, 但是當它們以不同的速度移動, 它們仍然可以被區分開來. 速度分辨率取決于啁啾的個數. 正如討論我們的情況下, 我們選擇發送128個啁啾. 較高的啁啾數量增加了速度分辨率, 但它也需要更長的時間來處理信號.

角度分辨率(Angle Resolution)

雷達能夠在空間上分離兩個目標. 即使兩個目標在相同的距離以相同的速度運動, 它們仍然可以通過雷達坐標系中的角度分辨出來.

Range Estimation

雷達通過測量目標反射的電磁信號的飛行時間來確定目標的飛行距離.

但是如何確定雷達信號的傳輸時間呢?　我們需要測量頻移.

FMCV 波形具有頻率隨時間線性變化的特點. 如果雷達能夠確定接收頻率和雷達的斜坡頻率之間的差值, 那么它就可以計算雷達信號的時間和距離目標的距離.

首先我們得理解一點, 如果目標是靜止的, 那么發射頻率和接收頻率是相同的. 但是, 雷達的斜坡頻率是隨時間不斷變化的, 所以, 當我們取得接收頻率和斜坡頻率之間的差(beat frequency) , 我們就得到了信號傳輸時間.

如上圖所表示, f_b是拍頻, 通過雷達的斜坡頻率減去接收到的頻率來測量.
$$f_b=f_{\text{ramping}}?f_{\text{received}}$$

如上圖所示, 雷達發射信號的頻率是77 GHz,　返回信號的頻率是77.01, 它們的差值delta稱為拍頻, 拍頻正比于啁啾時間(td).
$$\begin{gathered} \frac{t_d}{T_s}=\frac{f_b}{B_{\text{sweep}}} \\ {t}_d=\frac{2R}{c} \end{gathered}$$
基于上述的公式, 我們可以計算目標的距離
$$R=\frac{c{T}_s{f}_b}{2B_{sweep}}$$
距離計算需要啁啾時間T_s 和啁啾帶寬B_sweep, 而這些可以依據我們配置時的雷達距離分辨率和最大探測距離計算.

一般來說, 對于 FMCW 雷達系統, 掃描時間*(Ts/Tchirp*)至少應該是雷達最大距離往返時間的5-6倍.

此處我們假設是5.5倍：
$$T_{\text{chirp}}=5.5?2?R_{\max }/c$$
R_max為最大探測距離.

而B_sweep的計算公式為：
$$B_{sweep}=c/2?{\mathrm{delta}}_{?}r$$
delta_r為距離分辨率

Doppler Estimation

Doppler effect

雷達的速度估計是基于一種古老的現象, 叫做多普勒效應. 多普勒效應是波源和觀察者有相對運動時, 觀察者接受到波的頻率與波源發出的頻率并不相同的現象. 根據多普勒理論, 波源與目標接近使得接受和反射的頻率變高, 而波源遠離目標使得接受和反射的頻率變低. 同樣的原理也用于雷達槍來捕捉超速者, 甚至用于體育運動來測量球的速度.

如上所述, 由于目標速度的多普勒效應, 接收到的信號頻率會有一個頻移fD. 多普勒頻移與目標的速度成正比, 如下圖所示.
$$f_D=\frac{2{\nu }_r}{\lambda }$$

• fD: 　多普勒效應導致的傳輸頻率頻移
• νr:　與目標的相對速度
• λ : 　信號的波長

通過測量多普勒引起的頻率偏移, 雷達可以確定速度.

綜上, 拍頻由兩個頻率分量組成: fr(距離引起的頻率增量)和 fd(速度引起的頻率偏移). 值得注意的是在汽車雷達的情況下, fd與 fr相比是非常小的. 因此多普勒速度是通過測量多個啁啾之間的相位變化率來計算的.

下面的公式顯示了相位變化率與頻率之間的關系:
$$\frac{d\Phi }{dt}=\text{?frequency?}$$

Doppler phase shift

運動目標在每次啁啾持續時間內的微小位移會引起的相位變化. 由于每次啁啾的持續時間一般是微秒(μs), 因此會導致毫米級(mm)的微小位移. 這可以用來計算相位變化率, 從而確定多普勒頻率.
$$\begin{array}{c}\Delta \phi =\frac{\Delta x}{\lambda }\left(\lambda =2\pi \text{?or?}360^{°}\right)\\ \Delta \phi =f?\frac{\Delta x}{c}(\lambda =f/c)\\ \Delta f=\frac{\Delta \phi }{\Delta t}\end{array}$$
速度測量的執行代碼:

% Doppler Velocity Calculation c = 3*10^8; %speed of light frequency = 77e9; %frequency in Hz% TODO : Calculate the wavelengthwavelength = c/frequency;% TODO : Define the doppler shifts in Hz using the information from above doppler_shift= [3e3 -4.5e3 11e3 -3e3];% TODO : Calculate the velocity of the targets fd = 2*vr/lambdavr=doppler_shift*wavelength/2;% TODO: Display resultsdisp(vr)

Fast Fourier Transform (FFT)

在本博客中我們不需要研究快速傅里葉變換的數學實現, 只需要了解FFT如何幫助雷達監測.

到目前為止, 我們討論了距離多普勒測量的理論以及計算方程. 但是, 為了使雷達有效地對這些測量數字化處理, 需要將信號從模擬域轉換到數字域, 并進一步從時域轉換到頻域.

ADC (模擬數字轉換器)將模擬信號轉換成數字信號. 由于初始的傳輸信號是時域信號, 快速傅里葉變換被用來將信號從時域轉換到頻域. 頻域轉換是進行信號頻譜分析和確定由距離多普勒引起的頻率偏移的重要手段.

如上圖所示,時域信號包含多個頻率分量. FFT 技術可以分離所有的頻率成分, 給出回波信號的頻率響應.

在上圖中每個峰值的頻譜分別代表不同被檢測目標的特性.

1D FFT

在討論復雜的2D FFT(Range+Doppler)前, 我們先研究下1D FFT(Range).

如上圖所示, Range FFT 運行在每個樣本的每個啁啾上. 每個啁啾(time軸)取樣N次(range 軸), 并為每個樣本產生一個距離塊(range bin). 這個過程對每一次啁啾都會重復,因此它將產生一個 N * (啁啾個數)的Range FFT 塊. 這些 FFT bins的集合也稱為 FFT block.

block每列中的每一個bin表示新增的范圍值, 因此最后一個range bin的末端表示雷達的最大范圍.

下圖是1st FFT (即距離FFT)的輸出. 頻域中的三個峰值對應于距離自身車輛150、240和300米的三輛不同車輛的拍頻.

在MATLAB中我們可以利用FFT 函數對信號樣本N維數進行快速傅里葉變換.

以下為最終項目中1st FFT的執行代碼:

%% RANGE MEASUREMENT% *%TODO* : %reshape the vector into Nr*Nd array. Nr and Nd here would also define the size of %Range and Doppler FFT respectively.Mix_reshape = reshape(Mix,[Nr,Nd]);% *%TODO* : %run the FFT on the beat signal along the range bins dimension (Nr) and %normalize.sig_fft1=fft(Mix_reshape,Nr)./Nr;% *%TODO* : % Take the absolute value of FFT outputsig_fft1=abs(sig_fft1);% *%TODO* : % Output of FFT is double sided signal, but we are interested in only one side of the spectrum. % Hence we throw out half of the samples. sig_fft1=sig_fft1(1:Nr/2);%plotting the range figure ('Name','Range from First FFT') subplot(2,1,1)% *%TODO* :% plot FFT output plot(sig_fft1); axis ([0 200 0 1]);

輸出結果如下圖所示:

2D FFT

之前我們說過, 多普勒頻移可以通過處理多個啁啾之間的相位變化率來實現. 一旦所有的range bins在之前的1st FFT中確定后, 你需要沿著time軸實施2nd FFT, 從而確定多普勒頻移.

1st FFT 的輸出給出了每個目標的拍頻、幅度和相位. 然而當我們從一個啁啾移動到另一個啁啾(每行的一個格到另一個格)時, 由于目標的小位移, 這個相位還是會發生變化. 通過實施2nd FFT , 它能夠確定相位的變化率.

如上圖所示, 通過對各行 FFT bin進行2nd FFT 運算, 可以得到Doppler FFT. 這被稱為二維快速傅里葉變換.

經過二維快速傅里葉變換, 每列中的每個bin代表增加的距離值, 行中的每個bin對應一個速度值.

上圖是距離多普勒響應圖(RDM). 其中一個軸為距離軸, 另一個軸為多普勒速度. RDM通常會被顯示在終端中, 以方便了解對目標的感知.

最終項目中的2D FFT的實現代碼：

%% RANGE DOPPLER RESPONSE % The 2D FFT implementation is already provided here. This will run a 2DFFT % on the mixed signal (beat signal) output and generate a range doppler % map.You will implement CFAR on the generated RDM% Range Doppler Map Generation.% The output of the 2D FFT is an image that has reponse in the range and % doppler FFT bins. So, it is important to convert the axis from bin sizes % to range and doppler based on their Max values.Mix=reshape(Mix,[Nr,Nd]);% 2D FFT using the FFT size for both dimensions. sig_fft2 = fft2(Mix,Nr,Nd);% Taking just one side of signal from Range dimension. sig_fft2 = sig_fft2(1:Nr/2,1:Nd); sig_fft2 = fftshift (sig_fft2); RDM = abs(sig_fft2); RDM = 10*log10(RDM) ;%use the surf function to plot the output of 2DFFT and to show axis in both %dimensions doppler_axis = linspace(-100,100,Nd); range_axis = linspace(-200,200,Nr/2)*((Nr/2)/400); figure,surf(doppler_axis,range_axis,RDM);

值得注意的是, 2D FFT在MATLAB中有內置的函數, 可以一步到位, 無需先單獨運行一遍1D FFT.

下圖是最終項目中2D-FFT的輸出結果.

Clutter

在真實環境中, 雷達信號往往伴隨著大量的噪聲. 雷達不僅接收來自目標的反射信號, 而且還接收來自環境和不需要的目標的反射信號. 來自這些非必要源的散射波被稱為雜波.

去除雜波的一種方法是去除具有0多普勒速度的信號. 由于驅動場景中的雜波往往是由靜止目標產生的, 0多普勒濾波可以有效地去除這些雜波. 0多普勒濾波的缺點是雷達不能檢測到路徑中的靜止目標, 這將導致檢測失敗.

另一種方法是采用固定雜波閾值分割( fixed clutter thresholding). 在固定閾值的情況下, 對閾值以下的信號進行剔除. 該方法在檢測閾值設置過高的情況下, 會出現極少的虛警(false alarms), 但同時也會掩蓋有效目標. 如果閾值設置得太低, 則會導致過多的錯誤警報. 如在下圖中, 固定閾值導致虛警和漏檢弱目標.

虛警率(false alarm rate）是雷達通過噪聲或其他干擾信號發現錯誤信號的速率. 它是在沒有有效目標存在的情況下, 檢測到雷達目標存在的一種度量.

還有一種方法是使用動態閾值分割(dynamic thresholding). 動態閾值分割通過改變閾值水平來降低誤報率. 利用這種名為 CFAR(Constant False Alarm Rate)的技術, 可以監測每一個或每一組距離多普勒bin的噪聲, 并將信號與本地的噪聲水平進行比較. 此比較用于創建一個閾值, 該閾值為CFAR.

CFAR

CFAR 根據車輛周圍環境變化檢測閾值. 通過實現恒定的虛警率, 可以解決虛警問題.

目前有四種CFAR：

• Cell Averaging CFAR (CA-CFAR)
• Ordered Statistics CFAR (OS CFAR)
• Maximum Minimum Statistic (MAMIS CFAR)
• And, multiple variants of CA-CFAR

本文中討論的主要是CA-CFAR.

CA-CFAR

CA-CFAR 是最常用的恒虛警檢測技術. CA-CFAR測量被測單元(CUT)兩側的訓練單元的干擾程度. 然后用這個測量來決定目標是否在被測單元(CUT)中. 該過程遍歷所有的距離多普勒單元, 并根據噪聲估計確定目標的存在. 這一過程的基礎是, 當噪聲存在時, 感興趣的單元周圍的單元包括對噪聲的良好估計, 即假定噪音或干擾在空間上暫時是均勻的. 從理論上講, 它將產生一個不受噪聲和雜波影響的恒虛警率.

FFT bins是在通過多個啁啾的Range Doppler FFT生成的. CA-CFAR使用滑動窗口遍歷整個FFT bins . 每個窗口由以下單元格組成：

• Cell Under Test：通過比較信號電平和噪聲估計值(閾值)來檢測目標是否存在的單元.
• Training Cells：在訓練單元上測量噪聲水平. 訓練單元可以分為兩個區域, 滯后于CUT的單元稱為滯后訓練單元, 而領先于CUT的單元稱為前導訓練單元. 通過對訓練單元下的噪聲進行平均來估計噪聲. 在某些情況下, 采用前導或滯后的噪聲平均值, 而在其他情況下, 則合并前導和滯后的噪聲平均值, 并考慮兩者中較高的一個用于噪聲水平估計. 訓練單元的數量應根據環境確定. 如果交通場景繁忙, 則應使用較少的訓練單元, 因為間隔較近的目標會影響噪聲估計.
• Guard Cells ：緊鄰CUT的單元被指定為保護單元. 保護單元的目的是避免目標信號泄漏到訓練單元中, 這可能會對噪聲估計產生不利影響. 保護單元的數量應根據目標信號從被測單元中泄漏出來的情況來確定. 如果目標反射很強, 它們通常會進入周圍的單元.
• Threshold Factor (Offset)：使用偏移值來縮放噪聲閾值. 如果信號強度以對數形式定義, 則將此偏移值添加到平均噪聲估計中, 否則相乘.

接下來, 我們來實現1維的CA-CFAR.

T : Number of Training Cells

G : Number of Guard Cells

N : Total number of Cells

確定訓練單元和保衛單元的數量

開始在整個FFT 1D陣列上一次滑動一個單元格的窗口. 總窗口大小應為：2（T + G）+ CUT

對于每一步, 將所有前導或滯后訓練單元內的信號（噪聲）相加

對總和求平均值以確定噪聲閾值

使用適當的偏移量值縮放閾值

在CUT中的被測信號從窗口起點T + G + 1開始的

將5中測量的信號與4中測量的閾值進行比較

如果在CUT中測量的信號電平小于所測量的閾值, 則將0值分配給CUT中的信號, 否則分配1.

1D CA-CFAR的代碼實現:

% Implement 1D CFAR using lagging cells on the given noise and target scenario.% Close and delete all currently open figures close all;% Data_points Ns = 1000;% Generate random noise s=abs(randn(Ns,1));%Targets location. Assigning bin 100, 200, 300 and 700 as Targets with the amplitudes of 8, 9, 4, 11. s([100 ,200, 350, 700])=[8 15 7 13];%plot the output plot(s);% TODO: Apply CFAR to detect the targets by filtering the noise.% 1. Define the following: % 1a. Training Cells T = 12; % 1b. Guard Cells G = 4;% Offset : Adding room above noise threshold for desired SNR offset=5;% Vector to hold threshold values threshold_cfar = [];%Vector to hold final signal after thresholding signal_cfar = [];% 2. Slide window across the signal length for i = 1:(Ns-(G+T+1)) % 2. - 5. Determine the noise threshold by measuring it within the training cellsnoise_level =sum(s(i:i+T-1));% 6. Measuring the signal within the CUTthreshold = (noise_level/T)*offset;threshold_cfar=[threshold_cfar,{threshold}];signal=s(i+T+G);% 8. Filter the signal above the thresholdif (signal<threshold)signal=0;endsignal_cfar = [signal_cfar, {signal}]; end% plot the filtered signal plot (cell2mat(signal_cfar),'g--');% plot original sig, threshold and filtered signal within the same figure. figure,plot(s); hold on,plot(cell2mat(circshift(threshold_cfar,G)),'r--','LineWidth',2) hold on, plot (cell2mat(circshift(signal_cfar,(T+G))),'g--','LineWidth',4); legend('Signal','CFAR Threshold','detection')

下圖為一維CA-CFAR的結果：

2D CA-CFAR

二維恒虛警類似于一維恒虛警, 但在距離多普勒塊的兩個維度上都實現了. 2D CA-CFAR包括訓練單元,被測單元以及保護單元, 以防止目標信號對噪聲估計的影響.

接下來, 我們來實現2維的CFAR:

確定每個維度Tr和Td的訓練單元數. 同樣, 選擇保護單元格Gr和Gd的數量.

在整個單元矩陣上滑動待測單元(CUT).

選擇總的包含訓練, 保護和測試單元的網格. Grid Size = (2Tr+2Gr+1)(2Td+2Gd+1).

保護區域和被測單元的總數：(2Gr+1)(2Gd+1).

訓練單元的總數為：(2Tr+2Gr+1)(2Td+2Gd+1) - (2Gr+1)(2Gd+1)

測量并平均所有訓練單元的噪聲, 并獲得閾值.

將偏移量（如果以dB為單位的信號強度）添加到閾值, 以將錯誤警報保持在最低水平.

確定被測單元的信號電平.

如果CUT信號電平大于閾值, 則將值分配為1, 否則將其等于零.

由于被測單元不位于邊緣, 而訓練單元占據了邊緣, 我們將邊緣抑制為零. 任何既不是1也不是0的單元格值, 為其分配一個０.

以下是最終2D-CFAR項目的核心代碼實現:

% *%TODO* : %Select the number of Training Cells in both the dimensions.Tr=10; Td=8;% *%TODO* : %Select the number of Guard Cells in both dimensions around the Cell under %test (CUT) for accurate estimationGr=4; Gd=4;% *%TODO* : % offset the threshold by SNR value in dBoff_set=1.4; % *%TODO* : %design a loop such that it slides the CUT across range doppler map by %giving margins at the edges for Training and Guard Cells. %For every iteration sum the signal level within all the training %cells. To sum convert the value from logarithmic to linear using db2pow %function. Average the summed values for all of the training %cells used. After averaging convert it back to logarithimic using pow2db. %Further add the offset to it to determine the threshold. Next, compare the %signal under CUT with this threshold. If the CUT level > threshold assign %it a value of 1, else equate it to 0.% Use RDM[x,y] as the matrix from the output of 2D FFT for implementing % CFARRDM = RDM/max(max(RDM)); % Normalizing% *%TODO* : % The process above will generate a thresholded block, which is smaller %than the Range Doppler Map as the CUT cannot be located at the edges of %matrix. Hence,few cells will not be thresholded. To keep the map size same % set those values to 0. %Slide the cell under test across the complete martix,to note: start point %Tr+Td+1 and Td+Gd+1 for i = Tr+Gr+1:(Nr/2)-(Tr+Gr)for j = Td+Gd+1:(Nd)-(Td+Gd)%Create a vector to store noise_level for each iteration on training cellsnoise_level = zeros(1,1);%Step through each of bins and the surroundings of the CUTfor p = i-(Tr+Gr) : i+(Tr+Gr)for q = j-(Td+Gd) : j+(Td+Gd)%Exclude the Guard cells and CUT cellsif (abs(i-p) > Gr || abs(j-q) > Gd)%Convert db to powernoise_level = noise_level + db2pow(RDM(p,q));endendend%Calculate threshould from noise average then add the offsetthreshold = pow2db(noise_level/(2*(Td+Gd+1)*2*(Tr+Gr+1)-(Gr*Gd)-1));%Add the SNR to the thresholdthreshold = threshold + off_set;%Measure the signal in Cell Under Test(CUT) and compare againstCUT = RDM(i,j);if (CUT < threshold)RDM(i,j) = 0;elseRDM(i,j) = 1;endend endRDM(RDM~=0 & RDM~=1) = 0;

上圖是2D-CFAR的結果, 可以看到峰值在100ｍ的位置, 對應的速度約為30m/s.

Angle of Arrival(AOA)

相控陣天線是一種天線陣列, 可沿所需方向電子控制波束. 如果陣列中的每個天線元件都被具有特定相位值的信號激發, 則陣列將控制波束. 這種現象稱為光束掃描.

在上圖中, Φ代表移相器. 移相器是改變相位以使光束轉向所需方向的電子組件.

為了使天線波束轉向所需的方向, 移相器被編程為具有恒定的相位增量. 如果一個天線由六個輻射單元組成, 并且將波束引導到一個給定方向所需的相位差為15度, 那么以下是每個單元上的相位值[0,15,30,45,60,75]度. 增量相移隨天線單元之間的間距(d)增加. 使用以下方程確定天線的導向角
$$\Phi =360?d?\sin (\text{theta})/\lambda$$

• Φ= incremental phase shift
• d= spacing between antenna elements
• θ= steering direction from the normal of the antenna surface
• λ= wavelength of the signal

當雷達以程序控制的角度掃描周圍環境時, 它可以感知回波信號的角度. 這有助于雷達創建一個空間感知的環境.

它可以測量空間不同角度目標反射信號的信噪比. 這有助于為雷達的空間感知創建一個到達角/信噪比網格.

Clustering

為了提高自動駕駛的感知能力, 需要分別跟蹤多個目標. 目標跟蹤是計算代價高昂的, 同時跟蹤多個目標需要大量的處理能力和內存. 隨著雷達技術的進步和傳感分辨率的提高, 雷達可以根據目標上散射點的點數生成檢測結果. 因此, 重要的是將來自單個目標的所有檢測結果聚類, 并為每個目標分配一條航跡.

在這里, 我們將討論基于歐幾里得度量的基本聚類算法. 歐幾里得聚類根據檢測點之間距離的測量值對檢測點進行分組.

聚類算法將目標大小內的所有檢測點視為一個簇, 合并為一個質心位置, 并且為每個聚類分配一個新的距離和速度. 這是聚類的所有檢測點的測量距離和速度的平均值, 從而方便對每個目標進行有效跟蹤.

上面是集群場景的一個示例. 在圖像中, 藍色的汽車是一輛自我車(帶傳感器的汽車) , 探測器是由橙色和黃色的汽車產生的. 利用聚類算法將單個目標相關的所有檢測點合并成一個點. 這有助于檢測并將航跡分配給目標.

聚類的代碼實現：

%%% % *|clusterDetections|* % % This function merges multiple detections suspected to be of the same % vehicle to a single detection. The function looks for detections that are % closer than the size of a vehicle. Detections that fit this criterion are % considered a cluster and are merged to a single detection at the centroid % of the cluster. The measurement noises are modified to represent the % possibility that each detection can be anywhere on the vehicle. % Therefore, the noise should have the same size as the vehicle size. % % In addition, this function removes the third dimension of the measurement % (the height) and reduces the measurement vector to [x;y;vx;vy]. function detectionClusters = clusterDetections(detections, vehicleSize) N = numel(detections); distances = zeros(N); for i = 1:Nfor j = i+1:Nif detections{i}.SensorIndex == detections{j}.SensorIndexdistances(i,j) = norm(detections{i}.Measurement(1:2) - detections{j}.Measurement(1:2));elsedistances(i,j) = inf;endend end leftToCheck = 1:N; i = 0; detectionClusters = cell(N,1); while ~isempty(leftToCheck) % Remove the detections that are in the same cluster as the one under% consideration%TODO : Complete the clustering loop based on the implementation%discussed in the lesson underConsideration = leftToCheck(1);clusterInds = (distances(underConsideration,leftToCheck) < vehicleSize);detInds = leftToCheck(clusterInds);clusterDets = [detections{detInds}];clusterMeas = [clusterDets.Measurement];meas = mean(clusterMeas,2);meas2D = [meas(1:2);meas(4:5)];i = i+1;detectionClusters{i} = detections{detInds(1)};detectionClusters{i}.Measurement = meas2D;leftToCheck(clusterInds) = [];end detectionClusters(i+1:end) = [];

值得注意的是這里使用了MATLAB的自動駕駛工具箱.

上面的聚類實現使用以下步驟:

如果探測結果是來自同一個傳感器, 那么循環每一個單一的探測點, 并測量它們之間的歐幾里得度量.

繼續運行循環, 直到檢測列表為空

在While循環中實現以下內容:

選擇檢查列表中的第一個檢測, 并檢查其聚類鄰居.

如果第一個探測點和剩余探測點之間的距離小于車輛大小, 那么將這些探測點和它們各自的參數(距離和速度)分組.

對于該組, 采取的距離和速度測量的平均值. (雷達測量向量有6個值, 其中 x 和 y 坐標的距離和速度位于指數1,2,4和5: [ x, y,-, Vx, Vy,-])

創建一個新的集群 ID, 然后將所有組檢測分配給相同的 ID.

接下來, 分配聚類, 平均距離和速度.

最后, 從列表中刪除已經分配給一個集群的探測點.

繼續重復這個過程, 直到檢測列表為空

Kalman Filter

卡爾曼濾波器的目的是估計履帶車輛的狀態. 在這里, “狀態”可以包括位置, 速度, 加速度或其他性能的車輛被跟蹤. 卡爾曼濾波器使用長時間觀測到的含有噪聲或隨機變化和其他不準確性的測量值, 產生的數值往往更接近測量值的真實值及其相關的計算值. 它是大多數現代雷達跟蹤系統的核心算法.

x^k, the state of the vehicle at the kth step.

A, the state-transition model

Pk, the state covariance matrix - state estimation covariance error

B, control matrix - external influence

C, the observation/measurement model

Q, the covariance of the process noise

R, the covariance of the observation noise

在這里, 我們將簡單介紹卡爾曼濾波器, 之后我會詳細的在一篇博客中介紹卡爾曼濾波.

預測步驟

利用目標車輛的運動模型, 根據當前狀態預測目標車輛的未來狀態. 由于我們從上一個時間戳知道目標的當前位置和速度, 我們可以預測下一個時間戳的目標位置.

例如, 使用恒速模型, 目標車輛的新位置可以計算為：
$$x_{new}=x_{prev}+v?t$$

更新步驟

在這里, 卡爾曼濾波器使用來自傳感器的嘈雜的測量數據, 并將該數據與上一步的預測相結合, 以生成狀態的最佳估計.

在MATLAB自動駕駛工具箱中已經內置了卡曼濾波函數.

% This function initializes a constant velocity filter based on a detection. function filter = initSimDemoFilter(detection) % Use a 2-D constant velocity model to initialize a trackingKF filter. % The state vector is [x;vx;y;vy] % The detection measurement vector is [x;y;vx;vy] % As a result, the measurement model is H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1]%TODO: Implement the Kalman filter using trackingKF function. If stuck %review the implementation discussed in the project walkthrough H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1]; filter = trackingKF('MotionModel', '2D Constant Velocity', ...'State', H' * detection.Measurement, ...'MeasurementModel', H, ...'StateCovariance', H' * detection.MeasurementNoise * H, ...'MeasurementNoise', detection.MeasurementNoise); end

聚類和卡曼濾波在最終項目中并沒有使用, 請參考我的GITHUB庫中radar simulator代碼.

以下是最終結果:

Linkedin: https://linkedin.com/in/williamhyin

總結

以上是生活随笔為你收集整理的自动驾驶毫米波雷达物体检测技术-算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。