相机标定详解：从内参畸变到PnP定位与双目深度恢复

简介：移动机器人视觉中的相机标定与定位，是导航与增强现实系统的关键环节。文档系统梳理了相机模型数学推导、DLT算法、非线性优化校准参数等核心技术，并对比Tsai、Zhang等经典方法及P3P、EPnP等现代定位方案；还针对鱼眼、折反射等非常规相机，介绍了统一球形模型的处理思路，适合机器人视觉、无人机导航及AR领域的研究人员和技术人员参考。压缩包内含单个PDF文件，大小约3.44MB，内容不仅涵盖完整的数学公式推导，还配有大量图形解释，帮助理解标定板布置、特征点对应及坐标变换等操作要点，并讨论了透镜畸变的校正策略，覆盖从模型建立到参数优化的完整流程。目前已有113人学习下载，可用于系统掌握标定原理、理解不同场景下的定位算法选型，并提升移动机器人系统的准确性与鲁棒性。

1. 移动机器人视觉算法的第一道坎：为什么相机标定决定定位精度

做移动机器人视觉算法，最先让人翻车的往往不是识别模型，而是相机标定。我见过不少团队拿着厂商出厂内参去跑 ArUco 测距，结果 3 米外误差十几厘米，还以为是识别算法不行，最后查来查去发现是相机内参和畸变系数根本没标准。相机标定这件事，决定了后续所有定位方法的上限：内参不准，PnP 测位姿必然偏；畸变不矫正，像素坐标转相机坐标就带了系统性误差。这份资源把相机标定与定位方法按 DLT 算法、张氏算法、非线性优化三条线拆开讲，既有原理推导，也有 ROS 标定和 OpenCV 落地的完整步骤。适合正在做移动机器人视觉导航、需要把图像坐标换算成真实位姿的工程师和学生，尤其是准备用 d435i、双目相机或者普通 USB 单目相机的从业者。花一晚上把内参和畸变系数标对，后面省出的排错时间远不止这一晚上。

2. 相机模型与参数标定：内参、外参和畸变到底在算啥

2.1 针孔模型与四个坐标系：像素、图像、相机、世界

相机标定本质上是在恢复相机的内参和外参，而这一切都建立在针孔模型上。针孔模型描述的是三维世界点如何投影到二维像素平面的过程，这个过程会依次经过四个坐标系：世界坐标系、相机坐标系、图像坐标系和像素坐标系。

世界坐标系是物体在真实三维空间中的参考系，机器人底盘原点或者标定板平面都可以作为世界坐标系。相机坐标系以相机光心为原点，Z 轴指向镜头前方，把世界坐标系里的点变换到相机坐标系，靠的是外参——旋转矩阵 R 和平移向量 T。图像坐标系的原点在光轴与成像平面的交点，单位是毫米，它到像素坐标系还要做一次缩放和平移，把物理尺寸转成像素的行列号。

整个投影链就是：先外参变换，再内参投影。整合下来的公式是s * [u, v, 1]^T = K * [R | T] * [X, Y, Z, 1]^T，其中s是尺度因子，K是内参矩阵。DLT 算法（直接线性变换）的核心就是直接把这个方程展开，用多组已知的[X, Y, Z]和[u, v]对应点求解 12 个未知数。理解这四个坐标系的关系是后面调参的前提。

2.2 内参、外参与畸变系数：一张参数表的物理含义

拿到标定结果，会得到一组内参和畸变系数，它们不是随便填到代码里的数字。内参矩阵是：

K = [[fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1]]

fx = f/dx，fy = f/dy，即焦距除以单个像素的物理尺寸，单位是像素。由于 CMOS 像素通常不是正方形，fx 和 fy 会略有差异。cx, cy是主点坐标，理想情况下在图像中心，但实际组装误差会导致偏离几十像素，cx, cy一旦不准，目标测距整体偏移。

畸变系数分两类：径向畸变和切向畸变。径向畸变是镜头透镜形状造成的，用k1, k2, k3描述，表现为画面边缘的“桶形”或“枕形”形变；切向畸变是镜头与成像平面不平行造成的，用p1, p2描述。完整畸变模型如下：

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

其中r^2 = x^2 + y^2，x, y是归一化图像坐标。

外参[R | T]不在标定文件里静态保存，它随每张图变化。标定棋盘格时得到的 R、T 只是棋盘格相对相机的位置姿态。真正做移动机器人定位时，外参要根据实际的应用场景重新估计，标记板距离机器人多远、朝什么方向，都会改变 R 和 T 的数值。

下表列出常用参数的含义和典型取值范围：

2.3 张氏算法与 DLT：为什么黑白棋盘格是默认选择

