1.9 相机内参·标定·手眼标定·3D 抓取

本文档需要配套对应视频观看，请联系助手获取视频（微信：ffcv1024）

• 本节课代码：点击这里

本节课需要切换到 Ubuntu 系统（非虚拟机），建议使用 Conda 创建虚拟 Python 环境，我的环境信息如下：

• Conda 环境名称：episode_env
• Python 3.8.20
• NVIDIA GeForce RTX 3060 Ti
• 各种库版本信息(pip 导出)

还记得我们的学习目标嘛？我们的目标首先是做机械臂的 3D 视觉抓取！

在《七、运动控制：坐标系·姿态·欧拉角·旋转矩阵·齐次变换矩阵》中我们分析过：

实现 3D 视觉抓取的核心在于准确获取目标的三维坐标并完成机械臂位姿映射。为此，需要掌握深度相机的使用、手眼标定、机械臂运动学等知识。其中具体问题及本课程使用的方法如下：

• 如何获取物体三维坐标
  • 使用目标检测算法获取 RGB 图像中物体的几何中心
  • 使用深度相机 D435 获取物体中心位置三维坐标
• 物体三维坐标转为机械臂基坐标系
  • Aruco 眼在手外标定
• 机械臂如何运动到目标点
  • 获取物体位置相对于机械臂基座坐标系的坐标：
    • 直接调用 Move XYZ
    • 或者使用逆运动学得到机械臂关节角度，调用 MoveJ

我们已经学习了第三条机械臂如何运动到目标点，即：给机械臂一个目标位姿，调用逆运动学求解关节角度，就可以驱动机械臂移动到对应位姿。

现在我们来处理前两步骤，我们把问题再表述的严谨一些：

• 如何获取目标物体在相机坐标系下的三维坐标？
• 这个三维坐标又如何转为机械臂基坐标系下的坐标？

这两个问题，其实本质上都是如何在各种坐标系之间相互转换，我们先了解一下，机械臂视觉抓取一般会涉及哪些坐标系：

1. 常见的坐标系、定义与关系

相机相关坐标系	机械臂相关坐标系	标定板坐标系

记号	中文名称	常见符号	原点与轴的物理含义
	图像坐标系 / 像素平面		原点在左上角或主点； 轴向右， 轴向下
	相机坐标系		原点在相机光心； 轴沿光轴指向成像方向； 构成右手系
	标定板 / 目标坐标系		原点通常设在棋盘格左上角格点； 平行于格线， 法向外指
	末端执行器 / 工具坐标系	或	原点在 Tool Center Point；轴方向由机器人控制器定义
	机器人基座 / 世界坐标系	或	原点在机器人基座安装平面； 轴通常竖直向上

符号在不同教材中可能略有不同。

那么，我们需要解决的两个问题，它对应的转换要求如下：

• 如何获取目标物体在相机坐标系下的三维坐标？

  • 从

• 这个三维坐标又如何转为机械臂基坐标系下的坐标？

  • 从

2. 如何获取目标物体在相机坐标系下的三维坐标？

我们将按这几个步骤操作：

• 使用目标检测算法获取 RGB 图像中物体的几何中心，这里得到的几何中心是像素坐标
• 使用深度相机得到该像素坐标的深度值
• 使用相机标定或 API 得到相机内参
• 使用反投影，得到该像素坐标的相机坐标

2.1 相机内参——从

2.1.1 针孔相机模型

针孔相机模型通过一个简化的方式来描述光线如何在相机中形成图像。该模型把相机想象成一个封闭的暗盒，侧面只有一个微小的孔，另一侧是平坦的成像平面。除穿过小孔的光线外，其余光线都被盒壁遮挡。

当物体置于孔前时，来自物体各点的光线通过小孔后，在成像平面上形成倒立的图像。下图展示了针孔相机的工作原理：

如果没有暗盒与针孔，成像平面将受到来自半球空间所有方向的光照，因而只能得到均匀的亮斑，而无法形成可辨识的图像。

该投影过程涉及几个关键因素：

• 距离效应：离相机越远的物体在图像中看起来越小。
• 倒像现象：因为所有光线都在单一点（针孔）汇聚，最终在成像平面上得到的影像上下、左右皆倒置。
• 视场 (Field of View)：针孔直径越大或针孔到成像平面的距离越短，视场就越宽，能捕捉到更多场景；反之亦然。
• 成像尺寸：远处物体在图像上的尺寸与针孔到成像平面的距离成正比。

