1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217
| """ AWRL6432 CPD 算法实现 基于 TI 毫米波 SDK 参考代码 适用于 ARM Cortex-R4F + 硬件加速器 """
import numpy as np from scipy import signal from typing import Tuple, Optional
class CPDRadarProcessor: """ 儿童存在检测雷达信号处理器 Pipeline: 1. ADC 数据采集 → Range-FFT 2. Range-FFT → Doppler-FFT 3. 静止杂波去除 (MTI) 4. 微多普勒提取 (相位变化) 5. 生命体征检测 (呼吸/心跳) """ def __init__(self, config: dict): self.fps = config.get('fps', 17) self.num_chirps = config.get('num_chirps', 64) self.num_adc = config.get('num_adc', 256) self.num_rx = config.get('num_rx', 4) self.breath_range = (0.15, 0.5) self.heart_range = (0.8, 2.0) self.min_motion = 0.1 def process_frame(self, adc_data: np.ndarray) -> dict: """ 处理一帧雷达数据 Args: adc_data: (num_chirps, num_adc, num_rx) complex Returns: results: { 'range_profile': 距离剖面, 'doppler_profile': 速度剖面, 'static_targets': 静止目标, 'breathing_rate': 呼吸频率 (Hz), 'heart_rate': 心率 (Hz), 'presence': 是否存在生命, 'confidence': 置信度 } """ range_fft = np.fft.fft(adc_data, axis=1) range_profile = np.abs(range_fft).mean(axis=0) doppler_fft = np.fft.fft(range_fft, axis=0) doppler_profile = np.abs(doppler_fft).mean(axis=2) static_bin = doppler_profile[self.num_chirps // 2, :] static_targets = self._find_static_targets(static_bin) phase_data = self._extract_phase(range_fft, static_targets) breathing = self._detect_breathing(phase_data) heart = self._detect_heart(phase_data, breathing) presence = breathing is not None or heart is not None confidence = self._calc_confidence(phase_data, breathing, heart) return { 'range_profile': range_profile, 'doppler_profile': doppler_profile, 'static_targets': static_targets, 'breathing_rate': breathing, 'heart_rate': heart, 'presence': presence, 'confidence': confidence } def _find_static_targets(self, static_bin: np.ndarray) -> list: """从静止通道检测目标""" threshold = np.mean(static_bin) + 2 * np.std(static_bin) targets = np.where(static_bin > threshold)[0] range_res = 0.04 return [(idx * range_res, static_bin[idx]) for idx in targets] def _extract_phase(self, range_fft: np.ndarray, targets: list) -> np.ndarray: """ 提取目标位置的相位变化时序 这是 CPD 的核心:通过相位变化检测呼吸/心跳 """ if not targets: return np.array([]) target_idx = max(targets, key=lambda x: x[1])[0] / 0.04 target_idx = int(target_idx) phase = np.angle(range_fft[:, target_idx, :]) phase_unwrapped = np.unwrap(phase, axis=0) phase_avg = phase_unwrapped.mean(axis=1) return phase_avg def _detect_breathing(self, phase: np.ndarray) -> Optional[float]: """检测呼吸频率""" if len(phase) < 64: return None nyq = self.fps / 2 b, a = signal.butter(2, [self.breath_range[0]/nyq, self.breath_range[1]/nyq], btype='band') filtered = signal.filtfilt(b, a, phase) freqs = np.fft.rfftfreq(len(filtered), 1/self.fps) spectrum = np.abs(np.fft.rfft(filtered)) mask = (freqs >= self.breath_range[0]) & (freqs <= self.breath_range[1]) if not mask.any() or spectrum[mask].max() < 0.1: return None peak_freq = freqs[mask][np.argmax(spectrum[mask])] return peak_freq def _detect_heart(self, phase: np.ndarray, breathing: Optional[float]) -> Optional[float]: """检测心率""" if len(phase) < 128: return None if breathing is not None: nyq = self.fps / 2 b, a = signal.butter(2, [self.breath_range[0]/nyq, self.breath_range[1]/nyq], btype='bandstop') phase = signal.filtfilt(b, a, phase) nyq = self.fps / 2 b, a = signal.butter(2, [self.heart_range[0]/nyq, self.heart_range[1]/nyq], btype='band') filtered = signal.filtfilt(b, a, phase) freqs = np.fft.rfftfreq(len(filtered), 1/self.fps) spectrum = np.abs(np.fft.rfft(filtered)) mask = (freqs >= self.heart_range[0]) & (freqs <= self.heart_range[1]) if not mask.any() or spectrum[mask].max() < 0.05: return None peak_freq = freqs[mask][np.argmax(spectrum[mask])] return peak_freq def _calc_confidence(self, phase, breathing, heart): """计算检测置信度""" if phase is None or len(phase) == 0: return 0.0 motion = np.std(phase) conf = 0.0 if breathing is not None: conf += 0.5 if 0.2 <= breathing <= 0.4: conf += 0.2 if heart is not None: conf += 0.3 if 1.0 <= heart <= 1.8: conf += 0.1 return min(conf, 1.0)
if __name__ == "__main__": processor = CPDRadarProcessor({ 'fps': 17, 'num_chirps': 64, 'num_adc': 256, 'num_rx': 4 }) t = np.linspace(0, 64/17, 64) breathing_signal = 0.5 * np.sin(2 * np.pi * 0.3 * t) noise = 0.05 * np.random.randn(64, 256, 4) adc_data = noise.copy() adc_data[:, 128, :] += breathing_signal[:, np.newaxis, np.newaxis] result = processor.process_frame(adc_data.astype(np.complex64)) print(f"Static targets: {len(result['static_targets'])}") print(f"Breathing rate: {result['breathing_rate']:.3f} Hz" if result['breathing_rate'] else "No breathing detected") print(f"Heart rate: {result['heart_rate']:.3f} Hz" if result['heart_rate'] else "No heart rate detected") print(f"Presence: {result['presence']}") print(f"Confidence: {result['confidence']:.2f}")
|