2025 电赛 E 题视觉追踪系统开源实现

二、串口通信模块（舵机控制）

def servo_control(servo_id, angle, interval=50):
    """ 多圈角度控制
    功能：控制舵机旋转到指定角度
    输入：servo_id(0 上/1 下), angle(度), interval(ms)
    """
    angle_raw = int(angle * 10)  # 角度值放大 10 倍
    # 构建数据包
    packet = bytearray([0x12, 0x4C, 0x0D, 0x0B, servo_id])
    packet.extend(struct.pack('<i', angle_raw))
    packet.extend(struct.pack('<I', interval))
    packet.extend(struct.pack('<H', 0))  # power=0
    packet.append(sum(packet) % 256)  # 校验和
    uart1.write(bytes(packet))  # 发送指令

def servo_read(servo_id):
    """ 读取多圈角度
    功能：读取舵机当前角度和圈数
    输入：servo_id(0 上/1 下)
    """
    packet = bytearray([0x12, 0x4C, 0x10, 0x01, servo_id])
    packet.append(sum(packet) % 256)  # 校验和
    uart1.write(bytes(packet))  # 发送指令

def parse_response(data):
    """ 解析角度响应
    功能：解析舵机返回的角度数据
    输入：data 字节数据
    输出：(servo_id, angle, turns)或 None
    """
    if len(data) >= 12 and data[0] == 0x05 and data[2] == 0x10:
        servo_id = data[4]
        angle = struct.unpack('<i', data[5:9])[0] / 10.0
        turns = struct.unpack('<H', data[9:11])[0]
        return (servo_id, angle, turns)
    return None

def receive():
    """ 接收舵机响应数据
    功能：处理舵机返回的数据
    """
    data = uart1.read(128)  # 读取串口数据
    if data:
        result = parse_response(data)
        if result:
            servo_id, angle, turns = result
            print(f"舵机{servo_id}: {angle:.1f}度，{turns}圈")
        return result
    time.sleep_ms(10)

三、追踪控制模块（PID 算法）

def pid_control(error, last_error, integral, axis='x'):
    """ PID 控制函数
    功能：计算 PID 输出值
    输入：error(当前误差), last_error(上次误差), integral(积分项), axis(控制轴)
    输出：(output, last_error, integral)
    """
    # 根据控制轴选择参数
    if axis == 'x':
        # X 轴参数配置
        if abs(error) >= 0:  # 大误差：快速响应
            Kp, Ki, Kd, sep_thresh = 0.6, 0.2, 1, 10000000
        else:  # 小误差：精确控制
            Kp, Ki, Kd, sep_thresh = 0.5, 0.3, 0.2, 5000
    else:  # Y 轴
        if abs(error) >= 0:  # 大误差
            Kp, Ki, Kd, sep_thresh = 0.6, 0.2, 2, 8000
        else:  # 小误差
            Kp, Ki, Kd, sep_thresh = 0.6, 0.15, 0.2, 60

    # PID 计算
    Proportional = Kp * error  # 比例项
    # 积分分离：大误差时不累积积分
    if abs(error) < sep_thresh:
        integral += error
    integral_term = Ki * integral  # 积分项
    derivative = Kd * (error - last_error)  # 微分项
    output = Proportional + integral_term + derivative
    last_error = error
    return output, last_error, integral

def smooth_output(new_output, last_output, max_change):
    """ 输出平滑处理函数
    功能：限制输出变化率，防止突变
    输入：new_output(新输出值), last_output(上次输出值), max_change(最大允许变化量)
    输出：平滑后的输出值
    """
    # 首次输出直接返回
    if last_output is None:
        return new_output
    change = new_output - last_output  # 限制变化量
    if abs(change) > max_change:
        if change > 0:
            smoothed_output = last_output + max_change
        else:
            smoothed_output = last_output - max_change
    else:
        smoothed_output = new_output
    return smoothed_output

四、图像处理模块

def init_sensor():
    """初始化摄像头"""
    global sensor
    print("Starting camera tracking system...")
    sensor = Sensor(width=1920, height=1080)
    sensor.reset()
    sensor.set_framesize(Sensor.QVGA)  # 320*240
    sensor.set_pixformat(Sensor.RGB565)

