CN101630409B - 一种用于机器人制孔系统的手眼视觉标定方法 - Google Patents

Abstract

本发明公开了一种用于机器人制孔系统的手眼视觉标定方法，首先标定出机器人末端执行器上的工具坐标系原点即TCP，在所拍摄的平面内建立景物坐标系然后拍摄此平面内的两点，并用TCP接触这两点从而得到摄像机成像坐标系与景物坐标系之间的关系，并利用此关系计算出TCP在景物坐标系中的位置关系。最后，通过建立的景物坐标系作为中间转换坐标系间接得到了TCP与摄像机成像坐标系之间的位置关系，即得到了手眼关系。本发明忽略了手眼关系中摄像机的深度信息，将标定过程转化为几何关系，标定过程简便，计算量小，不需要昂贵的三坐标测量设备且精度较高，具有较高的实用价值，能够满足机器人制孔系统的实际工作需求。

Description

一种用于机器人制孔系统的手眼视觉标定方法

技术领域

本发明涉及一种用于机器人制孔系统的手眼视觉标定方法，属于先进制造与自动化领域。

背景技术

机器人制孔系统作为一种较新的自动化技术，对于飞机制造以及其他需要大量制孔的工业领域有着重要意义。为了实现机器人制孔系统的智能化和柔性化，需要为机器人加装视觉系统。通过视觉系统可以为机器人的制孔过程提供导航和定位信息。

现有技术中，机器人视觉主要有激光视觉或摄像机视觉两种视觉传感方式。前者主要是利用激光跟踪仪等设备获取目标点位置信息，其特点是精度高，但是设备价格昂贵而且得到的信息比较单一。摄像机视觉在机器人系统中也较为广泛，其特点是信息量大，速度较快，精度及自动化程度较高且成本相对较低，因此基于摄像机的手眼视觉可以适用于机器人制孔系统，手眼关系的标定是手眼视觉技术应用的关键环节。

以前的手眼视觉标定方法大多将摄像机的内、外参数同时求解出来，其过程复杂需要求解大量矩阵齐次变换方程AX＝XB(Y.C.Shiu and S.Ahmad，“Calibration ofwrist-mounted robotic sensors by solving homogeneous transform equations of theform AX＝XB”，IEEE Trans.Robot.Automat.，vol.5，pp.16-29，Feb.1989)，这些方法在计算过程中由于计算机舍入误差的积累最终影响标定精度。除此之外还可采用三坐标测量设备进行手眼标定(周富强、张广军，现场双经纬仪三维坐标测量系统[J].机械工程学报，vol.40，No.1，2004，pp.165-169)，但该方法成本高，且不适用于在工业环境下的现场标定。

Product manuals，ABB Robot Document IRB 1410M2004，2004公开了机器人“四点法”，用于标定机器人末端TCP(Tool Center Point)。

机器人工作前在所加工的零件上建立一个坐标系，此坐标系即为工件坐标系，机器人通常工作在工件坐标系下。例如工件坐标系的建立可以由“三点法”标定得到，见Productmanuals，ABB Robot Document IRB 1410 M2004，2004。工件坐标系是根据工件自由建立的，而且建立的方法不仅限于″三点法″，甚至可以把机器人基坐标系也建立为工件坐标系，工件坐标系的目的是为了方便机器人对工件的加工。

对于机器人制孔系统而言，由于机器人末端执行器要始终垂直于工件表面，手眼关系中的摄像机深度信息并不重要，因此可以将标定方法进行简化从而减少不必要的计算增强现场标定的可靠性与实用性。

发明内容

本发明的目的是为了解决现有技术存在的上述不足和缺陷，提供一种用于机器人制孔系统的二维手眼视觉标定方法，该方法操作简便易于掌握，计算量少，成本低且具有较高的精度，能够满足机器人制孔系统在生产过程中的应用需要。

本发明的一种用于机器人制孔系统的手眼视觉标定方法，包括以下几个步骤：

