Sipeed麦克风阵列获取声源角度

官方文档：MicArray 麦克风阵列 - Sipeed Wiki

在官方文档中，MicroPython 例程代码如下：

from Maix import MIC_ARRAY as mic
import lcd

lcd.init()
mic.init() #默认配置
#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)#可自定义配置 IO

while True:
    imga = mic.get_map()    # 获取声音源分布图像
    b = mic.get_dir(imga)   # 计算、获取声源方向
    a = mic.set_led(b,(0,0,255)) # 配置 RGB LED 颜色值
    imgb = imga.resize(160,160)
    imgc = imgb.to_rainbow(1) # 将图像转换为彩虹图像
    a = lcd.display(imgc)
mic.deinit()

上述代码能够实现检测声源位置，输出声源彩虹图像，并点亮麦克风阵列上相应的 LED 灯

但是，代码中并没有明确输出声源的位置信息，这就导致了我们无法直接获取到声源的角度、距离等信息，从而无法实现某些功能

通过观察与调试，我们不难发现，mic.get_dir(imga) 获取到的数据是一组元组数据，其包括十二个元素。当麦克风阵列接收到声音时，其在控制台的输出近似如下的数据：

(0, 0, 0, 5, 8, 6, 0, 0, 0, 0, 0, 0)

我们可以初步判断，这组元组数据表示的应该是 12 个 LED 方向上的声音强度

聪明的你马上就想到了，我们可以根据这组数据获取声源角度

我们知道，麦克风阵列上有 12 个 LED ，如果以丝印正位方向为正位，那么它们代表的角度分别是 0° 30° 60° 90° 120° 150° 180° 210° 240° 270° 300° 330°。据此，我们便可以通过计算十二个方向的矢量和来估算声源的角度：

import math

def get_sound_angle(energy_data):
    if not energy_data or sum(energy_data) == 0:
        return None  # 返回None表示无效数据

    # 矢量求和法
    x_sum = 0.0
    y_sum = 0.0

    for i in range(12):
        angle_rad = math.radians(i * 30)
        # 对能量值进行平方增强主声源权重
        weighted_energy = energy_data[i] ** 2
        x_sum += weighted_energy * math.cos(angle_rad)
        y_sum += weighted_energy * math.sin(angle_rad)

    # 计算角度
    angle = math.degrees(math.atan2(y_sum, x_sum))

    # 转换为-180~180范围
    if angle > 180:
        angle -= 360
    elif angle < -180:
        angle += 360

    return int(angle)

将例程中的 b = mic.get_dir(imga) 作为参数传递给该函数，便可以得到声源角度了。

我的任务是使用 K210 Maix Dock 搭配 Sipeed 麦克风阵列获取声源角度，控制舵机将激光照射在声源处。以下是我的程序源码：

from Maix import MIC_ARRAY as mic
from machine import Timer, PWM
import math
import time

# 全局变量
tim = None
pwm = None
angle_buffer = []  # 角度数据缓冲区
FILTER_WINDOW = 15  # 滤波窗口大小
SERVO_SMOOTH_FACTOR = 0.2  # 舵机运动平滑因子

def init():
    global pwm, tim
    tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
    pwm = PWM(tim, freq=50, duty=7.5, pin=0)  # 初始中间位置
    mic.init(i2s_d0=23, i2s_d1=22, i2s_d2=21, i2s_d3=20,
             i2s_ws=19, i2s_sclk=18, sk9822_clk=25, sk9822_dat=24)

def get_sound_angle(energy_data):
    if not energy_data or sum(energy_data) == 0:
        return None  # 返回None表示无效数据

    # 矢量求和法
    x_sum = 0.0
    y_sum = 0.0

    for i in range(12):
        angle_rad = math.radians(i * 30)
        # 对能量值进行平方增强主声源权重
        weighted_energy = energy_data[i] ** 2
        x_sum += weighted_energy * math.cos(angle_rad)
        y_sum += weighted_energy * math.sin(angle_rad)

    # 计算角度
    angle = math.degrees(math.atan2(y_sum, x_sum))

    # 转换为-180~180范围
    if angle > 180:
        angle -= 360
    elif angle < -180:
        angle += 360

    return int(angle)

