之前一直做的是机械臂的手眼标定（即相机外参标定），流程都是需要拍十几张标定板的图片进行计算。最近接触了割草机器人，发现量产中进行外参标定时，只需要拍一张标定板的图片就能计算得出割草机器人上相机的外参，当时没理解这样为什么可行，记录下来。

---

相机安装在割草机器人上之后，需要知道它相对于机器人的位置和朝向，这就是相机外参标定要解决的问题。

本文以一套量产工装标定方案为例：将割草机器人通过四个定位销固定在标定工装上面，棋盘格标定板也固定在工装上，相机内参和畸变参数已知，拍摄一张棋盘格照片后，通过 PnP 求解与坐标变换，便可得到相机外参。

1. 工装解决了什么问题

本方案将机器人的两个后轮中心中点作为机器人坐标系原点，约定 X 轴向前、Y 轴向左、Z 轴向上。

工装固定棋盘格，同时通过定位销固定机器人。因此，经过机械测量，可以提前确定棋盘格相对于机器人的完整位姿关系：既包括位置，也包括朝向，记为 T_board2robot。

照片则提供另一段关系：棋盘格相对于相机的位姿，记为 T_board2cam。将这两段关系连接起来，就能求出相机相对于机器人的位姿。

定位销不参与图像中的角点求解，它的作用是保证机器人与棋盘格之间的关系可重复。相机即使存在装配偏差，也能通过标定得到实际位置与朝向。

2. 棋盘格标定板的具体参数

2.1 标定板规格

标定配置中的参数为：

项目	参数
内角点列数 board_width	6 列
内角点行数 board_height	5 行
内角点总数	30 个
方格边长 square_size	80 mm，即 8 cm
每个方格尺寸	80 mm × 80 mm
内角点网格跨度	宽 400 mm × 高 320 mm

6×5 指的是内角点，不是黑白方格的数量。 内角点是棋盘内部黑白方格交汇的位置，OpenCV 检测的就是这些点。

对于采用该角点布局的标准完整棋盘：

• 黑白方格为 7 列 × 6 行，共 42 格。
• 黑白方格区域为 宽 560 mm × 高 480 mm。
• 上述尺寸不包含边框；底板总尺寸、边框宽度和材质无法从代码确定，需要机械图或实测。

代码将内角点网格中心设为棋盘格坐标系原点。第 $i$ 行、第 $j$ 列的角点坐标为：

$${\mathbf{P}}_{board}(i,j)=\left[\begin{array}{c}(j-2.5)\times 80\\ (i-2)\times 80\\ 0\end{array}\right]\text{ mm},i=0,\dots ,4,j=0,\dots ,5.$$

所有角点的 Z 坐标为零，表示它们位于棋盘格自身平面上。

2.2 完整标定配置

下面给出当前程序使用的完整配置，包含相机参数、棋盘格规格、工装位置、相机设计安装位姿及验收门限。参数数值与当前配置一致，注释按实际含义整理；原配置中的“前方 700 mm”“偏上 100 mm”等部分注释是旧值，不参与计算。

%YAML:1.0
---
cameraMatrix: !!opencv-matrix
     rows: 3
     cols: 3
     dt: d
     data: [ 2.7805558475653351e+02, 0., 3.2136391563229023e+02, 0.,
             2.4530212108564695e+02, 3.1749840316827328e+02, 0., 0., 1. ]
distCoeffs: !!opencv-matrix
     rows: 5
     cols: 1
     dt: d
     data: [ -2.9257009741304785e-02, 2.1646154559173788e-02,
             -2.1628000904575615e-03, 1.4719593518310052e-03,
             -3.7893060374887002e-03 ]

board_parameters:
    board_width: 6       # 内角点列数
    board_height: 5      # 内角点行数
    square_size: 80.0    # 方格边长，mm

board_position_robot:
    t_rb_x: 1088.34      # 棋盘格中心在机器人原点前方，mm
    t_rb_y: 0           # 向左偏移，mm
    t_rb_z: 133         # 向上偏移，mm

camera_installation_parameters:
    Cx_rcS: 458         # 相机光心的设计位置：向前，mm
    Cy_rcS: 0           # 相机光心的设计位置：向左，mm
    Cz_rcS: 86          # 相机光心的设计位置：向上，mm
    roll_rcS: 0         # 设计横滚角，°
    pitch_rcS: 12       # 设计俯角，低头 12°
    yaw_rcS: 0          # 设计偏航角，°

