波达方向估计DOA

最近一直致力于语音增强方面的工作，主要是增强目标位置发出的语音信号，削弱环境噪音。这里面最有效的方法就是波达方向估计和波束增强了。本篇主要介绍波达方向估计，其包含很多种算法：capon music  RSS GCC等。我这里主要是使用GCC算法，我的麦克阵列使用的是双麦克，8cm距离。以下是我写的C语言版本，由于是第一版，所以比较粗糙，不过性能还是稳定的，角度误差在20°左右，希望能帮到大家：

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include<string.h>
#include<fftw3.h>
#include<math.h>
#include"tool_spline.h"


#define PI 3.1416

typedef struct PCM PCM;
typedef struct Matrix Matrix;
typedef struct X X;
typedef struct conj_X conj_X;
typedef struct max_index max_index;
typedef struct doa_res doa_res;

// 读取文件后，文件内容和文件长度
struct PCM {
	int* data;
	int length;
};

// 把两个麦克的数据组成矩阵
struct Matrix {
	int * m[2];
	int length;
};
// fft变换后，二维的频率数据
struct X {
	fftw_complex* x[2];
};
// X数据的复数共轭
struct conj_X {
	fftw_complex* x[2];
};
// 求取最大值和最大值的索引
struct max_index {
	double data;
	int index;
};
// 最终角度的数据和长度
struct doa_res {
	double* data;
	int n_blocks;
};

// 读取文件
PCM readPCM(char* file_path) {
	FILE* file;
	const int  MAX_END = 2;
	const int  MAX_START = 0;

	file = fopen(file_path, "rb");
	if (file == NULL) {
		printf("open file failed. file_path is :%s\n", file_path);
		PCM cur_pcm = { NULL,0 };
		return cur_pcm;
	}

	fseek(file, 0L, MAX_END);
	int file_len = ftell(file);
	unsigned char* origin_data = (unsigned char*)malloc(file_len + 1);
	int* data = (int*)calloc(file_len / 2, sizeof(int));
	fseek(file, 0L, MAX_START);

	fread((char*)origin_data, 1, file_len, file);

	origin_data[file_len] = '\0'; // 空字符，表示字符串的结束
	int count = 0;
	for (int i = 0; i < file_len - 1; i += 2) {
		data[count++] = (int)((origin_data[i + 1] << 8) | origin_data[i]);
	}

	fclose(file);
	free(origin_data);
	origin_data = NULL;
	struct PCM cur_pcm = { data, file_len / 2 };
	return cur_pcm;
}


// 合成二维矩阵
Matrix getMatrix(char* file_path1, char* file_path2) {
	PCM data1 = readPCM(file_path1);
	PCM data2 = readPCM(file_path2);
	

	// 如果两个pcm文件长度不同，我们以短的作为标准
	if (data1.length > data2.length) {
		data1.length = data2.length;
	}
	else if (data1.length < data2.length) {
		data2.length = data1.length;
	}

	Matrix cur_m;
	for (int i = 0; i < 2; i++) {
		cur_m.m[i] = (int*)calloc(data1.length , sizeof(int));
	}

	memcpy(cur_m.m[0], data1.data, data1.length*sizeof(int));
	memcpy(cur_m.m[1], data2.data, data1.length*sizeof(int));
	cur_m.length = data1.length;

	free(data1.data);
	free(data2.data);
	data1.data = NULL;
	data2.data = NULL;
	return cur_m;

}
// 把src中的元素拷贝到des中，同时把数据从int转化为double
void cpyInt2Double(double *des, int *src,int start,int end){
	int i = 0;
	int count = 0;
	for (i = start; i < end; i++) {
		des[count++] = (double)src[i];
	}
	return;
}

