自动驾驶环境感知之毫米波雷达运动目标解析项目实战

目的：根据设置的雷达和目标参数生成模拟数据，并进行参数解析

1. 计算距离和速度的分辨率和测量范围，以及角度的测量范围

（1）代码实现

import numpy as np
from matplotlib import pyplot as plt


B = 0.5 * 1e9            # 带宽(GHz)
Tc = 40 * 1e-6           # Chirp时间(s)
S = B / Tc               # 频率变化率(GHz/s)
fc = 79 * 1e9            # 起始频率(GHz)
c = 3 * 1e8              # 光速(m/s)
wl = c / fc              # 波长(m)  \lambda
l =  wl / 2              # 接收天线间距(m)
chirp_num = 128          # 每帧Chirp数量       M
sample_num = 1024        # 每个Chirp的采样数量  N
antenna_num = 8          # 天线数量            K
angle_num = 180          # 角度数量
Fs = sample_num / Tc     # 数模转换频率

'''
计算距离和速度的分辨率和测量范围，以及角度的测量范围
    "1. 距离FFT的结果具有对称性, 因此只能采用一半的频谱。可以将FFT后的结果进行可视化, 加以验证
    "2. 速度的值是有正负的(符号表示速度方向, 0表示相对雷达静止)
    "3. 角度的值是有正负的(符号表示左右, 0度是雷达天线轴(Boresight)的方向)
'''

r_max = sample_num * c / (2 * B)
delta_r = c / (2 * B)
print('可测得的最大距离为: {:f}米'.format(r_max))
print('可测得的最小分辨距离为: {:f}米'.format(delta_r))
v_max = wl / (4 * Tc)
delta_v = wl / (2 * Tc * chirp_num)
print('可测得的速度范围为: ({:f}, {:f})'.format(-v_max, v_max))
print('可测得的最小分辨速度为: {:f}米/秒'.format(delta_v))
theta_max = np.arcsin(wl / (2*l))
print('可测得的最大角度为: {:f}rad'.format(theta_max))

R = 50  # 距离
D = 10  # 速度
A = 20  # 角度

# 生成模拟的雷达信号 (单目标)
const_a = l/wl * np.sin(np.deg2rad(A))
const_d = 2*fc/c*Tc * D
const_r = 2*S/c/sample_num*Tc * R
range_a = np.arange(0, antenna_num)
range_d = np.arange(0, chirp_num)
range_r = np.arange(0, sample_num)
grid_adr = np.stack(np.meshgrid(range_a, range_d, range_r, indexing='ij'), axis=-1)
signal = grid_adr * np.array([const_a, const_d, const_r])
signal = np.cos(2*np.pi * np.sum(signal, axis=-1))

# 距离FFT
s_r = np.fft.fft(signal, axis=2)
plt.plot(np.abs(s_r[0, 0, :]))  # 验证距离FFT结果的对称性
plt.xlabel('Range bin')
plt.ylabel('Amplitude')
s_r = s_r[:, :, 0:int(sample_num/2)]  # 考虑到对实数做FFT后频谱的对称性，只采用前半部分信号

# 速度FFT
s_rd = np.fft.fft(s_r, axis=1)

# 角度FFT
s_rda = np.fft.fft(s_rd, axis=0, n=angle_num) # 需要设置输出的角度Bin的个数

# 可视化 Range-Doppler map
rd_map = np.mean(np.abs(s_rd),axis=0)
plt.figure(figsize=(20,10))
plt.imshow(rd_map,origin='lower')
plt.xlabel('Range bin')
plt.ylabel('Doppler bin')
plt.show()



# 估计目标的距离，速度
# 在Range-Doppler map上找到range和doppler的峰值(rd_map中峰值处的索引)
[d_peak, r_peak] = np.unravel_index(np.argmax(rd_map, axis=None), rd_map.shape)

# 估计目标的距离
Delta_f = r_peak / sample_num * Fs # 计算range峰值对应的频率差（range bin转换为频率
R_est = Delta_f * c / (2*S)        # 公式: r = delta_f * c / (2S)
print("目标距离: {:f}米".format(R_est))