allowed_error:
    max_displacement_error: 30  # 安装差异的平移分量门限，mm
    max_angle_error: 5          # 安装差异的欧拉角分量门限，°

%YAML:1.0 和 !!opencv-matrix 是 OpenCV 的配置格式；矩阵的 rows、cols 表示行列数，dt: d 表示双精度浮点数，data 按行排列元素。

2.3 各组参数如何理解

相机内参 cameraMatrix。 矩阵中 $f_x\approx 278.056$、$f_y\approx 245.302$ 是以像素为单位的焦距；$c_x\approx 321.364$、$c_y\approx 317.498$ 是主点的像素坐标。它描述相机坐标如何投影到图像，不表示相机在机器人上的安装位置。

畸变参数 distCoeffs。 按当前标准 OpenCV 投影模型，五个参数的顺序是 $[k_1,k_2,p_1,p_2,k_3]$：$k_1,k_2,k_3$ 为径向畸变参数，$p_1,p_2$ 为切向畸变参数。它们与内参一起用于 PnP，必须与实际拍摄图像和镜头模型匹配。

棋盘格位置 board_position_robot。 三个数表示内角点网格中心在机器人坐标系中的位置：相对于两个后轮中心中点，前方 1088.34 mm、左右偏移 0 mm、上方 133 mm。这只是平移，标定板朝向还要结合代码中的固定旋转矩阵。

因此，本工装的完整棋盘格 → 机器人变换为：

$$T_{board2robot}=\left[\begin{array}{cccc}0& 0& 1& 1088.34\\ -1& 0& 0& 0\\ 0& -1& 0& 133\\ 0& 0& 0& 1\end{array}\right].$$

这里平移单位为毫米，采用齐次列向量：${\mathbf{p}}_{robot}=T_{board2robot}{\mathbf{p}}_{board}$。棋盘格 X、Y、Z 轴分别对应机器人向右、向下、向前，描述的是立式标定板。

相机设计安装参数 camera_installation_parameters。 设计光心位置是机器人原点前方 458 mm、左右偏移 0 mm、上方 86 mm，设计横滚、俯仰、偏航角分别为 0°、12°、0°。这些是用于验收的机械设计值，不是实际标定结果，也不是给 PnP 预设的答案。代码还会组合光学轴与机器人轴之间的固定转换，不能把这三个角度直接当成最终外参的欧拉角。

安装差异门限 allowed_error。 程序将标定位姿与设计位姿组成相对变换，检查最大绝对平移分量是否超过 30 mm、最大绝对欧拉角分量是否超过 5°。它不是三维距离门限，也不代表标定精度为 30 mm、5°。

这套配置中的尺寸以毫米、角度以度为单位。更换工装、相机或图像处理方式后，需要重新核对配套参数，不能直接照搬这些数值。

3. 为什么一张照片可以求出位姿

普通单目图像为什么没有唯一距离

一个像素只能确定一条视线。沿这条视线，不同距离的物点都可能投影到同一个像素。没有实际尺寸参照时，相机无法区分“小物体离得近”和“大物体离得远”。

棋盘格不同：它有已知的实际尺寸，相邻角点间距固定为 80 mm。照片中又能检测到 30 个角点，这些点必须共同组成一个固定大小的刚性网格，不能沿各自视线任意移动。

已知尺寸提供距离，透视变化提供朝向

先看一个简单例子：物体正对相机、忽略畸变时，实际宽度为 $L$、图像宽度为 $l$ 像素、焦距为 $f$ 像素，则：

$$Z\approx \frac{fL}{l}.$$

假设一个 80 mm 的间距在照片中占 40 像素，焦距约为 278 像素，则距离约为 556 mm。这只是帮助理解尺度来源的例子，不能直接用于斜视棋盘格的完整位姿求解。

棋盘格倾斜时，图像中的网格会产生透视变化，例如一侧大、一侧小。这些变化与整体位置、尺度一起，提供了估计朝向的信息。

PnP 在做什么

https://blog.csdn.net/qq_45445740/article/details/145780003

PnP 可以理解为：在相机前放一个真实尺寸的虚拟棋盘格，调整它的位置和朝向，让它的角点投影尽量与照片中的角点重合。

所需输入包括：

1. 棋盘角点的实际三维坐标。
2. 照片中对应角点的像素坐标。
3. 与这张照片匹配的相机内参和畸变参数。

求解结果是棋盘格到相机的旋转与平移：

