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 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296
| import numpy as np from scipy import signal from typing import Tuple, List
class ChildPresenceDetector: """ 60GHz雷达儿童存在检测 基于TI IWR6843AOP参考设计 """ def __init__(self, config: dict): self.config = config self.num_tx = 4 self.num_rx = 4 self.num_chirps = 128 self.num_samples = 256 self.range_fft_size = 512 self.doppler_fft_size = 128 self.breathing_rate_range = (0.2, 0.8) self.heartbeat_rate_range = (1.0, 2.0) self.pedestal_threshold = config.get('pedestal_threshold', -50) self.breathing_snr_threshold = config.get('breathing_snr', 6) def process_frame(self, adc_data: np.ndarray) -> dict: """ 处理单帧雷达数据 Args: adc_data: (num_chirps, num_rx, num_samples) ADC原始数据 Returns: detection_result: 包含检测结果、生命体征、置信度 """ range_doppler = self._range_doppler_fft(adc_data) detections = self._cfar_detection(range_doppler) clusters = self._cluster_detections(detections) vital_signs = self._extract_vital_signs(adc_data, clusters) is_child = self._classify_child(clusters, vital_signs) return { 'child_detected': is_child, 'num_targets': len(clusters), 'vital_signs': vital_signs, 'confidence': self._calculate_confidence(vital_signs) } def _range_doppler_fft(self, adc_data: np.ndarray) -> np.ndarray: """ 距离-多普勒FFT Args: adc_data: (num_chirps, num_rx, num_samples) Returns: range_doppler: (num_doppler, num_range) 幅度谱 """ range_fft = np.fft.fft(adc_data, n=self.range_fft_size, axis=2) doppler_fft = np.fft.fftshift( np.fft.fft(range_fft, n=self.doppler_fft_size, axis=0), axes=0 ) range_doppler = 20 * np.log10(np.abs(doppler_fft).mean(axis=1)) return range_doppler def _cfar_detection( self, range_doppler: np.ndarray, num_guard: int = 2, num_train: int = 4, p_fa: float = 1e-4 ) -> List[dict]: """ CFAR 目标检测 Args: range_doppler: (num_doppler, num_range) Returns: detections: 检测目标列表 """ detections = [] alpha = -2 * self.num_samples * np.log(p_fa) for d_idx in range(num_train, range_doppler.shape[0] - num_train): for r_idx in range(num_train, range_doppler.shape[1] - num_train): cut = range_doppler[d_idx, r_idx] train_cells = [] for di in range(-num_train - num_guard, num_train + num_guard + 1): for ri in range(-num_train - num_guard, num_train + num_guard + 1): if abs(di) > num_guard or abs(ri) > num_guard: train_cells.append(range_doppler[d_idx + di, r_idx + ri]) noise_level = np.mean(train_cells) threshold = noise_level + np.sqrt(alpha * noise_level) if cut > threshold and cut > self.pedestal_threshold: detections.append({ 'doppler_idx': d_idx, 'range_idx': r_idx, 'snr': cut - noise_level, 'amplitude': cut }) return detections def _cluster_detections(self, detections: List[dict]) -> List[dict]: """ 点云聚类(DBSCAN简化版) Args: detections: 检测目标列表 Returns: clusters: 聚类结果 """ if len(detections) == 0: return [] clusters = [] used = set() for i, det1 in enumerate(detections): if i in used: continue cluster = [det1] used.add(i) for j, det2 in enumerate(detections): if j in used: continue dr = abs(det1['range_idx'] - det2['range_idx']) dd = abs(det1['doppler_idx'] - det2['doppler_idx']) if dr < 5 and dd < 5: cluster.append(det2) used.add(j) clusters.append({ 'range_idx': int(np.mean([c['range_idx'] for c in cluster])), 'doppler_idx': int(np.mean([c['doppler_idx'] for c in cluster])), 'num_points': len(cluster), 'snr': np.mean([c['snr'] for c in cluster]) }) return clusters def _extract_vital_signs( self, adc_data: np.ndarray, clusters: List[dict] ) -> dict: """ 生命体征提取(呼吸、心跳) Args: adc_data: 原始ADC数据 clusters: 检测到的目标 Returns: vital_signs: 呼吸率、心率、信噪比 """ if len(clusters) == 0: return {'breathing_rate': 0, 'heartbeat_rate': 0, 'snr': -np.inf} target = max(clusters, key=lambda x: x['snr']) range_idx = target['range_idx'] phase_series = np.angle( np.fft.fft(adc_data[:, 0, :], n=self.range_fft_size, axis=1)[:, range_idx] ) phase_unwrap = np.unwrap(phase_series) n_fft = len(phase_unwrap) freq = np.fft.fftfreq(n_fft, d=1.0 / self.config.get('frame_rate', 10)) spectrum = np.abs(np.fft.fft(phase_unwrap)) breathing_mask = (np.abs(freq) >= self.breathing_rate_range[0]) & \ (np.abs(freq) <= self.breathing_rate_range[1]) breathing_spectrum = spectrum[breathing_mask] breathing_freq = freq[breathing_mask] if len(breathing_spectrum) > 0: breathing_rate = np.abs(breathing_freq[np.argmax(breathing_spectrum)]) breathing_snr = np.max(breathing_spectrum) / np.mean(breathing_spectrum) else: breathing_rate = 0 breathing_snr = 0 return { 'breathing_rate': breathing_rate, 'heartbeat_rate': 0, 'snr': breathing_snr } def _classify_child( self, clusters: List[dict], vital_signs: dict ) -> bool: """ 儿童判断逻辑 判断依据: 1. 存在呼吸信号(SNR > 阈值) 2. 呼吸频率在儿童范围内 3. 目标尺寸较小(点云数量) """ has_breathing = vital_signs['snr'] > self.breathing_snr_threshold breathing_valid = self.breathing_rate_range[0] <= vital_signs['breathing_rate'] <= self.breathing_rate_range[1] is_small_target = all(c['num_points'] < 10 for c in clusters) return has_breathing and breathing_valid and is_small_target def _calculate_confidence(self, vital_signs: dict) -> float: """计算检测置信度""" snr = vital_signs['snr'] if snr < self.breathing_snr_threshold: return 0.0 elif snr < 10: return 0.5 + 0.05 * (snr - self.breathing_snr_threshold) else: return 1.0
if __name__ == "__main__": config = { 'frame_rate': 10, 'pedestal_threshold': -50, 'breathing_snr': 6 } detector = ChildPresenceDetector(config) np.random.seed(42) adc_data = np.random.randn(128, 4, 256) * 100 for chirp_idx in range(128): phase_modulation = 0.5 * np.sin(2 * np.pi * 0.4 * chirp_idx / 10) adc_data[chirp_idx, 0, 50:70] += 1000 * np.exp(1j * phase_modulation) result = detector.process_frame(adc_data) print(f"儿童检测: {result['child_detected']}") print(f"呼吸频率: {result['vital_signs']['breathing_rate']:.2f} Hz") print(f"置信度: {result['confidence']:.2f}")
|