Eye-in-hand和Eye-to-hand问题求解和实验

2018/12/5 By Yang Yang（yangyang@ipp.ac.cn）

本文所有源码和仿真场景文件全部公开，点击Gitee仓库链接。

问题描述

机器人和摄像机的手眼标定问题分为两类构型：

• eye-to-hand，摄像机固定，与机器人基坐标系相对位置不变。
• eye-in-hand，摄像机安装在机器人末端，随着机器人一起移动。

本文主要介绍eye-in-hand构型的标定方法和仿真实验。对于eye-to-hand构型手眼系统，其标定方法与eye-in-hand构型的手眼系统没有本质区别。

Eye-in-hand问题求解公式推导

Eye-in-hand变换矩阵

$${}_e^w{H}_i{\ast }_c^{e}H{=}_g^{w}H{\ast }_c^g{H}_i$$
$${}_e^w{H}_j{\ast }_c^{e}H{=}_g^{w}H{\ast }_c^g{H}_j$$

其中：

• ${}_e^w{H}_i$ 和 ${}_e^w{H}_j$ 表示机器人末端（end）TCP坐标系相对于机器人基座坐标系的齐次变换矩阵，分别对应第 $i$ 次和第 $j$ 次样本。
• ${}_c^{e}H$ 表示摄像机（camera）坐标系相对于机器人末端（end）TCP坐标系的齐次变换矩阵，这是手眼标定问题的求解目标。
• ${}_g^{w}H$ 表示棋盘图（grid）相对于世界坐标系（也就是机器人基坐标系）的齐次变换矩阵，由于在整个标定过程中，棋盘图的位置不能变动，因此 ${}_g^{w}H$ 是一个常量矩阵。
• ${}_c^g{H}_i$ 和 ${}_c^g{H}_j$ 表示摄像机坐标系相对于棋盘图坐标系的齐次变换矩阵。注意：采用OpenCV的solvePnP函数得到的是棋盘图相对于摄像机坐标系的齐次矩阵，即 ${}_g^c{H}_i$ 和 ${}_g^c{H}_j$，因此代入以上两个公式时需要进行求逆计算。

齐次变换矩阵求逆方法
对于原齐次矩阵 $${}_B^{A}T=\left[\begin{array}{cc}{}_B^{A}R& {}^A{p}_{B_0}\\ 0& 1\end{array}\right]$$，其逆矩阵为 $${}_A^{B}T=\left[\begin{array}{cc}{}_B^A{R}^T& {-}_B^A{R}^T{\ast }^A{p}_{B_0}\\ 0& 1\end{array}\right]$$

求解方程

联立上述的两个公式，

${}_e^w{H}_i{\ast }_c^{e}H\ast {(}_c^g{H}_i{)}^{-1}{=}_g^{w}H$

${}_e^w{H}_j{\ast }_c^{e}H\ast {(}_c^g{H}_j{)}^{-1}{=}_g^{w}H$

消除常量矩阵 ${}_g^{w}H$，

${}_e^w{H}_i{\ast }_c^{e}H\ast {(}_c^g{H}_i{)}^{-1}{=}_e^w{H}_j{\ast }_c^{e}H\ast {(}_c^g{H}_j{)}^{-1}$

上式两边同时左乘 ${(}_e^w{H}_j{)}^{-1}$ 得，

${(}_e^w{H}_j{)}^{-1}{\ast }_e^w{H}_i{\ast }_c^{e}H\ast {(}_c^g{H}_i{)}^{-1}{=}_c^{e}H\ast {(}_c^g{H}_j{)}^{-1}$

上式两边同时右乘 ${}_c^g{H}_i$ 得

${(}_e^w{H}_j{)}^{-1}{\ast }_e^w{H}_i{\ast }_c^{e}H{=}_c^{e}H\ast {(}_c^g{H}_j{)}^{-1}{\ast }_c^g{H}_i$

考虑到${(}_c^g{H}_j{)}^{-1}={(}_g^c{H}_j)$ 和 ${(}_c^g{H}_i)={(}_g^c{H}_i{)}^{-1}$，因此上式可以转换为，

${(}_e^w{H}_j{)}^{-1}{\ast }_e^w{H}_i{\ast }_c^{e}H{=}_c^{e}H{\ast }_g^c{H}_j\ast {(}_g^c{H}_i{)}^{-1}$