$${\mathbf{P}}_{cam}=R_{board2cam}{\mathbf{P}}_{board}+t_{board2cam}.$$

位姿只有六个自由度：三个平移、三个旋转。多个角点共同约束同一个位姿，算法通过减小所有角点的重投影误差找到合适的解。重投影误差，就是预测的角点像素与实际检测像素之间的差异。

角点虽然共面，仍然可以估计完整姿态：两个面内轴的方向确定后，第三个轴由右手系确定。已知内参和真实尺寸使这类平面位姿问题可解；并不要求标定点一定分布在不同高度。

因此，一张清晰、角点分布良好的照片通常就包含足够信息。一张照片足够估计，不等于任何照片都可靠：目标太小、模糊、弱透视或角点编号错误，都可能导致不稳定或歧义，需要做质量检查。

4. 从棋盘格位姿得到相机外参

为了避免方向混淆，统一采用“来源 2 目标”的命名，并使用齐次列向量：

$${\mathbf{p}}_{\text{目标}}=T_{\text{来源2目标}}{\mathbf{p}}_{\text{来源}}.$$

变换	含义	来源
T_board2robot	棋盘格 → 机器人	工装测量，已知
T_board2cam	棋盘格 → 相机	PnP 求解
T_cam2robot	相机 → 机器人	最终目标

先求机器人到相机，再取逆

将工装变换取逆，得到机器人到棋盘格：

$$T_{robot2board}=T_{board2robot}^{-1}.$$

接上棋盘格到相机的变换：

$$T_{robot2cam}=T_{board2cam}{T}_{robot2board}.$$

矩阵从右向左作用，对应“机器人 → 棋盘格 → 相机”。再整体取逆：

$$T_{cam2robot}=T_{robot2cam}^{-1}.$$

利用 $(AB{)}^{-1}=B^{-1}{A}^{-1}$，可以直接写成：

$$\boxed{T_{cam2robot}=T_{board2robot}{T}_{board2cam}^{-1}}.$$

这也是当前代码采用的路线：先得到相机 → 棋盘格，再接上棋盘格 → 机器人。 两种计算方式完全等价，矩阵顺序不能交换。

最终外参表示什么

$$T_{cam2robot}=\left[\begin{array}{cc}R& t\\ 0_{1\times 3}& 1\end{array}\right].$$

• $t$：相机光心在机器人坐标系中的位置，即相对于两个后轮中心中点的位置。
• $R$：相机坐标轴在机器人坐标系中的朝向。

外参描述的是两个坐标系的关系，不只是两个中心点之间的距离。反方向的 T_robot2cam 也是同一关系的合法表达，使用时必须明确方向。

刚体变换取逆时，旋转变为 $R^T$，平移变为 $-R^{T}t$，不能只把平移取负。

5. 代码怎样实现

标定入口为 Calibration::CalibrationCameraExtrinsic()。

5.1 检测角点并建立实际坐标

先检测 6×5 个内角点，用 cornerSubPix() 精化像素位置，再按 80 mm 间距生成对应的实际坐标。

cv::Size patternSize(board_width_, board_height_);
cv::Mat image;
cvtColor(image_o, image, cv::COLOR_BGR2GRAY);
std::vector<cv::Point2f> imagePoints;
bool found = findChessboardCorners(image, patternSize, imagePoints);
if (!found) {
    calibration_result_.resultId = ResultId::FindChessboardCornersError;
    return calibration_result_;
}
cornerSubPix(image, imagePoints, cv::Size(11, 11), cv::Size(-1, -1),
                         cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::COUNT, 30, 0.1));
std::vector<cv::Point3f> objectPoints;
for (int i = 0; i < patternSize.height; i++) {
    for (int j = 0; j < patternSize.width; j++) {
        float img_x = (j - float(patternSize.width - 1.f) / 2.f) * square_size_;
        float img_y = (i - float(patternSize.height - 1.f) / 2.f) * square_size_;
        objectPoints.push_back(cv::Point3f(img_x, img_y, 0));
    }
}

objectPoints 与 imagePoints 必须一一对应：前者是毫米坐标，后者是像素坐标。

5.2 PnP 求解棋盘格到相机的位姿

cv::Mat rvec_ = cv::Mat::zeros(3, 1, CV_64F);
cv::Mat t_cb_ = cv::Mat::zeros(3, 1, CV_64F);
cv::solvePnP(objectPoints, imagePoints, cameraMatrix_, distCoeffs_, rvec_, t_cb_, false);