2.1.2 相机参数

下面给出针孔相机模型中几个基本参数的定义：

• 投影中心（Center of Projection）：针孔所在的空间点。
• 焦距（Focal Length）：从投影中心到成像平面的距离。
• 光轴（Optical Axis）：垂直于成像平面并通过投影中心的那条直线。
• 主点（Principal Point）：光轴与成像平面的交点。

我们可以观察到两个相似三角形，因此可推导出一个简单公式，将物体的实际尺寸与其在成像平面上的投影尺寸联系起来：

其中

• 表示物体在成像平面上的尺寸，
• 表示物体的真实尺寸，
• 为焦距，
• 为物体到针孔的距离。

2.1.3 焦距

为了方便观察，常见的做法是理想地交换针孔和成像平面的位置，如下图所示

空间中一点 （对应上图）与其在图像平面上的像素坐标 之间的关系可表示为：

投影变换 (projective transform) 将空间中坐标为 的点映射到图像平面上的投影 。

• 其中 、 是主点在图像坐标系中的绝对像素坐标（左上角为 ）；
• 、 本质上是以像素为单位表示的焦距。之所以需要两个焦距参数，是因为常见成像器件的像素通常呈矩形，导致 、 方向的像素尺寸不同。需注意：、、、、、 全部以像素为单位。
• 物理焦距 以毫米为单位，无法直接通过图像测量得到；而 和 可以通过 相机标定 (camera calibration) 的过程求得。

2.1.4 相机内参矩阵 (Camera Intrinsic Matrix)

把前面 2 个公式写成矩阵形式更简洁：

移到左侧，简写成：

即：

我们把中间的重组矩阵称为相机的内参数矩阵 K，相机的内参一般在出厂之后是固定的，不会在使用过程中发生变化。有的相机生产厂商会告诉你相机的内参，而有时需要你自己确定相机的内参，也就是所谓的相机标定。

标定通常使用已知几何图案（例如棋盘格）或尺寸精确的物体，并借助数学算法（如 Zhang’s method 张正友 法或 直接线性变换 Direct Linear Transform, DLT）得到 等参数。

相机内参矩阵 本身只包含 ；完全是常数矩阵，不带 。

2.1.5 反投影

假设我们已经知道了相机内参，在深度相机使用中，我们需要像素坐标 → 空间坐标 的反投影。

矩阵形式

如果你对逆矩阵不熟悉：

通俗的理解：矩阵的逆 = 反操作，用来“撤销”原始变换，是走回头路的工具。

可参考后文 附录 加深理解。

分量展开（无剪切、像素长宽比已吸收进 ）

这意味着我们只要知道像素坐标和深度，就可以还原空间三维点。

如 D435 深度相机提供了 rs.rs2_deproject_pixel_to_point API 反投影，其实就主要做了这个计算，我们可以手动反投影：

• 下载课程用 Aruco 标定图片，随便贴在一个平面上，或者直接用手机显示
• 使用代码 1.test_d435_aruco.py 验证。

# 深度 → 相机坐标
z = depth_frame.get_distance(x, y)
# 使用API反投影 
x_cam, y_cam, z_cam = rs.rs2_deproject_pixel_to_point(intr, [x, y], z)

# 手动反投影
X_m = (x - intr.ppx) * z / intr.fx
Y_m = (y - intr.ppy) * z / intr.fy
Z_m = z
​

2.1.6 相机内参标定

为了得到相机内参，我们需要做相机标定，一般使用 棋盘格 + OpenCV 标定相机内参（焦距 、主点 、畸变系数 …）

1. 打印棋盘格

   • 下载课程用的棋盘格标定图 Checkerboard-A1-65mm-11x8
     • 更多棋盘格下载地址
   • 贴在绝对平整的硬板上，避免翘曲
   • 课程直接使用的是电脑显示器
     • 前提：显示器上棋盘格格子水平与垂直方向尺寸一样