令$X{=}_c^{e}H$，$A={(}_e^w{H}_j{)}^{-1}{\ast }_e^w{H}_i$ 以及 $B{=}_g^c{H}_j\ast {(}_g^c{H}_i{)}^{-1}$，则上式可以化简为，

$AX=XB$

以上方程为手眼标定的最终求解方程，具体求解方式可以参考Tasi and Lenz，1989的论文A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration。

仿真实验

以下描述通过仿真实验的方式进行手眼标定测试和验证的方法。大致流程如下：

1. 手眼系统仿真场景搭建。
2. 标定样本采集。包括摄像机图像采集，以及对应的机器人关节构型采集。
3. 图像处理提取棋盘图角点并计算棋盘相对于摄像机坐标系的位姿。
4. 采用基于Tasi-lenz的公式进行计算。

仿真实验的主要环境配置和使用到的工具有：

• 操作系统：Windows 7 64bit
• 图像处理工具：Matlab，OpenCV
• 机器人和摄像机仿真环境：RobWork （需先安装QT、Boost等依赖库）
• 虚拟棋盘图编辑：AC3D，用于生成带贴图的棋盘标定板，加载到手眼仿真场景中

搭建手眼系统仿真环境

生成棋盘图

利用如下MATLAB代码"ChessboardGenerator"生成一个边长为30mm，7行10列的棋盘图，图像最终保存为ppm格式的图片文件。之所以选择生成7行10列的棋盘图，是因为这样能够方便的利用OpenCV进行棋盘角点检测，并且保证每张图像的棋盘图坐标系原点都是一致的，角点检测顺序也不会发生改变。不应当使用长和宽都是偶数的棋盘图，例如10行8列，这样会导致OpenCV无法统一棋盘原点，给下一步的手眼标定工作带来困难。

以下源码参考用Matlab编写的棋盘格图像生成程序

clc; close all;

GridSize = 30;  %length of the square
m =7;      %number of row
n = 10;       % number of col
margin = 10; % white boarder size
I = ones(m*GridSize+2*margin,n*GridSize+2*margin)*255;

%the first grid is black
for i = 1:m
    if mod(i,2)==1
        for j = 1:n
            if mod(j,2)==1
                I(1+(i-1)*GridSize+margin:i*GridSize+margin,...
                1+(j-1)*GridSize+margin:j*GridSize+margin) = 0;
            end
        end
    else
        for j = 1:n
            if mod(j,2)==0
               I(1+(i-1)*GridSize+margin:i*GridSize+margin,...
                1+(j-1)*GridSize+margin:j*GridSize+margin) = 0;
            end
        end
    end
end
imshow(I);

imwrite(I,'chessboard.ppm');

最终生成棋盘图如下所示。

生成仿真棋盘标定板

处理步骤：

1. 首先利用ChessboardGenerator生成棋盘图，格式为BMP。棋盘图的长边为10个方块，短边为7个方块，每个方块的尺寸为30个像素单位，另外棋盘图还有10个像素单位的白边。因此整个图像的分辨率为320x230。
2. 打开AC3D软件，新建一个Rect矩形，通过MoveTo调整矩形的中心点坐标。
3. 通过, 调整矩形的尺寸。
4. 选中矩形对象，在object->Texture设置纹理为步骤（1）中的ppm图像。（图像看起来比较模糊，测试：提高生成图像分辨率，能够获得更清晰的虚拟Chessboard Marker）
5. 将矩形保存成chessboard.ac格式文件，必要时用文本编辑器打开该.ac格式文件，如有必要，需修改纹理图像的路径为相对路径。

编写手眼系统场景文件

RobWork是一个由南丹麦大学SDURobtics团队开发的一款用于机器人运动学、动力学计算和仿真的代码库，其包含了一种用于机器人可视化仿真和摄像机仿真的软件RobWorkStudio。RobWork提供了一种基于XML文件的机器人场景构建方法，该场景文件可以用于机器人的碰撞检测、路径规划、视觉传感器仿真等，并且可以在RobWorStudio中进行场景的可视化。

将上一步生成的chessboard.ac文件添加到场景XML文件中，并且在UR机器人末端设置一个虚拟摄像机，最终得到的机器人运行场景如下图。

