前话
- 由于我们学校的校电赛选题是2022电赛题目,很多组都在卷小车,所以我们选了这道比较简单的题
- 本文章大部分思路都是参考大佬的文章,非常感谢!
方案选择
- 32主控+麦克风模块
- K210+官配麦克风阵列模块
- K210+自制麦克风模块
方案一:用32开发虽然自定义功能强大,但是比较繁琐,花费时间较长
方案二:由于官配麦克风模块是圆形的,用来做这题总感觉不太合适,效果应该没有直线排布的麦克风模块好,参考文章用的就是这个
方案三:自己根据K210的麦克风模块接口来做一个直线排布的麦克风阵列模块,感觉很刑!
官配麦克风模块
我们的麦克风模块
硬件设计
硬件设计都是闯哥做的,就根据官配麦克风模块设计,我也不太懂,就贴个图吧
编程思路
- 由K210自带的麦克风定位函数可以得到12个声源强度值
- 将前6个强度值取相反数,后6个值不变,然后12个值相加得出总声强值
- 将200ms(不能太长,因为要有实时性)内的总声强值累加后进行卡尔曼滤波,减少噪声干扰
- 测得两个声强边界值Vmin和Vmax(这一步尽量测准),计算出θmin和θmax(分别大约为-30度和30度)
- θ<0时,当前角度 θ = 当前总声强值 * ( θmin / 声强最小值);θ>0时,当前角度 θ = 当前总声强值 * ( θmax / 声强最大值)
- 随后便根据θ得出位置xy、距离γ
- 最后加上舵机和激光笔就可以指示跟踪啦
贴上代码和概念图~
概念图(懒得再画一张了,能看懂就好)
下面是200ms的代码
#注意:
#PIN4/5/25-29/36-47已被系统内定,不能自定义使用
#导入模块
from Maix import MIC_ARRAY as mic
from machine import UART,Timer,PWM
from fpioa_manager import fm
import lcd
import utime
import time
from Maix import GPIO
import math
#初始化变量
b_inv = [0 for i in range(6)]
b_mid = [0 for i in range(6)]
b_and = 0
gama_string = ''
sita_string = ''
x_string = ''
y_string = ''
v_now_string = ''
b_and_string = ''
sita = 0
sita_min = -30.96 #假定最小角度约为-30度
sita_max = 30.96 #假定最大角度约为30度
v_now = 0 #当前声强值
v_max = 11 #声强最大值(左边) -7 (k) ***待测量
v_min = -11 #声强最小值(右边) -16 -24(k) ***待测量
gama = 0
x = 0
y = 0
count = 0
#卡尔曼滤波变量
KF_lastP=0.1 #上次的协方差
KF_nowP=0 #本次的协方差
KF_x_hat=0 #卡尔曼滤波的计算值,即为后验最优值
KF_Kg=0 #卡尔曼增益系数
KF_Q=0 #过程噪声
KF_R=1 #测量噪声
#映射串口引脚
fm.register(6, fm.fpioa.UART1_RX, force=True)
fm.register(7, fm.fpioa.UART1_TX, force=True)
#初始化激光传感器
fm.register(14,fm.fpioa.GPIO0)
light = GPIO(GPIO.GPIO0,GPIO.OUT)
#初始化串口
uart = UART(UART.UART1, 9600, read_buf_len=4096)
#卡尔曼滤波
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
#舵机旋转角度函数
def Servo(servo,angle):
servo.duty((angle+90)/180*10+2.5)
#主要中断函数
def tim0_interrupt(tim0):
global b_and
global v_now_string
global sita_string
global gama_string
global x_string
global b_and_string
global y_string
global sita
global sita_max
global sita_min
global v_now
global v_max
global v_min
global x
global y
global count
#count += 1
v_now += Kalman_Filter(b_and)/5 #先滤波,后取平均值
light.value(0) #激光关闭,进入计算过程
if(v_now < -0.25): #计算sita
sita = v_now * sita_min / v_min
if(sita < sita_min):
sita = sita_min
elif(v_now > 0.25):
sita = v_now * sita_max / v_max
if(sita > sita_max):
sita = sita_max
else:
sita = 0
Servo(S1,sita) #舵机转动对应角度
y = 250 #***先假定为250,实际待测量
x = y * math.tan(sita * 3.14 / 180)
gama = abs( y / math.cos(sita * 3.14 / 180) ) #距离
light.value(1) #激光打开
b_and_string = str(b_and)
x_string = str(x) #转换成字符串形式
y_string = str(y)
v_now_string = str(v_now)
sita_string = str(sita)
gama_string = str(gama)
lcd.draw_string(50,60,"b_and=",lcd.WHITE)
lcd.draw_string(110,60,b_and_string,lcd.WHITE)
lcd.draw_string(50,80,"x=",lcd.WHITE)
lcd.draw_string(110,80,x_string,lcd.WHITE)
lcd.draw_string(50,100,"y=",lcd.WHITE)
lcd.draw_string(110,100,y_string,lcd.WHITE)
lcd.draw_string(50,120,"v_now=",lcd.WHITE)
lcd.draw_string(110,120,v_now_string,lcd.WHITE) #前两个参数为x,y轴起始坐标
lcd.draw_string(50,140,"gama=",lcd.WHITE)
lcd.draw_string(110,140,gama_string,lcd.WHITE)
lcd.draw_string(50,160,"sita=",lcd.WHITE)
lcd.draw_string(110,160,sita_string,lcd.WHITE)
v_now = 0
b_and = 0
#count = 0
#初始化LCD
lcd.init()
#初始化麦克风模块,引脚自定义
mic.init(i2s_ws=9,i2s_d2=10,i2s_d0=11,sk9822_clk=12,i2s_sclk=32,i2s_d3=33,i2s_d1=34,sk9822_dat=35)
#启动舵机
tim1 = tim = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=17)
#初始化定时器0
#***由于题目中说计算时间不能超过5s,所以我感觉可以适当增加定时时间以保证数据的准确性,不过如果要做第四题就要适当缩短时间
#tim0 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=1000, callback=tim0_interrupt)
tim0 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=200, callback=tim0_interrupt)
while True:
#获取原始的声源黑白位图,尺寸 16*16
imga = mic.get_map()
#获取声源方向并设置LED显示
b = mic.get_dir(imga)
#a = mic.set_led(b,(0,0,255))
#将声源地图重置成正方形,彩虹色
#imgb = imga.resize(160,160)
#imgc = imgb.to_rainbow(1)
#显示声源图
#lcd.display(imgc)
for i in b:
if(i<6):
b_mid[i] = -b[i]
b_inv[i] = b_mid[i]
b_and += b_inv[i]
b_and += b[6]
b_and += b[7]
b_and += b[8]
b_and += b[9]
b_and += b[10]
b_and += b[11]
#uart.write(v_now_string)
mic.deinit()
下面是6s的代码
#注意:
#PIN4/5/25-29/36-47已被系统内定,不能自定义使用
#导入模块
from Maix import MIC_ARRAY as mic
from machine import UART,Timer,PWM
from fpioa_manager import fm
import lcd
import utime
import time
from Maix import GPIO
import math
#初始化变量
b_inv = [0 for i in range(6)]
b_mid = [0 for i in range(6)]
v_now_array = [0]*6 #当前声强值
b_and = 0
gama_string = ''
sita_string = ''
x_string = ''
y_string = ''
v_now = 0
v_now_string = ''
sita = 0
sita_min = -30 #假定最小角度约为-30度
sita_max = 30 #假定最大角度约为30度
v_max = 3 #声强最大值(左边) -7 (k) ***待测量
v_min = -3 #声强最小值(右边) -16 -24(k) ***待测量
gama = 0
x = 0
y = 0
count = 0
Max = 0
Min = 0
#卡尔曼滤波变量
KF_lastP=0.1 #上次的协方差
KF_nowP=0 #本次的协方差
KF_x_hat=0 #卡尔曼滤波的计算值,即为后验最优值
KF_Kg=0 #卡尔曼增益系数
KF_Q=0 #过程噪声
KF_R=0.1 #测量噪声
#映射串口引脚
fm.register(6, fm.fpioa.UART1_RX, force=True)
fm.register(7, fm.fpioa.UART1_TX, force=True)
#初始化激光传感器
fm.register(14,fm.fpioa.GPIO0)
light = GPIO(GPIO.GPIO0,GPIO.OUT)
#初始化串口
uart = UART(UART.UART1, 9600, read_buf_len=4096)
#卡尔曼滤波
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
#舵机旋转角度函数
def Servo(servo,angle):
servo.duty((angle+90)/180*10+2.5)
#主要中断函数
def tim0_interrupt(tim0):
global b_and
global v_now_string
global sita_string
global gama_string
global x_string
global y_string
global sita
global sita_max
global sita_min
global v_now
global v_now_array
global v_max
global v_min
global x
global y
global count
global Max
global Min
count += 1
v_now = Kalman_Filter(b_and)/10 #先滤波,后取平均值
v_now_array[count-1] = v_now
#去除最大值和最小值
if count == 4 :
for i in range(3):
if v_now_array[i+1] > v_now_array[Max] :
Max = i+1
if v_now_array[i+1] < v_now_array[Min] :
Min = i+1
i = 0
v_now_array[Max] = 333
v_now_array[Min] = 333
for i in range(3) :
if v_now_array[i] != 333 :
v_now += v_now_array[i]
v_now = v_now / 2
light.value(0) #激光关闭,进入计算过程
if v_now < 0.5: #计算sita
sita = v_now * sita_min / v_min
if sita < sita_min:
sita = sita_min
elif v_now > 0.5:
sita = v_now * sita_max / v_max
if sita > sita_max:
sita = sita_max
else:
sita = 0
y = 250 #***先假定为250,实际待测量
x = y * math.tan(sita * 3.14 / 180)
if x > 150:
x = 150.00000
if x < -150:
x = -150.00000
gama = abs( y / math.cos(sita * 3.14 / 180) ) #距离
light.value(1) #激光打开
x_string = str(x) #转换成字符串形式
y_string = str(y)
v_now_string = str(v_now)
sita_string = str(sita)
gama_string = str(gama)
count = 0
Servo(S1,sita) #舵机转动对应角度
v_now = 0
b_and = 0
i = 0
Max = 0
Min = 0
for i in range(5) :
v_now_array[i] = 0
#初始化LCD
lcd.init()
#初始化麦克风模块,引脚自定义
mic.init(i2s_ws=9,i2s_d2=10,i2s_d0=11,sk9822_clk=12,i2s_sclk=32,i2s_d3=33,i2s_d1=34,sk9822_dat=35)
#启动舵机
tim1 = tim = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=17)
#初始化定时器0
#***由于题目中说计算时间不能超过5s,所以我感觉可以适当增加定时时间以保证数据的准确性,不过如果要做第四题就要适当缩短时间
#tim0 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=1000, callback=tim0_interrupt)
tim0 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=500, callback=tim0_interrupt)
while True:
#获取原始的声源黑白位图,尺寸 16*16
imga = mic.get_map()
#获取声源方向并设置LED显示
b = mic.get_dir(imga)
#a = mic.set_led(b,(0,0,255))
#将声源地图重置成正方形,彩虹色
#imgb = imga.resize(160,160)
#imgc = imgb.to_rainbow(1)
#显示声源图
#lcd.display(imgc)
for i in b:
if(i<6):
b_and += -b[i]
b_and += b[6]
b_and += b[7]
b_and += b[8]
b_and += b[9]
b_and += b[10]
b_and += b[11]
lcd.draw_string(50,80,"x=",lcd.WHITE)
lcd.draw_string(110,80,x_string,lcd.WHITE)
lcd.draw_string(50,100,"y=",lcd.WHITE)
lcd.draw_string(110,100,y_string,lcd.WHITE)
lcd.draw_string(50,120,"v_now=",lcd.WHITE)
lcd.draw_string(110,120,v_now_string,lcd.WHITE) #前两个参数为x,y轴起始坐标
lcd.draw_string(50,140,"gama=",lcd.WHITE)
lcd.draw_string(110,140,gama_string,lcd.WHITE)
lcd.draw_string(50,160,"sita=",lcd.WHITE)
lcd.draw_string(110,160,sita_string,lcd.WHITE)
#uart.write(v_now_string)
mic.deinit()
- 为什么会有6s的代码呢?
- 因为当时我觉得取多点数据然后再计算会比较准确,但事实与之相反,不仅歪的离谱而且反应很慢,不是很建议用。后面换了200ms之后就好很多了
- K210能够自定义函数吗?
- 很难,我最开始就是想修改K210的内部python函数,然后源码巨难看懂,而且修改了也没搞懂怎么烧录进去,就放弃了。后面发现用原配函数也可以实现
Debug
一开机灯就乱闪
很正常,一开始看到这样我都以为是硬件问题,结果硬着头皮把声源放上去之后会比较收敛,强度值也比较好,是没问题的
板子烧了
测试的时候我的降压模块的焊点和K210接触到了,然后板子一两秒就烧了55555(血亏),然后就长记性了,以后各种焊点都要封胶,还有就是引脚千万不要和金属物体相碰
麦克风模块分布
七个要尽量对称,多出的一个放在中间,其他都是左右声道
电源在比赛前一定要充好电!!!
比赛时演示到一半突然显示屏熄了,原来是电池没电了,幸好借到了小车组的电池
声源和麦克风模块最好保持同一高度
结束语
最后首先要感谢我的两位队友闯哥和10!其实整个过程非常坎坷,先是元器件断断续续的买,然后就是调试的时候没有一次是很成功的,各种乱转,然后比赛的时候是最成功的一次(幸运女神眷顾~)。希望省赛能再接再厉!