我們組本來是奔著視覺題去的，可是到比賽的時(shí)候突然發(fā)現(xiàn)好像就無(wú)人機(jī)比較合適，但是我們都沒玩過無(wú)人機(jī)，本想轉(zhuǎn)戰(zhàn)小車，可是材料突然發(fā)現(xiàn)要兩輛小車，材料也不夠來不及買，于是我們就選了聲源這道題，但是從來沒有接觸到這方面，最離譜的是剛開始我意會(huì)錯(cuò)題目，還想上攝像頭和激光雷達(dá)哈哈哈哈。

?

?

之后我們就選用了k210的麥克風(fēng)陣列來玩玩，還好之前有接觸過，本想自己組裝奈何實(shí)力不夠，只能用現(xiàn)成的模塊。

第一天我們?cè)嚵司W(wǎng)上的開源代碼，發(fā)現(xiàn)這個(gè)陣列模塊太容易受干擾了，于是我們組想先在k210陣列收音濾波后再轉(zhuǎn)stm32控制舵機(jī)。代碼如下，這個(gè)代碼也可以直接k210接舵機(jī)使用，但是我們發(fā)現(xiàn)效果不佳。

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 #導(dǎo)入 FFT 模塊#輸入時(shí)域數(shù)據(jù)（例如音頻數(shù)據(jù)）并進(jìn)行 FFT 運(yùn)算 #res = FFT.run(data, points, shift) ###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！這些字一定要讀！！！！！！！！！！ ###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！ ###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！ ########################################################################## ##角度只取了陣列上字母s方向的五個(gè)RGB燈珠，大概60°，覆蓋賽道，所以你的聲源超過這個(gè)范圍系統(tǒng)是不會(huì)反應(yīng)的############# ##############距離只能在比賽規(guī)定場(chǎng)地測(cè)算，注意聲源和系統(tǒng)垂直距離是275cm（D區(qū)中間）################################## #########舵機(jī)口17，舵機(jī)我用的是270°舵機(jī)#######紅外是1口，用HY.value(0)控制引腳高低電平################################################# ################你如果是180°舵機(jī)，需要把109行左右的Servo(S1,-Angle*0.666)中的0.666刪掉######################################### mic.init()#默認(rèn)配置 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)#配置兩個(gè)開關(guān)的GPIO 是0和2腳 01是第二題 10第三題 11第四題 KG0=GPIO(GPIO.GPIO1, GPIO.IN, GPIO.PULL_DOWN)#建議拿第四題測(cè)試 fm.register(2, fm.fpioa.GPIO2)#配置兩個(gè)開關(guān)的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) #加載字庫(kù) tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM) S1 = PWM(tim, freq=50, duty=0, pin=17)#舵機(jī)初始化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 #函數(shù)2標(biāo)志位 cishu=0 pid=0 err=0 JD=0 #以上變量都是給函數(shù)用的工具人，不用調(diào)用或者讀取，沒用##########################卡爾曼############################## KF_lastP=0.1 #上次的協(xié)方差 KF_nowP=0 #本次的協(xié)方差 KF_x_hat=0 #卡爾曼濾波的計(jì)算值，即為后驗(yàn)最優(yōu)值 KF_Kg=0 #卡爾曼增益系數(shù) KF_Q=0 #過程噪聲 KF_R=0.01 #測(cè)量噪聲 ##################################################卡爾曼########################## def Kalman_Filter(value):global KF_lastP #上次的協(xié)方差global KF_nowP #本次的協(xié)方差global KF_x_hat #卡爾曼濾波的計(jì)算值，即為后驗(yàn)最優(yōu)值global KF_Kg #卡爾曼增益系數(shù)global KF_Q #過程噪聲global KF_R #測(cè)量噪聲output=0 #output為卡爾曼濾波計(jì)算值x_t=KF_x_hat #當(dāng)前先驗(yàn)預(yù)測(cè)值 = 上一次最優(yōu)值KF_nowP=KF_lastP+KF_Q #本次的協(xié)方差矩陣KF_Kg=KF_nowP/(KF_nowP+KF_R)#卡爾曼增益系數(shù)計(jì)算output=x_t+KF_Kg*(value-x_t)#當(dāng)前最優(yōu)值KF_x_hat=output #更新最優(yōu)值KF_lastP=(1-KF_Kg)*KF_nowP#更新協(xié)方差矩陣return output##############################函數(shù)0############################################### def get_med(jiaodu,cs):global aglobal num2a.append(jiaodu)if num2==(cs-1) :num2=0for 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=[] #清空，準(zhǔn)備下一次測(cè)量return t2else :num2+=1return 1000##############################函數(shù)1############################################### def get_LB(jd,cs): #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)要大于2，越高越慢，返回濾波后的數(shù)據(jù)global Angle_LBglobal numglobal minnumglobal jiaoduglobal maxnumAngle_LB[num]=jdif Angle_LB[num]>Angle_LB[maxnum] : maxnum=num #取最大最小值的位號(hào)if Angle_LB[num]<Angle_LB[minnum] : minnum=numif num==cs:Angle_LB[maxnum]=0 #去掉最大值最小值A(chǔ)ngle_LB[minnum]=0for i in range(cs):jiaodu+=Angle_LB[i]jiaodu/=(cs-1)num=0maxnum=0minnum=0return int(jiaodu)else:num+=1return 1000 ##############################函數(shù)2##########舵機(jī)函數(shù)############################### def Servo(servo,angle):S1.duty((angle+90)/180*10+2.5) ##############################函數(shù)3######第四題函數(shù)############################### def get_mic_dir4():global Angle_LBglobal Angle_lastglobal pidglobal JDAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計(jì)算、獲取聲源方向(從聲源位圖計(jì)算聲源方向 返回12個(gè)強(qiáng)度值 對(duì)應(yīng)12個(gè)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) #計(jì)算坐標(biāo)轉(zhuǎn)換值A(chǔ)ngleY=round(AngleY,6)if AngleY<0:AngleAddPi=180if AngleX<0 and AngleY > 0:AngleAddPi=360if AngleX!=0 or AngleY!=0: #參數(shù)修正if AngleY==0:Angle=90 if AngleX>0 else 270 #填補(bǔ)X軸角度else:Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #計(jì)算角度if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):if Angle>315 and Angle<360: #處理315-360的角度值A(chǔ)ngle=Angle-360Angle=Angle_last*0.1+ Angle*0.9 #低通濾波t=get_LB(Angle,3) #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)，越高越慢，返回濾波后的數(shù)據(jù)if t !=1000 :#t1=get_med(t,3) #再取中位數(shù)#if t1 !=1000 :if t>30 : t=30if t<-30 : t=-30Angle=tKalman_Filter(Angle)Angle_last=Anglelcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)#Servo(S1,-pid*0.666) #控制270°舵機(jī)，180°把0.666刪除lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右邊的區(qū)域lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*167), 15, 15, (0, 255,200))#角度位置實(shí)時(shí)JD=AngleAngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #計(jì)算強(qiáng)度mic_list.append(AngleX) #X坐標(biāo)mic_list.append(AngleY) #Y坐標(biāo)mic_list.append(AngleR) #強(qiáng)度mic_list.append(Angle) #角度#Juli=2.5/math.cos(Angle)#mic_list.append(Juli)#print(Juli)a = mic.set_led(b,(10,10,0))# 配置 RGB LED 顏色值 （從計(jì)算的聲源方向設(shè)置點(diǎn)亮對(duì)應(yīng)的LED燈）#lcd.display(0,color=(255,0,0))#img1=draw_line(100,0,100,240,color=(255,0,0))#lcd.display(img1)#lcd._line(150,0,150,240,color=(255,0,0))#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))return mic_list #返回列表，X坐標(biāo)，Y坐標(biāo)，強(qiáng)度，角度 ##############################函數(shù)3######第三題函數(shù)############################### def get_mic_dir3():global Angle_LBglobal Angle_lastglobal cishuAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計(jì)算、獲取聲源方向(從聲源位圖計(jì)算聲源方向 返回12個(gè)強(qiáng)度值 對(duì)應(yīng)12個(gè)LED燈)#print(b[0],b[1],b[2],b[10],b[11])cishu+=1 #判斷進(jìn)入函數(shù)的次數(shù)if cishu==1 :HY.value(1) # 第一次設(shè)置紅外地線高電平，關(guān)閉紅外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) #計(jì)算坐標(biāo)轉(zhuǎn)換值A(chǔ)ngleY=round(AngleY,6)if AngleY<0:AngleAddPi=180if AngleX<0 and AngleY > 0:AngleAddPi=360if AngleX!=0 or AngleY!=0: #參數(shù)修正if AngleY==0:Angle=90 if AngleX>0 else 270 #填補(bǔ)X軸角度else:Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #計(jì)算角度if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):if Angle>315 and Angle<360: #處理330-360的角度值#if Angle<340 : Angle=Angle-5Angle=Angle-360#if Angle>10 and Angle<45:#Angle=Angle+10Angle=Angle_last*0.1+ Angle*0.9 #低通濾波t=get_LB(Angle,10) #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)，越高越慢，返回濾波后的數(shù)據(jù)if t !=1000 :t1=get_med(t,3) #再取中位數(shù)if t1 !=1000 :if t1>30 : t1=30if t1<-30 : t1=-30Angle=t1Angle_last=AngleHY.value(0) # 設(shè)置紅外地線低電平，開啟紅外lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)#距離只能在比賽規(guī)定場(chǎng)地測(cè)算，注意聲源和系統(tǒng)垂直距離是275cm（D區(qū)中間）Servo(S1,-Angle*0.666) #控制270°舵機(jī)，180°把0.75刪除lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右邊的區(qū)域lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置實(shí)時(shí)AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #計(jì)算強(qiáng)度mic_list.append(AngleX) #X坐標(biāo)mic_list.append(AngleY) #Y坐標(biāo)mic_list.append(AngleR) #強(qiáng)度mic_list.append(Angle) #角度#Juli=2.5/math.cos(Angle)#mic_list.append(Juli)#print(Juli)a = mic.set_led(b,(10,10,0))# 配置 RGB LED 顏色值 （從計(jì)算的聲源方向設(shè)置點(diǎn)亮對(duì)應(yīng)的LED燈）#lcd.display(0,color=(255,0,0))#img1=draw_line(100,0,100,240,color=(255,0,0))#lcd.display(img1)#lcd._line(150,0,150,240,color=(255,0,0))#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))return mic_list #返回列表，X坐標(biāo)，Y坐標(biāo)，強(qiáng)度，角度##############################函數(shù)5############################################### def get_mic_dir2(): ######################第二題函數(shù)global Angle_LBglobal Angle_lastAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計(jì)算、獲取聲源方向(從聲源位圖計(jì)算聲源方向 返回12個(gè)強(qiáng)度值 對(duì)應(yīng)12個(gè)LED燈)#print(b[0],b[1],b[2],b[10],b[11])Servo(S1,0) #控制270°舵機(jī)保持中立HY.value(1) # 設(shè)置紅外地線高電平，關(guān)閉紅外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) #計(jì)算坐標(biāo)轉(zhuǎn)換值A(chǔ)ngleY=round(AngleY,6)if AngleY<0:AngleAddPi=180if AngleX<0 and AngleY > 0:AngleAddPi=360if AngleX!=0 or AngleY!=0: #參數(shù)修正if AngleY==0:Angle=90 if AngleX>0 else 270 #填補(bǔ)X軸角度else:Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #計(jì)算角度if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):if Angle>315 and Angle<360: #處理330-360的角度值if Angle<330 : Angle=Angle-9Angle=Angle-350#Angle=Angle_last*0.3+ Angle*0.7 #低通濾波t=get_LB(Angle,5) #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)，越高越慢，返回濾波后的數(shù)據(jù)if t !=1000 :t1=get_med(t,3) #再取中位數(shù)if t1 !=1000 :if t1>30 : t1=30if t1<-30 : t1=-30Angle=t1Angle_last=Anglelcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)#距離只能在比賽規(guī)定場(chǎng)地測(cè)算，注意聲源和系統(tǒng)垂直距離是275cm（D區(qū)中間）lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右邊的區(qū)域lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置實(shí)時(shí)#Biaozhi2=1 #等于1就不會(huì)再進(jìn)入該函數(shù)了AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #計(jì)算強(qiáng)度mic_list.append(AngleX) #X坐標(biāo)mic_list.append(AngleY) #Y坐標(biāo)mic_list.append(AngleR) #強(qiáng)度mic_list.append(Angle) #角度#Juli=2.5/math.cos(Angle)#mic_list.append(Juli)#print(Juli)a = mic.set_led(b,(10,10,0))# 配置 RGB LED 顏色值 （從計(jì)算的聲源方向設(shè)置點(diǎn)亮對(duì)應(yīng)的LED燈）#lcd.display(0,color=(255,0,0))#img1=draw_line(100,0,100,240,color=(255,0,0))#lcd.display(img1)#lcd._line(150,0,150,240,color=(255,0,0))#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))return mic_list #返回列表，X坐標(biāo)，Y坐標(biāo)，強(qiáng)度，角度 ################################下面是只運(yùn)行一次的程序############################### lcd.fill_rectangle(46,5, 230, 4, (255, 0, 0)) #上邊線 lcd.fill_rectangle(46,5, 4, 230, (255, 0, 0)) #左邊線 lcd.fill_rectangle(46,235, 230, 4, (255, 0, 0)) #下邊線 lcd.fill_rectangle(276,5, 4, 234, (255, 0, 0)) #右邊線 lcd.fill_rectangle(237,5, 4, 234, (255, 0, 0)) #右邊線2lcd.fill_rectangle(0,77, 47, 4, (255, 0, 0)) #左區(qū)域上邊線 lcd.fill_rectangle(0,154, 47, 4, (255, 0, 0)) #左區(qū)域下邊線 lcd.fill_rectangle(0,77, 4, 77, (255, 0, 0)) #左區(qū)域左邊線 #img.draw_string(20, 60, b'你好，世界', scale=1, color=(0,0,255), x_spacing=2, mono_space=1)##############################主循環(huán)，############################################### #這里因?yàn)槲业?引腳有問題，下拉不了，如果你的引腳初始電平是0，就不用(~KG1.value()+2)這個(gè)處理了 #可以直接用KG1.value()==1來判斷，如果有問題，就把這幾個(gè)if刪掉，保留第四題中的函數(shù)，程序才能跑 while True:#引腳0和2的電平是01代表第二題，且第二題只會(huì)進(jìn)行一次，返回一個(gè)值在屏幕#if (KG0.value()==0)and((~KG1.value()+2)==1) :#get_mic_dir2()##引腳0和2的電平是10代表第三題#if (KG0.value()==1)and((~KG1.value()+2)==0) :#get_mic_dir3()###引腳0和2的電平是11代表第四題#if (KG0.value()==1)and((~KG1.value()+2)==1) :get_mic_dir4()err=JD-pid #簡(jiǎn)易PID控制舵機(jī)pid+=err*0.1Servo(S1,-pid*0.666) #控制270°舵機(jī)，180°把0.666刪除#print(KG0.value(),(~KG1.value()+2))#get_mic_dir()#time.sleep_ms(1)