安装在UR末端的摄像机的定义方式可以参考RobWork官网上的教程，本例中摄像机的定义方式如下如下：

<Frame name="Hand-Eye" refframe="UR.TCP">
	<RPY>0 -15 0</RPY>
	<Pos>0.1 0 0</Pos>
</Frame>

<Frame name="Hand-Eye-Sim" refframe="Hand-Eye" type="Movable">
	<RPY>0 0 180</RPY>
	<Pos>0 0 0</Pos>
	<Property name="CameraName">Hand-Eye</Property>
	<Property name="Camera">31.0482 960 720</Property>
</Frame>	

编写基于RobWorkStudio的标定图像采集模拟程序

需编写一个RobWorkStudio的插件，用于采集摄像机图片，以及与图像对应的机器人TCP相对于机器人基坐标系的齐次变换矩阵，并保存到本地硬盘上。

源码的运行要求安装RobWork，安装方法参考官网链接。

编译本教程提供源码时，需注意修改加载的机器人场景文件的路径。

编写基于OpenCV的标定计算程序

棋盘角点和位姿检测

    int cornerRow = 6;
	int cornerColum = 9;
	double patternLength = 30.0; //mm

	vector<rw::math::Transform3D<double> > chessboardPose;

	vector<Point3d> objectPoints;
	int pointNumber = cornerRow * cornerColum;
	for (int i = 0; i < cornerRow; i++)
	{
		for (int j = 0; j < cornerColum; j++)
		{
			Point3d p;
			p.x = patternLength * j;
			p.y = patternLength * i;
			p.z = 0;
			objectPoints.push_back(p);
		}
	}

	int imageResolutionX = 960;
	int imageResolutionY = 720;
	double fovy = 31.0482 * 3.14159265 / 180;
	double focalLength = (imageResolutionY / 2.0) / (tan(fovy / 2.0));

	Mat cameraMatrix = Mat(3, 3, CV_64FC1, 0.0f);
	cameraMatrix.at<double>(0, 0) = focalLength;
	cameraMatrix.at<double>(0, 2) = imageResolutionX / 2.0f;
	cameraMatrix.at<double>(1, 1) = focalLength;
	cameraMatrix.at<double>(1, 2) = imageResolutionY / 2.0f;
	cameraMatrix.at<double>(2, 2) = 1.0f;

	Mat distortionCoff(0, 0, 0, 0);
	cout << "\nCamera Matrix: \n" << cameraMatrix << endl;

	//namedWindow("Display Image", WINDOW_AUTOSIZE);

	int imageNumber = 25;

	for (int i = 0; i < imageNumber; i++)
	{
		char imageName[150];
		sprintf(imageName, "..\\..\\data\\test-data-01\\image%05d.bmp", i);

		Mat imageColored = imread(imageName, 1);
		if (!imageColored.data)
		{
			printf("No image data \n");
			return -1;
		}
		Mat image;
		cv::cvtColor(imageColored, image, CV_BGR2GRAY);

		Size patternSize(cornerColum, cornerRow);
		vector<Point2f> corners;

		bool patternfound = findChessboardCorners(image, patternSize, corners, CALIB_CB_ADAPTIVE_THRESH);

		if (patternfound)
		{
			printf("found chessboard of image%05d!\n", i);
			cornerSubPix(image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
		}

		Mat rvec; //reference to camera frame
		Mat tvec; //reference to camera frame
		solvePnP(objectPoints, corners, cameraMatrix, distortionCoff, rvec, tvec);

		Mat chessbaordRotationMatrix; //reference to camera frame
		Rodrigues(rvec, chessbaordRotationMatrix);

		rw::math::Transform3D<double> t;
		t(0, 0) = chessbaordRotationMatrix.at<double>(0, 0);
		t(0, 1) = chessbaordRotationMatrix.at<double>(0, 1);
		t(0, 2) = chessbaordRotationMatrix.at<double>(0, 2);
		t(0, 3) = tvec.at<double>(0) / 1000; //mm to m
		t(1, 0) = chessbaordRotationMatrix.at<double>(1, 0);
		t(1, 1) = chessbaordRotationMatrix.at<double>(1, 1);
		t(1, 2) = chessbaordRotationMatrix.at<double>(1, 2);
		t(1, 3) = tvec.at<double>(1) / 1000; //mm to m
		t(2, 0) = chessbaordRotationMatrix.at<double>(2, 0);
		t(2, 1) = chessbaordRotationMatrix.at<double>(2, 1);
		t(2, 2) = chessbaordRotationMatrix.at<double>(2, 2);
		t(2, 3) = tvec.at<double>(2) / 1000; //mm to m

		//printTransformation(t);

		chessboardPose.push_back(t);
	}

其中一张棋盘图的角点检测结果如下图：

标定计算

标定计算程序主要参考了经典手眼标定算法之Tsai-Lenz的OpenCV实现，主要源码如下：

cv::Mat skew(cv::Mat m)
{
	Mat t(3, 3, CV_64FC1);
	t.at<double>(0, 0) = 0;
	t.at<double>(0, 1) = - m.at<double>(2, 0);
	t.at<double>(0, 2) = m.at<double>(1, 0);
	t.at<double>(1, 0) = m.at<double>(2, 0);
	t.at<double>(1, 1) = 0;
	t.at<double>(1, 2) = - m.at<double>(0, 0);
	t.at<double>(2, 0) = - m.at<double>(1, 0);
	t.at<double>(2, 1) = m.at<double>(0, 0);
	t.at<double>(2, 2) = 0;

	return t;
}

void Tsai_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij)
{
	CV_Assert(Hgij.size() == Hcij.size());
	int nStatus = Hgij.size();

	Mat Rgij(3, 3, CV_64FC1);
	Mat Rcij(3, 3, CV_64FC1);

	Mat rgij(3, 1, CV_64FC1);
	Mat rcij(3, 1, CV_64FC1);

	double theta_gij;
	double theta_cij;

	Mat rngij(3, 1, CV_64FC1);
	Mat rncij(3, 1, CV_64FC1);

	Mat Pgij(3, 1, CV_64FC1);
	Mat Pcij(3, 1, CV_64FC1);

	Mat tempA(3, 3, CV_64FC1);
	Mat tempb(3, 1, CV_64FC1);

	Mat A;
	Mat b;
	Mat pinA;

	Mat Pcg_prime(3, 1, CV_64FC1);
	Mat Pcg(3, 1, CV_64FC1);
	Mat PcgTrs(1, 3, CV_64FC1);

	Mat Rcg(3, 3, CV_64FC1);
	Mat eyeM = Mat::eye(3, 3, CV_64FC1);

	Mat Tgij(3, 1, CV_64FC1);
	Mat Tcij(3, 1, CV_64FC1);

	Mat tempAA(3, 3, CV_64FC1);
	Mat tempbb(3, 1, CV_64FC1);

	Mat AA;
	Mat bb;
	Mat pinAA;

	Mat Tcg(3, 1, CV_64FC1);

	for (int i = 0; i < nStatus; i++)
	{
		Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
		Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);

		Rodrigues(Rgij, rgij);
		Rodrigues(Rcij, rcij);

		theta_gij = norm(rgij);
		theta_cij = norm(rcij);

		rngij = rgij / theta_gij;
		rncij = rcij / theta_cij;

		Pgij = 2 * sin(theta_gij / 2)*rngij;
		Pcij = 2 * sin(theta_cij / 2)*rncij;

		tempA = skew(Pgij + Pcij);
		tempb = Pcij - Pgij;

		A.push_back(tempA);
		b.push_back(tempb);
	}

	//Compute rotation
	invert(A, pinA, DECOMP_SVD);

	Pcg_prime = pinA * b;
	Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
	PcgTrs = Pcg.t();
	Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));

	//Computer Translation 
	for (int i = 0; i < nStatus; i++)
	{
		Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
		Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
		Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);
		Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);

		tempAA = Rgij - eyeM;
		tempbb = Rcg * Tcij - Tgij;

		AA.push_back(tempAA);
		bb.push_back(tempbb);
	}

	invert(AA, pinAA, DECOMP_SVD);
	Tcg = pinAA * bb;

	Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));
	Tcg.copyTo(Hcg(Rect(3, 0, 1, 3)));

	Hcg.at<double>(3, 0) = 0.0;
	Hcg.at<double>(3, 1) = 0.0;
	Hcg.at<double>(3, 2) = 0.0;
	Hcg.at<double>(3, 3) = 1.0;
}

