ROS2机器人手眼标定原理与OpenCV手眼标定（calibrateHandeye())

只要你按照我的步骤走应该做出来不是问题！！！

完整的python源代码！！！！！！！！！！！！！！！！！！！全部开源讲解步骤详细！！！！！

一、标定原理

机器人手眼标定分为eye in hand与eye to hand两种。

介绍之前进行变量定义说明：对于 Eye-in-hand 手眼标定方式，需要求解工业机器人的末端坐标系与相机坐标系之间的坐标转换关系。 Eye-in-hand 手眼标定的原理示意图如图 1所示。这其中有几个坐标系， 基础坐标系（用 base 表示） 是机器臂的基底坐标系，末端坐标系（用 end 表示） 是机器臂的末端坐标系， 相机坐标系（用 cam 表示） 是固定在机器臂上面的相机自身坐标系，标定物坐标系（用 cal 表示）是标定板所在的坐标系。任意移动两次机器臂，由于标定板和机器臂的基底是不动的，因此对于某个世界点，其在 base 坐标系和 cal 坐标系下的坐标值不变，在 end 坐标系和 cam 坐标系下的坐标值随着机器臂的运动而改变。根据这一关系，可以求解出end坐标系和 cam 坐标系之间的转换矩阵。具体求解过程如下：
{b}: base基坐标系

{g}: end夹具坐标系

{t}: cal标定板坐标系

{c}: camera相机坐标系

本文主要只讲解眼在手上，以及眼在手上的代码实现

眼在手上(eye in hand)

眼在手上，相机固定在机器人上。

重点讲代码实现

calibrateHandeye()
参数描述如下：R_gripper2base，t_gripper2base是机械臂抓手相对于机器人基坐标系的旋转矩阵与平移向量，需要通过机器人运动控制器或示教器读取相关参数转换计算得到；R_target2cam， t_target2cam 是标定板相对于相机的齐次矩阵，在进行相机标定时可以求取得到（calibrateCamera()得到）,也可以通过solvePnP()单独求取相机外参数获得 ； R_cam2gripper ，t_cam2gripper是求解的手眼矩阵分解得到的旋转矩阵与平移矩阵；OpenCV实现了5种方法求取手眼矩阵 ， Tsai两步法速度最快。

以上的参数无所就四个前面两个分别是基于机械臂base到爪子的end的旋转与平移矩阵。然后就是每次拍照的相机的每次外惨P的旋转与平移矩阵。

获取这四个参数 前面两个参数可以每次记录机械臂的移动的位姿就可以算出，后面的两个参数是计算每次拍照的外惨P。

实际开始操作

一准备标定纸

免费的棋盘格标定板生成网址： https://calib.io/pages/camera-calibration-pattern-generator

行列分别是8，11 则此标定纸规格为8*11并且其中每个小格子里面大小为10mm，使用a4白纸打印出来就可。

拍照准备

由于是相机开启是以视频流的形式所以需要写一个拍照的功能包

cd ~
mkdir paizhao/src
cd paizhao/src
运行：
ros2 pkg create python_img --build-type ament_python --dependencies rclpy cv_bridge sensor_msgs vision_msgs cv2 --license Apache-2.0 --node-name  img_huoqu
1
2
3
4
5

会出现以上的功能包python_img

cd ~/paizhao/src/python_img/python_img
1

