机器人 / 系统课时

根据手眼外参转换相机目标

根据手眼外参转换相机目标

相机识别到目标后,坐标通常仍相对于镜头,不能直接送给机器人。这里用一组已标定的手眼外参、机械臂当前法兰位姿,把深度图中的目标点换算成基座坐标,并用逆变换检查外参方向和单位。

学习目标

  • 区分眼在手上与眼在手外的坐标链和矩阵方向。
  • 从像素及深度恢复相机系三维点,并计算基座系目标位置。
  • 利用回代、实物基准点与时间戳排查转换错误。

约定坐标系与外参方向

本文以 B 表示机器人基座、F 表示法兰、C 表示相机光学坐标系,p 表示目标点。记号 ${}^{A}T_D 表示将 D 系坐标变为 A 系坐标的 4×4 齐次变换,上标是目标坐标系,下标是来源坐标系;相乘时中间坐标系必须匹配。

Ap=ATD Dp,Dp=[x,y,z,1]T{}^{A}p={}^{A}T_D\,{}^{D}p,\qquad{}^{D}p=[x,y,z,1]^\mathsf{T}

相机安装在末端(眼在手上)时,机器人正运动学提供 ${}^{B}T_F,手眼标定提供 ${}^{F}T_C,因此先相机到法兰、再法兰到基座:

Bp=BTF FTC Cp{}^{B}p={}^{B}T_F\,{}^{F}T_C\,{}^{C}p

固定相机(眼在手外)若已标定出 ${}^{B}T_C,可直接算 ${}^{B}p={}^{B}T_C\,{}^{C}p,不需要每帧接入法兰位姿。若存档的是反方向 ${}^{C}T_B 或 ${}^{C}T_F,要先对整个齐次矩阵求逆,而不是仅将平移向量取负。

眼在手上及固定相机的手眼外参转换链

OpenCV 4.x 的 cv2.calibrateHandEye 返回相机到夹爪坐标系的旋转和平移,即 ${}^{G}T_C;只有夹爪参考架 G 与此处 F 重合时,才能直接用作 ${}^{F}T_C。可对照官方接口文档。

由深度像素得到三维点

假设深度图已经对齐彩色图,像素 (u,v) 已去畸变,深度 z 表示光学轴 Z 方向的距离(而非相机到目标的斜距)。内参必须对应当前图像分辨率及对齐方式,针孔模型反投影为:

x=(u−cx)zfx,y=(v−cy)zfyx=(u-c_x)\frac{z}{f_x},\qquad y=(v-c_y)\frac{z}{f_y}

示例采用 fx=fy=400 px、主点 (cx,cy)=(320,240) px、目标像素 (520,340)、深度 0.40 m。得到相机坐标 ${}^{C}p=(0.20,0.10,0.40) 米。相机光学系通常 X 向右、Y 向下、Z 向前,不能直接当作基座轴定义;实际须以标定时采用的相机坐标系为准。

计算基座系目标与回代

以下使用 Python 3、NumPy;两组示例变换均绕各自 Z 轴旋转 90°,${}^{B}T_F 平移 (0.50,0.10,0.30) m,${}^{F}T_C 平移 (0.10,0,0.05) m。用模拟数据演示矩阵乘法,实际须替换为曝光时刻的机器人位姿和真实标定结果。

python
import numpy as np

def pose_z(degrees, xyz):
    angle = np.deg2rad(degrees)
    c, s = np.cos(angle), np.sin(angle)
    T = np.eye(4)
    T[:3, :3] = [[c, -s, 0], [s, c, 0], [0, 0, 1]]
    T[:3, 3] = xyz
    return T

# 深度单位为米,像素需先去畸变
fx = fy = 400.0
cx, cy = 320.0, 240.0
u, v, z = 520.0, 340.0, 0.40
p_c = np.array([(u - cx) * z / fx, (v - cy) * z / fy, z, 1.0])

T_bf = pose_z(90, (0.50, 0.10, 0.30))
T_fc = pose_z(90, (0.10, 0.00, 0.05))
T_bc = T_bf @ T_fc  # 右侧的相机点先变换到法兰
p_b = T_bc @ p_c
p_c_check = np.linalg.inv(T_bc) @ p_b

format_xyz = lambda p: ", ".join(f"{x:.3f}" for x in p[:3])
print("相机系 (m):", format_xyz(p_c))
print("基座系 (m):", format_xyz(p_b))
print("回代误差 (m):", f"{np.linalg.norm(p_c_check[:3] - p_c[:3]):.6f}")

预期输出:

text
相机系 (m): 0.200, 0.100, 0.400
基座系 (m): 0.300, 0.100, 0.750
回代误差 (m): 0.000000

此时目标在基座系的位置为 (0.30,0.10,0.75) m。这些数字是几何演示,不代表实际标定精度;回代为零只证明这组矩阵代数自洽,不能证明外参方向选对了。

验证外参方向和现场精度

  1. 检查帧名与方向:记录外参的来源系、目标系、长度单位及角度单位。若相机给出 ${}^{C}T_F,应写成 ${}^{F}T_C=({}^{C}T_F)^{-1};逆变换的平移为 $-R^\mathsf{T}t,并非 -t。在上面的算例中,错误地将 ${}^{F}T_C 再取逆,会得到 (0.600,0.200,0.650) m,与正确位置明显不同。
  2. 检查真实基准点:将基座坐标已知的标定球中心置于视野,比较预测 ${}^{B}p 和测得的参考位置。5 mm 可作为某个夹取工位的示例验收门槛,但必须结合深度噪声、机器人精度和抓取公差另行制定阈值。
  3. 检查帧间时序:眼在手上的相机应使用图像曝光时刻的 ${}^{B}T_F,不能收到图像后才取最新位姿。若末端平移速度 0.20 m/s、姿态与图像错开 50 ms,仅平移就可能造成约 10 mm 误差。
  4. 检查空间一致性:在视野左、中、右和多个深度各选静态点;若误差随距离放大,检查深度尺度、内参与畸变;若出现近似恒定偏移,检查外参平移、参考架及相机安装是否移动。

若算法还能估计目标物体坐标架 O 在相机中的姿态 ${}^{C}T_O,则可继续计算 ${}^{B}T_O={}^{B}T_F\,{}^{F}T_C\,{}^{C}T_O。只有一个三维目标点时不能凭空得到其朝向,不能将位置结果直接视为完整的六自由度抓取位姿。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

根据手眼外参转换相机目标

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通