标定结果比较

在自定义的手眼标定场景文件中，摄像机相对于机器人TCP的关系如下：

<Frame name="Hand-Eye" refframe="UR.TCP">
	<RPY>0 -15 0</RPY>
	<Pos>0.1 0 0</Pos>
</Frame>

转换成4 $\times$ 4的齐次变换矩阵如下：

$${}_c^{e}H=\left[\begin{array}{cccc}0.965926& 0& -0.258819& 0.1\\ 0& 1& 0& 0\\ 0.258819& 0& 0.965926& 0\\ 0& 0& 0& 1\end{array}\right]$$
通过Tasi-Lenz方法计算得出的（不考虑摄像机参数误差）摄像机相对于机器人TCP的关系如下：
$${}_c^{e}H=\left[\begin{array}{cccc}0.9659861360843613& 0.0003843732558368322& -0.2585935752295596& 0.1001249115855685\\ -0.0003187110704843794& 0.9999999054504882& 0.0002958416268544149& 7.92132967571962e-05\\ 0.2585936644932727& -0.0002033622748362118& 0.9659861672548555& 3.021520822331239e-05\\ 0& 0& 0& 1\end{array}\right]$$
通过对比以上两组齐次变换矩阵结果，可以两者是十分接近的，证明手眼标定结果是正确的。

