一、TDOA（时间差法）原理分析
声音定位的一种常用的方法(时间差法): 通过测量声音信号到达两个不同锚节点(声音接收模块)的时间差来计算未知节点(声响模块)位置。原理如图下：
其中,r12表示未知节点到达锚节点1的距离与未知节点到达锚节点2的距离差；r23示未知节点到达锚节点2的距离与未知节点到达锚节点3的距离差；假设三个锚节点的坐标与未知节点坐标分别为:
锚节点1:(x1,y1)
锚节点2:(x2,y2)
锚节点3:(x3,y3)
未知节点坐标:(x0,y0)
未知节点到达锚节点的时间为t1、t2和t3,则未知节点与锚节点的距离差分别为:
r12=r1-r2=c*t1-c*t2=c(t1- t2)
r23=r2-r3=c*t2-c*t3=c(t2- t3)
上式中c为声音在空气中的传播速度，一般为340m/s.由此可以建立双曲线方程组,如公式如下所示：
通过数学的方法求解双曲线方程组可以得到未知节点的置(r12 和 r23 的交点)。
r12 和 r23 的距离根据时间差和声音传播速度来计算，如何实现测量时间差ΔT1=t1- t2, ΔT1=t2- t3 呢？就要根据信标声音发射装置来确定了。
但是此种方法的定位精度和抗干扰能力不能达到最高最强，还需要优化。
二、H车模与信标场地实际场景应用分析
信标主要通过Chirp声音和RF信号导引，Chirp信号是像鸭子叫的一样不会连续的频谱信号
chirps声音_鹰短尾_ 啁啾03.mp3
(520.69 KB, 下载次数: 84)
信标场地如下：
车模使用麦克纳姆轮的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语言，如果需要完整工程，请留下邮箱一起交流学习。
提升卡
变色卡
千斤顶
1/9 
京公网安备 11010802033920号
Copyright © 2005-2025 EEWORLD.com.cn, Inc. All rights reserved
