基于时延的麦克风声源定位 - C实现

包含GCC-PHAT时延估计、球面相交定位和可视化功能。

1. 主程序框架

/*** 基于时延的麦克风声源定位系统* 方法：GCC-PHAT + 球面相交法* 编译：gcc -o sound_localization sound_localization.c -lm -lfftw3*/#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <fftw3.h>
#include <time.h>#define PI 3.14159265358979323846
#define SOUND_SPEED 343.0  // 声速 (m/s)
#define FS 16000            // 采样率 (Hz)
#define SIGNAL_LENGTH 0.5   // 信号时长 (s)
#define F0 1000             // 信号频率 (Hz)
#define SNR 20               // 信噪比 (dB)
#define N_MICS 4             // 麦克风数量// 麦克风位置 (x, y, z) 单位：米
double mic_positions[N_MICS][3] = {{0.0, 0.0, 0.0},   // 麦克风1{0.5, 0.0, 0.0},   // 麦克风2{0.0, 0.5, 0.0},   // 麦克风3{0.0, 0.0, 0.5}    // 麦克风4
};// 真实声源位置
double true_source_pos[3] = {2.0, 1.5, 1.2};// 函数声明
void generate_signals(double **signals, int n_samples);
void gcc_phat(double *sig1, double *sig2, int n_samples, double *delay);
void locate_source(double delays[N_MICS], double source_pos[3]);
double estimate_delay(double *sig1, double *sig2, int n_samples);
void add_noise(double *signal, int n_samples, double snr_db);int main() {printf("基于时延的麦克风声源定位系统\n");printf("==========================================\n");int n_samples = (int)(SIGNAL_LENGTH * FS);// 分配内存double **mic_signals = malloc(N_MICS * sizeof(double *));for (int i = 0; i < N_MICS; i++) {mic_signals[i] = malloc(n_samples * sizeof(double));}// 生成仿真信号printf("生成仿真信号...\n");generate_signals(mic_signals, n_samples);// 时延估计printf("\n时延估计结果:\n");printf("麦克风对\t估计时延(ms)\n");double delays[N_MICS];delays[0] = 0.0;  // 参考麦克风for (int i = 1; i < N_MICS; i++) {delays[i] = estimate_delay(mic_signals[0], mic_signals[i], n_samples);printf("1-%d\t\t%.3f\n", i+1, delays[i]*1000);}// 声源定位printf("\n进行声源定位...\n");double estimated_source_pos[3];locate_source(delays, estimated_source_pos);// 显示结果printf("\n定位结果对比:\n");printf("真实位置: [%.2f, %.2f, %.2f] m\n", true_source_pos[0], true_source_pos[1], true_source_pos[2]);printf("估计位置: [%.2f, %.2f, %.2f] m\n", estimated_source_pos[0], estimated_source_pos[1], estimated_source_pos[2]);double error = sqrt(pow(true_source_pos[0]-estimated_source_pos[0], 2) +pow(true_source_pos[1]-estimated_source_pos[1], 2) +pow(true_source_pos[2]-estimated_source_pos[2], 2));printf("定位误差: %.2f cm\n", error*100);// 释放内存for (int i = 0; i < N_MICS; i++) {free(mic_signals[i]);}free(mic_signals);return 0;
}

2. 信号生成与时延计算

void generate_signals(double **signals, int n_samples) {// 生成原始声源信号（正弦波）double *source_signal = malloc(n_samples * sizeof(double));for (int i = 0; i < n_samples; i++) {double t = (double)i / FS;source_signal[i] = sin(2 * PI * F0 * t);}// 为每个麦克风生成接收信号for (int mic = 0; mic < N_MICS; mic++) {// 计算声源到麦克风的距离double distance = sqrt(pow(true_source_pos[0] - mic_positions[mic][0], 2) +pow(true_source_pos[1] - mic_positions[mic][1], 2) +pow(true_source_pos[2] - mic_positions[mic][2], 2));// 计算时延double delay = distance / SOUND_SPEED;int delay_samples = (int)(delay * FS);// 添加时延for (int i = 0; i < n_samples; i++) {int source_idx = i - delay_samples;if (source_idx >= 0 && source_idx < n_samples) {signals[mic][i] = source_signal[source_idx];} else {signals[mic][i] = 0.0;}}// 添加噪声add_noise(signals[mic], n_samples, SNR);}free(source_signal);
}void add_noise(double *signal, int n_samples, double snr_db) {// 计算信号功率double signal_power = 0.0;for (int i = 0; i < n_samples; i++) {signal_power += signal[i] * signal[i];}signal_power /= n_samples;// 计算噪声功率double noise_power = signal_power / pow(10, snr_db/10.0);double noise_std = sqrt(noise_power);// 添加高斯白噪声srand(time(NULL));for (int i = 0; i < n_samples; i++) {// Box-Muller变换生成高斯噪声double u1 = (double)rand() / RAND_MAX;double u2 = (double)rand() / RAND_MAX;double z = sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2);signal[i] += noise_std * z;}
}

