企业官网建设流程全解析

去年年底接了个视觉引导抓取的项目，相机是一台结构光 3D 相机，机械臂是六轴协作臂。排期压得很紧，可真正卡住进度的不是抓取算法，而是手眼标定——相机装上去、标定板摆好、机械臂动起来，结果解出来的外参每次都不一样，改一个位姿结果就漂几毫米。后来我换了个思路：先在 CoppeliaSim（老版本叫 v-REP）里把整条链路搭出来跑通，拿到"标准答案"之后再上真机。这一步花了我大概两天，却省掉了后面至少两周的反复试错。

这篇就把 CoppeliasSim 里做机器人 3D 相机手眼标定、并为后续实时视觉追踪打基础的全过程拆开讲。核心关键词就四个：CoppeliaSim 仿真、机器人、3D 相机、手眼标定。不管你是刚接触手眼标定的学生，还是被真机标定折磨过的工程师，这里面的思路和坑都能直接拿去用。第一部分先聚焦"仿真建模 + 标定数据采集 + 求解验证"，实时追踪的实现放到第二部分。

1. 先想清楚：为什么手眼标定值得放进 CoppeliaSim 里做第一遍

1.1 真机上标定一次，成本到底花在哪里

很多人以为手眼标定的成本是"算"，其实计算只需要几毫秒。真机上真正吃时间的是三件事：摆位、采数、验证。

摆位这一步，你得让标定板在相机视野里、机械臂在工作空间内、每一帧都能稳定检出角点，还要保证位姿之间有足够的旋转差异。机械臂每走一个位姿，你要确认它没撞到标定板、没超出关节限位、相机线缆没被拉扯。二十个位姿走下来，半小时起步。

采数环节更麻烦。真实 3D 相机的输出带噪声、带缺失点，结构光在反光或者倾斜角度大的表面上直接丢点。你采了二十组数据，其中三组角点检出不稳，你还得判断是删掉重采还是留着。验证环节最要命：你没有真值。标定完只能看残差，残差小不代表结果对——坐标系搞反了，残差照样能很小，但抓取时机械臂会朝着完全错误的方向伸过去。

1.2 仿真给你的三样东西：真值、可重复、零成本试错

CoppeliaSim 这类仿真器最大的价值不是"看着像"，而是场景图里每一个物体的位姿都是精确已知的。这就意味着你能拿到真值。

拿真值能干什么？举几个我这几天用得最多的场景。

第一，验证数学方向。手眼标定有 eye-in-hand 和 eye-to-hand 两种构型，OpenCV 的cv2.calibrateHandEye返回的是cam2gripper，而很多网上代码在 eye-to-hand 场景下直接套用，结果全反。在仿真里，你知道相机和末端的真实相对位姿，只要把求解结果和真值一比，方向对不对立刻就清楚了，不用去猜。

第二，隔离变量。真机上你分不清误差是来自相机内参不准、角点检测偏移，还是机械臂运动学标定不准。仿真里你可以先用真值位姿喂给求解器，如果结果还不准，那一定是数学或者代码问题；如果结果准了，再把角点检测替换进去，误差立刻就能归因。

第三，可重复。同一组轨迹、同一组噪声种子，跑一百次结果完全一致。这一点在调试阶段特别重要。

1.3 仿真也会骗你：哪些误差它不会替你还原

这里必须泼一盆冷水。CoppeliaSim 不会自动帮你复现下面这些东西：

• 相机内参误差。仿真的内参是你自己根据视场角和分辨率算出来的，理想情况下完全准确。真机上镜头畸变、装配误差、焦距标称值偏差都会有。
• 机械臂运动学误差。仿真的正运动学是完美的 DH 参数计算，真机有连杆加工误差和减速器回差。
• 结构光特有的失效模式。多路径干涉、反光表面丢点、边缘飞点，这些在仿真里要自己手动注入才能复现。
• 时间同步误差。真机上相机曝光时刻和机械臂编码器读数时刻往往差几毫秒到几十毫秒，机械臂高速运动时这个误差会直接变成位姿误差。

所以我的建议是：仿真负责把算法链路和坐标变换跑对，真机负责验证鲁棒性和精度指标。别指望仿真出来的标定误差能代表真机精度，也别因为仿真里残差只有 0.01 毫米就觉得真机也能做到。

