从原理到代码：拆解睿尔曼机械臂手眼标定中的齐次变换矩阵
在机器人视觉引导系统中，手眼标定是连接机械臂与视觉传感器的关键环节。本文将深入探讨齐次变换矩阵的数学本质，并通过Python代码实例演示如何实现睿尔曼机械臂与深度相机的高精度标定。
1. 坐标系转换的数学基础
手眼标定的核心在于建立不同坐标系之间的映射关系。我们需要处理四种基本坐标系：
- 基坐标系（Base Frame）：机械臂的固定参考系
- 工具坐标系（Tool Frame）：机械臂末端的执行器坐标系
- 相机坐标系（Camera Frame）：深度相机的光学中心坐标系
- 标定板坐标系（Checkerboard Frame）：作为已知几何特征的参考系
齐次变换矩阵的通用形式为：
$$ T = \begin{bmatrix} R & t \ 0 & 1 \end{bmatrix} $$
其中$R$是3×3旋转矩阵，$t$是3×1平移向量。这个4×4矩阵能够同时表示旋转和平移变换。
1.1 旋转矩阵的物理意义
旋转矩阵的列向量实际上是目标坐标系各轴在参考坐标系中的方向余弦。例如，对于绕Z轴旋转θ角度的情况：
import numpy as np
def rotation_z(theta):
return np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
实际应用中，我们常使用欧拉角（rx, ry, rz）表示旋转。将欧拉角转换为旋转矩阵的函数如下：
from scipy.spatial.transform import Rotation as R
def euler_to_matrix(rx, ry, rz):
return R.from_euler('xyz', [rx, ry, rz]).as_matrix()
1.2 手眼标定的基本方程
手眼标定问题可以表述为经典的$AX=XB$方程：
- 眼在手上（Eye-in-Hand）：求解相机到工具坐标系的变换$X$
- 眼在手外（Eye-to-Hand）：求解相机到基坐标系的变换$X$
其中：
- $A$表示机械臂末端的运动变换
- $B$表示相机观察到的标定板运动变换
2. 数据采集与处理
高质量的数据采集是标定成功的前提条件。我们需要同步获取机械臂位姿和相机图像。
2.1 机械臂位姿采集
睿尔曼机械臂通常通过TCP/IP接口提供实时位姿数据。以下是获取位姿的示例代码：
import socket
import json
def get_arm_pose(ip='192.168.1.18', port=8080):
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.connect((ip, port))
cmd = '{"command": "get_current_arm_state"}'
s.sendall(cmd.encode())
data = s.recv(1024).decode()
return json.loads(data)['pose'] # [x,y,z,rx,ry,rz]
2.2 图像采集与角点检测
使用OpenCV进行棋盘格角点检测：
import cv2
def detect_checkerboard(image, pattern_size=(11,8)):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
if ret:
# 亚像素级角点优化
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners = cv2.cornerSubPix(gray, corners, (5,5), (-1,-1), criteria)
return ret, corners
2.3 数据采集的最佳实践
- 每次移动后保持1-2秒静止，避免运动模糊
- 确保标定板占据图像40%-80%的面积
- 在X/Y/Z三个轴上都有大于30°的旋转变化
- 采集15-20组不同姿态的数据
- 避免标定板平面与相机光轴平行
3. 标定算法的实现
OpenCV提供了多种手眼标定算法的实现，我们将重点分析Tsai-Lenz算法。
3.1 标定板位姿计算
首先需要计算标定板相对于相机的位姿：
def get_chessboard_pose(image, pattern_size, square_size):
# 准备标定板3D坐标
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2)
objp *= square_size
# 检测角点
ret, corners = detect_checkerboard(image, pattern_size)
if not ret:
return None
# 计算相机外参
ret, rvec, tvec = cv2.solvePnP(objp, corners, camera_matrix, dist_coeffs)
return rvec, tvec
3.2 手眼标定核心计算
使用OpenCV的calibrateHandEye函数实现：
def hand_eye_calibration(arm_poses, camera_poses, method=cv2.CALIB_HAND_EYE_TSAI):
R_target2cam = []
t_target2cam = []
R_base2tool = []
t_base2tool = []
for rvec, tvec in camera_poses:
R_target2cam.append(cv2.Rodrigues(rvec)[0])
t_target2cam.append(tvec)
for x,y,z,rx,ry,rz in arm_poses:
R = euler_to_matrix(rx, ry, rz)
t = np.array([x,y,z])
R_base2tool.append(R)
t_base2tool.append(t)
# 执行手眼标定
R, t = cv2.calibrateHandEye(
R_base2tool, t_base2tool,
R_target2cam, t_target2cam,
method=method)
return R, t
3.3 标定结果验证
验证标定结果的常用方法是重投影误差计算：
def validate_calibration(R, t, arm_poses, camera_poses):
errors = []
for (rvec_cam, tvec_cam), (_,_,_,rx,ry,rz) in zip(camera_poses, arm_poses):
# 理论变换
R_cam = cv2.Rodrigues(rvec_cam)[0]
R_tool = euler_to_matrix(rx, ry, rz)
# 实际观测与理论预测的差异
error = R_tool @ t + R @ tvec_cam - tvec_cam
errors.append(np.linalg.norm(error))
return np.mean(errors)
4. 标定结果的应用
获得标定矩阵后，我们可以实现视觉引导的精确抓取。
4.1 坐标转换基础函数
def transform_points(points, R, t):
""" 将点集从一个坐标系转换到另一个坐标系 """
if len(points.shape) == 1:
return R @ points + t
return (R @ points.T + t.reshape(-1,1)).T
def inverse_transform(R, t):
""" 计算变换矩阵的逆 """
R_inv = R.T
t_inv = -R_inv @ t
return R_inv, t_inv
4.2 眼在手上模式的应用
class EyeInHandSystem:
def __init__(self, R_cam2tool, t_cam2tool):
self.R_cam2tool = R_cam2tool
self.t_cam2tool = t_cam2tool
def camera_to_base(self, point_cam, tool_pose):
""" 将相机坐标系中的点转换到基坐标系 """
x,y,z,rx,ry,rz = tool_pose
R_base2tool = euler_to_matrix(rx, ry, rz)
t_base2tool = np.array([x,y,z])
# 相机坐标系→工具坐标系
point_tool = transform_points(point_cam, self.R_cam2tool, self.t_cam2tool)
# 工具坐标系→基坐标系
return transform_points(point_tool, R_base2tool, t_base2tool)
4.3 眼在手外模式的应用
class EyeToHandSystem:
def __init__(self, R_cam2base, t_cam2base):
self.R_cam2base = R_cam2base
self.t_cam2base = t_cam2base
def camera_to_base(self, point_cam):
""" 直接转换到基坐标系 """
return transform_points(point_cam, self.R_cam2base, self.t_cam2base)
5. 高级话题与优化技巧
5.1 标定精度的提升方法
-
运动轨迹设计：
- 确保在X/Y/Z三个轴上都有充分旋转
- 采用螺旋式运动轨迹覆盖工作空间
- 避免纯平移或绕单一轴旋转
-
数据筛选策略：
- 剔除角点检测不准确的帧
- 排除运动模糊导致的图像模糊
- 使用RANSAC算法剔除异常点
-
多算法融合：
def hybrid_calibration(arm_poses, camera_poses): methods = [ cv2.CALIB_HAND_EYE_TSAI, cv2.CALIB_HAND_EYE_PARK, cv2.CALIB_HAND_EYE_HORAUD ] results = [] for method in methods: R, t = hand_eye_calibration(arm_poses, camera_poses, method) results.append((R,t)) # 选择重投影误差最小的结果 errors = [validate_calibration(R,t,arm_poses,camera_poses) for R,t in results] return results[np.argmin(errors)]
5.2 在线标定与自适应校正
对于需要长期运行的系统，建议实现在线标定功能：
class OnlineCalibrator:
def __init__(self, initial_R, initial_t):
self.R = initial_R
self.t = initial_t
self.pose_pairs = []
def update(self, arm_pose, camera_pose):
self.pose_pairs.append((arm_pose, camera_pose))
if len(self.pose_pairs) > 10: # 保持最近的10组数据
self.pose_pairs.pop(0)
# 每5组新数据重新标定一次
if len(self.pose_pairs) % 5 == 0:
self.R, self.t = hand_eye_calibration(
[p[0] for p in self.pose_pairs],
[p[1] for p in self.pose_pairs])
5.3 标定结果的存储与加载
建议使用YAML格式存储标定结果：
import yaml
def save_calibration(filename, R, t):
data = {
'rotation_matrix': R.tolist(),
'translation_vector': t.tolist()
}
with open(filename, 'w') as f:
yaml.dump(data, f)
def load_calibration(filename):
with open(filename, 'r') as f:
data = yaml.safe_load(f)
return np.array(data['rotation_matrix']), np.array(data['translation_vector'])
在实际项目中，我们曾遇到机械臂重复定位精度影响标定结果的情况。通过增加数据量（30组以上）和优化运动轨迹，最终将标定误差控制在0.5mm以内。标定过程中特别需要注意机械臂各轴的温度变化，建议在机械臂预热30分钟后再进行标定操作。
1097
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