// 快速傅里叶变换
X fft(int start_index, int end_index, Matrix matrix,int N_L) {


	fftw_plan p_left, p_right, p1_left, p1_right;
	/*这里本来准备使用二维方法，不过二维方法反变换一直存在问题，所以暂且使用一维
	left和right其实表示矩阵的两列，即两个麦克风接收到的数据，之所以这么表示因为
	我们使用一维方式进行转换，所以需要挨个转换然后拼接，说白了就是把矩阵分开计算*/
	fftw_complex *out_left;
	fftw_complex *out_right;
	struct X fft_X;

	double *in_left;
	double *in_right;

	double *iout_left;
	double *iout_right;

	
	in_left = (double*)fftw_malloc(N_L * sizeof(double));
	in_right = (double*)fftw_malloc(N_L * sizeof(double));

	out_left = (fftw_complex*)fftw_malloc((N_L / 2 + 1) * sizeof(fftw_complex));
	out_right = (fftw_complex*)fftw_malloc((N_L / 2 + 1) * sizeof(fftw_complex));

	cpyInt2Double(in_left, matrix.m[0], start_index, end_index);
	cpyInt2Double(in_right, matrix.m[1], start_index, end_index);

	p_left = fftw_plan_dft_r2c_1d(N_L, in_left, out_left, FFTW_ESTIMATE);
	p_right = fftw_plan_dft_r2c_1d(N_L, in_right, out_right, FFTW_ESTIMATE);
	fftw_execute(p_left);
	fftw_execute(p_right);

	

	fft_X.x[0] = out_left[0];
	fft_X.x[1] = out_right[0];
	

	fftw_destroy_plan(p_left);
	fftw_destroy_plan(p_right);
	

	fftw_free(in_left);
	fftw_free(in_right);
	return fft_X;
}

// 求一个复数的共轭
void conj(fftw_complex old) {
	old[1] = 0 - old[1];
}

// 获取最大值和最大值索引
max_index getMax_index(double* yy,int num) {
	max_index m;
	m.data = yy[0];
	m.index = 0;
	for (int j = 0; j < num-1; j++) {
		for (int jj = j + 1; jj < num; jj++) {
			if (yy[jj] >= m.data) {
				m.data = yy[jj];
				m.index = jj;
			}	
		}
		break;
	}
	return m;
}



// 波达方向估计
doa_res DOA(int fs,Matrix matrix) {
	int Mic_Num = 2;
	double Marray[2][2] = {0.04, 0.00, -0.04, 0.00};
	int N_L = 1024;
	int N = 256;
	int Snap = N_L / N;
	int n_blocks = floor(matrix.length / N_L);
	int c = 340; 

	// 最终角度集合
	double* angle_gcc = (double*)calloc(n_blocks, sizeof(double)); 
	fftw_plan p;
	
	for (int i = 0; i < n_blocks; i++) {
		int start_index = i*N_L; // C语言中 索引从0开始
		int end_index = (i + 1)*N_L;
		X x = fft(start_index, end_index, matrix, N_L);

		//TODO优化
		double(*delay)[2] = (double(*)[2])calloc((Mic_Num - 1)*Mic_Num, sizeof(double));

		for (int chm = 0; chm < Mic_Num - 1; chm++)
			for (int chn = chm + 1; chn < Mic_Num; chn++) {
				fftw_plan p;
				fftw_complex* Rmn;
				double* Rt;
				Rmn = (fftw_complex*)fftw_malloc((N_L / 2 + 1) * sizeof(fftw_complex));
				Rt = (double*)fftw_malloc(N_L * sizeof(fftw_complex));
				for (int col = 0; col < (N_L / 2 + 1); col++) {
					double a = (x.x[chm][col][0] * x.x[chn][col][0] + x.x[chm][col][1] * x.x[chn][col][1]);
					double b = (x.x[chm][col][1] * x.x[chn][col][0] - x.x[chm][col][0] * x.x[chn][col][1]);
					Rmn[col][0] = a;
					Rmn[col][1] = b;
				}

				p = fftw_plan_dft_c2r_1d(N_L, Rmn, Rt, FFTW_ESTIMATE);
				fftw_execute(p);

				for (int j = 0; j < N_L; j++) {
					Rt[j] /= N_L;
				}

				int step = 1;
				double xx[1024] = { 0.0 };
				double x_old[1024] = { 0.0 };

				for (int j = 0; j < N_L; j += step) {
					xx[j] = (double)j;
				}
				for (int j = 0; j < N_L; j++) {
					x_old[j] = (double)j;
				}

				double yy[1024];
				SPL(N_L, xx, Rt, N_L, x_old, yy);
				max_index m = getMax_index(yy, N_L);

				if (m.index < N_L / 2) {
					delay[chm][chn] = (double)m.index;
				}
				if (m.index > N_L / 2) {
					delay[chm][chn] = (double)-(N_L - m.index);
				}

				fftw_free(Rmn);
				fftw_free(Rt);
				fftw_destroy_plan(p);
			}

		fftw_free(x.x[0]);
		fftw_free(x.x[1]);

		for (int cm = 0; cm < Mic_Num - 1; cm++)
			for (int cn = 0; cn < Mic_Num; cn++) {
				delay[cm][cn] = delay[cm][cn] / fs*c;
		}
		
		double(*dMArray)[2] = (int(*)[2])calloc(Mic_Num*Mic_Num  ,sizeof(double));
		double *dDelay = (double*)calloc(Mic_Num*(Mic_Num / 2), sizeof(double));

		int n = 0;
		for (int chm = 0; chm < Mic_Num - 1; chm++) 
			for (int chn = chm + 1; chn < Mic_Num; chn++) {
				for (int j = 0; j < 2; j++) {
					dMArray[n][j] = Marray[chm][j] - Marray[chn][j];
					dDelay[n] = -delay[chm][chn];
				}
				n += 1;
			}

		int step = 1;
		double *J1 = (double*)calloc(180 / step, sizeof(double));
		
		double Jmin = 1000.0;
		int k = 0;
		double phi1_f=0.0;
		for (int phi = 1; phi <= 180; phi += step) {
			
			double phi1 = phi*PI/180;
			double sc[2] = { cos(phi1) ,sin(phi1) };
			double new_dMArray=0;

			for(int row=0;row<n;row++)	
				for (int col = 0; col < 2; col++) {
				   new_dMArray=new_dMArray+dMArray[row][col] * sc[col];
				}

			double tmp = fabs(dDelay[0] - new_dMArray);
			J1[k]=pow(fabs(dDelay[0] - new_dMArray), 2);

			double J = J1[k];
			if (J < Jmin) {
				Jmin = J;
				phi1_f = phi1;
			}
			k = k + 1;
		}
		
		angle_gcc[i] = phi1_f * 180 / PI;

		
		free(dMArray);
		free(delay);
		free(dDelay);
		free(J1);
		
	}

	doa_res doa = {
		angle_gcc,n_blocks
	};

	return doa;
}