2. 把场景搭起来：机器人本体、3D 相机与标定板的坐标约定

2.1 机械臂模型与 TCP 定标点的建立

CoppeliaSim 的模型库（Models → robots → non-mobile）里有现成的六轴机械臂，UR5、KUKA LBR iiwa、Franka Panda 都有。选哪个其实差别不大，关键是关节数要够、工作空间要能覆盖标定板的摆放范围。我用的是一台 UR 系的六轴臂，原因是它的关节限位比较宽松，做大幅度姿态变化时不容易撞限位。

模型拖进场景之后，第一件事不是急着装相机，而是确认末端法兰的坐标系原点在哪。这一点很容易被忽略。CoppeliaSim 里机械臂模型的末端tip或者connectiondummy 往往挂在法兰中心，但真实的抓取工具（吸盘、夹爪）会往前伸出一段距离。如果你在仿真里把相机装在法兰上，但实际相机装在夹爪末端，那标定出来的结果就差一个固定的平移量。

我的做法是：在法兰前方按真实工具的长度新建一个 Dummy，命名为TCP，把它作为机械臂tip的子对象。后面所有涉及"末端位姿"的读取，全部读TCP相对于base的变换矩阵，而不是读tip。这个约定一旦定下来，就不要中途改。

注意：CoppeliaSim 里 Dummy 默认的朝向可能和法兰不一致，建完之后一定要用sim.getObjectMatrix打印一次相对变换，确认它是不是单位矩阵加一个已知平移。如果带上了一个莫名的旋转，后面手眼标定的旋转部分会整体偏掉。

2.2 用视觉传感器"造"一台 3D 相机：深度图、内参与点云

CoppeliaSim 里没有现成的"3D 相机"对象，但有视觉传感器（Vision Sensor）。它的渲染管线会输出两张图：RGB 图和深度缓冲。把深度缓冲配合内参还原成点云，你就得到了一台虚拟的深度相机。

具体步骤是这样的。先往场景里加一个 Vision Sensor，挂到 TCP 下面。然后在它的属性面板里设置：

内参怎么算？理想针孔模型下，如果视场角作用在水平方向：

fx = (img_w / 2) / tan(hfov / 2) fy = fx # 像素为正方形时两者相等 cx = img_w / 2 cy = img_h / 2

但 CoppeliaSim 的Perspective angle具体作用在哪个方向，不同版本的手册描述不完全一致。我的做法是用标定板反标一次：把一块已知尺寸的棋盘格放在相机正前方已知距离处，渲染一帧，检测角点像素坐标，反过来拟合 fx、fy、cx、cy。这一步花不了十分钟，但能省掉后面无数次的怀疑人生。

深度缓冲转点云的时候，有个坐标系约定必须提前搞清楚。CoppeliaSim 视觉传感器的局部坐标系是：X 轴在图像中指向左侧，Y 轴指向上方，Z 轴是视线方向。而 OpenCV 的相机坐标系是 X 右、Y 下、Z 前。两者相差一个绕 Z 轴 180° 的旋转。

这个旋转矩阵写出来就是：

import numpy as np # CoppeliaSim 视觉传感器坐标系 -> OpenCV 相机坐标系 R_cv_from_sim_cam = np.array([ [-1.0, 0.0, 0.0], [ 0.0, -1.0, 0.0], [ 0.0, 0.0, 1.0], ]) # det = +1，是合法旋转矩阵，即绕 Z 轴转 180°

深度缓冲的取值是归一化的 0~1，0 对应近裁剪面。映射到米制深度时，是线性映射还是透视映射，不同版本的实现我见过差异，所以别直接抄公式。稳妥做法是：在相机正前方放三个已知距离的平面（比如 0.3 m、0.8 m、1.5 m），各渲染一帧，读出中心像素的深度值，用三个点拟合出映射曲线，再决定用线性还是二次模型。

点云生成代码长这样：

def depth_to_cloud(depth_norm, near, far, fx, fy, cx, cy, img_w, img_h): u, v = np.meshgrid(np.arange(img_w), np.arange(img_h)) # 先把归一化深度还原成米制（这里用线性模型举例，务必先用已知平面验证） z = near + depth_norm * (far - near) # 再按 OpenCV 相机坐标系输出：X 右、Y 下、Z 前 x = (u - cx) * z / fx y = (v - cy) * z / fy return np.stack([x, y, z], axis=-1)

