相机外参自动标定简单思路

一、解决实验室现有问题

问题描述：

1.在相机坐标系来标定像素坐标的时候是手动查找相机里的九点，并且将记录的九点自己录入来进行矩阵计算，操作流程比较不方便。

2.进行机器人世界坐标系标定时需要使用示教器对准每个点进行坐标记录，然后将记录的坐标与像素九点坐标进行矩阵计算，同样十分不方便。

3.目前缺少一种对外参标定误差的评判准则，需要对外参标定误差进行评判。（杨师兄提出，但目前我还没去做，后面如果完善了会来补全这一块）

解决方案：

①使相机对九点标定框进行识别，视觉找到需要标定的九个点，并自动保存坐标像素坐标到config.里，并且可以实时更新。

②规定好机器人的九点运动程序，并且记录第一个点的坐标，使每次机器人都能按照特定的程序运行，然后根据运行的步骤来摆放标定板。待实现九个点的标定后，将世界坐标保存到config中。

③ 使用重投影技术来对外参的标定误差进行评判。

二、解决的思路规划

<1>.像素坐标的获取（目前的效果不是很好，但是，代码还是有改进的空间，目前只展示部分关于识别的代码）：

1绘制标定板并确定标定板的尺寸

2对获取的图像进行预处理：

      灰度图、自适应阈值、形态学操作、去噪

3查找九点的函数：

       过滤小轮廓、近似多边形查找、获取四个顶点、获取其余四个中心点、计算中点、绘制出第一个找到的九点。

4.双击事件回调：

      双击屏幕则保存目前像素帧里的坐标并保存。下面为识别到的点的图像，其中红色为第一个点的坐标从左往右从上到下依次排列。

        其中查找九点函数思路如下图所示：

# 查找九个点函数
def find_nine_points(image):
    thresh = preprocess_image(image)
    contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    corners_matrix = []

    for cnt in contours:
        # 过滤小轮廓
        if cv2.contourArea(cnt) < 1000:  # 这里的100可以根据需要调整
            continue

        # 近似多边形
        epsilon = 0.02 * cv2.arcLength(cnt, True)
        approx = cv2.approxPolyDP(cnt, epsilon, True)

        if len(approx) == 4:
            # 获取四个顶点
            p0, p1, p2, p3 = approx.reshape(4, 2)

            # 计算中点和中心点
            points = [
                p0,
                ((p0 + p1) // 2).astype(int),
                p1,
                ((p1 + p2) // 2).astype(int),
                p2,
                ((p2 + p3) // 2).astype(int),
                p3,
                ((p3 + p0) // 2).astype(int),
                ((p0 + p2) // 2).astype(int)
            ]

            corners_matrix.append(points)
            # 只绘制第一个找到的九点
            for i in range(len(points)):
                point = (int(points[i][0]), int(points[i][1]))
                cv2.circle(image, point, 5, points_color[i], -1)  # -1 填充圆形

            break  # 找到第一个九点后，停止搜索

    return corners_matrix

<2>.世界坐标的获取（主要是学习python代码控制机械臂的运动，然后给定九个点，使得机械臂能够安装九个特定的点移动到特定的位置，其中特定点的位置的世界坐标已经保存在了config文件里，可以调用进行矩阵运算。）

1.机械臂标定流程

        直接给定特定的运行程序走规定的路径，走到第一个点后对标定板的中点进行校准，然后走第二个点，对标定板的第二个点进行校准，然后观察机械臂对第三个点到第九个点是否标准。

        下面是机器人的参数设置以及一些简单的控制机械臂移动的代码：

# 设置机器人的参数
# 启用机器人事件（如状态更新等）和初始化配置文件。
# 设置机器人的关节最大速度和加速度，这些设置对机器人的运动性能有影响。
robot.enable_robot_event()
robot.init_profile()
# v（6个关节）
joint_maxvelc = (1, 1, 1, 1, 1, 1)
# a (6个关节)
joint_maxacc = (
    17 / 2.5, 17/ 2.5, 17/ 2.5, 17/ 2.5, 17/ 2.5, 17/ 2.5)
# joint_maxacc = (
# 17.308779 / 2.5, 17.308779 / 2.5, 17.308779 / 2.5, 17.308779 / 2.5, 17.308779 / 2.5, 17.308779 / 2.5)
# # 调用 set_joint_maxacc() 和 set_joint_maxvelc() 来设置各个关节的最大加速度和最大速度。
robot.set_joint_maxacc(joint_maxacc)
robot.set_joint_maxvelc(joint_maxvelc)
# # 调用 set_arrival_ahead_blend() 设置到达目标位置前的提前混合值，用于平滑运动。
robot.set_arrival_ahead_blend(0.05)

# 初始化全局配置文件
# 通过调用 init_profile() 初始化机械臂的配置文件。这可能涉及到读取配置文件或设置一些全局的控制参数。
robot.init_profile()


# 获取当前位置
# 获取机械臂当前的位置信息（当前的工作坐标或关节角度）。get_current_waypoint() 方法可能返回一个包含位置信息的字典对象。
current_pos = robot.get_current_waypoint()
logger.info(f"Current waypoint: {current_pos}")

# 轴动到初始位置
# 设定一个初始的关节角度，表示机械臂的初始位置。通过调用 move_joint(joint_radian) 方法，机械臂将移动到这些角度的位置。
#移动到初始位姿
angles = (-0.000172, -8.423339, -81.027975, 17.378440, -89.999982, -0.000458)
joint_radian = degrees_to_radians(angles)
robot.move_joint(joint_radian)
# 移动到0位姿
joint_radian = (0, 0, 0, 0, 0, 0)
robot.move_joint(joint_radian)
# 移动到初始位置
angles = (-0.000172, -8.423339, -81.027975, 17.378440, -89.999982, -0.000458)
joint_radian = degrees_to_radians(angles)
robot.move_joint(joint_radian)
# 移动到工作位置
robot.move_joint(joint_radian)
angles = (1.912307, 51.715443, -83.964158, -45.679663, -89.999784, 1.912059)
joint_radian = degrees_to_radians(angles)
robot.move_joint(joint_radian)  

三、总结

        关于识别九点的学习：通过学习使用双击鼠标回调、在config文件夹下写入以及读取txt文件、以及各种数据格式的处理，实现了一个识别精度一般的自动化程序，后面可根据不断的调整，使找到九点的函数的精度更高。

        关于机械臂控制代码的学习：学习了简单的ip通讯，使电脑能够连接机械臂等通讯原理；以及一些机械臂的控制代码的学习。