首先标定出机器人末端执行器上的工具坐标系原点即TCP，在所拍摄的平面内建立景物坐标系然后拍摄此平面内的两点，并用TCP接触这两点从而得到摄像机成像坐标系与景物坐标系之间的关系，并利用此关系计算出TCP在景物坐标系中的位置关系。最后，通过建立的景物坐标系作为中间转换坐标系间接得到了TCP与摄像机成像坐标系之间的位置关系，即得到了手眼关系。具体为：

步骤一：利用“四点法”将机器人末端执行器的刀具中心点标定为工具坐标系原点即TCP；

步骤二：将标定模板平面与机器人基坐标系底平面平行放置；

步骤三：调整机器人末端与摄像机同时垂直于标定模板；

步骤四：记录摄像机拍摄标定模板成清晰图像时，工具坐标系TCP的位置，在图像中选择任意两点p′₁与p′₂，并计算出两点在摄像机成像坐标系x′o′y′中的位置以及与坐标系x′轴所成的夹角γ；

步骤五：用TCP点接触标定模板上确定的p₁与p₂，并记录p₁与p₂在机器人基坐标系W中的位置坐标，计算出p₁与p₂连线在机器人基坐标系W中与坐标系x_w轴所成的夹角θ；

步骤六：计算机器人末端TCP距离标定模板高度为H_w时摄像机成像坐标x′o′y′系与景物坐标系xoy的比例关系k；

步骤七：计算摄像机成像坐标系x′o′y′与景物坐标系xoy之间的旋转角度、位置转换关系、TCP在景物坐标系xoy中的位置坐标以及TCP在摄像机成像坐标系x′o′y′中对应的位置关系，得到机器人手眼的相对关系；

步骤八：确定目标点在景物坐标系xoy中的位置，得到目标点在机器人的工件坐标系中的位置坐标，实现目标点的定位并为机器人制孔提供导航信息。

本发明的优点在于：

本发明忽略了手眼关系中摄像机的深度信息，将标定过程转化为几何关系，标定过程简便，计算量小，不需要昂贵的三坐标测量设备且精度较高，具有较高的实用价值，能够满足机器人制孔系统的实际工作需求。

附图说明

图1是本发明的一种用于机器人制孔系统的手眼视觉标定方法的流程图；

图2是本发明的坐标系在器人制孔系统中的关系示意图；

图3是本发明的步骤一到步骤七中的坐标示意图；

图4是本发明的步骤八中的坐标示意图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明是一种用于机器人制孔系统的手眼视觉标定方法，流程如图1所示，包括以下几个步骤：

本发明所用的坐标系转换关系，如图2所示，其中W为机器人的世界坐标系(WorldCoordinate)，通常此坐标系与机器人基坐标系重合，本发明中所述的世界坐标系与基坐标系表示的是同一个坐标系。E为机器人末端坐标系，在工业机器人中此坐标系一般称为工具坐标系(Tool Coordinate)，此坐标系的原点为TCP(Tool Center Point)。C为摄像机成像坐标系(Imaging Coordinate)。

景物坐标系xoy定义在摄像机所拍摄的平面，其原点o为摄像机成像坐标系x′o′y′原点在摄像机所拍摄的平面中的对应点即摄像机所能拍摄最大范围的左上角点，其坐标轴x、y与摄像机成像坐标系x′、y′平行。

步骤一：利用“四点法”将机器人末端执行器的刀具中心点标定为工具坐标系原点即TCP。

利用机器人工具坐标系的“四点法”将机器人末端执行器上刀具的中心尖点标定为TCP，如图3所示，其中P点代表末端执行器上的TCP，Px_w与Py_w分别代表平行机器人基坐标系两坐标轴的方向，P点在景物坐标系xoy中的坐标为(x_p，y_p)。

步骤二：将标定模板平面与机器人基坐标系底平面平行放置。

将所用的标定模板放在平台上，调整平台使标定模板所在平面与机器人基坐标系底平面平行。

步骤三：调整机器人末端与摄像机同时垂直于标定模板。

使机器人末端垂直于机器人基坐标系底平面，通过安装或调节摄像机光轴，使得摄像机也垂直于机器人基坐标系底平面，此时机器人末端与摄像机同时垂直于标定模板。