如果你要模拟的是结构光 3D 相机而不是理想深度相机，还要再叠两层：

• 轴向噪声。结构光的深度噪声近似随距离平方增长，可以写成sigma_z = a * z^2 + b * z + c，根据你手上相机的实际指标拟合。远距离处噪声几个毫米是常态。
• 丢点。按距离阈值和局部梯度阈值随机把一部分像素置为无效值，模拟反光和遮挡导致的缺失。这一步特别重要，因为真实的点云从来不是稠密的，你的算法必须在有空洞的情况下也能工作。

2.3 标定板的摆放与共视区域检查

标定板用什么？我推荐棋盘格或者 ChArUco。棋盘格胜在角点定位精度高，ChArUco 胜在即使部分遮挡也能识别出板子在哪个位置。仿真里图像干净锐利，棋盘格足够了。

摆位置的原则是：标定板要固定在场景里不动（eye-in-hand 场景下，板子固定、相机跟着末端动），而且要落在机械臂能带着相机绕它转一圈的范围内。板子太小，角点像素精度不够；板子太大，机械臂姿态变化时会出视野。

我的经验值是：标定板对角线长度占相机视野的 1/3 到 1/2，距离相机 0.4~0.8 m。这个范围内，角点在图像上分布得开，姿态估计的旋转分量比较稳。

摆好之后一定要做一次共视检查：手动拖机械臂走几个极限姿态，看标定板是不是始终有至少 80% 的面积在视野内。CoppeliaSim 的视觉传感器可以在场景里打开"显示视野锥"的选项，能直观看到覆盖范围。

3. AX=XB 到底在解什么：手眼标定的数学内核

3.1 eye-in-hand 与 eye-to-hand 的方程写法差异

手眼标定的核心方程就一行：AX = XB。但很多人卡在"这个 X 到底是什么"上，因为它取决于你用的是哪种构型。

eye-in-hand（眼在手上）：相机固定在机械臂末端，跟着一起动。标定板固定在工作台上。此时 X 是相机在末端坐标系下的位姿，也就是T_gripper_cam。

推导很简单。标定板在基座系下的位姿是固定的：

T_base_target = T_base_gripper_i · T_gripper_cam · T_cam_target_i

右边对任意位姿 i 都相等，取两个位姿 i、j 联立，就能整理成A X = X B的形式，其中 A 是末端的相对运动，B 是标定板的相对运动。

eye-to-hand（眼在手外）：相机固定在工作台上，标定板跟着末端动。此时联立方程整理出来还是AX=XB，但解出来的 X 变成了标定板在末端坐标系下的位姿，也就是T_gripper_target。

这个差异是很多人翻车的地方。eye-to-hand 下用cv2.calibrateHandEye直接跑，拿到一个矩阵就当成cam2base用，实际上它根本不是相机外参。正确做法是：先解出T_gripper_target，再用任意一个位姿反推相机的基座系位姿：

T_base_cam = T_base_gripper_i · T_gripper_target · T_cam_target_i^-1

还有一个更省事的做法：把target2cam的那一组位姿整体取逆再传进求解器，相当于把运动链方向翻转过来。我自己的习惯是在仿真里把两种传法都跑一遍，看哪一种能复现出真值，确定下来之后再写死到代码里。

3.2 旋转先解、平移后解的理由

AX=XB这个方程，写成旋转和平移两部分是这样的：

R_A · R_X = R_X · R_B R_A · t_X + t_A = R_X · t_B + t_X

第一行只含旋转，第二行把平移和旋转耦合在一起。所以所有经典解法（Tsai-Lenz、Park、Horaud、Daniilidis）都是先解旋转，再代回去解平移。

为什么要分两步？因为旋转方程R_A R_X = R_X R_B可以等价改写成把R_X看成把R_A和R_B联系起来的相似变换，用四元数或者旋转向量表示之后会变成一个线性最小二乘问题，有闭式解。而平移方程里t_X的系数是(R_A - I)，这个矩阵在 A 的旋转角接近 0 的时候会接近奇异，解出来的平移会非常不稳定。

这就引出两个实操结论：

1. 位姿序列里必须包含足够的旋转。如果机械臂只做纯平移，R_A = I，平移方程直接退化成t_A = 0，无解。
2. 不要出现绕单一轴转的情况。如果所有相对旋转都绕同一个轴，旋转方程会欠定，解出来的R_X在垂直于这个轴的平面上是任意值。