第二天我們組裝麥克風(fēng)陣列和云臺(tái)開始調(diào)試。代碼如下最后發(fā)現(xiàn)還是定點(diǎn)還是會(huì)有很大的誤差，時(shí)準(zhǔn)時(shí)不準(zhǔn)。代碼更改如下

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 #導(dǎo)入 FFT 模塊#輸入時(shí)域數(shù)據(jù)（例如音頻數(shù)據(jù)）并進(jìn)行 FFT 運(yùn)算 #res = FFT.run(data, points, shift) ###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！這些字一定要讀！！！！！！！！！！ ###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！ ###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！ ########################################################################## ##角度只取了陣列上字母s方向的五個(gè)RGB燈珠，大概60°，覆蓋賽道，所以你的聲源超過這個(gè)范圍系統(tǒng)是不會(huì)反應(yīng)的############# ##############距離只能在比賽規(guī)定場(chǎng)地測(cè)算，注意聲源和系統(tǒng)垂直距離是275cm（D區(qū)中間）################################## #########舵機(jī)口17，舵機(jī)我用的是270°舵機(jī)#######紅外是1口，用HY.value(0)控制引腳高低電平################################################# ################你如果是180°舵機(jī)，需要把109行左右的Servo(S1,-Angle*0.666)中的0.666刪掉######################################### mic.init(i2s_d0=23, i2s_d1=22, i2s_d2=21, i2s_d3=20, i2s_ws=19, i2s_sclk=18, sk9822_dat=24, sk9822_clk=25) 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)#配置兩個(gè)開關(guān)的GPIO 是0和2腳 01是第二題 10第三題 11第四題 KG0=GPIO(GPIO.GPIO1, GPIO.IN, GPIO.PULL_DOWN)#建議拿第四題測(cè)試 fm.register(2, fm.fpioa.GPIO2)#配置兩個(gè)開關(guān)的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) #加載字庫(kù) tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM) S1 = PWM(tim, freq=50, duty=0, pin=17)#舵機(jī)初始化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 #函數(shù)2標(biāo)志位 cishu=0 pid=0 err=0 JD=0 #以上變量都是給函數(shù)用的工具人，不用調(diào)用或者讀取，沒用##########################卡爾曼############################## KF_lastP=0.1 #上次的協(xié)方差 KF_nowP=0 #本次的協(xié)方差 KF_x_hat=0 #卡爾曼濾波的計(jì)算值，即為后驗(yàn)最優(yōu)值 KF_Kg=0 #卡爾曼增益系數(shù) KF_Q=0 #過程噪聲 KF_R=0.01 #測(cè)量噪聲 ##################################################卡爾曼########################## def Kalman_Filter(value):global KF_lastP #上次的協(xié)方差global KF_nowP #本次的協(xié)方差global KF_x_hat #卡爾曼濾波的計(jì)算值，即為后驗(yàn)最優(yōu)值global KF_Kg #卡爾曼增益系數(shù)global KF_Q #過程噪聲global KF_R #測(cè)量噪聲output=0 #output為卡爾曼濾波計(jì)算值x_t=KF_x_hat #當(dāng)前先驗(yàn)預(yù)測(cè)值 = 上一次最優(yōu)值KF_nowP=KF_lastP+KF_Q #本次的協(xié)方差矩陣KF_Kg=KF_nowP/(KF_nowP+KF_R)#卡爾曼增益系數(shù)計(jì)算output=x_t+KF_Kg*(value-x_t)#當(dāng)前最優(yōu)值KF_x_hat=output #更新最優(yōu)值KF_lastP=(1-KF_Kg)*KF_nowP#更新協(xié)方差矩陣return output##############################函數(shù)0############################################### def get_med(jiaodu,cs):global aglobal num2a.append(jiaodu)if num2==(cs-1) :num2=0for 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=[] #清空，準(zhǔn)備下一次測(cè)量return t2else :num2+=1return 1000##############################函數(shù)1############################################### def get_LB(jd,cs): #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)要大于2，越高越慢，返回濾波后的數(shù)據(jù)global Angle_LBglobal numglobal minnumglobal jiaoduglobal maxnumAngle_LB[num]=jdif Angle_LB[num]>Angle_LB[maxnum] : maxnum=num #取最大最小值的位號(hào)if Angle_LB[num]<Angle_LB[minnum] : minnum=numif num==cs:Angle_LB[maxnum]=0 #去掉最大值最小值A(chǔ)ngle_LB[minnum]=0for i in range(cs):jiaodu+=Angle_LB[i]jiaodu/=(cs-1)num=0maxnum=0minnum=0return int(jiaodu)else:num+=1return 1000 ##############################函數(shù)2##########舵機(jī)函數(shù)############################### def Servo(servo,angle):S1.duty((angle+90)/180*10+2.5) ##############################函數(shù)3######第四題函數(shù)############################### def get_mic_dir4():global Angle_LBglobal Angle_lastglobal pidglobal JDAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計(jì)算、獲取聲源方向(從聲源位圖計(jì)算聲源方向 返回12個(gè)強(qiáng)度值 對(duì)應(yīng)12個(gè)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) #計(jì)算坐標(biāo)轉(zhuǎn)換值A(chǔ)ngleY=round(AngleY,6)if AngleY<0:AngleAddPi=180if AngleX<0 and AngleY > 0:AngleAddPi=360if AngleX!=0 or AngleY!=0: #參數(shù)修正if AngleY==0:Angle=90 if AngleX>0 else 270 #填補(bǔ)X軸角度else:Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #計(jì)算角度if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):if Angle>315 and Angle<360: #處理315-360的角度值A(chǔ)ngle=Angle-360Angle=Angle_last*0.1+ Angle*0.9 #低通濾波+t=get_LB(Angle,3) #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)，越高越慢，返回濾波后的數(shù)據(jù)if t !=1000 :#t1=get_med(t,3) #再取中位數(shù)#if t1 !=1000 :if t>30 : t=30if t<-30 : t=-30Angle=tKalman_Filter(Angle)Angle_last=Anglelcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)#Servo(S1,-pid*0.666) #控制270°舵機(jī)，180°把0.666刪除lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右邊的區(qū)域lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*167), 15, 15, (0, 255,200))#角度位置實(shí)時(shí)JD=AngleAngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #計(jì)算強(qiáng)度mic_list.append(AngleX) #X坐標(biāo)mic_list.append(AngleY) #Y坐標(biāo)mic_list.append(AngleR) #強(qiáng)度mic_list.append(Angle) #角度#Juli=2.5/math.cos(Angle)#mic_list.append(Juli)#print(Juli)a = mic.set_led(b,(10,10,0))# 配置 RGB LED 顏色值 （從計(jì)算的聲源方向設(shè)置點(diǎn)亮對(duì)應(yīng)的LED燈）#lcd.display(0,color=(255,0,0))#img1=draw_line(100,0,100,240,color=(255,0,0))#lcd.display(img1)#lcd._line(150,0,150,240,color=(255,0,0))#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))return mic_list #返回列表，X坐標(biāo)，Y坐標(biāo)，強(qiáng)度，角度 ##############################函數(shù)3######第三題函數(shù)############################### def get_mic_dir3():global Angle_LBglobal Angle_lastglobal cishuAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計(jì)算、獲取聲源方向(從聲源位圖計(jì)算聲源方向 返回12個(gè)強(qiáng)度值 對(duì)應(yīng)12個(gè)LED燈)#print(b[0],b[1],b[2],b[10],b[11])cishu+=1 #判斷進(jìn)入函數(shù)的次數(shù)if cishu==1 :HY.value(1) # 第一次設(shè)置紅外地線高電平，關(guān)閉紅外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) #計(jì)算坐標(biāo)轉(zhuǎn)換值A(chǔ)ngleY=round(AngleY,6)if AngleY<0:AngleAddPi=180if AngleX<0 and AngleY > 0:AngleAddPi=360if AngleX!=0 or AngleY!=0: #參數(shù)修正if AngleY==0:Angle=90 if AngleX>0 else 270 #填補(bǔ)X軸角度else:Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #計(jì)算角度if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):if Angle>315 and Angle<360: #處理330-360的角度值#if Angle<340 : Angle=Angle-5Angle=Angle-360#if Angle>10 and Angle<45:#Angle=Angle+10Angle=Angle_last*0.1+ Angle*0.9 #低通濾波t=get_LB(Angle,10) #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)，越高越慢，返回濾波后的數(shù)據(jù)if t !=1000 :t1=get_med(t,3) #再取中位數(shù)if t1 !=1000 :if t1>30 : t1=30if t1<-30 : t1=-30Angle=t1Angle_last=AngleHY.value(0) # 設(shè)置紅外地線低電平，開啟紅外lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)#距離只能在比賽規(guī)定場(chǎng)地測(cè)算，注意聲源和系統(tǒng)垂直距離是275cm（D區(qū)中間）Servo(S1,-Angle*0.666) #控制270°舵機(jī)，180°把0.75刪除lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右邊的區(qū)域lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置實(shí)時(shí)AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #計(jì)算強(qiáng)度mic_list.append(AngleX) #X坐標(biāo)mic_list.append(AngleY) #Y坐標(biāo)mic_list.append(AngleR) #強(qiáng)度mic_list.append(Angle) #角度#Juli=2.5/math.cos(Angle)#mic_list.append(Juli)#print(Juli)a = mic.set_led(b,(10,10,0))# 配置 RGB LED 顏色值 （從計(jì)算的聲源方向設(shè)置點(diǎn)亮對(duì)應(yīng)的LED燈）#lcd.display(0,color=(255,0,0))#img1=draw_line(100,0,100,240,color=(255,0,0))#lcd.display(img1)#lcd._line(150,0,150,240,color=(255,0,0))#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))return mic_list #返回列表，X坐標(biāo)，Y坐標(biāo)，強(qiáng)度，角度##############################函數(shù)5############################################### def get_mic_dir2(): ######################第二題函數(shù)global Angle_LBglobal Angle_lastAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計(jì)算、獲取聲源方向(從聲源位圖計(jì)算聲源方向 返回12個(gè)強(qiáng)度值 對(duì)應(yīng)12個(gè)LED燈)#print(b[0],b[1],b[2],b[10],b[11])Servo(S1,0) #控制270°舵機(jī)保持中立HY.value(1) # 設(shè)置紅外地線高電平，關(guān)閉紅外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) #計(jì)算坐標(biāo)轉(zhuǎn)換值A(chǔ)ngleY=round(AngleY,6)if AngleY<0:AngleAddPi=180if AngleX<0 and AngleY > 0:AngleAddPi=360if AngleX!=0 or AngleY!=0: #參數(shù)修正if AngleY==0:Angle=90 if AngleX>0 else 270 #填補(bǔ)X軸角度else:Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #計(jì)算角度if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):if Angle>315 and Angle<360: #處理330-360的角度值if Angle<330 : Angle=Angle-9Angle=Angle-350#Angle=Angle_last*0.3+ Angle*0.7 #低通濾波t=get_LB(Angle,5) #濾波，jd是傳入數(shù)據(jù)，cs是濾波等級(jí)，越高越慢，返回濾波后的數(shù)據(jù)if t !=1000 :t1=get_med(t,3) #再取中位數(shù)if t1 !=1000 :if t1>30 : t1=30if t1<-30 : t1=-30Angle=t1Angle_last=Anglelcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)#距離只能在比賽規(guī)定場(chǎng)地測(cè)算，注意聲源和系統(tǒng)垂直距離是275cm（D區(qū)中間）lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右邊的區(qū)域lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置實(shí)時(shí)#Biaozhi2=1 #等于1就不會(huì)再進(jìn)入該函數(shù)了AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #計(jì)算強(qiáng)度mic_list.append(AngleX) #X坐標(biāo)mic_list.append(AngleY) #Y坐標(biāo)mic_list.append(AngleR) #強(qiáng)度mic_list.append(Angle) #角度#Juli=2.5/math.cos(Angle)#mic_list.append(Juli)#print(Juli)a = mic.set_led(b,(10,10,0))# 配置 RGB LED 顏色值 （從計(jì)算的聲源方向設(shè)置點(diǎn)亮對(duì)應(yīng)的LED燈）#lcd.display(0,color=(255,0,0))#img1=draw_line(100,0,100,240,color=(255,0,0))#lcd.display(img1)#lcd._line(150,0,150,240,color=(255,0,0))#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))return mic_list #返回列表，X坐標(biāo)，Y坐標(biāo)，強(qiáng)度，角度 ################################下面是只運(yùn)行一次的程序############################### lcd.fill_rectangle(46,5, 230, 4, (255, 0, 0)) #上邊線 lcd.fill_rectangle(46,5, 4, 230, (255, 0, 0)) #左邊線 lcd.fill_rectangle(46,235, 230, 4, (255, 0, 0)) #下邊線 lcd.fill_rectangle(276,5, 4, 234, (255, 0, 0)) #右邊線 lcd.fill_rectangle(237,5, 4, 234, (255, 0, 0)) #右邊線2lcd.fill_rectangle(0,77, 47, 4, (255, 0, 0)) #左區(qū)域上邊線 lcd.fill_rectangle(0,154, 47, 4, (255, 0, 0)) #左區(qū)域下邊線 lcd.fill_rectangle(0,77, 4, 77, (255, 0, 0)) #左區(qū)域左邊線 #img.draw_string(20, 60, b'你好，世界', scale=1, color=(0,0,255), x_spacing=2, mono_space=1)##############################主循環(huán)，############################################### #這里因?yàn)槲业?引腳有問題，下拉不了，如果你的引腳初始電平是0，就不用(~KG1.value()+2)這個(gè)處理了 #可以直接用KG1.value()==1來判斷，如果有問題，就把這幾個(gè)if刪掉，保留第四題中的函數(shù)，程序才能跑 while True:#引腳0和2的電平是01代表第二題，且第二題只會(huì)進(jìn)行一次，返回一個(gè)值在屏幕#if (KG0.value()==0)and((~KG1.value()+2)==1) :#get_mic_dir2()##引腳0和2的電平是10代表第三題#if (KG0.value()==1)and((~KG1.value()+2)==0) :#get_mic_dir3()###引腳0和2的電平是11代表第四題#if (KG0.value()==1)and((~KG1.value()+2)==1) :get_mic_dir4()err=JD-pid #簡(jiǎn)易PID控制舵機(jī)pid+=err*0.1Servo(S1,-pid*0.666) #控制270°舵機(jī)，180°把0.666刪除#print(KG0.value(),(~KG1.value()+2))#get_mic_dir()#time.sleep_ms(1)