3. GCC-PHAT时延估计算法

void gcc_phat(double *sig1, double *sig2, int n_samples, double *delay) {int n_fft = 1;while (n_fft < 2*n_samples-1) n_fft <<= 1;// 分配FFT输入输出数组fftw_complex *in1 = fftw_alloc_complex(n_fft);fftw_complex *in2 = fftw_alloc_complex(n_fft);fftw_complex *out1 = fftw_alloc_complex(n_fft);fftw_complex *out2 = fftw_alloc_complex(n_fft);fftw_complex *cross_spec = fftw_alloc_complex(n_fft);// 创建FFT计划fftw_plan p1 = fftw_plan_dft_1d(n_fft, in1, out1, FFTW_FORWARD, FFTW_ESTIMATE);fftw_plan p2 = fftw_plan_dft_1d(n_fft, in2, out2, FFTW_FORWARD, FFTW_ESTIMATE);// 填充输入数据for (int i = 0; i < n_samples; i++) {in1[i] = sig1[i] + 0*I;in2[i] = sig2[i] + 0*I;}for (int i = n_samples; i < n_fft; i++) {in1[i] = 0.0 + 0*I;in2[i] = 0.0 + 0*I;}// 执行FFTfftw_execute(p1);fftw_execute(p2);// 计算互功率谱并进行PHAT加权for (int i = 0; i < n_fft; i++) {double complex cross = out1[i] * conj(out2[i]);double magnitude = cabs(cross);if (magnitude > 1e-10) {cross_spec[i] = cross / magnitude;  // PHAT加权} else {cross_spec[i] = 0.0 + 0*I;}}// 逆FFT得到互相关fftw_plan p_inv = fftw_plan_dft_1d(n_fft, cross_spec, in1, FFTW_BACKWARD, FFTW_ESTIMATE);fftw_execute(p_inv);// 找到峰值位置double max_val = 0.0;int max_idx = 0;for (int i = 0; i < n_fft; i++) {double val = cabs(in1[i]);if (val > max_val) {max_val = val;max_idx = i;}}// 计算时延if (max_idx <= n_fft/2) {*delay = (double)max_idx / FS;} else {*delay = (double)(max_idx - n_fft) / FS;}// 清理fftw_destroy_plan(p1);fftw_destroy_plan(p2);fftw_destroy_plan(p_inv);fftw_free(in1);fftw_free(in2);fftw_free(out1);fftw_free(out2);fftw_free(cross_spec);
}double estimate_delay(double *sig1, double *sig2, int n_samples) {double delay;gcc_phat(sig1, sig2, n_samples, &delay);return delay;
}

4. 声源定位算法（球面相交法）

void locate_source(double delays[N_MICS], double source_pos[3]) {// 使用最小二乘法求解// 方程形式: 2*(mic_i - mic_ref)·x = ||mic_i||² - ||mic_ref||² + 2*c*Δt*(c*Δt - 2*||mic_ref||)int n_eq = N_MICS - 1;double A[n_eq][3];double b[n_eq];double mic_ref[3] = {mic_positions[0][0], mic_positions[0][1], mic_positions[0][2]};double ref_norm_sq = mic_ref[0]*mic_ref[0] + mic_ref[1]*mic_ref[1] + mic_ref[2]*mic_ref[2];for (int i = 1; i < N_MICS; i++) {double mic_i[3] = {mic_positions[i][0], mic_positions[i][1], mic_positions[i][2]};double mic_norm_sq = mic_i[0]*mic_i[0] + mic_i[1]*mic_i[1] + mic_i[2]*mic_i[2];// A矩阵行A[i-1][0] = 2.0 * (mic_i[0] - mic_ref[0]);A[i-1][1] = 2.0 * (mic_i[1] - mic_ref[1]);A[i-1][2] = 2.0 * (mic_i[2] - mic_ref[2]);// b向量元素double c_dt = SOUND_SPEED * (delays[i] - delays[0]);b[i-1] = mic_norm_sq - ref_norm_sq + 2.0 * c_dt * (c_dt - 2.0 * sqrt(ref_norm_sq));}// 使用正规方程求解最小二乘问题: A^T A x = A^T bdouble ATA[3][3] = {{0}};double ATb[3] = {0};// 计算A^T Afor (int i = 0; i < 3; i++) {for (int j = 0; j < 3; j++) {for (int k = 0; k < n_eq; k++) {ATA[i][j] += A[k][i] * A[k][j];}}}// 计算A^T bfor (int i = 0; i < 3; i++) {for (int k = 0; k < n_eq; k++) {ATb[i] += A[k][i] * b[k];}}// 解线性方程组 (使用高斯消元法)// 前向消元for (int i = 0; i < 3; i++) {// 寻找主元int pivot = i;double max_val = fabs(ATA[i][i]);for (int j = i+1; j < 3; j++) {if (fabs(ATA[j][i]) > max_val) {max_val = fabs(ATA[j][i]);pivot = j;}}// 交换行if (pivot != i) {for (int j = 0; j < 3; j++) {double temp = ATA[i][j];ATA[i][j] = ATA[pivot][j];ATA[pivot][j] = temp;}double temp = ATb[i];ATb[i] = ATb[pivot];ATb[pivot] = temp;}// 消元for (int j = i+1; j < 3; j++) {double factor = ATA[j][i] / ATA[i][i];for (int k = i; k < 3; k++) {ATA[j][k] -= factor * ATA[i][k];}ATb[j] -= factor * ATb[i];}}// 回代source_pos[2] = ATb[2] / ATA[2][2];source_pos[1] = (ATb[1] - ATA[1][2] * source_pos[2]) / ATA[1][1];source_pos[0] = (ATb[0] - ATA[0][1] * source_pos[1] - ATA[0][2] * source_pos[2]) / ATA[0][0];
}

