声源定位算法的简单实现

项目的选择

这个项目是我在学习单片机这门课程的时候一边上课一边做的，选择该项目的原因是这个项目是去年电赛的其中一道题，我对此也很感兴趣，想。因为是去年的赛题，所以已经有人实现了，但是大多数都是固定了几个角度，而且使用的硬件都是集成度很高的麦克风阵列模块，我就想挑战一下能不能做一个使用麦克风数量较少而且能够实现180度追踪的声源定位系统。我在网上看了很多相关的视频，也在网上收集了许多相关资料，浏览了一些论文，初步验证了我想法的可行性。

实现过程

我的初步方案是从模拟信号入手，使用LM386运放芯片实现了对声音信号的放大以及简单的滤波，然后进行到算法的时候以为只要把傅里叶变换学习掌握就可以了，但事实是傅里叶变换算法牵扯到了更多的数学知识，其中就包括欧拉公式，并且傅里叶变换也只是对声音信号处理的第一步，后续还需要配合其他更高阶的算法使用。这时已经用了课程时间的一半了，考虑到学习的周期很长以及目前自身能力水平不足，所以就采取第二种方案：数字信号，由于声音到达每个麦克风的时间不一样，可以采取使用单片机来捕捉声音信号的上升沿或者下降沿，再配合定时器来记录声音到达每个麦克风的时刻，配合我们在论文中找到的算法来计算声音的角度，以下是我最后采取的方案的思路：

方案需要的单片机对3通道声源的信号进行采集，所以需要使用单片机3个外部中断，其次要求单片机具有定时器产生PWM波形以驱动舵机。

现在较为常用的单片机为Intel 8031指令系统内核的89C51或89C52单片机，由于其价格低，推出时间较长，所以广泛应用在民用，工业，商业等，在众多单片机家族中，51单片机是应用最为广泛的一款单片机。由于51只有两个外部中断（INT0,INT1），不能满足该方案的要求，所以选择了支持更多外部中断的STM32F407。

STM32F4系列是意法半导体（(STMicroelectronics)）专为要求高性能、低成本、低功耗的嵌入式应用专门设计的ARM Cortex-M4内核的单片机，其中STM32F407系列单片机IO口有从36脚到142脚具有多种选择，而单片机的管脚完全兼容，这十分利于方案的扩展，而且单片机拥有多达16个外部中断，定时器的计数器能实现微秒级以及高达32位的计数，同时32位单片机更擅长浮点型数据的运算，综上所述，STM32为这个方案提供了极大的便利，所以我自主学习并采用了这款单片机。

本方案采用的拾音器为三个麦克风模块，本来想自己制作的，整理好电路原理图后发现之前买的模块刚好有我想用的功能。该模块集成了放大电路、滤波电路以及比较器，可以输出模拟信号和数字信号，这个方案采用的是下降沿中断，故使用的是数字信号。当麦克风接收到声音时可以产生低电平，单片机捕捉下降沿产生中断。

STM32F4的GPIO口工作模式，速率等均可以使用GPIO配置寄存器进行设置，其共有8种模式：

本方案使用三个中断,一个脉冲输出，一个参考定时器其中中断使用PA0,PA1,PA2三个GPIO,脉冲输出使用的TIM3通道1即PB6，再使用TIM2产生微秒级的参考时间。

定时器配置微秒级计数：uint32_t TIM_Prescaler = 84 - 1;uint32_t TIM_Period = 0xFFFFFFFF;TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;TIM_TimeBaseStructure.TIM_Prescaler = TIM_Prescaler;TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM_TimeBaseStructure.TIM_Period = TIM_Period;TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);中断函数：//中断0处理函数void EXTI0_IRQHandler(){ if(flag[0] == 0){time.time0 = TIM2->CNT;flag[0] = 1;}EXTI_ClearITPendingBit(EXTI_Line0);}//中断1处理函数void EXTI1_IRQHandler(){if(flag[1] == 0){time.time1 = TIM2->CNT;flag[1] = 1;}EXTI_ClearITPendingBit(EXTI_Line1);}//中断2处理函数void EXTI2_IRQHandler(){if(flag[2] == 0){time.time2 = TIM2->CNT;flag[2] = 1;}EXTI_ClearITPendingBit(EXTI_Line2);}

产生中断之后读取当前TIM2计数寄存器的值，并存储在结构体变量中，三个中断分别代表三个麦克风。由于一个声音可能产生产生多个脉冲信号，需要使用if语句上锁，防止多次读取时间，并且使用延时屏蔽掉多余的脉冲信号：

if(flag[0] == 1 && flag[1] == 1 && flag[2] == 1){angle = Calculate(time);sprintf(str,"angle=%f\n", angle);printf(str);delay(1);flag[0] = 0;flag[1] = 0;flag[2] = 0;}