Eigen::Vector3d t_cb;
t_cb << t_cb_.at<double>(0, 0), t_cb_.at<double>(1, 0), t_cb_.at<double>(2, 0);
Eigen::Vector3d rvec;
rvec << rvec_.at<double>(0, 0), rvec_.at<double>(1, 0), rvec_.at<double>(2, 0);
Eigen::Matrix3d rotation_cb = Eigen::AngleAxisd(rvec.norm(), rvec.normalized()).toRotationMatrix();
Eigen::Isometry3d T_cb = Eigen::Isometry3d::Identity();
T_cb.linear() = rotation_cb;
T_cb.translation() = t_cb;
Eigen::Isometry3d T_bc = T_cb.inverse();

rvec_ 是轴角形式的旋转向量，不是三个欧拉角。T_cb 对应 T_board2cam；取逆后的 T_bc 表示相机 → 棋盘格。实际使用时，还应检查 solvePnP() 的返回值，并处理旋转向量为零的情况，摘录保留了当前实现。

5.3 连接工装变换，得到相机外参

第 2 节的 board_position_robot 提供棋盘格位置，板面方向由以下固定旋转矩阵给出，二者共同构成完整工装变换：

Eigen::Matrix3d rotation_rb;
rotation_rb << 0, 0, 1, -1, 0, 0, 0, -1, 0;
Eigen::Vector3d t_rb(t_rb_x_, t_rb_y_, t_rb_z_);
Eigen::Isometry3d T_rb = Eigen::Isometry3d::Identity();
T_rb.linear() = rotation_rb;
T_rb.translation() = t_rb;

Eigen::Isometry3d T_rc = T_rb * T_bc;
calibration_result_.T_rc = T_rc;
Eigen::Vector3d t_rc = T_rc.translation();
Eigen::Matrix3d rotation_rc = T_rc.rotation();
Eigen::Vector3d euler_angles_rc = GetEulerAngles(rotation_rc);

T_rb 对应 T_board2robot，因此 T_rc = T_rb * T_bc 就是第 4 节的核心公式。保存的 T_rc 是相机到机器人，而不是机器人到相机。

该矩阵将棋盘 X、Y、Z 轴分别映射到机器人负 Y、负 Z、正 X 轴，描述了一块垂直于机器人前向的立式棋盘格。这些工装参数不能照搬到其他机器人，位置、朝向和长度单位都必须匹配。

5.4 检查安装差异并保存外参

代码先按机械设计值构造相机的设计位姿，再与标定结果比较：