将以下代码复制到img_huoqu.py即可

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class ImageListener(Node):

    def __init__(self):
        super().__init__('image_listener')
        self.subscription = self.create_subscription(
            Image,
            'camera/color/image_raw',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

        self.bridge = CvBridge()
        self.count = 0

    def listener_callback(self, data):
        self.get_logger().info('Receiving video frame')
        cv_image = self.bridge.imgmsg_to_cv2(data, desired_encoding='bgr8')
        
        # 显示图像
        cv2.imshow("Image Window", cv_image)
        
        # 按下“t”键保存图像
        if cv2.waitKey(1) & 0xFF == ord('t'):
            self.count += 1
            filename = f'image_{self.count}.bmp'
            cv2.imwrite(filename, cv_image)
            self.get_logger().info(f'Image saved: {filename}')

def main(args=None):
    rclpy.init(args=args)

    image_listener = ImageListener()

    rclpy.spin(image_listener)

    # 销毁窗口
    cv2.destroyAllWindows()
    
    # 关闭节点
    image_listener.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

编译

cd ~/paizhao
colcon build 
1
2

运行

cd ~/paizhao
. ./install/setup.bash
ros2 run python_img img_huoqu
1
2
3

运行前需要首先启动相机 见本系列的第一篇文章，启动后会出现实时的视频图片按t时就会保存当前照片保存在同src目录。

就可以开始启动机械臂与相机进行标定了！！！

首先固定机械臂的base与标定纸不动，然后进行移动改变机械臂进行拍照，拍标定纸，每次拍的标定纸需要拍全，不然后面角点检测不能检测出所有角点，就造成浪费拍照,每次拍照需要记录机械臂的位姿，以及拍的照片与机械臂位置的对应，因为后面需要挑选出来检测不出来的角点照片，将其抛弃。

其中一次的机械臂的位姿，后续需要做成一个excel表格存储数据。

不同位置拍摄到的标定白纸

jiaodian.py角点检测 cv2.findChessboardCorners（）我们使用这个函数来判断我们拍到的图片是否每张都可以检测出角点。检测不出所有的角点则是无效的图片。

import cv2
import numpy as np


def find_chessboard_corners(image, pattern_size):
    """
    在图像中查找棋盘格的角点。

    :param image: 要检测的棋盘格图像。
    :param pattern_size: 棋盘格的内部角点数量，格式为(row, col)。
    :return: 是否找到角点，角点列表。
    """
    # 将图像转换为灰度
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    cv2.imshow('gray',gray)
    cv2.waitKey(0)

    # 寻找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    corners_subpixel = None

    # 如果找到了足够的角点，将其细化并绘制
    if ret:
        # 获取亚像素精度的角点位置
        corners_subpixel = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
                                          (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1))
        # 绘制角点
        cv2.drawChessboardCorners(image, pattern_size, corners_subpixel, ret)

    return ret, corners_subpixel


# 使用示例
if __name__ == '__main__':
    # 加载图像
    image = cv2.imread('/home/zy/temp/image_3.bmp')
    # cv2.imshow('first', image)
    # cv2.waitKey(0)

    # 检查图像是否成功加载
    if image is None:
        print("Could not open or find the image.")
        exit()

    # 设置棋盘格的内部角点数量，例如一个10x7的棋盘格
    pattern_size = (7, 10)

    # 查找并绘制角点
    found, corners = find_chessboard_corners(image, pattern_size)

    if found:
        print("Chessboard corners found.")
        print(corners.shape)
        cv2.imshow('Chessboard Corners', image)
        key = cv2.waitKey(0)
        if key & 0xff == ord('q'):
            cv2.destroyAllWindows()
    else:
        print("Chessboard corners not found.")

    # cv2.destroyAllWindows()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

检测成功则可以画出检测出来的角点。（只要拍全了一般都可以检测出来）

容易出错的点 角点检测还记得我之前的标定纸的规格是（8，11）即是8行11列，但是输如进角点检测的 pattern_size=（规格行数-1，规格列数-1）！！！！！当时卡了好久！！不信的可以试试源代码改为（8，11）指定是反馈没找到对应的角点。

开始计算calibrateHandeye()

将前面提到的机械臂的位姿存入表格里面，格式数据ax-ay是存放的translation单位为mm与da-dy存放的是角度

计算相机相对于末端的变换矩阵源代码：

import cv2
import numpy as np
import glob
from math import *
import pandas as pd
import os

K = np.array([[360.2007751464844, 0, 323.737548828125],
              [0, 360.2007751464844, 176.65692138671875],
              [0, 0, 1]], dtype=np.float64)  #大恒相机内参
chess_board_x_num = 7  #棋盘格x方向格子数
chess_board_y_num = 10  #棋盘格y方向格子数
chess_board_len = 10  #单位棋盘格长度,mm


