2022电赛声源定位系统

前话

  • 由于我们学校的校电赛选题是2022电赛题目,很多组都在卷小车,所以我们选了这道比较简单的题
  • 本文章大部分思路都是参考大佬的文章,非常感谢!


方案选择

  1. 32主控+麦克风模块
  2. K210+官配麦克风阵列模块
  3. K210+自制麦克风模块

方案一:用32开发虽然自定义功能强大,但是比较繁琐,花费时间较长

方案二:由于官配麦克风模块是圆形的,用来做这题总感觉不太合适,效果应该没有直线排布的麦克风模块好,参考文章用的就是这个

方案三:自己根据K210的麦克风模块接口来做一个直线排布的麦克风阵列模块,感觉很刑!

官配麦克风模块 

我们的麦克风模块


硬件设计

硬件设计都是闯哥做的,就根据官配麦克风模块设计,我也不太懂,就贴个图吧

 


 编程思路

  1. 由K210自带的麦克风定位函数可以得到12个声源强度值
  2. 将前6个强度值取相反数,后6个值不变,然后12个值相加得出总声强值
  3. 将200ms(不能太长,因为要有实时性)内的总声强值累加后进行卡尔曼滤波,减少噪声干扰
  4. 测得两个声强边界值Vmin和Vmax(这一步尽量测准),计算出θmin和θmax(分别大约为-30度和30度)
  5. θ<0时,当前角度 θ = 当前总声强值 * ( θmin / 声强最小值);θ>0时,当前角度 θ = 当前总声强值 * ( θmax / 声强最大值)
  6. 随后便根据θ得出位置xy、距离γ
  7. 最后加上舵机和激光笔就可以指示跟踪啦

贴上代码和概念图~

概念图(懒得再画一张了,能看懂就好)

下面是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!其实整个过程非常坎坷,先是元器件断断续续的买,然后就是调试的时候没有一次是很成功的,各种乱转,然后比赛的时候是最成功的一次(幸运女神眷顾~)。希望省赛能再接再厉!

  • 8
    点赞
  • 50
    收藏
    觉得还不错? 一键收藏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值