2. 拍摄校准图片

   • 使用脚本：2.capture_images.py
   • 修改尺寸数据
   • 20 张以上，棋盘格占画面不同位置、距离、倾斜角；
   • 光照均匀、不反光，角点清晰；
   • 分辨率保持相机原始大小。
   • 需要关闭自动对焦

   下面是推荐拍摄策略（典型 24 张）：

   #	棋盘格朝向	相机视角†	距离	建议张数	目的 / 备注
   1	横放 （Landscape，长边水平）	正对	近	1	让角点占满画面，提供高分辨率细节
   2			中	1	覆盖中距离几何畸变
   3			远	1	提供“整体”参考，约束焦距
   4	横放	左偏 ≈15° yaw	近	1	横向畸变 / 主点(c_x) 约束
   5			中	1
   6			远	1
   7	横放	右偏 ≈15° yaw	近	1	与左偏成对，抵消系统误差
   8			中	1
   9			远	1
   10	横放	俯视 ≈20° pitch↓	近	1	约束垂直方向畸变、(c_y)
   11			中	1
   12		仰视 ≈20° pitch↑	近	1
   13			中	1
   14	竖放 （Portrait，长边竖直）	正对	中	1	让角点触及上、下边缘，增强(c_y) 可观性
   15		左偏 ≈15° yaw	中	1	垂直方向非对称姿态
   16		右偏 ≈15° yaw	中	1
   17–24	任选 横／竖、各类角度	近/中/远	8	在不同光照、焦段 (若可变焦) 下再补 8 张，保证 ≥24 张有效图

3. Opencv 相机标定

   使用脚本：3.calibrate_intrinsics.py （注意修改尺寸数据）

步骤	OpenCV API	说明
1 读取图像	cv2.imread	获取灰度图
2 检测角点	cv2.findChessboardCorners	找到内角点
3 亚像素精细化	cv2.cornerSubPix	提升精度
4 收集 object / image points	—	3D-2D 对应表
5 求内参 + 畸变	cv2.calibrateCamera	主函数
6 评价重投影误差	手动循环 cv2.projectPoints	判断好坏
7 保存结果	cv2.FileStorage or np.save	YAML / npy / json

ret 即 RMS 重投影误差，理想 < 0.5 像素；< 0.2 像素更优。

相机标定的最终目标是为了准确反投影。因为 RealSense 输出的 color 图像经过轻微矫正（变化了的图像和你标定时假设的成像模型就不一样了），导致自己标定的 K 矩阵可能和实际图像不匹配。所以在实际应用中，应直接使用 RealSense API 提供的 intrinsics 进行反投影，以确保三维定位、手眼标定和机械抓取的准确性，或者通过 Intel 官方提供的相机校准工具重新进行标定。

3. 相机坐标系如何转机械臂基坐标系？

• 眼在手外（eye-to-hand，图 a）
  • 相机固定在机器人工作区域外部，不随机械臂移动
  • 相机观测整个工作区域或机器人
• 眼在手上（eye-in-hand，图 b）：
  • 相机安装在机械臂末端（通常安装在夹爪或末端执行器上）
  • 相机会随着机械臂的运动而一起移动

3.1 手眼标定原理

OPENCV 参考文档

上图中是眼在手上的示意图，出现的符号如下：

符号	释义	是否变化
	相机坐标系到夹爪坐标系的变换矩阵	常量
	标定板坐标系到基座坐标系的变换矩阵	常量
	夹爪坐标系到基座坐标系的变换矩阵， 表示第 次采样的位姿	变量
	夹爪坐标系到基座坐标系的变换矩阵， 表示第 次采样的位姿	变量
	标定板坐标系到相机坐标系的变换矩阵， 表示第 次采样的位姿	变量
	标定板坐标系到相机坐标系的变换矩阵， 表示第 次采样的位姿	变量

• 根据我们学过的知识，假设在空间有一点，它在基座坐标系、标定板坐标系、夹爪坐标系、相机坐标系下分别表示为，我们做抓取的最终目标就是：

  其中：

  • 可以通过深度相机 API 和相机内参反投影得到
  • 可以通过正运动学得到
  • 需要求解是的，即相机与夹爪之间的转换关系，这也是我们手眼标定（眼在手上）的目标

• 点，它在基座坐标系、标定板坐标系下有关系如下：

• 点，在夹爪坐标系、相机坐标系下有关系如下：

• 由上述两个公式，可以得到：

• 在眼在手上标定过程中，标定板和机械臂底座不动、夹爪和相机的相对位置不变，所以和是常量。对于第次标定，两次不同位置的标定可得到

• 我们的目标是求解 ：

  • 这是矩阵方程求解问题
  • 可以将等式变换得到 的形式来求解