步骤四：记录摄像机拍摄标定模板成清晰图像时，工具坐标系TCP的位置，在图像中选择任意两点p′₁与p′₂，并计算出两点在摄像机成像坐标系x′o′y′中的位置以及与坐标系x′轴所成的夹角γ。

用摄像机拍摄标定模板并调整机器人末端TCP与标定模板之间的距离，直到拍摄所成的图像清晰为止，记录此时由机器人系统得到的TCP的坐标值为(x_p ^w，y_p ^w，z_p ^w)。在图像中选择拍摄到的任意两点p′₁与p′₂，如图3所示，摄像机成像坐标系为x′o′y′，两个点分别为p′₁与p′₂，p′₁与p′₂在摄像机成像坐标系x′o′y′中的位置坐标为(x′₁，y′₁)与(x′₂，y′₂)，标定模板上与p′₁与p′₂对应的两点记为p₁与p₂，坐标为(x₁，y₁)与(x₂，y₂)，根据直角三角形的边角关系可以计算出p′₁与p′₂连线‖p′₁p′₂‖的距离为：

$\left|\right|{p^{\′}}_1{p^{\′}}_2\left|\right|=\sqrt{{({x^{\′}}_2-{x^{\′}}_1)}^2+{({y^{\′}}_2-{y^{\′}}_2)}^2}---\left(1\right)$

‖p′₁p′₂‖为摄像机成像坐标系x′o′y′内p′₁与p′₂两点间连线距离的像素值；

p′₁与p′₂两点连线与x′轴的夹角：

$\mathrm{\γ}={\mathrm{tg}}^{-1}\frac{{y^{\′}}_1-{y^{\′}}_2}{{x^{\′}}_1-{x^{\′}}_2}---\left(2\right)$

步骤五：用TCP点接触标定模板上确定的p₁与p₂，并记录p₁与p₂在机器人基坐标系W中的位置坐标，计算出p₁与p₂连线在机器人基坐标系W中与坐标系x_w轴所成的夹角θ。

用TCP点接触标定模板上在步骤四中选择的p₁与p₂，通过工具坐标系E到机器人基坐标系W的转换可以从工业机器人系统中得到p₁与p₂两点在机器人基坐标系W中的位置坐标分别为(x₁ ^w，y₁ ^w，z₁ ^w)与(x₂ ^w，y₂ ^w，z₂ ^w)，因为p₁与p₂所在标定模板平面与机器人基坐标系底面平行，因此两点在在轴方向的值相同，即 $z_1^w=z_2^w,$ 则与步骤四同理计算出p₁与p₂连线的距离：

$\left|\right|p_1{p}_2\left|\right|=\sqrt{{(x_2^w-x_1^w)}^2+{(y_2^w-y_1^w)}^2}---\left(3\right)$

‖p₁p₂‖为景物坐标系xoy中p₁与p₂两点连线的实际距离；

p₁与p₂连线与基坐标系x_w轴方向的夹角θ：

$\mathrm{\θ}={\mathrm{tg}}^{-1}\frac{y_1^w-y_2^w}{x_1^w-x_2^w}---\left(4\right)$

由于机器人末端垂直于标定模板因此用步骤四记录的拍摄清晰时TCP的位置坐标以及TCP与标定模板上p₁或p₂点接触时TCP的位置坐标在机器人基坐标系Z轴方向的值相减，算出拍摄清晰时TCP距标定模板垂直距离，此距离将作为定位时机器人末端TCP距离标定模板的高度H_w：

$H_w=|z_p-z_1^w|=|z_p-z_2^w|---\left(5\right)$

步骤六：计算机器人末端TCP距离标定模板高度为H_w时摄像机成像坐标x′o′y′系与景物坐标系xoy的比例关系k。

在机器人末端TCP距离标定模板的高度为H_w时，将步骤四及步骤五中确定的‖p′₁p′₂‖、‖p₁p₂‖相比，计算出在此高度时摄像机的放大率，即摄像机成像坐标系x′o′y′与景物坐标系xoy的比例关系。

