# K230视觉巡线模块# 功能：检测地面路线，计算路径偏移角度和中心位置# 输出：路径偏移角度、路径中心坐标、线段信息等# 作者：KizAI# 日期：2024-07-11import image,
time,math,os, sysfrom media.sensor import *from media.display import *from media.media import *sensor = None# ################## 系统配置 ################### 摄像头配置 # 巡线参数配置LINE_COLOR_THRESHOLD = (0, 80) # 黑色线阈值 (L*通道)ROI = (0, 180, 320, 60) # 感兴趣区域(图像底部)MIN_LINE_LENGTH = 50 # 最小线段长度MAX_LINE_ANGLE_DIFF = 20 # 最大线段角度差CENTER_X = 160 # 图像中心X坐标# ################## 辅助函数 ##################def calculate_path_center(left_line, right_line): """计算两条线段中点之间的中心点""" # 计算线段1的中点 mid1_x = (left_line.x1() + left_line.x2()) // 2 mid1_y = (left_line.y1() + left_line.y2()) // 2 # 计算线段2的中点 mid2_x = (right_line.x1() + right_line.x2()) // 2 mid2_y = (right_line.y1() + right_line.y2()) // 2 # 计算两个中点之间的中心点 center_x = (mid1_x + mid2_x) // 2 center_y = (mid1_y + mid2_y) // 2 return center_x, center_ydef calculate_offset_angle(left_line, right_line): """计算路径偏移角度""" # 计算左右线的平均角度 left_angle = left_line.theta() right_angle = right_line.theta() # 处理角度值（确保在0-180范围内） if left_angle < 0: left_angle += 180 if right_angle < 0: right_angle += 180 # 计算平均角度（路径方向） avg_angle = (left_angle + right_angle) / 2 # 计算相对于垂直方向的偏移角度（90度为垂直方向） offset_angle = avg_angle - 90 return offset_angledef get_line_data(lines): """处理检测到的线段，返回左右线信息""" left_lines = [] right_lines = [] valid_lines = [] for line in lines: # 计算线段角度 (转换为0-180度范围) angle = line.theta() if angle < 0: angle += 180 # 过滤掉过短或过平的线段 if line.length() < MIN_LINE_LENGTH or (angle < 10 or angle > 170): continue # 根据角度区分左右线 if 70 < angle < 110: # 近似水平的线 continue elif angle > 90: # 左线 (135-180度) left_lines.append(line) else: # 右线 (0-45度) right_lines.append(line) valid_lines.append(line) # 选择最长的左线和右线 left_line = max(left_lines, key=lambda l: l.length()) if left_lines else None right_line = max(right_lines, key=lambda l: l.length()) if right_lines else None return left_line, right_line, valid_linestry: sensor=Sensor(id=2) sensor.reset() # 复位摄像头 sensor.set_pixformat(sensor.RGB565) # 使用RGB565格式 sensor.set_framesize(sensor.QVGA) # 使用QVGA分辨率(320x240) sensor.set_vflip(True) # 垂直翻转图像（根据摄像头安装方式） sensor.set_hmirror(True) # 水平镜像图像（根据摄像头安装方式） sensor.skip_frames(time=500) # 等待摄像头稳定 Display.init(Display.VIRT, width=320, height=240, to_ide=True) # 初始化媒体管理器 MediaManager.init() # 启动传感器 sensor.run() # ################## 主循环 ################## clock = time.clock() while True: os.exitpoint() clock.tick() # 1. 图像采集 img = sensor.snapshot() # 2. 图像预处理 img.lens_corr(1.8) # 镜头畸变校正 img_gray = img.to_grayscale(copy=True) img_bin = img_gray.binary([LINE_COLOR_THRESHOLD], invert=True, roi=ROI) # 3. 检测线段 lines = img_bin.find_lines(threshold=1000, theta_margin=25, rho_margin=25, roi=ROI) # 4. 处理线段数据 left_line, right_line, valid_lines = get_line_data(lines) # 5. 计算路径信息 path_data = { 'center_x': CENTER_X, # 图像中心X 'center_y': ROI[1] + ROI[3]//2, # ROI中心Y 'offset_angle': 0, # 偏移角度 'offset_error': 0, # 水平偏移误差 'left_line': None, # 左线数据 'right_line': None, # 右线数据 'path_found': False # 是否检测到路径 } if left_line and right_line: # 计算路径中心点 path_center_x, path_center_y = calculate_path_center(left_line, right_line) # 计算偏移角度 offset_angle = calculate_offset_angle(left_line, right_line) # 计算水平偏移误差 offset_error = path_center_x - CENTER_X # 更新路径数据 path_data.update({ 'center_x': path_center_x, 'center_y': path_center_y, 'offset_angle': offset_angle, 'offset_error': offset_error, 'left_line': (left_line.x1(), left_line.y1(), left_line.x2(), left_line.y2()), 'right_line': (right_line.x1(), right_line.y1(), right_line.x2(), right_line.y2()), 'path_found': True }) # 6. 输出路径数据 print("路径数据:", path_data) # 7. 可视化（可选，用于调试） # 绘制ROI区域 img.draw_rectangle(ROI, color=(0, 255, 0)) # 绘制检测到的线段 for line in valid_lines: img.draw_line(line.line(), color=(255, 0, 0), thickness=2) # 绘制路径中心点 if path_data['path_found']: img.draw_cross(int(path_data['center_x']), int(path_data['center_y']), color=(0, 255, 255), size=10, thickness=2) # 绘制图像中心线 img.draw_line((CENTER_X, 0, CENTER_X, 240), color=(0, 0, 255), thickness=1) # 显示调试信息 fps = clock.fps() img.draw_string(10, 10, "FPS: %.1f" % fps, color=(255, 255, 255)) img.draw_string(10, 30, "偏移角度: %.1f°" % path_data['offset_angle'], color=(255, 255, 255)) img.draw_string(10, 50, "偏移误差: %d" % path_data['offset_error'], color=(255, 255, 255)) # 控制帧率 time.sleep_ms(50) except KeyboardInterrupt as e: print("用户停止: ", e)except BaseException as e: print(f"异常: {e}")finally: # 停止传感器运行 if isinstance(sensor, Sensor): sensor.stop() # 反初始化显示模块 Display.deinit() os.exitpoint(os.EXITPOINT_ENABLE_SLEEP) time.sleep_ms(100) # 释放媒体缓冲区 MediaManager.deinit()# ################## 输出数据结构说明 ##################"""返回的路径数据字典包含以下字段：{ 'center_x': 路径中心点的X坐标, 'center_y': 路径中心点的Y坐标, 'offset_angle': 路径偏移角度（度）, - 正值：路径向右偏转 - 负值：路径向左偏转 - 0：路径直行 'offset_error': 水平偏移误差（像素）, - 正值：路径中心在图像中心右侧 - 负值：路径中心在图像中心左侧 'left_line': 左边界线坐标 (x1, y1, x2, y2), 'right_line': 右边界线坐标 (x1, y1, x2, y2), 'path_found': 是否成功检测到路径 (True/False)}"""期待
