信标导航声音定位与识别——声源定位算法分析

一、TDOA（时间差法）原理分析

声音定位的一种常用的方法(时间差法): 通过测量声音信号到达两个不同锚节点(声音接收模块)的时间差来计算未知节点(声响模块)位置。原理如图下：

其中,r₁₂表示未知节点到达锚节点1的距离与未知节点到达锚节点2的距离差；r₂₃示未知节点到达锚节点2的距离与未知节点到达锚节点3的距离差；假设三个锚节点的坐标与未知节点坐标分别为:

锚节点1:(x1,y1)

锚节点2:(x2,y2)

锚节点3:(x3,y3)

未知节点坐标:(x0,y0)

未知节点到达锚节点的时间为t₁、t₂和t₃,则未知节点与锚节点的距离差分别为:

r₁₂=r₁-r₂=c_*t₁-c_*t₂=c(t₁- t₂)

r₂₃=r₂-r₃=c_*t₂-c_*t₃=c(t₂- t₃)

上式中c为声音在空气中的传播速度，一般为340m/s.由此可以建立双曲线方程组,如公式如下所示：

通过数学的方法求解双曲线方程组可以得到未知节点的置(r_{12 和} r_{23 的交点})。

r_{12 和} r_{23 的距离根据时间差和声音传播速度来计算，}如何实现测量时间差ΔT1=t₁- t_2, ΔT1=t₂- t_{3 呢？就要根据信标声音发射装置来确定了。}

但是此种方法的定位精度和抗干扰能力不能达到最高最强，还需要优化。

二、H车模与信标场地实际场景应用分析

  信标主要通过Chirp声音和RF信号导引，Chirp信号是像鸭子叫的一样不会连续的频谱信号

信标场地如下：

车模使用麦克纳姆轮的H车模，车模尺寸不超过30cm见方，假设安装线性麦克风阵列，麦克风阵列见声音定位——麦克风阵列。假设我们搭建的车模是这样的：

则可通过上述算法锁定声源的位置，麦克纳姆轮更多的控制算法就是逆运动算法，线性麦克风阵列可以控制小车转向，RF信号可以用接收机来拟合距离控制小车的速度。

三、TDOA（时间差法）程序代码