// 求取角度平均值，就是最终结果
int angle_mean(doa_res doa) {

	double sums = 0.0;
	for (int i = 0; i < doa.n_blocks; i++) {
		sums = sums + doa.data[i];
	}
	int angle = (int)(sums / doa.n_blocks+0.5);
	return angle;
}


// 测试使用 结果与matlab保持一致 速度比matlab要快
int main(void) {
	
	 const int fs = 16000;// 采样率
	// const char* file_path1 = "res/mic1_2017-6-22_2-42-4152.pcm";
	// const char* file_path2 = "res/mic2_2017-6-22_2-42-4152.pcm";
	 Matrix matrix = getMatrix(file_path1, file_path2);
	 
	//for (int i = 0; i < 20; i++) {
//		printf("%d   %d\n", matrix.m[i][0], matrix.m[i][1]);
	//}
	doa_res doa=DOA(fs, matrix);
	int angle=angle_mean(doa);

	printf("%d", angle);

	free(matrix.m[0]);
	free(matrix.m[1]);
	free(doa.data);

	printf("hello");
}

这里使用了fftw的第三方工具进行傅里叶变换，tool_spline是网上的一位朋友写的工具类，我这里直接拿来用了如下：

#include <stdio.h>
#include <math.h>

int spline(int n, int end1, int end2,
	double slope1, double slope2,
	double x[], double y[],
	double b[], double c[], double d[],
	int *iflag)
{
	int nm1, ib, i, ascend;
	double t;
	nm1 = n - 1;
	*iflag = 0;
	if (n < 2)
	{  /* no possible interpolation */
		*iflag = 1;
		goto LeaveSpline;
	}
	ascend = 1;
	for (i = 1; i < n; ++i) if (x[i] <= x[i - 1]) ascend = 0;
	if (!ascend)
	{
		*iflag = 2;
		goto LeaveSpline;
	}
	if (n >= 3)
	{
		d[0] = x[1] - x[0];
		c[1] = (y[1] - y[0]) / d[0];
		for (i = 1; i < nm1; ++i)
		{
			d[i] = x[i + 1] - x[i];
			b[i] = 2.0 * (d[i - 1] + d[i]);
			c[i + 1] = (y[i + 1] - y[i]) / d[i];
			c[i] = c[i + 1] - c[i];
		}
		/* ---- Default End conditions */
		b[0] = -d[0];
		b[nm1] = -d[n - 2];
		c[0] = 0.0;
		c[nm1] = 0.0;
		if (n != 3)
		{
			c[0] = c[2] / (x[3] - x[1]) - c[1] / (x[2] - x[0]);
			c[nm1] = c[n - 2] / (x[nm1] - x[n - 3]) - c[n - 3] / (x[n - 2] - x[n - 4]);
			c[0] = c[0] * d[0] * d[0] / (x[3] - x[0]);
			c[nm1] = -c[nm1] * d[n - 2] * d[n - 2] / (x[nm1] - x[n - 4]);
		}
		/* Alternative end conditions -- known slopes */
		if (end1 == 1)
		{
			b[0] = 2.0 * (x[1] - x[0]);
			c[0] = (y[1] - y[0]) / (x[1] - x[0]) - slope1;
		}
		if (end2 == 1)
		{
			b[nm1] = 2.0 * (x[nm1] - x[n - 2]);
			c[nm1] = slope2 - (y[nm1] - y[n - 2]) / (x[nm1] - x[n - 2]);
		}
		/* Forward elimination */
		for (i = 1; i < n; ++i)
		{
			t = d[i - 1] / b[i - 1];
			b[i] = b[i] - t * d[i - 1];
			c[i] = c[i] - t * c[i - 1];
		}
		/* Back substitution */
		c[nm1] = c[nm1] / b[nm1];
		for (ib = 0; ib < nm1; ++ib)
		{
			i = n - ib - 2;
			c[i] = (c[i] - d[i] * c[i + 1]) / b[i];
		}
		b[nm1] = (y[nm1] - y[n - 2]) / d[n - 2] + d[n - 2] * (c[n - 2] + 2.0 * c[nm1]);
		for (i = 0; i < nm1; ++i)
		{
			b[i] = (y[i + 1] - y[i]) / d[i] - d[i] * (c[i + 1] + 2.0 * c[i]);
			d[i] = (c[i + 1] - c[i]) / d[i];
			c[i] = 3.0 * c[i];
		}
		c[nm1] = 3.0 * c[nm1];
		d[nm1] = d[n - 2];
	}
	else
	{
		b[0] = (y[1] - y[0]) / (x[1] - x[0]);
		c[0] = 0.0;
		d[0] = 0.0;
		b[1] = b[0];
		c[1] = 0.0;
		d[1] = 0.0;
	}
LeaveSpline:
	return 0;
}
double seval(int n, double u,
	double x[], double y[],
	double b[], double c[], double d[],
	int *last)
{
	int    i, j, k;
	double w;
	i = *last;
	if (i >= n - 1) i = 0;
	if (i < 0)  i = 0;
	if ((x[i] > u) || (x[i + 1] < u))
	{
		i = 0;
		j = n;
		do
		{
			k = (i + j) / 2;
			if (u < x[k])  j = k;
			if (u >= x[k]) i = k;
		} while (j > i + 1);
	}
	*last = i;
	w = u - x[i];
	w = y[i] + w * (b[i] + w * (c[i] + w * d[i]));
	return (w);
}
void SPL(int n, double *x, double *y, int ni, double *xi, double *yi)
{
	double *b, *c, *d;
	int iflag, last, i;
	b = (double *)malloc(sizeof(double) * n);
	c = (double *)malloc(sizeof(double) * n);
	d = (double *)malloc(sizeof(double) * n);
	if (!d) { printf("no enough memory for b,c,d\n"); }
	else {
		spline(n, 0, 0, 0, 0, x, y, b, c, d, &iflag);
		// if (iflag == 0)  printf("I got coef b,c,d now\n"); else printf("x not in order or other error\n");
		for (i = 0; i<ni; i++) yi[i] = seval(ni, xi[i], x, y, b, c, d, &last);
		free(b); free(c); free(d);
	};
}
/**
main() {
	double x[6] = { 0.,1.,2.,3.,4.,5 };
	double y[6] = { 0.,0.5,2.0,1.6,0.5,0.0 };
	double u[8] = { 0.5,1,1.5,2,2.5,3,3.5,4 };
	double s[8];
	int i;
	SPL(6, x, y, 8, u, s);
	for (i = 0; i<8; i++) printf("%lf %lf \n", u[i], s[i]);
	return 0;
}**/

希望能帮到大家。

转载自原文链接, 如需删除请联系管理员。

原文链接：波达方向估计DOA，转载请注明来源！