OpenCV 提供了五种解法，我实测下来的倾向是：

仿真数据干净的情况下五种方法结果应该高度一致。如果你发现它们给出的结果差异很大，那说明数据本身有问题，而不是方法选错了。

3.3 为什么仿真里也必须注入噪声

仿真里数据是完美的，五种方法解出来的结果几乎一样，误差在浮点数精度级别。这看起来很爽，但它会给你一个虚假的安全感。

举我自己的例子。仿真里用完美数据跑出来，旋转误差 0.0001 度，平移误差 0.0001 毫米。然后我把这套代码搬到真机上，误差直接到了 3 毫米。中间发生了什么？噪声放大。

手眼标定对噪声的放大倍数，和你的位姿序列条件数直接相关。条件数差的数据，噪声放大几十倍很正常。所以在仿真里做验证时，一定要主动注入噪声，把真机的噪声水平搬过来：

• 机械臂位姿噪声：在t_gripper2base上加高斯噪声，标准差取 0.1~0.5 mm；在旋转上叠加 0.01~0.05 度的小角度扰动。这对应真机的运动学误差。
• 目标位姿噪声：这个更关键。在t_target2cam上加 0.5~2 mm 的噪声，旋转加 0.1~0.5 度。因为标定板的位姿是从图像角点解出来的，远端的角点像素误差会被放大成可观的位姿误差。

注入噪声之后再跑，看误差怎么变。如果你的算法在仿真噪声下就开始崩，那真机上必然不行。这一步是最有价值的门槛测试。

4. 采集标定数据的实操：位姿怎么选才不至于解出废解

4.1 关节轨迹设计：绕多个轴转，而不是只转一个轴

这是我看过最多的新手错误：手动拖机械臂，只绕着腕部关节转了几圈。结果所有相对旋转都绕同一个轴，解出来的平移在另外两个方向上是随机的。

好的轨迹应该满足：相对旋转的转轴要张开在三维空间里。我的具体做法是分三组：

第一组，平移主导。保持姿态基本不变，让末端在平面上走一个 3×3 的网格，间距 5 cm。这组用来提供平移信息，但注意不要全是纯平移，适当地在每个点加一点点姿态变化。

第二组，绕三个轴各转一次。在同一个位置，分别绕末端坐标系的 X、Y、Z 轴旋转 ±20~30 度，各取 4 个位姿。这组提供三个正交方向的旋转约束。

第三组，复合运动。平移加旋转同时做，模拟实际工作时的姿态。取 6~8 个位姿。

加起来大概 20 组数据。每组数据包含一组关节角和一组采集到的位姿，采集完立刻在脚本里算一次当前位姿和前面所有位姿的旋转角，如果发现某个位姿和所有已采集位姿的旋转角都小于 5 度，就把它丢掉重采。

4.2 一次完整采集的脚本流程与数据结构

在 CoppeliaSim 里采集数据，用 Python 远程 API 最顺手（4.5 之后推荐用 ZMQ 那套）：

from coppeliasim_zmqremoteapi_client import RemoteAPIClient import numpy as np import cv2 client = RemoteAPIClient() sim = client.require('sim') base = sim.getObject('/UR5/base') tcp = sim.getObject('/UR5/tip/TCP') cam = sim.getObject('/UR5/tip/TCP/visionSensor') board = sim.getObject('/CalibrationBoard') def sim_matrix_to_RT(m): M = np.array(m, dtype=np.float64).reshape(3, 4) # 行优先的 3x4 return M[:3, :3].copy(), M[:3, 3].copy() R_g2b_list, t_g2b_list = [], [] R_t2c_list, t_t2c_list = [], [] for i, q in enumerate(pose_list): # 1) 设置关节角（位置控制或直接设，取决于你的模型配置） for j in range(6): sim.setJointPosition(joints[j], q[j]) client.step() # 步进一帧，等待渲染完成 client.step() # 结构光需要多帧稳定，多走几帧更保险 # 2) 读末端相对基座的真值位姿 R_g2b, t_g2b = sim_matrix_to_RT(sim.getObjectMatrix(tcp, base)) # 3) 读标定板相对相机坐标系的位姿（仿真真值，用于先把数学链路跑通） R_t2c, t_t2c = sim_matrix_to_RT(sim.getObjectMatrix(board, cam)) R_g2b_list.append(R_g2b); t_g2b_list.append(t_g2b) R_t2c_list.append(R_t2c); t_t2c_list.append(t_t2c) # 4) 保存一帧图像，供后面做真实的角点检测 img, res = sim.getVisionSensorImg(cam) save_frame(i, img, res)