如图3所示，摄像机成像坐标系为x′o′y′，与之对应的景物坐标系为xoy。步骤四中在图像中选择任意两点为p′₁与p′₂，在摄像机成像坐标系x′o′y′中的位置坐标为(x′₁，y′₁)与(x′₂，y′₂)p₁与p₂在景物坐标系xoy的位置坐标分别记为(x₁，y₁)与(x₂，y₂)。

由于景物坐标系xoy与摄像机成像坐标系x′o′y′平行，因此p₁，p₂连线与景物坐标系x轴夹角也为γ，且摄像机成像坐标系x′o′y′与景物坐标系xoy为比例放大关系，比例系数k为：

$k=\frac{\left|\right|{p^{\′}}_1{p^{\′}}_2\left|\right|}{\left|\right|p_1{p}_2\left|\right|}---\left(6\right)$

则景物坐标系xoy中的(x、y)与摄像机成像坐标系为x′o′y′中(x′、y′)的关系为：

$\left[\begin{array}{c}x\\ y\end{array}\right]=k\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\end{array}\right]---\left(7\right)$

由步骤四得到的p′₁与p′₂坐标，也就得到了p₁与p₂在景物坐标系xoy中的具体坐标值分别为

(x₁，y₁)＝k(x′₁，y′₁)(8)

(x₂，y₂)＝k(x′₂，y′₂)(9)

步骤七：计算摄像机成像坐标系x′o′y′与景物坐标系xoy之间的旋转角度、位置转换关系、TCP在景物坐标系xoy中的位置坐标以及TCP在摄像机成像坐标系x′o′y′中对应的位置关系，得到机器人手眼的相对关系。

由p′₁与p′₂两点连线与x′轴的夹角γ与P₁与P₂连线与基坐标系x_w轴方向的夹角θ得到景物坐标系xoy相对于世界坐标系W的转角φ为：

φ＝θ-γ(10)

根据步骤四得到的拍摄清晰时TCP的位置坐标(x_p，y_p，z_p)以及步骤五中p₁在机器人基坐标系W中的位置坐标(x₁ ^w，y₁ ^w，z₁ ^w)，得出TCP从拍摄位置到与p₁接触时TCP所移动的距离投影在景物坐标系xoy平面内的距离‖pp₁‖为：

$\left|\right|{\mathrm{pp}}_1\left|\right|=\sqrt{{(x_p-x_1^w)}^2+{(y_p-y_1^w)}^2}---\left(11\right)$

pp₁与世界坐标系x_w轴的夹角δ为：

$\mathrm{\δ}={\mathrm{tg}}^{-1}\frac{y_p-y_1^w}{x_p-x_1^w}---\left(12\right)$

由此可得pp₁与景物坐标系x轴的夹角ω为：

ω＝δ-φ(13)

将‖pp₁‖在景物坐标系得两轴方向上分解，记x轴方向分解的距离为D，y轴方向分解的距离为H，则可以得到

D＝‖pp₁‖·cosω(14)

H＝‖pp₁‖·sinω(15)

通过步骤六求出的p₁在景物坐标系中的坐标值(x₁，y₁)＝k(x′₁，y′₁)算出TCP在景物坐标系xoy中距离原点的x轴方向的偏移量d以及y轴方向的偏移量h，即图3中P点在景物坐标系xoy中的坐标：

$\left[\begin{array}{c}{x}_p\\ y_p\end{array}\right]=\left[\begin{array}{c}d\\ h\end{array}\right]=\left[\begin{array}{c}{x}_1-D\\ H+y_1\end{array}\right]=\left[\begin{array}{c}{{\mathrm{kx}}^{\′}}_1-\left|\right|{\mathrm{pp}}_1\left|\right|\·\cos (\mathrm{\δ}+\mathrm{\γ}-\mathrm{\θ})\\ \left|\right|{\mathrm{pp}}_1\left|\right|\·\sin (\mathrm{\δ}+\mathrm{\γ}-\mathrm{\θ})+{{\mathrm{ky}}^{\′}}_1\end{array}\right]---\left(16\right)$