DLT 算法可以解决线性方程求出投影矩阵，但它完全不考虑相机内参的物理约束，噪声大时结果不稳定，而且没有畸变模型，所以现在直接拿来用的场景很少。更常用的方案是张氏标定法（Zhang's method），它要求相机拍摄不同姿态下的平面靶标，利用平面靶标的单应性矩阵约束来求解内参。

张氏算法的思想是：棋盘格被看作一个平面，它到图像平面的映射是单应性矩阵 H。一张棋盘格能够给 H 提供 8 个约束方程，不断变换棋盘格姿态，积累足够的约束后就能解出内参。这是“相机标定”这一步的主流理论基础。先通过线性方法求初始解，再带入畸变模型做最大似然估计。

这一步里的搜索关键词是“非线性优化”。张氏标定法给出的初始解并不考虑畸变，要得到精确结果，得把畸变系数放进目标函数，用 Levenberg-Marquardt 算法最小化重投影误差，让标定板上的三维角点投影到像素平面后，和实际检测到的角点之差最小。OpenCV 的calibrateCamera内部做的就是这件事，它接收角点三维坐标和对应的像素坐标，返回内参、畸变系数、每张图的外参和重投影均方根误差rms。

棋盘格之所以成为默认靶标，是因为黑白格子交界处有明确的角点可以用亚像素级精度提取，而且平面度容易保证。打印在普通 A4 纸上的棋盘格，贴在硬板上就能用。相比之下，圆点标定板在透视角度大时圆心提取容易偏，AprilTag 更适合后期自动识别，作为标定靶标时角点数量又不如棋盘格丰富。选棋盘格是最稳的开局。

3. 动手标定内参：从 ROS 工具到 OpenCV 脚本的完整流程

3.1 准备标定板：棋盘格尺寸与 AprilTag 的选型

标定板是内参标定最重要的物理工具，标注尺寸直接进算法。常见的棋盘格配置是内部角点数量为9x6或者7x8。注意这里的数字是指角点数，不是黑白格的总数，比如打印出来是 10x7 个格子，内部角点就是 9x6。很多人第一次就错在这里，把size填成了格子总数，camera_calibration 工具直接检测不到角点。

格子边长通常选择 20 毫米到 50 毫米之间。边长太小，在相机距离较远时角点间距低于成像分辨率；边长太大，标定板在近距离时出不了画面。我用过的组合是：d435i 标定用 25 毫米格边长、9x6 内部角点的棋盘格，相机离板约 0.3-1 米，角点清晰可辨。如果是广角镜头，建议边长 30 毫米以上，因为边缘畸变严重，小格子提取容易失败。

打印方面，A4 纸贴在硬纸板上是开局方案，但纸会变形；长期做标定建议用铝板附哑光膜打印，或者直接买陶瓷标定板。标定板表面不要反光，否则在灯光下角点附近会出现高光斑。

3.2 ROS camera_calibration 在线标定：d435i 采集与参数设置

如果机器人系统已经跑了 ROS，标准做法是用camera_calibration功能包在线标定。以 d435i 为例，先启动相机驱动，确认图像话题名：

roslaunch realsense2_camera rs_camera.launch rostopic list | grep image

拿到彩色图像话题，比如/camera/color/image_raw。然后启动标定节点：

rosrun camera_calibration cameracalibrator.py \ --size 9x6 \ --square 0.025 \ image:=/camera/color/image_raw \ camera:=/camera/color

--size 9x6是棋盘格的内部角点行列数，--square 0.025是每个格子的实际边长，单位是米。这两个参数填错任何一个，标定出来的 fx、fy 都会放大或缩小相同倍数，而且不会在 rms 上体现出来，属于最隐蔽的错误。image:=后面的部分，必须替换成自己相机发布图像消息的话题名。

工具启动后，会打开标定窗口，把棋盘格在相机视野里做三种运动：左右平移、前后移动、倾斜旋转。窗口右侧有几个进度条：X表示标定板在画面中的左右覆盖程度，Y表示上下覆盖程度，Size表示标定板在画面中的大小变化，Skew表示标定板倾斜程度。四个条都变绿后，CALIBRATE按钮才会亮起。

点击CALIBRATE，工具片计算出结果并显示在界面下方。此时不要急着点SAVE，点一下VIEW可以查看矫正后的图像，检查棋盘格边缘是否拉直。确认无误后SAVE会生成一个calibrationdata.tar.gz压缩包，里面包含ost.txt和ost.yaml。ost.txt适合直接读，ost.yaml适合加载到 ROS 参数服务器。整套流程大概是 10 分钟。

3.3 OpenCV 离线标定：代码精讲与重投影误差评判

在没有 ROS 的环境下，或者要看每张图的外参和重投影误差时，我一般直接用 OpenCV 写离线标定脚本。这里给出一个可跑的版本，它读入一组棋盘格图片，提取角点后调用标定函数：