这里有个细节值得说：client.step()要调用两次。第一次让物理引擎推进，第二次让视觉传感器完成渲染。只调一次的话，你读到的图像可能是上一帧的，机械臂已经动了但图像没更新。这个坑我在真机上对标定时也遇到过——工业相机的触发和机械臂状态读取之间需要对时，本质上是同一类问题。

4.3 目标位姿的两种获取方式：真值直取与图像检测

注意上面代码里的第 3 步，我用的是sim.getObjectMatrix(board, cam)直接读真值。这是故意的，也是仿真最大的便利。

正确的调试顺序应该是这样：

第一阶段，用真值位姿跑手眼标定。这一步的目的是验证数学链路、坐标系约定、求解器调用顺序。如果你用真值都解不出正确结果，那问题百分之百在代码或者坐标变换上，跟检测精度无关。

第二阶段，把真值换成图像检测结果。用cv2.findChessboardCorners在渲染图上找角点，再cv2.solvePnP解出标定板在相机下的位姿。这时候如果结果变差了，说明是检测环节的问题——可能是内参不对、可能是渲染图对比度不够、可能是棋盘格尺寸填错了。

第三阶段，在点云上做检测。真实的 3D 相机不一定给你 RGB 图，可能只给点云。这时候要改成在点云里拟合平面、提取角点。仿真里这一步可以先用第一阶段的真值点云验证算法，再叠加上 2.2 节说的噪声。

三阶段分开做，每阶段的误差来源完全不同，归因特别清楚。合在一起做的话，你永远不知道是哪个环节出的问题。

提示：cv2.findChessboardCorners在渲染图上偶尔会因为抗锯齿导致角点定位有亚像素偏移。开cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE标志，再用cv2.cornerSubPix做一次亚像素细化，精度能提升一个档次。

5. 求解与复验：从 cv2.calibrateHandEye 到回灌场景做闭环

5.1 calibrateHandEye 的输入顺序与返回值方向

cv2.calibrateHandEye的签名是这样的：