由于景物坐标系xoy与摄像机成像坐标系x′o′y′的比例关系k已知，则通过式(16)建立了TCP与摄像机成像坐标系x′o′y′的关系，完成标定，得到的机器人手眼的相对关系。

步骤八：确定目标点在景物坐标系xoy中的位置，得到目标点在机器人的工件坐标系中的位置坐标，实现目标点的定位并为机器人制孔提供导航信息。

以与标定时相同的机器人末端姿态及拍摄距离拍摄目标点，利用步骤七得到的机器人手眼关系进行目标点的定位，通过计算目标点在摄像机成像坐标系x′o′y′中的位置以及利用摄像机成像坐标系x′o′y′与景物坐标系xoy之间的关系确定目标点在景物坐标系xoy中的位置，然后通过TCP与景物坐标系xoy之间的位置关系，得到目标点相对于TCP之间的位置关系，从而得到了目标点在机器人建立的工件坐标系中的位置坐标实现了目标点的定位并为机器人制孔提供了导航信息。

机器人末端姿态与标定时相同以及TCP距离工件的高度H_w保持不变，则比例系数k以及景物坐标系xoy相对于工件坐标系的转角φ不变，TCP在景物坐标系xoy中的位置坐标也不变，如图4所示，图中x′_w与y′_w为与末端执行器垂直的工件坐标系两轴方向平行，设A点为机器人需要制孔的点，坐标为工件坐标系中的坐标(x_A，y_A)，当摄像机拍摄到A点时，得到图像中A′点在摄像机成像坐标系x′o′y′中的坐标为(x′_A，y′_A)，则A点在景物坐标系xoy中的坐标为(x_A，y_A)＝k(x′_A，y′_A)。

P点为末端执行器TCP在景物坐标系xoy中的位置，其坐标值(x_p，y_p)已经由步骤七获得，其具体值由图4中TCP在两轴方向上的偏移量d′和h′来表示。

A点到TCP的距离分别在景物坐标系xoy中x轴与y轴方向上分解的距离分别为D′和H′，如图4所示：

D′＝x_A-d′(17)

H′＝h′-y_A(18)

根据几何关系得出A点到TCP的距离‖PA‖：

$\left|\right|\mathrm{PA}\left|\right|=\sqrt{H^{\′2}+D^{\′2}}---\left(19\right)$

A点到TCP连线与景物坐标系xoy中x轴的夹角ω′为：

${\mathrm{\ω}}^{\′}={\mathrm{tg}}^{-1}\frac{H^{\′}}{D^{\′}}---\left(20\right)$

得到A点到TCP连线与工件坐标系平行的x′_w轴的夹角δ′为：

δ′＝ω′+φ(21)

将‖PA‖在x′_w轴与y′_w轴方向上分解得到TCP移动到A点在工件坐标系下两轴方向上的偏移量分别为和

$D_{x_w}=\left|\right|\mathrm{PA}\left|\right|\·\cos {\mathrm{\δ}}^{\′}---\left(22\right)$

$H_{y_w}=\left|\right|\mathrm{PA}\left|\right|\·\sin {\mathrm{\δ}}^{\′}---\left(23\right)$

则A点在工件坐标系中的位置坐标(x_A，y_A)为：

(x_A，y_A)＝(x_p+‖PA‖·cosδ′，y_p-‖PA‖·sinδ′)(24)

求出的A点在工件坐标系中的坐标，实现为机器人末端执行器制孔提供导航信息。

实施例：

应用本发明所述的方法，首先用末端执行器进行制孔得到孔的实际位置坐标，然后用摄像机拍摄，进行定位得到孔的定位坐标并算出定位误差，数据如表1所示：

表1定位误差数据表

组号	实际坐标	定位坐标	Δx/mm	Δy/mm	定位误差E/mm
						1	(1168.24，-5.19)	(1168.25，-5.16)	0.01	0.03	±0.03