#用于根据欧拉角计算旋转矩阵
def myRPY2R_robot(x, y, z):
    Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
    Ry = np.array([[cos(y), 0, sin(y)], [0, 1, 0], [-sin(y), 0, cos(y)]])
    Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
    R = Rz @ Ry @ Rx
    return R


#用于根据位姿计算变换矩阵
def pose_robot(x, y, z, Tx, Ty, Tz):
    thetaX = x / 180 * pi
    thetaY = y / 180 * pi
    thetaZ = z / 180 * pi
    R = myRPY2R_robot(thetaX, thetaY, thetaZ)
    t = np.array([[Tx], [Ty], [Tz]])
    RT1 = np.column_stack([R, t])  # 列合并
    RT1 = np.row_stack((RT1, np.array([0, 0, 0, 1])))
    # RT1=np.linalg.inv(RT1)
    return RT1


#用来从棋盘格图片得到相机外参
def get_RT_from_chessboard(img_path, chess_board_x_num, chess_board_y_num, K, chess_board_len):
    '''
    :param img_path: 读取图片路径
    :param chess_board_x_num: 棋盘格x方向格子数
    :param chess_board_y_num: 棋盘格y方向格子数
    :param K: 相机内参
    :param chess_board_len: 单位棋盘格长度,mm
    :return: 相机外参
    '''
    img = cv2.imread(img_path)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (chess_board_x_num , chess_board_y_num ), None)
    # print(corners)
    corner_points = np.zeros((2, corners.shape[0]), dtype=np.float64)
    for i in range(corners.shape[0]):
        corner_points[:, i] = corners[i, 0, :]
    # print(corner_points)
    object_points = np.zeros((3, chess_board_x_num * chess_board_y_num), dtype=np.float64)
    flag = 0
    for i in range(chess_board_y_num):
        for j in range(chess_board_x_num):
            object_points[:2, flag] = np.array([(11 - j - 1) * chess_board_len, (8 - i - 1) * chess_board_len])
            flag += 1
    # print(object_points)

    retval, rvec, tvec = cv2.solvePnP(object_points.T, corner_points.T, K, distCoeffs=None)
    # print(rvec.reshape((1,3)))
    # RT=np.column_stack((rvec,tvec))
    RT = np.column_stack(((cv2.Rodrigues(rvec))[0], tvec))
    RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    # RT=pose(rvec[0,0],rvec[1,0],rvec[2,0],tvec[0,0],tvec[1,0],tvec[2,0])
    # print(RT)

    # print(retval, rvec, tvec)
    # print(RT)
    # print('')
    return RT


folder = "/home/zy/temp"  #棋盘格图片存放文件夹
# files = os.listdir(folder)
# file_num=len(files)
# RT_all=np.zeros((4,4,file_num))

# print(get_RT_from_chessboard('calib/2.bmp', chess_board_x_num, chess_board_y_num, K, chess_board_len))
'''
这个地方很奇怪的特点，有些棋盘格点检测得出来，有些检测不了，可以通过函数get_RT_from_chessboard的运行时间来判断
'''
good_picture = [3, 4, 5, 6, 7, 8, 9, 10, 11, 12]  #存放可以检测出棋盘格角点的图片
# good_picture=[1,3,10,11,12]
file_num = len(good_picture)

#计算board to cam 变换矩阵
R_all_chess_to_cam_1 = []
T_all_chess_to_cam_1 = []
for i in good_picture:
    # print(i)
    image_path = folder + '/image_' + str(i) + '.bmp'
    RT = get_RT_from_chessboard(image_path, chess_board_x_num, chess_board_y_num, K, chess_board_len)

    # RT=np.linalg.inv(RT)

    R_all_chess_to_cam_1.append(RT[:3, :3])
    T_all_chess_to_cam_1.append(RT[:3, 3].reshape((3, 1)))
# print(T_all_chess_to_cam.shape)

