2020年09月10日 | 一文详解机器人的视觉伺服

常见的机器人视觉伺服中要实现像素坐标与实际坐标的转换，首先就要进行标定，对于实现视觉伺服控制，这里的标定不仅包括摄像机标定，也包括机器人系统的手眼标定。以常见的机器人系统为例，有两种构型，如下：

即：摄像机固定于机器手和摄像机固定于外部场景；

本文针对前一种构型：摄像机固定于机器手。

1、摄像机标定技术

（1）理论部分：

以张正友的棋盘标定法为摄像机标定方式，由于摄像机标定结果要用到后面的手眼标定中，所以此处进行不同方位的棋盘图片拍摄时需要遵守：标定板固定位置不动，手眼组合体变换姿态拍摄图片。

摄像机标定的目的：得到两组坐标系的两两转化矩阵：T1和T2；

1）得到图片像素坐标系P与摄像机坐标系C之间的转换矩阵T1，准确说应该是摄像机坐标系转化为图片像素坐标系的转换矩阵。可表示为：

P=T1*C;

解释：T1在摄像机标定结果中就是内参矩阵3x3；

2）得到摄像相机坐标系C与棋盘上建立的世界坐标系G之间的转换矩阵T2，准确说应该是坐标系G转化为摄像机坐标系的转换矩阵。可表示为：

C=T2*G;

解释：T2在摄像机标定结果中就是外参矩阵4x4，由旋转矩阵r和平移向量t构成［ t r; 0 0 0 1］；

（2）方法：

摄像机标定方法有两种可选：openCV或者标定箱；

建议选择MATLAB应用程序——图像处理与视觉——Camera Calibrator，直接导入拍摄好的图片即可。但是要注意，使用matlab标定工具箱所得到的内参矩阵、外参旋转矩阵、外参平移向量都要经过转置才是正确的结果。

如下图，MATLAB标定得到的红框中依次是外参平移向量、内参矩阵、外参旋转矩阵，它们都需要做转置后才能应用于本文的公式计算：

2、手眼标定技术

（1）理论部分：

手眼标定目的：得到摄像机坐标系C与机器手（或工具）坐标系H之间的转换矩阵T3，准确说应该是机器手坐标系转化为摄像机坐标系的转化矩阵。可表示为：

C=T3*H;

解释：T3需要根据公式CX=XD得到；实际中，分别知道C、D求出来的X有无穷多个解。所以为了实现唯一解，我们至少需要两组C和D，即至少需要3个位置的摄像机标定结果。

其中C的求法如下：

C是两个摄像机坐标系之间的变换矩阵。可以根据上述任一两张标定图片所得的两个摄像机标定外参A、B按公式C=A*inv（B）计算得到的。假设上述摄像机标定中有3张标定图片的外参标定结果分别是T21、T22、T23，那么可以得到两个C矩阵：

C1=T21*inv（T22）;

C2=T22*inv（T23）;

D的求法如下：

D是两个机器手坐标系之间的变换矩阵。假设上述摄像机标定中的3张标定图片所一一对应的机器手坐标系在基坐标系（也可以是工件坐标系或者其他固定的参考坐标系）中的描述矩阵结果分别是H1、H2、H3（H需要从机器人控制器或示教器中读取），那么可以得到两个D矩阵：

D1=inv（H1）*H2;

D2=inv（H2）*H3;

由以上两组C和D，代入CX=XD就可以得到唯一解X，从而T3=X；

注：上述H1、H2、H3是每张标定图片对应的机器手坐标系描述矩阵，正好说明了摄像机标定中所谓的“标定板固定，手眼运动”的正确性。如果手眼不动，改变标定板姿态进行拍摄，那么H的值都是一样的。

（2）方法：

1）根据摄像机标定已知摄像机外参矩阵T21、T22、T23，还要从机器人控制器中读取T21、T22、T23分别对应的机器手（或工具）坐标系H1、H2、H3。控制器中的坐标系描述矩阵不是直接读取的，它是以平移向量和欧拉角（或四元数）模式存在的，如下：

平移向量+欧拉角模式：

平移向量+四元数模式：

选取其中任一模式即可，然后将其转化为描述矩阵。

上述工作完成后，就已经获取了3个外参矩阵（再次提醒，摄像机标定使用MATLAB标定工具箱的话，所得到的外参旋转矩阵和平移向量先要转置，即R=r‘，T=t’，然后外参矩阵EX=［R T；0 0 0 1］）和 3个机械手坐标系矩阵，因此可以分别将3个二维矩阵合为一个三维矩阵，matlab命令如下：

C_ext=cat（3， C_ext1， C_ext2， C_ext3）;

H=cat（3， H1， H2 ，H3）

最后将C_ext和H作为参数代入到如下MATLAB函数中：

funcTIon Tch = GetCamera2Hantrix（C_ext，H）

% 以下变量：

% C_ext是3个位置的摄像机外参矩阵：3x4x4

% H1、H2、H3分别是3个位置的机械手坐标系的姿态矩阵：3x4x4