2	(1176.87，-81.80)	(1176.99，-81.77)	0.12	0.03	±0.12
						3	(1132.69，-35.58)	(1132.87，-35.44)	0.18	0.14	±0.23
4	(1086.87，-8.79)	(1087.01，-8.68)	0.14	0.11	±0.18
						5	(1135.85，-66.08)	(1135.98，-66.00)	0.13	0.08	±0.15

6	(1117.18，-40.99)	(1117.04，-40.90)	-0.14	0.09	±0.17
						7	(1095.93，-36.56)	(1095.81，-36.38)	-0.12	0.18	±0.22
8	(1126.31，-28.24)	(1126.41，-28.11)	0.10	0.13	±0.16

表中Δx，Δy为定位坐标与实际坐标在两轴方向上的偏差，定位误差根据下式计算：

$E=\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2}---\left(25\right)$

从表1中的结果可以看出本发明所述方法的定位误差在±0.3mm以内，可以满足机器人制孔系统的定位精度要求。

Claims (1)

1.一种用于机器人制孔系统的手眼视觉标定方法，其特征在于，包括以下几个步骤：

W为机器人的世界坐标系(World Coordinate)，世界坐标系与基坐标系表示的是同一个坐标系；E为机器人末端坐标系，也为工具坐标系(Tool Coordinate)，此坐标系的原点为TCP(Tool Center Point)；C为摄像机成像坐标系(Imaging Coordinate)；

景物坐标系xoy定义在摄像机所拍摄的平面，其原点o为摄像机成像坐标系x′o′y′原点在摄像机所拍摄的平面中的对应点即摄像机所能拍摄最大范围的左上角点，其坐标轴x、y与摄像机成像坐标系x′、y′平行；

步骤一：利用“四点法”将机器人末端执行器的刀具的中心尖点标定为工具坐标系原点即TCP；

利用机器人工具坐标系的“四点法”将机器人末端执行器上刀具的中心尖点标定为TCP，计为P，在景物坐标系xoy中位置坐标为(x_p，y_p)；

步骤二：将标定模板平面与机器人基坐标系底平面平行放置；

将所用的标定模板放在平台上，调整平台使标定模板所在平面与机器人基坐标系底平面平行；

步骤三：调整机器人末端与摄像机同时垂直于标定模板；

使机器人末端垂直于机器人基坐标系底平面，通过安装或调节摄像机光轴，使得摄像机也垂直于机器人基坐标系底平面，此时机器人末端与摄像机同时垂直于标定模板；

步骤四：记录摄像机拍摄标定模板成清晰图像时工具坐标系TCP的位置，在图像中选择任意两点p′₁与p′₂，并计算出两点在摄像机成像坐标系x′o′y′中的位置以及与坐标系x′轴所成的夹角γ；

用摄像机拍摄标定模板并调整机器人末端TCP与标定模板之间的距离，直到拍摄所成的图像清晰为止，记录此时由机器人系统得到的TCP的坐标值为

在图像中选择拍摄到的任意两点p′₁与p′₂，p′₁与p′₂在摄像机成像坐标系x′o′y′中的位置坐标为(x′₁，y′₁)与(x′₂，y′₂)，标定模板上与p′₁与p′₂对应的两点记为p₁与p₂，坐标为(x₁，y₁)与(x₂，y₂)，根据直角三角形的边角关系计算出p′₁与p′₂连线||p′₁p′₂||的距离为：

$\left|\right|{p^{\′}}_1{p^{\′}}_2\left|\right|=\sqrt{{({x^{\′}}_2-{x^{\′}}_1)}^2+{({y^{\′}}_2-{y^{\′}}_1)}^2}---\left(1\right)$

||p′₁p′₂||为摄像机成像坐标系x′o′y′内p′₁与p′₂两点间连线距离的像素值；

p′₁与p′₂两点连线与x′轴的夹角：

$\mathrm{\γ}={\mathrm{tg}}^{-1}\frac{{y^{\′}}_1-{y^{\′}}_2}{{x^{\′}}_1-{x^{\′}}_2}---\left(2\right)$

