机械臂要想到达期望的位置，必须将其感知系统和机械臂运动产生联系，这关键的两步就是手眼标定和坐标系转换。按我所讲的步骤进行调试一定可以成功。
1.手眼标定
机械臂手眼标定目的是为了求得三个参数：机械臂末端位姿矩阵、末端与相机的变换矩阵以及相机到标定板的变换矩阵。其中，末端与相机的变换矩阵是求解的关键。机械臂的末端位姿矩阵可通过ROS订阅话题得出，相机到标定板的变换矩阵可通过外参标定得出，末端与相机的变换矩阵可通过AX=XB模型求出。
1.1 相机标定
相机标定是手眼标定的最先应进行的工作，目的是为了获取相机的内外参数，畸变矩阵。相机标定不仅可以用于机械臂手眼标定，还可以用于多个相机间的校准、对齐，实现多模态图像配准。
1.将标定板放置在距离机械臂一定距离处，距离不应太远。值得注意的是，标定板的横向格数一定不能等于纵向格数，否则不同图像中的同一个角点坐标并不对应，如下图为错误案例。
2.将realsense相机稳定固定在机械臂末端，打开realsense-viewer，通过施教模式控制机械臂到达某个位置，依次在终端输入下面命令可订阅机械臂的位姿，记录此时的机械臂四元数矩阵和realsense拍摄的画面。
rosrun moveit_commander moveit_commander_cmdline.py
use <group name>
current3.将四元数转换为机械臂旋转矩阵，如下代码
from scipy.spatial.transform import Rotation as R
Rq=[-0.756325124972, 0.269649470864,-0.54434743131, 0.24279073752] # 四元数
Rm = R.from_quat(Rq)
rotation_matrix = Rm.as_matrix() # 旋转矩阵
print('rotation:\n',rotation_matrix)4.将旋转矩阵和第2步得到的平移量记录为四行四列的齐次矩阵，并记录旋转矩阵和平移矩阵于RobotToolPose.csv中。
1.2 手眼标定
原理讲解（不想看的可以跳过）：
A：机器人末端在机械臂坐标系下的位姿，这其实就是机器人运动学正解的问题。（已知）。
B：相机在机器人末端坐标系下的位姿，这个变换是固定的，只要知道这个变换，我们就可以随时计算相机的实际位置，所以这就是我们想求的东西。（未知，待求）
C：相机在标定板坐标系下的位姿，这个其实就是求解相机的外参（已知）
------------------------------------------------------------------------------------------------------------------------------
话不多说，直接上代码
# coding=utf-8
# copied by ysh in 2021/12/08
"""
用于相机标定和相机的手眼标定
A2^{-1}*A1*X=X*B2*B1^{−1}
"""
import os.path
import cv2
import numpy as np
np.set_printoptions(precision=8,suppress=True)
import glob
path = "D:/hand_eye_image/"
# 角点的个数以及棋盘格间距
XX = 8
YY = 6
L = 0.03 # 格子大小
# 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)
# 获取标定板角点的位置
objp = np.zeros((XX * YY, 3), np.float32)
objp[:, :2] = np.mgrid[0:XX, 0:YY].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
objp = L*objp
obj_points = [] # 存储3D点
img_points = [] # 存储2D点
images = glob.glob('{}/*.png'.format(path))
print(images)
i = 0
for fname in images:
print(fname)
img = cv2.imread(fname)
# cv2.imshow('img',img)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (XX, YY), None)
print(ret)
if ret:
obj_points.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (3, 3), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点
#print(corners2)
if [corners2]:
img_points.append(corners2)
else:
img_points.append(corners)
cv2.drawChessboardCorners(img, (XX, YY), corners, ret)
# 红色为第一个点，蓝色为最后一个点，先X轴再Y轴
cv2.imwrite('{}/figure_save/{}.png'.format(path,i), img)
i = i+1
# cv2.imshow('img', img)
# cv2.waitKey(2000)
N = len(img_points)
print(f'图像个数：{N}')
# cv2.destroyAllWindows()
# 标定,得到图案在相机坐标系下的位姿
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)
# print("ret:", ret)
print("内参矩阵:\n", mtx) # 内参数矩阵
print("畸变系数:\n", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
print("rvecs:\n", rvecs) # 旋转向量 # 外参数
print("tvecs:\n", tvecs ) # 平移向量 # 外参数
print("-----------------------------------------------------")
# img2 = cv2.imread(f'{path}/figure/*.jpg')
i = 0
for fname in images:
figure = cv2.imread(fname)
h, w = figure.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),0,(w,h)) # 自由比例参数,用于去除畸变矫正后图像四周黑色的区域。alpha=0，则去除所有黑色区域，alpha=1，则保留所有原始图像像素，其他值则得到介于两者之间的效果。
dst = cv2.undistort(figure, mtx, dist, None, newcameramtx)
cv2.imwrite('{}/figure_undist/{}.png'.format(path,i), dst)
i = i + 1
# 机器人末端在基座标系下的位姿
tool_pose = np.loadtxt('{}/RobotToolPose.csv'.format(path),delimiter=',',encoding='utf-8-sig')
R_tool = []
t_tool = []
for i in range(int(N)):
R_tool.append(tool_pose[0:3,4*i:4*i+3])
t_tool.append(tool_pose[0:3,4*i+3])
R, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI) # R_tool, t_tool手爪相对于机器人基坐标系的旋转矩阵与平移向量；rvecs, tvecs标定板相对于双目相机的齐次矩阵
T_tool_camera = np.hstack((R, t)) # R,T就是相机到机械臂末端的旋转偏移矩阵
T_tool_camera = np.vstack((T_tool_camera, np.array([0,0,0,1])))
print(f'相机在机器人末端坐标系的位姿：\n{T_tool_camera}')
with open('{}/camera.txt'.format(path), 'w') as f:
f.write(f'{mtx}\n') # 内参矩阵
f.write(f'{dist}\n') # 畸变矩阵
f.write(f'{T_tool_camera}') # 相机到机械臂的外参矩阵
需要自己调整的参数有：标定板分别在XX,YY角点数量，标定板单位格的宽（高）L，存储地址path，机器人位姿RobotToolPose.csv。最后可以得到相机内参矩阵mtx，相机到机械臂的外参矩阵T_tool_camera。
附：手眼标定原理讲解http://t.csdn.cn/KoFM9
2. 像素-世界坐标系转换
完成手眼标定后，我们手中就有了相机内参矩阵mtx，相机到机械臂的外参矩阵T_tool_camera。接下来就是最后一步：坐标系转换。
原理讲解(不想看的可以跳过)：
（1）顺序变换
世界坐标系-相机坐标系
（1）
相机坐标系-像素坐标系
（2）
KI是内参矩阵，xc,yc,zc为相机坐标系坐标，u,v是像素坐标系坐标
像素坐标系到世界坐标系
（3）
u,v是像素坐标系坐标，xw yw zw是世界坐标系坐标，等式右第一个矩阵是内参矩阵，第二个矩阵是外参矩阵。但是不能直接乘，因为涉及到求得3*4矩阵无法求逆矩阵，所以不能直接用
，而是要用
。
将（1）变形：
（4）
将（2）带入（4）：
（2）直接变换
其中，XwYwZw为世界坐标系坐标，R为外参中的旋转矩阵，KI为内参矩阵，Zc为相机坐标系中的深度值，u，v为像素坐标
---------------------------------------------------------------------------------------------------------------------------------
话不多说，直接上代码
import numpy as np
matrixHand2Camera = np.array([[ 0.56186877, -0.82718502, -0.00827197, 0.00665606],
[ 0.82722097,0.56180082, 0.00923615, -0.0420225],
[-0.00299281 , -0.01203225, 0.99992313, 0.02549419],
[ 0, 0, 0, 1 ]]) # 手眼矩阵THC
matrixBase2Hand = np.array([[0.26195007, -0.14356031, 0.95434407, 0.319418241631],
[-0.67221037, -0.73668364, 0.07369148, -0.010993728332],
[0.69247049, -0.66082346, -0.28947706, 0.689715275419],
[0, 0, 0, 1 ]]) # 末端姿态TBH
matrixCamera2Pixel = np.array([[922.78685321, 0, 657.06183115],
[ 0, 923.92329954, 366.06665478],
[ 0, 0, 1 ]]) # 内参
matrixBase2Camera = np.dot(matrixBase2Hand,matrixHand2Camera)
matrixCamera2Base = np.linalg.inv(matrixBase2Camera)
zc = 0.495
u = 801
v = 452
# 直接变换
outputBase2 = np.dot(np.linalg.inv(matrixCamera2Base[0:3,0:3]),zc*np.dot(np.linalg.inv(matrixCamera2Pixel),np.array([u,v,1]).reshape(3,1))-matrixCamera2Base[:3,3].reshape(3,1))
print("直接变换",outputBase2)根据上述代码，即可求得像素坐标系一点在机械臂基坐标系下的位置，到此就完成了视觉系统感知+机械臂运动一体化流程。
值得注意的是，matrixBase2Camera要先求逆才可带入中，因为公式中的R和T实际为基坐标相对于相机的位置
，而matrixBase2Camera为相机相对于基坐标位姿
，根据
，求得基坐标相对于相机的位置matrixCamera2Base。
附：坐标系转换http://t.csdn.cn/r4Pwq
8421
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