R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye( R_gripper2base, t_gripper2base, # 列表，每个元素 3x3 / 3x1 R_target2cam, t_target2cam, # 列表，长度必须一致 method=cv2.CALIB_HAND_EYE_PARK )

三个必须记住的点：

第一，输入顺序不能反。第一个参数是末端相对基座的位姿，第二个参数是标定板相对相机的位姿。反了的话，解出来的东西物理意义完全不对。

第二，返回值是cam2gripper，不是gripper2cam。这个命名规则是"从相机系变换到末端系"，也就是T_gripper_cam。如果你需要反过来的T_cam_gripper，自己取逆。

第三，eye-to-hand 要改数据方向。前面 3.1 节说过，把target2cam那一组位姿取逆之后再传进去，求解出来的才是相机外参。我建议在仿真里把两种传法各跑一遍，用真值判断。

取逆的辅助函数：

def invert_RT(R, t): Rt = R.T tt = -R.T @ t return Rt, tt

5.2 用仿真真值量化误差，而不是只看残差

这是仿真相对真机最大的优势。真机上你只有残差，仿真里你有真值。

具体做法：在场景里读一次相机相对末端的真实位姿，和求解结果对比。

# 真值：相机在 TCP 坐标系下的位姿 R_gt, t_gt = sim_matrix_to_RT(sim.getObjectMatrix(cam, tcp)) # 求解结果 R_est, t_est = R_cam2gripper, t_cam2gripper # 旋转误差（角度制） R_err = R_gt.T @ R_est cos_angle = np.clip((np.trace(R_err) - 1.0) / 2.0, -1.0, 1.0) angle_err_deg = np.degrees(np.arccos(cos_angle)) # 平移误差 t_err_mm = np.linalg.norm(t_est - t_gt) * 1000.0 print(f"旋转误差 {angle_err_deg:.4f} 度, 平移误差 {t_err_mm:.4f} 毫米")

有了这两个数，你就能建立一个判断标准。我自己的经验阈值是：仿真无噪声数据下，旋转误差应该小于 0.001 度、平移小于 0.01 毫米；注入真机级噪声后，旋转 0.05 度以内、平移 0.5 毫米以内算合格；如果注入噪声后误差超过 2 毫米，位姿序列的条件数大概率有问题，应该回去重新设计轨迹。

还有一个更有信息量的检查：把每一组数据代进T_base_gripper · T_gripper_cam · T_cam_target，算出标定板在基座系下的位姿。理论上这 20 组算出来的应该完全一致（板子没动）。计算它们两两之间的距离，画出分布。如果某几组明显偏离，那几组的数据就是坏点。

results = [] for R_g2b, t_g2b, R_t2c, t_t2c in zip(R_g2b_list, t_g2b_list, R_t2c_list, t_t2c_list): T_bg = Rt_to_T(R_g2b, t_g2b) T_gc = Rt_to_T(R_cam2gripper, t_cam2gripper) # 注意这是 T_gripper_cam T_ct = Rt_to_T(R_t2c, t_t2c) T_bt = T_bg @ T_gc @ T_ct results.append(T_bt) # 计算所有结果的平移分量标准差和旋转分量标准差 t_std_mm = np.std([T[:3, 3] for T in results], axis=0) * 1000.0 print("标定板位姿一致性（毫米）:", t_std_mm)

这个指标比残差直观得多——残差是拟合的产物，一致性是物理约束。板子没动却算出了不同的位置，那一定有问题。

5.3 回灌验证：让机械臂去"抓"一次虚拟目标

数值上的误差看着漂亮不代表能用。我喜欢做最后一个动作：把标定结果回灌到场景里，做一次视觉引导的虚拟抓取。

流程是这样的：

1. 在场景里随便放一个小方块，位置不要告诉算法。
2. 机械臂带着相机走到某个姿态，渲染一帧，从点云里算出方块在相机坐标系下的三维位置t_cam_obj。
3. 用标定结果换算到基座系：t_base_obj = T_base_gripper · T_gripper_cam · t_cam_obj。
4. 把计算结果和方块的真值位置对比，看偏差多少。
5. 更进一步：让机械臂直接运动到这个算出来的位置，看末端是不是真的贴近了方块。

第 5 步最有说服力。数值误差是抽象的，看到末端离方块差了 3 厘米，你立刻就知道这套标定不能用。

注意：回灌验证时，T_base_gripper必须是和标定时同一套正运动学读出来的。如果你标定时读的是TCP，验证时读的是tip，那中间相差的工具偏移会直接体现在误差里，白白让你怀疑标定结果。

6. 最容易翻车的三处坐标变换：Z-up、单位与欧拉角

6.1 CoppeliaSim 的 Z-up 世界与 OpenCV 的相机坐标系

CoppeliaSim 的世界坐标系是Z 轴向上的右手系，和 ROS 一致。OpenCV 用的是相机坐标系 X 右、Y 下、Z 前。这两个体系之间的转换本身不难，难的是你会在多个地方需要转换，而每个地方的方向定义还不一样。

最容易搞混的是这几个：

CoppeliaSim 视觉传感器到 OpenCV 相机之间是绕 Z 轴 180°。CoppeliaSim 世界到 OpenCV 相机之间，则取决于相机本身的安装姿态，没有固定公式，必须通过sim.getObjectMatrix(cam, sim.handle_world)实时读取。

我的经验做法是：所有跨坐标系的运算，一律先转成 4×4 齐次矩阵，用矩阵乘法串起来，绝不在中间环节用欧拉角。欧拉角只用来在最后显示给人看。

6.2 单位、欧拉角顺序与旋转矩阵的转换细节

CoppeliaSim 内部用的是米，OpenCV 也是米。这一点倒是一致的。但标定板的格子尺寸你如果按毫米填，solvePnP解出来的平移就会大 1000 倍。这种错误非常隐蔽，因为残差依然很小，只是整体尺度错了。

更麻烦的是欧拉角。CoppeliaSim 的sim.getObjectOrientation返回三个欧拉角，按 X→Y→Z 的顺序作用。但"作用顺序"这个词本身就歧义：是绕固定轴依次转，还是绕当前轴依次转？两者的旋转矩阵一个是Rz·Ry·Rx，另一个是Rx·Ry·Rz。

我的建议是：绕开欧拉角，直接读矩阵。CoppeliaSim 提供了sim.getObjectMatrix，返回行优先排列的 3×4 矩阵前 12 个元素，直接包含旋转和平移。用这个，就完全不会有欧拉角顺序的歧义。

def sim_matrix_to_RT(m): M = np.array(m, dtype=np.float64).reshape(3, 4) R = M[:3, :3] t = M[:3, 3] # 检查一下正交性，CoppeliaSim 偶尔会返回带数值误差的矩阵 U, _, Vt = np.linalg.svd(R) R = U @ Vt if np.linalg.det(R) < 0: U[:, -1] *= -1 R = U @ Vt return R, t

这段代码里的 SVD 正交化不能省。CoppeliaSim 通过 API 传回来的矩阵是从内部四元数或者变换栈里还原的，可能带 1e-8 级别的数值误差。cv2.calibrateHandEye里对旋转矩阵的正交性有要求，轻微不正交在某些解法下会被放大。

6.3 坐标系错位时的典型症状对照表

调试时最有用的不是公式，是症状。下面这张表是我自己攒的，出问题时先对号入座：

这张表里的每一条我都至少踩过一次。特别是第一条和最后一条，"固定的大向量"这个特征非常典型——它不是随机误差，是系统性偏差，一定来自某个被遗漏的静态变换。

7. 给实时视觉追踪留好接口（为第二部分做准备）

7.1 从 T_gripper_cam 到目标在基座系下的三维坐标

标定的最终目的不是为了那个矩阵，而是为了把相机看到的东西换算到机械臂能理解的位置。

有了T_gripper_cam，任意一个目标在相机坐标系下的位置p_cam，都可以换算到基座系：

p_base = T_base_gripper · T_gripper_cam · p_cam

注意T_base_gripper必须是实时读取的，不能缓存在标定时刻的值。机械臂一动，这个矩阵就变了。

如果目标是用点云检测出来的（比如做平面拟合找圆柱物体的中心），你还需要把相机系的旋转也带上：

def cam_point_to_base(p_cam, R_g2b, t_g2b, R_c2g): # R_c2g / t_c2g 是 calibrateHandEye 的返回值，即 T_gripper_cam p_base = R_g2b @ (R_c2g @ p_cam + t_c2g) + t_g2b return p_base

这个函数是实时追踪循环里被调用最频繁的一段。它的输入只有相机检测结果和当前末端位姿，整个过程不涉及任何图像处理，可以跑到几千赫兹。性能瓶颈永远在检测那一端。

7.2 仿真主循环里的实时数据回传与闭环控制切入点

第二部分要做的实时视觉追踪，本质上就是把这个换算放进一个循环里，再把结果送给机械臂。在 CoppeliaSim 里，这个循环有两种搭法。

一种是外部循环。用 Python 远程 API，主脚本里跑 while 循环：读图像 → 检测 → 换算 → 发送目标关节角 →client.step()。优点是算法全在 Python 里，能用 OpenCV、NumPy 全套生态；缺点是每一步都要走一次网络往返，版本不同、配置不同，实际循环频率可能只有 20~50 Hz。

另一种是内部循环。把检测算法直接写成 CoppeliaSim 的子脚本（Lua）或者嵌入式 Python 脚本，跑在仿真线程里。频率能到几百赫兹，控制延迟小，但调试不方便，而且需要用 CoppeliaSim 自己的图像 API。

我的选择是混合：检测用外部循环做原型，验证通了之后再往内部搬。因为实时追踪的难点从来不是循环频率，而是当机械臂动起来之后，标定结果还准不准。

这里有个必须在第二部分解决、但仿真里能提前发现的问题：如果相机固定安装（eye-to-hand），手眼标定的结果对追踪是直接可用的；如果相机装在末端（eye-in-hand），每次机械臂移动你都要重新计算目标在基座系下的位置，而这个位置理论上应该不变。这个"不变性"是检验标定质量最好的实时指标——让机械臂在工作空间里随便走一圈，看同一个静止目标算出来的基座系坐标漂不漂。漂多少，就是你的标定精度上限。

仿真里跑这一步几乎没有成本，鼠标一拖就能试几十个姿态。真机上做同样的事，得写轨迹、躲障碍、防碰撞，半天就过去了。把这一步放在仿真里做完，是我这轮项目里做得最值的一个决定。