当三个麦克风都接收到声音信号后进行运算：

使用读取到的三个时刻值两两相减，就可以得到声音到达个麦克风的时间差T1、T2，使用时间差转换单位后乘上声音在空气中的传播速度V=340m/s就可以得到距离差。

结合以下算法：（参考自全文阅读--XML全文阅读--中国知网 ()）

以中间的麦克风为原点，我采用麦克风之间的间隔为D=0.15m，可以计算出声源与中间麦克风的距离R,以及在平面上x，y的值，再结合arccos或arcsin可以进一步计算出声源的角度，进而驱动舵机指向声源。

计算函数：

uint32_t calculate_pulse_width(float angle){return (uint32_t)((PULSE_MAX - PULSE_MIN) * angle / (ANGLE_MAX - ANGLE_MIN) + PULSE_MIN);}//声音先到达中间double Scheme1(struct time_def time){char str[40];double T1 = (double)(time.time0 - time.time1)/1000000;double T2 = (double)(time.time2 - time.time1)/1000000;double R, x, y;R = (double)(2* pow(D,2)-pow(V,2)*(pow(T1,2)+pow(T2,2))) / (2*V*(T1+T2));x = (pow((R+V*T2),2)-pow((R+V*T1),2)) / (4*D);y = sqrt((pow(R,2)-pow(x,2)));OLED_Init();OLED_Clear();sprintf(str,"R=%fm",fabs(R));OLED_ShowString(4,0,str,12);sprintf(str,"X=%fm",x);OLED_ShowString(4,11,str,12);sprintf(str,"Y=%fm",y);OLED_ShowString(4,23,str,12);OLED_Refresh_Gram();return acos(x/R);}//声音先到达左边double Scheme2(struct time_def time){char str[40];double T1 = (double)(time.time0 - time.time1)/1000000;double T2 = (double)(time.time1 - time.time2)/1000000;double R, x, y;R = (double)(2*pow(D,2)-pow(V,2)*(pow(T1,2)+pow(T2,2))) / (2*V*fabs((double)(T2-T1)));x = (double)(pow((R+V*T2),2)-pow((R-V*T1),2)) / (4*D);y = sqrt(fabs((pow(R,2)-pow(x,2))));OLED_Init();OLED_Clear();sprintf(str,"R=%fm",fabs(R));OLED_ShowString(4,0,str,12);sprintf(str,"X=%fm",x);OLED_ShowString(4,11,str,12);sprintf(str,"Y=%fm",y);OLED_ShowString(4,23,str,12);OLED_Refresh_Gram();return acos(x/R);}//声音先到达右边double Scheme3(struct time_def time){char str[40];double T1 = (double)(time.time1 - time.time0)/1000000;double T2 = (double)(time.time2 - time.time1)/1000000;double R,x,y;R = (double)(2*pow(D,2)-pow(V,2)*(pow(T1,2)+pow(T2,2))) / (2*V*fabs((double)(T2-T1)));x = (double)(pow((R-V*T2),2)-pow((R+V*T1),2)) / (4*D);y = sqrt((pow(R,2)-pow(x,2)));OLED_Init();OLED_Clear();sprintf(str,"R=%fm",fabs(R));OLED_ShowString(4,0,str,12);sprintf(str,"X=%fm",x);OLED_ShowString(4,11,str,12);sprintf(str,"Y=%fm",y);OLED_ShowString(4,23,str,12);OLED_Refresh_Gram();return acos(x/R);}float Calculate(struct time_def time){float angle = 0;if(time.time1 <= time.time0 && time.time1 <= time.time2)//L0<L1,L0<L2{angle = (float)Scheme1(time);}else if(time.time1 > time.time2 && time.time0 > time.time1)//L2<L1<L0{angle = (float)Scheme2(time);}else if(time.time0 < time.time1 && time.time1 < time.time2)//L0<L1<L2{angle = (float)Scheme3(time);}return angle;}

目前实现的功能以及扩展：

由于得到的数据会受到声音振幅和环境噪声的影响，数据波动比较大：计算声音到达中间麦克风的距离误差在十厘米左右，当结合acos或者asin进行运算时会产生很大的误差，使计算出来的角度不够精确，所以目前只实现了对三个角度声音的定位，并且保留了计算结果，拓展OLED屏幕显示声音到中间麦克风的距离R还有大概的方位x，y，以便进行参考。

后续可以优化算法，由于不同响度的声音产生的信号振幅（电压差）不一样，可以利用A/D转换，间接获取声音的响度，再对获取到的时间点进行相应的补偿，可以得到更精确的数据。也可以增加多个麦克风来实现更多角度更精确的定位。