简介这份PDF资料聚焦机器人视觉中的手眼标定技术面向计算机视觉、双目视觉与机器人抓取方向的学习者和研究者系统讲解末端执行器与摄像头坐标系转换关系的求解思路。内容从Euler角、Rodrigues表示、Cayley表示及FOE点等基础知识切入逐步展开手眼关系数学模型并重点对比传统两步法、Ma自标定法与Lei自标定法三种方案的实现过程最后从精度、稳定性、设备需求与计算复杂度等维度给出实验比较与选型建议。资源包内共1个PDF文件约864KB篇幅紧凑、结构清晰适合作为算法入门与方案选型的参考材料。目前已有1235人学习下载读者可借此理解AXXB方程组的构建与求解、旋转矩阵的Rodrigues推导以及不同自标定方法在动态环境或特定场景下的适用边界为后续工程实践提供理论支撑。1. 三种手眼标定方法的实现与比较从 AXXB 到 FOE哪条路在产线上不翻车产线换型时机械臂抓取精度突然掉了 2 mm十有八九是手眼标定该重做了。手眼标定要解决的事情很朴素让相机坐标系和机械臂末端或基座坐标系对齐标定结果直接决定抓取点算得准不准。业内最常落地的三条路线是基于 AXXB 的经典闭式解、基于 Rodrigues/Cayley 参数化的非线性优化、以及基于 FOEFocus of Expansion的运动场方法。三者不是替代关系而是分别对应「快、稳、省标定板」三种诉求。这篇笔记按我实际在 6 轴臂 2D 相机 3D 相机混用场景里的经验把三种方法的推导、代码、参数和踩坑一次讲透新手能照着跑通熟手能看清各自边界。2. AXXB 闭式解为什么它是大多数项目的默认起点2.1 AXXB 到底在解什么手眼标定分两种构型eye-in-hand相机装在末端和 eye-to-hand相机固定在外部看工作区。无论哪种本质都是求解相机坐标系与机械臂坐标系之间的刚体变换。经典做法是让机械臂走 N 个位姿每个位姿下拍一张标定板得到相机相对标定板的位姿再联立方程。eye-in-hand 构型下相邻两次运动的约束写成A_i X X B_i其中 A_i 是机械臂末端两次位姿之间的相对变换由机器人正运动学给出B_i 是相机两次观测之间的相对变换由标定板位姿给出X 就是待求的手眼矩阵。这个方程之所以经典是因为它把「绝对位姿未知」消掉了只依赖相对运动对机器人绝对定位误差不敏感。展开成旋转和平移两部分R_A R_X R_X R_B R_A t_X t_A R_X t_B t_X旋转部分只和 R_X 有关可以先解旋转再解平移这就是两步法的由来。常见闭式解有 Tsai-Lenz、Park-Martin、Horaud-Andreff 等工程上 Tsai 用得最多因为它对噪声的鲁棒性在多数场景够用且实现简单。2.2 用 Python 跑通 Tsai 两步法下面是我常用的最小实现依赖 numpy 和 scipy输入是 N 组 (R_A, t_A, R_B, t_B)。import numpy as np from scipy.linalg import logm, expm def skew(v): return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) def tsai_hand_eye(RAs, tAs, RBs, tBs): # 第一步用旋转约束解 R_X # 构造 (P - Q) 形式的最小二乘P、Q 由旋转轴和角度决定 M [] for RA, RB in zip(RAs, RBs): # 旋转向量轴角表示 rA logm(RA)[None] rB logm(RB)[None] # 取实部对应的旋转向量 a np.array([rA[0,2,1]-rA[0,1,2], rA[0,0,2]-rA[0,2,0], rA[0,1,0]-rA[0,0,1]]) / 2.0 b np.array([rB[0,2,1]-rB[0,1,2], rB[0,0,2]-rB[0,2,0], rB[0,1,0]-rB[0,0,1]]) / 2.0 M.append(skew(a b)) M np.vstack(M) # 对 M 做 SVD最小奇异值对应的右奇异向量即旋转轴 _, _, Vt np.linalg.svd(M) p Vt[-1] # 由轴角构造 R_X # 这里用 Rodrigues 公式角度由各次运动平均得到 angles [] for RA, RB in zip(RAs, RBs): rA logm(RA) rB logm(RB) a np.array([rA[0,2,1]-rA[0,1,2], rA[0,0,2]-rA[0,2,0], rA[0,1,0]-rA[0,0,1]]) / 2.0 b np.array([rB[0,2,1]-rB[0,1,2], rB[0,0,2]-rB[0,2,0], rB[0,1,0]-rB[0,0,1]]) / 2.0 angles.append(np.arccos(np.clip((np.trace(RA)-1)/2, -1, 1)) / np.arccos(np.clip((np.trace(RB)-1)/2, -1, 1))) theta np.mean(angles) K skew(p) RX np.eye(3) np.sin(theta)*K (1-np.cos(theta))*KK # 第二步用平移约束解 t_X C [] d [] for RA, tA, RB, tB in zip(RAs, tAs, RBs, tBs): C.append(RA - np.eye(3)) d.append(RX tB - tA) C np.vstack(C) d np.hstack(d) tX np.linalg.lstsq(C, d, rcondNone)[0] return RX, tX逻辑说明旋转部分把每次运动的旋转轴方向作为约束堆叠后做 SVD 取最小奇异向量这是 Tsai 方法的核心角度用各次运动的旋转角比值平均避免单次噪声主导。平移部分把旋转结果代入平移方程转成线性最小二乘。参数上RAs/tAs 是机器人相邻位姿的相对变换RBs/tBs 是相机相邻观测的相对变换N 建议不少于 10 组且运动要覆盖三个旋转轴否则 M 矩阵秩亏解会飘。2.3 采集位姿的三个硬约束第一相邻位姿的旋转角不能太小一般要求大于 15 度否则旋转轴估计噪声极大。第二运动要绕至少两个不平行的轴最好三个轴都覆盖纯平移运动对旋转约束没有贡献。第三标定板在每张图里都要完整可见且姿态有变化不能所有图都正对相机。我一般让机械臂走 15 到 20 个位姿其中 5 个绕 Z 转、5 个绕 X 转、5 个绕 Y 转剩下几个做复合运动。3. Rodrigues 与 Cayley 参数化把非线性优化做稳的关键3.1 为什么闭式解之后还要上优化Tsai 这类闭式解在噪声小、位姿分布好的时候精度够用但产线上常见的问题是标定板检测有 0.3 像素的抖动、机器人重复定位有 0.05 mm 误差、某些位姿下旋转角接近 180 度导致 logm 不稳定。这时候闭式解的残差会明显偏大。非线性优化把所有约束放在一起最小化重投影误差或位姿残差能把这些噪声摊平。优化的难点在于旋转矩阵不能直接当普通向量优化必须参数化。Rodrigues 用旋转向量3 维表示旋转Cayley 用 3 维参数表示两者都能保证优化过程中旋转矩阵正交。Rodrigues 在角度接近 π 时有奇异Cayley 在角度接近 π 时数值也不稳但 Cayley 没有三角函数计算更轻。3.2 用 scipy 做带参数化的最小二乘import numpy as np from scipy.optimize import least_squares from scipy.spatial.transform import Rotation as R def rodrigues_to_matrix(rvec): return R.from_rotvec(rvec).as_matrix() def cayley_to_matrix(c): # Cayley 变换R (I - K)^-1 (I K)K 为 c 的反对称矩阵 K np.array([[0, -c[2], c[1]], [c[2], 0, -c[0]], [-c[1], c[0], 0]]) I np.eye(3) return np.linalg.inv(I - K) (I K) def residual(params, RAs, tAs, RBs, tBs, moderodrigues): if mode rodrigues: RX rodrigues_to_matrix(params[:3]) else: RX cayley_to_matrix(params[:3]) tX params[3:6] res [] for RA, tA, RB, tB in zip(RAs, tAs, RBs, tBs): # 旋转残差R_A R_X - R_X R_B 的 Frobenius 范数 res.append((RA RX - RX RB).ravel()) # 平移残差R_A t_X t_A - R_X t_B - t_X res.append(RA tX tA - RX tB - tX) return np.hstack(res) # 初值用 Tsai 结果避免陷入局部极小 x0 np.hstack([R.from_matrix(RX_init).as_rotvec(), tX_init]) sol least_squares(residual, x0, args(RAs, tAs, RBs, tBs, rodrigues), methodlm, max_nfev200)逻辑说明残差函数同时包含旋转残差和平移残差旋转残差用矩阵差的 Frobenius 范数比角度差更平滑。参数向量前 3 维是旋转参数后 3 维是平移。初值必须用闭式解否则 LM 容易收敛到错误局部极小。参数上method 选 lm 适合小残差问题max_nfev 给 200 足够再多说明初值或数据有问题。3.3 Rodrigues 和 Cayley 的选型对比对比项RodriguesCayley参数维度33计算开销含 sin/cos仅矩阵运算接近 π 时奇异数值不稳实现复杂度低scipy 直接支持中需手写变换适合场景一般工业位姿嵌入式实时优化我一般默认用 Rodrigues因为 scipy 的 Rotation 类已经处理好边界代码短、可读性好。只有在嵌入式端做在线优化、算力紧张时才换成 Cayley省掉三角函数开销。注意两者都不能在角度接近 180 度时硬用遇到这种位姿要么剔除要么改用四元数参数化。4. FOE 方法没有标定板时怎么靠运动场估手眼关系4.1 FOE 的物理直觉FOEFocus of Expansion来自光流领域当相机沿光轴方向平移时图像上的光流向量会从一个点向外辐射这个点就是 FOE。在 eye-in-hand 构型下如果机械臂做纯平移运动相机也跟着平移FOE 的位置和运动方向直接相关。通过多组平移运动估计 FOE再结合机器人平移量可以反推相机与末端的旋转关系。FOE 方法最大的价值是不依赖标定板只需要场景里有足够纹理让光流能算出来。适合标定板放不下、或者相机视野被遮挡的场合。代价是精度通常比 AXXB 低一个量级且对光流质量敏感。4.2 用光流估 FOE 的最小流程import cv2 import numpy as np def estimate_foe(prev_gray, curr_gray, K): # 稀疏光流Shi-Tomasi 角点 Lucas-Kanade pts_prev cv2.goodFeaturesToTrack(prev_gray, maxCorners200, qualityLevel0.01, minDistance10) pts_curr, status, _ cv2.calcOpticalFlowPyrLK( prev_gray, curr_gray, pts_prev, None, winSize(21, 21), maxLevel3) valid status.ravel() 1 p0 pts_prev[valid].reshape(-1, 2) p1 pts_curr[valid].reshape(-1, 2) # 光流向量 flow p1 - p0 # FOE 是光流延长线的交点用最小二乘解 # 每条光流线p0 t * flow求所有线的最接近点 A [] b [] for (x0, y0), (dx, dy) in zip(p0, flow): # 线方程dy * x - dx * y dy * x0 - dx * y0 A.append([dy, -dx]) b.append(dy * x0 - dx * y0) A np.array(A) b np.array(b) foe, _, _, _ np.linalg.lstsq(A, b, rcondNone) return foe, p0, flow逻辑说明先用 goodFeaturesToTrack 找角点再用 LK 光流跟踪得到每对点的位移向量。FOE 定义为所有光流延长线的交点把每条线写成一般式后堆叠成超定方程最小二乘解就是 FOE。参数上maxCorners 控制特征点数量200 在 640x480 下够用winSize 21 适合中等运动maxLevel 3 处理较大位移。光流质量差时valid 比例会低于 0.5这时候要降机械臂速度或增加纹理。4.3 从 FOE 到手眼旋转得到 FOE 后结合相机内参 K 和机械臂平移方向可以构造约束。设机械臂第 i 次纯平移的方向为 d_i末端坐标系FOE 在归一化像平面上的位置为 f_i则存在关系R_X * d_i ∝ K^-1 * [f_i, 1]^T多组平移运动堆叠后用 SVD 解 R_X。平移部分再用一次带旋转的线性最小二乘即可。注意 FOE 方法要求运动是纯平移任何旋转都会让 FOE 估计失效所以采集数据时要让机械臂只走平移这在 6 轴臂上需要仔细规划。5. 避坑与排查三种方法都会遇到的五个翻车点5.1 现象标定结果每次跑都不一样重复性差原因位姿采集时旋转角太小或者所有位姿绕同一个轴导致旋转约束矩阵秩亏。解决检查相邻位姿的旋转角确保大于 15 度且覆盖至少两个不平行轴。用 numpy 算一下约束矩阵的条件数超过 1e6 就重新采。5.2 现象Tsai 解出的 R_X 明显不对但残差看起来不大原因logm 在旋转角接近 0 或 π 时数值不稳定导致旋转轴估计错误。解决剔除旋转角小于 5 度或大于 175 度的位姿对或者改用四元数做旋转约束。我一般会在预处理阶段直接过滤掉这些位姿。5.3 现象非线性优化不收敛残差震荡原因初值太差或者残差函数里旋转和平移量纲不一致旋转残差是弧度级平移残差是毫米级LM 会被大量纲项主导。解决给平移残差乘一个权重让两者量级接近初值必须用闭式解不要用零向量。5.4 现象FOE 估计出来在图像外面光流明显不对原因机械臂运动包含旋转或者场景纹理太弱光流跟踪大量失败。解决确认运动是纯平移检查 valid 比例低于 0.5 就换场景或加纹理必要时用稠密光流替代稀疏光流但计算量会上去。5.5 现象三种方法结果差很多不知道信哪个原因各自假设不同AXXB 假设刚体且无噪声优化假设残差高斯FOE 假设纯平移。解决用同一批数据分别跑比较重投影误差或抓取点误差选误差最小的。我一般以优化结果为准闭式解做初值FOE 只在没有标定板时兜底。6. 进阶技巧用交叉验证挑方法而不是拍脑袋三种方法都实现之后真正的问题变成「这个项目该用哪个」。我的习惯是做一个交叉验证流程把采集的 N 组位姿分成训练集和验证集用训练集标定在验证集上算抓取点误差。具体做法是留出 3 到 5 组位姿不参与标定标定完成后用这些位姿的机器人正解和相机观测算预测抓取点和真实抓取点比。def cross_validate(RAs, tAs, RBs, tBs, methodtsai, holdout3): n len(RAs) idx np.arange(n) np.random.shuffle(idx) test_idx idx[:holdout] train_idx idx[holdout:] # 用训练集标定 if method tsai: RX, tX tsai_hand_eye([RAs[i] for i in train_idx], [tAs[i] for i in train_idx], [RBs[i] for i in train_idx], [tBs[i] for i in train_idx]) else: # 优化方法初值用 Tsai RX0, tX0 tsai_hand_eye([RAs[i] for i in train_idx], [tAs[i] for i in train_idx], [RBs[i] for i in train_idx], [tBs[i] for i in train_idx]) x0 np.hstack([R.from_matrix(RX0).as_rotvec(), tX0]) sol least_squares(residual, x0, args([RAs[i] for i in train_idx], [tAs[i] for i in train_idx], [RBs[i] for i in train_idx], [tBs[i] for i in train_idx], rodrigues), methodlm) RX rodrigues_to_matrix(sol.x[:3]) tX sol.x[3:6] # 在验证集上算误差 errs [] for i in test_idx: pred RAs[i] tX tAs[i] obs RX tBs[i] tX errs.append(np.linalg.norm(pred - obs)) return np.mean(errs), np.std(errs)逻辑说明holdout 控制验证集大小3 到 5 组足够反映泛化误差。误差定义用机器人预测平移和相机观测平移的差直接对应抓取点偏差。参数上如果误差标准差大于均值的一半说明数据一致性差要回去检查采集流程。我一般会跑三遍交叉验证取平均误差和标准差。经验值是AXXB 在数据干净时误差 0.2 到 0.5 mm优化能压到 0.1 到 0.3 mmFOE 通常在 1 mm 以上。如果优化结果比闭式解还差八成是初值或权重出了问题别急着换方法先查这两处。最后一个习惯每次标定完把 R_X 和 t_X 存成带时间戳的文件产线换型或撞车后先比对历史值偏差超过阈值再重标。这个后悔药我吃过一次亏之后就一直留着希望帮到你。本文还有配套的精品资源点击获取
简介这份PDF资料聚焦机器人视觉中的手眼标定技术面向计算机视觉、双目视觉与机器人抓取方向的学习者和研究者系统讲解末端执行器与摄像头坐标系转换关系的求解思路。内容从Euler角、Rodrigues表示、Cayley表示及FOE点等基础知识切入逐步展开手眼关系数学模型并重点对比传统两步法、Ma自标定法与Lei自标定法三种方案的实现过程最后从精度、稳定性、设备需求与计算复杂度等维度给出实验比较与选型建议。资源包内共1个PDF文件约864KB篇幅紧凑、结构清晰适合作为算法入门与方案选型的参考材料。目前已有1235人学习下载读者可借此理解AXXB方程组的构建与求解、旋转矩阵的Rodrigues推导以及不同自标定方法在动态环境或特定场景下的适用边界为后续工程实践提供理论支撑。1. 三种手眼标定方法的实现与比较从 AXXB 到 FOE哪条路在产线上不翻车产线换型时机械臂抓取精度突然掉了 2 mm十有八九是手眼标定该重做了。手眼标定要解决的事情很朴素让相机坐标系和机械臂末端或基座坐标系对齐标定结果直接决定抓取点算得准不准。业内最常落地的三条路线是基于 AXXB 的经典闭式解、基于 Rodrigues/Cayley 参数化的非线性优化、以及基于 FOEFocus of Expansion的运动场方法。三者不是替代关系而是分别对应「快、稳、省标定板」三种诉求。这篇笔记按我实际在 6 轴臂 2D 相机 3D 相机混用场景里的经验把三种方法的推导、代码、参数和踩坑一次讲透新手能照着跑通熟手能看清各自边界。2. AXXB 闭式解为什么它是大多数项目的默认起点2.1 AXXB 到底在解什么手眼标定分两种构型eye-in-hand相机装在末端和 eye-to-hand相机固定在外部看工作区。无论哪种本质都是求解相机坐标系与机械臂坐标系之间的刚体变换。经典做法是让机械臂走 N 个位姿每个位姿下拍一张标定板得到相机相对标定板的位姿再联立方程。eye-in-hand 构型下相邻两次运动的约束写成A_i X X B_i其中 A_i 是机械臂末端两次位姿之间的相对变换由机器人正运动学给出B_i 是相机两次观测之间的相对变换由标定板位姿给出X 就是待求的手眼矩阵。这个方程之所以经典是因为它把「绝对位姿未知」消掉了只依赖相对运动对机器人绝对定位误差不敏感。展开成旋转和平移两部分R_A R_X R_X R_B R_A t_X t_A R_X t_B t_X旋转部分只和 R_X 有关可以先解旋转再解平移这就是两步法的由来。常见闭式解有 Tsai-Lenz、Park-Martin、Horaud-Andreff 等工程上 Tsai 用得最多因为它对噪声的鲁棒性在多数场景够用且实现简单。2.2 用 Python 跑通 Tsai 两步法下面是我常用的最小实现依赖 numpy 和 scipy输入是 N 组 (R_A, t_A, R_B, t_B)。import numpy as np from scipy.linalg import logm, expm def skew(v): return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) def tsai_hand_eye(RAs, tAs, RBs, tBs): # 第一步用旋转约束解 R_X # 构造 (P - Q) 形式的最小二乘P、Q 由旋转轴和角度决定 M [] for RA, RB in zip(RAs, RBs): # 旋转向量轴角表示 rA logm(RA)[None] rB logm(RB)[None] # 取实部对应的旋转向量 a np.array([rA[0,2,1]-rA[0,1,2], rA[0,0,2]-rA[0,2,0], rA[0,1,0]-rA[0,0,1]]) / 2.0 b np.array([rB[0,2,1]-rB[0,1,2], rB[0,0,2]-rB[0,2,0], rB[0,1,0]-rB[0,0,1]]) / 2.0 M.append(skew(a b)) M np.vstack(M) # 对 M 做 SVD最小奇异值对应的右奇异向量即旋转轴 _, _, Vt np.linalg.svd(M) p Vt[-1] # 由轴角构造 R_X # 这里用 Rodrigues 公式角度由各次运动平均得到 angles [] for RA, RB in zip(RAs, RBs): rA logm(RA) rB logm(RB) a np.array([rA[0,2,1]-rA[0,1,2], rA[0,0,2]-rA[0,2,0], rA[0,1,0]-rA[0,0,1]]) / 2.0 b np.array([rB[0,2,1]-rB[0,1,2], rB[0,0,2]-rB[0,2,0], rB[0,1,0]-rB[0,0,1]]) / 2.0 angles.append(np.arccos(np.clip((np.trace(RA)-1)/2, -1, 1)) / np.arccos(np.clip((np.trace(RB)-1)/2, -1, 1))) theta np.mean(angles) K skew(p) RX np.eye(3) np.sin(theta)*K (1-np.cos(theta))*KK # 第二步用平移约束解 t_X C [] d [] for RA, tA, RB, tB in zip(RAs, tAs, RBs, tBs): C.append(RA - np.eye(3)) d.append(RX tB - tA) C np.vstack(C) d np.hstack(d) tX np.linalg.lstsq(C, d, rcondNone)[0] return RX, tX逻辑说明旋转部分把每次运动的旋转轴方向作为约束堆叠后做 SVD 取最小奇异向量这是 Tsai 方法的核心角度用各次运动的旋转角比值平均避免单次噪声主导。平移部分把旋转结果代入平移方程转成线性最小二乘。参数上RAs/tAs 是机器人相邻位姿的相对变换RBs/tBs 是相机相邻观测的相对变换N 建议不少于 10 组且运动要覆盖三个旋转轴否则 M 矩阵秩亏解会飘。2.3 采集位姿的三个硬约束第一相邻位姿的旋转角不能太小一般要求大于 15 度否则旋转轴估计噪声极大。第二运动要绕至少两个不平行的轴最好三个轴都覆盖纯平移运动对旋转约束没有贡献。第三标定板在每张图里都要完整可见且姿态有变化不能所有图都正对相机。我一般让机械臂走 15 到 20 个位姿其中 5 个绕 Z 转、5 个绕 X 转、5 个绕 Y 转剩下几个做复合运动。3. Rodrigues 与 Cayley 参数化把非线性优化做稳的关键3.1 为什么闭式解之后还要上优化Tsai 这类闭式解在噪声小、位姿分布好的时候精度够用但产线上常见的问题是标定板检测有 0.3 像素的抖动、机器人重复定位有 0.05 mm 误差、某些位姿下旋转角接近 180 度导致 logm 不稳定。这时候闭式解的残差会明显偏大。非线性优化把所有约束放在一起最小化重投影误差或位姿残差能把这些噪声摊平。优化的难点在于旋转矩阵不能直接当普通向量优化必须参数化。Rodrigues 用旋转向量3 维表示旋转Cayley 用 3 维参数表示两者都能保证优化过程中旋转矩阵正交。Rodrigues 在角度接近 π 时有奇异Cayley 在角度接近 π 时数值也不稳但 Cayley 没有三角函数计算更轻。3.2 用 scipy 做带参数化的最小二乘import numpy as np from scipy.optimize import least_squares from scipy.spatial.transform import Rotation as R def rodrigues_to_matrix(rvec): return R.from_rotvec(rvec).as_matrix() def cayley_to_matrix(c): # Cayley 变换R (I - K)^-1 (I K)K 为 c 的反对称矩阵 K np.array([[0, -c[2], c[1]], [c[2], 0, -c[0]], [-c[1], c[0], 0]]) I np.eye(3) return np.linalg.inv(I - K) (I K) def residual(params, RAs, tAs, RBs, tBs, moderodrigues): if mode rodrigues: RX rodrigues_to_matrix(params[:3]) else: RX cayley_to_matrix(params[:3]) tX params[3:6] res [] for RA, tA, RB, tB in zip(RAs, tAs, RBs, tBs): # 旋转残差R_A R_X - R_X R_B 的 Frobenius 范数 res.append((RA RX - RX RB).ravel()) # 平移残差R_A t_X t_A - R_X t_B - t_X res.append(RA tX tA - RX tB - tX) return np.hstack(res) # 初值用 Tsai 结果避免陷入局部极小 x0 np.hstack([R.from_matrix(RX_init).as_rotvec(), tX_init]) sol least_squares(residual, x0, args(RAs, tAs, RBs, tBs, rodrigues), methodlm, max_nfev200)逻辑说明残差函数同时包含旋转残差和平移残差旋转残差用矩阵差的 Frobenius 范数比角度差更平滑。参数向量前 3 维是旋转参数后 3 维是平移。初值必须用闭式解否则 LM 容易收敛到错误局部极小。参数上method 选 lm 适合小残差问题max_nfev 给 200 足够再多说明初值或数据有问题。3.3 Rodrigues 和 Cayley 的选型对比对比项RodriguesCayley参数维度33计算开销含 sin/cos仅矩阵运算接近 π 时奇异数值不稳实现复杂度低scipy 直接支持中需手写变换适合场景一般工业位姿嵌入式实时优化我一般默认用 Rodrigues因为 scipy 的 Rotation 类已经处理好边界代码短、可读性好。只有在嵌入式端做在线优化、算力紧张时才换成 Cayley省掉三角函数开销。注意两者都不能在角度接近 180 度时硬用遇到这种位姿要么剔除要么改用四元数参数化。4. FOE 方法没有标定板时怎么靠运动场估手眼关系4.1 FOE 的物理直觉FOEFocus of Expansion来自光流领域当相机沿光轴方向平移时图像上的光流向量会从一个点向外辐射这个点就是 FOE。在 eye-in-hand 构型下如果机械臂做纯平移运动相机也跟着平移FOE 的位置和运动方向直接相关。通过多组平移运动估计 FOE再结合机器人平移量可以反推相机与末端的旋转关系。FOE 方法最大的价值是不依赖标定板只需要场景里有足够纹理让光流能算出来。适合标定板放不下、或者相机视野被遮挡的场合。代价是精度通常比 AXXB 低一个量级且对光流质量敏感。4.2 用光流估 FOE 的最小流程import cv2 import numpy as np def estimate_foe(prev_gray, curr_gray, K): # 稀疏光流Shi-Tomasi 角点 Lucas-Kanade pts_prev cv2.goodFeaturesToTrack(prev_gray, maxCorners200, qualityLevel0.01, minDistance10) pts_curr, status, _ cv2.calcOpticalFlowPyrLK( prev_gray, curr_gray, pts_prev, None, winSize(21, 21), maxLevel3) valid status.ravel() 1 p0 pts_prev[valid].reshape(-1, 2) p1 pts_curr[valid].reshape(-1, 2) # 光流向量 flow p1 - p0 # FOE 是光流延长线的交点用最小二乘解 # 每条光流线p0 t * flow求所有线的最接近点 A [] b [] for (x0, y0), (dx, dy) in zip(p0, flow): # 线方程dy * x - dx * y dy * x0 - dx * y0 A.append([dy, -dx]) b.append(dy * x0 - dx * y0) A np.array(A) b np.array(b) foe, _, _, _ np.linalg.lstsq(A, b, rcondNone) return foe, p0, flow逻辑说明先用 goodFeaturesToTrack 找角点再用 LK 光流跟踪得到每对点的位移向量。FOE 定义为所有光流延长线的交点把每条线写成一般式后堆叠成超定方程最小二乘解就是 FOE。参数上maxCorners 控制特征点数量200 在 640x480 下够用winSize 21 适合中等运动maxLevel 3 处理较大位移。光流质量差时valid 比例会低于 0.5这时候要降机械臂速度或增加纹理。4.3 从 FOE 到手眼旋转得到 FOE 后结合相机内参 K 和机械臂平移方向可以构造约束。设机械臂第 i 次纯平移的方向为 d_i末端坐标系FOE 在归一化像平面上的位置为 f_i则存在关系R_X * d_i ∝ K^-1 * [f_i, 1]^T多组平移运动堆叠后用 SVD 解 R_X。平移部分再用一次带旋转的线性最小二乘即可。注意 FOE 方法要求运动是纯平移任何旋转都会让 FOE 估计失效所以采集数据时要让机械臂只走平移这在 6 轴臂上需要仔细规划。5. 避坑与排查三种方法都会遇到的五个翻车点5.1 现象标定结果每次跑都不一样重复性差原因位姿采集时旋转角太小或者所有位姿绕同一个轴导致旋转约束矩阵秩亏。解决检查相邻位姿的旋转角确保大于 15 度且覆盖至少两个不平行轴。用 numpy 算一下约束矩阵的条件数超过 1e6 就重新采。5.2 现象Tsai 解出的 R_X 明显不对但残差看起来不大原因logm 在旋转角接近 0 或 π 时数值不稳定导致旋转轴估计错误。解决剔除旋转角小于 5 度或大于 175 度的位姿对或者改用四元数做旋转约束。我一般会在预处理阶段直接过滤掉这些位姿。5.3 现象非线性优化不收敛残差震荡原因初值太差或者残差函数里旋转和平移量纲不一致旋转残差是弧度级平移残差是毫米级LM 会被大量纲项主导。解决给平移残差乘一个权重让两者量级接近初值必须用闭式解不要用零向量。5.4 现象FOE 估计出来在图像外面光流明显不对原因机械臂运动包含旋转或者场景纹理太弱光流跟踪大量失败。解决确认运动是纯平移检查 valid 比例低于 0.5 就换场景或加纹理必要时用稠密光流替代稀疏光流但计算量会上去。5.5 现象三种方法结果差很多不知道信哪个原因各自假设不同AXXB 假设刚体且无噪声优化假设残差高斯FOE 假设纯平移。解决用同一批数据分别跑比较重投影误差或抓取点误差选误差最小的。我一般以优化结果为准闭式解做初值FOE 只在没有标定板时兜底。6. 进阶技巧用交叉验证挑方法而不是拍脑袋三种方法都实现之后真正的问题变成「这个项目该用哪个」。我的习惯是做一个交叉验证流程把采集的 N 组位姿分成训练集和验证集用训练集标定在验证集上算抓取点误差。具体做法是留出 3 到 5 组位姿不参与标定标定完成后用这些位姿的机器人正解和相机观测算预测抓取点和真实抓取点比。def cross_validate(RAs, tAs, RBs, tBs, methodtsai, holdout3): n len(RAs) idx np.arange(n) np.random.shuffle(idx) test_idx idx[:holdout] train_idx idx[holdout:] # 用训练集标定 if method tsai: RX, tX tsai_hand_eye([RAs[i] for i in train_idx], [tAs[i] for i in train_idx], [RBs[i] for i in train_idx], [tBs[i] for i in train_idx]) else: # 优化方法初值用 Tsai RX0, tX0 tsai_hand_eye([RAs[i] for i in train_idx], [tAs[i] for i in train_idx], [RBs[i] for i in train_idx], [tBs[i] for i in train_idx]) x0 np.hstack([R.from_matrix(RX0).as_rotvec(), tX0]) sol least_squares(residual, x0, args([RAs[i] for i in train_idx], [tAs[i] for i in train_idx], [RBs[i] for i in train_idx], [tBs[i] for i in train_idx], rodrigues), methodlm) RX rodrigues_to_matrix(sol.x[:3]) tX sol.x[3:6] # 在验证集上算误差 errs [] for i in test_idx: pred RAs[i] tX tAs[i] obs RX tBs[i] tX errs.append(np.linalg.norm(pred - obs)) return np.mean(errs), np.std(errs)逻辑说明holdout 控制验证集大小3 到 5 组足够反映泛化误差。误差定义用机器人预测平移和相机观测平移的差直接对应抓取点偏差。参数上如果误差标准差大于均值的一半说明数据一致性差要回去检查采集流程。我一般会跑三遍交叉验证取平均误差和标准差。经验值是AXXB 在数据干净时误差 0.2 到 0.5 mm优化能压到 0.1 到 0.3 mmFOE 通常在 1 mm 以上。如果优化结果比闭式解还差八成是初值或权重出了问题别急着换方法先查这两处。最后一个习惯每次标定完把 R_X 和 t_X 存成带时间戳的文件产线换型或撞车后先比对历史值偏差超过阈值再重标。这个后悔药我吃过一次亏之后就一直留着希望帮到你。本文还有配套的精品资源点击获取