def init_display():
    """初始化显示屏"""
    Display.init(Display.ST7701, to_ide=True, fps=60)
    MediaManager.init()  # 初始化媒体管理器

def find_a4_paper(img):
    """ 寻找 A4 纸
    功能：检测 A4 纸的黑色边框和白色区域
    输入：img(图像对象)
    输出：(outer, inner) - 外框和内框坐标
    """
    # 1. 检测黑色边框
    black_blobs = img.find_blobs([black_threshold], area_threshold=1, merge=True, margin=1, invert=True)
    for blob in black_blobs:
        x, y, w, h = blob.rect()
        # 跳过太小的区域
        if w < 1 or h < 1:
            continue
        # 2. 在黑色边框内检测白色区域
        inner_w = int(w * 0.9)  # 假设白色区域比边框小 10%
        inner_h = int(h * 0.9)
        inner_x = x + (w - inner_w) // 2
        inner_y = y + (h - inner_h) // 2
        # 检查白色区域
        white_pixels = 0
        total_pixels = 0
        # 采样内部区域中心部分
        for i in range(inner_x + 5, inner_x + inner_w - 5, 5):
            for j in range(inner_y + 5, inner_y + inner_h - 5, 5):
                pixel = img.get_pixel(i, j)  # 亮度检测
                if pixel[0] + pixel[1] + pixel[2] > white_threshold:
                    white_pixels += 1
                    total_pixels += 1
        # 白色像素占比超过 45% 视为有效
        if total_pixels > 0 and white_pixels / total_pixels > 0.45:
            return (x, y, w, h), (inner_x, inner_y, inner_w, inner_h)
    return None, None

def img_process(img):
    """ 图像处理流程
    功能：检测 A4 纸，绘制边框和中心点
    输入：img(图像对象)
    输出：(center_x, center_y)或 (None, None)
    """
    if img:
        outer, inner = find_a4_paper(img)
        if outer:
            # 绘制黑色外边框（绿色）
            img.draw_rectangle(outer[0], outer[1], outer[2], outer[3], color=(0, 255, 0), thickness=2)
            if inner:
                # 绘制白色内区域（蓝色边框）
                img.draw_rectangle(inner[0], inner[1], inner[2], inner[3], color=(0, 0, 255), thickness=2)
            # 计算中心点
            center_x = outer[0] + outer[2] // 2
            center_y = outer[1] + outer[3] // 2
            img.draw_cross(center_x, center_y, color=(255, 0, 0), size=10)
            # 显示图像
            Display.show_image(img, x=round((640 - sensor.width()) / 2), y=round((480 - sensor.height()) / 2))
        return center_x, center_y
    else:
        print("检测到黑色边框但未确认白色内区域")
        Display.show_image(img, x=round((640 - sensor.width()) / 2), y=round((480 - sensor.height()) / 2))
        return None, None
    else:
        print("未检测到 A4 纸")
        Display.show_image(img, x=round((640 - sensor.width()) / 2), y=round((480 - sensor.height()) / 2))
        return None, None
else:
    print("图像捕获失败")
    return None, None

五、主任务模块

任务 2：固定位置追踪

