easy_handeye原理

### EasyHandEye 校准原理 EasyHandEye 是一种用于机器人视觉系统的校准方法，旨在简化相机与机械臂之间的相对位置关系的标定过程。该技术的核心在于通过一系列已知运动来计算相机相对于末端执行器的位置和姿态。 由于摄像机被机械固定于手指上，在机器人上的安装不需要额外进行摄像机—机器人—世界坐标系间的复杂校正工作[^1]。因此，这种配置使得系统能够更稳定地应对物理冲击和其他环境变化带来的影响。具体来说： - **数据采集阶段**：操作员控制机器人的工具中心点 (TCP) 访问多个预定义的空间位姿，并记录下每次访问时所对应的图像以及 TCP 的实际位姿。 - **优化求解阶段**：基于收集到的数据集，采用非线性最小二乘法或其他合适的算法估计出摄像机相对于 TCP 的变换矩阵 \( T_{c}^{e} \)，即所谓的手眼关系模型参数。 ```python import numpy as np from scipy.optimize import least_squares def hand_eye_calibration(A_list, B_list): """ Solve for the transformation matrix between two coordinate systems using Horn's method. Parameters: A_list : list of 4x4 homogeneous matrices representing poses in one frame B_list : corresponding list of 4x4 homogeneous matrices representing poses in another frame Returns: X : estimated 4x4 homogenous transform from first to second frame """ n = len(A_list) M = [] for i in range(n): R_Ai_inv = np.linalg.inv(A_list[i][:3,:3]) t_Ai = -R_Ai_inv @ A_list[i][:3,3] R_Bi = B_list[i][:3,:3] t_Bi = B_list[i][:3,3] Mi = np.block([[R_Ai_inv@R_Bi , R_Ai_inv@t_Bi.reshape((3,1))], [-t_Ai.T@R_Ai_inv@R_Bi, -t_Ai.T@R_Ai_inv@t_Bi]]) M.append(Mi) # Convert list into array and solve linear equation Ax=b where b=0 except last element which equals sum of all elements in column before it M_array = np.vstack(M) _, s, vh = np.linalg.svd(M_array) null_space_vector = vh[-1].reshape(-1,) scale_factor = abs(null_space_vector[-1])**(-1/len(B_list)) scaled_null_space_vector = null_space_vector * scale_factor rotation_matrix = scaled_null_space_vector[:9].reshape(3,3).T translation_vector = scaled_null_space_vector[9:].reshape(3,)[:-1]*scale_factor X = np.eye(4) X[:3,:3]=rotation_matrix X[:3,3]=translation_vector.flatten() return X ``` 此代码片段展示了如何利用Horn的方法解决两个不同参照系之间转换矩阵的问题，这是实现易手眼标定的关键部分之一。