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
| import numpy as np from scipy.signal import butter, filtfilt, fft from typing import Tuple
class RadarHeartRateEstimator: """ 毫米波雷达心率估计器 核心算法: 呼吸谐波干扰抑制 参考: MDPI Sensors 2026, 26(17):5587 """ def __init__(self, sample_rate: float = 100.0): self.sample_rate = sample_rate self.breath_freq_range = (0.15, 0.5) self.heart_freq_range = (0.8, 2.0) def estimate_heart_rate(self, radar_signal: np.ndarray) -> dict: """ 从雷达回波信号估计心率 Args: radar_signal: 雷达相位信号, shape=(N,) Returns: 心率估计结果 """ signal = self._detrend(radar_signal) breath_freq = self._estimate_breath_freq(signal) harmonics = self._compute_harmonics(breath_freq, n_harmonics=5) cleaned = signal for h_freq in harmonics: if self.heart_freq_range[0] <= h_freq <= self.heart_freq_range[1]: cleaned = self._notch_filter(cleaned, h_freq, Q=30) heart_band = self._bandpass_filter( cleaned, self.heart_freq_range[0], self.heart_freq_range[1] ) freqs, spectrum = self._compute_spectrum(heart_band) heart_freq = freqs[np.argmax(spectrum)] heart_rate_bpm = heart_freq * 60 snr = self._compute_snr(spectrum, heart_freq) confidence = self._estimate_confidence(snr, heart_freq, breath_freq) return { 'heart_rate_bpm': heart_rate_bpm, 'heart_freq_hz': heart_freq, 'breath_freq_hz': breath_freq, 'breath_rate_bpm': breath_freq * 60, 'harmonics_removed': len([h for h in harmonics if self.heart_freq_range[0] <= h <= self.heart_freq_range[1]]), 'snr_db': snr, 'confidence': confidence, } def _detrend(self, signal: np.ndarray) -> np.ndarray: """去趋势""" n = len(signal) t = np.arange(n) coeffs = np.polyfit(t, signal, 1) return signal - np.polyval(coeffs, t) def _estimate_breath_freq(self, signal: np.ndarray) -> float: """估计呼吸基频""" filtered = self._bandpass_filter(signal, 0.1, 0.6) freqs, spectrum = self._compute_spectrum(filtered) breath_mask = (freqs >= self.breath_freq_range[0]) & \ (freqs <= self.breath_freq_range[1]) breath_freqs = freqs[breath_mask] breath_spec = spectrum[breath_mask] return breath_freqs[np.argmax(breath_spec)] def _compute_harmonics(self, base_freq: float, n_harmonics: int) -> list: """计算谐波频率""" return [base_freq * (i + 1) for i in range(n_harmonics)] def _notch_filter(self, signal: np.ndarray, freq: float, Q: int = 30) -> np.ndarray: """陷波滤波器""" nyq = self.sample_rate / 2 w0 = freq / nyq bandwidth = w0 / Q low = max(w0 - bandwidth / 2, 0.001) high = min(w0 + bandwidth / 2, 0.999) b, a = butter(4, [low, high], btype='bandstop') return filtfilt(b, a, signal) def _bandpass_filter(self, signal: np.ndarray, low: float, high: float) -> np.ndarray: """带通滤波""" nyq = self.sample_rate / 2 b, a = butter(4, [low / nyq, high / nyq], btype='band') return filtfilt(b, a, signal) def _compute_spectrum(self, signal: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """计算功率谱""" n = len(signal) freqs = np.fft.rfftfreq(n, 1.0 / self.sample_rate) spectrum = np.abs(np.fft.rfft(signal)) ** 2 return freqs, spectrum def _compute_snr(self, spectrum: np.ndarray, peak_freq: float) -> float: """计算峰值信噪比""" peak_idx = np.argmax(spectrum) peak_power = spectrum[peak_idx] exclude_range = 5 noise_mask = np.ones(len(spectrum), dtype=bool) noise_mask[max(0, peak_idx - exclude_range):peak_idx + exclude_range + 1] = False noise_power = np.mean(spectrum[noise_mask]) if np.any(noise_mask) else 1e-10 if noise_power == 0: return 60.0 return 10 * np.log10(peak_power / noise_power) def _estimate_confidence(self, snr: float, heart_freq: float, breath_freq: float) -> float: """置信度评估""" snr_score = min(snr / 15.0, 1.0) freq_score = 1.0 if 0.8 <= heart_freq <= 2.0 else 0.5 harmonic_proximity = min( abs(heart_freq - breath_freq * i) for i in range(2, 6) ) if breath_freq > 0 else 1.0 harmonic_score = min(harmonic_proximity / 0.1, 1.0) return 0.4 * snr_score + 0.3 * freq_score + 0.3 * harmonic_score
if __name__ == "__main__": np.random.seed(42) estimator = RadarHeartRateEstimator(sample_rate=100.0) t = np.arange(10000) / 100.0 breath = 5.0 * np.sin(2 * np.pi * 0.3 * t) heartbeat = 0.5 * np.sin(2 * np.pi * 1.2 * t) breath_h2 = 2.0 * np.sin(2 * np.pi * 0.6 * t) breath_h3 = 1.0 * np.sin(2 * np.pi * 0.9 * t) breath_h4 = 0.5 * np.sin(2 * np.pi * 1.2 * t) signal = breath + heartbeat + breath_h2 + breath_h3 + breath_h4 + \ 0.1 * np.random.randn(len(t)) result = estimator.estimate_heart_rate(signal) print("=== 雷达心率估计 ===") print(f"真实心率: 72 bpm") print(f"估计心率: {result['heart_rate_bpm']:.1f} bpm") print(f"呼吸频率: {result['breath_rate_bpm']:.1f} bpm") print(f"去除谐波数: {result['harmonics_removed']}") print(f"SNR: {result['snr_db']:.1f} dB") print(f"置信度: {result['confidence']:.2%}")
|