这个算法在单片机上也许跑不动。。。
今天偶然看见云汉上了个新板子STM32F413H-DISCO，这板子很有意思地带了个5麦克风阵列，可能是要面向一些DSP应用吧。
于是我开了个脑洞，能不能把电磁波阵列雷达的原理应用在声波上！！！
如果每个麦克风搞个相位测距，然后一系列阵列一起用来定位，最后再加个连续波测距解决相位多解的问题，那岂不美滋滋。
当然，我也不能无凭无据就脑残一拍就申请对不对，
万一拿到板子做不出来坑爹了，那多对不起一起竞争申请的小伙伴们啊。
所以呢，我先用数学方法建了个模，然后仿真了一下算法，要用到的软件是matlab。
我用了一下午时间，调通了算法。但是发现在高性能x86_64平台上跑起来也速度略慢。
:L :L
都是各种大矩阵，也许该找个好显卡用GPU算吧。
也许这个算法不适合在arm板子上跑，一是速度，二是内存，所以我放弃申请了。
这对小伙伴来说应该是个好消息吧，少一个对手，多一丝希望。
言归正传，今天我先分享一下这个声波雷达的模型，或者说是算法吧。
假设有个空间A，深度1米，左右各宽1米，有个音源放在这个空间的某个坐标上。我们该怎样用相位方法对它定位呢？
首先，定位需要至少两个麦克风。我原打算用笔记本跑这个算法的，所以先做了个双麦克风的算法，实现XY坐标系二维的定位。
这个算法其实只要再在与原两个麦克风不共线的位置布置一颗麦克风，就可以很容易地改成XYZ三维空间的定位。
要做相位测距，就要用到快速傅里叶变换。
为了让能分辨的频率正好处于整数位置，我设置了采样点数和采样频率都是1024，这样可以对1到512Hz的声波进行“点对点”的相位计算，这在以后增加连续波算法时候会有方法上的优势。因为单片机的频率都是整数的，产生一个整数频率的声波，要比带一堆小数点的容易一些。
我定义了一下基本的物理参数，作为各个变量的初始设置。以下是matlab代码：
%初始设置
pp=200;%空间离散的点数
p1=0.035;%左麦克与中轴距离
p2=-0.035;%右麦克与中轴距离
va=340;%声速
Fs=1024;%采样频率
T=1/Fs;%采样周期
L=1024;%采样数
t=(0:L-1)*T;%时间离散化
接着，对音源做了建模。先试一下100Hz吧。由波长分析可以知道，如果频率较高，不一定能在空间A里都有实解，也许会有多解，或复数解。
f=100;%信号源频率 取FFT实际可解析的值
orig=cos(2*pi*f*t);%信号源波形
%设置信号源
subplot(2,1,1)
plot(Fs*t(1:50),orig(1:50)) %只画前50毫秒就够了，太密了也没意思
title('音源波形')
xlabel('毫秒')
%测试代码，对音源做FFT变换，验证算法
NFFT = 2^nextpow2(L); %采样数L下 FFT的信号长度的最小值
YY = fft(orig,NFFT)/L;%FFT变换
ff = Fs/2*linspace(0,1,NFFT/2+1); %FFT实际可解析的频率
subplot(2,1,2)
plot(ff,2*abs(YY(1:NFFT/2+1))) %画频谱图
title('音源频谱')
xlabel('频率(Hz)')
ylabel('强度')
%相位角算法验证
DB=2*abs(YY(1:NFFT/2+1));%先把音源频率列出来
ii=find(DB==max(DB));%然后找到音源的主频在数组里的第几个
HZ=ff(ii);%找到主频
angleorig=angle(YY(ii));%最后计算相位角，理论上应该是个接近0的无穷小数。
100Hz的音源的信息如下图所示。
我们假设音源可以位于空间A的某个点上。这个点应该有实际的坐标可以定位，并且其与两个麦克风的距离也可以精确地给定。这部分是很基础的物理建模。
%物理建模。两个麦克与XY空间上各点的距离
x=linspace(-1,1,pp);%x空间离散化
y=linspace(0.1,1.0,pp);%y空间离散化
=meshgrid(x,y);%组合个坐标矩阵
R1=((X-p1).^2+Y.^2).^0.5;%与左麦克距离
R2=((X-p2).^2+Y.^2).^0.5;%与右麦克距离
然后，根据声速算时间差。这样，音源位于XY空间上任意点时候，两个麦克风得到的波形应该满足基本的物理规律，波动方程。代码里有个防止多解的修饰，这个在数学上不是必要的，因为可以有复数解。但是工程上应该对这样的点做逻辑修饰，给它个不会跑飞并且值域跨度不大的实数解。
%物理建模。音源位于XY空间上各点时，声速时间差。
dt1=R1;%音源位于XY空间上各点时，声音传播到左麦克的时间，这是个矩阵。
%介绍一个matlab定义矩阵数组并申请数组空间的小技巧，用一个维度相同的任意数组，直接等于就好了
dt2=R2;%右麦克，其他同上
for i=1:pp;
for j =1:pp;
dt1(j,i)=R1(j,i)/va;
dt2(j,i)=R2(j,i)/va;
%防止多解的修饰
if dt1(j,i)>=1/f
dt1(j,i)=0.999*1/f;
end
if dt2(j,i)>=1/f
dt2(j,i)=0.999*1/f;
end
end
end
推导出麦克风接收到的波形以后，物理建模部分结束。下面代码只演示地描述了音源在(j,i)点时候左麦克风的波形，完整的代码最后会给大家。
%左麦克
sig1=cos(2*pi*f*(t-dt1(j,i)));%物理建模
接下来就要跑定位算法了。
先要对这个收到的波形进行相位计算。当然是要用经典的FFT变换
%先物理建模得到波形。然后模拟定位算法，用FFT计算相位。
angle1=R1;%音源位于XY空间上各点时，左麦克得到的相位角，这是个矩阵。
%介绍一个matlab定义矩阵数组并申请数组空间的小技巧，用一个维度相同的任意数组，直接等于就好了
ff1=R1;%音源位于XY空间上各点时，左麦克得到的FFT变换结果，这是个复数矩阵。
angle2=R2;%右麦克，其他同上
ff2=R2;%右麦克，其他同上
%下面这个循环用于计算音源位于XY空间上各点时，两个麦克的相位角
for i=1:pp;
for j =1:pp;
%左麦克
sig1=cos(2*pi*f*(t-dt1(j,i)));%物理建模
YY = fft(sig1,NFFT)/L;%FFT变换
DB=2*abs(YY(1:NFFT/2+1));
ii=find(DB==max(DB));
ff1(j,i)=ff(ii);
angle1(j,i)=angle(YY(ii));
%右麦克
sig2=cos(2*pi*f*(t-dt2(j,i)));%物理建模
YY = fft(sig2,NFFT)/L;%FFT变换
DB=2*abs(YY(1:NFFT/2+1));
ii=find(DB==max(DB));
ff2(j,i)=ff(ii);
angle2(j,i)=angle(YY(ii));
end
end
%下面把相位角可视化一下
figure;
subplot(2,1,1)
mesh(X,Y,angle1)
title('音源位于各点时左麦克测得的相位')
subplot(2,1,2)
mesh(X,Y,angle2)
title('音源位于各点时右麦克侧得相位')
当音源在XY空间的各个点上时，两个麦克风测得的相位应该是这样的
然后是距离计算，代码如下。因为前面做过修饰，很巧妙地规避了多解。
%定位算法。计算距离。
dis1=angle1;%音源与左麦克的距离，定义矩阵数组的技巧同上一段代码
dis2=angle2;%音源与右麦克的距离
for i=1:pp;
for j =1:pp;
%矩阵初值0
dis1(j,i)=0;
dis2(j,i)=0;
%左麦克
if angle1(j,i)<=0
dis1(j,i)=-1*va*angle1(j,i)/(2*pi*f);
end
if angle1(j,i) >0
dis1(j,i)=-1*va*(angle1(j,i)-2*pi)/(2*pi*f);
end
%右麦克
if angle2(j,i)<=0
dis2(j,i)=-1*va*angle2(j,i)/(2*pi*f);
end
if angle2(j,i) >0
dis2(j,i)=-1*va*(angle2(j,i)-2*pi)/(2*pi*f);
end
end
end
%把定位算法得到的距离可视化一下
figure;
subplot(2,1,1)
mesh(X,Y,dis1)
title('音源距离左麦克')
subplot(2,1,2)
mesh(X,Y,dis2)
title('音源距离右麦克')
当音源在XY空间的各个点上时，两个麦克风测得的音源距离是下图这样的。它们有细微的差别，这个差别是多麦克风定位的基础，这有点像双目视觉。
接着，以左右麦克风为圆心，左右麦克风测得的音源距离分别为两个圆的半径，画两个圆。应该会有两个交点。选y处于正解区的那个解，虽然数学上可以有两个解，但是工程上的计算一定要做逻辑判断滤掉无效解。
最后，这个交点的XY坐标就是我们的声波相位雷达对音源的空间定位坐标。
代码如下。千万小心那个负数开根号，分分种进入复数空间的节奏，当然我已经在算法上修饰了。
%定位算法。两圆相交求xy坐标。
X1=angle1;
Y1=angle1;
for i=1:pp;
for j =1:pp;
X1(j,i)=0;
Y1(j,i)=0;
X1(j,i)=-(- p1^2 + p2^2 + dis1(j, i)^2 - dis2(j, i)^2)/(2*(p1 - p2));
if (p1 - p2 + dis1(j, i) - dis2(j, i))*(p1 - p2 - dis1(j, i) + dis2(j, i))>0.000
Y1(j,i)=((p1 - p2 + dis1(j, i) + dis2(j, i))*(p2 - p1 + dis1(j, i) + dis2(j, i))*(p1 - p2 + dis1(j, i) - dis2(j, i))*(p1 - p2 - dis1(j, i) + dis2(j, i)))^(1/2)/(2*(p1 - p2));
end
end
end
figure;
subplot(2,1,1)
mesh(X,Y,X1)
title('各点的x坐标定位')
subplot(2,1,2)
mesh(X,Y,Y1)
title('各点的y坐标定位')
解的图片如下。解算得到的X和Y坐标值分两个图画出来了。很完美，解全在实数区，基本上线性连续平滑且单调，看上去是个正确的答案。
解有多精确，用眼睛看或是用语言描述是没有说服力的。算法要定量分析，要用实实在在的数据做证明。
我们得承认解算得到的这个坐标值和一开始物理建模得到的精确坐标值是不一样的，有一定的误差。
下面我们做一个误差估计。代码如下。
%误差估计
ERR1=X1-X;
ERR2=Y1-Y;
ERR=(ERR1.^2+ERR2.^2).^0.5
figure;
mesh(X,Y,ERR)
title('各点的定位误差')
误差在XY空间上的分布画出来了。
我们可以发现，这个算法在较远的位置和靠近两个麦克风中轴线附近有非常好的精度。在偏离中轴线并且靠近麦克风阵列时误差较大，但依然在可以忍受的范围（竖坐标的数量级是10的负12）
现在可以说“完美”了:lol
这个算法在单片机上也许跑不动。。。
今天偶然看见云汉上了个新板子STM32F413H-DISCO，这板子很有意思地带了个5麦克风阵列，可能是要面向一些DSP应用吧。
于是我开了个脑洞，能不能把电磁波阵列雷达的原理应用在声波上！！！
如果每个麦克风搞个相位测距，然后一系列阵列一起用来定位，最后再加个连续波测距解决相位多解的问题，那岂不美滋滋。
当然，我也不能无凭无据就脑残一拍就申请对不对，
万一拿到板子做不出来坑爹了，那多对不起一起竞争申请的小伙伴们啊。
所以呢，我先用数学方法建了个模，然后仿真了一下算法，要用到的软件是matlab。
我用了一下午时间，调通了算法。但是发现在高性能x86_64平台上跑起来也速度略慢。
:L :L
都是各种大矩阵，也许该找个好显卡用GPU算吧。
也许这个算法不适合在arm板子上跑，一是速度，二是内存，所以我放弃申请了。
这对小伙伴来说应该是个好消息吧，少一个对手，多一丝希望。
言归正传，今天我先分享一下这个声波雷达的模型，或者说是算法吧。
假设有个空间A，深度1米，左右各宽1米，有个音源放在这个空间的某个坐标上。我们该怎样用相位方法对它定位呢？
首先，定位需要至少两个麦克风。我原打算用笔记本跑这个算法的，所以先做了个双麦克风的算法，实现XY坐标系二维的定位。
这个算法其实只要再在与原两个麦克风不共线的位置布置一颗麦克风，就可以很容易地改成XYZ三维空间的定位。
要做相位测距，就要用到快速傅里叶变换。
为了让能分辨的频率正好处于整数位置，我设置了采样点数和采样频率都是1024，这样可以对1到512Hz的声波进行“点对点”的相位计算，这在以后增加连续波算法时候会有方法上的优势。因为单片机的频率都是整数的，产生一个整数频率的声波，要比带一堆小数点的容易一些。
我定义了一下基本的物理参数，作为各个变量的初始设置。以下是matlab代码：
%初始设置
pp=200;%空间离散的点数
p1=0.035;%左麦克与中轴距离
p2=-0.035;%右麦克与中轴距离
va=340;%声速
Fs=1024;%采样频率
T=1/Fs;%采样周期
L=1024;%采样数
t=(0:L-1)*T;%时间离散化
接着，对音源做了建模。先试一下100Hz吧。由波长分析可以知道，如果频率较高，不一定能在空间A里都有实解，也许会有多解，或复数解。
f=100;%信号源频率 取FFT实际可解析的值
orig=cos(2*pi*f*t);%信号源波形
%设置信号源
subplot(2,1,1)
plot(Fs*t(1:50),orig(1:50)) %只画前50毫秒就够了，太密了也没意思
title('音源波形')
xlabel('毫秒')
%测试代码，对音源做FFT变换，验证算法
NFFT = 2^nextpow2(L); %采样数L下 FFT的信号长度的最小值
YY = fft(orig,NFFT)/L;%FFT变换
ff = Fs/2*linspace(0,1,NFFT/2+1); %FFT实际可解析的频率
subplot(2,1,2)
plot(ff,2*abs(YY(1:NFFT/2+1))) %画频谱图
title('音源频谱')
xlabel('频率(Hz)')
ylabel('强度')
%相位角算法验证
DB=2*abs(YY(1:NFFT/2+1));%先把音源频率列出来
ii=find(DB==max(DB));%然后找到音源的主频在数组里的第几个
HZ=ff(ii);%找到主频
angleorig=angle(YY(ii));%最后计算相位角，理论上应该是个接近0的无穷小数。
100Hz的音源的信息如下图所示。
我们假设音源可以位于空间A的某个点上。这个点应该有实际的坐标可以定位，并且其与两个麦克风的距离也可以精确地给定。这部分是很基础的物理建模。
%物理建模。两个麦克与XY空间上各点的距离
x=linspace(-1,1,pp);%x空间离散化
y=linspace(0.1,1.0,pp);%y空间离散化
=meshgrid(x,y);%组合个坐标矩阵
R1=((X-p1).^2+Y.^2).^0.5;%与左麦克距离
R2=((X-p2).^2+Y.^2).^0.5;%与右麦克距离
然后，根据声速算时间差。这样，音源位于XY空间上任意点时候，两个麦克风得到的波形应该满足基本的物理规律，波动方程。代码里有个防止多解的修饰，这个在数学上不是必要的，因为可以有复数解。但是工程上应该对这样的点做逻辑修饰，给它个不会跑飞并且值域跨度不大的实数解。
%物理建模。音源位于XY空间上各点时，声速时间差。
dt1=R1;%音源位于XY空间上各点时，声音传播到左麦克的时间，这是个矩阵。
%介绍一个matlab定义矩阵数组并申请数组空间的小技巧，用一个维度相同的任意数组，直接等于就好了
dt2=R2;%右麦克，其他同上
for i=1:pp;
for j =1:pp;
dt1(j,i)=R1(j,i)/va;
dt2(j,i)=R2(j,i)/va;
%防止多解的修饰
if dt1(j,i)>=1/f
dt1(j,i)=0.999*1/f;
end
if dt2(j,i)>=1/f
dt2(j,i)=0.999*1/f;
end
end
end
推导出麦克风接收到的波形以后，物理建模部分结束。下面代码只演示地描述了音源在(j,i)点时候左麦克风的波形，完整的代码最后会给大家。
%左麦克
sig1=cos(2*pi*f*(t-dt1(j,i)));%物理建模
接下来就要跑定位算法了。
先要对这个收到的波形进行相位计算。当然是要用经典的FFT变换
%先物理建模得到波形。然后模拟定位算法，用FFT计算相位。
angle1=R1;%音源位于XY空间上各点时，左麦克得到的相位角，这是个矩阵。
%介绍一个matlab定义矩阵数组并申请数组空间的小技巧，用一个维度相同的任意数组，直接等于就好了
ff1=R1;%音源位于XY空间上各点时，左麦克得到的FFT变换结果，这是个复数矩阵。
angle2=R2;%右麦克，其他同上
ff2=R2;%右麦克，其他同上
%下面这个循环用于计算音源位于XY空间上各点时，两个麦克的相位角
for i=1:pp;
for j =1:pp;
%左麦克
sig1=cos(2*pi*f*(t-dt1(j,i)));%物理建模
YY = fft(sig1,NFFT)/L;%FFT变换
DB=2*abs(YY(1:NFFT/2+1));
ii=find(DB==max(DB));
ff1(j,i)=ff(ii);
angle1(j,i)=angle(YY(ii));
%右麦克
sig2=cos(2*pi*f*(t-dt2(j,i)));%物理建模
YY = fft(sig2,NFFT)/L;%FFT变换
DB=2*abs(YY(1:NFFT/2+1));
ii=find(DB==max(DB));
ff2(j,i)=ff(ii);
angle2(j,i)=angle(YY(ii));
end
end
%下面把相位角可视化一下
figure;
subplot(2,1,1)
mesh(X,Y,angle1)
title('音源位于各点时左麦克测得的相位')
subplot(2,1,2)
mesh(X,Y,angle2)
title('音源位于各点时右麦克侧得相位')
当音源在XY空间的各个点上时，两个麦克风测得的相位应该是这样的
然后是距离计算，代码如下。因为前面做过修饰，很巧妙地规避了多解。
%定位算法。计算距离。
dis1=angle1;%音源与左麦克的距离，定义矩阵数组的技巧同上一段代码
dis2=angle2;%音源与右麦克的距离
for i=1:pp;
for j =1:pp;
%矩阵初值0
dis1(j,i)=0;
dis2(j,i)=0;
%左麦克
if angle1(j,i)<=0
dis1(j,i)=-1*va*angle1(j,i)/(2*pi*f);
end
if angle1(j,i) >0
dis1(j,i)=-1*va*(angle1(j,i)-2*pi)/(2*pi*f);
end
%右麦克
if angle2(j,i)<=0
dis2(j,i)=-1*va*angle2(j,i)/(2*pi*f);
end
if angle2(j,i) >0
dis2(j,i)=-1*va*(angle2(j,i)-2*pi)/(2*pi*f);
end
end
end
%把定位算法得到的距离可视化一下
figure;
subplot(2,1,1)
mesh(X,Y,dis1)
title('音源距离左麦克')
subplot(2,1,2)
mesh(X,Y,dis2)
title('音源距离右麦克')
当音源在XY空间的各个点上时，两个麦克风测得的音源距离是下图这样的。它们有细微的差别，这个差别是多麦克风定位的基础，这有点像双目视觉。
接着，以左右麦克风为圆心，左右麦克风测得的音源距离分别为两个圆的半径，画两个圆。应该会有两个交点。选y处于正解区的那个解，虽然数学上可以有两个解，但是工程上的计算一定要做逻辑判断滤掉无效解。
最后，这个交点的XY坐标就是我们的声波相位雷达对音源的空间定位坐标。
代码如下。千万小心那个负数开根号，分分种进入复数空间的节奏，当然我已经在算法上修饰了。
%定位算法。两圆相交求xy坐标。
X1=angle1;
Y1=angle1;
for i=1:pp;
for j =1:pp;
X1(j,i)=0;
Y1(j,i)=0;
X1(j,i)=-(- p1^2 + p2^2 + dis1(j, i)^2 - dis2(j, i)^2)/(2*(p1 - p2));
if (p1 - p2 + dis1(j, i) - dis2(j, i))*(p1 - p2 - dis1(j, i) + dis2(j, i))>0.000
Y1(j,i)=((p1 - p2 + dis1(j, i) + dis2(j, i))*(p2 - p1 + dis1(j, i) + dis2(j, i))*(p1 - p2 + dis1(j, i) - dis2(j, i))*(p1 - p2 - dis1(j, i) + dis2(j, i)))^(1/2)/(2*(p1 - p2));
end
end
end
figure;
subplot(2,1,1)
mesh(X,Y,X1)
title('各点的x坐标定位')
subplot(2,1,2)
mesh(X,Y,Y1)
title('各点的y坐标定位')
解的图片如下。解算得到的X和Y坐标值分两个图画出来了。很完美，解全在实数区，基本上线性连续平滑且单调，看上去是个正确的答案。
解有多精确，用眼睛看或是用语言描述是没有说服力的。算法要定量分析，要用实实在在的数据做证明。
我们得承认解算得到的这个坐标值和一开始物理建模得到的精确坐标值是不一样的，有一定的误差。
下面我们做一个误差估计。代码如下。
%误差估计
ERR1=X1-X;
ERR2=Y1-Y;
ERR=(ERR1.^2+ERR2.^2).^0.5
figure;
mesh(X,Y,ERR)
title('各点的定位误差')
误差在XY空间上的分布画出来了。
我们可以发现，这个算法在较远的位置和靠近两个麦克风中轴线附近有非常好的精度。在偏离中轴线并且靠近麦克风阵列时误差较大，但依然在可以忍受的范围（竖坐标的数量级是10的负12）
现在可以说“完美”了:lol
我们假设音源可以位于空间A的某个点上。这个点应该有实际的坐标可以定位，并且其与两个麦克风的距离也可以精确地给定。这部分是很基础的物理建模。
然后是距离计算，代码如下。因为前面做过修饰，很巧妙地规避了多解。
接着，以左右麦克风为圆心，左右麦克风测得的音源距离分别为两个圆的半径，画两个圆。应该会有两个交点。选y处于正解区的那个解，虽然数学上可以有两个解，但是工程上的计算一定要做逻辑判断滤掉无效解。
最后，这个交点的XY坐标就是我们的声波相位雷达对音源的空间定位坐标。
代码如下。千万小心那个负数开根号，分分种进入复数空间的节奏，当然我已经在算法上修饰了。
解有多精确，用眼睛看或是用语言描述是没有说服力的。算法要定量分析，要用实实在在的数据做证明。
我们得承认解算得到的这个坐标值和一开始物理建模得到的精确坐标值是不一样的，有一定的误差。
下面我们做一个误差估计。代码如下。
我们可以发现，这个算法在较远的位置和靠近两个麦克风中轴线附近有非常好的精度。在偏离中轴线并且靠近麦克风阵列时误差较大，但依然在可以忍受的范围（竖坐标的数量级是10的负12）
现在可以说“完美”了:lol
suoma
2017-08-06
xukejing
回复
2017-08-08
