声源定位跟踪系统（2022 年全国大学生电子设计竞赛H题）

代码链接为：声源定位

摘要

本设计是声源定位跟踪系统，可根据麦克风阵列检测的参数信息分析蜂鸣器的位置和距离。该装置由主电路模块、麦克风阵列装置和云台装置组成。主电路模块用于控制麦克风阵列装置和云台装置，麦克风阵列装置用于采集声源数据，云台装置用于转动激光笔方向从而实现声源的定位。主电路模块以K210为主控，控制麦克风阵列模块直接完成声源参数的测量。装置具有检测和分析监测功能，在检测数据之后，会对数据进行分析，从而对云台进行相应调整。云台装置有两个舵机组成，下方的舵机控制左右方向，上方的舵机控制上下方向，在主电路分析完数据之后，会通过云台调节激光笔的位置，从而实现声源定位。
关键词：K210，PID，声源定位。

1.系统方案

1.1 声源定位跟踪系统的原理和结构

在麦克风孔径不大的情况下，一般都符合远场模型，因此这里应用远场模型进行声源定位。声源位置在大约在45度角，麦克风之间的距离为0.15m，48 kHz的采样率，使用gcc-phat进行时延估计，结果如下图所示。
可以看到大体上方向还是被正确估计到，发现声源定位的一些数据可以进行VAD结果的判定。这里采用的是单源自由场模型，真实情况下需要考虑更多的问题，比如房间的混响，噪声，声源个数等问题。另外gcc-phat只能用于双麦克风阵列，如果有多个麦克风，可以使用Spatial Linear Prediction Method 方法去利用麦克风之间的冗余信息获得更为精确的定位结果。此外波束成形(Beam forming, BF)和声源定位联系比较密切，因为时延和BF所要求的导向量等价，因此也有基于BF的声源定位算法。如果使用机器学习的定位方法，前面的流程还是不变，只是最后通过最大值估计时延的这一步换成了使用机器学习模型来估计时延，即模型输入为gcc-phat，输出结果为时延。然后根据这个时延进行声源定位。

1.2.1 系统供电方案

对于供电没有要求。通过18650电池接在可以实现12V转5V的LM2956S模块上，实现LM2956S模块对主电路以及舵机驱动电路供电，且符合题意。

1.2.2. 采样模块的论证和选择

麦克风阵列采用的是矽速的模块，该模块基于MSM261S4030H0数字麦克风设计的，模块声音识别灵敏度、信噪比都比较高，在声源定位方面比较常用
相较于麦克风阵列，如果使用MAX9814麦克风模块，需要同时将多个MAX9814麦克风模块拼成一个阵列，才能使用，且占用主控的引脚较多，较为不便，而采用麦克风阵列可以降低时间成本的同时，还能减少引脚的使用，使用选择麦克风阵列更方便。

1.3设计方案

一款基于K210的声源定位跟踪系统，本设计中采用麦克风阵列来采样声源数据。本设计由主电路模块、麦克风阵列装置和云台装置组成。主电路模块用于控制麦克风阵列模块采集声源数据以及舵机的转向，云台装置用于实现舵机转向从而达到改变激光笔方向的功能。

3.程序设计

3.1程序功能描述

根据题目要求软件部分主要实现声源采集分析、控制舵机云台转动功能。
1）声源采集分析部分：通过采集麦克风阵列的特征参数，将其转换成数字量，然后进行计算，换算成坐标。
2）控制舵机云台部分：根据声源采集分析部分获得的坐标值进行转动。

3.2程序设计思路

不断采集数据并分析变量，将采集的数据不断计算，来确定声源位置，然后控制舵机云台转动激光笔实现定位。

4.测试结果分析

精度不是很高，如果需要提高精度，可能需要自制麦克风电路
代码入下：

