MATLAB - 机械臂手眼标定（眼在手外） - 估算固定相机相对于机器人基座的姿态

系列文章目录

---

前言

在拾取和放置任务中，例如垃圾桶拾取，通常会在环境中的固定位置安装摄像头，以便机器人操纵器检测工作区中的物体。基本感知管道使用该摄像头来估计目标物体相对于摄像头坐标系的姿态。然后将该姿态转换到机器人的基准坐标系，使机器人能够成功地用末端执行器拾取物体。要将物体的姿态转换到机器人基座坐标系，首先必须确定摄像机相对于机器人基座的姿态。本图显示了这种拾放构型以及所需的同质变换矩阵 TCameraToBase，它可将摄像机坐标系中的姿势变换到机器人底座坐标系中。

由于这些因素的影响，确定相机坐标系的准确姿态成为难题：

照相机制造商通常不会在设备上直接标系照相机坐标系，也不会提供相关规格或物理指标。此外，与取放操作相关的坐标系并不是相机坐标系本身，而是位于图像传感器或平面中心的坐标系。

假定相机坐标系以镜头为中心，并依靠人工用尺子测量的估算方法很容易出现误差。

鉴于上述挑战，本示例展示了通过使用手眼校准工作流程确定变换矩阵的过程，即安装在横梁上的摄像头相对于机器人底座的姿态。

本示例与 "为带摄像头的机械臂执行手眼校准 "示例相关，后者展示了带直接安装在机械臂上的摄像头（即手眼构型）的机械臂的校准过程。本示例展示了如何解决眼到手构型的摄像头校准问题。

---

**一、**估算相机固有参数

了解摄像机的固有参数（如焦距和光学中心）对于准确解读图像至关重要。本示例在 estimateCameraPoseWrtRobotBase.mat 文件中提供了此设置的相机固有参数。但是，对于不同的相机和设置，您必须在收集手眼校准数据之前确定相机本征。您可以使用 Camera Calibrator（计算机视觉工具箱）应用程序来估算固定摄像头的这些参数。有关如何使用此应用程序的更多详情，请参阅使用单摄像头校准器应用程序（计算机视觉工具箱）。

二、收集手眼校准数据

要收集手眼构型的校准数据，必须遵循以下步骤：

1. 将校准板安装在机器人的末端执行器上。
2. 将机器人移动到一个构型，在该构型中，安装的摄像头可以完全看到校准板的图案。
3. 从摄像头捕捉校准板的图像，并记录机器人相应的关节构型。
4. 重复步骤 1-3，收集至少 15 张此类图像和相应的机器人关节构型。

在此示例设置中，校准数据已收集到 calibrationData.zip 文件中。该文件包含校准图像和相应的机器人关节构型。

解压缩校准数据并显示所有校准图像。

unzip("calibrationData.zip")
calibimgds = imageDatastore('calibrationData/*.jpg');
figure(Name="Calibration Images")
montage(calibimgds);

从 jointAngles.mat 文件中加载并显示相应的关节构型。每个关节构型的行号与相应校准图像的名称一致。例如，关节角度的第 10 行对应校准图像 10.jpg。关节角度以度为单位。

load("calibrationData/jointAngles")
disp(jointAngles)

  346.8241  352.5997  191.5695  212.1722  355.5775   38.5151  184.8700
  346.8241  352.5996  191.5695  212.1724  355.5776   20.6987  184.8699
  346.8241  352.5996  191.5695  212.1723  355.5775   55.1762  184.8699
  346.8241  352.5996  191.5695  212.1723  355.5775   55.1761  159.7137
  344.9588  352.5997  191.5695  212.1723  355.5775   47.9125  202.4223
  344.9591  350.7478  191.5696  212.1723  334.9004   43.3278  191.6003
  344.9592  350.7479  191.5696  212.1722   22.5804   43.3278  191.6003
  344.9592  350.7477  191.5696  212.1723   14.6105   17.6080  191.6004
  344.9592  350.7478  191.5696  212.1723   65.1178   27.7940  121.8693
    9.4972  350.7477  191.5696  212.1723   65.1178   27.7940  121.8693
    2.4139  356.8379  179.3088  225.4389   79.5231   35.3008  125.8531
   15.2262  356.8380  179.3092  225.4386   48.9265   55.5291  125.8531
   15.2261    4.5792  179.3092  225.4386   48.9264   55.5291  175.2737
   31.0168    4.5793  179.3092  225.4386   48.9263   55.5291  174.4471
   34.9532  358.5155  179.3093  225.4386   48.9264   55.5294  145.9294

要以某种关节构型显示机器人，请加载相应的机器人模型，并以所需的关节构型显示机器人模型。

加载 KINOVA® Gen3 机器人的刚体树形机器人模型，并将其可视化为第 7 个关节构型。

