3、视觉传感器与标定：让机器人的眼睛看得准

做视觉SLAM和机械臂协同控制，第一步就是搞定传感器。说白了，你得先让机器人看清楚这个世界，然后才能谈定位和抓取。这一章我们聚焦相机成像模型、标定方法和深度相机选型——这些都是后续所有工作的基础。

3.1 相机成像模型

相机怎么把三维世界变成二维图像？这背后是几何投影的数学关系。理解这个模型，你才能知道标定到底在标什么。

3.1.1 针孔模型

针孔模型是最基础的成像模型。想象一个暗箱，前面有个小孔，光线穿过小孔在后面的感光平面上成像。这个模型用四个坐标系来描述：世界坐标系、相机坐标系、图像坐标系、像素坐标系。

从三维点到像素的映射关系可以写成：

s * [u, v, 1]^T = K * [R | t] * [Xw, Yw, Zw, 1]^T

其中 K 是内参矩阵，包含 fx、fy（焦距）、cx、cy（主点偏移）。[R | t] 是外参，描述相机在世界坐标系中的位姿。

我个人习惯把内参理解为「相机自己的脾气」——每个镜头都不一样，必须通过标定来获取。外参则是「相机放在哪、朝哪看」，每次安装位置变了都要重新标。

3.1.2 畸变模型

现实中的镜头不是完美针孔。透镜的物理形状会引入畸变，主要是两种：

• 径向畸变：光线通过透镜边缘时弯曲更厉害，造成图像边缘的直线变弯。分为桶形畸变（负向）和枕形畸变（正向）。
• 切向畸变：透镜和成像平面不平行，导致图像倾斜。这个在制造装配中产生，通常比径向畸变小。

畸变模型用多项式来近似：

x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y

其中 k1、k2、k3 是径向畸变系数，p1、p2 是切向畸变系数。标定就是求解这些参数。

3.2 相机内参标定：张正友法

张正友法是目前最主流的相机标定方法。它只需要拍摄不同角度的棋盘格图像，就能解算出内参和畸变系数。为什么它这么流行？因为操作简单、精度高、鲁棒性好。

基本流程是这样的：

1. 准备棋盘格：打印一张棋盘格，贴在平整的板上。格子数量建议 9×6 或 7×5，每个格子边长已知。
2. 采集图像：从不同角度拍摄 15-20 张棋盘格图像。注意覆盖视野的各个区域，尤其是边缘。
3. 角点检测：用 OpenCV 的 findChessboardCorners() 提取角点。
4. 求解内参：先忽略畸变，用线性方法求解内参初值。然后加入畸变模型，用非线性优化（比如 Levenberg-Marquardt）迭代求精。
5. 评估结果：看重投影误差，一般小于 0.5 像素就算不错。

代码实现很简单：

import cv2
import numpy as np

# 棋盘格尺寸
pattern_size = (9, 6)
square_size = 0.025  # 25mm

# 准备世界坐标系中的点
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2)
objp *= square_size

# 存储所有图像的点
objpoints = []  # 3D点
imgpoints = []  # 2D点

# 遍历图像
for fname in image_files:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None)

print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

3.3 手眼标定

相机标定好了，但相机是装在机械臂上的。怎么把相机看到的物体位置告诉机械臂？这就需要手眼标定。

3.3.1 Eye-in-Hand vs Eye-to-Hand

两种安装方式，标定逻辑完全不同：

对比项	Eye-in-Hand（眼在手上）	Eye-to-Hand（眼在手外）
相机位置	固定在机械臂末端	固定在外部，不随机械臂运动
标定目标	求相机坐标系到末端坐标系的变换	求相机坐标系到基座坐标系的变换
优势	视野随机械臂移动，灵活；近距离测量精度高	视野固定，适合全局监控；标定后不用再动
劣势	标定复杂；机械臂运动时图像会模糊	远距离测量精度受限；可能被机械臂遮挡
典型应用	抓取、装配、焊接	视觉引导、安全监控、全局定位

我个人更倾向于 Eye-in-Hand 做精密抓取。为什么呢？因为相机跟着机械臂走，可以凑近看，精度高。但标定确实麻烦一些。

3.3.2 标定原理与步骤

手眼标定的核心是求解 AX = XB 方程。A 是机械臂末端位姿变化，B 是相机看到的标定板位姿变化，X 就是我们要找的手眼变换矩阵。

具体步骤：

1. 在机械臂工作空间内固定一个标定板。
2. 控制机械臂运动到多个不同位姿，记录每个位姿的关节角度和末端位姿。
3. 在每个位姿拍摄标定板图像，用相机标定结果解算标定板在相机坐标系下的位姿。
4. 构建 AX = XB 方程组，用 Tsai 法或 Park 法求解。

我用 OpenCV 的 calibrateHandEye() 函数实现过：

import cv2
import numpy as np

# R_gripper2base, t_gripper2base: 机械臂末端在基座坐标系下的位姿（多组）
# R_target2cam, t_target2cam: 标定板在相机坐标系下的位姿（多组）
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,
    R_target2cam, t_target2cam,
    method=cv2.CALIB_HAND_EYE_TSAI
)

3.4 深度相机选型

单目相机只能得到二维图像，没有深度信息。要做三维定位，需要深度相机。目前主流的有三种技术路线。

3.4.1 三种技术对比

技术	原理	工作距离	精度	抗环境光	典型产品
结构光	投射红外点阵/条纹，根据变形计算深度	0.1-5m	高（mm级）	弱（室内可用）	Kinect v1, Intel RealSense D415
ToF（飞行时间）	发射红外脉冲，测量往返时间	0.3-10m	中（cm级）	较强	Kinect v2, Azure Kinect, PMD
双目立体视觉	两个相机视差计算深度	0.1-20m+	中高（依赖基线）	强（被动式）	ZED, Intel RealSense T265

3.4.2 选型建议

怎么选？我根据项目经验给几个建议：

• 室内近距离抓取（0.3-1m）：结构光最合适，精度高。我用 RealSense D415 做过桌面抓取，效果不错。
• 中距离导航（1-5m）：ToF 更稳定，不受纹理影响。Azure Kinect 在这个范围表现很好。
• 室外或大范围（5m+）：双目是唯一选择，因为它不依赖主动光源。但要注意，双目在弱纹理区域（比如白墙）会失效。
• 高动态场景：ToF 的帧率可以做到 30-60fps，双目也能做到 30fps，结构光稍慢。

3.5 本章知识体系

下面这张图把本章的核心内容串起来了：

从这张图可以清楚看到，整个视觉传感器与标定的知识体系分为三大块：先理解相机成像模型，然后做内参标定，再根据安装方式做手眼标定，最后根据应用场景选深度相机。每一步都是下一步的基础，环环相扣。