# 估计目标的速度
if d_peak >= chirp_num/2: # 频谱的后半段对应负的速度
    d_peak -= chirp_num
Delta_phi = d_peak / chirp_num * 2*np.pi # 计算doppler峰值对应的相位差（doppler bin转换为相位
D_est = Delta_phi * wl / (4*np.pi * Tc)  # 公式: v = (delta_phi * \lambda) / (4*pi*Tc)
print("目标速度: {:f}米/秒".format(D_est))

（2）运行结果

（3）可视化结果及FFT对称性验证

2 估计目标的角度

（1）代码实现

import numpy as np
from matplotlib import pyplot as plt


'''
估计目标的角度
    1. 在Range和Doppler的峰值位置, 找到Angle频谱的峰值位置
    2. 频谱的后半段对应负的角度（参考速度的处理
    3. 越靠近90度时, 角度估计越不准确
'''

GUARD_CELLS = 1
BG_CELLS = 1
ALPHA = 2*1e-9
CFAR_UNITS = 1 + (GUARD_CELLS*2) + (BG_CELLS*2)

def cfar(singal):
    singal_out = np.zeros((singal.shape[0], singal.shape[1], singal.shape[2], 3), np.uint8)
    # 滑窗遍历
    for i in range(singal.shape[0] - CFAR_UNITS):
        center_cell_x = i + BG_CELLS + GUARD_CELLS
        for j in range(singal.shape[1] - CFAR_UNITS):
            center_cell_y = j + BG_CELLS + GUARD_CELLS
            for k in range(singal.shape[2] - CFAR_UNITS):
                center_cell_z = k + BG_CELLS + GUARD_CELLS
                average = 0
                # 处理滑窗
                for m in range(CFAR_UNITS):
                    for n in range(CFAR_UNITS):
                        for l in range(CFAR_UNITS):
                            # 剔除保护单元
                            if (m >= BG_CELLS) and (m < (CFAR_UNITS - BG_CELLS)) and (n >= BG_CELLS) and (n < (CFAR_UNITS - BG_CELLS)) \
                                and (l >= BG_CELLS) and (l < (CFAR_UNITS - BG_CELLS)):
                                continue
                            # 计算滑窗邻域内的平均值
                            average += singal[i+m][j+n][k+l]
                average /= (CFAR_UNITS * CFAR_UNITS * CFAR_UNITS) - ((GUARD_CELLS*2+1) * (GUARD_CELLS*2+1) * (GUARD_CELLS*2+1))
                print(singal[center_cell_x][center_cell_y][center_cell_z])
                print(average * ALPHA)
                if singal[center_cell_x][center_cell_y][center_cell_z] > (average * ALPHA):
                    singal_out[center_cell_x][center_cell_y][center_cell_z] = singal[center_cell_x][center_cell_y][center_cell_z]
    
    return singal_out


B = 0.5 * 1e9
Tc = 40 * 1e-6
S = B / Tc
fc = 79 * 1e9
c = 3 * 1e8
wl = c / fc
l =  wl / 2
chirp_num = 128
sample_num = 1024
antenna_num = 8
angle_num = 180
Fs = sample_num / Tc

# 设置目标信息  表示有2个目标
R = [20, 100] # range of targets (m)
D = [10, 20]  # doppler of targets (m/s)
A = [20, 40]  # angle of targets (degree)
target_num = len(R)

