2022年电赛E题声源定位跟踪系统

池子tt

已于 2022-11-09 20:12:14 修改

阅读量1.9w

点赞数 66

文章标签：单片机嵌入式硬件

于 2022-09-21 23:27:08 首次发布

本文链接：https://blog.csdn.net/qq_53357019/article/details/126982978

版权

我们组本来是奔着视觉题去的，可是到比赛的时候突然发现好像就无人机比较合适，但是我们都没玩过无人机，本想转战小车，可是材料突然发现要两辆小车，材料也不够来不及买，于是我们就选了声源这道题，但是从来没有接触到这方面，最离谱的是刚开始我意会错题目，还想上摄像头和激光雷达哈哈哈哈。

之后我们就选用了k210的麦克风阵列来玩玩，还好之前有接触过，本想自己组装奈何实力不够，只能用现成的模块。

第一天我们试了网上的开源代码，发现这个阵列模块太容易受干扰了，于是我们组想先在k210阵列收音滤波后再转stm32控制舵机。代码如下，这个代码也可以直接k210接舵机使用，但是我们发现效果不佳。

from Maix import MIC_ARRAY as mic
from Maix import GPIO
import lcd,time,image
import math
import utime
from Maix import GPIO
from board import board_info
from fpioa_manager import fm
from machine import Timer,PWM
import time
#导入 FFT 模块

#输入时域数据（例如音频数据）并进行 FFT 运算
#res = FFT.run(data, points, shift)
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！这些字一定要读！！！！！！！！！！
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！
##########################################################################
##角度只取了阵列上字母s方向的五个RGB灯珠，大概60°，覆盖赛道，所以你的声源超过这个范围系统是不会反应的#############
##############距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）##################################
#########舵机口17，舵机我用的是270°舵机#######红外是1口，用HY.value(0)控制引脚高低电平#################################################
################你如果是180°舵机，需要把109行左右的Servo(S1,-Angle*0.666)中的0.666删掉#########################################
mic.init()#默认配置
lcd.init()   #320*240
#mic.init(i2s_d0=34, i2s_d1=8, i2s_d2=33, i2s_d3=9, i2s_ws=32, i2s_sclk=10,\
            #sk9822_dat=7, sk9822_clk=35)#可自定义配置 IO
io_led_red = 1
fm.register(io_led_red, fm.fpioa.GPIO0)#配置红外的GPIO
HY=GPIO(GPIO.GPIO0, GPIO.OUT)

fm.register(0, fm.fpioa.GPIO1)#配置两个开关的GPIO   是0和2脚 01是第二题 10第三题 11第四题
KG0=GPIO(GPIO.GPIO1, GPIO.IN, GPIO.PULL_DOWN)
                                                 #建议拿第四题测试
fm.register(2, fm.fpioa.GPIO2)#配置两个开关的GPIO   是0和2脚 01是第二题 10第三题 11第四题
KG1=GPIO(GPIO.GPIO2, GPIO.IN, GPIO.PULL_DOWN)
#KG0.value(1)
#image.font_load(image.UTF8, 16, 16, '/sd/0xA00000_font_uincode_16_16_tblr.Dzk')
#image.font_load(image.UTF8, 16, 16, 0xA00000)   #加载字库
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=17)#舵机初始化

num=0
num2=0
Angle_LB = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
t=0
t1=0
t2=0
maxnum=0
minnum=0
jiaodu=0
Angle_last=0
b=[3,1,2,5,6]
i=100
a=[]
Biaozhi2=0    #函数2标志位
cishu=0
pid=0
err=0
JD=0
#以上变量都是给函数用的工具人，不用调用或者读取，没用

##########################卡尔曼##############################
KF_lastP=0.1  #上次的协方差
KF_nowP=0   #本次的协方差
KF_x_hat=0     #卡尔曼滤波的计算值，即为后验最优值
KF_Kg=0        #卡尔曼增益系数
KF_Q=0         #过程噪声
KF_R=0.01         #测量噪声
##################################################卡尔曼##########################
def Kalman_Filter(value):
    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

##############################函数0###############################################
def get_med(jiaodu,cs):
    global a
    global num2
    a.append(jiaodu)
    if num2==(cs-1) :
        num2=0
        for j in range(1,len(a)):
            for i in range(len(a)-1):
                if a[i] > a[i+1]:
                    a[i], a[i+1] = a[i+1],a[i]
        t2=a[int((cs-1)/2)]
        a=[]      #清空，准备下一次测量
        return t2

    else :
        num2+=1
        return 1000

##############################函数1###############################################
def get_LB(jd,cs):       #滤波，jd是传入数据，cs是滤波等级要大于2，越高越慢，返回滤波后的数据
    global Angle_LB
    global num
    global minnum
    global jiaodu
    global maxnum
    Angle_LB[num]=jd
    if Angle_LB[num]>Angle_LB[maxnum] : maxnum=num  #取最大最小值的位号
    if Angle_LB[num]<Angle_LB[minnum] : minnum=num


    if num==cs:
       Angle_LB[maxnum]=0                          #去掉最大值最小值
       Angle_LB[minnum]=0
       for i in range(cs):
           jiaodu+=Angle_LB[i]
       jiaodu/=(cs-1)
       num=0
       maxnum=0
       minnum=0
       return int(jiaodu)
    else:
        num+=1
        return 1000
##############################函数2##########舵机函数###############################
def Servo(servo,angle):
    S1.duty((angle+90)/180*10+2.5)
##############################函数3######第四题函数###############################
def get_mic_dir4():
    global Angle_LB
    global Angle_last
    global pid
    global JD
    AngleX=0
    AngleY=0
    AngleR=0
    Angle=0
    AngleAddPi=0
    Juli=0
    mic_list=[]
    imga = mic.get_map()    # 获取声音源分布图像(返回声源黑白位图)
    b = mic.get_dir(imga)   # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
    #print(b[0],b[1],b[2],b[10],b[11])

    for i in range(len(b)):
        if b[i]>=0:
            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.degr