如下结合附图，对《基于平面的机器人三维寻位纠偏方法》作进一步描述：

如图1-4所示，一种基于平面的机器人三维寻位纠偏方法，包括如下步骤：

S01:在工件上定义工件坐标系wobjA，并定义基于所述工件坐标系wobjA的用户坐标系wobjB；

S02:沿所述工件坐标系wobjA的XY平面的X轴方向，间隔相同距离获取两个坐标点值P1（X1，Y1）和P2（X2，Y2）；

S03：沿所述工件坐标系wobjA的XY平面的Y轴方向，间隔相同距离获取两个坐标点值P3（X3，Y3）和P4（X4，Y4）；

S04：利用上述坐标点值中的两个，根据反正切三角函数ATan计算出基于工件坐标系wobjA的偏移角度A；该实施例中取坐标点值P3（X3，Y3）和P4（X4，Y4）进行计算，A＝-（arctan（（X4-X3）/（Y4-Y3）））；

S05：利用上述坐标点值P1（X1，Y1）、P2（X2，Y2）、P3（X3，Y3）和P4（X4，Y4），根据直线方程求解基于工件坐标系的X轴偏移量△X与Y轴偏移量△Y；

直线K1的方程是，K1＝（Y1-Y2）/（X1-X2）；

直线K2的方程是，K2＝（Y3-Y4）/（X3-X4）；

X轴偏移量，△X＝（K1*X1-K2*X3 Y3-Y1）/（K1-K2）；

Y轴偏移量，△Y＝Y1 （△X-X1）*K1；

S06：将上述偏移角度A转换成可供机器识别的姿态四元数；

S07：将该姿态四元数、X轴偏移量△X和Y轴偏移量△Y赋值至所述用户坐标wobjB，以实现机器人的姿态调整。

在该实施例中，上述取点、计算、姿态四元数转换由机器人编译工具实现，具体代码如下：

一、坐标点值获取：

MoveLpA1，v1000，fine，Tooldata_2\WObj:＝wobjA；

注：MoveL机器人直线运动指令，直线运动到pA1点，v1000是速度，find是准确到达该点，tooldata_2\WObj:＝wobjA是工具坐标\工件坐标；

SearchL\Stop，Di1Find，target_base{1}，pA2，v5，Tooldata_2\WObj:＝wobjA；

注：SearchL\Stop机器人直线寻边指令，从上一个点以v5的速度运行到pA2，当信号DiFind＝1时停止，并将当前位置坐标（x，y，z）储存在target_base{1}，tooldata_2\WObj:＝wobjA是工具坐标\工件坐标；

MoveLpC1，v1000，fine，Tooldata_2\WObj:＝wobjA；

SearchL\Stop，Di1Find，target_base{3}，pC2，v5，Tooldata_2\WObj:＝wobjA；

MoveLpD1，v1000，fine，Tooldata_2\WObj:＝wobjA；

SearchL\Stop，DI10_3，target_base{4}，pD2，v5，Tooldata_2\WObj:＝wobjA；

注：利用上述指令获取的4个点（target_base{1}、target_base{2}、target_base{3}、target_base{4}）

Y{1}:＝target_base{1}.trans.y-wobjA，uframe.tran.y；

X{1}:＝target_base{1}.trans.x-wobjA，uframe.tran.x；

Y{2}:＝target_base{2}.trans.y-wobjA，uframe.tran.y；

X{2}:＝target_base{2}.trans.x-wobjA，uframe.tran.x；

Y{3}:＝target_base{3}.trans.y-wobjA，uframe.tran.y；

X{3}:＝target_base{3}.trans.x-wobjA，uframe.tran.x；

Y{4}:＝target_base{4}.trans.y-wobjA，uframe.tran.y；

X{4}:＝target_base{4}.trans.x-wobjA，uframe.tran.x；

注：分别将4个点的X、Y值赋值到变量里用于计算。

二、偏角计算：

angle:＝-（ATan（（X{4}-X{3}）/（Y{4}-Y{3}）））；

注：使用机器人函数Atan，用其中P3和P4的数据求出相对wobjA的角度储存在变量angle中；

三、X轴偏移量△X与Y轴偏移量△Y计算：

K1:＝（Y{1}-Y{2}）/（X{1}-X{2}）；

K2:＝（Y{3}-Y{4}）/（X{3}-X{4}）；

X{5}:＝（K1*X{1}-K2*X{3} Y{3}-Y{1}）/（K1-K2）；

Y{5}:＝Y{1} （X{5}-X{1}）*K1；

四、姿态四元数转换：

wobjB:＝wobjA；

wobjB.oframe.rot:＝OrientZYX（angle，0，0）；

wobjB.oframe.trans.x:＝X{5}；

wobjB.oframe.trans.y:＝Y{5}；

注：将wobjA复制到wobjB；将求出的角度变量angle通过机器人编译工具中的OrientZYX函数转换为姿态四元数后赋值到wobjB的姿态值上；将求出的变量X{5}、Y{5}，赋值到wobjB上；

一种基于上述基于平面的机器人三维寻位纠偏方法的焊接机器人，其包括一具有寻边探头11的机械臂1，所述寻边探头11包括弹簧、探针111以及与焊枪连接的固定支架13；所述探针通过导线与机器人的I/O模块连接，该导线上串联有继电器。所述焊接机器人由数控程序控制，并以上述纠偏方案减少误差。

以焊接厨房用水槽为例，具体操作是将水槽的槽体2与桌子3焊接。

首先，在产品上建立工件坐标系wobjA，通过使用寻边探头11在相对于建立在产品上坐标系wobjA的x轴、y轴方向分别隔固定距离获得两个位置，并记录所获物4个位置值P1（X1，Y1）、P2（X2，Y2）、P3（X3，Y3）和P4（X4，Y4）。然后由上述程序利用4点坐标数据计算得到一组偏差数（△X、△Y和偏角A），再赋值至用户坐标系以纠正焊接路径。