基于K210的声源定位系统

2022年全国大学生电子设计竞赛——声源定位跟踪系统(E题)

设计一种能够检测声音源的偏航角、俯仰角及距离来定位和指示声源的装置。水平方向上采用K210结合麦克风阵列采集不同方向麦克风的强度,从而计算偏航角和距离;从而控制舵机,将激光笔指到声源位置。

所需硬件

maixbit(k210芯片)、麦克风阵列、舵机、lcd显示屏

核心代码:

卡尔曼滤波:
def Kalman_Filter(value):  # 传angle
    global KF_lastP        #上次的协方差
    global KF_nowP         #本次的协方差
    global KF_x_hat        #卡尔曼滤波的计算值,即为后验最优值
    global KF_Kg           #卡尔曼增益系数
    global KF_Q            #过程噪声
    global KF_R            #测量噪声
    output=0        #output为卡尔曼滤波计算值
    x_t=KF_x_hat    #当前先验预测值 = 上一次最优值
    KF_nowP=KF_lastP+KF_Q       #本次的协方差矩阵
    KF_Kg=KF_nowP/(KF_nowP+KF_R)#卡尔曼增益系数计算
    output=x_t+KF_Kg*(value-x_t)#当前最优值
    KF_x_hat=output     #更新最优值
    KF_lastP=(1-KF_Kg)*KF_nowP#更新协方差矩阵
    return output
获取麦克风阵列的坐标位置、声强、夹角等
def get_mic_dir():
    AngleX=0
    AngleY=0
    AngleR=0
    Angle=0
    AngleAddPi=0
    Angle_Z=0
    AngleR_Z=0
    mic_list=[]
    img = mic.get_map()
    imga=img# 获取声音源分布图像
    b = mic.get_dir(imga)   # 计算、获取声源方向
    for i in range(len(b)):
        if b[i]>=2:
            AngleX+= b[i]*math.sin(i*math.pi/6)
            AngleY+= b[i]*math.cos(i*math.pi/6)
    AngleX=round(AngleX,6) #计算坐标转换值
    AngleY=round(AngleY,6)
    if AngleY<0:AngleAddPi=180
    if AngleX<0 and AngleY > 0:AngleAddPi=360
    if AngleX!=0 or AngleY!=0: #参数修正
        if AngleY==0:
            Angle=90 if AngleX>0 else 270 #填补X轴角度
        else:
            Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
        AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
        AngleR_Z=int(AngleR)
        Angle_Z=int(Angle)
        mic_list.append(AngleX)
        mic_list.append(AngleY)
        mic_list.append(AngleR)
        mic_list.append(Angle)

完整代码和效果见:
https://m.tb.cn/h.UdDKJML?tk=stEl2zanG9O CZ0001
点击链接直接打开

你可能感兴趣的:(K210,嵌入式C语言学习,单片机,python)