Eigen::Matrix3d R_rcStructure;
R_rcStructure = Eigen::AngleAxisd(yaw_rcS_ / 180 * M_PI, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(pitch_rcS_ / 180 * M_PI, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(roll_rcS_ / 180 * M_PI, Eigen::Vector3d::UnitX());
R_rcStructure = R_rcStructure * rotation_rb;
Eigen::Vector3d t_rcStructure(Cx_rcS_, Cy_rcS_, Cz_rcS_);
Eigen::Isometry3d T_rcStructure = Eigen::Isometry3d::Identity();
T_rcStructure.linear() = R_rcStructure;
T_rcStructure.translation() = t_rcStructure;

Eigen::Isometry3d T_ccStructure = T_rc.inverse() * T_rcStructure;
Eigen::Vector3d t_ccStructure = T_ccStructure.translation();
Eigen::Matrix3d R_ccStructure = T_ccStructure.rotation();
Eigen::Vector3d Euler_R_ccStructure = GetEulerAngles(R_ccStructure);

double maxAbs_t = t_ccStructure.cwiseAbs().maxCoeff();
double maxAbs_Euler = Euler_R_ccStructure.cwiseAbs().maxCoeff();
calibration_result_.resultDisplacementError = maxAbs_t;
calibration_result_.resultAngleError = maxAbs_Euler;
if (maxAbs_t > Error_Install_displacement_ || maxAbs_Euler > Error_Install_angle_) {
    calibration_result_.resultId = ResultId::CameraExtrinsicError;
}

上述设计值和门限来自第 2 节的 camera_installation_parameters、allowed_error，只用于求解后的验收。随后保存外参：

cv::FileStorage fs_out(filename_out, cv::FileStorage::WRITE);
if (!fs_out.isOpened()) {
    calibration_result_.resultId = ResultId::WrittenError;
} else {
    fs_out << "T_rc" << cv::Mat(4, 4, CV_64F, T_rc.matrix().data()).t();
    fs_out << "t_rc" << cv::Mat(3, 1, CV_64F, t_rc.data());
    fs_out << "euler_angles_rc" << cv::Mat(3, 1, CV_64F, euler_angles_rc.data());
    fs_out.release();
}
return calibration_result_;

矩阵写出时的 .t() 用于处理 Eigen 列主序与 OpenCV 行主序的存储差异，并不是将外参取逆。即使安装差异超限，当前代码仍会尝试写文件，因此必须检查返回的 resultId，不能只看文件是否生成。

6. 从“算出结果”到“结果可信”

实际标定时，重点检查四件事：

1. 棋盘格准确。 方格尺寸、板面平整度、角点编号必须与模型一致。尺寸写错会影响平移尺度，编号翻转可能导致朝向错误。
2. 工装定位可靠。 机器人完全入销、支撑稳定，棋盘格相对机器人原点的测量准确。否则算法可能把定位误差当成相机装配误差。
3. 图像与相机模型匹配。 内参和畸变参数应对应实际拍摄图像；缩放、裁剪、旋转或去畸变后，不能直接混用原参数。
4. 有质量检查。 检查求解状态、角点是否位于相机前方、重投影误差及安装合理性。多次拍摄和重新入销后重复标定，可以分别检查算法与工装的重复性。

当前代码的重投影误差计算段被注释，因此不能把它当成已经启用的验收项。重投影误差小也只说明视觉拟合较好，仍需独立检查工装关系。

7.总结

这套方法可以概括为：已知尺寸棋盘格提供尺度，PnP 求出棋盘格到相机的位姿，工装提供棋盘格到机器人的位姿，最后通过坐标变换得到相机外参。

量产过程中，只要最终算出来的相机相对于机器人的中心点位姿（位置误差在设置的距离内如1cm，姿态误差在设置的弧度内如1度，就认为安装是合格的，否则就需要重新安装相机的位置，直到通过标定的校验。）

8.思考：割草机器人和机械臂相机外参标定的区别

• 关于机械臂和割草机器人的相机外参标定流程

以当前割草机器人的相机外参标定情况，可以类比为机械臂的眼在手上标定。

机械臂眼在手上标定的流程是：相机固定在机械臂末端，标定板固定不动，控制机械臂移动到不同的路点对标定板进行拍照，计算标定板到相机的位姿关系，通过十几组的数据，通过 AX = XB 计算得到相机到机械臂末端的位姿关系。
https://blog.csdn.net/qq_45445740/article/details/122170029

割草机器人相机外参标定的流程：提前制作好一个标定工装，包含固定好的棋盘格标定板和固定割草机器人的定位结构如定位销，这样割草机器人放在工装上后，结构可以提前计算得到标定板到割草机器人的两个后轮中心中点的位姿关系。（类比为机械臂的底座基座标原点）
割草机器人中心和棋盘格的位姿关系也是已知的即T_board2robot，割草机器人放在工装上后，通过相机拍照一张标定板，再通过相机内参和pnp算法，也能知道一张图像中标定板相对于相机的位姿即T_board2cam，然后先把T_board2robot求逆得到T_robot2board，最终通过 T_board2cam x T_robot2board = T_robot2cam，再求一次逆得到 T_cam2robot，即可求得相机到割草机器人中心点的位姿关系，即相机的外参。

所以割草机器人仅拍一张标定板照片就能够完成相机外参标定是因为提前已知了标定板和机器人中心原点的位姿关系。

• 割草机器人相机外参标定工装改进和标定精度提升思考

相机外参标定本质上是计算相机到机器人中心原点的位姿关系齐次矩阵，即旋转矩阵和平移向量，总共12个未知数，通过构建方程进行迭代解的方式逼近真值，这里面是存在误差的，同时视觉的角点检测也是存在误差的，割草机器人的标定工装虽然通过定位销等方式固定机器人，但还是存在一些人为的误差或加工误差等等，同时拍一张图像视觉上也有误差，所以考虑能否让工装放置机器人后，通过固定夹紧方式，提前设置一个固定的点位，让机器人进行整体的平移或旋转，这样有多个点位进行这个方程的迭代解计算。

或许也有可能是因为割草机器人对识别的精度没那么高，毕竟地图上一个栅格都5cm或3cm了，比起机械臂正负0.02mm的重复定位精度差太多。

Ref

• OpenCV：PnP 位姿估计