工业相机与SCARA机械臂的坐标系标定
工业相机与SCARA机械臂的坐标系标定
在工业自动化领域，工业相机与机械臂的协同工作是实现智能化生产的关键。本文将详细介绍如何进行工业相机与SCARA机械臂的坐标系标定，包括标定原理、程序实现以及实际应用案例。
标定原理
如图1所示，OXY为机器人坐标系，O’X’Y’是相机坐标系。
当相机获取目标物体在相机坐标系下的坐标(imageP3.x,imageP3.y)时，如何进行计算得到对应在机器人坐标系下的坐标(robotP3.x,robotP3.y)呢？
首先，需要通过两个已知坐标点进行标定。P1点在机器人坐标系下和相机坐标系下的坐标为(imageP1.x,imageP1.y)，(robotP1.x,robotP1.y)；P2点在机器人坐标系下和相机坐标系下的坐标为(imageP2.x,imageP2.y)，(robotP2.x,robotP2.y)。
求得机器人坐标系下的向量robotP1P2以及相机坐标系下的向量imageP1P2:
imageP1P2.x = imageP2.x - imageP1.x
imageP1P2.y = imageP2.y - imageP1.y
robotP1P2.x = robotP2.x - robotP1.x
robotP1P2.y = robotP2.y - robotP1.y
然后，根据这两个坐标点求得现实距离与图像像素之间的比例系数factor以及两个坐标系之间的偏转角thetaRI。
factor = sqrt(robotP1P2.xrobotP1P2.x + robotP1P2.yrobotP1P2.y)/ sqrt(imageP1P2.ximageP1P2.x + imageP1P2.yimageP1P2.x)
偏转角thetaRI的求取：如图2所示。
先求得机器人坐标系下向量robotP1P2相对于x轴的偏转角thetaR，然后求得相机坐标系下的向量imageP1P2相对于x轴的偏转角thetaI；最后两者相减就是两个坐标系的偏转角thetaR。
thetaR = atan2((robotP2.y - robotP1.y) ,(robotP2.x - robotP1.x))
thetaI = atan2((imageP2.y - imageP1.y) , (imageP2.x - imageP1.x))
thetaRI = thetaR - thetaI
最后，根据比例系数factor以及偏转角thetaRI就可以对相机坐标系内的任意一个坐标进行机器人坐标系坐标转换。原理图如图3所示。
相机坐标系下的坐标(imageP3.x,imageP3.y)，那么相机坐标系下的向量imageP1P3为
imageP1P3.x = imageP3.x - imageP1.x
imageP1P3.y = imageP3.y - imageP1.y
还需要求得相机坐标系下的相对于x轴偏转角thetaN = atan2(imageP1P3.y, imageP1P3.x)。这样通过图3中的原理就可以得出机器人坐标系下的坐标了。
robotP3.x = factorsqrt(imageP1P3.ximageP1P3.x + imageP1P3.yimageP1P3.y) cos(thetaN + thetaRI) + robotP1.x
robotP3.y = factorsqrt(imageP1P3.ximageP1P3.x + imageP1P3.yimageP1P3.y) sin(thetaN + thetaRI) + robotP1.y
得到最终的robotP3坐标就可以通信给机器人让机器人移动到该座标下，也就是目标物体的位置处。
标定程序实现
完整的C++程序实现如下所示：
/*
机器人坐标系与相机坐标系转换程序
第一步：记录A点的机器人坐标系下坐标以及图像坐标系下坐标，记录B点的机器人坐标系下坐标以及图像坐标系下坐标；
第二步：计算得到现实距离与图像像素之间的比例系数，以及两坐标系的偏转角；
前两步完成后，即完成了标定过程，接下来就可以根据一个新的点的图像坐标系下的坐标，求得该点的机器人坐标点；
第三步：输入一个图像坐标系下的点，程序自动得到机器人坐标下的点。
*/
#include<iostream>
#include<cmath>
using namespace std;
struct position {
double x;
double y;
};
struct vector {
double x;
double y;
};
int main()
{
//1 input the positions in the camera coodinate and robot coodinate
position imageP1, imageP2, robotP1, robotP2;
cout << "请输入相机坐标系下的坐标" << endl;
cin >> imageP1.x >> imageP1.y >> imageP2.x >> imageP2.y;
cout << "请输入机器人坐标系下的坐标" << endl;
cin >> robotP1.x >> robotP1.y >> robotP2.x >> robotP2.y;
//2 caculate the factor and tangle
vector imageP1P2, robotP1P2;
imageP1P2.x = imageP2.x - imageP1.x;
imageP1P2.y = imageP2.y - imageP1.y;
robotP1P2.x = robotP2.x - robotP1.x;
robotP1P2.y = robotP2.y - robotP1.y;
double factor, thetaI, thetaR, thetaRI;
factor = sqrt(robotP1P2.x*robotP1P2.x + robotP1P2.y*robotP1P2.y)/ sqrt(imageP1P2.x*imageP1P2.x + imageP1P2.y*imageP1P2.x);
thetaR = atan2((robotP2.y - robotP1.y) ,(robotP2.x - robotP1.x));
thetaI = atan2((imageP2.y - imageP1.y) , (imageP2.x - imageP1.x));
thetaRI = thetaR - thetaI;
cout <<"两坐标系的弧度偏离角为"<< thetaRI << endl;
//3 input a new postion in the camera coodinate, computer will caculate the postion information in the robot coodinate
position imageP3,robotP3;
cout << "请输入一个新的图像坐标系点" << endl;
cin >> imageP3.x >> imageP3.y;
vector imageP1P3;
imageP1P3.x = imageP3.x - imageP1.x;
imageP1P3.y = imageP3.y - imageP1.y;
double thetaN;
thetaN = atan2(imageP1P3.y, imageP1P3.x);
robotP3.x = factor*sqrt(imageP1P3.x*imageP1P3.x + imageP1P3.y*imageP1P3.y)* cos(thetaN + thetaRI) + robotP1.x;
robotP3.y = factor*sqrt(imageP1P3.x*imageP1P3.x + imageP1P3.y*imageP1P3.y)* sin(thetaN + thetaRI) + robotP1.y;
cout << "机器人坐标系下的该点坐标为：" << endl;
cout << robotP3.x << "," << robotP3.y << endl;
system("pause");
return 0;
}
MATLAB计算程序如下：
clear; clc; close all;
Ar = [411.428 16.614]; % point A in robot coordinate
Br = [403.211 -109.381]; % point B in robot coordinate
Ai = [947 90]; % point A in image coordinate
Bi = [525 70]; % point B in image coordinate
AiBi = Bi - Ai
ArBr = Br - Ar
% calculate mm and pixel coefficient
MM_PER_PIXEL = norm(ArBr) / norm(AiBi)
% swap image coordinate xi and yi
theta_i = atan2d( AiBi(1), AiBi(2) )
% theta_i = atan2d( AiBi(2), AiBi(1) )
theta_r = atan2d( ArBr(2), ArBr(1) )
THETA_IR = theta_r - theta_i
%%
Ni = [627 333]; % new point in image coordinate
AiNi = Ni - Ai;
theta_ni = atan2d(AiNi(1),AiNi(2));
Nrx = MM_PER_PIXEL*norm(AiNi)*cosd(theta_ni+THETA_IR) + Ar(1);
Nry = MM_PER_PIXEL*norm(AiNi)*sind(theta_ni+THETA_IR) + Ar(2);
Nr = [Nrx Nry]
% robot = [428.8647 59.8150]
% z = 227.8875
工业智能相机与SCARA机械手坐标系的标定实例
硬件设备为固高智能相机以及固高系统的SCARA机械手，该实例演示整个标定过程并验证标定效果。
以一枚硬币作为目标物体，分别从放置P1点与P2点，分别得到两个坐标点在相机坐标系以及机器人坐标系下的位置，通过路由器将三者连接起来，并通过ping来测试三者是否通信顺畅。如图4所示。
图4 机器人，相机以及PC机连接示意图
物体在相机坐标系的坐标信息从固高智能相机软件中找到，如图5a所示；
图5 相机坐标系下的坐标获取
物体在机器人坐标系的坐标信息需要移动机器人至物体上方，然后从示教器界面中查看位置信息，如图6所示；
图6 机器人坐标系下的坐标获取
通过两个点的不同坐标系的坐标信息输入，计算得到了比例系数和偏转角，最后放置目标物体在一个新的位置，输入物体在相机坐标系的坐标，获得物体在机器人坐标系下的坐标，如图7所示。注意：相机坐标系输入坐标的时候，需要将x与y坐标对调，因为机器人坐标系的z轴是向上的，根据右手定则相机坐标系的z轴向下，所以相机坐标系中软件显示的x坐标实为y坐标，y坐标实为x坐标。
图7 坐标求取结果示意图
根据图7所示，机器人坐标系下该点坐标应该为441.261,28.5031；然后，可以通过示教器直接进行点位移动来验证。当然，更好的方式是通过示教编程，因为只有弄懂了示教编程，才能进行下一步的相机进行模式识别，然后将坐标传送给计算机，计算机通过软件进行计算并把机器人坐标系下的坐标信息发送回机器人，机器人进行自动化跟踪抓取。
示教程序如下：
NOP
MOVJ P87 V=20% BL=0 VBL=0
SET STR=7
SOCKCLOSE str2
SOCKOPEN str2 type=SERVER
TIMER T=10000ms
SOCKRECV str2 str7 I1
STRSPLIT DELIM=, str7 str8 N=1
STRSPLIT DELIM=, str7 str9 N=2
STR2REAL str8 R8
STR2REAL str9 R9
SETE P88.1 R8
SETE P88.2 R9
MOVJ P88 V=20% BL=0 VBL=0
NOP
程序解释如下：首先将机器人移动至P87点（可随意选取，主要是为了观察机器人是否正常和接下来移动方便），使用str7来存储441.261,28.5031；接下来就要建立通信连接，str2存储的字符为pc，pc在通信设置中设置的是电脑端的IP地址等信息；如图8所示。
图8 数值型变量字符型界面示意图
在机器人等待接受数据的10秒间隔期间，将机器人坐标系下的位置信息通过TCP调试助手发送给机器人，保存在str7中，然后将str7中的字符进行分割，分割的标志为“,”，“,”前面的分割存入str8，“,”后面的分割存入str9，然后将其转换为实型变量，并将其复制给P88点，然后让机器人移动至P88点——即(441.261,28.5031)。具体的机器人编程请看《工业机器人控制系统用户手册.pdf》。
程序编写完成后，打开PC机的网络调试助手，并输入机器人端的IP地址以及端口号，以及待发送的内容，如图9所示。
图9 网络调试助手通信示意图
观察发现机器人移动至物体上方，如图10所示，验证完成。
图10 机器人移动结果示意图
观察发现机器人与目标物体的位置间有小的偏差，原因一是在于机器人未进行精准的零位标定（主要原因）；二是因为对物体的坐标选取都为目测得到。