基于opencv-python的薄片颜色、形状、姿态倾斜角度、同名边框的识别控制机械臂抓取分类，来源于第九届集创赛海云捷讯杯
本文介绍了一个基于计算机视觉的自动化机械臂抓取分类系统，该系统通过摄像头实时识别工作台上不同颜色和形状的物块，计算其空间位置和姿态，并控制机械臂完成精确抓取和分类搬运。系统采用HSV色彩空间和分水岭算法进行目标识别，通过透视变换将图像坐标转换为实际坐标，结合机械臂逆运动学实现精确控制。用户可自定义目的地区域和放置角度，系统支持红、黄、蓝、黑四种颜色及正方形、圆形、六边形、三角形四种形状的识别。关键
·
1. 项目简介
本项目旨在开发一个基于计算机视觉的自动化机械臂抓取与分类系统。该系统能够通过摄像头实时获取工作台图像，自动识别图像中不同颜色、形状的物块，并计算其精确的空间位置、姿态和倾斜角度。随后，系统根据预设的分类规则（颜色、形状、目的地编号及额外的旋转角度），生成控制机械臂进行精确抓取和放置的指令，并通过串口通信驱动机械臂完成物块的自动化分类搬运。
核心功能：
-
实时图像捕获与校准： 利用摄像头获取视频流，并通过用户定义的绿色区域进行透视变换校准，将图像坐标转换为真实世界坐标。
-
多目标物块识别： 运用图像处理技术识别画面中红、黄、蓝、黑四种颜色，以及正方形、圆形、六边形、直角三角形四种形状的物块。
-
精确姿态与倾斜角度计算： 计算每个物块的几何中心点或关键点（如直角三角形的斜边中点），以及其在二维平面上的倾斜角度。
-
机械臂逆运动学求解： 基于物块的真实世界坐标，结合机械臂的结构参数，计算出机械臂各关节的目标角度（a0, a1, a2, a3）。
-
精细抓取与放置角度控制： 引入a5（抓取姿态角度）、a6（参考白点辅助角度）、a7（目标放置角度）和a9（用户自定义放置角度微调）等多个角度参数，实现对物块抓取和放置时的精确旋转控制。
-
自动化指令生成与通信： 将计算出的机械臂角度、目标放置点等信息封装成特定的十六进制指令，通过串口发送给机械臂控制模块，并等待模块响应以确保操作完成。
-
交互式用户界面： 提供实时预览、参数调整、目的地分配等交互功能，方便用户进行系统设置与操作。
2. 项目方法与步骤
本项目主要分为两大阶段：目的地分配与初始化，以及实时图像处理与机械臂控制。
2.1 目的地分配与初始化
-
目的地预设： 在系统启动之初，用户需要为每种颜色和形状的物块（例如：红色正方形、黄色圆形等）预先分配一个或多个目的地编号（1-24）。
-
创新点： 目的地分配时，用户可额外输入一个三位数字代码，其中第一位（0-3）用于指定放置时a9的额外旋转角度（0°、90°、180°、270°），后两位为目的地编号。这提供了在特定目的地进行物块定向放置的能力。
-
系统会记录这些分配规则，并检查目的地编号的重复性，确保每个目的地只分配给一种特定类型的物块。
-
-
串口初始化： 系统尝试初始化与机械臂通信的串口连接，设置波特率等参数，并打印连接状态。
2.2 实时图像处理与机械臂控制
-
摄像头启动： 启动摄像头并设置分辨率（1280x720）。
-
绿色框校准： 在实时预览窗口中，用户需要依次点击图像中的四个点，定义一个“绿色校准框”。这四个点应对应工作台上的一个矩形区域，用于后续的透视变换。
-
系统会实时绘制已标记的点和连线，并在四个点设置完成后绘制出根据FIXED_POINT_OFFSET计算出的“固定点”（机械臂基准点在图像中的映射）。
-
-
白点标记（可选）： 绿色框设置完成后，用户可以在物体附近点击标记白色点。这些白点在计算某些特定形状的a6角度时作为参考。
-
动态阈值调整： 在实时预览或处理结果显示阶段，用户可以通过键盘U/D键实时调整Watershed分割算法的distance_threshold参数，以优化物块的分割效果。
-
图像捕获： 用户在实时预览界面按下q键，系统将捕获当前帧进行处理。
-
透视变换： 根据用户定义的绿色框四点，计算出透视变换矩阵M和逆矩阵M_inv。这将用于将像素坐标转换为实际物理世界的厘米坐标。
-
ROI掩膜生成： 使用绿色框的区域生成一个掩膜，确保后续的图像处理仅限于工作台区域内。
-
颜色分割：
-
将RGB图像转换为HSV色彩空间，因为HSV对光照变化更鲁棒。
-
针对预设的“红”、“黄”、“蓝”、“黑”四种颜色，应用相应的HSV阈值范围进行二值化，生成颜色掩膜。
-
将颜色掩膜与绿色框掩膜进行按位与操作，只保留绿色框内的特定颜色区域。
-
-
Watershed 分割：----------------------------------------------------------------------------------------
-
对颜色掩膜进行形态学操作（腐蚀、膨胀），以分离相互靠近的物块。
-
计算距离变换，生成前景（sure_fg）和背景（sure_bg）标记。
-
利用Watershed算法对图像进行过分割，确保每个独立的物块都被正确分割。------
-
-
形状识别与姿态计算：------------------------------------------------------------------------------------
-
遍历Watershed分割出的每个独立区域。
-
对每个区域提取轮廓，并进行多边形逼近（approxPolyDP）以获取鲁棒的角点。
-
根据角点数量判断形状：3个角为“直角三角形”，4个角为“正方形”，6个角为“六边形”，其他为“圆形”。
-
关键点确定： 对于直角三角形，计算其斜边中点作为关键点；对于其他形状，计算轮廓的几何中心作为关键点。
-
倾斜角度计算：
-
圆形： 倾斜角度固定为0°。
-
六边形/正方形： 通过识别最上方角点及其相邻角点来计算参考边的倾斜角。
-
直角三角形： 识别90度角顶点，然后计算斜边两端点的中点，并根据斜边的方向计算倾斜角度，并进行45度的调整。---------------------------------------------------------
-
-
-
坐标转换与机械臂角度计算：-------------------------------------------------------------------------------
-
将识别到的关键点像素坐标通过透视变换矩阵M转换为实际物理世界的厘米坐标。
-
计算到固定点的距离： 计算物块关键点到预设“固定点”（机械臂基准点）的实际距离（毫米）。
-
计算a0角度： 计算物块关键点与固定点连线相对于Y轴的夹角。
-
机械臂逆运动学：
-
距离 <= MAX_DISTANCE (260.2mm)： 使用fsolve求解基于机械臂连杆长度（L1, L2, L3, L4）的非线性方程组，计算出a1, a2, a3三个关节角。
-
距离 > MAX_DISTANCE： 采用简化模型，将L2和L3视为共线，计算出a1, a2, a3（此时a2固定为0）。这部分逻辑针对机械臂伸展较远时的特定姿态。
-
如果计算失败，则使用预设的备用角度。----------------------------------------------------
-
-
-
生成唯一ID与数据存储： 为每个识别出的物块生成一个唯一的ID（颜色代码+形状代码+序号），并将形状、关键点、实际坐标、倾斜角度、机械臂角度等信息存储在shapes列表中。
-
结果可视化： 在处理后的图像上绘制物块轮廓、关键点、唯一ID、倾斜角度、机械臂角度等信息。
-
目的地匹配： 根据之前预设的shape_destinations规则，将识别到的物块分配到对应的目的地，并获取该目的地的a9角度。
-
计算精细抓取/放置角度：
-
a5 (物块抓取角度)： 对于正方形和直角三角形，基于物块的倾斜角度和a0计算；对于圆形和六边形，若有最近的白点，则基于a6计算，否则也基于倾斜角度。
-
a6 (白点参考角度)： 若存在靠近物块的白点，计算物块关键点与最近白点连线的角度。
-
a7 (目的地放置角度)： 根据目的地编号查询预设的DEST_ANGLES表。
-
a8 (最终放置调整角度)： a8 = a5 - a7 + a9，其中a9是用户为特定目的地定义的额外旋转。
-
详细结果输出： 在终端打印每个物块的详细信息，包括颜色、形状、实际坐标、倾斜角度、各关节角度（a0, a1, a2, a3, a5, a6, a7, a8）、以及最终的控制值。
-
-
指令发送：
-
用户在终端输入m指令后，系统将按预设的颜色和形状顺序，依次处理每个已识别且有目的地的物块。
-
对于每个物块，生成十六进制的机械臂控制指令（包含a0, a1, a2, a3对应的控制值，以及a8和目的地编号）。
-
通过串口发送指令，并等待机械臂返回0x52的响应，表示当前物块已处理完成，然后继续处理下一个物块。
-
如果串口通信失败，流程将中止。
-
3. 核心原理
3.1 计算机视觉
-
透视变换 (Perspective Transform)： 将摄像头捕捉到的二维图像平面上的点，映射到三维世界中的二维平面上。通过校准点（绿色框），建立像素坐标与实际物理尺寸（厘米）之间的线性关系，解决了图像畸变和视角倾斜问题。
-
HSV色彩空间 (Hue, Saturation, Value)： 相比于RGB，HSV色彩空间更符合人类感知颜色的方式，且其色相(Hue)通道对光照变化不敏感，使得颜色分割更加稳定和鲁棒。
-
Watershed 分割算法： 是一种基于图像形态学的图像分割方法。它将图像想象成地形图，灰度值高的区域对应山峰，灰度值低的区域对应山谷。通过“注水”过程，将图像分割成多个区域（集水盆），有效地分离了相互粘连的物体。其核心在于距离变换（distanceTransform）和标记（markers）的生成。
-
轮廓检测与逼近 (Contour Detection & Approximation)： 通过cv2.findContours识别物体的外形边界。cv2.approxPolyDP则用于对复杂轮廓进行简化，减少顶点数量，使其更接近标准几何形状的特点（如直角三角形、正方形）。
-
图像矩 (Image Moments)： cv2.moments函数用于计算图像区域的几何属性，如中心坐标（m10/m00, m01/m00），这是确定物块抓取点的关键。
3.2 机械臂运动学
-
逆运动学 (Inverse Kinematics - IK)： 已知机械臂末端执行器（抓手）在空间中的目标位置和姿态，求解机械臂各关节所需转动的角度。本项目采用数学方程组求解（scipy.optimize.fsolve）和分段处理（距离阈值）相结合的方式。
-
L1, L2, L3, L4： 代表机械臂的各连杆长度，是IK求解的结构参数。
-
a0, a1, a2, a3： 分别代表机械臂的基座旋转角、第一关节、第二关节、第三关节角度。
-
-
角度参数的引入：
-
a5 (物块抓取角度)： 用于计算机械臂在抓取物块时，其末端执行器（夹爪）需要旋转到的角度，以与物块的倾斜角度对齐。
-
a6 (白点辅助角度)： 当物块（如圆形）本身难以确定明确倾斜角度时，可通过用户标记的白点作为参考，计算夹爪的初始对齐角度。
-
a7 (目的地固定角度)： 预设的24个放置目的地，每个目的地对应一个固定的旋转角度，确保物块被放置到指定方向。
-
a8 (夹爪最终旋转调整角度)： a8 = a5 - a7 + a9 这一公式巧妙地将物块自身的倾斜、目的地要求以及用户自定义的微调结合起来，实现对夹爪放置角度的精确控制。
-
a9 (用户自定义放置角度)： 允许用户在为特定物块分配目的地时，额外指定一个放置时的微调角度，进一步增强系统的灵活性和精确性。
-
3.3 串口通信协议
-
十六进制指令帧： 采用特定的帧格式AABB + 5个16位角度控制值 + 1个8位目的地编号 + CC。AABB和CC作为帧头和帧尾，用于数据包的同步和校验。
-
控制值计算： 机械臂的每个关节角度（a0, a1, a2, a3, a8）被转换为特定的16位整数控制值，可能对应于机械臂内部的步进电机脉冲数或PWM信号。转换公式int(X + Y * 4096 / 360)表明这是一个将角度映射到特定控制范围的线性转换。
-
握手机制： 发送指令后，系统会等待从模块返回0x52的响应。这种握手机制确保了主控知道从模块已接收并开始执行任务，从而避免指令的堆积或丢失，保证了操作的顺序性和可靠性。
4. 总结与展望
本项目成功构建了一个集成了计算机视觉、机器人运动学和串口通信的自动化抓取分类系统。其模块化的设计、交互式的校准流程以及对复杂物块姿态的精细控制，展示了强大的实用性和可扩展性。
潜在的优化方向：
-
动态光照适应性： 引入更高级的颜色校准或自适应阈值算法，以应对环境光照变化对颜色识别的影响。
-
更鲁棒的形状识别： 考虑使用机器学习（如SVM或CNN）来识别更复杂的形状或处理部分遮挡的情况。
-
三维位姿估计： 结合深度相机（如Realsense）获取物块的三维信息，实现更精确的Z轴高度抓取，并处理倾斜的物块。
-
碰撞检测与路径规划： 在机械臂运动过程中考虑避开障碍物，优化运动路径，提高效率和安全性。
-
故障诊断与日志记录： 增加更详细的错误日志，记录摄像头、串口通信和运动学计算中的异常，便于后期维护和问题排查。
-
配置文件管理： 将所有可配置参数（颜色阈值、机械臂尺寸、串口设置等）外部化到配置文件（如YAML/JSON），提高系统的部署和维护效率。
-
Web/GUI界面： 开发一个更友好的图形用户界面（GUI），替代目前的终端交互，提升用户体验。
5.图像识别的核心流程
-
捕获图像:
-
当用户在实时预览模式下按下 'q' 键时，当前帧会被捕获并用于后续处理。
-
HSV 颜色空间转换:
-
捕获的图像首先被转换到 HSV (Hue, Saturation, Value) 颜色空间 cv2.cvtColor(captured_image, cv2.COLOR_BGR2HSV)。HSV 颜色空间在颜色分割方面通常比 BGR 更稳定和直观。
-
绿色框掩码:
-
创建一个与图像大小相同的全黑掩码 green_box_mask。
-
使用 cv2.fillPoly() 函数将用户定义的 user_green_box 区域填充为白色 (255)，从而创建一个只包含绿色框内部的掩码。这确保了后续的形状识别只发生在用户指定的工作区域内。
-
-
颜色分割与阈值处理:
-
代码遍历预定义的 COLOR_THRESHOLDS 字典，该字典为每种颜色（红、黄、蓝、黑）定义了一组 HSV 颜色范围。
-
对于每种颜色，使用 cv2.inRange() 创建一个二值掩码，该掩码中只有属于该颜色范围的像素为白色。
-
cv2.bitwise_and(color_mask, green_box_mask) 操作确保只保留绿色框内部的特定颜色区域，排除了框外的干扰。
-
-
分水岭算法 (Watershed Algorithm):
-
-
形态学操作:
-
cv2.erode(): 对颜色掩码进行腐蚀操作，以消除小的噪声点和断开连接的区域，使前景对象更紧凑。
-
cv2.dilate(): 对腐蚀后的图像进行膨胀操作，填充腐蚀造成的空洞，同时形成“确定背景”区域 sure_bg。
-
-
距离变换:
-
cv2.distanceTransform(eroded, cv2.DIST_L2, 5) 计算前景像素到最近背景像素的距离，结果图像中像素值越大表示离背景越远，越可能是前景中心。
-
cv2.threshold(dist_transform, distance_threshold * dist_transform.max(), 255, 0) 对距离变换结果进行阈值处理，提取出“确定前景”区域 sure_fg。distance_threshold 是一个可调节的参数（0.10 到 0.80），用于控制前景的识别严格度。
-
-
未知区域: cv2.subtract(sure_bg, sure_fg) 计算确定背景和确定前景之间的区域，这部分被认为是“未知”区域。
-
连通组件与标记:
-
cv2.connectedComponents(sure_fg) 找到确定前景中的所有连通组件，并为每个组件分配一个唯一的标记。
-
markers 数组用于分水岭算法，其中确定前景被标记，未知区域被标记为0。
-
-
应用分水岭: cv2.watershed(captured_image, markers) 运行分水岭算法。它将根据图像的梯度信息，将未知区域分割并分配给最近的确定前景区域，从而实现准确的对象分割。
-
-
形状分析与特征提取:
-
遍历分水岭算法得到的每个独立标记（即每个识别出的对象）。
-
轮廓提取: cv2.findContours() 提取每个对象的轮廓。
-
面积过滤: cv2.contourArea(cnt) < 500 过滤掉过小的噪声区域。
-
鲁棒角点检测: get_robust_corners(contour, epsilon_ratio=0.03) 函数使用 cv2.arcLength 和 cv2.approxPolyDP 来近似多边形轮廓，从而获得稳定的角点数量。epsilon_ratio 控制近似的精度。
-
形状分类: get_shape_by_corners(corner_count) 根据角点数量（3个：三角形，4个：正方形，6个：六边形，其他：圆形）对形状进行分类。
-
关键点确定:
-
直角三角形: 对于识别为直角三角形的，会额外识别出直角顶点和斜边中点，斜边中点作为关键点。
-
其他形状: 其他形状的关键点通常是其轮廓的几何中心 (cv2.moments 计算矩，然后得出中心)。
-
-
倾斜角度计算: calculate_tilt_angle(contour, shape, corners) 函数根据形状类型和角点信息计算对象的倾斜角度。对于圆形，角度为0；对于正方形和六边形，通过选取顶部左侧角点和顶部角点计算连线的倾斜角度；对于直角三角形，通过斜边计算其倾斜角度并进行特定调整。
-
-
6.源代码
import numpy as np
import cv2
import serial
import time
from scipy.optimize import fsolve
# ====================== 全局变量 ======================
click_points = [] # 存储用户标记的白色点（原始图像坐标）
user_green_box = [] # 存储用户设置的绿色框四点（替代固定的GREEN_BOX_POINTS）
shapes = [] # 存储识别出的形状信息
serial_port = None # 串口对象（全局，避免重复打开）
result = None # 全局结果图像
scaled_result = None # 缩放后的结果图像
cap = None # 摄像头对象
is_processing = False # 是否处于处理后图像显示状态
is_setting_green_box = True # 是否处于设置绿色框四点的阶段（新增状态）
# ====================== 配置参数 ======================
REAL_WIDTH = 26.5 # 实际宽度（厘米）
REAL_HEIGHT = 17.5 # 实际高度（厘米）
CAM_WIDTH = 1280
CAM_HEIGHT = 720
# 更新：颜色和形状代码映射
COLOR_CODE = {
"Red": "1",
"Yellow": "2",
"Blue": "3",
"Black": "4"
}
SHAPE_CODE = {
"Square": "1",
"Circle": "2",
"Hexagon": "3",
"Right Triangle": "4"
}
SHAPE_NAMES = {
"Square": "正方形",
"Circle": "圆形",
"Hexagon": "六边形",
"Right Triangle": "三角形"
}
COLOR_NAMES = {
"Red": "红色",
"Yellow": "黄色",
"Blue": "蓝色",
"Black": "黑色"
}
# 新增：a9角度映射表（第一位数字对应角度）
A9_MAP = {
0: 0.0,
1: 90.0,
2: 180.0,
3: 270.0
}
FIXED_POINT_OFFSET = (134, 84) # 固定点偏移（毫米）
L1, L2, L3, L4 = 113, 144, 122, 165 # 机械臂结构参数（毫米）
MAX_DISTANCE = 260.2 # 距离阈值（毫米）
PRESET_ANGLES = (75.0, 0.0, 91.0) # 超出阈值计算失败时的备用角度
WHITE_POINT_DIST_THRESHOLD = 200 # 白色点与形状的最大有效距离（像素）
ANGLE_CALCULATION_THRESHOLD = 150 # 白点与形状的最大计算距离（像素）
# 目的地1~24对应的角度（按表格顺序：行1→列1-4，行2→列1-4...行6→列1-4）
DEST_ANGLES = [
43.0, 53.8, 60.1, 65.1, # 行1（目的地1-4）
57.5, 67.0, 70.7, 74.2, # 行2（目的地5-8）
77.2, 81.4, 83.6, 85.0, # 行3（目的地9-12）
99.5, 98.8, 95.1, 94.7, # 行4（目的地13-16）
120.6, 112.1, 108.4, 105.2, # 行5（目的地17-20）
133.8, 125.4, 118.7, 114.9 # 行6（目的地21-24）
]
# 串口配置
SERIAL_PORT = "COM13" # 串口端口
BAUD_RATE = 115200 # 波特率
RESPONSE_EXPECTED = b'\x52' # 预期响应（16进制52）
TIMEOUT = 10 # 等待响应超时时间（秒）
# 颜色阈值
COLOR_THRESHOLDS = {
"Red": [
(np.array([0, 50, 50]), np.array([10, 255, 255])), # 低红色
(np.array([170, 50, 50]), np.array([180, 255, 255])) # 高红色
],
"Yellow": [
(np.array([11, 43, 46]), np.array([110, 255, 255])) # 黄色
],
"Blue": [
(np.array([110, 43, 46]), np.array([140, 255, 255])) # 蓝色
],
"Black": [
(np.array([0, 0, 0]), np.array([180, 255, 46])) # 黑色
]
}
# ====================== 指令码生成模块 ======================
def calculate_values(x, y, z, a):
val1 = int(1157 - x * 4096 / 360) # 1163
val2 = int(2043 + y * 4096 / 360) # 2040
val3 = int(1986 + z * 4096 / 360) # 2019
val4 = int(2106 + a * 4096 / 360) # 2102
return val1, val2, val3, val4
def generate_master_command(ctrl_vals, destination, a8):
"""生成主控发送给从模块的16进制指令"""
angle1_hex = f"{ctrl_vals[0]:04X}" # 第一个控制值（16bit）
angle2_hex = f"{ctrl_vals[1]:04X}" # 第二个控制值（16bit）
angle3_hex = f"{ctrl_vals[2]:04X}" # 第三个控制值（16bit）
angle4_hex = f"{ctrl_vals[3]:04X}" # 第四个控制值（16bit）
# 计算1958 - a8*4096/360并转换为16进制
val5 = int(1958 - a8 * 4096 / 360)
# 确保在16位无符号整数范围内
val5 = max(0, min(65535, val5))
angle5_hex = f"{val5:04X}" # 转换为4位16进制字符串
dest_hex = f"{destination:02X}" # 目的地编号（8bit）
# 拼接完整指令的16进制字符串
command_hex = f"AABB{angle1_hex}{angle2_hex}{angle3_hex}{angle4_hex}{angle5_hex}{dest_hex}CC"
# 转换为字节流（16进制发送）
return bytes.fromhex(command_hex), command_hex
# ====================== 串口通信模块 ======================
def init_serial():
"""初始化串口连接"""
try:
ser = serial.Serial(
port=SERIAL_PORT,
baudrate=BAUD_RATE,
timeout=0.1, # 读取超时
write_timeout=2 # 写入超时
)
if ser.is_open:
print(f"串口已打开：{SERIAL_PORT}（波特率：{BAUD_RATE}）")
return ser
else:
print("串口打开失败")
return None
except serial.SerialException as e:
print(f"串口初始化错误：{e}")
return None
def send_single_command(ser, ctrl_vals, destination, a8):
"""发送单条指令并等待响应"""
# 生成指令
command_bytes, command_hex = generate_master_command(ctrl_vals, destination, a8)
try:
# 发送指令
ser.write(command_bytes)
print(f"\n发送指令：{command_hex}")
print(f"控制值：{ctrl_vals}，目的地：{destination}，a8: {a8}")
# 无限等待响应（16进制52）
print("等待从模块响应...")
while True:
response = ser.read(1) # 读取1字节
if response == RESPONSE_EXPECTED:
print(f"收到响应：{response.hex().upper()}（从模块完成运动）")
return True
time.sleep(0.1) # 短暂延迟，降低CPU占用
except serial.SerialException as e:
print(f"串口发送错误：{e}")
return False
except KeyboardInterrupt:
print("\n用户中断等待")
return False
def send_all_commands(assigned_locations, a8_values):
"""按形状分组发送指令，处理完当前形状可搬运物块后自动处理下一个形状"""
# 初始化串口
ser = init_serial()
if not ser:
return
try:
# 1. 按固定形状顺序（Square→Circle→Hexagon→Right Triangle）和颜色分组物块
color_order = ["Red", "Yellow", "Blue", "Black"]
shape_order = ["Square", "Circle", "Hexagon", "Right Triangle"]
shape_groups = {color: {shape: [] for shape in shape_order} for color in color_order} # 按颜色和形状分组
# 将物块分配到对应颜色和形状组
for shape in shapes:
color = shape[9] # 颜色信息
shape_type = shape[0] # 形状类型
if color in shape_groups and shape_type in shape_groups[color]:
shape_groups[color][shape_type].append(shape)
# 2. 按顺序处理每个颜色和形状的物块
total_processed = 0 # 记录总处理数量
for color in color_order:
color_name = COLOR_NAMES[color]
for shape_type in shape_order:
shape_name = SHAPE_NAMES[shape_type]
shape_items = shape_groups[color][shape_type] # 当前颜色和形状的所有物块
# 筛选当前颜色和形状的有效目的地
dests = []
for info in assigned_locations.values():
if (
isinstance(info, dict) and
"number" in info and
isinstance(info["number"], int) and # 确保是整数
1 <= info["number"] <= 24 and
any(s[5] == uid for s in shape_items for uid in assigned_locations if
assigned_locations[uid] == info)
):
dests.append(info)
dest_count = len(dests)
item_count = len(shape_items)
process_count = min(dest_count, item_count) # 实际处理数量（取最小值）
print(f"\n==== 开始处理{color_name}{shape_name} ====")
print(
f" 识别到{item_count}个{color_name}{shape_name}，预设{dest_count}个目的地，将处理{process_count}个")
if process_count == 0:
print(f" 无有效可处理的{color_name}{shape_name}，跳过")
continue
# 处理当前颜色和形状的前process_count个物块
for i in range(process_count):
total_processed += 1
shape = shape_items[i]
unique_id = shape[5]
# 找到对应的目的地信息
dest_info = next(
(info for uid, info in assigned_locations.items()
if uid == unique_id and isinstance(info, dict) and
isinstance(info["number"], int) and 1 <= info["number"] <= 24),
None # 默认值
)
if not dest_info:
print(f" 警告：物块{unique_id}找不到有效目的地，跳过")
continue
dest_number = dest_info["number"]
a8 = a8_values.get(unique_id, 0.0)
ctrl_vals = shape[7]
print(
f"\n----- 发送第{total_processed}条指令（{color_name}{shape_name} {i + 1}/{process_count}）-----")
success = send_single_command(ser, ctrl_vals, dest_number, a8)
if not success:
print(f" {color_name}{shape_name}第{i + 1}个发送失败，终止处理流程")
ser.close()
return
# 处理完当前颜色和形状的所有可处理物块
remaining = item_count - process_count
if remaining > 0:
print(f" {color_name}{shape_name}处理完成，剩余{remaining}个因无匹配目的地将跳过")
else:
print(f" {color_name}{shape_name}全部处理完成")
print(f"\n==== 所有可处理物块处理完毕，共处理{total_processed}个 ====")
finally:
if ser.is_open:
ser.close()
print(f"串口已关闭：{SERIAL_PORT}")
# ====================== 核心功能函数 ======================
def is_point_inside(x, y, polygon):
return cv2.pointPolygonTest(np.float32(polygon), (x, y), False) >= 0
def get_perspective_matrices(green_box_points):
"""根据用户设置的绿色框四点计算透视变换矩阵"""
src = np.float32(green_box_points)
dst = np.float32([[0, 0], [REAL_WIDTH, 0], [REAL_WIDTH, REAL_HEIGHT], [0, REAL_HEIGHT]])
M = cv2.getPerspectiveTransform(src, dst)
M_inv = cv2.getPerspectiveTransform(dst, src)
return M, M_inv
def pixel_to_real(pixel_x, pixel_y, M):
point_homo = np.array([[pixel_x, pixel_y, 1]], dtype=np.float32).T
real_point = M @ point_homo
return (real_point[0, 0] / real_point[2, 0], real_point[1, 0] / real_point[2, 0])
def get_robust_corners(contour, epsilon_ratio=0.03):
perimeter = cv2.arcLength(contour, True)
epsilon = epsilon_ratio * perimeter
approx = cv2.approxPolyDP(contour, epsilon, True)
return [tuple(point[0]) for point in approx], len(approx)
def get_shape_by_corners(corner_count):
if corner_count == 3:
return "Right Triangle"
elif corner_count == 4:
return "Square"
elif corner_count == 6:
return "Hexagon"
else:
return "Circle"
def find_hypotenuse_endpoints(corners):
dists = [np.linalg.norm(np.array(corners[i]) - np.array(corners[(i + 1) % 3])) for i in range(3)]
max_idx = np.argmax(dists)
return corners[max_idx], corners[(max_idx + 1) % 3]
def calculate_midpoint(p1, p2):
return ((p1[0] + p2[0]) // 2, (p1[1] + p2[1]) // 2)
def calculate_tilt_angle(contour, shape, corners):
if shape == "Circle":
return 0.0
p1, p2 = None, None
if shape == "Hexagon":
top_corner_idx = np.argmin([y for x, y in corners])
top_corner = corners[top_corner_idx]
left_corners = []
for i, (x, y) in enumerate(corners):
if i != top_corner_idx and x < top_corner[0]:
left_corners.append((i, np.linalg.norm(np.array([x, y]) - np.array(top_corner))))
if left_corners:
left_corner_idx = min(left_corners, key=lambda x: x[1])[0]
p1 = corners[left_corner_idx]
p2 = top_corner
else:
p1 = top_corner
p2 = corners[(top_corner_idx + 1) % 6]
elif shape == "Square":
top_corner_idx = np.argmin([y for x, y in corners])
top_corner = corners[top_corner_idx]
left_corners = []
for i, (x, y) in enumerate(corners):
if i != top_corner_idx and x < top_corner[0]:
left_corners.append((i, np.linalg.norm(np.array([x, y]) - np.array(top_corner))))
if left_corners:
left_corner_idx = min(left_corners, key=lambda x: x[1])[0]
p1 = corners[left_corner_idx]
p2 = top_corner
else:
p1 = top_corner
p2 = corners[(top_corner_idx + 1) % 4]
elif shape == "Right Triangle":
angles = []
for i in range(3):
p0 = corners[i]
p1_corner = corners[(i + 1) % 3]
p2_corner = corners[(i + 2) % 3]
v1 = np.array(p0) - np.array(p1_corner)
v2 = np.array(p2_corner) - np.array(p1_corner)
dot = np.dot(v1, v2)
norm = np.linalg.norm(v1) * np.linalg.norm(v2)
angle_deg = np.degrees(np.arccos(dot / norm)) if norm != 0 else 0
angles.append((angle_deg, p1_corner))
right_vertex = min(angles, key=lambda x: abs(x[0] - 90))[1]
hypotenuse_points = [p for p in corners if p != right_vertex]
hypotenuse_p1, hypotenuse_p2 = hypotenuse_points[0], hypotenuse_points[1]
p1, p2 = hypotenuse_p1, hypotenuse_p2
midpoint = calculate_midpoint(hypotenuse_p1, hypotenuse_p2)
dx = p2[0] - p1[0]
dy_image = p2[1] - p1[1]
dy_math = -dy_image
angle_rad = np.arctan2(dy_math, dx)
angle_deg = np.degrees(angle_rad)
if right_vertex[0] > midpoint[0]:
angle_deg += 180
angle_deg = angle_deg % 360
adjusted_angle = (angle_deg - 45) % 360
return round(adjusted_angle, 2)
dx = p2[0] - p1[0]
dy_image = p2[1] - p1[1]
dy_math = p1[1] - p2[1]
if dx == 0:
angle_deg = 90.0
else:
theta = np.arctan2(dy_math, dx)
angle_rad = theta % (2 * np.pi)
angle_deg = np.degrees(angle_rad)
if shape in ["Square", "Hexagon"]:
angle_deg = min(angle_deg, 180 - angle_deg, 180 + angle_deg, 360 - angle_deg)
return round(angle_deg, 2)
# ====================== 机械臂角度计算 ======================
def calculate_mech_angles(b_mm):
"""距离≤260.2mm时，计算机械臂角度"""
def equations(P, b):
x, y = P
eq1 = x ** 2 + (y - L4) ** 2 - L3 ** 2
eq2 = (b - x) ** 2 + (L1 - y) ** 2 - L2 ** 2
return [eq1, eq2]
x0, y0 = b_mm / 2, (L1 + L4) / 2
solution = fsolve(equations, [x0, y0], args=(b_mm,))
x, y = solution
eq1_res = abs(x ** 2 + (y - L4) ** 2 - L3 ** 2)
eq2_res = abs((b_mm - x) ** 2 + (L1 - y) ** 2 - L2 ** 2)
if eq1_res > 1e-4 or eq2_res > 1e-4:
raise ValueError("超出计算范围")
cos_a1 = (y - L1) / L2
a1 = np.degrees(np.arccos(np.clip(cos_a1, -1, 1)))
vec_L3 = (-x, L4 - y)
vec_L2 = (b_mm - x, L1 - y)
cos_a2 = np.dot(vec_L3, vec_L2) / (L3 * L2)
a2 = 180 - np.degrees(np.arccos(np.clip(cos_a2, -1, 1)))
cos_a3 = (y - L4) / L3
a3 = np.degrees(np.arccos(np.clip(cos_a3, -1, 1)))
return a1, a2, a3
def calculate_mech_angles_over_max(b_mm):
"""距离>260.2mm时，基于新几何关系计算a1、a3（a2=0）"""
# 1. 计算直角三角形（L1, b_mm, R1）的参数
R1 = np.sqrt(L1 ** 2 + b_mm ** 2) # 斜边R1 = sqrt(L1² + R²)，其中R = b_mm
theta = np.degrees(np.arctan(b_mm / L1)) # R（b_mm）对的角
# 2. 计算三角形（R1, L4=165, L2+L3=266）的角度
L_sum = L2 + L3 # 266mm（共线长度）
# 角α：L4（165）对的角（用余弦定理）
numerator_alpha = R1 ** 2 + L_sum ** 2 - L4 ** 2
denominator_alpha = 2 * R1 * L_sum
cos_alpha = numerator_alpha / denominator_alpha
cos_alpha = np.clip(cos_alpha, -1, 1) # 避免数值溢出
alpha = np.degrees(np.arccos(cos_alpha))
# 角β：R1对的角（用余弦定理）
numerator_beta = L4 ** 2 + L_sum ** 2 - R1 ** 2
denominator_beta = 2 * L4 * L_sum
cos_beta = numerator_beta / denominator_beta
cos_beta = np.clip(cos_beta, -1, 1) # 避免数值溢出
beta = np.degrees(np.arccos(cos_beta))
# 3. 计算a1和a3
a1 = 180 - theta - alpha # 180 - θ - α
a3 = 180 - beta # 180 - β
a2 = 0.0 # 固定为0°
return a1, a2, a3
def calculate_a0(click_real, fixed_real):
dx = click_real[0] - fixed_real[0]
dy = click_real[1] - fixed_real[1]
angle_rad = np.arctan2(dx, dy)
return np.degrees(angle_rad)
def calculate_fixed_point(green_box_points):
"""根据用户设置的绿色框四点计算固定点像素坐标"""
src = np.float32(green_box_points)
dst = np.float32([[0, 0], [REAL_WIDTH, 0], [REAL_WIDTH, REAL_HEIGHT], [0, REAL_HEIGHT]])
M_inv = cv2.getPerspectiveTransform(dst, src)
fixed_real = (FIXED_POINT_OFFSET[0] / 10, -FIXED_POINT_OFFSET[1] / 10) # 转换为厘米
fixed_homo = np.array([[fixed_real[0], fixed_real[1], 1]], dtype=np.float32).T
src_point = M_inv @ fixed_homo
src_point = src_point[:2, 0] / src_point[2, 0]
return tuple(src_point.astype(int))
def calculate_distance_to_fixed_point(real_coord):
fixed_real = (FIXED_POINT_OFFSET[0] / 10, -FIXED_POINT_OFFSET[1] / 10)
dx = real_coord[0] - fixed_real[0]
dy = real_coord[1] - fixed_real[1]
return np.sqrt(dx ** 2 + dy ** 2) * 10 # 转换为毫米
def calculate_robot_angles(real_coord):
fixed_real = (FIXED_POINT_OFFSET[0] / 10, -FIXED_POINT_OFFSET[1] / 10)
distance = calculate_distance_to_fixed_point(real_coord)
a0 = calculate_a0(real_coord, fixed_real)
if distance > MAX_DISTANCE:
# 超过阈值：用新方法计算角度（a2=0）
a0 = a0 + 1.5
try:
a1, a2, a3 = calculate_mech_angles_over_max(distance)
except Exception as e:
print(f"超过阈值时计算失败: {e}，使用备用角度")
a1, a2, a3 = PRESET_ANGLES
else:
# 未超过阈值：正常计算
try:
a1, a2, a3 = calculate_mech_angles(distance)
except ValueError:
print("距离在阈值内，但机械臂计算失败，使用备用角度")
a1, a2, a3 = PRESET_ANGLES
return (round(a0, 2), round(a1, 2), round(a2, 2), round(a3, 2))
# ====================== 鼠标回调函数（实时预览窗口） ======================
def mouse_callback(event, x, y, flags, param):
global click_points, user_green_box, is_setting_green_box, is_processing
# 仅在实时预览状态（非处理后图像）响应
if not is_processing and event == cv2.EVENT_LBUTTONDOWN:
if is_setting_green_box:
# 处于设置绿色框阶段：收集4个角点
if len(user_green_box) < 4:
user_green_box.append((x, y))
print(f"已记录绿色框点 {len(user_green_box)}/4：({x}, {y})")
if len(user_green_box) == 4:
is_setting_green_box = False # 切换到标记白点阶段
print("绿色框四点设置完成！接下来点击的点将作为白点标记")
else:
# 绿色框已设置：收集白点
click_points.append((x, y))
print(f"已记录白色点：({x}, {y})")
# ====================== 关闭所有窗口和摄像头 ======================
def close_windows_and_camera():
global cap
cv2.destroyAllWindows()
if cap is not None and cap.isOpened():
cap.release()
print("已关闭所有图像窗口和摄像头")
# ====================== 主函数 ======================
def main():
global click_points, result, scaled_result, shapes, cap, is_processing, user_green_box, is_setting_green_box
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAM_WIDTH)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAM_HEIGHT)
if not cap.isOpened():
print("Error: 无法打开摄像头！")
return
distance_threshold = 0.40
threshold_min = 0.10
threshold_max = 0.80
threshold_step = 0.02
assigned_locations = {} # 存储分配的位置，值为{"number": 编号, "a9": 角度}
a8_values = {} # 存储每个形状的a8值
# 第一步：目的地分配（修改为三位数字输入，包含颜色和形状）
print("==== 先进行目的地分配 ====")
# 颜色顺序：红、黄、蓝、黑；形状顺序：正方形、圆形、六边形、三角形
color_order = ["Red", "Yellow", "Blue", "Black"]
shape_order = ["Square", "Circle", "Hexagon", "Right Triangle"]
# 初始化目的地字典
shape_destinations = {color: {shape: [] for shape in shape_order} for color in color_order}
used_locations = set() # 用于检查目的地编号重复
for color in color_order:
color_name = COLOR_NAMES[color]
color_code = COLOR_CODE[color]
for shape in shape_order:
shape_name = SHAPE_NAMES[shape]
shape_code = SHAPE_CODE[shape]
full_code = f"{color_code}{shape_code}"
while True:
input_str = input(
f"请为{color_name}{shape_name}（{full_code}）分配目的地（用逗号分隔三位数字，第一位0-3表示a9角度，后两位1-24表示位置，直接回车表示没有该形状）：")
if not input_str.strip():
shape_destinations[color][shape] = []
break
try:
locations = [x.strip() for x in input_str.split(',') if x.strip()]
if not locations:
shape_destinations[color][shape] = []
break
parsed_dests = []
valid = True
for loc in locations:
# 验证输入格式（三位数字）
if len(loc) != 3 or not loc.isdigit():
print(f"错误：'{loc}' 必须是三位数字")
valid = False
break
# 解析a9代码（第一位）和目的地编号（后两位）
a9_code = int(loc[0])
dest_number = int(loc[1:])
# 验证范围
if a9_code not in [0, 1, 2, 3]:
print(f"错误：'{loc}' 第一位必须是0-3（对应a9角度0°/90°/180°/270°）")
valid = False
break
if dest_number < 1 or dest_number > 24:
print(f"错误：'{loc}' 后两位必须是1-24（目的地编号）")
valid = False
break
if dest_number in used_locations:
print(f"错误：目的地编号{dest_number}已被使用")
valid = False
break
# 存储解析结果
parsed_dests.append({
"number": dest_number,
"a9": A9_MAP[a9_code]
})
used_locations.add(dest_number)
if not valid:
continue # 重新输入
shape_destinations[color][shape] = parsed_dests
break
except ValueError:
print("输入格式错误，请使用逗号分隔的三位数字")
# 显示已分配的目的地
print("\n==== 已完成目的地分配 ====")
for color in color_order:
color_name = COLOR_NAMES[color]
for shape in shape_order:
shape_name = SHAPE_NAMES[shape]
dests = shape_destinations[color][shape]
if dests:
dest_str = [f"{d['number']}(a9={d['a9']}°)" for d in dests]
print(f"{color_name}{shape_name}：{dest_str}")
else:
print(f"{color_name}{shape_name}：未分配（无该形状）")
# 第二步：图像识别与交互（核心逻辑不变，仅修改目的地匹配部分）
print("\n操作说明：先点击4个点设置绿色框，完成后按 'q' 捕获图像并识别，按 'r' 重置绿色框，按 'ESC' 退出 >>")
print(f"当前距离变换阈值：{distance_threshold:.2f}（按U键增加0.02，按D键减少0.02）")
while True:
# 重置状态：进入实时预览模式
is_processing = False
click_points = [] # 初始清空白点
captured_image = None
# 创建并绑定实时预览窗口的鼠标回调
cv2.namedWindow("Camera Preview")
cv2.setMouseCallback("Camera Preview", mouse_callback)
print("\n进入实时预览模式：")
if is_setting_green_box:
print("请点击4个点作为绿色框的四角（建议顺序：左上→右上→右下→左下）")
else:
print("绿色框已设置，点击画面标记白点，按q处理图像，按r重置绿色框")
# 实时预览循环（等待用户操作）
while True:
ret, frame = cap.read()
if not ret:
print("Error: 无法获取图像帧！")
break
# 绘制已设置的绿色框点和连线
# 绘制已设置的绿色框点和连线
if len(user_green_box) > 0:
# 绘制点和标注（G4的标注单独移到左下方）
for i, (x, y) in enumerate(user_green_box):
# 点的位置不变，仍绘制绿色圆点
cv2.circle(frame, (x, y), 5, (0, 255, 0), -1)
# 判断是否为G4（索引i=3，因为第4个点的索引是3）
if i == 3: # G4点：标注在左下方
# x-30：向左偏移30像素；y+20：向下偏移20像素（可根据显示效果调整）
text_pos = (x - 30, y + 20)
else: # G1~G3：保持原位置（右上方）
text_pos = (x + 10, y - 10)
# 绘制标注文本
cv2.putText(frame, f"G{i + 1}", text_pos,
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
# （后续连线绘制逻辑...）
# 绘制连线（如果有2个及以上点）
for i in range(1, len(user_green_box)):
cv2.line(frame, user_green_box[i - 1], user_green_box[i], (0, 255, 0), 2)
# 绘制闭合线（如果4个点都设置了）
if len(user_green_box) == 4:
cv2.line(frame, user_green_box[-1], user_green_box[0], (0, 255, 0), 2)
# 绘制固定点（绿色框设置完成后）
fixed_point_pixel = calculate_fixed_point(user_green_box)
cv2.drawMarker(frame, fixed_point_pixel, (0, 0, 255), cv2.MARKER_STAR, 10, 2)
cv2.putText(frame, "固定点", (fixed_point_pixel[0] + 15, fixed_point_pixel[1] - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
# 绘制已标记的白点
for (x, y) in click_points:
overlay = frame.copy()
cv2.circle(overlay, (x, y), 1, (192, 192, 192), -1)
cv2.addWeighted(overlay, 0.2, frame, 0.8, 0, frame) # 50%透明度
# 仅显示距离阈值（左上角注释只保留距离阈值）
cv2.putText(frame, f"阈值: {distance_threshold:.2f}", (20, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
cv2.imshow("Camera Preview", frame)
key = cv2.waitKey(1)
# 调整阈值
if key == ord('u') or key == ord('U'):
if distance_threshold < threshold_max:
distance_threshold += threshold_step
distance_threshold = round(distance_threshold, 2)
print(f"距离变换阈值已增加到：{distance_threshold:.2f}")
elif key == ord('d') or key == ord('D'):
if distance_threshold > threshold_min:
distance_threshold -= threshold_step
distance_threshold = round(distance_threshold, 2)
print(f"距离变换阈值已减少到：{distance_threshold:.2f}")
# 按r键：重置绿色框设置
if key == ord('r') or key == ord('R'):
user_green_box = []
is_setting_green_box = True
click_points = []
print("已重置绿色框和白点，请重新点击4个点作为绿色框的四角")
# 按q键：捕获当前图像并处理（需绿色框已设置）
if key == ord('q'):
if len(user_green_box) != 4:
print("请先设置完4个绿色框点再处理图像！")
continue
captured_image = frame.copy()
print(f"\n已捕获图像，使用已标记的{len(click_points)}个白点进行处理...")
break
# 按ESC键：退出程序
elif key == 27:
close_windows_and_camera()
return
if captured_image is None:
print("Error: 未捕获到图像！")
continue
# 进入处理后模式
is_processing = True
cv2.destroyWindow("Camera Preview")
# 图像识别处理（使用用户设置的绿色框）
hsv = cv2.cvtColor(captured_image, cv2.COLOR_BGR2HSV)
# 用用户设置的绿色框生成掩码（只处理绿色框内的区域）
green_box_mask = np.zeros(hsv.shape[:2], np.uint8)
cv2.fillPoly(green_box_mask, [np.int32(user_green_box)], 255)
# 颜色识别和形状分析
shapes = []
# 初始化计数器：color -> shape -> count
counters = {color: {shape: 1 for shape in shape_order} for color in color_order}
# 创建一个空白的结果图像，用于叠加所有颜色的识别结果
final_result = captured_image.copy()
# 对每种颜色进行处理
for color in color_order:
# 创建颜色掩码
color_mask = np.zeros(hsv.shape[:2], np.uint8)
for lower, upper in COLOR_THRESHOLDS[color]:
color_mask = cv2.bitwise_or(color_mask, cv2.inRange(hsv, lower, upper))
# 只保留绿色框内的颜色区域
color_mask = cv2.bitwise_and(color_mask, green_box_mask)
# 分水岭分割
kernel = np.ones((3, 3), np.uint8)
eroded = cv2.erode(color_mask, kernel, iterations=2)
sure_bg = cv2.dilate(eroded, kernel, iterations=3)
dist_transform = cv2.distanceTransform(eroded, cv2.DIST_L2, 5)
ret, sure_fg = cv2.threshold(dist_transform, distance_threshold * dist_transform.max(), 255, 0)
sure_fg = np.uint8(sure_fg)
unknown = cv2.subtract(sure_bg, sure_fg)
ret, markers_fg = cv2.connectedComponents(sure_fg)
markers = markers_fg + 1
markers[unknown == 255] = 0
markers = cv2.watershed(captured_image, markers)
# 为当前颜色创建一个临时结果图像
color_result = captured_image.copy()
# 分析每个区域
labels = np.unique(markers)
for label in labels:
if label in [0, 1, -1]:
continue
region_mask = (markers == label).astype(np.uint8) * 255
contours, _ = cv2.findContours(region_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
continue
cnt = max(contours, key=cv2.contourArea)
if cv2.contourArea(cnt) < 500:
continue
corners, corner_count = get_robust_corners(cnt)
shape = get_shape_by_corners(corner_count)
tilt_angle = calculate_tilt_angle(cnt, shape, corners)
# 确定关键坐标
if shape == "Right Triangle" and corner_count == 3:
angles = []
for i in range(3):
p0 = corners[i]
p1_corner = corners[(i + 1) % 3]
p2_corner = corners[(i + 2) % 3]
v1 = np.array(p0) - np.array(p1_corner)
v2 = np.array(p2_corner) - np.array(p1_corner)
dot = np.dot(v1, v2)
norm = np.linalg.norm(v1) * np.linalg.norm(v2)
angle_deg = np.degrees(np.arccos(dot / norm)) if norm != 0 else 0
angles.append((angle_deg, p1_corner))
right_vertex = min(angles, key=lambda x: abs(x[0] - 90))[1]
hypotenuse_points = [p for p in corners if p != right_vertex]
hypotenuse_p1, hypotenuse_p2 = hypotenuse_points[0], hypotenuse_points[1]
key_point = calculate_midpoint(hypotenuse_p1, hypotenuse_p2)
point_type = "Hypotenuse Midpoint"
cv2.circle(color_result, right_vertex, 10, (255, 0, 255), -1)
cv2.putText(color_result, "90°", (right_vertex[0] + 15, right_vertex[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 2)
cv2.line(color_result, hypotenuse_p1, hypotenuse_p2, (0, 255, 255), 2)
else:
M_moments = cv2.moments(cnt)
if M_moments["m00"] == 0:
continue
key_point = (int(M_moments["m10"] / M_moments["m00"]), int(M_moments["m01"] / M_moments["m00"]))
point_type = "Center"
# 计算实际坐标和机械臂角度
M, _ = get_perspective_matrices(user_green_box)
real_x, real_y = pixel_to_real(key_point[0], key_point[1], M)
real_coord = (round(real_x, 2), round(real_y, 2))
robot_angles = calculate_robot_angles(real_coord)
ctrl_vals, hex_frames = calculate_values(*robot_angles), [] # 控制值
# 生成唯一ID：前两位是颜色和形状代码，后两位是序号
current_count = counters[color][shape]
color_code = COLOR_CODE[color]
shape_code = SHAPE_CODE[shape]
unique_id = f"{color_code}{shape_code}{current_count:02d}"
counters[color][shape] += 1
shapes.append((
shape, key_point, real_coord, tilt_angle,
point_type, unique_id, robot_angles,
ctrl_vals, hex_frames, color # 添加颜色信息作为第9个元素
))
# 绘制识别结果
color_bgr = {
"Red": (0, 0, 255),
"Yellow": (0, 255, 255),
"Blue": (255, 0, 0),
"Black": (0, 0, 0)
}[color]
# 在当前颜色的临时结果图像上绘制轮廓和关键点
cv2.drawContours(color_result, [cnt], -1, color_bgr, 2)
for (x, y) in corners:
cv2.circle(color_result, (x, y), 5, (255, 0, 0), -1)
cv2.circle(color_result, key_point, 7, (0, 255, 0), -1)
cv2.putText(color_result, unique_id, (key_point[0] + 15, key_point[1] - 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
coord_text = f"Mid:{key_point}" if shape == "Right Triangle" else f"Center:{key_point}"
cv2.putText(color_result, coord_text, (key_point[0] + 15, key_point[1] + 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 255), 1)
cv2.putText(color_result, f"Angle:{tilt_angle}°", (key_point[0] + 15, key_point[1] + 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 255), 1)
angle_texts = [f"a0:{robot_angles[0]}°", f"a1:{robot_angles[1]}°",
f"a2:{robot_angles[2]}°", f"a3:{robot_angles[3]}°"]
for i, text in enumerate(angle_texts):
cv2.putText(color_result, text, (key_point[0] + 15, key_point[1] + 50 + i * 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 0), 1)
# 绘制固定点连线
fixed_point_pixel = calculate_fixed_point(user_green_box)
cv2.line(color_result, key_point, fixed_point_pixel, (255, 0, 0), 1)
# 将当前颜色的识别结果叠加到最终图像上
color_mask_bgr = cv2.cvtColor(color_mask, cv2.COLOR_GRAY2BGR)
final_result = np.where(color_mask_bgr > 0, color_result, final_result)
# 使用合并后的最终结果
result = final_result
# 分析每个区域
labels = np.unique(markers)
for label in labels:
if label in [0, 1, -1]:
continue
region_mask = (markers == label).astype(np.uint8) * 255
contours, _ = cv2.findContours(region_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
continue
cnt = max(contours, key=cv2.contourArea)
if cv2.contourArea(cnt) < 500:
continue
corners, corner_count = get_robust_corners(cnt)
shape = get_shape_by_corners(corner_count)
tilt_angle = calculate_tilt_angle(cnt, shape, corners)
# 确定关键坐标
if shape == "Right Triangle" and corner_count == 3:
angles = []
for i in range(3):
p0 = corners[i]
p1_corner = corners[(i + 1) % 3]
p2_corner = corners[(i + 2) % 3]
v1 = np.array(p0) - np.array(p1_corner)
v2 = np.array(p2_corner) - np.array(p1_corner)
dot = np.dot(v1, v2)
norm = np.linalg.norm(v1) * np.linalg.norm(v2)
angle_deg = np.degrees(np.arccos(dot / norm)) if norm != 0 else 0
angles.append((angle_deg, p1_corner))
right_vertex = min(angles, key=lambda x: abs(x[0] - 90))[1]
hypotenuse_points = [p for p in corners if p != right_vertex]
hypotenuse_p1, hypotenuse_p2 = hypotenuse_points[0], hypotenuse_points[1]
key_point = calculate_midpoint(hypotenuse_p1, hypotenuse_p2)
point_type = "Hypotenuse Midpoint"
cv2.circle(result, right_vertex, 10, (255, 0, 255), -1)
cv2.putText(result, "90°", (right_vertex[0] + 15, right_vertex[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 2)
cv2.line(result, hypotenuse_p1, hypotenuse_p2, (0, 255, 255), 2)
else:
M_moments = cv2.moments(cnt)
if M_moments["m00"] == 0:
continue
key_point = (int(M_moments["m10"] / M_moments["m00"]), int(M_moments["m01"] / M_moments["m00"]))
point_type = "Center"
# 计算实际坐标和机械臂角度
M, _ = get_perspective_matrices(user_green_box)
real_x, real_y = pixel_to_real(key_point[0], key_point[1], M)
real_coord = (round(real_x, 2), round(real_y, 2))
robot_angles = calculate_robot_angles(real_coord)
ctrl_vals, hex_frames = calculate_values(*robot_angles), [] # 控制值
# 生成唯一ID：前两位是颜色和形状代码，后两位是序号
current_count = counters[color][shape]
color_code = COLOR_CODE[color]
shape_code = SHAPE_CODE[shape]
unique_id = f"{color_code}{shape_code}{current_count:02d}"
counters[color][shape] += 1
shapes.append((
shape, key_point, real_coord, tilt_angle,
point_type, unique_id, robot_angles,
ctrl_vals, hex_frames, color # 添加颜色信息作为第9个元素
))
# 绘制识别结果
color_bgr = {
"Red": (0, 0, 255),
"Yellow": (0, 255, 255),
"Blue": (255, 0, 0),
"Black": (0, 0, 0)
}[color]
cv2.drawContours(result, [cnt], -1, color_bgr, 2)
for (x, y) in corners:
cv2.circle(result, (x, y), 5, (255, 0, 0), -1)
cv2.circle(result, key_point, 7, (0, 255, 0), -1)
cv2.putText(result, unique_id, (key_point[0] + 15, key_point[1] - 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
# coord_text = f"Mid:{key_point}" if shape == "Right Triangle" else f"Center:{key_point}"
# cv2.putText(result, coord_text, (key_point[0] + 15, key_point[1] + 10),
# cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 255), 1)
# cv2.putText(result, f"Angle:{tilt_angle}°", (key_point[0] + 15, key_point[1] + 30),
# cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 255), 1)
# angle_texts = [f"a0:{robot_angles[0]}°", f"a1:{robot_angles[1]}°",
# f"a2:{robot_angles[2]}°", f"a3:{robot_angles[3]}°"]
# for i, text in enumerate(angle_texts):
# cv2.putText(result, text, (key_point[0] + 15, key_point[1] + 50 + i * 20),
# cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 0), 1)
# # 绘制固定点连线
fixed_point_pixel = calculate_fixed_point(user_green_box)
cv2.line(result, key_point, fixed_point_pixel, (255, 0, 0), 1)
# 匹配预设的目的地（包含a9信息）
assigned_locations = {}
for color in color_order:
for shape in shape_order:
shape_items = [s for s in shapes if s[0] == shape and s[9] == color]
dests = shape_destinations[color][shape] # 每个元素是{"number": 编号, "a9": 角度}
if not dests:
for item in shape_items:
assigned_locations[item[5]] = {"number": "未分配", "a9": 0.0}
continue
sorted_items = sorted(shape_items, key=lambda x: x[1][1]) # 按y坐标排序
if len(sorted_items) > len(dests):
print(
f"警告：{COLOR_NAMES[color]}{SHAPE_NAMES[shape]}识别数量（{len(sorted_items)}）多于预设目的地数量（{len(dests)}）")
elif len(sorted_items) < len(dests):
print(
f"警告：{COLOR_NAMES[color]}{SHAPE_NAMES[shape]}识别数量（{len(sorted_items)}）少于预设目的地数量（{len(dests)}）")
# 分配目的地和a9角度
for i in range(min(len(sorted_items), len(dests))):
unique_id = sorted_items[i][5]
assigned_locations[unique_id] = {
"number": dests[i]["number"],
"a9": dests[i]["a9"]
}
# 未分配的形状
for i in range(len(dests), len(sorted_items)):
unique_id = sorted_items[i][5]
assigned_locations[unique_id] = {"number": "未分配（数量超出）", "a9": 0.0}
# 准备显示处理后图像
cv2.putText(result, f"{distance_threshold:.2f}", (20, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
cv2.imwrite("detection_result.png", result)
scaled_result = cv2.resize(result, (int(result.shape[1] * 0.6), int(result.shape[0] * 0.6)))
cv2.imshow("Detection Result (按N输出结果，按L重新识别，ESC退出)", scaled_result)
print(f"\n图像处理完成，当前距离变换阈值：{distance_threshold:.2f}（按U/D调整）")
print("显示处理结果中...（按N输出结果，按L重新识别，ESC退出）")
# 处理后图像的操作等待循环
while True:
key = cv2.waitKey(0)
# 调整阈值（更新显示）
if key == ord('u') or key == ord('U'):
if distance_threshold < threshold_max:
distance_threshold += threshold_step
distance_threshold = round(distance_threshold, 2)
print(f"距离变换阈值已增加到：{distance_threshold:.2f}")
updated_result = result.copy()
cv2.putText(updated_result, f"{distance_threshold:.2f}", (20, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
scaled_result = cv2.resize(updated_result,
(int(updated_result.shape[1] * 0.6), int(updated_result.shape[0] * 0.6)))
cv2.imshow("Detection Result (按N输出结果，按L重新识别，ESC退出)", scaled_result)
else:
print("已达到最大阈值（0.80）")
elif key == ord('d') or key == ord('D'):
if distance_threshold > threshold_min:
distance_threshold -= threshold_step
distance_threshold = round(distance_threshold, 2)
print(f"距离变换阈值已减少到：{distance_threshold:.2f}")
updated_result = result.copy()
cv2.putText(updated_result, f"{distance_threshold:.2f}", (20, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
scaled_result = cv2.resize(updated_result,
(int(updated_result.shape[1] * 0.6), int(updated_result.shape[0] * 0.6)))
cv2.imshow("Detection Result (按N输出结果，按L重新识别，ESC退出)", scaled_result)
else:
print("已达到最小阈值（0.10）")
# 按n键：输出结果（包含a9角度）
elif key == ord('n') or key == ord('N'):
print("\n==== 最终放置指导 ====")
for (shape, key_point, real_coord, tilt_angle, point_type,
unique_id, robot_angles, ctrl_vals, hex_frames, color) in shapes:
color_name = COLOR_NAMES[color]
dest_info = assigned_locations.get(unique_id, {"number": "未分配", "a9": 0.0})
dest_number = dest_info["number"]
a9 = dest_info["a9"] # 获取a9角度
a0, a1, a2, a3 = robot_angles
a6 = 0.0
closest_w = None
closest_dist = float('inf')
# 计算a7角度（根据分配的目的地）
a7 = 0.0
if isinstance(dest_number, int) and 1 <= dest_number <= 24:
a7 = DEST_ANGLES[dest_number - 1]
# 计算a6角度（使用标记的白点）
if shape in ["Hexagon", "Circle"]:
for i, (px, py) in enumerate(click_points):
dist = np.hypot(px - key_point[0], py - key_point[1])
if dist < closest_dist and dist < ANGLE_CALCULATION_THRESHOLD:
closest_dist = dist
closest_w = i + 1
if closest_w is not None:
px, py = click_points[closest_w - 1]
x = px - key_point[0]
y = key_point[1] - py
angle_rad = np.arctan2(y, x)
a6 = np.degrees(angle_rad) % 180
if a6 < 0:
a6 += 180
a6 = round(a6, 1)
else:
a6 = 0.0
# 计算a5角度
if shape in ["Square", "Right Triangle"]:
a5 = tilt_angle - a0 + 270
else:
if closest_w is not None:
a5 = a6 - a0 + 270
else:
a5 = tilt_angle - a0 + 270
# 角度归一化
a5 = a5 % 360
if a5 > 180:
a5 -= 360
elif a5 <= -180:
a5 += 360
a5 = round(a5, 1)
# 计算a8角度
a8 = round(a5 - a7 + a9, 1) # 加入a9角度调整
if a8 > 180:
a8 -= 360
elif a8 <= -180:
a8 += 360
# 存储a8值
a8_values[unique_id] = a8
# 输出结果（包含颜色和a9角度）
print(f"物块编号{unique_id}：{color_name}{SHAPE_NAMES[shape]}")
print(f" 实际坐标：{real_coord} 厘米")
print(f" 倾斜角度：{tilt_angle}°")
print(f" 目的地：{dest_number}，a9角度：{a9}°") # 显示a9角度
print(f" 机械臂角度：a0={a0}°, a1={a1}°, a2={a2}°, a3={a3}°")
print(f" a5角度：{a5}°")
if closest_w is not None:
print(f" a6角度（与最近白点W{closest_w}连线）：{a6}°")
else:
print(f" a6角度：未计算（附近无有效白点）")
print(f" a7角度（目的地角度）：{a7:.1f}°")
print(f" a8角度（a5 - a7 + a9，调整后）：{a8:.1f}°")
print(f" 控制值：{ctrl_vals}")
print(" --------------------")
# 关闭处理结果窗口
cv2.destroyWindow("Detection Result (按N输出结果，按L重新识别，ESC退出)")
# 提示用户输入命令
print("\n请输入命令（'m'发送指令，'l'重新识别，'esc'退出）：")
while True:
user_input = input(">>> ").strip().lower()
if user_input == 'm':
print("\n----- 准备发送指令 -----")
send_all_commands(assigned_locations, a8_values)
print("\n请输入命令（'m'重新发送指令，'l'重新识别，'esc'退出）：")
elif user_input == 'l':
print("\n重新开始识别...")
break # 跳出终端输入循环，回到实时预览
elif user_input in ['esc', 'exit', 'q']:
print("程序退出")
close_windows_and_camera()
return
else:
print("无效命令，请输入'm'发送指令，'l'重新识别，'esc'退出")
# 按l键：重新识别（重置状态，返回实时预览）
elif key == ord('l') or key == ord('L'):
print("\n重新开始识别...")
break # 跳出处理后操作循环，回到实时预览
# 按ESC键：退出程序
elif key == 27:
close_windows_and_camera()
print("程序退出")
return
else:
print("无效按键，请按N（输出结果）、L（重新识别）或ESC（退出）")
if __name__ == "__main__":
main()
火山引擎开发者社区是火山引擎打造的AI技术生态平台，聚焦Agent与大模型开发，提供豆包系列模型（图像/视频/视觉）、智能分析与会话工具，并配套评测集、动手实验室及行业案例库。社区通过技术沙龙、挑战赛等活动促进开发者成长，新用户可领50万Tokens权益，助力构建智能应用。
更多推荐
30
0- 0
已为社区贡献2条内容
所有评论(0)