1. Analysis of TDOA (Time Difference Algorithm) Principle
A commonly used method for sound localization ( time difference method ): the position of an unknown node ( sound module ) is calculated by measuring the time difference between the sound signal reaching two different anchor nodes ( sound receiving modules ) . The principle is shown below:
Where r12 represents the distance difference between the unknown node and anchor node 1 and the distance difference between the unknown node and anchor node 2 ; r23 represents the distance difference between the unknown node and anchor node 2 and the distance difference between the unknown node and anchor node 3 ; Assume that the coordinates of the three anchor nodes and the coordinates of the unknown node are :
Anchor node 1: (x1, y1)
Anchor node 2: (x2, y2)
Anchor node 3: (x3, y3)
Unknown node coordinates : (x0, y0)
The time when the unknown node reaches the anchor node is t 1 , t 2 and t 3 , then the distance difference between the unknown node and the anchor node is :
r 12 =r 1 -r 2 =c * t 1 -c * t 2 =c(t 1 - t 2 )
r 23 =r 2 -r 3 =c * t 2 -c * t 3 =c(t 2 - t 3 )
In the above formula, c is the speed of sound propagation in the air, which is generally 340m/s. From this, a set of hyperbolic equations can be established , as shown below:
The location of the unknown node ( the intersection of r12 and r23 ) can be obtained by solving the hyperbolic equations mathematically .
The distance between r 12 and r 23 is calculated based on the time difference and the speed of sound propagation. How to measure the time differenceΔT1 =t1- t2, ΔT1 =t2- t3 ? It depends on the beacon sound emission device.
However, the positioning accuracy and anti-interference ability of this method cannot reach the highest level and still need to be optimized.
2. Analysis of the actual application scenarios of H car models and beacon sites
Beacons are guided mainly by chirp sounds and RF signals. Chirp signals are discontinuous spectrum signals like the quacks of ducks.
chirps声音_鹰短尾_ 啁啾03.mp3
(520.69 KB, downloads: 84)
The beacon venues are as follows:
The car model uses a H car model with Mecanum wheels. The size of the car model does not exceed 30cm square. It is assumed that a linear microphone array is installed. The microphone array can be found in Sound Localization - Microphone Array . Suppose the car model we build is like this:
The above algorithm can be used to lock the position of the sound source. The more control algorithm of the Mecanum wheel is the inverse motion algorithm. The linear microphone array can control the steering of the car. The RF signal can be used by the receiver to fit the distance to control the speed of the car.
3. TDOA ( Time Difference Algorithm ) Program Code
Please click on the video to see the positioning effect of linear microphone array using the above algorithm TDOA (time difference method): Linear microphone array positioning effect
Positioning algorithm code:
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;
}
The code runs in KEIL5, C language. If you need a complete project, please leave your email address to communicate and learn together.
提升卡
变色卡
千斤顶
京公网安备 11010802033920号
