原文：/qq_36552550/article/details/79409242
注：感谢固高长江研究院徐工程师的技术讲解，以及matlab程序，机器人系统程序的提供。
在工业现场当中，相机拍摄到的图像有一个相机坐标系，而机器人自身也有一个机器人自身的坐标系，两者互相独立；当我们通过相机进行对物体进行拍摄，通过模式识别得到了目标物体在相机坐标系下的坐标位置，那么如何通过计算来让机器人知道目标物体在机器人坐标系的位置，以便机器人进行抓取操作呢？
这一章，首先进行标定原理的讲解，然后通过程序进行实现；最后，以洛阳理工学院实验室的固高工业智能相机及SCARA机械手为硬件基础，进行实验验证。
10.1标定原理
如图10-1所示，OXY为机器人坐标系，O’X’Y’是相机坐标系。
图10-1机器人坐标系与相机坐标系示意图
当相机获取目标物体在相机坐标系下的坐标(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.x*robotP1P2.x + robotP1P2.y*robotP1P2.y)/ sqrt(imageP1P2.x*imageP1P2.x + imageP1P2.y*imageP1P2.x)
偏转角thetaRI的求取：如图10-2所示。
图10-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就可以对相机坐标系内的任意一个坐标进行机器人坐标系坐标转换。原理图如图10-3所示。
图10-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)。这样通过图10-3中的原理就可以得出机器人坐标系下的坐标了。
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
得到最终的robotP3坐标就可以通信给机器人让机器人移动到该座标下，也就是目标物体的位置处。
10.2标定程序实现
完整的程序实现如下所示：
/*
机器人坐标系与相机坐标系转换程序
第一步：记录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
10.3工业智能相机与SCARA机械手坐标系的标定实例
硬件设备为固高智能相机以及固高系统的SCARA机械手，该实例演示整个标定过程并验证标定效果。
以一枚硬币作为目标物体，分别从放置P1点与P2点，分别得到两个坐标点在相机坐标系以及机器人坐标系下的位置，通过路由器将三者连接起来，并通过ping来测试三者是否通信顺畅。如图10-4所示。
图10-4机器人，相机以及PC机连接示意图
物体在相机坐标系的坐标信息从固高智能相机软件中找到，如图10-5a所示；
图10-5相机坐标系下的坐标获取
物体在机器人坐标系的坐标信息需要移动机器人至物体上方，然后从示教器界面中查看位置信息，如图10-6所示；
图10-6机器人坐标系下的坐标获取
通过两个点的不同坐标系的坐标信息输入，计算得到了比例系数和偏转角，最后放置目标物体在一个新的位置，输入物体在相机坐标系的坐标，获得物体在机器人坐标系下的坐标，如图10-7所示。注意：相机坐标系输入坐标的时候，需要将x与y坐标对调，因为机器人坐标系的z轴是向上的，根据右手定则相机坐标系的z轴向下，所以相机坐标系中软件显示的x坐标实为y坐标，y坐标实为x坐标。
图10-7坐标求取结果示意图
根据10-7所示，机器人坐标系下该点坐标应该为441.261,28.5031；然后，可以通过示教器直接进行点位移动来验证。当然，更好的方式是通过示教编程，因为只有弄懂了示教编程，才能进行下一步的相机进行模式识别，然后将坐标传送给计算机，计算机通过软件进行计算并把机器人坐标系下的坐标信息发送回机器人，机器人进行自动化跟踪抓取。
示教程序如下：
程序解释如下：首先将机器人移动至P87点（可随意选取，主要是为了观察机器人是否正常和接下来移动方便），使用str7来存储441.261,28.5031；接下来就要建立通信连接，str2存储的字符为pc，pc在通信设置中设置的是电脑端的IP地址等信息；如图10-8所示。
图10-8数值型变量字符型界面示意图
在机器人等待接受数据的10秒间隔期间，将机器人坐标系下的位置信息通过TCP调试助手发送给机器人，保存在str7中，然后将str7中的字符进行分割，分割的标志为“,”，“,”前面的分割存入str8，“,”后面的分割存入str9，然后将其转换为实型变量，并将其复制给P88点，然后让机器人移动至P88点——即(441.261,28.5031)。具体的机器人编程请看《工业机器人控制系统用户手册.pdf》。
程序编写完成后，打开PC机的网络调试助手，并输入机器人端的IP地址以及端口号，以及待发送的内容，如图10-9所示。
图10-9网络调试助手通信示意图
观察发现机器人移动至物体上方，如图10-10所示，验证完成。
图10-10机器人移动结果示意图
观察发现机器人与目标物体的位置间有小的偏差，原因一是在于机器人未进行精准的零位标定（主要原因）；二是因为对物体的坐标选取都为目测得到。