def apply_filters(raw_angle):
    global angle_buffer

    # 1. 无效数据跳过
    if raw_angle is None:
        return None

    # 2. 更新数据缓冲区
    angle_buffer.append(raw_angle)
    if len(angle_buffer) > FILTER_WINDOW:
        angle_buffer.pop(0)

    # 3. 中值滤波
    sorted_angles = sorted(angle_buffer)
    median_angle = sorted_angles[len(sorted_angles) // 2]

    # 4. 移动平均滤波
    avg_angle = sum(angle_buffer) / len(angle_buffer)

    # 5. 加权组合滤波结果
    filtered_angle = 0.6 * median_angle + 0.4 * avg_angle

    # 6. 角度连续性处理
    if filtered_angle > 180:
        filtered_angle -= 360
    elif filtered_angle < -180:
        filtered_angle += 360

    return int(filtered_angle)

def convert_angle(angle):
    """将声源角度映射到舵机控制信号
    参数: angle - 声源角度(-180~180度)
    返回: (有效角度, duty_val)
    """
    # 将角度转换为0~360范围用于计算
    angle_360 = angle % 360
    if angle_360 < 0:
        angle_360 += 360

    # 检查角度是否在有效范围内 (270°~90° 的半圆)
    if 90 < angle_360 < 270:
        return False, None  # 无效角度

    # 映射角度到舵机控制信号:
    # - 90° -> duty 2.5 (右边)
    # - 0° -> duty 7.5 (正前方)
    # - -90°/270° -> duty 12.5 (左边)

    # 线性映射到duty值
    # 角度范围: -90°到 90°映射到 duty 12.5 到 2.5
    duty_val = 7.5 - (angle / 18.0)

    # 确保duty值在有效范围内
    duty_val = max(2.5, min(12.5, duty_val))
    return True, duty_val

def main():
    global angle_buffer
    last_valid_duty = 7.5   # 最后有效的duty值(初始为中间位置)

    try:
        # 初始化角度缓冲区
        print("初始化角度缓冲区...")
        for i in range(FILTER_WINDOW):
            imga = mic.get_map()
            energy_data = mic.get_dir(imga)
            raw_angle = get_sound_angle(energy_data)
            angle_buffer.append(raw_angle if raw_angle is not None else 0)
            print("填充缓冲区 %d/%d" % (i+1, FILTER_WINDOW))
            time.sleep_ms(50)

        print("开始主循环...")
        while True:
            start_time = time.ticks_ms()

            # 获取原始数据
            imga = mic.get_map()
            energy_data = mic.get_dir(imga)
            mic.set_led(energy_data, (0, 0, 255))

            # 计算原始角度
            raw_angle = get_sound_angle(energy_data)

            # 应用多级滤波
            filtered_angle = apply_filters(raw_angle)

            # 如果本次数据无效，使用上次滤波结果
            if filtered_angle is None:
                filtered_angle = angle_buffer[-1] if angle_buffer else 0

            raw_str = str(raw_angle) if raw_angle is not None else "--"
            info = "原始角度: %s°  ->  滤波角度: %d°" % (raw_str, filtered_angle)
            info += "\n能量数据: " + str(energy_data)
            print(info)
            print('-' * 30)

            # 转换角度到舵机控制信号
            is_valid, duty_val = convert_angle(filtered_angle)

            if is_valid:
                # 有效角度，使用平滑处理
                target_duty = duty_val
                # 平滑处理：混合上次的duty值和本次的目标值
                smoothed_duty = last_valid_duty * (1 - SERVO_SMOOTH_FACTOR) + target_duty * SERVO_SMOOTH_FACTOR
                last_valid_duty = smoothed_duty
                duty_val = smoothed_duty
                print("有效角度，duty_val:", duty_val)
            else:
                # 无效角度范围，保持上次的有效duty值
                duty_val = last_valid_duty
                print("角度 %d° 在无效范围，保持上次duty值: %.2f" % (filtered_angle, duty_val))

            pwm.duty(duty_val)

            loop_time = time.ticks_diff(time.ticks_ms(), start_time)
            delay_time = max(10, 100 - loop_time)
            time.sleep_ms(delay_time)

    except KeyboardInterrupt:
        print('用户退出')
    finally:
        mic.deinit()
        pwm.duty(7.5)  # 回到中间位置
        print("资源已释放")

if __name__ == '__main__':
    init()
    main()