---

Eye-to-Hand问题求解公式推导

Eye-to-Hand变换矩阵

$${}_c^{w}H{\ast }_g^c{H}_i{=}_e^w{H}_i{\ast }_g^{e}H$$
$${}_c^{w}H{\ast }_g^c{H}_j{=}_e^w{H}_j{\ast }_g^{e}H$$
其中：

• ${}_c^{w}H$表示摄像机（camera）坐标系相对于机器人基坐标系（也是世界坐标，world）的齐次变换矩阵。这是Eye-to-hand问题的求解目标。
• ${}_g^c{H}_i$和${}_g^c{H}_i$表示棋盘图（grid）相对于摄像机坐标系的齐次变换矩阵，分别对应第 $i$ 次和第 $j$ 次样本。
• ${}_e^w{H}_i$ 和 ${}_e^w{H}_j$ 表示机器人末端（end）TCP坐标系相对于机器人基座坐标系的齐次变换矩阵，分别对应第 $i$ 次和第 $j$ 次样本。
• ${}_g^{e}H$ 表示棋盘图（grid）相对于机器人末端TCP的齐次变换矩阵，由于在整个标定过程中，棋盘图固定连接在机器人末端，因此 ${}_g^{e}H$ 是一个常量矩阵。

求解方程

改写以上公式：
${(}_e^w{H}_i{)}^{-1}\ast {}_c^{w}H{\ast }_g^c{H}_i{=}_g^{e}H$
${(}_e^w{H}_j{)}^{-1}\ast {}_c^{w}H{\ast }_g^c{H}_j{=}_g^{e}H$
消除常量${}_g^{e}H$，
${(}_e^w{H}_i{)}^{-1}\ast {}_c^{w}H{\ast }_g^c{H}_i={(}_e^w{H}_j{)}^{-1}\ast {}_c^{w}H{\ast }_g^c{H}_j$
上式两边同时左乘${}_e^w{H}_j$得，
${}_e^w{H}_j\ast {(}_e^w{H}_i{)}^{-1}\ast {}_c^{w}H{\ast }_g^c{H}_i={}_c^{w}H{\ast }_g^c{H}_j$
上式两边同时右乘${(}_g^c{H}_i{)}^{-1}$，
${}_e^w{H}_j\ast {(}_e^w{H}_i{)}^{-1}\ast {}_c^{w}H={}_c^{w}H{\ast }_g^c{H}_j\ast {(}_g^c{H}_i{)}^{-1}$
令$X{=}_c^{w}H$，$A{=}_e^w{H}_j\ast {(}_e^w{H}_i{)}^{-1}$，$B{=}_g^c{H}_j\ast {(}_g^c{H}_i{)}^{-1}$，可得
$AX=XB$
以上方程是Eye-to-Hand标定的最终求解方程，具体求解方式可以参考Tasi and Lenz，1989的论文A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration。

具体的eye-to-hand求解仿真可以参考eye-in-hand标定求解仿真进行，这里不再赘述。

本文所有源码和仿真场景文件全部公开，点击Gitee仓库链接。