def main_task2():
    """任务 2：固定位置追踪"""
    try:
        # 初始化硬件
        init_sensor()
        time.sleep_ms(200)
        init_display()
        sensor.run()
        clock = time.clock()
        # 全局变量声明
        global last_error_x, last_error_y, integral_x, integral_y
        global last_output_x, last_output_y
        # 状态变量
        valid_count_fps = 0
        valid_count_lazer = 0
        # 摆正逻辑
        servo_control(0, 73.5)  # Y 轴位置
        servo_control(1, 0)     # X 轴位置
        while True:
            clock.tick()
            img = sensor.snapshot()
            center_x, center_y = img_process(img)
            if center_x is not None:
                valid_count_fps += 1
                if valid_count_fps >= 3:  # 连续 3 帧检测到目标
                    # 计算偏差
                    error_x = target_x - center_x
                    error_y = target_y - center_y
                    # 死区处理
                    if abs(error_x) < 5:
                        error_x = 0
                    if abs(error_y) < 5:
                        error_y = 0
                    # 激光锁定条件
                    if abs(error_y) < 5 and abs(error_x) < 5:
                        valid_count_lazer += 1
                        if valid_count_lazer >= 15:  # 连续 15 帧稳定
                            lazer.value(1)  # 开启激光
                            # PID 控制
                            output_x, last_error_x, integral_x = pid_control(
                                error_x, last_error_x, integral_x, 'x')
                            output_y, last_error_y, integral_y = pid_control(
                                error_y, last_error_y, integral_y, 'y')
                            # 输出转换
                            output_x = output_x * 0.1
                            output_y = output_y * 0.1
                            # Y 轴处理
                            base_angle_y = 73
                            raw_output_y = base_angle_y + output_y
                            raw_output_y = max(20, min(120, raw_output_y))  # 限幅
                            raw_output_y = 140 - raw_output_y  # 方向转换
                            # X 轴处理
                            raw_output_x = max(-360, min(360, output_x))
                            raw_output_x = -raw_output_x
                            # 平滑处理
                            smooth_output_x = smooth_output(raw_output_x, last_output_x, max_change_x)
                            smooth_output_y = smooth_output(raw_output_y, last_output_y, max_change_y)
                            # 更新输出值
                            last_output_x = smooth_output_x
                            last_output_y = smooth_output_y
                            # 发送控制指令
                            servo_control(1, smooth_output_x)
                            servo_control(0, smooth_output_y)
                            # 调试信息
                            print("原始输出：({:.2f}, {:.2f})".format(raw_output_x, raw_output_y))
                            print("平滑输出：({:.2f}, {:.2f})".format(smooth_output_x, smooth_output_y))
                            print("积分：{} {}".format(integral_x, integral_y))
                            print("比例项：{} {}".format(Kp * error_x, Kp * error_y))
                            print("微分项：{} {}".format(Kd * error_x, Kd * error_y))
                            print(clock.fps())
                    else:
                        valid_count_fps = 0  # 重置计数器
            # 异常处理
    except KeyboardInterrupt:
        print("Program stopped by user")
    except Exception as e:
        print(f"Error occurred: {str(e)}")
    finally:
        # 清理资源
        if 'sensor' in locals() and isinstance(sensor, Sensor):
            sensor.stop()
        Display.deinit()
        os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
        time.sleep_ms(100)
        lazer.value(0)  # 关闭激光
        MediaManager.deinit()
        print("System shutdown complete")

任务 3：动态搜索追踪（右转版）