#计算end to base变换矩阵
file_address = '/home/zy/temp/A.xlsx'  #从记录文件读取机器人六个位姿
sheet_1 = pd.read_excel(file_address)
R_all_end_to_base_1 = []
T_all_end_to_base_1 = []
# print(sheet_1.iloc[0]['ax'])
good_picture_1=[0,1,2,3,4,5,6,7,8,9,]
for i in good_picture_1:
    # print(sheet_1.iloc[i-1]['ax'],sheet_1.iloc[i-1]['ay'],sheet_1.iloc[i-1]['az'],sheet_1.iloc[i-1]['dx'],
    #                                   sheet_1.iloc[i-1]['dy'],sheet_1.iloc[i-1]['dz'])
    RT = pose_robot(sheet_1.iloc[i - 1]['ax'], sheet_1.iloc[i - 1]['ay'], sheet_1.iloc[i - 1]['az'],
                    sheet_1.iloc[i - 1]['dx'],
                    sheet_1.iloc[i - 1]['dy'], sheet_1.iloc[i - 1]['dz'])
    # RT=np.column_stack(((cv2.Rodrigues(np.array([[sheet_1.iloc[i-1]['ax']],[sheet_1.iloc[i-1]['ay']],[sheet_1.iloc[i-1]['az']]])))[0],
    #                    np.array([[sheet_1.iloc[i-1]['dx']],
    #                                   [sheet_1.iloc[i-1]['dy']],[sheet_1.iloc[i-1]['dz']]])))
    # RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
    # RT = np.linalg.inv(RT)

    R_all_end_to_base_1.append(RT[:3, :3])
    T_all_end_to_base_1.append(RT[:3, 3].reshape((3, 1)))

# print(R_all_end_to_base_1)
R, T = cv2.calibrateHandEye(R_all_end_to_base_1, T_all_end_to_base_1, R_all_chess_to_cam_1, T_all_chess_to_cam_1)  #手眼标定
RT = np.column_stack((R, T))
RT = np.row_stack((RT, np.array([0, 0, 0, 1])))  #即为cam to end变换矩阵
print('相机相对于末端的变换矩阵为：')
print(RT)

#结果验证，原则上来说，每次结果相差较小
for i in range(len(good_picture)):
    RT_end_to_base = np.column_stack((R_all_end_to_base_1[i], T_all_end_to_base_1[i]))
    RT_end_to_base = np.row_stack((RT_end_to_base, np.array([0, 0, 0, 1])))
    # print(RT_end_to_base)

    RT_chess_to_cam = np.column_stack((R_all_chess_to_cam_1[i], T_all_chess_to_cam_1[i]))
    RT_chess_to_cam = np.row_stack((RT_chess_to_cam, np.array([0, 0, 0, 1])))
    # print(RT_chess_to_cam)

    RT_cam_to_end = np.column_stack((R, T))
    RT_cam_to_end = np.row_stack((RT_cam_to_end, np.array([0, 0, 0, 1])))
    # print(RT_cam_to_end)

    RT_chess_to_base = RT_end_to_base @ RT_cam_to_end @ RT_chess_to_cam  #即为固定的棋盘格相对于机器人基坐标系位姿
    RT_chess_to_base = np.linalg.inv(RT_chess_to_base)
    print('第', i, '次')
    print(RT_chess_to_base[:3, :])
    print('')

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154

k是自己相机的内参参考本专题的第一篇文章

chess_board_x_num = 7 #棋盘格x方向格子数
chess_board_y_num = 10 #棋盘格y方向格子数
chess_board_len = 10 #单位棋盘格长度,mm

K = np.array([[360.2007751464844, 0, 323.737548828125],
              [0, 360.2007751464844, 176.65692138671875],
              [0, 0, 1]], dtype=np.float64)  #相机内参
标定纸的规格（8*11）每个小单位大小是10mm之前说过
chess_board_x_num = 7  #棋盘格x方向格子数 
chess_board_y_num = 10  #棋盘格y方向格子数
chess_board_len = 10  #单位棋盘格长度,mm
good_picture_1=[0,1,2,3,4,5,6,7,8,9,] #是你有多少行的excel的数据
1
2
3
4
5
6
7
8
9

输出