• 我们的目标是消去中间的 ，从而得到 的形式。

  逆矩阵与单位矩阵的性质请查看附录

  • 第一步：将等式左乘
  • 第二步：将等式右乘

  • 第三步：定义变换关系

    令：

    则可得：

为什么得到 的形式就可以求解？

尽管 是非线性的（因为包含旋转矩阵），它具有一些重要特性：

• 和 是已知的；
• 我们可以从不同的运动对（多个 ）获得多个 方程；
• 利用多个观测对 ，可以构建一个过约束方程组；
• 数学上，这属于一个矩阵方程求解问题，常见解法：Tsai–Lenz、Park–Martin、Dual Quaternion 最小二乘。

直观理解： 让相机和末端各动几次，比较“眼里”与“手里”看到的位姿差，把两个差对齐，就能求出它们之间的固定关系 。

这组方程可以帮我们推导出相机和机械臂之间固定不变的空间关系。

• 在眼在手上实际操作过程中
  • 我们需要移动机器人末端执行器以获取多个不同的姿态
  • 不同位姿的 可以通过正运动学得到，也就是可求
  • 不同位姿的 可以通过 PnP 等 2D-3D 点对应的方法求，也就是可求
  • 再使用 Tsai–Lenz 等方法求得，即

---

• 同理，对于眼在手外，我们需要求变换矩阵 ，即从相机坐标系到机器人基座坐标系的变换。你可以自己推理出

• 即：

• 同样的
  • 不同位姿的 ，可以通过正运动学得到（），也就是可求
  • 不同位姿的 可以通过 PnP 等 2D-3D 点对应的方法求，也就是可求
  • 再使用 Tsai–Lenz 等方法求得，即

总结

类型	求解	使用
眼在手上（eye-in-hand）
眼在手外（eye-to-hand）

3.2 眼在手外 3D 视觉抓取实战

说明：对于 3D 抓取，假设机械臂末端没有姿态，比如是三轴机械臂，手眼标定需末端在多姿态（含旋转 + 平移）下运动才能保证方程组有解。若仅固定姿态、只做平移，缺乏旋转信息，方程退化，无法求解。

这里我们使用线性最小二乘拟合，我们将在下一节 6D 抓取使用

• 使用脚本：4.3d_calibrate_test.py

  • 课程代码是基于 Episode 1 机械臂 API 编写的，如果你使用自己的机械臂，请更换成自己的 API、物料

• 设备准备：

  • 要求机械臂提供 XYZ 运动控制 API，可以运动到对应位置（不必是 6 轴）
  • 如果是六轴机械臂，保持末端欧拉角固定

• 安装深度相机，固定机械臂底座，固定吸盘夹具，参考 Episode1，要求相机能覆盖机械臂预设标定点：

  使用之前下载的 1.test_d435_aruco.py 测试检测是否可以覆盖

  侧视图	深度相机视角

• 制作 Aruco 标定板：

  • 进入 Aruco 标定图片下载网页，选择 Dictionary、Marker ID 与我一致
  • 自己在网页中设置好 Marker size，即尺寸，使打印出来的 Aruco 码边长约为 50mm，方便嵌入标定板
  • 使用胶棒粘贴好 Aruco 码
  • 使用之前下载的 1.test_d435_aruco.py 测试检测是否正常

  Aruco 网页下载界面	标定板尺寸图（mm）	实拍图

• 标定运行命令：

  # 标定
  python 4.3d_calibrate_test.py --visualize --calibrate
  
  
  # 求解T_camera2base代码核心
  self.T_camera2base = base_coords @ np.linalg.pinv(cam_coords)  # 计算变换矩阵
  ​

  它使用 线性最小二乘拟合，求出一个 齐次矩阵 ，启动标定程序后：

  • 机械臂会运行到准备位置，并开启负压吸盘

  • 你有 10 秒钟装好 Aruco 标定板，并严格调整好：

    • 如果 10 秒钟不够，请修改代码
    • 让标定板圆形凹面对准吸嘴
    • 让标定板笔直朝前，即沿着底座 X 方向
    

    • 侧面观察标定板，标定板需要尽可能水平，如果不平，可以先用书本压一段时间

      

    • 机械臂运动到 96 个预设点位进行标定，标注完毕，会在 save_parms 文件夹下生成 camera2base.npy 数据。