figure
kinova = loadrobot('kinovaGen3',DataFormat="row");
show(kinova,deg2rad(jointAngles(7,:)));
title("Joint Angles in 7th Configuration")
axis equal

三、计算摄像机外特性和末端执行器姿势

手眼校准问题需要几组姿势。每组包含两个姿势：

1. 相机外参照姿态，即棋盘原点相对于相机的姿态。
2. 末端执行器相对于机器人基本坐标系的姿态。

您可以使用校准图像和相应的关节构型推导出这两种姿势。

3.1 估算摄像机外特性

首先计算摄像机外特性，即同质变换矩阵 TWorldToCamera。

创建用于存储每幅校准图像的相机外特性的相机外特性向量。

numimgs = length(calibimgds.Files);
camextrinsics = repmat(rigidtform3d,1,numimgs);

在估算摄像机外特性之前，必须确定摄像机的内在参数和每个校准图像中棋盘格的大小。在本示例中，本征参数和棋盘格大小均已计算完毕。有关计算摄像机固有参数的更多信息，请参阅 estimateCameraParameters（计算机视觉工具箱）功能页面上的单摄像机校准示例。

加载相机固有参数和棋盘格大小。

load("estimateCameraPoseWrtRobotBase","intrinsics");
load("estimateCameraPoseWrtRobotBase","checkerboardSquareSizeInMeters");

对于数据集中的每幅图像，都要计算相机的外特性。这一估算需要校准图像、校正畸变后的图像以及世界坐标系中棋盘格点的位置。然后使用这些数据来估计摄像机的姿态。

for r = 1:numimgs

    % Load and undistort each calibration image based on precomputed intrinsics.
    fname = fullfile('calibrationData',string(r)+".jpg");
    checkerboardImage = imread(fname);
    checkerboardImageUndistorted = undistortImage(checkerboardImage,intrinsics);

    % Detect the checkerboard corners, discarding partial detections.
    [imgpts,boardsz] = detectCheckerboardPoints(checkerboardImageUndistorted,PartialDetections=false);

    % Generate world coordinates of checkerboard points based on board size and square size.
    worldpts = patternWorldPoints("checkerboard",boardsz,checkerboardSquareSizeInMeters);

    % Estimate the camera extrinsics (pose) from image points and world points.
    camextrinsics(r) = estimateExtrinsics(imgpts,worldpts,intrinsics);
end

3.2 计算末端执行器姿态

接下来，使用前向运动学计算机器人末端执行器相对于机器人底座坐标系的姿态。表示该姿态的均质变换矩阵为 TEndEffectorToBase。

在 TEndEffectorToBase 向量中记录末端执行器相对于机器人基本坐标系的位置。

TEndEffectorToBase = repmat(rigidtform3d,1,numimgs);

对于每个关节构型，使用 getTransform 函数应用前向运动学。这会计算机器人 "EndEffector_Link "体相对于机器人基本坐标系的姿态。

for r = 1:numimgs
    jointconfig = deg2rad(jointAngles(r,:));
    TEndEffectorToBase(r) = getTransform(kinova,jointconfig,"EndEffector_Link");
end

四、解决校准问题

利用校准数据中的摄像机外特性和末端执行器姿态，估算摄像机相对于机器人基座的姿态。

TCameraToBase = helperEstimateHandEyeTransform(camextrinsics,TEndEffectorToBase,"eye-to-hand");

接下来，计算校准棋盘点相对于机器人基本坐标系的位置。

TWorldToBase = rigidtform3d(TCameraToBase.A*camextrinsics(end).A);
worldpts3d = transformPointsForward(TWorldToBase,[worldpts,zeros(size(worldpts,1),1)]);

用棋盘格点显示机器人，并放大与校准图像进行比较。

tiledlayout(1,2)
nexttile
show(kinova,deg2rad(jointAngles(r,:)));
hold on
scatter3(worldpts3d(:,1),worldpts3d(:,2),worldpts3d(:,3),'*')
view(95,45)
axis([-0.05 0.95 -0.65 0.35 0.25 0.75])
zoom(2.5)
hold off
nexttile
imshow("calibrationData/"+string(r)+".jpg")
sgtitle("Computed Checkerboard Points vs Calibration Image")

五、可视化摄像机坐标系与机器人基本坐标系的关系

绘制摄像机、机器人和棋盘格点在机器人基准坐标系中的解析图。

figure
show(kinova,deg2rad(jointAngles(r,:)));
hold on
scatter3(worldpts3d(:,1),worldpts3d(:,2),worldpts3d(:,3),'*')
plotCamera(AbsolutePose=TCameraToBase,Size=0.05,Label="Fixed Camera");
title("Estimated Camera Frame With Respect to Robot Base")
view([-40,10])
zoom(1.5)
axis equal
hold off

与实际设置进行比较后发现，相对于机器人基座，摄像机的估计姿态是正确的。