步骤五：用TCP点接触标定模板上确定的p₁与p₂，并记录p₁与p₂在机器人基坐标系W中的位置坐标，计算出p₁与p₂连线在机器人基坐标系W中与坐标系x_w轴所成的夹角θ；用TCP点接触标定模板上的p₁与p₂，通过工具坐标系E到机器人基坐标系W的转换从工业机器人系统中得到p₁与p₂两点在机器人基坐标系W中的位置坐标分别为

与

因为p₁与p₂所在标定模板平面与机器人基坐标系底面平行，因此两点在在轴方向的值相同

则与步骤四同理计算出p₁与p₂连线的距离：

$\left|\right|p_1{p}_2\left|\right|=\sqrt{{(x_2^w-x_1^w)}^2+{(y_2^w-y_1^w)}^2}---\left(3\right)$

||p₁p₂||为景物坐标系xoy中p₁与p₂两点连线的实际距离；

p₁与p₂连线与基坐标系x_w轴方向的夹角θ：

$\mathrm{\θ}={\mathrm{tg}}^{-1}\frac{y_1^w-y_2^w}{x_1^w-x_2^w}---\left(4\right)$

由于机器人末端垂直于标定模板，因此用步骤四记录的拍摄清晰时TCP的位置坐标以及TCP与标定模板上p₁或p₂点接触时TCP的位置坐标在机器人基坐标系Z轴方向的值相减，则算出拍摄清晰时TCP距标定模板垂直距离，此距离将作为定位时机器人末端TCP距离标定模板的高度H_w：

$H_w=|z_p-z_1^w|=|z_p-z_2^w|---\left(5\right)$

步骤六：计算机器人末端TCP距离标定模板高度为H_w时摄像机成像坐标系x′o′y′与景物坐标系xoy的比例关系k；

在机器人末端TCP距离标定模板的高度为H_w时，将步骤四及步骤五中确定的||p′₁p′₂||、||p₁p₂||相比，计算高度为H_w时摄像机的放大率，摄像机成像坐标系x′o′y′与景物坐标系xoy的比例关系k；

$k=\frac{\left|\right|{p^{\′}}_1{p^{\′}}_2\left|\right|}{\left|\right|p_1{p}_2\left|\right|}---\left(6\right)$

则景物坐标系xoy中的(x、y)与摄像机成像坐标系为x′o′y′中(x′、y′)的关系为：

$\left[\begin{array}{c}x\\ y\end{array}\right]=k\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\end{array}\right]---\left(7\right)$

由步骤四得到的p′₁与p′₂坐标，得到p₁与p₂在景物坐标系xoy中的具体坐标值分别为

(x₁，y₁)＝k(x′₁，y′₁)    (8)

(x₂，y₂)＝k(x′₂，y′₂)    (9)

步骤七：计算摄像机成像坐标系x′o′y′与景物坐标系xoy之间的旋转角度、位置转换关系、TCP在景物坐标系xoy中的位置坐标以及TCP在摄像机成像坐标系x′o′y′中对应的位置关系，得到机器人手眼的相对关系；

由p′₁与p′₂两点连线与x′轴的夹角γ与P₁与P₂连线与基坐标系x_w轴方向的夹角θ得到景物坐标系xoy相对于世界坐标系W的转角φ为：

φ＝θ-γ            (10)

根据步骤四得到的拍摄清晰时TCP的位置坐标(x_p，y_p，z_p)以及步骤五中p₁在机器人基坐标系W中的位置坐标

得出TCP从拍摄位置到与p₁接触时TCP所移动的距离投影在景物坐标系xoy平面内的距离||pp₁||为：

$\left|\right|{\mathrm{pp}}_1\left|\right|=\sqrt{{(x_p-x_1^w)}^2+{(y_p-y_1^w)}^2}---\left(11\right)$

pp₁与世界坐标系x_w轴的夹角δ为：

$\mathrm{\δ}={\mathrm{tg}}^{-1}\frac{y_p-y_1^w}{x_p-x_1^w}---\left(12\right)$

由此得到pp₁与景物坐标系x轴的夹角ω为：