% Tcg--机器手坐标系（或工具坐标系）在摄像机坐标系中的姿态和位置变换矩阵

% C1、D1、C2、D2、R、w、q、kc1、kc2、kc3、kd1、kd2、kd3、a、b、c、d、h、y均为临时变量

C1=C_ext（：，：，1）*inv（C_ext（：，：，2））

C2=C_ext（：，：，2）*inv（C_ext（：，：，3））

D1=inv（H（：，：，1））*H（：，：，2）

D2=inv（H（：，：，2））*H（：，：，3）

R=C1（1:3，1:3）;

q=acos（（trace（R）-1）/2）;

w（1，1）=q/（2*sin（q））*（R（3，2）-R（2，3））;

w（2，1）=q/（2*sin（q））*（R（1，3）-R（3，1））;

w（3，1）=q/（2*sin（q））*（R（2，1）-R（1，2））;

kc1=w;

R=C2（1:3，1:3）;

q=acos（（trace（R）-1）/2）;

w（1，1）=q/（2*sin（q））*（R（3，2）-R（2，3））;

w（2，1）=q/（2*sin（q））*（R（1，3）-R（3，1））;

w（3，1）=q/（2*sin（q））*（R（2，1）-R（1，2））;

kc2=w;

R=D1（1:3，1:3）;

q=acos（（trace（R）-1）/2）;

w（1，1）=q/（2*sin（q））*（R（3，2）-R（2，3））;

w（2，1）=q/（2*sin（q））*（R（1，3）-R（3，1））;

w（3，1）=q/（2*sin（q））*（R（2，1）-R（1，2））;

kd1=w;

R=D2（1:3，1:3）;

q=acos（（trace（R）-1）/2）;

w（1，1）=q/（2*sin（q））*（R（3，2）-R（2，3））;

w（2，1）=q/（2*sin（q））*（R（1，3）-R（3，1））;

w（3，1）=q/（2*sin（q））*（R（2，1）-R（1，2））;

kd2=w;

kc3=cross（kc1，kc2）;

kd3=cross（kd1，kd2）;

a=［kc1 kc2 kc3］;

b=［kd1 kd2 kd3］;

R=a*inv（b）; %得到旋转关系矩阵

tc1=C1（1:3，4）;

tc2=C2（1:3，4）;

td1=D1（1:3，4）;

td2=D2（1:3，4）;

c=R*td1-tc1;

d=R*td2-tc2;

a=C1（1:3，1:3）-［1 0 0;0 1 0;0 0 1］;

b=C2（1:3，1:3）-［1 0 0;0 1 0;0 0 1］;

h=［a;b］;

y=［c;d］;

t=inv（h‘*h）*h’*y; %得到平移关系矩阵

Tch=［R t;0 0 0 1］; %得到最终结果

end

3、根据标定结果对固定高度目标实现单目定位

（1）理论部分：

由上述1、2两个标定已经得到：

摄像机坐标系C-》像素坐标系P的转换矩阵Tpc（即内参矩阵，MATLAB标定得到的要转置）；

机械手（或工具）坐标系H-》摄像机坐标系C的转化矩阵Tch；

从控制器读取的机械手（或工具）坐标系H-》基坐标系B（这个根据情况自己在控制器设定是基坐标还是工件坐标系，本文用基坐标系）的转化矩阵Tbh；

已知目标高度固定，为z；

那么基坐标系转化为像素坐标系的变换矩阵就是：Gpb=Tpc*Tch*inv（Tbh）;

根据Gpb和z可以得到如下图所示的变换过程，分解后可根据像素坐标（u，v）求得实际坐标（x，y，z）：

其中，Tpc需要注意，应在内参矩阵最后添加一个全零列，变为3x4矩阵，如下：

（2）代码实现：

funcon P= GetObjectLocaTIon（ u，v，Gtb）

% 参数（u，v）为目标在图片中的像素坐标

% 参数Gtb是工具在机器人基坐标中的描述矩阵（也就是工具坐标系-》基坐标系的变换矩阵）

%内参矩阵

Kl=［ 1851 9.7 550.5 0;

0 1844.4 299.7 0;

0 0 1.0 0］;

%摄像机与工具关系矩阵

Gctl= ［-0.9620 -0.2974 0.0156 -2.6405;

0.3266 -0.9552 0.0056 59.7141;

0.0130 0.0003 1.0161 145.3381;

0 0 0 1.0000］;

G=inv（Gtb）;

z=10; %指定物体的高度

M=Kl*Gctl*G;

Ml=［u*M（3，1）-M（1，1） u*M（3，2）-M（1，2） ; v*M（3，1）-M（2，1） v*M（3，2）-M（2，2）］;

Mr=［M（1，4）-u*M（3，4）-（u*M（3，3）-M（1，3））*z; M（2，4）-v*M（3，4）-（v*M（3，3）-M（2，3））*z］;

P=inv（Ml）*Mr; %得到物体的位置