四麦克风声源定位实战：基于 GCC-PHAT + 最小二乘法实现 DOA

文章目录

前言
一、整体设计
- 1.mic阵列设计
- 2.整体算法流程设计
二、核心数学模型回顾
- 1.物理方程
- 2.矩阵形式
三、代码核心解析
- 1.主流程
- [2.GCC-PHAT 实现](#2.GCC-PHAT 实现)
- - [① 加窗](#① 加窗)
  - [② FFT](#② FFT)
  - [③ 计算互功率谱](#③ 计算互功率谱)
  - [④ PHAT 加权](#④ PHAT 加权)
  - [⑤ 频带限制](#⑤ 频带限制)
  - [⑥ IFFT](#⑥ IFFT)
  - [⑦ 找峰值](#⑦ 找峰值)
- 3.方向求解
- - [1️⃣ 构造矩阵](#1️⃣ 构造矩阵)
  - [2️⃣ 手写 2×2 逆矩阵](#2️⃣ 手写 2×2 逆矩阵)
  - [3️⃣ 归一化](#3️⃣ 归一化)
  - [4️⃣ 角度](#4️⃣ 角度)
四、策略解释
- 1.平滑滤波
- 2.最大时延
五、代码实现
总结

前言

在之前的文章👉 声源定位------TDOA多麦克声源定位全解析我们详细推导了二维平面下 TDOA 声源定位的数学原理。

本文将基于：

🎯 GCC-PHAT 计算时延

🎯 三组 TDOA 构建超定方程

🎯 最小二乘求解方向向量

🎯 向量归一化得到 DOA

完整实现一个 4 Mic 平面声源定位系统。

|版本声明：山河君，未经博主允许，禁止转载

一、整体设计

1.mic阵列设计

我们使用一个 2cm × 2cm 的正方形阵列：

bash 复制代码

mic2 (0.02,0.02)      mic3 (0.02,0.00)

mic0 (0,0)            mic1 (0.00,0.02)

对应标注代码为：

cpp 复制代码

const Mic init_mics[4] = {
    { 0.00f,  0.00f},
    { 0.02f,  0.00f},
    { 0.00f,  0.02f},
    { 0.02f,  0.02f}
};

其中固定 mic0 为参考麦克风。

2.整体算法流程设计

bash 复制代码

PCM
 ↓
分离4通道
 ↓
3组 GCC-PHAT
 ↓
3个 τ
 ↓
最小二乘求 u
 ↓
归一化
 ↓
atan2
 ↓
cos/sin 平滑
 ↓
DOA角度

二、核心数学模型回顾

1.物理方程

声源方向单位向量：
u = [ u x , u y ] u = [ux, uy] u=[ux,uy]

每个麦克风满足：
r i ⋅ u = c τ i r_i · u = c \tau_i ri⋅u=cτi

其中：

r i r_i ri：麦克风位置向量
τ i \tau_i τi：相对于参考麦克风的时延
c c c：声速

2.矩阵形式

定义矩阵：
A = [ x 1 , y 1 x 2 , y 2 x 3 , y 3 ] b = [ τ 1 c τ 2 c τ 3 c ] A=\left[ \begin{matrix} x_1,y_1 \\ x_2,y_2 \\ x_3,y_3 \\ \end{matrix} \right] \quad b=\left[ \begin{matrix} \tau_1 c \\ \tau_2 c \\ \tau_3 c \\ \end{matrix} \right] A= x1,y1x2,y2x3,y3 b= τ1cτ2cτ3c

因为是 3 条方程、2 个未知量，是超定方程，所以使用最小二乘：
u = ( A T A ) − 1 A T b u=(A^TA)^{-1}A^Tb u=(ATA)−1ATb

三、代码核心解析

1.主流程

cpp 复制代码

float DoaEstimator4Mic::process(void* pcm)

流程如下：

分离 4 通道
计算 3 组时延
解方向向量
平滑滤波
输出角度

2.GCC-PHAT 实现

对于GCC-PHAT的原理在：语音数字信号处理------GCC-PHAT计算回声时延。这里对于原理不做赘述，对于实现细节进行备注，核心函数在：

cpp 复制代码

float DoaEstimator4Mic::estimateDelay(...)

① 加窗

使用 Hann 窗减少频谱泄露。

cpp 复制代码

m_window[n] = 0.5f * (1 - cos(...))

② FFT

cpp 复制代码

fftwf_execute(m_planX);
fftwf_execute(m_planY);

③ 计算互功率谱

cpp 复制代码

cross_real = X_real * Y_real + X_imag * Y_imag;
cross_imag = X_imag * Y_real - X_real * Y_imag;

④ PHAT 加权

只保留相位信息

cpp 复制代码

m_pR[i][0] = cross_real / mag;
m_pR[i][1] = cross_imag / mag;

⑤ 频带限制

只保留语音频段，提升鲁棒性

cpp 复制代码

if (freq < 300 || freq > 4000)

⑥ IFFT

cpp 复制代码

fftwf_execute(m_planR);

⑦ 找峰值

这里需要关注负延迟

cpp 复制代码

if (max_idx > FRAME_SAMPLE / 2)
    max_idx -= FRAME_SAMPLE;

最终得到时延：

cpp 复制代码

τ = index / Fs

3.方向求解

核心函数：

cpp 复制代码

float DoaEstimator4Mic::solveAngle(const std::vector<float>& tau)

1️⃣ 构造矩阵

cpp 复制代码

A[i][0] = m_mic[i+1].x;
A[i][1] = m_mic[i+1].y;
b[i] = SOUND_SPEED * tau[i];

2️⃣ 手写 2×2 逆矩阵

这里为了方便理解，所以没有使用依赖库，而是手写方式进行计算

cpp 复制代码

float det = ATA[0][0]*ATA[1][1] - ATA[0][1]*ATA[1][0];

3️⃣ 归一化

得到单位方向向量

cpp 复制代码

norm = sqrt(ux² + uy²)

4️⃣ 角度

最终计算角度

cpp 复制代码

theta = atan2(uy, ux)

四、策略解释

1.平滑滤波

cpp 复制代码

m_fCos = (1 - ALPHA) * m_fCos + ALPHA * cos_new;
m_fSin = (1 - ALPHA) * m_fSin + ALPHA * sin_new;

这里值得注意的是没有直接滤波角度，原因是由于角度在 ± 180 ∘ \pm 180^{\circ} ±180∘存在跳变，例如： 179° → -179°，如果直接滤波会产生错误跳变。所以采用 cos ⁡ / sin ⁡ \cos /\sin cos/sin滤波可以避免角度不连续问题。

2.最大时延

cpp 复制代码

m_fTauMax = sqrt(0.02*0.02 * 2) / SOUND_SPEED;

这是由于最大距离是在于对角线
d = 0.02 2 + 0.02 2 d=\sqrt{0.02^2+0.02^2} d=0.022+0.022

最大时延：
τ m a x = d / c \tau_{max}=d/c τmax=d/c

这里还可以针对每一个mic距离进行分别计算。而超过该范围说明：

噪声
错误峰值
混响干扰

五、代码实现

1.工程实现效果

我这边使用QT进行图像化显示，整体工程和测试文件数据已经打包，有需要的同学可以下载地址，这里是演示效果：

1.GCC-PHAT

cpp 复制代码

float DoaEstimator4Mic::estimateDelay(float *x1,float * x2)
{
    for (int i = 0; i < FRAME_SAMPLE; ++i)
    {
        m_pDateX[i] = x1[i] * m_window[i];
        m_pDataY[i] = x2[i] * m_window[i];
    }

    fftwf_execute(m_planX);
    fftwf_execute(m_planY);

    for (int i = 0; i < N_FFT; ++i) {

        float freq = (float)i * SAMPLE_RATE / FRAME_SAMPLE;
        if (freq < 300.0f || freq > 4000.0f)
        {
            m_pR[i][0] = 0;
            m_pR[i][1] = 0;
            continue;
        }

        const double X_real = m_pX[i][0];
        const double X_imag = m_pX[i][1];
        const double Y_real = m_pY[i][0];
        const double Y_imag = m_pY[i][1];
        const double cross_real = X_real * Y_real + X_imag * Y_imag;
        const double cross_imag = X_imag * Y_real - X_real * Y_imag;
        float mag = hypotf(cross_real, cross_imag);
        if (mag < 1e-9f) {
            m_pR[i][0] = 0;
            m_pR[i][1] = 0;
        }
        else
        {
            m_pR[i][0] = cross_real / mag;
            m_pR[i][1] = cross_imag / mag;
        }
    }

    fftwf_execute(m_planR);

    int max_idx = 0;
    double max_val = fabs(m_pDataR[0]) / FRAME_SAMPLE;
    for (int i = 1; i < FRAME_SAMPLE; ++i) {
        double val = fabs(m_pDataR[i]) / FRAME_SAMPLE;
        if (val > max_val) {
            max_val = val;
            max_idx = i;
        }
    }
    if (max_idx > FRAME_SAMPLE / 2)
        max_idx -= FRAME_SAMPLE;

    return float(max_idx) / SAMPLE_RATE;
}

2.角度计算

cpp 复制代码

float DoaEstimator4Mic::solveAngle(const std::vector<float>& tau)
{
    float A[3][2];
    float b[3];

    for(int i=0;i<3;i++)
    {
        A[i][0] = m_mic[i+1].x;
        A[i][1] = m_mic[i+1].y;
        b[i] = SOUND_SPEED * tau[i];
    }

    // 计算 A^T A
    float ATA[2][2] = {0};
    for(int i=0;i<3;i++)
    {
        ATA[0][0] += A[i][0]*A[i][0];
        ATA[0][1] += A[i][0]*A[i][1];
        ATA[1][0] += A[i][1]*A[i][0];
        ATA[1][1] += A[i][1]*A[i][1];
    }

    // 计算 A^T b
    float ATb[2] = {0};
    for(int i=0;i<3;i++)
    {
        ATb[0] += A[i][0]*b[i];
        ATb[1] += A[i][1]*b[i];
    }

    // 2x2 逆
    float det = ATA[0][0]*ATA[1][1] - ATA[0][1]*ATA[1][0];
    if(fabs(det) < 1e-10f)
        return m_fTheta;

    float inv[2][2];
    inv[0][0] =  ATA[1][1] / det;
    inv[0][1] = -ATA[0][1] / det;
    inv[1][0] = -ATA[1][0] / det;
    inv[1][1] =  ATA[0][0] / det;

    float ux = inv[0][0]*ATb[0] + inv[0][1]*ATb[1];
    float uy = inv[1][0]*ATb[0] + inv[1][1]*ATb[1];
    float norm = sqrtf(ux*ux + uy*uy);
    if (norm < 1e-6f)
        return m_fTheta;

    ux /= norm;
    uy /= norm;

    m_fTheta = atan2f(uy, ux);  // rad
    return m_fTheta;
}

3.平滑滤波

cpp 复制代码

	float cos_new = cosf(theta);
    float sin_new = sinf(theta);
    m_fCos = (1 - ALPHA) * m_fCos + ALPHA * cos_new;
    m_fSin = (1 - ALPHA) * m_fSin + ALPHA * sin_new;

    return atan2f(m_fSin, m_fCos) * 180.0f / M_PI;

总结

这套 4Mic DOA 实现本质是：

GCC-PHAT 提供高精度 TDOA
最小二乘法将 TDOA 转换为方向向量
归一化得到角度

核心公式只有一句： u = ( A T A ) − 1 A T b u=(A^TA)^{-1}A^Tb u=(ATA)−1ATb

而对于后续的优化方向：

亚采样插值：对峰值做二次插值提升精度。
使用加权最小二乘：基于不同的mic对不同 τ \tau τ设置权重。
使用 SVD 替代手写逆矩阵：增强数值稳定性。
增加多帧统计：避免瞬时误判。