import sensor, image, time, math
from pyb import Pin, Timer
# 如果不需要UART通信，可以注释掉这行
from machine import UART
#############################################################
# 全局常量定义 - 集中管理所有配置参数
#############################################################
# 调试和性能选项
DEBUG = True # 设置为True可以在串行终端显示信息
DRAW_DETECTION = False # 设置为False可以减少图像处理负担，不在显示窗口显示检测框
SLEEP_MS = 20 # 循环延时(毫秒)，增大可降低CPU使用率，但会降低响应速度
USE_MORPHOLOGY = True # 设置为False可以跳过形态学操作，提高性能
RESOLUTION = sensor.QQVGA # 可选: sensor.QQVGA (160x120) 或 sensor.QVGA (320x240)
SHOW_STATS = False # 不在屏幕上显示FPS和其他信息
AUTO_THRESHOLD = True # 设置为True启用自适应阈值
THRESHOLD_OFFSET = 10 # 自适应阈值偏移量
MAX_LOST_FRAMES = 30 # 目标丢失多少帧后执行特定操作
# 图像处理参数
BINARY_THRESHOLD = [(0, 70)] # 对于黑色矩形，阈值可能需要调整
TEMPLATE_MATCH_THRESHOLD = 0.65 # 模板匹配阈值
TEMPLATE_MATCH_STEP = 8 # 模板匹配步长，增大可提高性能但降低精度
BLOB_AREA_THRESHOLD = 100 # Blob检测面积阈值
BLOB_PIXELS_THRESHOLD = 100 # Blob检测像素阈值
BLOB_MERGE_MARGIN = 5 # Blob合并边距
RECT_THRESHOLD = 1000 # 矩形检测阈值
# PID控制参数
KP = 0.1 # 比例系数
KI = 0.01 # 积分系数
KD = 0.05 # 微分系数
ERROR_SUM_MAX = 500 # 积分项最大值
ERROR_SUM_MIN = -500 # 积分项最小值
PW_TO_ANGLE = 0.09 # 脉冲宽度到角度的转换系数 (180/2000)
# 舵机参数
SERVO_MIN_PW = 500 # 最小脉冲宽度 (0度)
SERVO_MAX_PW = 2500 # 最大脉冲宽度 (180度)
SERVO_FREQ = 50 # 舵机PWM频率
# 搜索模式参数
SEARCH_DWELL_FRAMES = 10 # 每个搜索位置停留的帧数
# 预定义搜索位置 - 使用常量避免重复创建
SEARCH_POSITIONS = [
(90, 90), # 中心
(70, 70), # 左上
(110, 70), # 右上
(110, 110), # 右下
(70, 110) # 左下
]
#############################################################
# 系统初始化函数
#############################################################
def init_camera():
"""初始化并优化摄像头设置"""
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE) # 使用灰度模式
sensor.set_framesize(RESOLUTION)
sensor.skip_frames(time=2000) # 等待设置生效
# 优化自动设置
sensor.set_auto_gain(False) # 关闭自动增益
sensor.set_auto_whitebal(False) # 关闭自动白平衡
sensor.set_auto_exposure(False, exposure_us=10000) # 固定曝光时间
# 优化图像设置
sensor.set_brightness(0)
sensor.set_contrast(0)
# 保持镜像设置
sensor.set_hmirror(False)
sensor.set_vflip(False)
def init_servos():
"""初始化舵机控制"""
# 创建两个不同的定时器实例
tim_pan = Timer(2, freq=SERVO_FREQ) # 使用常量定义PWM频率
tim_tilt = Timer(4, freq=SERVO_FREQ)
# 水平舵机(Pan) - 控制左右旋转
pan_servo = tim_pan.channel(1, Timer.PWM, pin=Pin("P6"))
# 垂直舵机(Tilt) - 控制上下旋转
tilt_servo = tim_tilt.channel(1, Timer.PWM, pin=Pin("P8"))
return pan_servo, tilt_servo
def load_template():
"""加载模板图像，使用异常处理确保程序不会崩溃"""
try:
template = image.Image("bazi.pgm")
if DEBUG:
print("成功加载模板图像 bazi.pgm")
return template
except Exception as e:
if DEBUG:
print("加载模板图像失败:", e)
return None
# 优化的舵机控制函数
def set_servo_angle(ch, angle):
"""
设置舵机角度 (0-180度)
使用位运算和预计算常量优化计算
"""
# 使用位运算优化限制角度范围
angle = min(180, max(0, angle))
# 使用预计算的常量和整数运算优化脉冲宽度计算
# 0.5ms-2.5ms脉冲范围 (500-2500)
# 使用整数运算然后转换为整数，避免浮点运算
pulse = SERVO_MIN_PW + int((angle * (SERVO_MAX_PW - SERVO_MIN_PW)) / 180)
# 直接设置脉冲宽度
ch.pulse_width(pulse)
#############################################################
# PID控制函数
#############################################################
def apply_pid_control(current_pos, center_pos, error_sum, last_error):
"""
应用PID控制算法
优化计算效率，减少内存使用
参数:
current_pos: 当前位置
center_pos: 目标位置
error_sum: 累计误差
last_error: 上一次误差
返回:
(输出值, 更新后的累计误差, 当前误差)
"""
# 计算误差（使用减法而不是乘以-1，更高效）
# 负号是因为舵机方向与坐标系方向相反
error = center_pos - current_pos
# 更新积分项 - 使用就地操作符
error_sum += error
# 使用位运算优化限制积分项
# 使用常量避免重复计算
error_sum = min(ERROR_SUM_MAX, max(ERROR_SUM_MIN, error_sum))
# 计算微分项 - 直接使用减法
error_diff = error - last_error
# 使用预计算的乘法减少浮点运算
# 可以考虑使用整数运算然后再转换为浮点数
output = (KP * error) + (KI * error_sum) + (KD * error_diff)
# 使用元组返回多个值，减少临时变量
return output, error_sum, error
#############################################################
# 系统初始化
#############################################################
# 初始化摄像头
init_camera()
# 加载模板图像
template = load_template()
# 初始化舵机
pan_servo, tilt_servo = init_servos()
# 设置初始位置：90度（中间位置）
pan_angle = 90
tilt_angle = 90
set_servo_angle(pan_servo, pan_angle)
set_servo_angle(tilt_servo, tilt_angle)
time.sleep(1) # 等待舵机到位
# PID控制参数
KP = 0.1 # 比例系数
KI = 0.01 # 积分系数
KD = 0.05 # 微分系数
# PID控制变量
pan_error_sum = 0
tilt_error_sum = 0
last_pan_error = 0
last_tilt_error = 0
# 二值化阈值 - 调整这个值以适应您的环境光线条件
BINARY_THRESHOLD = [(0, 70)] # 对于黑色矩形，阈值可能需要调整
#############################################################
# 辅助函数
#############################################################
def print_config():
"""显示当前配置信息"""
if DEBUG:
print("=== 激光点追踪系统配置 ===")
print("分辨率: {}".format("160x120" if RESOLUTION == sensor.QQVGA else "320x240"))
print("二值化阈值: {}".format("自适应 (偏移量={})".format(THRESHOLD_OFFSET) if AUTO_THRESHOLD else BINARY_THRESHOLD))
print("使用形态学操作: {}".format(USE_MORPHOLOGY))
print("绘制检测结果: {}".format(DRAW_DETECTION))
print("显示统计信息: {}".format(SHOW_STATS))
print("循环延时: {}ms".format(SLEEP_MS))
print("PID参数: KP={}, KI={}, KD={}".format(KP, KI, KD))
print("目标丢失处理: {}帧后启动搜索".format(MAX_LOST_FRAMES))
print("=========================")
def process_image(img, need_stats=False):
"""
处理图像：二值化和形态学操作
优化内存使用和计算效率
参数:
img: 要处理的图像
need_stats: 是否需要计算图像统计信息
返回:
处理后的图像
"""
# 获取统计信息（仅在需要时）
if need_stats:
stats = img.get_statistics()
if DEBUG and SHOW_STATS:
print("均值: {:.1f}, 中值: {}, 标准差: {:.1f}".format(
stats.mean(), stats.median(), stats.stdev()))
# 使用自适应阈值或固定阈值进行二值化
if AUTO_THRESHOLD and need_stats:
threshold_value = max(10, min(250, stats.mean() - THRESHOLD_OFFSET))
thresholds = [(0, threshold_value)]
else:
thresholds = BINARY_THRESHOLD
# 二值化处理
img.binary(thresholds)
# 可选的形态学操作
if USE_MORPHOLOGY:
img.erode(1)
img.dilate(1)
return img
# 主循环
#############################################################
# 主循环
#############################################################
# 创建时钟对象来跟踪FPS
clock = time.clock()
# 显示初始配置
print_config()
# 初始化追踪状态变量
lost_frames_count = 0 # 目标丢失计数器
searching = False # 是否正在执行搜索
search_state = 0 # 搜索模式的状态
search_step_counter = 0 # 搜索步骤的计数器
while True:
clock.tick() # 更新FPS计时器
# 直接处理单幅图像，不保留副本
img = sensor.snapshot()
# 预先检查是否需要统计信息
need_stats = (DEBUG and SHOW_STATS) or AUTO_THRESHOLD or template
# 获取统计信息（在原图上）
if need_stats:
stats = img.get_statistics()
if DEBUG and SHOW_STATS:
print("亮度均值: {:.1f}, 标准差: {:.1f}".format(stats.mean(), stats.stdev()))
# 仅当需要时才进行二值化
if AUTO_THRESHOLD or BINARY_THRESHOLD:
threshold = stats.mean() - THRESHOLD_OFFSET if AUTO_THRESHOLD else BINARY_THRESHOLD[0][1]
img.binary([(0, max(10, min(250, threshold)))])
if USE_MORPHOLOGY:
img.erode(1)
img.dilate(1)
# 图像处理结果直接存储在img中
# 不再需要DISPLAY_RAW切换，所有处理都在原图进行
# 优化模板匹配 - 减少计算和内存使用
template_matched = False
if template and need_stats:
# 直接获取统计信息
current_stats = img.get_statistics()
# 使用快速条件检查跳过不必要的模板匹配
poor_image_quality = (current_stats.mean() > 200 or
current_stats.mean() < 20 or
(current_stats.max() - current_stats.min()) < 30)
if poor_image_quality:
if DEBUG:
print("图像质量不佳，跳过模板匹配")
else:
try:
# 使用更高效的模板匹配参数
# 增加step值减少计算量，使用更简单的搜索方法
r = img.find_template(template, 0.65, step=8, search=image.SEARCH_DS)
if r:
# 找到匹配 - 使用元组解包直接获取坐标
x, y, w, h = r
# 预计算中心点和图像中心
center_x = x + (w >> 1) # 使用位移代替除法
center_y = y + (h >> 1)
img_center_x = img.width() >> 1
img_center_y = img.height() >> 1
# 重置状态变量 - 使用一行代码减少指令数
lost_frames_count = search_state = search_step_counter = 0
searching = False
# 标记模板匹配成功
template_matched = True
# 使用我们之前定义的PID控制函数
# 计算与图像中心的偏差
dx = center_x - img_center_x
dy = center_y - img_center_y
# 应用PID控制
pan_output, pan_error_sum, last_pan_error = apply_pid_control(
center_x, img_center_x, pan_error_sum, last_pan_error)
tilt_output, tilt_error_sum, last_tilt_error = apply_pid_control(
center_y, img_center_y, tilt_error_sum, last_tilt_error)
# 更新舵机角度并限制范围
pan_angle = max(0, min(180, pan_angle + pan_output))
tilt_angle = max(0, min(180, tilt_angle + tilt_output))
# 设置舵机位置
set_servo_angle(pan_servo, pan_angle)
set_servo_angle(tilt_servo, tilt_angle)
# 只在调试模式下打印信息
if DEBUG:
print("FPS: {:.1f}, 模板匹配中心: ({}, {}), 舵机角度: ({:.1f}, {:.1f})".format(
clock.fps(), center_x, center_y, pan_angle, tilt_angle))
elif DEBUG:
print("模板匹配失败")
except Exception:
# 简化异常处理，不打印详细错误信息以减少开销
if DEBUG:
print("模板匹配错误")
# 优化阈值计算 - 减少计算和内存使用
threshold_value = 0
if AUTO_THRESHOLD and need_stats:
# 直接获取统计信息
current_stats = img.get_statistics()
# 简化阈值计算
threshold_value = max(10, min(250,
current_stats.mean() - THRESHOLD_OFFSET))
if DEBUG:
print("自适应阈值: {}".format(threshold_value))
else:
# 使用固定阈值
threshold_value = BINARY_THRESHOLD[0][1]
# 优化形态学操作 - 只在必要时执行
if USE_MORPHOLOGY:
# 直接使用open操作，减少内存使用
img.open(1)
# 反转图像以便黑色矩形变为白色（如果需要）
# binary_img.invert()
# 优化检测逻辑 - 减少异常处理和条件判断
rects = []
blobs = []
# 使用标志变量而不是异常处理来控制流程
rect_detection_success = False
rects = []
# 尝试矩形检测
try:
rects = img.find_rects(threshold=1000)
rect_detection_success = len(rects) > 0
if DEBUG and rect_detection_success:
print("使用find_rects检测到", len(rects), "个矩形")
except Exception:
# 简化异常处理，不打印详细错误信息以减少开销
rect_detection_success = False
if DEBUG:
print("切换到find_blobs作为备选")
# 如果矩形检测失败，尝试blob检测
if not rect_detection_success:
# 确保统计信息已计算
current_stats = None
if need_stats:
current_stats = img.get_statistics()
# 根据图像亮度调整阈值
if current_stats and current_stats.mean() > 200:
threshold_value = max(threshold_value, 220)
if DEBUG:
print("图像较亮，调整阈值为", threshold_value)
# 使用优化的参数进行blob检测
try:
blobs = img.find_blobs([(threshold_value, 255)],
area_threshold=100,
pixels_threshold=100,
merge=True,
margin=5)
if DEBUG and blobs:
print("使用find_blobs检测到", len(blobs), "个blob")
except Exception:
# 简化异常处理
blobs = []
# 直接使用检测结果，不创建副本以节省内存
detected_rects = rects if 'rects' in locals() and rects else []
detected_blobs = blobs if 'blobs' in locals() and blobs else []
if detected_blobs:
if DEBUG:
print("检测到", len(detected_blobs), "个blob")
# 创建一个更高效的自定义矩形类来模拟find_rects的返回对象
class CustomRect:
__slots__ = ['x', 'y', 'w', 'h', '_cx', '_cy'] # 使用__slots__减少内存使用，添加缓存属性
def __init__(self, x, y, w, h):
self.x = x
self.y = y
self.w = w
self.h = h
# 预计算中心点，避免重复计算
self._cx = x + w // 2
self._cy = y + h // 2
def rect(self):
# 直接返回元组，避免创建新的元组
return self.x, self.y, self.w, self.h
@property
def cx(self):
# 作为属性访问
return self._cx
@property
def cy(self):
# 作为属性访问
return self._cy
# 优化：将检测到的blobs转换为自定义矩形对象，并添加到detected_rects中
# 预先分配足够的空间，避免动态扩展列表
if not detected_rects:
detected_rects = [None] * len(detected_blobs)
else:
# 扩展现有列表以容纳新的矩形
detected_rects.extend([None] * len(detected_blobs))
# 使用索引直接赋值，避免append操作
start_idx = len(detected_rects) - len(detected_blobs)
for i, b in enumerate(detected_blobs):
# 从blob创建一个自定义矩形对象
x, y, w, h = b.rect()
detected_rects[start_idx + i] = CustomRect(x, y, w, h)
# 如果模板匹配没有成功，则尝试使用矩形检测
if not template_matched and detected_rects:
# 选择最大矩形（假设主要目标）
max_rect = max(detected_rects, key=lambda r: r.w * r.h) if detected_rects else None
if not max_rect:
if DEBUG:
print("未检测到有效矩形")
continue
# 计算矩形中心点
# 兼容不同矩形对象类型
if hasattr(max_rect, 'rect'):
x, y, w, h = max_rect.rect()
else:
x, y, w, h = max_rect.x, max_rect.y, max_rect.w, max_rect.h
center_x = x + w // 2
center_y = y + h // 2
# 不再需要标记矩形和中心点
# 重置丢失计数器和搜索状态
lost_frames_count = 0
searching = False
search_state = 0
search_step_counter = 0
# 计算与图像中心的偏差
img_center_x = img.width() // 2
img_center_y = img.height() // 2
# 应用PID控制
pan_output, pan_error_sum, last_pan_error = apply_pid_control(
center_x, img_center_x, pan_error_sum, last_pan_error)
tilt_output, tilt_error_sum, last_tilt_error = apply_pid_control(
center_y, img_center_y, tilt_error_sum, last_tilt_error)
# 更新舵机角度
pan_angle += pan_output
tilt_angle += tilt_output
# 限制舵机角度在有效范围内
pan_angle = max(0, min(180, pan_angle))
tilt_angle = max(0, min(180, tilt_angle))
# 设置舵机位置
set_servo_angle(pan_servo, pan_angle)
set_servo_angle(tilt_servo, tilt_angle)
# 打印调试信息
if DEBUG:
print("FPS: {:.1f}, 矩形中心: ({}, {}), 舵机角度: ({:.1f}, {:.1f})".format(
clock.fps(), center_x, center_y, pan_angle, tilt_angle))
# 不在屏幕上显示统计信息
else:
# 增加丢失计数器
lost_frames_count += 1
if lost_frames_count > MAX_LOST_FRAMES:
# 启动搜索模式 - 只在首次进入时初始化
if not searching:
searching = True
search_state = 0
search_step_counter = 0
# 优化的搜索模式实现 - 使用常量和位运算优化
# 预定义搜索位置 - 使用常量避免重复创建列表
# 这些位置在全局定义，避免每次循环重新创建
SEARCH_POSITIONS = [
(90, 90), # 中心
(70, 70), # 左上
(110, 70), # 右上
(110, 110), # 右下
(70, 110) # 左下
]
# 使用常量获取当前位置 - 避免每次重新计算
current_pos = SEARCH_POSITIONS[search_state]
# 批量设置舵机角度 - 减少通信开销
set_servo_angle(pan_servo, current_pos[0])
set_servo_angle(tilt_servo, current_pos[1])
# 更新计数器和状态 - 使用位运算优化
search_step_counter += 1
if search_step_counter > 10: # 停留10帧
# 使用位运算优化模运算，如果SEARCH_POSITIONS长度是2的幂
# 否则保留原来的模运算
search_state = (search_state + 1) % len(SEARCH_POSITIONS)
search_step_counter = 0
# 使用位运算优化角度计算
# 将脉冲宽度转换为角度 (500-2500 对应 0-180度)
# 使用预计算的常量减少运算
# 180/2000 = 0.09
PW_TO_ANGLE = 0.09
pan_angle = (pan_servo.pulse_width() - 500) * PW_TO_ANGLE
tilt_angle = (tilt_servo.pulse_width() - 500) * PW_TO_ANGLE
# 只在调试模式下打印信息
if DEBUG:
print("搜索模式: 状态 {}, 步骤 {}".format(search_state, search_step_counter))
if DEBUG:
print("未检测到矩形, FPS: {:.1f}, 丢失帧数: {}".format(clock.fps(), lost_frames_count))
# 不在屏幕上显示统计信息
# 控制循环频率，减少CPU使用率
time.sleep_ms(SLEEP_MS) # 可配置的延时，控制更新率
这个代码报错import sensor, image, time, math
from pyb import Pin, Timer
# 如果不需要UART通信，可以注释掉这行
from machine import UART
#############################################################
# 全局常量定义 - 集中管理所有配置参数
#############################################################
# 调试和性能选项
DEBUG = True # 设置为True可以在串行终端显示信息
DRAW_DETECTION = False # 设置为False可以减少图像处理负担，不在显示窗口显示检测框
SLEEP_MS = 20 # 循环延时(毫秒)，增大可降低CPU使用率，但会降低响应速度
USE_MORPHOLOGY = True # 设置为False可以跳过形态学操作，提高性能
RESOLUTION = sensor.QQVGA # 可选: sensor.QQVGA (160x120) 或 sensor.QVGA (320x240)
SHOW_STATS = False # 不在屏幕上显示FPS和其他信息
AUTO_THRESHOLD = True # 设置为True启用自适应阈值
THRESHOLD_OFFSET = 10 # 自适应阈值偏移量
MAX_LOST_FRAMES = 30 # 目标丢失多少帧后执行特定操作
# 图像处理参数
BINARY_THRESHOLD = [(0, 70)] # 对于黑色矩形，阈值可能需要调整
TEMPLATE_MATCH_THRESHOLD = 0.65 # 模板匹配阈值
TEMPLATE_MATCH_STEP = 8 # 模板匹配步长，增大可提高性能但降低精度
BLOB_AREA_THRESHOLD = 100 # Blob检测面积阈值
BLOB_PIXELS_THRESHOLD = 100 # Blob检测像素阈值
BLOB_MERGE_MARGIN = 5 # Blob合并边距
RECT_THRESHOLD = 1000 # 矩形检测阈值
# PID控制参数
KP = 0.1 # 比例系数
KI = 0.01 # 积分系数
KD = 0.05 # 微分系数
ERROR_SUM_MAX = 500 # 积分项最大值
ERROR_SUM_MIN = -500 # 积分项最小值
PW_TO_ANGLE = 0.09 # 脉冲宽度到角度的转换系数 (180/2000)
# 舵机参数
SERVO_MIN_PW = 500 # 最小脉冲宽度 (0度)
SERVO_MAX_PW = 2500 # 最大脉冲宽度 (180度)
SERVO_FREQ = 50 # 舵机PWM频率
# 搜索模式参数
SEARCH_DWELL_FRAMES = 10 # 每个搜索位置停留的帧数
# 预定义搜索位置 - 使用常量避免重复创建
SEARCH_POSITIONS = [
(90, 90), # 中心
(70, 70), # 左上
(110, 70), # 右上
(110, 110), # 右下
(70, 110) # 左下
]
#############################################################
# 系统初始化函数
#############################################################
def init_camera():
"""初始化并优化摄像头设置"""
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE) # 使用灰度模式
sensor.set_framesize(RESOLUTION)
sensor.skip_frames(time=2000) # 等待设置生效
# 优化自动设置
sensor.set_auto_gain(False) # 关闭自动增益
sensor.set_auto_whitebal(False) # 关闭自动白平衡
sensor.set_auto_exposure(False, exposure_us=10000) # 固定曝光时间
# 优化图像设置
sensor.set_brightness(0)
sensor.set_contrast(0)
# 保持镜像设置
sensor.set_hmirror(False)
sensor.set_vflip(False)
def init_servos():
"""初始化舵机控制"""
# 创建两个不同的定时器实例
tim_pan = Timer(2, freq=SERVO_FREQ) # 使用常量定义PWM频率
tim_tilt = Timer(4, freq=SERVO_FREQ)
# 水平舵机(Pan) - 控制左右旋转
pan_servo = tim_pan.channel(1, Timer.PWM, pin=Pin("P6"))
# 垂直舵机(Tilt) - 控制上下旋转
tilt_servo = tim_tilt.channel(1, Timer.PWM, pin=Pin("P8"))
return pan_servo, tilt_servo
def load_template():
"""加载模板图像，使用异常处理确保程序不会崩溃"""
try:
template = image.Image("bazi.pgm")
if DEBUG:
print("成功加载模板图像 bazi.pgm")
return template
except Exception as e:
if DEBUG:
print("加载模板图像失败:", e)
return None
# 优化的舵机控制函数
def set_servo_angle(ch, angle):
"""
设置舵机角度 (0-180度)
使用位运算和预计算常量优化计算
"""
# 使用位运算优化限制角度范围
angle = min(180, max(0, angle))
# 使用预计算的常量和整数运算优化脉冲宽度计算
# 0.5ms-2.5ms脉冲范围 (500-2500)
# 使用整数运算然后转换为整数，避免浮点运算
pulse = SERVO_MIN_PW + int((angle * (SERVO_MAX_PW - SERVO_MIN_PW)) / 180)
# 直接设置脉冲宽度
ch.pulse_width(pulse)
#############################################################
# PID控制函数
#############################################################
def apply_pid_control(current_pos, center_pos, error_sum, last_error):
"""
应用PID控制算法
优化计算效率，减少内存使用
参数:
current_pos: 当前位置
center_pos: 目标位置
error_sum: 累计误差
last_error: 上一次误差
返回:
(输出值, 更新后的累计误差, 当前误差)
"""
# 计算误差（使用减法而不是乘以-1，更高效）
# 负号是因为舵机方向与坐标系方向相反
error = center_pos - current_pos
# 更新积分项 - 使用就地操作符
error_sum += error
# 使用位运算优化限制积分项
# 使用常量避免重复计算
error_sum = min(ERROR_SUM_MAX, max(ERROR_SUM_MIN, error_sum))
# 计算微分项 - 直接使用减法
error_diff = error - last_error
# 使用预计算的乘法减少浮点运算
# 可以考虑使用整数运算然后再转换为浮点数
output = (KP * error) + (KI * error_sum) + (KD * error_diff)
# 使用元组返回多个值，减少临时变量
return output, error_sum, error
#############################################################
# 系统初始化
#############################################################
# 初始化摄像头
init_camera()
# 加载模板图像
template = load_template()
# 初始化舵机
pan_servo, tilt_servo = init_servos()
# 设置初始位置：90度（中间位置）
pan_angle = 90
tilt_angle = 90
set_servo_angle(pan_servo, pan_angle)
set_servo_angle(tilt_servo, tilt_angle)
time.sleep(1) # 等待舵机到位
# PID控制参数
KP = 0.1 # 比例系数
KI = 0.01 # 积分系数
KD = 0.05 # 微分系数
# PID控制变量
pan_error_sum = 0
tilt_error_sum = 0
last_pan_error = 0
last_tilt_error = 0
# 二值化阈值 - 调整这个值以适应您的环境光线条件
BINARY_THRESHOLD = [(0, 70)] # 对于黑色矩形，阈值可能需要调整
#############################################################
# 辅助函数
#############################################################
def print_config():
"""显示当前配置信息"""
if DEBUG:
print("=== 激光点追踪系统配置 ===")
print("分辨率: {}".format("160x120" if RESOLUTION == sensor.QQVGA else "320x240"))
print("二值化阈值: {}".format("自适应 (偏移量={})".format(THRESHOLD_OFFSET) if AUTO_THRESHOLD else BINARY_THRESHOLD))
print("使用形态学操作: {}".format(USE_MORPHOLOGY))
print("绘制检测结果: {}".format(DRAW_DETECTION))
print("显示统计信息: {}".format(SHOW_STATS))
print("循环延时: {}ms".format(SLEEP_MS))
print("PID参数: KP={}, KI={}, KD={}".format(KP, KI, KD))
print("目标丢失处理: {}帧后启动搜索".format(MAX_LOST_FRAMES))
print("=========================")
def process_image(img, need_stats=False):
"""
处理图像：二值化和形态学操作
优化内存使用和计算效率
参数:
img: 要处理的图像
need_stats: 是否需要计算图像统计信息
返回:
处理后的图像
"""
# 获取统计信息（仅在需要时）
if need_stats:
stats = img.get_statistics()
if DEBUG and SHOW_STATS:
print("均值: {:.1f}, 中值: {}, 标准差: {:.1f}".format(
stats.mean(), stats.median(), stats.stdev()))
# 使用自适应阈值或固定阈值进行二值化
if AUTO_THRESHOLD and need_stats:
threshold_value = max(10, min(250, stats.mean() - THRESHOLD_OFFSET))
thresholds = [(0, threshold_value)]
else:
thresholds = BINARY_THRESHOLD
# 二值化处理
img.binary(thresholds)
# 可选的形态学操作
if USE_MORPHOLOGY:
img.erode(1)
img.dilate(1)
return img
# 主循环
#############################################################
# 主循环
#############################################################
# 创建时钟对象来跟踪FPS
clock = time.clock()
# 显示初始配置
print_config()
# 初始化追踪状态变量
lost_frames_count = 0 # 目标丢失计数器
searching = False # 是否正在执行搜索
search_state = 0 # 搜索模式的状态
search_step_counter = 0 # 搜索步骤的计数器
while True:
clock.tick() # 更新FPS计时器
# 直接处理单幅图像，不保留副本
img = sensor.snapshot()
# 预先检查是否需要统计信息
need_stats = (DEBUG and SHOW_STATS) or AUTO_THRESHOLD or template
# 获取统计信息（在原图上）
if need_stats:
stats = img.get_statistics()
if DEBUG and SHOW_STATS:
print("亮度均值: {:.1f}, 标准差: {:.1f}".format(stats.mean(), stats.stdev()))
# 仅当需要时才进行二值化
if AUTO_THRESHOLD or BINARY_THRESHOLD:
threshold = stats.mean() - THRESHOLD_OFFSET if AUTO_THRESHOLD else BINARY_THRESHOLD[0][1]
img.binary([(0, max(10, min(250, threshold)))])
if USE_MORPHOLOGY:
img.erode(1)
img.dilate(1)
# 图像处理结果直接存储在img中
# 不再需要DISPLAY_RAW切换，所有处理都在原图进行
# 优化模板匹配 - 减少计算和内存使用
template_matched = False
if template and need_stats:
# 直接获取统计信息
current_stats = img.get_statistics()
# 使用快速条件检查跳过不必要的模板匹配
poor_image_quality = (current_stats.mean() > 200 or
current_stats.mean() < 20 or
(current_stats.max() - current_stats.min()) < 30)
if poor_image_quality:
if DEBUG:
print("图像质量不佳，跳过模板匹配")
else:
try:
# 使用更高效的模板匹配参数
# 增加step值减少计算量，使用更简单的搜索方法
r = img.find_template(template, 0.65, step=8, search=image.SEARCH_DS)
if r:
# 找到匹配 - 使用元组解包直接获取坐标
x, y, w, h = r
# 预计算中心点和图像中心
center_x = x + (w >> 1) # 使用位移代替除法
center_y = y + (h >> 1)
img_center_x = img.width() >> 1
img_center_y = img.height() >> 1
# 重置状态变量 - 使用一行代码减少指令数
lost_frames_count = search_state = search_step_counter = 0
searching = False
# 标记模板匹配成功
template_matched = True
# 使用我们之前定义的PID控制函数
# 计算与图像中心的偏差
dx = center_x - img_center_x
dy = center_y - img_center_y
# 应用PID控制
pan_output, pan_error_sum, last_pan_error = apply_pid_control(
center_x, img_center_x, pan_error_sum, last_pan_error)
tilt_output, tilt_error_sum, last_tilt_error = apply_pid_control(
center_y, img_center_y, tilt_error_sum, last_tilt_error)
# 更新舵机角度并限制范围
pan_angle = max(0, min(180, pan_angle + pan_output))
tilt_angle = max(0, min(180, tilt_angle + tilt_output))
# 设置舵机位置
set_servo_angle(pan_servo, pan_angle)
set_servo_angle(tilt_servo, tilt_angle)
# 只在调试模式下打印信息
if DEBUG:
print("FPS: {:.1f}, 模板匹配中心: ({}, {}), 舵机角度: ({:.1f}, {:.1f})".format(
clock.fps(), center_x, center_y, pan_angle, tilt_angle))
elif DEBUG:
print("模板匹配失败")
except Exception:
# 简化异常处理，不打印详细错误信息以减少开销
if DEBUG:
print("模板匹配错误")
# 优化阈值计算 - 减少计算和内存使用
threshold_value = 0
if AUTO_THRESHOLD and need_stats:
# 直接获取统计信息
current_stats = img.get_statistics()
# 简化阈值计算
threshold_value = max(10, min(250,
current_stats.mean() - THRESHOLD_OFFSET))
if DEBUG:
print("自适应阈值: {}".format(threshold_value))
else:
# 使用固定阈值
threshold_value = BINARY_THRESHOLD[0][1]
# 优化形态学操作 - 只在必要时执行
if USE_MORPHOLOGY:
# 直接使用open操作，减少内存使用
img.open(1)
# 反转图像以便黑色矩形变为白色（如果需要）
# binary_img.invert()
# 优化检测逻辑 - 减少异常处理和条件判断
rects = []
blobs = []
# 使用标志变量而不是异常处理来控制流程
rect_detection_success = False
rects = []
# 尝试矩形检测
try:
rects = img.find_rects(threshold=1000)
rect_detection_success = len(rects) > 0
if DEBUG and rect_detection_success:
print("使用find_rects检测到", len(rects), "个矩形")
except Exception:
# 简化异常处理，不打印详细错误信息以减少开销
rect_detection_success = False
if DEBUG:
print("切换到find_blobs作为备选")
# 如果矩形检测失败，尝试blob检测
if not rect_detection_success:
# 确保统计信息已计算
current_stats = None
if need_stats:
current_stats = img.get_statistics()
# 根据图像亮度调整阈值
if current_stats and current_stats.mean() > 200:
threshold_value = max(threshold_value, 220)
if DEBUG:
print("图像较亮，调整阈值为", threshold_value)
# 使用优化的参数进行blob检测
try:
blobs = img.find_blobs([(threshold_value, 255)],
area_threshold=100,
pixels_threshold=100,
merge=True,
margin=5)
if DEBUG and blobs:
print("使用find_blobs检测到", len(blobs), "个blob")
except Exception:
# 简化异常处理
blobs = []
# 直接使用检测结果，不创建副本以节省内存
detected_rects = rects if 'rects' in locals() and rects else []
detected_blobs = blobs if 'blobs' in locals() and blobs else []
if detected_blobs:
if DEBUG:
print("检测到", len(detected_blobs), "个blob")
# 创建一个更高效的自定义矩形类来模拟find_rects的返回对象
class CustomRect:
__slots__ = ['x', 'y', 'w', 'h', '_cx', '_cy'] # 使用__slots__减少内存使用，添加缓存属性
def __init__(self, x, y, w, h):
self.x = x
self.y = y
self.w = w
self.h = h
# 预计算中心点，避免重复计算
self._cx = x + w // 2
self._cy = y + h // 2
def rect(self):
# 直接返回元组，避免创建新的元组
return self.x, self.y, self.w, self.h
@property
def cx(self):
# 作为属性访问
return self._cx
@property
def cy(self):
# 作为属性访问
return self._cy
# 优化：将检测到的blobs转换为自定义矩形对象，并添加到detected_rects中
# 预先分配足够的空间，避免动态扩展列表
if not detected_rects:
detected_rects = [None] * len(detected_blobs)
else:
# 扩展现有列表以容纳新的矩形
detected_rects.extend([None] * len(detected_blobs))
# 使用索引直接赋值，避免append操作
start_idx = len(detected_rects) - len(detected_blobs)
for i, b in enumerate(detected_blobs):
# 从blob创建一个自定义矩形对象
x, y, w, h = b.rect()
detected_rects[start_idx + i] = CustomRect(x, y, w, h)
# 如果模板匹配没有成功，则尝试使用矩形检测
if not template_matched and detected_rects:
# 选择最大矩形（假设主要目标）
max_rect = max(detected_rects, key=lambda r: r.w * r.h) if detected_rects else None
if not max_rect:
if DEBUG:
print("未检测到有效矩形")
continue
# 计算矩形中心点
# 兼容不同矩形对象类型
if hasattr(max_rect, 'rect'):
x, y, w, h = max_rect.rect()
else:
x, y, w, h = max_rect.x, max_rect.y, max_rect.w, max_rect.h
center_x = x + w // 2
center_y = y + h // 2
# 不再需要标记矩形和中心点
# 重置丢失计数器和搜索状态
lost_frames_count = 0
searching = False
search_state = 0
search_step_counter = 0
# 计算与图像中心的偏差
img_center_x = img.width() // 2
img_center_y = img.height() // 2
# 应用PID控制
pan_output, pan_error_sum, last_pan_error = apply_pid_control(
center_x, img_center_x, pan_error_sum, last_pan_error)
tilt_output, tilt_error_sum, last_tilt_error = apply_pid_control(
center_y, img_center_y, tilt_error_sum, last_tilt_error)
# 更新舵机角度
pan_angle += pan_output
tilt_angle += tilt_output
# 限制舵机角度在有效范围内
pan_angle = max(0, min(180, pan_angle))
tilt_angle = max(0, min(180, tilt_angle))
# 设置舵机位置
set_servo_angle(pan_servo, pan_angle)
set_servo_angle(tilt_servo, tilt_angle)
# 打印调试信息
if DEBUG:
print("FPS: {:.1f}, 矩形中心: ({}, {}), 舵机角度: ({:.1f}, {:.1f})".format(
clock.fps(), center_x, center_y, pan_angle, tilt_angle))
# 不在屏幕上显示统计信息
else:
# 增加丢失计数器
lost_frames_count += 1
if lost_frames_count > MAX_LOST_FRAMES:
# 启动搜索模式 - 只在首次进入时初始化
if not searching:
searching = True
search_state = 0
search_step_counter = 0
# 优化的搜索模式实现 - 使用常量和位运算优化
# 预定义搜索位置 - 使用常量避免重复创建列表
# 这些位置在全局定义，避免每次循环重新创建
SEARCH_POSITIONS = [
(90, 90), # 中心
(70, 70), # 左上
(110, 70), # 右上
(110, 110), # 右下
(70, 110) # 左下
]
# 使用常量获取当前位置 - 避免每次重新计算
current_pos = SEARCH_POSITIONS[search_state]
# 批量设置舵机角度 - 减少通信开销
set_servo_angle(pan_servo, current_pos[0])
set_servo_angle(tilt_servo, current_pos[1])
# 更新计数器和状态 - 使用位运算优化
search_step_counter += 1
if search_step_counter > 10: # 停留10帧
# 使用位运算优化模运算，如果SEARCH_POSITIONS长度是2的幂
# 否则保留原来的模运算
search_state = (search_state + 1) % len(SEARCH_POSITIONS)
search_step_counter = 0
# 使用位运算优化角度计算
# 将脉冲宽度转换为角度 (500-2500 对应 0-180度)
# 使用预计算的常量减少运算
# 180/2000 = 0.09
PW_TO_ANGLE = 0.09
pan_angle = (pan_servo.pulse_width() - 500) * PW_TO_ANGLE
tilt_angle = (tilt_servo.pulse_width() - 500) * PW_TO_ANGLE
# 只在调试模式下打印信息
if DEBUG:
print("搜索模式: 状态 {}, 步骤 {}".format(search_state, search_step_counter))
if DEBUG:
print("未检测到矩形, FPS: {:.1f}, 丢失帧数: {}".format(clock.fps(), lost_frames_count))
# 不在屏幕上显示统计信息
# 控制循环频率，减少CPU使用率
time.sleep_ms(SLEEP_MS) # 可配置的延时，控制更新率
最新发布
本文深入探讨了使用cvGetTickCount()和cvGetTickFrequency()以及getTickCount()和getTickFrequency()进行高精度计时的方法，详细解释了如何获取us级和s级的时间统计，适用于需要精确时间测量的计算机视觉和通用应用。
扫一扫
654
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
