去年年底接了个视觉引导抓取的项目相机是一台结构光 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 下面。然后在它的属性面板里设置参数建议值说明Resolution X / Y640 × 480和真机保持一致别用 1280×960点云处理会慢很多Perspective angle60° 左右这个角度作用在宽高较大的那个维度上务必实测确认Near / Far clipping0.05 m / 5 m近裁剪面太大会丢掉近处目标太小会让深度精度变差Render modeOpenGL / OpenGL3用带光照的模式方便后面做角点检测内参怎么算理想针孔模型下如果视场角作用在水平方向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~10 对应近裁剪面。映射到米制深度时是线性映射还是透视映射不同版本的实现我见过差异所以别直接抄公式。稳妥做法是在相机正前方放三个已知距离的平面比如 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. AXXB 到底在解什么手眼标定的数学内核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眼在手外相机固定在工作台上标定板跟着末端动。此时联立方程整理出来还是AXXB但解出来的 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 旋转先解、平移后解的理由AXXB这个方程写成旋转和平移两部分是这样的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 的时候会接近奇异解出来的平移会非常不稳定。这就引出两个实操结论位姿序列里必须包含足够的旋转。如果机械臂只做纯平移R_A I平移方程直接退化成t_A 0无解。不要出现绕单一轴转的情况。如果所有相对旋转都绕同一个轴旋转方程会欠定解出来的R_X在垂直于这个轴的平面上是任意值。OpenCV 提供了五种解法我实测下来的倾向是方法常量名特点Tsai-LenzCALIB_HAND_EYE_TSAI经典速度快对噪声较敏感适合数据干净时快速验证ParkCALIB_HAND_EYE_PARK用李代数做线性化整体最稳我默认用这个HoraudCALIB_HAND_EYE_HORAUD几何意义清晰精度和 Tsai 接近AndreffCALIB_HAND_EYE_ANDREFF线性方法不用做旋转矩阵正交化但对噪声敏感DaniilidisCALIB_HAND_EYE_DANIILIDIS双四元数法同时解旋转和平移收敛慢一点仿真数据干净的情况下五种方法结果应该高度一致。如果你发现它们给出的结果差异很大那说明数据本身有问题而不是方法选错了。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, dtypenp.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, # 列表长度必须一致 methodcv2.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, tt5.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], axis0) * 1000.0 print(标定板位姿一致性毫米:, t_std_mm)这个指标比残差直观得多——残差是拟合的产物一致性是物理约束。板子没动却算出了不同的位置那一定有问题。5.3 回灌验证让机械臂去抓一次虚拟目标数值上的误差看着漂亮不代表能用。我喜欢做最后一个动作把标定结果回灌到场景里做一次视觉引导的虚拟抓取。流程是这样的在场景里随便放一个小方块位置不要告诉算法。机械臂带着相机走到某个姿态渲染一帧从点云里算出方块在相机坐标系下的三维位置t_cam_obj。用标定结果换算到基座系t_base_obj T_base_gripper · T_gripper_cam · t_cam_obj。把计算结果和方块的真值位置对比看偏差多少。更进一步让机械臂直接运动到这个算出来的位置看末端是不是真的贴近了方块。第 5 步最有说服力。数值误差是抽象的看到末端离方块差了 3 厘米你立刻就知道这套标定不能用。注意回灌验证时T_base_gripper必须是和标定时同一套正运动学读出来的。如果你标定时读的是TCP验证时读的是tip那中间相差的工具偏移会直接体现在误差里白白让你怀疑标定结果。6. 最容易翻车的三处坐标变换Z-up、单位与欧拉角6.1 CoppeliaSim 的 Z-up 世界与 OpenCV 的相机坐标系CoppeliaSim 的世界坐标系是Z 轴向上的右手系和 ROS 一致。OpenCV 用的是相机坐标系 X 右、Y 下、Z 前。这两个体系之间的转换本身不难难的是你会在多个地方需要转换而每个地方的方向定义还不一样。最容易搞混的是这几个坐标系手性主要轴方向常见来源CoppeliaSim 世界右手Z 向上场景根坐标系CoppeliaSim 视觉传感器右手X 左、Y 上、Z 为视线方向相机对象自身坐标系OpenCV 相机右手X 右、Y 下、Z 前solvePnP输出ROS 相机光学系右手X 右、Y 下、Z 前与 OpenCV 一致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, dtypenp.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 坐标系错位时的典型症状对照表调试时最有用的不是公式是症状。下面这张表是我自己攒的出问题时先对号入座症状最可能的原因旋转误差很小平移误差是个固定的大向量相机坐标系轴约定搞反少乘了那个 180° 旋转或者工具偏移没算进去旋转误差稳定在 90° 或 180° 附近手性搞错了或者欧拉角作用顺序反了每次加一组数据结果就大变位姿序列条件数差旋转轴太集中对称的两个姿态结果差很多某组数据的角点检测错误或者该姿态超出了关节限位真值数据准换成检测数据就不准相机内参不对或者标定板格子尺寸填错整体尺度差 1000 倍单位问题毫米和米混用了平移方向大致对但偏了几厘米TCP 定义的位置和实际相机安装位置不一致这张表里的每一条我都至少踩过一次。特别是第一条和最后一条固定的大向量这个特征非常典型——它不是随机误差是系统性偏差一定来自某个被遗漏的静态变换。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每次机械臂移动你都要重新计算目标在基座系下的位置而这个位置理论上应该不变。这个不变性是检验标定质量最好的实时指标——让机械臂在工作空间里随便走一圈看同一个静止目标算出来的基座系坐标漂不漂。漂多少就是你的标定精度上限。仿真里跑这一步几乎没有成本鼠标一拖就能试几十个姿态。真机上做同样的事得写轨迹、躲障碍、防碰撞半天就过去了。把这一步放在仿真里做完是我这轮项目里做得最值的一个决定。