def main_task3_right():
    """任务 3：动态搜索追踪（右转版）"""
    try:
        # 初始化硬件
        init_sensor()
        time.sleep_ms(200)
        init_display()
        sensor.run()
        clock = time.clock()
        # 全局变量声明
        global last_error_x, last_error_y, integral_x, integral_y
        global last_output_x, last_output_y
        # 状态变量
        valid_count_fps = 0
        valid_count_lazer = 0
        search_error_x = -100  # 搜索方向参数（右转）
        base_angle_x = 0       # X 轴基准角度
        while True:
            clock.tick()
            img = sensor.snapshot()
            center_x, center_y = img_process(img)
            if center_x is not None:
                # 检测到目标
                valid_count_fps += 1
                if valid_count_fps >= 3:
                    # 计算偏差
                    error_x = target_x - center_x
                    error_y = target_y - center_y
                    # 死区处理
                    if abs(error_x) < 5:
                        error_x = 0
                    if abs(error_y) < 5:
                        error_y = 0
                    # 激光锁定条件
                    if abs(error_y) < 5 and abs(error_x) < 5:
                        valid_count_lazer += 1
                        if valid_count_lazer >= 15:
                            lazer.value(1)
                            # PID 控制
                            output_x, last_error_x, integral_x = pid_control(
                                error_x, last_error_x, integral_x, 'x')
                            output_y, last_error_y, integral_y = pid_control(
                                error_y, last_error_y, integral_y, 'y')
                            # 输出转换
                            output_x = output_x * 0.1
                            output_y = output_y * 0.1
                            # Y 轴处理
                            base_angle_y = 70
                            raw_output_y = base_angle_y + output_y
                            raw_output_y = max(20, min(120, raw_output_y))
                            raw_output_y = 140 - raw_output_y
                            # X 轴处理
                            raw_output_x = base_angle_x + output_x
                            raw_output_x = max(-360, min(360, raw_output_x))
                            raw_output_x = -raw_output_x
                            # 平滑处理
                            smooth_output_x = smooth_output(raw_output_x, last_output_x, max_change_x)
                            smooth_output_y = smooth_output(raw_output_y, last_output_y, max_change_y)
                            # 更新输出值
                            last_output_x = smooth_output_x
                            last_output_y = smooth_output_y
                            # 发送控制指令
                            servo_control(1, smooth_output_x)
                            servo_control(0, smooth_output_y)
                            # 调试信息
                            print("原始输出：({:.2f}, {:.2f})".format(raw_output_x, raw_output_y))
                            print("平滑输出：({:.2f}, {:.2f})".format(smooth_output_x, smooth_output_y))
                            print("积分：{} {}".format(integral_x, integral_y))
                            print("比例项：{} {}".format(Kp * error_x, Kp * error_y))
                            print("微分项：{} {}".format(Kd * error_x, Kd * error_y))
                            print(clock.fps())
                    else:
                        # 未检测到目标，进入搜索模式
                        valid_count_fps = 0
                        valid_count_lazer = 0
                        # 固定偏差搜索
                        output_x, last_error_x, integral_x = pid_control(
                            search_error_x, last_error_x, integral_x, 'x')
                        # 输出转换
                        output_x = output_x * 0.1
                        # X 轴处理
                        raw_output_x = base_angle_x + output_x
                        raw_output_x = max(-360, min(360, raw_output_x))
                        raw_output_x = -raw_output_x
                        # Y 轴保持固定
                        raw_output_y = 70
                        raw_output_y = max(20, min(120, raw_output_y))
                        raw_output_y = 140 - raw_output_y
                        # 平滑处理
                        smooth_output_x = smooth_output(raw_output_x, last_output_x, max_change_x)
                        smooth_output_y = smooth_output(raw_output_y, last_output_y, max_change_y)
                        # 更新输出值
                        last_output_x = smooth_output_x
                        last_output_y = smooth_output_y
                        # 发送控制指令
                        servo_control(1, smooth_output_x)
                        servo_control(0, smooth_output_y)
                        print(f"搜索模式 - X 轴积分：{integral_x:.2f}, 输出：{smooth_output_x:.2f}")
            # 异常处理
    except KeyboardInterrupt:
        print("Program stopped by user")
    except Exception as e:
        print(f"Error occurred: {str(e)}")
    finally:
        # 清理资源
        if 'sensor' in locals() and isinstance(sensor, Sensor):
            sensor.stop()
        Display.deinit()
        os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
        time.sleep_ms(100)
        lazer.value(0)
        MediaManager.deinit()
        print("System shutdown complete")

六、程序入口

# 程序入口
if __name__ == "__main__":
    while True:
        print("程序开始，等待按钮")
        if key0.value() == 0:  # 检测按键按下
            time.sleep_ms(20)
            if key0.value() == 0:  # 确认按键按下
                print("已检测到按下按钮")
                lazer.value(1)  # 开启激光
                main_task2()  # 启动任务 2
            else:
                time.sleep_ms(100)  # 等待按键

七、系统特点总结

1. 自适应 PID 控制：
   • 根据误差大小动态调整参数
   • 大误差区快速响应，小误差区精细控制
   • 积分分离防止过冲
2. 智能目标搜索：
   • 目标丢失自动进入搜索模式
   • 可配置左右搜索方向
   • 搜索速度可调
3. 运动平滑处理：
   • 输出变化率限制
   • 防止舵机抖动
   • 提升系统稳定性
4. 双重目标验证：
   • 黑色边框检测
   • 内部白色区域验证
   • 提高识别准确率