• 抓取测试运行命令：

  # 测试抓取
  python 4.3d_calibrate_test.py --visualize --recognize
  
  # 使用T_camera2base
  cam_pt = np.array([*ctr, 1.0])  # 相机坐标齐次
  base_pt = self.T_camera2base @ cam_pt  # 转换到基座坐标
  ​

  在抓取时，用的公式就是

  在测试过程中，机械臂会识别 ArUco 标定板的几何中心，利用吸盘对该位置进行抓取，并将其移动至一个随机位置，循环执行上述操作。

  • 正常误差范围：由于存在标定误差以及 Episode1 J1 底座减速器的回隙（详见：回隙说明），抓取位置与几何中心的偏差在 ±10mm 范围内属于正常现象。
  • 标定检查建议：若偏差超过 10mm，可能是以下问题：
    • 深度相机数据有问题，尤其是二手的深度相机
    • 深度相机视角有问题，未尽量覆盖全部标定点
    • 光照问题
    • Aruco 标定板安装不规范
    • Episode 底座侧边紧固螺丝未紧固，回隙过大
    • 桌面抖动，导致相机或机械臂移位
  • 减少回隙建议：如需尽可能减小减速器回隙影响，欢迎联系助手购买 J1 谐波减速器升级套件（需要更换第一个关节电机、减速器、法兰、相应螺钉等，底座外壳等不需要更换）。

• 测试 Dino 模型：

  Grounding DINO 是一个“零样本”目标检测模型，底层由 CLIP 视觉-文本对齐特征支撑，意思是它不用针对新物体做专门训练就能识别。你只需在运行时输入想找的类别（比如“苹果”或“瓶子”），它就会在图像中画出相应的框并给出置信度。它通过把图像特征和文字描述一起送入模型，然后直接输出带标签的边界框。不需要额外数据标注或微调，就能灵活地检测各种从未见过的对象，非常适合快速原型和少样本场景。

  你可以将 Dino 模型换成其他检测模型，如 YOLO 等。

  # 先参考我的环境信息配置好运行环境
  
  # 启动
  python 5.dino_detect.py
  # 然后输入对应英文类别名称，以逗号分隔
  # 如我的输入是：banana,pink box,pen
  ​

  

• 测试 3D 抓取代码

  使用脚本：6.dino_grasp.py

  带有玻璃的物体表面可能导致深度信息不准确，无法抓取。如手机屏幕，可以改成背面朝上。

附录一：本课用到的矩阵的性质

设 （即 的可逆实矩阵），则存在唯一矩阵 使得

其中 是 的单位矩阵，在矩阵乘法中充当乘法单位元：

注意前提

• 必须是可逆方阵（），否则 不存在。
• 单位矩阵 的阶数必须与参与运算的矩阵匹配，否则乘积无意义。

可以把三者类比为

1. → 一个“操作”按钮
2. → 与之配套的“撤销”按钮
3. → 数字 $1$，乘谁都不会改变谁

附录二：矩阵顺序与逆矩阵理解

下载代码查看演示

场景设定

• 坐标系 A：原点
• 坐标系 B：相对于 A 向右平移 3 个单位（无旋转）
• 点 ，表示在 B 坐标系中位置为 (1, 0)

我们想求：

1. 这个点在 A 坐标系中的位置是多少？
2. 如果有 A 坐标下的点，如何反推出在 B 坐标系中的坐标？

构造变换矩阵

B 相对于 A 的齐次变换矩阵（向右平移 3）：

它的逆矩阵（A 相对于 B）是向左平移 3：

通俗的理解：矩阵的逆 = 反操作，用来“撤销”原始变换，是走回头路的工具。

情况 1：从 B 到 A

点在 B 中是：

计算它在 A 中的坐标：

所以，在 A 坐标系中，它的位置是 (4, 0)。

情况 2：从 A 回到 B（用逆矩阵）

现在我们反过来，已知：

用逆矩阵转换回 B 坐标系：

成功回到了原来的 B 坐标 (1, 0)。

错误示例：顺序错或不取逆

如果直接用 继续乘 ：

显然错了。

附录三：反投影矩阵形式推导

在前向投影里，相机内参矩阵 把相机坐标点 映射到像素坐标 ：

反投影（de-projection）

已知像素坐标 和该像素的深度 ，要恢复相机坐标 。
把上式两边同乘 ，再左乘 ：

其中

展开即可得到常见的分量式