固定向上相机如何旋转纠偏？
固定向上相机配合机器人，
在机器人Tool0下吸取物料拍照并一次性纠偏。
—— Edited By Hugo
/目前：很多机器人以及第三方软件平台都有自己的标定方法，但毕竟还有一些机器人以及非标机构需要自己去实现这样的手眼标定，这里提供一种一次性纠偏的解题思路，仅供大家参考。/
# 1
常规解题思路一般有两种：
第一种：利用机器人自带的刷新函数，计算出物料中心距离Tool0的偏移值以及偏移角度，最后通过类似ToolSet的函数，直接设置成机器人的TCP工具，即可直接使用更新后的工具进行纠偏放料。
第二种：大家往往会采取两步走的方法进行纠偏，第一次拍照计算物料的角度，让机器人旋转至物料放置的角度一致。第二次拍照计算与标准模板相比较XY的偏移量。再次让机器人平移到放料位置。
第一种方式，简单方便，但是算法跟某种机器人绑定，第二种方式，适合所有机器人以及执行机构使用，但是需要拍两次，对于节拍要求严格的场合就不太合适了。
现在要对第二种方式进行优化：
# 2
下面开始进入正题：
求旋转中心。即当机器人吸取物料后需要旋转一定角度放料时，需求出物料是绕着哪个中心进行旋转的。
如上图：
为了方便演示，我们假设旋转中心为原点O(0,0),实际这个旋转中心原点是未知的，需要我们计算得出。直线f可以假设是旋转轴延伸出来的吸嘴治具，A点是我们吸取着物料的特征点位置。
STEP 1:得到三个旋转之后的坐标。
首先，我们先让机器人或者执行机构旋转3次，这里每次逆时针旋转了角AOB15°，分别得到三个坐标位置A,B,C。（注意，这三个位置最好是9点标定转换出来的机器人坐标系下的坐标，如果是像素坐标，记得最后根据像素单量进行转换）。
例如:得到的三个坐标分别为
A（3,1）
B (2.64,1.74)
C (2.1,237)
STEP 2:求线段AB的长度。
Lab = SQRT((Ax-Bx)^2+(Ay-By)^2)=0.8229
STEP 3:求OA,OB的长度。
由上图可知，以AB为底边的是等腰三角形OAB。
Loa=(Lab /2)/SIN((角AOB/2)*PI()/180)=3.1523
Lob=3.1523
STEP 4:求AB边的在该坐标系的方位角：
αAB=DEGREES(PI()*(1-SIGN(By-Ay)/2)-ATAN((Bx-Ax)/(By-Ay)))=115.9422
如果αAB小于0 则αAB加上360，这里大于零，所以不需要加了。
STEP 5:求得OA边在该坐标系的方位角：
由于具有对称性通常有两个解，
但是三点只能仅有一个圆心，我们后面将利用第三个点进行剔除。
αAO1=αAB -（90-角AOB/2）=33.44
αAO2=αAB +（90-角AOB/2）=198.44
STEP 6: 求得旋转中心。
解1：
Ox1=(Ax+Loa*COS(αAO1*PI()/180))=5.6
Oy1=(Ay+Loa*COS(αAO1*PI()/180))=2.73
解2：
Ox2=(Ax+Loa*COS(αAO2*PI()/180))=0.0096
Oy2=(Ay+Loa*COS(αAO2*PI()/180))=0.0028
STEP 7: 判断哪个结果是有效值。
由上分析可知：
线段BC与线段AO的的夹角小于90°时
此时才是有效旋转中心。
根据向量公式：cos<a,b>=a.b/|a||b|
分别计算BC与AO1 BC与AO2的角度进行对比
可知：BC与AO1夹角为97.16
BC与AO2夹角为67.84
即该旋转中心为：Ox2，Oy2
与我们假设中心（0,0）接近，验证成功。
说在后面，可能有同学会问为啥不直拟合圆求圆心，其实也可以，只不过这样精度会更高一些。而且这样会简化用户操作，只需示教三个点。
Halcon拟合结果
VM拟合结果
# 3
旋转中心已经求出来了，下面可以直接计算旋转后的位置差值。
STEP 1:先示教一个放料位置，然后再把物料吸取起来，移动到固定向上相机的拍照位置。
拍照后记录当前物料特征中心坐标值以及角度：refPos(x,y,u)
STEP 2:重新吸取一个新的物料，拍照得到当前物料特征中心坐标值以及角度：curPos(x,y,u)
STEP 3:根据绕点旋转公式：
x0= (x - rx0)* cos(a) - (y - ry0)*sin(a) + rx0 ; y0= (x - rx0)* sin(a) + (y - ry0)*cos(a) + ry0 ;
其中，
x=curPos_x; y=curPos_y;
rx0=Ox2; ry0=Oy2;
a=curPos_u-refPos_u;
可得旋转后的坐标位置为:x0,y0;
STEP 4: 计算偏移量：
旋转后的偏移量为，x0-refPos_x，y0-refPos_y
STEP 4: 最后发送给机器人或者执行机构的偏移值为：x0-refPos_x，y0-refPos_y，a
根据上述过程已经写成代码欢迎下载测试使用
测试代码：
请关注公众号 【AmazingRobot+】后回复 ‘固定向上相机标定’进行下载
- end -
视觉选型|机器人选型|手眼标定
图像处理|视觉软件开发|案例应用分享