第三天，選音樂，沒辦法像大佬他們一樣用特定頻率去接收音頻，接著就是調(diào)試，調(diào)試

?第四天寫論文聽天由命哈哈哈哈，之后就封箱等檢測(cè)了。需要stm32端代碼的可以私聊我。

最近有玩這聲源題的小伙伴有點(diǎn)多，在這里放百度云的鏈接，之前打電賽的代碼都放里面了，有單純k210和模塊的連接就可運(yùn)行的代碼，也有k210與stm32之間通訊控制的代碼，繼電器控制激光筆，用stm32代碼先燒錄k210對(duì)應(yīng)的代碼進(jìn)k210在接stm32。代碼因?yàn)楸荣悤r(shí)間緊寫得不好，我們是在里面寫了兩套，一套是兩個(gè)麥克風(fēng)陣列模塊，一套是我們比賽只用一個(gè)麥克風(fēng)陣列模塊，都寫在一個(gè)stm32代碼里面了，有需要的小伙伴可參考一下，代碼寫得不好的話輕噴哈哈哈。

鏈接：https://pan.baidu.com/s/1-y6ok394kM8b801JyysLbQ?
提取碼：1111?
--來自百度網(wǎng)盤超級(jí)會(huì)員V3的分享

總結(jié)

以上是生活随笔為你收集整理的2022年电赛E题声源定位跟踪系统的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。