系列文章目录
前言
在拾取和放置任务中，例如垃圾桶拾取，通常会在环境中的固定位置安装摄像头，以便机器人操纵器检测工作区中的物体。基本感知管道使用该摄像头来估计目标物体相对于摄像头坐标系的姿态。然后将该姿态转换到机器人的基准坐标系，使机器人能够成功地用末端执行器拾取物体。要将物体的姿态转换到机器人基座坐标系，首先必须确定摄像机相对于机器人基座的姿态。本图显示了这种拾放构型以及所需的同质变换矩阵 TCameraToBase，它可将摄像机坐标系中的姿势变换到机器人底座坐标系中。
由于这些因素的影响，确定相机坐标系的准确姿态成为难题：
照相机制造商通常不会在设备上直接标系照相机坐标系，也不会提供相关规格或物理指标。此外，与取放操作相关的坐标系并不是相机坐标系本身，而是位于图像传感器或平面中心的坐标系。
假定相机坐标系以镜头为中心，并依靠人工用尺子测量的估算方法很容易出现误差。
鉴于上述挑战，本示例展示了通过使用手眼校准工作流程确定变换矩阵的过程，即安装在横梁上的摄像头相对于机器人底座的姿态。
本示例与 “为带摄像头的机械臂执行手眼校准 ”示例相关，后者展示了带直接安装在机械臂上的摄像头（即手眼构型）的机械臂的校准过程。本示例展示了如何解决眼到手构型的摄像头校准问题。
一、估算相机固有参数
了解摄像机的固有参数（如焦距和光学中心）对于准确解读图像至关重要。本示例在 estimateCameraPoseWrtRobotBase.mat 文件中提供了此设置的相机固有参数。但是，对于不同的相机和设置，您必须在收集手眼校准数据之前确定相机本征。您可以使用 Camera Calibrator（计算机视觉工具箱）应用程序来估算固定摄像头的这些参数。有关如何使用此应用程序的更多详情，请参阅使用单摄像头校准器应用程序（计算机视觉工具箱）。
二、收集手眼校准数据
要收集手眼构型的校准数据，必须遵循以下步骤：
- 将校准板安装在机器人的末端执行器上。
- 将机器人移动到一个构型，在该构型中，安装的摄像头可以完全看到校准板的图案。
- 从摄像头捕捉校准板的图像，并记录机器人相应的关节构型。
- 重复步骤 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
三、计算摄像机外特性和末端执行器姿势
手眼校准问题需要几组姿势。每组包含两个姿势：
- 相机外参照姿态，即棋盘原点相对于相机的姿态。
- 末端执行器相对于机器人基本坐标系的姿态。
您可以使用校准图像和相应的关节构型推导出这两种姿势。
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);
end3.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
与实际设置进行比较后发现，相对于机器人基座，摄像机的估计姿态是正确的。