import cv2 import numpy as np import glob chessboard_size = (9, 6) # 内部角点数 square_size = 0.025 # 格子边长，米 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-3) objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32) objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2) objp *= square_size obj_points = [] img_points = [] images = sorted(glob.glob('calib_imgs/*.png')) for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None) if ret: obj_points.append(objp) corners_refined = cv2.cornerSubPix( gray, corners, (5, 5), (-1, -1), criteria) img_points.append(corners_refined) cv2.drawChessboardCorners(img, chessboard_size, corners_refined, ret) cv2.imshow('corners', img) cv2.waitKey(100) cv2.destroyAllWindows() rms, mtx, dist, rvecs, tvecs = cv2.calibrateCamera( obj_points, img_points, gray.shape[::-1], None, None, flags=cv2.CALIB_ZERO_TANGENT_DIST) print("RMS:", rms) print("Camera matrix:\n", mtx) print("Distortion coefficients:", dist.ravel())

这段脚本里，objp是标定板平面上角点的三维坐标，z恒为 0，这是张氏算法的前提。cv2.findChessboardCorners负责检测角点，检测失败时ret为False，该张图直接跳过；检测成功后用cv2.cornerSubPix把角点精度提到亚像素级，这一步对 rms 的影响比想象中大。cv2.calibrateCamera内部用 LM 算法做非线性优化。

参数说明：

• flags=cv2.CALIB_ZERO_TANGENT_DIST：强制切向畸变p1, p2为 0，如果镜头是小畸变镜头可以加上，能减少过拟合。
• gray.shape[::-1]传入(width, height)，对应图像的宽高，必须和实际图片一致。
• rms是重投影均方根误差，单位是像素。经验上小于 0.5 可用，小于 0.3 算不错。如果大于 1，基本可以判定采集图像有问题或者角点提取错误。

采集图像时有一个容易被忽略的点：需要不同距离和角度的图像各 10 张以上，而且要把棋盘格尽量放在画面边缘。边缘区域的畸变最明显，恰恰是估计畸变系数最需要的信息。只看画面中心的图像，再标都标不准边缘畸变。

4. 标定排查与避坑：从翻车现场总结的五个血泪教训

4.1 现象：每次标定出的内参都不一样，fx 漂移几百像素

换了三组照片重新标定，fx 从 620 漂到 830，主点 cx 也跳了四五十像素，整个相机模型变成黑匣子。

原因：采集的图像质量不足。最常见的翻车姿势是，为了省事直接用视频抽帧，相邻帧之间画面几乎一样，信息冗余；或者所有图像里棋盘格都放在画面中央，视角变化太小，内参约束不足，非线性优化就落到了局部极小值。

解决：图像数量增加到 15-20 张，并且保证棋盘格覆盖画面的九个区域：左上、右上、中心、左下、右下等。每张图之间变换棋盘格的姿态，倾斜角度从 15 度到 45 度之间变化。用视频抽帧时把间隔拉开到 30 帧以上，保证标定板位姿差异明显。

4.2 现象：ROS 在线标定输出为零，或者尺寸参数全部变成 0

cameracalibrator.py启动后画面正常，能检测到棋盘格，但 CALIBRATE 点击后输出的相机矩阵全是 0。

原因：--size和--square配置与实际标定板不匹配。size填成了黑白格总数而不是内部角点数，或者square填了毫米数却没有转成米。还有一种情况是图像话题分辨率与相机的实际输出不一致，ROS 标定工具内部读取的camera_info和image分辨率对不上。

解决：先仔细数棋盘格内部角点数。用rostopic echo /camera/color/camera_info查看图像分辨率与方法，再重新启动标定节点。square固定用米，25 毫米就是0.025，不要写成25。

4.3 现象：重投影误差小于 0.1，但 ArUco 测距依旧偏 5%

内参看起来完美，rms 只有 0.08，但放一个 ArUco 码测距离，3 米外相差 15 厘米且随距离非线性增加。

原因：内参没问题，外参错了。solvePnP求位姿时，需要物体的三维模型点和图像点一一对应，任何一点匹配错位，或者物体坐标系定义不一致，都会导致 R、T 系统性偏移。此时重投影误差低不代表位姿准确，它只说明当前的参数高度自洽。

解决：验证外参时，不要只用单一距离测，而是从 0.5 米到 3 米每隔 0.5 米测一组，做线性和非线性拟合检查。如果偏差随距离呈现一次函数变化，优先检查标定板图案的真实尺寸，以及aruco_dict中 marker 的边长定义与打印尺寸是否一致。

4.4 现象：D435i 标定棋盘格经常检测不到角点

