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

bqgup 发表于 2020-3-10 15:18

<div class='showpostmsg'>一、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信号是像鸭子叫的一样不会连续的频谱信号



信标场地如下：

 



车模使用麦克纳姆轮的H车模，车模尺寸不超过30cm见方，假设安装线性麦克风阵列，麦克风阵列见<a href="https://bbs.eeworld.com.cn/forum.php?mod=viewthread&tid=1112483&fromuid=1014845" target="_blank">声音定位——麦克风阵列</a>。假设我们搭建的车模是这样的：

 



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

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

通过上述算法TDOA(时间差法）线性麦克风阵列定位效果请戳大佬视频：<a href="https://b23.tv/av93906103" target="_blank">线性麦克风阵列定位效果</a>

定位算法代码：

<pre>
<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;
 }</code></pre>

代码运行环境为KEIL5，C语言，如果需要完整工程，请留下邮箱一起交流学习。
</div><script> var loginstr = '<div class="locked">查看精华帖全部内容，请<a href="javascript:;" style="color:#e60000" class="loginf">登录</a>或者<a href="https://bbs.eeworld.com.cn/member.php?mod=register_eeworld.php&action=wechat" style="color:#e60000" target="_blank">注册</a></div>';

if(parseInt(discuz_uid)==0){
(function($){
var postHeight = getTextHeight(400);
$(".showpostmsg").html($(".showpostmsg").html());
$(".showpostmsg").after(loginstr);
$(".showpostmsg").css({height:postHeight,overflow:"hidden"});
})(jQuery);
}
</script><script type="text/javascript">(function(d,c){var a=d.createElement("script"),m=d.getElementsByTagName("script"),eewurl="//counter.eeworld.com.cn/pv/count/";a.src=eewurl+c;m.parentNode.insertBefore(a,m)})(document,523)</script>

rshjh 发表于 2020-3-10 15:51

大神，今年菜鸡入坑，请指导：rshjh@qq.com，谢谢！

Handsomebj 发表于 2020-3-10 16:46

神，今年菜鸡入坑，请指导：761490193@qq.com，谢谢！

kbkjbjn 发表于 2020-3-10 16:52

前辈你好我想学习下您的代码，如果方便交流的话可以将您的工程发我一份吗？这是我的邮箱319024367@qq.com

ABCBJ 发表于 2020-3-11 00:04

能给一下完整的工程吗？

ABCBJ 发表于 2020-3-11 00:04

前辈我的邮箱是2634655801@qq.com

乐乐爱玩耍 发表于 2020-3-11 11:02

大佬，可以发一下工程学习学习吗，874818045@qq.com，谢谢大佬:)。

按位取反 发表于 2020-3-11 11:49

大佬，可以给我发完整的工程吗，我的邮箱是1873619703@qq.com

865439320 发表于 2020-3-11 12:09

谢谢大佬的分享，我的邮箱是865439320@qq.com,希望大佬能发一份工程给我交流学习谢谢

scasc 发表于 2020-3-11 15:53

可以发一下完整的代码工程吗 2090147256@qq.com 谢谢

Toubidou 发表于 2020-3-11 16:58

前辈你好我想学习下您的代码，如果方便交流的话可以将您的工程发我一份吗？这是我的邮箱285619046@qq.com

Anthony0619 发表于 2020-3-11 23:02

前辈可以探讨一下吗，我的邮箱是56672035@qq.com，最近的项目也是卡在了这个问题上，感谢前辈的指导了

HEHW 发表于 2020-3-12 09:36

您好，最近也在研究信标组的声源定位，非常感谢您给出的这份思路，希望和您交流学习一下，谢谢！mi985211211100c@163.com

leo1210 发表于 2020-3-12 09:41

前辈，我想学习下这个声音定位，我的邮箱1776111872@qq.com

拾一号 发表于 2020-3-12 20:30

冒昧的问一下，文中的“未知节点”指的是信标位置吗，“锚节点”是麦克风位置吗？如果是的话，为什么要计算r12和r23啊？

nemon 发表于 2020-3-13 00:24

感谢楼主分享核心算法。先学习一下。

xmprocat 发表于 2020-3-13 00:41

您好，可以分享一下完整工程么2591081434@qq.com，有兴趣交流的话也可以加波好友

bqgup 发表于 2020-3-13 09:42

拾一号发表于 2020-3-12 20:30
冒昧的问一下，文中的“未知节点”指的是信标位置吗，“锚节点”是麦克风位置吗？如果 ...

“未知节点”指的是信标位置，“锚节点”是麦克风位置。通过r12和r23求解信标的坐标

魔咒2 发表于 2020-3-13 16:10

前辈，您好，我的邮箱是1964624254@qq.com ，谢谢您，很需要您的完整工程

魔咒2 发表于 2020-3-13 16:13

前辈，您好，很需要您的完整工程，我的邮箱是1964624254@qq.com,谢谢您

页: [1] 2 3 4 5

电子工程世界-论坛's Archiver

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