【五】FMCW角度测量

阵列结构

通过查询BGT60TR13C的数据手册，我们可以发现这款毫米波雷达是L型阵列；水平方向2个RX垂直方向2个RX，可以同时测量水平角（Azimuth）和俯仰角（Elevation）。

Pasted image 20260508154054

测角的本质是检测接收信号的相位变化，因此通过Rx1，Rx3的相位差可以得到水平角；Rx2，Rx3的相位差可以得到俯仰角。

单脉冲法

单脉冲法的核心逻辑是利用电磁波到达不同位置天线的时间差（即路程差）所导致的相位偏移来反推目标的方位。它专注于视野中最接近的运动物体，而不需要全空间的扫描，因此计算量小，适合部署到边缘设备中。

几何模型

假设有两个接收天线之间的间距为 $d$ 。当远场目标以角度 $\theta$ 入射时，到达两个天线的波程存在差异，即波程差为 $\Delta s = d \cdot \sin(\theta)$ 。而这个路程差在信号相位上表现为 $\Delta \phi$ 。相位差与波程差的关系如下：

\Delta \phi = \frac{2\pi \cdot \Delta s}{\lambda} = \frac{2\pi \cdot d \cdot \sin(\theta)}{\lambda}

解得

\theta = \arcsin\left( \frac{\Delta \phi \cdot \lambda}{2\pi \cdot d} \right)

因此可以得到求解角度的基本思路。

基本思路

在RD图中锁定目标点 $(r, v)$ 后，提取对应的复数数据 $X_1 = |X_1| e^{j\phi_1}$ 和 $X_3 = |X_3| e^{j\phi_3}$ 。
利用复数共轭相乘来提取相位差： $\Delta \phi = \text{angle}(X_1 \cdot X_3^*) = \phi_1 - \phi_3$
应用 $\arcsin$ 公式得到 $\theta$ 。

代码实现

由于RX3是水平角和俯仰角都需要的一个阵元，因此我们用它得到的RD图作为标准，检测最大值所在的坐标。

num_ant = 3 # 天线数量
mti_alpha = 0.8 # MTI滤波器的平滑系数
mti_history = np.zeros((config.num_chirps, config.chirp.num_samples, num_ant))

range_window = signal.windows.blackmanharris(config.chirp.num_samples).reshape(1, config.chirp.num_samples)
doppler_window = signal.windows.blackmanharris(config.num_chirps).reshape(config.num_chirps, 1)

range_skip = 8

frame_contents = device.get_next_frame()
frame_contents_squeezed = np.squeeze(frame_contents, 0)

ant_num = frame_contents_squeezed.shape[0]
RD_map_all = np.zeros_like(frame_contents_squeezed, dtype=np.complex128) # 用于存储每个天线的RD图历史，供MTI使用
doppler_peak_value = np.zeros((ant_num, 1), dtype=np.complex128) # 用于存储每个天线的峰值位置索引，供后续分析使用

doppler_peak_index = np.zeros(2, dtype=int) # 用于存储每个天线的峰值位置索引，供后续分析使用

# 得到三个RX的RD图 优先处理RX3，寻找峰值位置，后续RX1和RX2在该位置寻找对应的值进行角度计算
for i_ant in range(ant_num - 1, -1, -1):
  mat = frame_contents_squeezed[i_ant, :, :]

  # Step 1 - 去除杂波
  data = mat - np.average(mat)

  # Step 2 - MTI去除静态物体
  data_mti = data - mti_history[:, :, i_ant]
  mti_history[:, :, i_ant] = data * mti_alpha + mti_history[:, :, i_ant] * (1 - mti_alpha)

  # Step 3 - 计算RD图
  doppler_fft = tools.fft_2d_spectrum(data_mti, range_window, doppler_window)

  # Step 4 - 以RX3为参考天线，寻找峰值位置
  if i_ant == 2:
    # 采用OS CFAR算法做目标识别
    cfar_result = tools.os_cfar_2d(doppler_fft, guard_len=4, ref_len=4, rank=156, scale=1.5)
    after_cfar = cfar_result * doppler_fft

    # 提取RX3最大值所在位置
    doppler_peak_index = np.unravel_index(np.argmax(np.abs(after_cfar[:, range_skip:])), after_cfar[:, range_skip:].shape)

  # Step 5 - 提取每个RX这个位置对应的复数值
  doppler_peak_value[i_ant] = doppler_fft[doppler_peak_index[0], doppler_peak_index[1]+range_skip]

# Step 6 - 方位角计算
delta_phi_1_3 = np.angle(doppler_peak_value[0] * np.conj(doppler_peak_value[2])) # 计算RX1和RX3的峰值之间的相位差
delta_phi_2_3 = np.angle(doppler_peak_value[1] * np.conj(doppler_peak_value[2])) # 计算RX2和RX3的峰值之间的相位差

azimuth_arg = delta_phi_1_3[0] * wavelength_m / (2 * math.pi * d_m)
elevation_arg = delta_phi_2_3[0] * wavelength_m / (2 * math.pi * d_m)

azimuth_arg = np.clip(azimuth_arg, -1.0, 1.0)
elevation_arg = np.clip(elevation_arg, -1.0, 1.0)

azimuth_angle_rad = np.arcsin(azimuth_arg) # 水平方位角计算：根据相位差计算水平角度
elevation_angle_rad = np.arcsin(elevation_arg) # 垂直方位角计算：根据相位差计算垂直角度

azimuth_deg = float(np.degrees(azimuth_angle_rad))
elevation_deg = float(np.degrees(elevation_angle_rad))

print(f"Azimuth Angle: {azimuth_deg:.2f} degrees, Elevation Angle: {elevation_deg:.2f} degrees")

未完待续。如果学了其它角度检测方法会及时更新。

← Back to blog