5. 增强版：实时处理与可视化

// 实时处理版本
typedef struct {double *buffer[N_MICS];int buffer_size;int write_index;int read_index;
} CircularBuffer;void init_circular_buffer(CircularBuffer *cb, int size) {cb->buffer_size = size;cb->write_index = 0;cb->read_index = 0;for (int i = 0; i < N_MICS; i++) {cb->buffer[i] = malloc(size * sizeof(double));}
}void process_frame(CircularBuffer *cb, double source_pos[3]) {// 从缓冲区读取一帧数据double frame[N_MICS][512]; // 假设帧长512// 估计时延double delays[N_MICS];delays[0] = 0.0;for (int i = 1; i < N_MICS; i++) {delays[i] = estimate_delay(frame[0], frame[i], 512);}// 定位locate_source(delays, source_pos);
}// 可视化函数（需要配合图形库）
void visualize_results(double true_pos[3], double est_pos[3]) {printf("\n=== 定位结果可视化 ===\n");printf("麦克风阵列配置:\n");for (int i = 0; i < N_MICS; i++) {printf("Mic %d: (%.2f, %.2f, %.2f)\n", i+1, mic_positions[i][0], mic_positions[i][1], mic_positions[i][2]);}printf("\n定位结果:\n");printf("真实位置: (%.2f, %.2f, %.2f)\n", true_pos[0], true_pos[1], true_pos[2]);printf("估计位置: (%.2f, %.2f, %.2f)\n", est_pos[0], est_pos[1], est_pos[2]);// 计算误差double error = sqrt(pow(true_pos[0]-est_pos[0], 2) +pow(true_pos[1]-est_pos[1], 2) +pow(true_pos[2]-est_pos[2], 2));printf("定位误差: %.2f cm\n", error*100);
}// 鲁棒性测试
void robustness_test() {printf("\n=== 鲁棒性测试 ===\n");double test_positions[][3] = {{1.0, 1.0, 1.0},{3.0, 2.0, 1.5},{0.5, 2.5, 0.8},{2.5, 0.5, 2.0}};int n_tests = sizeof(test_positions) / sizeof(test_positions[0]);for (int t = 0; t < n_tests; t++) {printf("\n测试 %d: 真实位置 (%.1f, %.1f, %.1f)\n", t+1, test_positions[t][0], test_positions[t][1], test_positions[t][2]);// 这里可以添加完整的测试流程// 1. 生成信号// 2. 估计时延// 3. 定位// 4. 计算误差}
}

6. Makefile编译脚本

# Makefile for sound localization system
CC = gcc
CFLAGS = -Wall -O2 -std=c99
LIBS = -lm -lfftw3
TARGET = sound_localizationSRCS = sound_localization.c
OBJS = $(SRCS:.c=.o)all: $(TARGET)$(TARGET): $(OBJS)$(CC) $(OBJS) -o $(TARGET) $(LIBS)%.o: %.c$(CC) $(CFLAGS) -c $< -o $@clean:rm -f $(OBJS) $(TARGET)run: $(TARGET)./$(TARGET).PHONY: all clean run

7. 使用说明

编译和运行：

# 安装FFTW库（Ubuntu/Debian）
sudo apt-get install libfftw3-dev# 编译
make# 运行
make run

参考代码 基于时延的麦克风声源定位 www.youwenfan.com/contentcnt/135826.html

算法特点：

1. 高精度时延估计：使用GCC-PHAT算法，对混响和噪声鲁棒
2. 三维定位：支持三维空间定位
3. 最小二乘求解：使用正规方程求解超定方程组
4. 模块化设计：便于集成到实时系统中

扩展建议：

1. 添加SRP-PHAT算法提高鲁棒性
2. 实现TDOA多基站定位
3. 添加卡尔曼滤波进行轨迹跟踪
4. 集成波束形成算法
5. 支持实时音频流处理