# 生成模拟的雷达信号 (多目标)
const_a = l/wl 
const_d = 2*fc/c*Tc
const_r = 2*S/c/sample_num*Tc
range_a = np.arange(0, antenna_num)
range_d = np.arange(0, chirp_num)
range_r = np.arange(0, sample_num)
grid_adr = np.stack(np.meshgrid(range_a, range_d, range_r, indexing='ij'), axis=-1)
signal = np.zeros([antenna_num, chirp_num, sample_num], dtype=np.float)
for i in range(target_num):
    single_target = grid_adr * np.array([const_a*np.sin(np.deg2rad(A[i])), const_d*D[i], const_r*R[i]])
    single_target = np.cos(2*np.pi * np.sum(single_target, axis=-1))
    signal += single_target  # 多目标信号叠加

    # 距离FFT
    s_r = np.fft.fft(signal, axis=2)
    s_r = s_r[:, :, 0:int(sample_num/2)]
    # 速度FFT
    s_rd = np.fft.fft(s_r, axis=1)
    # 角度FFT
    s_ra = np.fft.fft(s_r, axis=0, n=angle_num)
    rd_map = np.mean(np.abs(s_ra), axis=0)

    [a_peak, d_peak] = np.unravel_index(np.argmax(rd_map, axis=None), rd_map.shape)

    # 估计目标角度
    if a_peak >= angle_num/2:
        a_peak -= angle_num
    Delta_phi = a_peak / angle_num * 2*np.pi
    theta = np.arcsin((wl * Delta_phi) / (2 * np.pi * l))
    print("目标{:d}角度: {:f}rad".format(i+1, theta))


# 可视化
plt.figure(figsize=(20,10))
plt.imshow(rd_map,origin='lower')
plt.xlabel('Angle bin')
plt.ylabel('Range bin')
plt.show()

（2）多目标角度估计结果

（3）FFT可视化结果

3. 估计多个目标的距离，速度和角度

（1）代码实现

import numpy as np
from matplotlib import pyplot as plt


B = 0.5 * 1e9
Tc = 40 * 1e-6
S = B / Tc
fc = 79 * 1e9
c = 3 * 1e8
wl = c / fc
l =  wl / 2
chirp_num = 128
sample_num = 1024
antenna_num = 8
angle_num = 180
Fs = sample_num / Tc

'''
估计多个目标的距离，速度和角度
    "1. 生成多目标的模拟信号
    "2. 3次FFT处理,可视化Range-Doppler map
    "3. 在Range-Doppler map上找到多个目标对应的峰值(可以调用库函数)
    "4. 设定一个合适的噪声阈值，过滤掉假目标"
'''

GUARD_CELLS = 20
BG_CELLS = 10
ALPHA = 0.5
CFAR_UNITS = 1 + (GUARD_CELLS*2) + (BG_CELLS*2)

def cfar_2d(rd_map):
    rd_map_out = np.zeros((rd_map.shape[0], rd_map.shape[1]), np.uint)
    # 滑窗遍历
    for i in range(rd_map.shape[0] - CFAR_UNITS):
        center_cell_x = i + BG_CELLS + GUARD_CELLS
        for j in range(rd_map.shape[1] - CFAR_UNITS):
            center_cell_y = j + BG_CELLS + GUARD_CELLS
            average = 0
            # 处理滑窗
            for m in range(CFAR_UNITS):
                for n in range(CFAR_UNITS):
                    # 剔除保护单元
                    if (m >= BG_CELLS) and (m < (CFAR_UNITS - BG_CELLS)) and (n >= BG_CELLS) and (n < (CFAR_UNITS - BG_CELLS)):
                        continue
                    # 计算滑窗邻域内的平均值
                    average += rd_map[i+m][j+n]
            average /= (CFAR_UNITS * CFAR_UNITS) - ((GUARD_CELLS*2+1) * (GUARD_CELLS*2+1))
            if rd_map[center_cell_x][center_cell_y] > (average * ALPHA):
                rd_map_out[center_cell_x][center_cell_y] = rd_map[center_cell_x][center_cell_y]
    
    return rd_map_out

# 设置目标信息  表示有2个目标
R = [20, 100] # range of targets (m)
D = [10, 20] # doppler of targets (m/s)
A = [20, 40] # angle of targets (degree)
target_num = len(R)