ω＝δ-φ        (13)

将||pp₁||在景物坐标系得两轴方向上分解，记x轴方向分解的距离为D，y轴方向分解的距离为H，则得到

D＝||pp₁||·cosω(14)

H＝||pp₁||·sinω(15)

通过步骤六求出的p₁在景物坐标系中的坐标值(x₁，y₁)＝k(x′₁，y′₁)算出TCP在景物坐标系xoy中距离原点的x轴方向的偏移量d以及y轴方向的偏移量h，P点在景物坐标系xoy中的坐标：

$\left[\begin{array}{c}{x}_p\\ y_p\end{array}\right]=\left[\begin{array}{c}d\\ h\end{array}\right]=\left[\begin{array}{c}{x}_1-D\\ H+y_1\end{array}\right]=\left[\begin{array}{c}{{\mathrm{kx}}^{\′}}_1-\left|\right|{\mathrm{pp}}_1\left|\right|\·\cos (\mathrm{\δ}+\mathrm{\γ}-\mathrm{\θ})\\ \left|\right|{\mathrm{pp}}_1\left|\right|\·\sin (\mathrm{\δ}+\mathrm{\γ}-\mathrm{\θ})+{{\mathrm{ky}}^{\′}}_1\end{array}\right]---\left(16\right)$

比例关系k为已知，则通过式(16)建立了TCP与摄像机成像坐标系x′o′y′的关系，完成标定，得到机器人手眼的相对关系；

步骤八：确定目标点在景物坐标系xoy中的位置，得到目标点在机器人的工件坐标系中的位置坐标，实现目标点的定位并为机器人制孔提供导航信息；

机器人末端姿态与标定时相同以及TCP距离工件的高度H_w保持不变，则比例系数k以及景物坐标系xoy相对于工件坐标系的转角φ不变，TCP在景物坐标系xoy中的位置坐标也不变，x′_w与y′_w为与末端执行器垂直的工件坐标系两轴方向平行，当摄像机拍摄到A点时，得到图像中A点在摄像机成像坐标系x′o′y′中的坐标为(x′_A，y′_A)，则A点在景物坐标系xoy中的坐标为(x_A，y_A)＝k(x′_A，y′_A)；

P点为末端执行器TCP在景物坐标系xoy中的位置，其坐标值(x_p，y_p)已经由步骤七获得，其具体值由TCP在x轴、y轴方向上的偏移量d′和h′来表示；

设A点为机器人需要制孔的点，坐标为工件坐标系中的坐标(x_A，y_A)，A点到TCP的距离分别在景物坐标系xoy中x轴与y轴方向上分解的距离分别为D′和H′：

D′＝x_A-d′    (17)

H′＝h′-y_A    (18)

根据几何关系得出A点到TCP的距离||PA||：

$\left|\right|\mathrm{PA}\left|\right|=\sqrt{H^{\′2}+D^{\′2}}---\left(19\right)$

A点到TCP连线与景物坐标系xoy中x轴的夹角ω′为：

${\mathrm{\ω}}^{\′}={\mathrm{tg}}^{-1}\frac{H^{\′}}{D^{\′}}---\left(20\right)$

得到A点到TCP连线与工件坐标系平行的x′_w轴的夹角δ′为：

δ′＝ω′+φ                    (21)

将||PA||在x′_w轴与y′_w轴方向上分解得到TCP移动到A点在工件坐标系下两轴方向上的偏移量分别为

和

$D_{x_w}=\left|\right|\mathrm{PA}\left|\right|\·\cos {\mathrm{\δ}}^{\′}---\left(22\right)$

$H_{y_w}=\left|\right|\mathrm{PA}\left|\right|\·\sin {\mathrm{\δ}}^{\′}---\left(23\right)$

则A点在工件坐标系中的位置坐标(x_A，y_A)为：

(x_A，y_A)＝(x_p+||PA||·cosδ′，y_p-||PA||·sinδ′)  (24)

求出的A点在工件坐标系中的坐标，实现为机器人末端执行器制孔提供导航信息。

Images