#引入模块
import sensor, image, time, lcd, math,gc
from Maix import MIC_ARRAY as mic
from Maix import GPIO
from fpioa_manager import fm
from machine import Timer,PWM,UART
#映射引脚 复用IO口
fm.register(16, fm.fpioa.GPIO0)
fm.register(6, fm.fpioa.GPIOHS0)
fm.register(8, fm.fpioa.GPIOHS1)
fm.register(14, fm.fpioa.GPIO1)
#构建对象
Laser = GPIO(GPIO.GPIO0, GPIO.OUT)                         #激光
KEY1 = GPIO(GPIO.GPIOHS0, GPIO.IN,GPIO.PULL_UP)            #按键1，测试用，实际没用到
KEY2 = GPIO(GPIO.GPIOHS1, GPIO.IN,GPIO.PULL_UP)            #按键2，测试用，实际没用到
LED_R = GPIO(GPIO.GPIO1, GPIO.OUT, value = 1)              #LED红色，测试用，实际没用到
#定义变量
Precise_Angle = 0          #精确角度
Distance = 0               #声源距离
State = 0                  #模式标志位
Tracking_Angle = 0         #跟踪角度
Last_Location = 0          #上一个跟踪角度
Current_Location = 0       #当前的跟踪角度
E_measure = 8              #卡尔曼滤波方法中的估计误差
Flag = 0                   #首次显示标志位
Direction = 0              #跟踪方向变量
#初始化舵机PWM
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=4.17, pin=17)#duty=4.17
#初始化蜂鸣器PWM
tim1 = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PWM)
beep = PWM(tim1, freq = 3000, duty = 0, pin=15)#测试用，实际没用到
#初始化LCD
lcd.init()
lcd.clear(lcd.BLACK)
#初始化麦克风阵列
mic.init()
gc.collect()    #垃圾回收器
#快排函数，测试用，实际没用到
def quick_sort(array, start, end):
    if start >= end:
        return
    mid_data, left, right = array[start], start, end
    while left < right:
        while array[right] >= mid_data and left < right:
            right -= 1
        array[left] = array[right]
        while array[left] < mid_data and left < right:
            left += 1
        array[right] = array[left]
    array[left] = mid_data
    quick_sort(array, start, left-1)
    quick_sort(array, left+1, end)
#舵机驱动函数
def Servo(servo,angle):
    S1.duty((angle+90)/180*10+2.5)
#获取声源角度函数
def Get_SoundInform():
    X = 0
    Y = 0
    ang = 0
    AngleAdd = 0
    #处理声源信号
    imga = mic.get_map() #获取原始的声源黑白位图
    Voice = mic.get_dir(imga)#获取声音数组
    mic.set_led(Voice,(0,0,255))

    for i in range(len(Voice)):
        if Voice[i] >= 2:
            X += Voice[i] * math.cos(-1 * i * math.pi / 6)
            Y += Voice[i] * math.sin(-1 * i * math.pi / 6)

    X = round(X,2) #四舍五入取整，计算坐标转换值
    Y = round(Y,2)

    #参数修正
    if X < 0:  #修正反三角函数
        AngleAdd = 180
    if X > 0 and Y < 0:  #修正负角度
        AngleAdd = 360
    if X != 0 or Y != 0: #防止出现Y等于0的情况
        if X == 0:
            ang = 90
            if Y < 0:
                ang = 270
        else:
            ang = AngleAdd + round(math.degrees(math.atan(Y/X)),2) #计算角度
        #AngleR=round(math.sqrt(X * X + Y * Y),4) #计算强度
    return ang

#卡尔曼滤波
def Kalman_Filter():
    global Flag
    #global E
    e = 12
    E = Get_SoundInform()
    while True:
        Z = Get_SoundInform()     #采样
        K = e / (e + E_measure)   #计算Kerman增益
        E = E + K * (Z - E)       # 迭代目标值
        e = (1 - K) * e           # 更新估计误差
        print(E)
        if K <= 0.012 and Flag == 0:
            Show_Angle(E)
            Flag = 1
        if K <= 0.005:             #成功判断
            break
    return E

#显示角度
def Show_Angle(angle_show):
    #修正参数
    angle_show -= 15

    if 59 <= angle_show <= 63:
        angle_show += 3
    elif 69.5 <= angle_show <= 72.5:
        angle_show += 3
    elif 86 <= angle_show <= 90:
        angle_show += 2
    elif 116 <= angle_show <= 120:
        angle_show -= 3
    elif 98 <= angle_show <= 103:
        angle_show += 5
    elif 55 <= angle_show < 59:
        angle_show += 5

    lcd.draw_string(142, 50, "            ",lcd.WHITE, lcd.BLACK)
    lcd.draw_string(115, 80, "            ",lcd.WHITE, lcd.BLACK)

    if angle_show != 0:
        Distance = round(275 / math.sin(angle_show / 180 * math.pi),1)
        lcd.draw_string(20, 80, "Sound Angle:" + str(90 - angle_show),lcd.GREEN, lcd.BLACK)
        lcd.draw_string(20, 50, "Sound Distance:" + str(Distance),lcd.WHITE, lcd.BLACK)