# 生成模拟的雷达信号 (多目标)
const_a = l/wl 
const_d = 2*fc/c*Tc
const_r = 2*S/c/sample_num*Tc
range_a = np.arange(0, antenna_num)
range_d = np.arange(0, chirp_num)
range_r = np.arange(0, sample_num)
grid_adr = np.stack(np.meshgrid(range_a, range_d, range_r, indexing='ij'), axis=-1)
signal = np.zeros([antenna_num, chirp_num, sample_num], dtype=np.float)
for i in range(target_num):
    single_target = grid_adr * np.array([const_a*np.sin(np.deg2rad(A[i])), const_d*D[i], const_r*R[i]])
    single_target = np.cos(2*np.pi * np.sum(single_target, axis=-1))
    signal += single_target  # 多目标信号叠加

    # 距离FFT
    s_r = np.fft.fft(signal, axis=2)
    s_r = s_r[:, :, 0:int(sample_num/2)]
    # 速度FFT
    s_rd = np.fft.fft(s_r, axis=1)
    # 角度FFT
    s_ra = np.fft.fft(s_r, axis=0, n=angle_num)
    s_rda = np.fft.fft(s_rd, axis=0, n=angle_num)

    ad_map = np.mean(np.abs(s_rda), axis=2)  # [a d]
    a_map = np.mean(np.abs(ad_map), axis=1)  # [a]
    rd_map = np.mean(np.abs(s_rd), axis=0)   # [d r]
    d_map = np.mean(np.abs(rd_map), axis=1)  # [d]
    r_map = np.mean(np.abs(rd_map), axis=0)  # [r]

    [a_peak] = np.unravel_index(np.argmax(a_map, axis=None), a_map.shape)
    [d_peak] = np.unravel_index(np.argmax(d_map, axis=None), d_map.shape)
    [r_peak] = np.unravel_index(np.argmax(r_map, axis=None), r_map.shape)

    # 估计目标的距离
    Delta_f = r_peak / sample_num * Fs # 计算range峰值对应的频率差（range bin转换为频率
    R_est = Delta_f * c / (2*S)        # 公式: r = delta_f * c / (2S)
    print('目标{:d}的距离为: {:f}米'.format(i+1, R_est))

    # 估计目标的速度
    if d_peak >= chirp_num/2: # 频谱的后半段对应负的速度
        d_peak -= chirp_num
    Delta_phi = d_peak / chirp_num * 2*np.pi # 计算doppler峰值对应的相位差（doppler bin转换为相位
    D_est = Delta_phi * wl / (4*np.pi * Tc)  # 公式: v = (delta_phi * \lambda) / (4*pi*Tc)
    print('目标{:d}的速度为: {:f}米/秒'.format(i+1, D_est))

    # 估计目标角度
    if a_peak >= angle_num/2:
        a_peak -= angle_num
    Delta_phi = a_peak / angle_num * 2*np.pi
    theta = np.arcsin((wl * Delta_phi) / (2 * np.pi * l))
    print('目标{:d}的角度为: {:f}rad'.format(i+1, theta))

# 可视化
plt.figure(figsize=(20,10))
plt.imshow(rd_map,origin='lower')
plt.xlabel('Angle bin')
plt.ylabel('Range bin')
plt.show()


# 过滤假目标
rd_map = cfar_2d(rd_map)

# 可视化
plt.figure(figsize=(20,10))
plt.imshow(rd_map,origin='lower')
plt.xlabel('Angle bin')
plt.ylabel('Range bin')
plt.show()

（2）运行结果

（3）可视化结果

CFAR过滤后可视化结果如下：

4 调整雷达参数, 区分距离或速度相近的目标

（1）调整雷达参数，使其可区分20.0米和20.2米的目标，以及10.2米/秒和10.4米/秒的目标。为使在距离和速度上同时能够区分，根据距离和速度分辨率，雷达调整参数如下：
$$B=0.75\ast 1e9fc=146.484375\ast 1e9$$
（2）代码实现

import numpy as np
from matplotlib import pyplot as plt


B = 0.75 * 1e9
Tc = 40 * 1e-6
S = B / Tc
fc = 146.484375 * 1e9
c = 3 * 1e8
wl = c / fc
l =  wl / 2
chirp_num = 128
sample_num = 1024
antenna_num = 8
angle_num = 180
Fs = sample_num / Tc

