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

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

1. 主程序框架

c 复制代码

/**
 * 基于时延的麦克风声源定位系统
 * 方法：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. 信号生成与时延计算

c 复制代码

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时延估计算法

c 复制代码

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;
    }
    
    // 执行FFT
    fftw_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. 声源定位算法（球面相交法）

c 复制代码

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 b
    double ATA[3][3] = {{0}};
    double ATb[3] = {0};
    
    // 计算A^T A
    for (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 b
    for (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. 增强版：实时处理与可视化

c 复制代码

// 实时处理版本
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 复制代码

# Makefile for sound localization system
CC = gcc
CFLAGS = -Wall -O2 -std=c99
LIBS = -lm -lfftw3
TARGET = sound_localization

SRCS = 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. 使用说明

编译和运行：

bash 复制代码

# 安装FFTW库（Ubuntu/Debian）
sudo apt-get install libfftw3-dev

# 编译
make

# 运行
make run

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

算法特点：

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

扩展建议：

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