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
| """
雷达心率估计系统 - 呼吸谐波干扰消除
基于 Sensors 2026 论文的方法架构
适配到 IMS 座舱场景
"""
import numpy as np
from scipy.signal import butter, filtfilt, find_peaks
from scipy.fft import fft, fftfreq
class RadarHeartRateEstimator:
"""
mmWave 雷达心率估计器
处理流程：
1. 雷达相位提取 → 胸壁位移信号
2. 带通滤波 → 分离呼吸和心跳
3. 呼吸谐波检测 → 识别干扰频率
4. 自适应消除 → 去除谐波干扰
5. 心率估计 → 最终心率输出
"""
def __init__(self, sample_rate: float = 50.0):
self.fs = sample_rate
self.resp_band = (0.1, 0.5)
self.heart_band = (0.8, 3.0)
def process(self, radar_phase: np.ndarray) -> dict:
"""
处理雷达相位信号
Args:
radar_phase: (N,) 雷达相位信号（已解卷绕）
Returns:
dict with 'heart_rate', 'resp_rate', 'quality'
"""
signal = radar_phase - np.mean(radar_phase)
resp_signal = self._bandpass(signal, self.resp_band)
heart_signal_raw = self._bandpass(signal, self.heart_band)
resp_freq = self._dominant_freq(resp_signal)
harmonics = self._generate_harmonics(resp_freq, len(signal))
heart_signal_clean = self._adaptive_cancel(
heart_signal_raw, harmonics
)
heart_rate = self._dominant_freq(heart_signal_clean) * 60
snr = self._compute_snr(heart_signal_clean)
quality = 'good' if snr > 3 else 'poor'
return {
'heart_rate': heart_rate,
'resp_rate': resp_freq * 60,
'snr': snr,
'quality': quality,
'heart_signal': heart_signal_clean,
}
def _bandpass(self, signal: np.ndarray, band: tuple) -> np.ndarray:
"""带通滤波"""
b, a = butter(4, [band[0]/(self.fs/2), band[1]/(self.fs/2)],
btype='band')
return filtfilt(b, a, signal)
def _dominant_freq(self, signal: np.ndarray) -> float:
"""找到主频"""
N = len(signal)
spectrum = np.abs(fft(signal))
freqs = fftfreq(N, 1/self.fs)
pos_mask = freqs > 0
return freqs[pos_mask][np.argmax(spectrum[pos_mask])]
def _generate_harmonics(self, base_freq: float, length: int) -> dict:
"""生成呼吸谐波"""
t = np.arange(length) / self.fs
harmonics = {}
for n in [2, 3, 4, 5]:
freq = base_freq * n
if 0.8 <= freq <= 3.0:
harmonics[f'harmonic_{n}'] = np.sin(2 * np.pi * freq * t)
return harmonics
def _adaptive_cancel(self, signal: np.ndarray,
harmonics: dict) -> np.ndarray:
"""自适应谐波消除"""
if not harmonics:
return signal
harmonic_matrix = np.array(list(harmonics.values())).T
weights, *_ = np.linalg.lstsq(harmonic_matrix, signal, rcond=None)
interference = harmonic_matrix @ weights
return signal - interference
def _compute_snr(self, signal: np.ndarray) -> float:
"""计算信噪比"""
spectrum = np.abs(fft(signal))
freqs = fftfreq(len(signal), 1/self.fs)
pos_mask = freqs > 0
spectrum = spectrum[pos_mask]
freqs = freqs[pos_mask]
peak_idx = np.argmax(spectrum)
peak_power = spectrum[peak_idx]
noise_mask = np.ones(len(spectrum), dtype=bool)
noise_mask[max(0,peak_idx-3):peak_idx+4] = False
noise_power = np.mean(spectrum[noise_mask])
if noise_power == 0:
return 100.0
return 10 * np.log10(peak_power / noise_power)
if __name__ == "__main__":
import time
estimator = RadarHeartRateEstimator(sample_rate=50.0)
t = np.arange(0, 30, 1/50)
true_hr = 75
true_rr = 16
heartbeat = 0.1 * np.sin(2 * np.pi * true_hr/60 * t)
breathing = 4.0 * np.sin(2 * np.pi * true_rr/60 * t)
resp_h3 = 0.5 * np.sin(2 * np.pi * 3*true_rr/60 * t)
resp_h4 = 0.3 * np.sin(2 * np.pi * 4*true_rr/60 * t)
noise = 0.05 * np.random.randn(len(t))
radar_signal = heartbeat + breathing + resp_h3 + resp_h4 + noise
start = time.time()
result = estimator.process(radar_signal)
process_time = (time.time() - start) * 1000
print("=" * 60)
print("雷达心率估计测试")
print("=" * 60)
print(f"真实心率: {true_hr} bpm")
print(f"真实呼吸: {true_rr} /min")
print(f"呼吸三次谐波: {3*true_rr/60:.3f} Hz = {3*true_rr:.0f} /min")
print(f"呼吸四次谐波: {4*true_rr/60:.3f} Hz = {4*true_rr:.0f} /min")
print(f"\n估计心率: {result['heart_rate']:.1f} bpm")
print(f"估计呼吸: {result['resp_rate']:.1f} /min")
print(f"SNR: {result['snr']:.2f} dB")
print(f"质量: {result['quality']}")
print(f"处理时间: {process_time:.1f} ms")
error = abs(result['heart_rate'] - true_hr)
print(f"\n心率误差: {error:.1f} bpm")
if error < 5:
print("✅ 误差 <5 bpm，满足监测要求")
else:
print("⚠️ 误差 ≥5 bpm，需优化")
|