delta_r = c / (2 * B)
print("可测得的最小分辨距离: ", delta_r)
delta_v = wl / (2 * Tc * chirp_num)
print('可测得的最小分辨速度: ', delta_v)


'''
调整雷达参数, 区分距离或速度相近的目标
    "任务1: 区分距离为20.0米和20.2米的目标（速度和角度随意
    "任务2: 区分速度为10.2米/秒和10.4米/秒的目标(距离和角度随意)
    说明
    "1. 按照作业3中的流程生成多个目标的模拟数据, 并解析它们的距离和速度
    "2. 调整雷达参数, 使得任务1中的目标在距离上可以区分
    "3. 调整雷达参数, 使得任务2中的目标在速度上可以区分
'''

# 设置目标信息  表示有2个目标
R = [20, 20.2] # range of targets (m)
D = [10.2, 10.4] # doppler of targets (m/s)
A = [20, 30] # angle of targets (degree)
target_num = len(R)

# 生成模拟的雷达信号 (多目标)
const_a = l/wl 
const_d = 2*fc/c*Tc
const_r = 2*S/c/sample_num*Tc
range_a = np.arange(0, antenna_num)
range_d = np.arange(0, chirp_num)
range_r = np.arange(0, sample_num)
grid_adr = np.stack(np.meshgrid(range_a, range_d, range_r, indexing='ij'), axis=-1)
signal = np.zeros([antenna_num, chirp_num, sample_num], dtype=np.float)
for i in range(target_num):
    single_target = grid_adr * np.array([const_a*np.sin(np.deg2rad(A[i])), const_d*D[i], const_r*R[i]])
    single_target = np.cos(2*np.pi * np.sum(single_target, axis=-1))
    signal += single_target  # 多目标信号叠加

    # 距离FFT
    s_r = np.fft.fft(signal, axis=2)
    s_r = s_r[:, :, 0:int(sample_num/2)]
    # 速度FFT
    s_rd = np.fft.fft(s_r, axis=1)
    # 角度FFT
    s_ra = np.fft.fft(s_r, axis=0, n=angle_num)
    s_rda = np.fft.fft(s_rd, axis=0, n=angle_num)

    ad_map = np.mean(np.abs(s_rda), axis=2)  # [a d]
    a_map = np.mean(np.abs(ad_map), axis=1)  # [a]
    rd_map = np.mean(np.abs(s_rd), axis=0)   # [d r]
    d_map = np.mean(np.abs(rd_map), axis=1)  # [d]
    r_map = np.mean(np.abs(rd_map), axis=0)  # [r]

    [a_peak] = np.unravel_index(np.argmax(a_map, axis=None), a_map.shape)
    [d_peak] = np.unravel_index(np.argmax(d_map, axis=None), d_map.shape)
    [r_peak] = np.unravel_index(np.argmax(r_map, axis=None), r_map.shape)

    # 估计目标的距离
    Delta_f = r_peak / sample_num * Fs # 计算range峰值对应的频率差（range bin转换为频率
    R_est = Delta_f * c / (2*S)        # 公式: r = delta_f * c / (2S)
    print('目标{:d}的距离为: {:f}米'.format(i+1, R_est))

    # 估计目标的速度
    if d_peak >= chirp_num/2: # 频谱的后半段对应负的速度
        d_peak -= chirp_num
    Delta_phi = d_peak / chirp_num * 2*np.pi # 计算doppler峰值对应的相位差（doppler bin转换为相位
    D_est = Delta_phi * wl / (4*np.pi * Tc)  # 公式: v = (delta_phi * \lambda) / (4*pi*Tc)
    print('目标{:d}的速度为: {:f}米/秒'.format(i+1, D_est))

    # 估计目标角度
    if a_peak >= angle_num/2:
        a_peak -= angle_num
    Delta_phi = a_peak / angle_num * 2*np.pi
    theta = np.arcsin((wl * Delta_phi) / (2 * np.pi * l))
    print('目标{:d}的角度为: {:f}rad'.format(i+1, theta))

（3）运行结果