#获取跟踪角度函数
def Get_TrackingInform():
    global Current_Location
    global Direction
    global E
    e = 12
    E = Get_SoundInform()
    while True:
        Z = Get_SoundInform()     #采样
        K = e / (e + E_measure)   #计算Kerman增益
        E = E + K * (Z - E)       # 迭代目标值
        e = (1 - K) * e           # 更新估计误差
        print("E:", E)
        print("K:", K)
        if K <= 0.05:             #成功判断
            break
    E -= 15
    if 59 <= E <= 63:                 #实测修正麦克风阵列参数
        E += 3.5
    elif 55 <=  E < 59:
        E += 5
    elif 88 <= E <= 90:
        E += 2
    elif 100 <= E <= 102:
        E += 5
    elif 95 <= E < 98:
        E += 3
    elif 106 <= E <= 107:
        E += 2
    elif(66 <= E <= 70):
        E += 5
    if Direction == 0:                #判断跟踪方向
        Current_Location = E
    elif Direction == 1:              #从右向左
        if E >= Last_Location:
            Current_Location = E
    elif Direction == -1:             #从左向右
        if E <= Last_Location:
            Current_Location = E

#自定义LCD初始化函数
def LCD_Init():
    lcd.draw_string(90, 20, "Welcome to use!",lcd.WHITE, lcd.BLACK)
    lcd.draw_string(20, 50, "Sound Distance:" + str(Distance),lcd.WHITE, lcd.BLACK)
    lcd.draw_string(20, 80, "Sound Angle:" + str(Precise_Angle),lcd.WHITE, lcd.BLACK)

#LCD显示循迹位置，测试用，实际没用到
def LCD_Tracking():
    global Distance
    lcd.clear(lcd.GREEN)
    if Current_Location != 0:
        Distance = round(275 / math.sin(Current_Location / 180 * math.pi),1)
    lcd.draw_string(90, 30, "Trachking Mode!",lcd.RED, lcd.GREEN)
    lcd.draw_string(20, 50, "Sound Distance:" + str(Distance),lcd.WHITE, lcd.BLACK)
    lcd.draw_string(20, 80, "Sound Angle:" + str(Current_Location),lcd.WHITE, lcd.BLACK)#str(90-Current_Location)

#蜂鸣器响函数，测试用，实际没用到
def Beep_ON():  #蜂鸣器响一秒，测试用，实际没用到
    beep.duty(80)
    time.sleep_ms(10)
    beep.duty(0)
#配置按键外部中断，测试用，实际没用到
def Key1_Num(KEY1):     #KEY1中断函数，测试用，实际没用到
    global State
    time.sleep_ms(10)
    if KEY1.value() == 0:
        State = 1
def Key2_Num(KEY2):     #KEY2中断函数，测试用，实际没用到
    global State
    time.sleep_ms(10)
    if KEY2.value() == 0:
        State = 2

KEY1.irq(Key1_Num, GPIO.IRQ_FALLING)
KEY2.irq(Key2_Num, GPIO.IRQ_FALLING)

LCD_Init()

#主程序
while True:
    lcd.clear(lcd.GREEN)#绿色清屏
    lcd.draw_string(90, 30, "Trachking Mode!",lcd.RED, lcd.BLACK)#模式
    lcd.draw_string(90, 10, "XueHao:21113017",lcd.RED, lcd.BLACK)#学号
    Laser.value(1)  #开启激光
    while True:#主函数
        Get_TrackingInform() #获取初始位置
        #Kalman_Filter()#卡尔曼滤波，没有用到
        Last_Location = Current_Location#位置转换
        if(Last_Location <= 90):
            Direction = 1
        else:
            Direction = -1
        Distance = round(130 / math.sin(Current_Location / 180 * math.pi),1)#距离
        lcd.draw_string(20, 80, "Sound Angle:" + str(Current_Location),lcd.GREEN, lcd.BLACK)#位置显示
        lcd.draw_string(20, 50, "Sound Distance:" + str(Distance),lcd.WHITE, lcd.BLACK)#角度显示
        Servo(S1,(E-30))#舵机转动
        gc.collect()    #垃圾回收器，清理缓存数据
        time.sleep_ms(500)#定时器延时延时
mic.deinit()#待机

遇事不决，可问春风！