通过上述算法TDOA(时间差法）线性麦克风阵列定位效果请戳大佬视频：线性麦克风阵列定位效果

定位算法代码：

 if(INT1==1&&INT2==1&&INT3==1)
			   {
				
					printf("TimeDelay1=%.8f\t",TimeDelay1);
					printf("TimeDelay2=%.8f\t",TimeDelay2);
					printf("TimeDelay3=%.8f\t",TimeDelay3);
			/**************声源模块距离接受模块2最远时算法如下*********************/
					if(TimeDelay2>TimeDelay1&&TimeDelay2>TimeDelay3)
					{
						printf("接受模块2最远\n");
						d1=(TimeDelay2-TimeDelay1)*V;
						d2=(TimeDelay2-TimeDelay3)*V;
						printf("d1=%f\t",d1);
						printf("d2=%f\t",d2);
						A=4*(pow(d2,2)-pow(d1,2)-pow(a,2));
						printf("A=%f\t",A);
						B=4*(d2*pow(a,2)-pow(d2,3)+d1*pow(a,2)-pow(d1,3));
						printf("B=%f\t",B);
						C=pow((pow(a,2)-pow(d2,2)),2)-pow((pow(a,2)-pow(d1,2)),2);
						printf("C=%f\t",C);
					     X1=(-B+sqrt(pow(B,2)-4*A*C))/2.0*A;
						 X2=(-B-sqrt(pow(B,2)-4*A*C))/2.0*A;
											 	
						 if(X1>0)
						 { 	printf("X1=%d\t",X1);
						   sin=(pow(X1,2)+pow(a,2)-pow((X1-d1),2))/(2*a*X1);
						   printf("sin=%f\t",sin);
						   cos=(pow(X1,2)+pow(a,2)-pow((X1-d2),2))/(2*a*X1);
						   printf("cos=%f\t",cos);
						   x=X1*cos;
						   y=X1*sin;
						 }

						 if(X2>0)
						 {		
						    printf("X2=%d\t",X2);
						 	sin=(pow(X2,2)+pow(a,2)-pow((X2-d1),2))/(2*a*X2);
							printf("sin=%f\t",sin);
						    cos=(pow(X2,2)+pow(a,2)-pow((X2-d2),2))/(2*a*X2);
							printf("cos=%f\t",cos);
						 	x=X2*cos;
						    y=X2*sin;
						 }
					}
			 /**************声源模块距离接受模块2最近时算法如下*********************/
			  	if(TimeDelay2<TimeDelay1&&TimeDelay2<TimeDelay3)
					{
						printf("接受模块2最近\n");
						d1=(TimeDelay1-TimeDelay2)*V;
						d2=(TimeDelay3-TimeDelay2)*V;
						printf("d1=%f\t",d1);
						printf("d2=%f\t",d2);
						A=4*(pow(d2,2)-pow(d1,2)-pow(a,2));
						printf("A=%f\t",A);
						B=4*(d2*pow(a,2)+pow(d2,3)+d1*pow(a,2)+pow(d1,3));
						printf("B=%f\t",B);
						C=pow((pow(a,2)-pow(d2,2)),2)-pow((pow(a,2)-pow(d1,2)),2);
						printf("C=%f\t",C);
					    X1=(-B+sqrt(pow(B,2)-4*A*C))/2.0*A;
						X2=(-B-sqrt(pow(B,2)-4*A*C))/2.0*A;
//						 printf("X1=%d\t",X1);
//						 printf("X2=%d\t",X2);					 	 
						 if(X1>0)
						 {		 
						   sin=(pow(X1,2)+pow(a,2)-pow((X1+d1),2))/(2*a*X1);
						   printf("sin=%f\t",sin);
						   cos=(pow(X1,2)+pow(a,2)-pow((X1+d2),2))/(2*a*X1);
						   printf("cos=%f\t",cos);
						   x=X1*cos;
						   y=X1*sin;
						 }

						 if(X2>0)
						 {	 
						 	
						 	sin=(pow(X2,2)+pow(a,2)-pow((X2-d1),2))/(2*a*X2);
							printf("sin=%f\t",sin);
						    cos=(pow(X2,2)+pow(a,2)-pow((X2-d2),2))/(2*a*X2);
							printf("cos=%f\t",cos);
						 	x=X2*cos;
						    y=X2*sin;
						 }
					}
			 	 /**************声源模块距离接受模块1最近 接受模块3最远时算法如下*********************/
				  if(TimeDelay2>TimeDelay1&&TimeDelay2<TimeDelay3)
				  {
				    printf("接受模块3最远\t");
				  	d1=(TimeDelay2-TimeDelay1)*V;
				    d2=(TimeDelay3-TimeDelay2)*V;
					printf("d1=%f\t",d1);
					printf("d2=%f\t",d2);
			   	    B=4*(d2*pow(a,2)+pow(d2,3)+d1*pow(a,2)-pow(d1,3));
					printf("B=%f\t",B);
					A=4*(pow(d2,2)-pow(d1,1)-pow(a,2));
					printf("A=%f\t",A);
					C=pow((pow(a,2)-pow(d2,2)),2)-pow((pow(a,2)-pow(d1,2)),2);
					printf("C=%f\t",C);
					X1=((-B)+sqrt(pow(B,2)-(4*A*C)))/2.0*A;
					X2=((-B)-sqrt(pow(B,2)-(4*A*C)))/2.0*A;
									 
					
					if(X1>0)
					{		
					    printf("X1=%d\t",X1);	
					    sin=(pow(X1,2)+pow(a,2)-pow((X1-d1),2))/(2*a*X1);
						printf("sin=%f\t",sin);
					    cos=(pow(X1,2)+pow(a,2)-pow((X1+d2),2))/(2*a*X1);
						printf("cos=%f\t",cos);
						x=X1*cos;
						y=X1*sin;
					}
					 if(X2>0)
					 {	printf("X2=%d\t",X2);
					 	sin=(pow(X1,2)+pow(a,2)-pow((X1-d1),2))/(2*a*X1);
						printf("sin=%f\t",sin);
					    cos=(pow(X1,2)+pow(a,2)-pow((X1+d2),2))/(2*a*X1);
						printf("cos=%f\t",cos);
						x=X2*cos;
						y=X2*sin;
					 }
			   	   }
				/**************声源模块距离接受模块1最远 接受模块3最近时算法如下*********************/
					if(TimeDelay2>TimeDelay3&&TimeDelay2<TimeDelay1)
				  {
				  	printf("接受模块1最远\t");
				  	d1=(TimeDelay1-TimeDelay2)*V;
				    d2=(TimeDelay2-TimeDelay3)*V;
					printf("d1=%f\t",d1);
					printf("d2=%f\t",d2);
			   	    B=4*(d2*pow(a,2)-pow(d2,3)+d1*pow(a,2)+pow(d1,3));
					printf("B=%f\t",B);
					A=4*(pow(d2,2)-pow(d1,1)-pow(a,2));
					printf("A=%f\t",A);
					C=pow((pow(a,2)-pow(d2,2)),2)-pow((pow(a,2)-pow(d1,2)),2);
					printf("C=%f\t",C);
					X1=(-B+sqrt(pow(B,2)-4*A*C))/2.0*A;
					X2=(-B-sqrt(pow(B,2)-4*A*C))/2.0*A;
										 
					
					if(X1>0)
					{  	printf("X1=%d\t",X1);
					    sin=(pow(X1,2)+pow(a,2)-pow((X1+d1),2))/(2*a*X1);
						printf("sin=%f\t",sin);
					    cos=(pow(X1,2)+pow(a,2)-pow((X1-d2),2))/(2*a*X1);
						printf("cos=%f\t",cos);
						x=X1*cos;
						y=X1*sin;
					}
					 if(X2>0)
					 {	printf("X2=%d\t",X2);
					 	sin=(pow(X1,2)+pow(a,2)-pow((X1+d1),2))/(2*a*X1);
							printf("sin=%f\t",sin);
					    cos=(pow(X1,2)+pow(a,2)-pow((X1-d2),2))/(2*a*X1);
							printf("cos=%f\t",cos);
						x=X2*cos;
						y=X2*sin;
					 }
			   	   }
		  printf("横坐标X=%f\t",x);
		  printf("纵坐标Y=%f\t",y);
	                 INT1= INT2=INT3=0;
					 TimeDelay3=TimeDelay2=TimeDelay1=0;
					 Flag=0;
			   }

代码运行环境为KEIL5，C语言，如果需要完整工程，请留下邮箱一起交流学习。

本文来自论坛，点击查看完整帖子内容。