机械臂视觉抓取手眼标定:Numpy实现AX=XB与避坑指南
干机械臂视觉抓取这一年多,我最怕的不是模型推理不够快,也不是夹爪电机没力,而是手眼标定矩阵算错之后,机械臂当着全实验室的面,对着空桌子来了一招“虚空抓取”。那种明明视觉检测框已经牢牢锁住物体,机械臂却偏出好几厘米的感觉,真的很折磨人。
机械臂视觉抓取里,手眼标定矩阵差不多是“地基中的地基”。很多刚入坑的朋友,把相机装好、标定板摆好、跑完OpenCV的solvePnP,就急着让机械臂去抓东西,结果一抓一个歪。问题往往就出在:你根本没搞清楚手眼标定矩阵到底在算什么,代码里的矩阵是哪里来的,又该怎么验证。这篇文章我就把自己用Numpy从零实现手眼标定、以及踩过的各种坑一次讲清楚,代码可以直接拷走,但更重要的是,我会告诉你每一步背后的“为什么”。
这篇文章适合两类人:一类是刚接触机械臂视觉抓取,想弄懂手眼标定原理的学生或工程师;另一类是已经能跑通流程,但偶尔会被标定偏差折磨得睡不着觉的实操党。无论你是用UR、AR3、松灵Piper还是自组机械臂,底层的矩阵关系都是一样的。
1. 先搞懂手眼标定到底在算什么
1.1 两种安装方式,对应完全不同的变换链
手眼标定里的“手”指机械臂末端,“眼”指相机。根据相机装在哪,分成两种完全不同的情况。
眼在手上(eye-in-hand)是目前视觉抓取最常用的方案:相机固定在机械臂末端法兰上,跟着机械臂一起动。这样有两个好处,一是近看目标更清楚,二是就算目标被部分遮挡,换个角度还能看。眼在手外(eye-to-hand)则是相机固定在天花板或支架上,机械臂在相机视野里活动,适合大范围的静态分拣场景。
这两种方式的变换链不一样,标定要解的矩阵也不一样。眼在手上,你要求的是相机坐标系到机械臂末端坐标系的变换,也就是相机相对于“手”装得有多歪;眼在手外,你要求的是相机坐标系到机械臂基座坐标系的变换。我第一次做的时候,就是没区分清楚,把眼在手外的标定板摆位思路硬套到眼在手上,结果折腾了一个下午。
所以动手之前,先问自己一句:我的相机是装在机械臂上,还是装在外面?这一点决定了后面所有公式里的X是哪两个坐标系之间的变换。
1.2 变换链拆开看:从像素到机械臂基座
视觉抓取的本质,是把相机图像里的一个像素点,换算成机械臂基坐标系下的一个三维坐标。这条链路里全是坐标系变换。
眼在手上的完整链路是:像素坐标 → 相机坐标系 → 末端坐标系 → 基座坐标系。眼在手外则是:像素坐标 → 相机坐标系 → 基座坐标系。每一步都用齐次变换矩阵表示,一个4x4的矩阵,同时包含旋转和平移。
相机内参负责像素到相机坐标系的转换,这一步由相机标定完成,不是手眼标定的范围。手眼标定负责的是“相机坐标系到某个机器人坐标系”的外参。很多教程喜欢直接甩给你一个AX=XB公式,但如果你不清楚A、B、X各自代表哪两个坐标系之间的变换,代码抄了也是白抄。
顺手说个经验:我在代码里命名变量时,强烈建议用
H_cam2gripper
、
H_gripper2base
这种带明确方向的命名,而不是偷懒写
H1
、
H2
。不然过两周你自己回来看代码,都得猜半天这是谁到谁的矩阵。
1.3 标定矩阵差之毫厘,抓取谬以千里
有人觉得手眼标定差不多就行了,反正后面还有视觉检测的误差、机械臂的运动误差。这个想法很危险。手眼标定的误差是会放大的。
举个具体数字:如果你的相机距离目标物体500mm,手眼矩阵的旋转分量误差只有0.5度,那在目标位置产生的平移偏差大约是500 × tan(0.5°) ≈ 4.4mm。如果距离更远,比如800mm,偏差直接到7mm左右。这还没算上末端执行器本身的误差。对于稍微紧一点的夹爪或者吸盘来说,7mm足以让你从一个圆珠笔帽旁边擦过去。
所以手眼标定矩阵不是“能解出来就行”,而是要解到旋转误差尽可能接近0。后面我会讲怎么用仿真数据验证算法本身对不对,再用真实数据验证采集过程对不对。
2. 数学原理:AX=XB是怎么来的
2.1 所有手眼标定都绕不开的AX=XB
手眼标定的核心方程是AX=XB。这里的X就是你要标定的手眼矩阵。A和B不是随便取的,它们从机械臂和相机的两次运动里来。
假设机械臂从位姿1运动到位姿2。对眼在手上的情况,机械臂末端相对基座的运动可以算出来:A = H_gripper2base_2 × inv(H_gripper2base_1)。同时,相机看到的标定板相对相机的运动也能算出来:B = H_cam2target_2 × inv(H_cam2target_1)。
A是机械臂末端的运动,B是相机视野里目标板的运动。因为相机固定在末端上,这两个运动描述的是同一个刚体运动,只是坐标系不同。在这两个坐标系之间,就差一个固定不变的手眼矩阵X。整理出来就是A X = X B。
这个方程看起来很简洁,但里面有个细节:A和B必须来自同一个“时间段”的运动。你不能用第1组到第2组的机械臂运动,却用第3组到第4组的视觉运动去配对,那样方程根本不成立。我见过有人写代码时循环里索引写错了,最后标定出来的矩阵长得跟随机数一样。
2.2 旋转部分为什么能单独先解
A X = X B能不能直接一步把X解出来?可以,但工程上很少有人这么做。因为6自由度的刚体变换涉及旋转和平移耦合,直接非线性求解容易陷入局部最优。
标准做法是先解旋转,再解平移。把X拆成旋转矩阵R_x和平移向量t_x,AX=XB可以展开成两个方程:
- 旋转方程:R_A × R_x = R_x × R_B
- 平移方程:R_A × t_x + t_A = R_x × t_B + t_x
注意看,旋转方程里只有R_x,没有t_x。所以理论上我们可以完全不管平移,先把旋转解出来。这就是为什么手眼标定算法几乎都是“两步走”。
解旋转方程的方法很多。Tsai-Lenz算法用了一个很巧妙的思路:不直接用旋转矩阵,而是把旋转矩阵转换成“修正罗德里格斯向量”,也就是P = 2 × sin(θ/2) × 单位旋转轴。这个向量的好处是,它把旋转矩阵的乘积关系变成了线性关系,从而可以用最小二乘法求解。用Numpy写这个转换并不难,后面我会给出完整代码。
2.3 平移部分的最小二乘
旋转解出来之后,平移就简单了。把第二个方程整理一下:
(R_A - I) × t_x = R_x × t_B - t_A
这是一个关于t_x的线性方程。每一组运动都能提供一个3x3的系数矩阵和一个3维的右侧向量。但一组运动是不够的,因为机械臂的某个特定运动可能在某个方向上没有约束,所以我们需要很多组运动,把所有方程堆叠起来,用最小二乘解一个超定方程组。
实际操作中,我一般最少采15组有效运动。所谓有效,不是说你拍了15张照片就行,而是这15组运动之间要有足够的旋转差异。如果机械臂只是平移着走,旋转方程会变得病态,标定出来的旋转误差会非常大。
2.4 常见算法对比:Tsai-Lenz、Park、OpenCV calibrateHandEye
手眼标定算法圈子里比较有名的有Tsai-Lenz、Park-Martin、Daniilidis等。它们的核心区别在于怎么表示旋转。
Tsai-Lenz用轴角/修正罗德里格斯向量,把问题线性化,代码简单,计算快,是目前最流行的工程方案。Park-Martin通常用四元数或旋转矩阵的SVD分解来做,稳定性也不错。OpenCV从4.x开始提供了
cv2.calibrateHandEye
,内部实现了好几种方法,可以直接调用。
| 算法 | 旋转表示 | 特点 | 工程建议 |
|---|---|---|---|
| Tsai-Lenz | 轴角 | 简单快速,应用最广 | 首选 |
| Park-Martin | 四元数/旋转矩阵 | 数值稳定性较好 | 对噪声大时更稳 |
| Daniilidis | 双四元数 | 能同时估计旋转和平移 | 精度高但实现复杂 |
| OpenCV calibrateHandEye | 多算法内置 | 开箱即用 | 项目赶进度时直接用 |
如果是自己学习,我建议用Numpy手写一遍Tsai-Lenz;如果是赶项目,直接调OpenCV,但也要知道它内部在算什么。下面这部分,我给出一个完整的Numpy实现。
3. 用Numpy从零手写标定核心
3.1 数据准备:H_gripper2base 与 H_cam2target 怎么来
写算法之前,先要把输入数据准备好。第一个输入
H_gripper2base
是机械臂末端在基座坐标系下的位姿,一般从机械臂的SDK里直接读。有些SDK返回的是位置加欧拉角,有些返回四元数,还有一些可以直接读齐次矩阵。
我的建议是:不要手动去算欧拉角转换,太容易翻车。尽量让SDK输出四元数或旋转矩阵,然后在代码里转成4x4齐次矩阵。下面这个函数可以把四元数转成旋转矩阵。
import numpy as np
def quat_to_rotation_matrix(x, y, z, w):
x, y, z, w = float(x), float(y), float(z), float(w)
xx, yy, zz = x * x, y * y, z * z
xy, xz, yz = x * y, x * z, y * z
wx, wy, wz = w * x, w * y, w * z
R = np.array([
[1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)],
[2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)],
[2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)]
])
return R
def make_homogeneous(R, t):
H = np.eye(4)
H[:3, :3] = R
H[:3, 3] = np.asarray(t).flatten()
return H
第二个输入
H_cam2target
是标定板在相机坐标系下的位姿。这个一般通过OpenCV的
solvePnP
得到,输出一个旋转向量和位移向量,然后转成齐次矩阵。
def rvec_tvec_to_homogeneous(rvec, tvec):
R, _ = cv2.Rodrigues(rvec)
H = np.eye(4)
H[:3, :3] = R
H[:3, 3] = np.asarray(tvec).flatten()
return H
注意,solvePnP返回的平移向量的单位取决于你输入标定板角点坐标时用的单位。如果你用毫米输入,那tvec的单位就是毫米;如果你用米输入,tvec的单位就是米。机械臂SDK返回的位姿一般是毫米或米,这个单位必须统一,否则后面标定出来的平移分量会差1000倍。
3.2 相对运动与轴角提取
准备好每一组位姿之后,就要计算相邻两组之间的相对运动。为什么用相邻两组?因为AX=XB里的A和B就是要用“运动”来构造的。A = H_gripper2base[i+1] × inv(H_gripper2base[i]),B = H_cam2target[i+1] × inv(H_cam2target[i])。
在Tsai-Lenz算法里,旋转部分不是直接用旋转矩阵,而是要转成“修正罗德里格斯向量”。绕了半天,最后其实就是从旋转矩阵里提取旋转轴和旋转角,然后做一次变换。
def skew(v):
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])
def rodrigues_to_axis_angle(R):
theta = np.arccos(np.clip((np.trace(R) - 1) / 2, -1.0, 1.0))
if theta < 1e-12:
return np.zeros(3)
r = theta / (2 * np.sin(theta)) * np.array([
R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1]
])
return r
def modified_rodrigues(R):
r = rodrigues_to_axis_angle(R)
theta = np.linalg.norm(r)
if theta < 1e-12:
return np.zeros(3)
axis = r / theta
return 2.0 * np.sin(theta / 2.0) * axis
这个
modified_rodrigues
函数是Tsai-Lenz算法的关键。它把旋转矩阵映射成一个三维向量,这个向量的模长等于2×sin(θ/2),方向是旋转轴。这样旋转矩阵的乘积关系就能变成线性关系。
3.3 Tsai-Lenz实现:完整代码
有了前面的工具函数,核心求解就简单了。完整代码如下,可以直接复制运行。
def tsai_lenz_calibration(H_gripper2base_list, H_cam2target_list):
n = len(H_gripper2base_list)
assert n == len(H_cam2target_list) and n >= 3
# 旋转部分:构造线性方程 skew(Pg + Pc) * Pcg = Pc - Pg
A_mat = []
b_vec = []
for i in range(n - 1):
A = H_gripper2base_list[i + 1] @ np.linalg.inv(H_gripper2base_list[i])
B = H_cam2target_list[i + 1] @ np.linalg.inv(H_cam2target_list[i])
Pg = modified_rodrigues(A[:3, :3])
Pc = modified_rodrigues(B[:3, :3])
A_mat.append(skew(Pg + Pc))
b_vec.append(Pc - Pg)
A_mat = np.vstack(A_mat)
b_vec = np.concatenate(b_vec)
Pcg, _, _, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
# 从 Pcg 恢复旋转矩阵 R_x
norm_pcg = np.linalg.norm(Pcg)
if norm_pcg < 1e-12:
R_x = np.eye(3)
else:
# Pcg = 2 * sin(theta/2) * axis
sin_half_theta = min(norm_pcg / 2.0, 1.0)
theta = 2.0 * np.arcsin(sin_half_theta)
axis = Pcg / norm_pcg
K = skew(axis)
R_x = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K)
# 平移部分:(R_A - I) * t_x = R_x * t_B - t_A
M_mat = []
y_vec = []
for i in range(n - 1):
A = H_gripper2base_list[i + 1] @ np.linalg.inv(H_gripper2base_list[i])
B = H_cam2target_list[i + 1] @ np.linalg.inv(H_cam2target_list[i])
R_A = A[:3, :3]
t_A = A[:3, 3]
R_B = B[:3, :3]
t_B = B[:3, 3]
M_mat.append(R_A - np.eye(3))
y_vec.append(R_x @ t_B - t_A)
M_mat = np.vstack(M_mat)
y_vec = np.concatenate(y_vec)
t_x, _, _, _ = np.linalg.lstsq(M_mat, y_vec, rcond=None)
X = np.eye(4)
X[:3, :3] = R_x
X[:3, 3] = t_x
return X
看到这里你可能有个疑问:为什么旋转部分构造的方程是
skew(Pg + Pc) * Pcg = Pc - Pg
?这个式子我没打算让你死记,但你要记住一点:Tsai-Lenz通过修正罗德里格斯向量,把原本非线性的旋转矩阵方程变成了线性最小二乘问题。这也是为什么它能一次算出结果、不需要迭代的原因。
3.4 用仿真数据验证代码对不对
代码写完了,最怕的是算法本身有bug但你不知道。我强烈建议先用仿真数据验证一遍。怎么验证?自己生成一组“真值手眼矩阵”,再随机生成机械臂位姿和标定板位姿,反推出相机的观测值,最后用手眼标定算法反解,看能不能还原真值。
def random_transform(max_trans=0.5):
R, _ = np.linalg.qr(np.random.randn(3, 3))
if np.linalg.det(R) < 0:
R[:, 0] *= -1
t = np.random.uniform(-max_trans, max_trans, 3)
H = np.eye(4)
H[:3, :3] = R
H[:3, 3] = t
return H
np.random.seed(42)
X_true = random_transform(0.15)
T_target = random_transform(0.4)
G_list = [random_transform(0.5) for _ in range(20)]
H_ct_list = []
for G in G_list:
H_ct = np.linalg.inv(X_true) @ np.linalg.inv(G) @ T_target
H_ct_list.append(H_ct)
X_est = tsai_lenz_calibration(G_list, H_ct_list)
rot_err = np.linalg.norm(X_est[:3, :3] - X_true[:3, :3])
pos_err = np.linalg.norm(X_est[:3, 3] - X_true[:3, 3])
print("旋转矩阵误差:", rot_err)
print("平移向量误差:", pos_err)
这组仿真数据里,
G_list
就是机械臂末端在基座坐标系下的位姿,
T_target
是标定板在基座坐标系下的固定位姿,
X_true
是相机在末端坐标系下的真值手眼矩阵。如果代码没问题,旋转误差应该在1e-6量级,平移误差也接近0。如果误差很大,先别急着上真机,回头查查你的
modified_rodrigues
函数是不是写错了。
4. 实战避坑清单:这些坑我基本都踩过
4.1 采集数据:位姿太少或太相似等于白采
手眼标定最常见的失败原因,不是算法不行,而是输入数据不行。我见过不少人拿着八九组数据跑来问我为什么标定结果不稳,我一看数据,机械臂基本都在同一个位置小幅度挪动,旋转变化几乎没有。这种数据喂给任何算法都白搭。
原因在于手眼标定方程需要从不同的旋转运动中提取约束。如果你的运动全是纯平移,旋转方程里Pc和Pg基本是零向量,线性方程组退化,求出来的旋转矩阵自然不可靠。反过来,运动全是原地旋转也不行,平移部分会缺约束。
我的经验是:采集至少15到20组位姿,每组位姿之间要有明显的旋转差异。具体操作上,让机械臂带着相机在标定板前做“绕不同轴转动+平移”的复合运动。比如先绕Z轴转30度,再绕X轴转20度,同时移动一点距离。最好让标定板在相机视野里处于不同位置,不要每次都出现在正中央。
这里还有个小技巧:采集过程中,机械臂每到一个位姿,停稳之后再拍照。机械臂运动中拍的图会糊,角点提取位置会偏,标定结果自然就差。
4.2 标定板与角点:棋盘格还是圆点
标定板选择看起来不重要,实际上很影响最终精度。OpenCV的
solvePnP
需要你提供标定板上角点的三维坐标和对应的二维像素坐标。棋盘格和圆点阵列是两种最常见的标定板。
棋盘格的好处是角点检测简单,OpenCV的
findChessboardCorners
能稳定输出亚像素级角点。但棋盘格在光照不均匀或者有反光的情况下,角点也可能被“吃掉”。圆点阵列的话,
findCirclesGrid
在某些光照下更稳,而且圆形中心对透视变形的鲁棒性比棋盘格角点好一些。
我自己的偏好是:如果是室内稳定光照环境,用棋盘格,配合
cornerSubPix
做亚像素细化;如果光照条件不可控,或者标定板要倾斜很多角度,用非对称圆点阵列。另外,标定板一定要贴平,不能褶皱,否则三维坐标本身就是错的,后面所有计算都是空中楼阁。
4.3 单位、坐标系方向与符号:最容易翻车
如果手眼标定结果里,平移向量的数值看起来“特别顺眼”但抓取全错,那十有八九是单位或者坐标系方向出了问题。
单位问题前面提过:机械臂SDK返回的位移可能是毫米,也可能是米;solvePnP的tvec单位取决于你输入的三维点单位。这两边必须统一。我一般统一用米,因为后面接到ROS和MoveIt的时候,默认单位就是米。
坐标系方向问题也很隐蔽。机械臂的基座坐标系通常是右手系,Z轴向上;但不同厂家的坐标系定义有差异,有的末端坐标系Z轴指向法兰盘的“前方”,有的指向“下方”。相机坐标系则是Z轴沿光轴方向,一般是向前。如果你的相机安装方向导致相机坐标系的Z轴和机械臂末端坐标系的某个轴方向相反,标定出来的旋转矩阵会有180度的分量,而且你用重投影误差还很难发现。
最稳的做法是:全部转成齐次变换矩阵,不要用欧拉角直接代入计算。矩阵是不带歧义的,只要你知道每个矩阵“从哪到哪”,方向问题就能通过画图或者打印坐标变换链条来检查。
4.4 标定结果怎么验证:别只看重投影误差
很多人标定完,看一眼重投影误差只有零点几像素,就高高兴兴去抓取了。这里我必须泼一盆冷水:手眼标定的重投影误差低,只能说明solvePnP阶段做得不错,不能说明手眼矩阵本身是对的。因为重投影误差计算的是“标定板角点在图像上的投影误差”,它和手眼矩阵的准确性不是一回事。
我常用的验证方法有两步。第一步是“坐标变换一致性检查”:取采集到的数据,随机拿一组位姿,把标定板角点在相机坐标系下的坐标,通过手眼矩阵和机械臂位姿变换到基座坐标系下。因为标定板固定在空间里,所以不管机械臂怎么动,变换到基座坐标系后的坐标都应该落在一个固定的位置。把多组数据都算一遍,看这些点云发散程度。如果发散在几毫米内,说明手眼矩阵基本靠谱;如果发散几厘米,说明标定有问题。
第二步是实机抓取验证,这个放到后面再细说。总之,永远不要只凭一个数字就下结论。
5. 从标定到抓取:补上最后一公里
5.1 标定结果接入ROS的tf树
如果你的项目是ROS或ROS 2,手眼标定算出来的X一般会作为一个静态变换发布到tf树里。眼在手上的情况下,变换关系大概是:base_link → tool0/ee_link → camera_link → target_frame。你算出来的X是camera_link到ee_link(或tool0)的变换,具体哪个parent,要看你机械臂的URDF里末端坐标系叫什么名字。
发布静态变换很简单,但如果tf树的parent和child搞反了,就会导致坐标跳变。我见过最经典的错误是:发布的时候把平移和旋转填对了,但是parent和child写反,结果整个抓取路径全乱套。这里可以用
tf2_echo
或者RVIZ里的TF面板来检查:把机械臂移到某个位姿,看相机坐标系在RVIZ里是不是大概指着你预期的方向。
另外,尽量不要在代码里手动做矩阵乘法去替代tf。tf会帮你处理时间戳、插值、坐标系更新这些麻烦事,手动搞很容易在某个角落忘了更新。
5.2 深度相机对齐与像素转3D
手眼标定做完,接下来就是抓取流程。以常见的Intel RealSense D435i为例,你要先用
rs2::align
把深度图对齐到彩色图,然后根据像素坐标查询深度值,再通过相机内参反投影到相机坐标系下的三维点。
这个三维点是在相机坐标系下的,之后要转到机械臂基座坐标系。眼在手上的转换链条是:相机系→末端系→基座系。也就是用你标定出来的X,再乘上机械臂当前的末端位姿。如果你用的是ROS,直接
tf2::transformPoint
更省事。
有个容易忽略的坑:深度相机在物体边缘、反光表面上的深度值经常是NaN或者跳变,直接反投影会得到飞点。我通常会在拿到深度值后加一个邻域滤波,比如取中心像素周围5x5区域的中值。这个操作能省掉后面大量的抓取失败调试时间。
5.3 快速自检脚本
最后分享一个我一直放在机械臂工控机上的小脚本思路。每次标定完,不急着上正式抓取流程,先跑一个自动自检:让机械臂带相机走5到8个预先设定的位姿,每个位姿都拍一张标定板的照片,解出标定板在相机坐标系下的位姿,然后用你的手眼矩阵把标定板原点变换到基座坐标系。理想情况下,这些原点应该在空间里聚成一团。
如果聚成一团,再做一步实物验证:在标定板上放一个很小的目标物,让机械臂基于视觉定位去抓一次。偏差在5毫米以内,基本可以放心用;偏差在1厘米以上,回头查数据采集和你标定板角点的亚像素精度。这套流程熟练之后,整个自检十分钟内就能跑完。
写到最后,我想说的是,Numpy代码只是手眼标定里最“死”的一部分,真正决定成败的是你对坐标变换的理解,以及你愿不愿意在数据采集和验证上多花时间。我自己也曾经图快,跳过验证直接抓取,结果在项目汇报前一天发现所有结果都偏了同一方向。从那以后,我给自己定了个规矩:标定完不验证,就不许上抓取程序。这个习惯,比任何标定算法都值钱。
更多推荐
所有评论(0)