from pyb import UART
import gc
import sensor, image, time
import math
# ==================================================
# ### 调试参数区(所有可调参数集中在此,方便修改) ###
# ==================================================
## ---------------------- 图像配置 ----------------------
RESOLUTION = sensor.QQVGA # 分辨率:sensor.QQVGA(160x120) / sensor.QVGA(320x240)
V_FLIP = True # 垂直翻转(True/False,根据安装方向调整)
H_MIRROR = True # 水平翻转(True/False,根据安装方向调整)
FILTER_TYPE = 'median' # 滤波类型:'gaussian'(高斯)/'median'(中值,抗抖动更强)
FILTER_KERNEL = 3 # 滤波核大小(奇数,3=3x3,越大去噪越强但越慢)
## ---------------------- 矩形识别 ----------------------
RECT_THRESHOLD = 12000 # 矩形检测阈值(越小越灵敏,推荐10000~20000)
RECT_GRADIENT = 10 # 边缘梯度(越小检测越灵敏,推荐8~15)
MAX_HISTORY = 3 # 矩形历史记录帧数(越多跟踪越稳,推荐2~5)
INIT_CONFIRM_THRESH = 3 # 初始识别确认帧数(连续N帧才确认,推荐2~5)
RECT_INIT_DIST = 20 # 初始识别允许的中心距离(像素,推荐15~30)
RECT_TRACK_DIST = 30 # 跟踪阶段允许的中心距离(像素,推荐25~40)
MIN_RECT_AREA = 500 # 最小矩形面积(过滤小矩形)
MAX_RECT_AREA = 3000 # 最大矩形面积(过滤大矩形)
RECT_PARALLEL_THRESH = 15 # 矩形边平行度阈值(度,用于筛选规则矩形)
## ---------------------- 双矩形特定参数 ----------------------
REQUIRE_DUAL_RECT = True # 是否需要同时检测两个矩形
RECT_DISTANCE_MIN = 50 # 两矩形最小距离(像素)
RECT_DISTANCE_MAX = 200 # 两矩形最大距离(像素)
RECT_SIZE_SIMILARITY = 0.7 # 两矩形大小相似度(0~1,1为完全相同)
## ---------------------- 激光识别 ----------------------
LASER_LAB = (91, 100, -128, 127, -128, 127) # 激光LAB阈值(用OpenMV IDE校准)
LASER_STABLE_THRESH = 2 # 激光稳定帧数(连续N帧才确认,推荐2~4)
LASER_CIRCULARITY = 0.5 # 激光圆形度阈值(越小允许形状越不规则,推荐0.5~0.7)
BLOB_PIXEL_THRESH = 5 # 激光色块像素阈值(过滤小噪点,推荐3~10)
BLOB_AREA_THRESH = 5 # 激光色块面积阈值(过滤小区域,推荐3~10)
## ---------------------- 通信配置 ----------------------
UART_PORT_NUM = 3 # 串口号(UART3)
BAUD_RATE = 115200 # 波特率(需与STM32一致)
PROTOCOL_HEADER = "DATA" # 串口协议头(方便STM32解析)
# ==================================================
# ### 初始化(引用调试参数区变量) ###
# ==================================================
# 摄像头初始化
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(RESOLUTION)
sensor.skip_frames(time=2000)
sensor.set_vflip(V_FLIP)
sensor.set_hmirror(H_MIRROR)
sensor.set_auto_gain(False) # 关闭自动增益,避免激光亮度变化
sensor.set_auto_whitebal(False) # 关闭自动白平衡
# 串口初始化
uart = UART(UART_PORT_NUM, BAUD_RATE)
uart.init(BAUD_RATE, bits=8, parity=None, stop=1, timeout_char=1000)
# 全局变量(基于调试参数初始化)
rects_history = [] # 矩形历史记录 [[rect1_corners, rect2_corners], ...]
last_laser_x, last_laser_y = 0, 0 # 激光历史坐标
stable_count = 0 # 激光稳定计数器
screen_center = (sensor.width()//2, sensor.height()//2) # 屏幕中心
# ==================================================
# ### 辅助函数 ###
# ==================================================
def get_rect_center(corners):
"""计算矩形中心坐标"""
cx = sum(p[0] for p in corners) // 4
cy = sum(p[1] for p in corners) // 4
return (cx, cy)
def rect_distance(corners1, corners2):
"""计算两个矩形的中心距离"""
cx1, cy1 = get_rect_center(corners1)
cx2, cy2 = get_rect_center(corners2)
return math.hypot(cx1 - cx2, cy1 - cy2)
def rect_area(corners):
"""计算矩形面积(基于边界框)"""
x_coords = [p[0] for p in corners]
y_coords = [p[1] for p in corners]
return (max(x_coords) - min(x_coords)) * (max(y_coords) - min(y_coords))
def is_rect_parallel(corners):
"""判断矩形是否近似直角(边是否平行)"""
edges = []
for i in range(4):
x1, y1 = corners[i]
x2, y2 = corners[(i+1)%4]
dx = x2 - x1
dy = y2 - y1
angle = math.degrees(math.atan2(dy, dx)) % 180
edges.append(angle)
# 检查对边是否平行(角度差在阈值内)
parallel1 = abs(edges[0] - edges[2]) < RECT_PARALLEL_THRESH
parallel2 = abs(edges[1] - edges[3]) < RECT_PARALLEL_THRESH
# 检查邻边是否垂直(角度差约90度)
perpendicular1 = abs(abs(edges[0] - edges[1]) - 90) < RECT_PARALLEL_THRESH
perpendicular2 = abs(abs(edges[1] - edges[2]) - 90) < RECT_PARALLEL_THRESH
return parallel1 and parallel2 and perpendicular1 and perpendicular2
def are_rects_similar(rect1, rect2):
"""判断两个矩形大小是否相似"""
area1 = rect_area(rect1)
area2 = rect_area(rect2)
if area1 == 0 or area2 == 0:
return False
# 计算面积比(取较小值除以较大值)
ratio = min(area1, area2) / max(area1, area2)
return ratio >= RECT_SIZE_SIMILARITY
def filter_valid_rects(rects):
"""筛选符合条件的矩形"""
valid_rects = []
for rect in rects:
corners = rect.corners()
area = rect_area(corners)
# 面积筛选
if not (MIN_RECT_AREA <= area <= MAX_RECT_AREA):
continue
# 形状规则性筛选
if not is_rect_parallel(corners):
continue
valid_rects.append(corners)
return valid_rects
def find_best_dual_rects(valid_rects):
"""从有效矩形中找到最佳的一对矩形"""
if len(valid_rects) < 2:
return None
best_pair = None
best_score = 0
# 检查所有可能的矩形对
for i in range(len(valid_rects)):
for j in range(i+1, len(valid_rects)):
rect1 = valid_rects[i]
rect2 = valid_rects[j]
dist = rect_distance(rect1, rect2)
# 距离筛选
if not (RECT_DISTANCE_MIN <= dist <= RECT_DISTANCE_MAX):
continue
# 大小相似度筛选
if not are_rects_similar(rect1, rect2):
continue
# 计算匹配分数(距离适中+大小相似)
score = (1 - abs(dist - (RECT_DISTANCE_MIN+RECT_DISTANCE_MAX)/2) /
(RECT_DISTANCE_MAX-RECT_DISTANCE_MIN)) * 0.5
score += are_rects_similar(rect1, rect2) * 0.5
if score > best_score:
best_score = score
best_pair = (rect1, rect2)
return best_pair
# ==================================================
# ### 主逻辑 ###
# ==================================================
clock = time.clock()
while True:
clock.tick()
img = sensor.snapshot()
# ---------- 1. 图像预处理(滤波) ----------
if FILTER_TYPE == 'gaussian':
img = img.gaussian(FILTER_KERNEL)
elif FILTER_TYPE == 'median':
img = img.median(FILTER_KERNEL)
# ---------- 2. 矩形识别与跟踪 ----------
detected_rects = None # 存储最终确认的矩形(单矩形或双矩形)
# 检测所有矩形
all_rects = img.find_rects(threshold=RECT_THRESHOLD,
x_gradient=RECT_GRADIENT,
y_gradient=RECT_GRADIENT)
# 筛选有效矩形
valid_rects = filter_valid_rects(all_rects)
# 处理矩形(单/双矩形模式)
if REQUIRE_DUAL_RECT:
# 寻找最佳的两个矩形
current_pair = find_best_dual_rects(valid_rects)
if current_pair:
# 初始识别验证
if not rects_history:
rects_history.append(current_pair)
init_confirm_count = 1
else:
# 计算与历史矩形对的平均距离
prev_pair = rects_history[-1]
dist1 = rect_distance(current_pair[0], prev_pair[0])
dist2 = rect_distance(current_pair[1], prev_pair[1])
avg_dist = (dist1 + dist2) / 2
if avg_dist < RECT_INIT_DIST:
init_confirm_count += 1
rects_history.append(current_pair)
if len(rects_history) > MAX_HISTORY:
rects_history.pop(0)
# 满足确认阈值
if init_confirm_count >= INIT_CONFIRM_THRESH:
detected_rects = current_pair
else:
init_confirm_count = 1
rects_history = [current_pair]
else:
# 单矩形模式
if valid_rects:
current_rect = valid_rects[0]
# 初始识别验证
if not rects_history:
rects_history.append([current_rect])
init_confirm_count = 1
else:
prev_rect = rects_history[-1][0]
if rect_distance(current_rect, prev_rect) < RECT_INIT_DIST:
init_confirm_count += 1
rects_history.append([current_rect])
if len(rects_history) > MAX_HISTORY:
rects_history.pop(0)
if init_confirm_count >= INIT_CONFIRM_THRESH:
detected_rects = [current_rect]
else:
init_confirm_count = 1
rects_history = [[current_rect]]
# ---------- 3. 激光识别与跟踪 ----------
laser_detected = False
laser_x, laser_y = 0, 0
blobs = img.find_blobs(
[LASER_LAB],
pixels_threshold=BLOB_PIXEL_THRESH,
area_threshold=BLOB_AREA_THRESH,
merge=True,
margin=2
)
target_blob = None
max_brightness = 0
for blob in blobs:
# 圆形度筛选
perimeter = blob.perimeter()
circularity = 4 * math.pi * blob.area() / (perimeter ** 2) if perimeter > 0 else 0
if circularity > LASER_CIRCULARITY:
# 亮度筛选(保留最亮目标)
stats = img.get_statistics(roi=blob.rect())
brightness = stats.l_mean()
if brightness > max_brightness:
max_brightness = brightness
target_blob = blob
if target_blob:
curr_x, curr_y = target_blob.cx(), target_blob.cy()
# 坐标稳定性判断
if abs(curr_x - last_laser_x) < 5 and abs(curr_y - last_laser_y) < 5:
stable_count += 1
else:
stable_count = 1
last_laser_x, last_laser_y = curr_x, curr_y
if stable_count >= LASER_STABLE_THRESH:
laser_detected = True
laser_x, laser_y = curr_x, curr_y
img.draw_cross(laser_x, laser_y, color=(0, 255, 0), size=8) # 绿色十字
# ---------- 4. 可视化绘制 ----------
# 绘制矩形(若检测到)
rect_centers = []
if detected_rects:
colors = [(255, 0, 0), (0, 0, 255)] # 两个矩形用不同颜色
for i, rect_corners in enumerate(detected_rects):
# 绘制边框
for j in range(4):
img.draw_line(
rect_corners[j][0], rect_corners[j][1],
rect_corners[(j+1)%4][0], rect_corners[(j+1)%4][1],
color=colors[i]
)
# 绘制顶点
for p in rect_corners:
img.draw_circle(p[0], p[1], 3, color=(0, 255, 0))
# 计算并绘制中心
center = get_rect_center(rect_corners)
rect_centers.append(center)
img.draw_circle(center[0], center[1], 5, color=colors[i], fill=True)
# 绘制双矩形平均中心(如果启用双矩形模式)
if REQUIRE_DUAL_RECT and len(rect_centers) == 2:
avg_cx = (rect_centers[0][0] + rect_centers[1][0]) // 2
avg_cy = (rect_centers[0][1] + rect_centers[1][1]) // 2
img.draw_circle(avg_cx, avg_cy, 6, color=(255, 255, 0), fill=True) # 黄色平均中心
img.draw_string(avg_cx-10, avg_cy-10, "A", color=(255, 255, 255))
# 绘制画面中心
img.draw_circle(screen_center[0], screen_center[1], 3, color=(255, 0, 0), fill=True)
img.draw_string(screen_center[0]-5, screen_center[1]-10, "C", color=(255, 0, 0))
# ---------- 5. 串口通信 ----------
# 准备发送数据
data_parts = [PROTOCOL_HEADER]
# 激光坐标
data_parts.extend([str(laser_x) if laser_detected else "-1",
str(laser_y) if laser_detected else "-1"])
# 矩形坐标
if detected_rects:
if REQUIRE_DUAL_RECT and len(rect_centers) == 2:
# 双矩形:发送两个中心和平均中心
data_parts.extend([str(rect_centers[0][0]), str(rect_centers[0][1]),
str(rect_centers[1][0]), str(rect_centers[1][1]),
str((rect_centers[0][0]+rect_centers[1][0])//2),
str((rect_centers[0][1]+rect_centers[1][1])//2)])
else:
# 单矩形:发送中心
data_parts.extend([str(rect_centers[0][0]), str(rect_centers[0][1]), "-1", "-1"])
else:
# 未检测到矩形
data_parts.extend(["-1", "-1", "-1", "-1", "-1", "-1"])
# 发送数据
data_str = ",".join(data_parts) + "\r\n"
uart.write(data_str.encode())
# 调试信息
if detected_rects:
print(f"矩形中心: {rect_centers} | 激光: {'检测到' if laser_detected else '未检测到'}")
else:
print(f"未检测到有效矩形 | 激光: {'检测到' if laser_detected else '未检测到'}")
# ---------- 6. 性能与内存管理 ----------
print(f"帧率: {clock.fps():.1f}fps")
gc.collect() # 强制回收内存
帮我移植到K230上