同一块棋盘格，用普通 USB 相机能检测到，换到 d435i 就频繁丢帧，棋盘格稍微远一点就检测不到。

原因：d435i 的红外散斑发射器投射出随机纹理，在棋盘格上形成干扰，导致黑白格边界在灰度图上变得不清晰。曝光时间太长时，白色格子过曝，角点附近饱和，也会让findChessboardCorners失败。

解决：标定前关闭红外激光发射器。用roslaunch realsense2_camera rs_camera.launch emitter_enabled:=false启动，或者用 RealSense 官方 SDK 关闭。标定棋盘格时不要开自动曝光，手动降低曝光时间到 30-50 毫秒，避免高光。尽量用彩色图像标定，不要用红外图。

4.5 现象：标定结果在图像矫正后出现边缘紫色或绿色条纹

用initUndistortRectifyMap去畸变后，画面四周出现紫边，棋盘格边缘也有一圈彩色。

原因：畸变系数估计偏大，矫正时边缘像素被过度拉伸，原始图像边缘的色差被放大。大多因为标定板贴得不够平，或者采集的标定板图像多数处于倾斜大角度状态，畸变参数被过拟合。

解决：拿到畸变系数后先检查数值量级，切向畸变p1, p2突然出现几十甚至上百的值，基本说明标定失败。重新采集图像时保证棋盘格平面和相机光轴夹角不要超过 60 度，同时减少极度边缘位置的图像，画面内保留一定比例的中间态图像。矫正后的边缘裁剪掉 5% 也可以缓解紫边。

5. 从标定到定位：PnP 位姿估计与双目深度恢复的落地技巧

5.1 PnP 定位：一张 ArUco 图算出机器人相对位姿

标定内参的最终目的是定位。最常用的轻量定位方法是 PnP 位姿估计：已知 marker 的三维模型点，以及它在图像里的像素坐标，通过内参求解 marker 相对相机的位姿。ArUco 库已经封装好了角点提取，OpenCV 的cv2.solvePnP直接接上内参即可：

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250) params = cv2.aruco.DetectorParameters() detector = cv2.aruco.ArucoDetector(aruco_dict, params) corners, ids, _ = detector.detectMarkers(gray) if ids is not None and ids[0] == 0: marker_len = 0.15 obj_points = np.array([[-marker_len/2, marker_len/2, 0], [ marker_len/2, marker_len/2, 0], [ marker_len/2, -marker_len/2, 0], [-marker_len/2, -marker_len/2, 0]], dtype=np.float32) success, rvec, tvec = cv2.solvePnP( obj_points, corners[0], mtx, dist, flags=cv2.SOLVEPNP_ITERATIVE) tvec = tvec.flatten() dist_to_marker = np.linalg.norm(tvec)

这里mtx和dist就是第 3 章标定得到的内参和畸变系数。obj_points是四个角点相对 ArUco 码中心的三维坐标，单位是米。marker_len必须和打印出来的实际尺寸一致，差 1 毫米，3 米外距离误差就超过 2 厘米。tvec的范数就是相机光心到 marker 中心的距离。注意solvePnP输出的 rvec 是旋转向量，需要的话先用Rodrigues转成旋转矩阵。

5.2 双目相机标定与深度恢复：基线距离与视差映射

双目定位比单目多一个深度维度，前提是双目相机的外参和视差计算必须校准。双目标定的核心是左右目之间的相对 R、T。拿到内参标定结果后，左右目同时采集同一标定板的图像，用cv2.stereoCalibrate求解左右相机相对位姿，再通过视差计算深度：

depth = f * baseline / disparity

f是焦距（像素），baseline是左右目光心的水平距离，disparity是同一个三维点在左右图像中的水平像素差。这个公式说明：视差误差 1 个像素，深度误差与 distance^2 成正比，所以远距离深度本身就不稳定。室内机器人导航用双目，有效距离一般在 0.3 到 3 米，超过这个范围还是要考虑其他传感器。

验证双目定位精度时，我的习惯是放一个盒子在已知距离处，用深度图算中心区域深度均值，再与激光测距仪对比。误差在 1%-3% 以内属于正常水平；如果系统偏差超过 5%，回头检查基线长度是否填对了，或者左右图矫正后行对齐是否满足要求。每次做视觉定位实验，我都把标定数据和一组实测距离点保存下来，这组数据就是判断当前版本算法是否退化的基准。

从那以后我再也不直接拿厂商内参去跑定位，先花二十分钟重标定一次，把 rms 和实测距离误差记在小本子上。用久了你会发现，标定不是一个可以一步到位的操作，它需要针对相机型号、镜头畸变程度、使用场景反复校准，配合重投影误差和实际测距验证才能靠谱。希望帮到你。

本文还有配套的精品资源，点击获取