基于MATLAB的声源定位系统

clc; clear; figure(1); sound_position=[5,6]; %声源实际位置 %scatter(sound_position(1,1), sound_position(1,2), 'r', 'filled');%在图中标出实际位置 axis([0, 10, 0, 10]); %限定X,Y轴的范围 grid on; %画出网格线 mic1_positon=[-10,0]; mic2_positon=[0,0]; mic3_positon=[10,0]; wave = audioread('sample.wav'); wave = wave(:,1); %数组第一列 scale = 0.8/max(wave); wave = scale*wave; Trials = 10; %测试点的个数 Radius = 50; N = 3; Sen_position=[mic1_positon;mic2_positon;mic3_positon]; True_position = zeros(Trials, 3); Est_position = zeros(Trials,3); % 生成十个随机位置，半径在50以内 for i=1:Trials r = rand(1)*Radius; t = rand(1)*pi; x = r*cos(t); y = r*sin(t); True_position(i,1) = x; True_position(i,2) =y; end %计算距离 Distances = zeros(Trials,N); for i=1:Trials for j=1:N x1 = True_position(i,1); y1 = True_position(i,2); x2 = Sen_position(j,1); y2 = Sen_position(j,2); Distances(i,j) = sqrt((x1-x2)^2 + (y1-y2)^2 ); end end TimeDelay = Distances./340.29; Padding = TimeDelay*44100; %时延乘采样率 1s的数据的时延，十个数据点，八个MIC for i=1:Trials x = True_position(i,1); y = True_position(i,2); xstr = num2str(round(x)); ystr = num2str(round(y)); istr = num2str(i); mic1 = [zeros(round(Padding(i,1)),1) ; wave]; mic2 = [zeros(round(Padding(i,2)),1) ; wave]; mic3 = [zeros(round(Padding(i,3)),1) ; wave]; %创建一个全零矩阵，矩阵长度为padding，再续上一个wave %麦克一开始什么都收不到，经过一定时间的延迟，收到了音频信号 l1 = length(mic1); l2 = length(mic2); l3 = length(mic3); lenvec = [l1 l2 l3]; m = max(lenvec); c = [m-l1, m-l2, m-l3]; %找到最远的接受端 mic1 = [mic1; zeros(c(1),1)]; mic2 = [mic2; zeros(c(2),1)]; mic3 = [mic3; zeros(c(3),1)]; %补零，最近的声音结束了，最远的没有，通过补零让数组都一样长 mic1 = mic1./(Distances(i,1))^2; mic2 = mic2./(Distances(i,2))^2; mic3 = mic3./(Distances(i,3))^2; %声强和距离的关系是平方I = P / (4 * π * r?) multitrack = [mic1, mic2, mic3]; % wavwrite(multitrack, 44100, name); [x y] = Locate(Sen_position, multitrack); Est_position(i,1) = x; Est_position(i,2) = y; end plot(True_position(:,1),True_position(:,2),'bd',Est_position(:,1),Est_position(:,2),'r+','LineWidth',2); legend('True Position','Estimated Position'); xlabel('X coordinate of target'); ylabel('Y coordinate of target'); title('TDOA Hyperbolic Localization'); axis([-50 50 -50 50]); %越接近0度和180度时误差越大，这是原理上的问题 function [x y] = Locate(Sen_position, multitrack) s = size(Sen_position); len = s(1); timedelayvec = zeros(len,1); for i=1:len timedelayvec(i) = timedelayfunc(multitrack(:,1),multitrack(:,i)); end t1=timedelayvec(1)-timedelayvec(2); t2=timedelayvec(3)-timedelayvec(2); r=(2*(10)^2-340.29^2*(t1^2+t2^2))/(2*340.29*(t1+t2)); a=acos((t1-t2)/20*(340.29^2*(t1+t2)/(2*r)+340.29)); x=r*cos(a); y=r*sin(a); end function out = timedelayfunc(x,y) % suppose sampling rate is 44100 % Let Tx be transit time for x % Let Ty be transit time for y % out is Ty - Tx c = xcorr(x, y); %互相关函数 [C I] = max(c); out = ((length(c)+1) - I)/44100; end