/home/zy/anaconda3/envs/lableimg/bin/python3.8 /home/zy/ZY/Python/Temp/rt.py 
相机相对于末端的变换矩阵为：
[[  0.63862265   0.08529824   0.76477796  51.26101178]
 [  0.14605989   0.96233497  -0.22929875 -23.83712011]
 [ -0.75553135   0.25813876   0.60211025 -16.68651879]
 [  0.           0.           0.           1.        ]]
第 0 次
[[ 8.09165804e-01  5.32251040e-01 -2.48916719e-01 -1.28822056e+02]
 [-6.77023672e-02  5.05263185e-01  8.60305471e-01 -9.99946717e+01]
 [ 5.83666936e-01 -6.79277516e-01  4.44876347e-01 -3.93351031e+02]]

第 1 次
[[ 1.17769749e-01  7.87851505e-01 -6.04500035e-01 -8.46698165e+00]
 [ 2.94972379e-01  5.53507106e-01  7.78858895e-01 -1.00539764e+02]
 [ 9.48220218e-01 -2.70036830e-01 -1.67208040e-01 -3.94339956e+02]]

第 2 次
[[-5.71028866e-01  8.07200351e-01 -1.49511297e-01  5.86255553e+01]
 [ 4.58459627e-01  4.64642871e-01  7.57576249e-01 -1.38566749e+02]
 [ 6.80985172e-01  3.64053013e-01 -6.35393264e-01 -2.34357678e+02]]

第 3 次
[[-4.23638255e-01  5.12154207e-01 -7.47147038e-01 -5.31925717e+01]
 [ 7.06147180e-01  7.03332427e-01  8.17291639e-02  1.14924972e+02]
 [ 5.67350675e-01 -4.92972174e-01 -6.59614772e-01 -1.09600469e+02]]

第 4 次
[[ 4.95857214e-01  6.47857770e-01 -5.78278422e-01  1.24190440e+02]
 [ 4.38150677e-01  3.88290772e-01  8.10712193e-01  2.54024309e+01]
 [ 7.49766369e-01 -6.55370572e-01 -9.13225401e-02 -9.88927414e+01]]

第 5 次
[[ 8.12065360e-01  4.66192129e-01 -3.51019587e-01  1.59286158e+02]
 [-5.91239090e-03  6.08049184e-01  7.93877341e-01 -3.24281884e+01]
 [ 5.83536542e-01 -6.42604924e-01  4.96531990e-01 -1.48777560e+02]]

第 6 次
[[ 8.10598990e-01  2.97702419e-01 -5.04284193e-01 -1.21642319e+02]
 [ 4.85286487e-01  1.40483574e-01  8.62995592e-01 -1.52217153e+02]
 [ 3.27759521e-01 -9.44265659e-01 -3.05951176e-02 -2.64163592e+02]]

第 7 次
[[ 8.43703499e-01 -2.48462721e-01 -4.75847330e-01 -1.49385842e+02]
 [ 5.25409896e-01  2.00506185e-01  8.26886758e-01 -1.63486422e+02]
 [-1.10040202e-01 -9.47662148e-01  2.99712542e-01 -2.11411259e+02]]

第 8 次
[[ 9.04377761e-01 -1.29452604e-01 -4.06623767e-01 -1.64552215e+02]
 [ 4.26356534e-01  3.14112838e-01  8.48264835e-01 -1.24995087e+02]
 [ 1.79156536e-02 -9.40518553e-01  3.39269629e-01 -2.73727354e+02]]

第 9 次
[[ 7.68795493e-01  4.03889006e-01 -4.95809601e-01 -1.05512710e+02]
 [ 2.02538477e-01  5.81619289e-01  7.87843365e-01 -7.70707092e+01]
 [ 6.06573701e-01 -7.06110950e-01  3.65343224e-01 -3.61701280e+02]]


进程已结束，退出代码为 0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

总结

只要你按照我的步骤走应该做出来不是问题！！！

还有就是感谢大佬的文章本文主要是基于http://t.csdnimg.cn/xMoAm这篇文章写的，欢迎大家讨论。