视觉9点标定原理及RAPID计算

1. 视觉的9点标定（记录机器人运动的在机器人base下的9个点和对应的像素坐标），本质就是仿射变换的求解。即已知一些点在一个坐标系下的表示和在另一个坐标系下的表示，计算这个转换关系。该计算也可以在机器人侧实现（例如EPSON机器人的视觉功能等）

1. ‌旋转‌：旋转可以通过以下公式实现：
   x1=x2cos⁡α−y2sin⁡αx1=x2cosα−y2sinα
   y1=x2sin⁡α+y2cos⁡αy1=x2sinα+y2cosα
   其中，αα 是旋转角度。

2. ‌平移‌：平移可以通过以下公式实现：
   x1=x2+k1x1=x2+k1
   y1=y2+k2y1=y2+k2
   其中，k1k1 和 k2k2 分别是x轴和y轴的平移量。

3. ‌缩放‌：缩放可以通过以下公式实现：
   x1=t1x2x1=t1x2
   y1=t2y2y1=t2y2
   其中，t1t1 和 t2t2 分别是x轴和y轴的缩放因子。

4. ‌综合变换‌：将旋转、平移和缩放结合，可以得到仿射变换的一般公式：
   x1=t1x2cos⁡α−t2y2sin⁡α+k1x1=t1x2cosα−t2y2sinα+k1
   y1=t2x2sin⁡α+t2y2cos⁡α+k2y1=t2x2sinα+t2y2cosα+k2
   这个公式描述了仿射变换的基本形式，其中包含了旋转、缩放和平移的组合。

5. ‌简化公式‌：通过代数操作，可以将上述公式简化为：
   x1=ax2+by2+cx1=ax2+by2+c
   y1=cx2+dy2+ey1= dx2+ ey2+ f

6. 将多点坐标进行整理，可以得到以下公式

RAPID中实现代码如下：

 RECORD point    num x;    num y; ENDRECORD PERS point img_points{9}:=[[0,0],[250,0],[500,0],[500,250],[250,250],[0,250],[0,500],[250,500],[500,500]];PERS point real_points{9}:=[[683.1,-353.55], [1036.74,0.00],[859.97,176.78],[683.19,0.00], [506.41,-176.78], [329.64,0.00],                           [506.41,176.78],[683.19,353.55]];! 仿射计算结果PERS num Affine{6}:=[0.707167,-0.707047,683.15,0.707107,0.707107,-353.553];
! 使用放射计算结果，将图像坐标转到实际PERS point NewPointReal:=[683.18,0];PERS point NewPointImg:=[250,250];
PROC test900()    calculateAffineMatrix img_points,real_points,Affine;    calNewPoint NewPointImg,NewPointReal;ENDPROC
proc calculateAffineMatrix(Point img_points{*},Point real_points{*},inout num affine_matrix{*})    var dnum A{20,6};    var dnum b{20};    VAR dnum x{6};    VAR num A_m;
    A_m:=dim(img_points,1)*2;    ! Ax = b      for i from 1 to A_m/2 do        A{2*i-1,1}:=numtodnum(img_points{i}.x);        A{2*i-1,2}:=numtodnum(img_points{i}.y);        A{2*i-1,3}:=1;        A{2*i-1,4}:=0;        A{2*i-1,5}:=0;        A{2*i-1,6}:=0;
        A{2*i,1}:=0;        A{2*i,2}:=0;        A{2*i,3}:=0;        A{2*i,4}:=numtodnum(img_points{i}.x);        A{2*i,5}:=numtodnum(img_points{i}.y);        A{2*i,6}:=1;
        b{2*i-1}:=numtodnum(real_points{i}.x);        b{2*i}:=numtodnum(real_points{i}.y);    endfor    !  x =[a, b, tx, c, d, ty ]    MatrixSolve A\A_m:=A_m,b,x;
    FOR i FROM 1 TO 6 DO        affine_matrix{i}:=dnumtonum(x{i});    ENDFOR    AffinePos.x:=affine_matrix{3};    AffinePos.y:=affine_matrix{6};    AffinePos.z:=acos(affine_matrix{1});endproc
PROC calNewPoint(point img_points,inout point real_points)    real_points.x:=Affine{1}*img_points.x+Affine{2}*img_points.y+Affine{3};    real_points.y:=Affine{4}*img_points.x+Affine{5}*img_points.y+Affine{6};
ENDPROC

********************************