简介移动机器人视觉中的相机标定与定位是导航与增强现实系统的关键环节。文档系统梳理了相机模型数学推导、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/dxfy 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^2x, y是归一化图像坐标。外参[R | T]不在标定文件里静态保存它随每张图变化。标定棋盘格时得到的 R、T 只是棋盘格相对相机的位置姿态。真正做移动机器人定位时外参要根据实际的应用场景重新估计标记板距离机器人多远、朝什么方向都会改变 R 和 T 的数值。下表列出常用参数的含义和典型取值范围参数物理含义典型值参考fx横向等效焦距像素320×240 图像下约 200-400fy纵向等效焦距像素320×240 图像下约 200-400cx主点 x 坐标像素接近图像宽度的一半cy主点 y 坐标像素接近图像高度的一半k1, k2径向畸变系数通常在 -0.5 到 0.5 之间p1, p2切向畸变系数通常绝对值小于 0.1k3三阶径向系数很低大广角镜头才明显2.3 张氏算法与 DLT为什么黑白棋盘格是默认选择DLT 算法可以解决线性方程求出投影矩阵但它完全不考虑相机内参的物理约束噪声大时结果不稳定而且没有畸变模型所以现在直接拿来用的场景很少。更常用的方案是张氏标定法Zhangs 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, flagscv2.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 算法做非线性优化。参数说明flagscv2.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 在线标定输出为零或者尺寸参数全部变成 0cameracalibrator.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]], dtypenp.float32) success, rvec, tvec cv2.solvePnP( obj_points, corners[0], mtx, dist, flagscv2.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 / disparityf是焦距像素baseline是左右目光心的水平距离disparity是同一个三维点在左右图像中的水平像素差。这个公式说明视差误差 1 个像素深度误差与 distance^2 成正比所以远距离深度本身就不稳定。室内机器人导航用双目有效距离一般在 0.3 到 3 米超过这个范围还是要考虑其他传感器。验证双目定位精度时我的习惯是放一个盒子在已知距离处用深度图算中心区域深度均值再与激光测距仪对比。误差在 1%-3% 以内属于正常水平如果系统偏差超过 5%回头检查基线长度是否填对了或者左右图矫正后行对齐是否满足要求。每次做视觉定位实验我都把标定数据和一组实测距离点保存下来这组数据就是判断当前版本算法是否退化的基准。从那以后我再也不直接拿厂商内参去跑定位先花二十分钟重标定一次把 rms 和实测距离误差记在小本子上。用久了你会发现标定不是一个可以一步到位的操作它需要针对相机型号、镜头畸变程度、使用场景反复校准配合重投影误差和实际测距验证才能靠谱。希望帮到你。本文还有配套的精品资源点击获取