CN112779830B - 一种利用机器人定位辙叉的测量方法 - Google Patents

Abstract

本发明提供一种利用机器人定位辙叉的测量方法，所述机器人的输出端上设有测距传感器，其特征在于，包括如下步骤：基于所述测距传感器的工具坐标系生成标准向量；对所述辙叉进行第一次定位，检测所述辙叉在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量；计算所述交点在世界坐标系中的坐标位置；根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的标准工件坐标系W_obj，通过机器人可以准确测量辙叉上料后的空间位置，从而可以根据该空间位置对机器人程序进行补偿，保证能够准确对辙叉进行打磨。本发明的技术方案降低铁路辙叉的测量时间，提高工作效率，减少工作强度。

Description

一种利用机器人定位辙叉的测量方法

技术领域

本发明涉及工业测量技术领域，特别涉及一种利用机器人定位辙叉的测量方法。

背景技术

铁路辙叉是使火车车轮由一股线路转换到另一股路线的轨线平面交叉部件，该部件为消耗品，当磨损到一定程度后就需要进行更换，因此该部件有大量的生产需求。该铁路辙叉的两端需要与钢轨进行精确装配，与钢轨配合的装配面有较高的尺寸精度要求，传统的方式是通过人工打磨方式来实现尺寸要求。该方式对工人的技术水平要求较高，不仅生产效率低下，打磨产生的粉尘对人体也有很大伤害，因此我们开发了机器人打磨技术，但是由于辙叉为铸造件，外形尺寸最长达4米，重将近1吨，自身尺寸偏差很大，在机器人打磨系统中很难通过装卡的方式保证辙叉的空间位置。

发明内容

本发明的目的在于提供一种利用机器人定位辙叉的测量方法，用于测量人工上料后的辙叉相对于工业机器人的空间位置，以保证机器人对辙叉进行准确打磨。

为了实现上述目的，本发明提供如下技术方案：一种利用机器人定位辙叉的测量方法，所述机器人的输出端上设有测距传感器，其特征在于，包括如下步骤：

S1：基于所述测距传感器的工具坐标系生成标准向量；

S2：对所述辙叉进行第一次定位，检测所述辙叉在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量；

S3：对所述辙叉进行第二次定位，选取所述辙叉的一个端点和与所述端点相邻的三个测量面，通过所述测距传感器对三个所述测量面和三个所述测量面之间的交点进行测量，通过计算所述交点在世界坐标系中的坐标位置；

S4：根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的标准工件坐标系W_obj。

进一步地，还包括步骤S5：在所述辙叉被加工后重复步骤S3，并根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的补偿工件坐标系，计算所述补偿工件坐标系与所述标准工件坐标系Wobj之间的差值，并将所述差值补充到所述标准工件坐标系Wobj中。

进一步地，在所述步骤S3中，所述通过所述测距传感器对三个所述测量面和三个所述测量面之间的交点进行测量包括：

S31：通过所述测距传感器测量每个所述测量面上的至少5个测定点；

S32：通过每个所述测量面上被测量的至少5个测定点计算出所述测量面在所述世界坐标系内的坐标；

S33：基于所述测量面在所述世界坐标系中的位置计算所述交点在所述世界坐标系的坐标。

进一步地，在所述步骤S3中，所述测量面包括第一测量面、第二测量面和第三测量面，所述第一测量面上的所述测定点包括：第一测定点P₃₁、第二测定点P₃₂、第三测定点P₃₃、第四测定点P₃₄和第五测定点P₃₅，所述第一测定点P₃₁、所述第二测定点P₃₂、所述第三测定点P₃₃、所述第四测定点P₃₄和所述第五测定点P₃₅的平均值

所述第一测定点P₃₁与所述平均值P_average之间的差值P₁₀₁＝<P₃₁[1]-P_average[1]|P₃₁[2]-P_average[2]|P₃₁[3]-P_average[3]>

公式(2)

所述第二测定点P₃₂与所述平均值P_average之间的差值P₁₀₂＝<P₃₂[1]-P_average[1]|P₃₂[2]-P_average[2]|P₃₂[3]-P_average1[3]>

公式(3)

所述第三测定点P₃₃与所述平均值P_average之间的差值P₁₀₃＝<P₃₃[1]-P_average[1]|P₃₃[2]-P_average[2]|P₃₃[3]-P_average[3]>

公式(4)

所述第四测定点P₃₄与所述平均值P_average之间的差值P₁₀₄＝<P₃₄[1]-P_average[1]|P₃₄[2]-P_average[2]|P₃₄[3]-P_average[3]>

公式(5)

所述第五测定点P₃₅与所述平均值P_average之间的差值P₁₀₅＝<P₃₅[1]-P_average[1]|P₃₅[2]-P_average[2]|P₃₅[3]-P_average[3]>

公式(6)

矩阵F＝A·C 公式(7)

矩阵C＝<<P₁₀₁[1]|P₁₀₁[3]|-1>，<P₁₀₂[1]|P₁₀₂[3]|-1>，

<P₁₀₃[1]|P₁₀₃[3]|-1><P₁₀₄[1]|P₁₀₄[3]|-1>，<P₁₀₅[1]|P₁₀₅[3]|-1>>公式(8)

矩阵E＝<-P₁₀₁[2]，-P₁₀₂[2]，-P₁₀₃[2]，-P₁₀₄[2]，-P₁₀₅[2]>

公式(9)

矩阵A＝C^T 公式(10)

矩阵B＝A·E 公式(11)

B＝<<(P₁₀₁[1]·P₁₀₁[1]+P₁₀₂[1]·P₁₀₂[1]+P₁₀₃[1]·P₁₀₃[1]+P₁₀₄[1]·P₁₀₄[1]+P₁₀₅[1]·P₁₀₅[1])|(P₁₀₁[1]·P₁₀₁[3]+P₁₀₂[1]·P₁₀₂[3]+P₁₀₃[1]·P₁₀₃[3]+P₁₀₄[1]·P₁₀₄[3]+P₁₀₅[1]·P₁₀₅[3])|P₁₀₁[1]+P₁₀₂[1]+P₁₀₃[1]+P₁₀₄[1]+P₁₀₅[1]>，<(P₁₀₁[3]·P₁₀₁[1]+P₁₀₂[3]·P₁₀₂[1]+P₁₀₃[3]·P₁₀₃[1]+P₁₀₄[3]·P₁₀₄[1]+P₁₀₅[3]·P₁₀₅[1])|(P₁₀₁[3]·P₁₀₁[3]+P₁₀₂[3]·P₁₀₂[3]+P₁₀₃[3]·P₁₀₃[3]+P₁₀₄[3]·P₁₀₄[3]+P₁₀₅[3]·P₁₀₅[3])|-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[4]+P₁₀₅[5])>，<-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[4]+P₁₀₅[5])|-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[3]+P₁₀₅[3])|(1+1+1+1+1)>> 公式(12)

F·X＝B 公式(13)

所述公式(14)中的X的求解函数L＝F[1][1]·(F[2][2]·F[3][3]+F[2][3]·F[3][2])+F[1][1]·(F[2][3]·F[3][1]-F[2][1]·F[3][3])+F[1][3]·(F[2][1]·F[3][2]-F[2][2]·F[3][1]) 公式(14)

令B[1]·(F[2][2]·F[3][3]+F[2][3]·F[3][2])+F[1][2]·(F[2][3]·B[3]-B[2]·F[3][3])+F[1][3]·(B[2]·F[3][2]-F[2][2]·F[2][2]·B[3])＝h[1]

公式(15)

令F[1][1]·(B[2]·F[3][3]+F[2][3]·B[3])+B[1]·(F[2][3]·F[3][1]-F[2][1]·F[3][3])+F[1][3]·(F[2][1]·B[3]-B[2]·F[3][1])＝h[2]

公式(16)

令F[1][1]·(B[2]·F[3][3]+F[2][3]·B[3])+F[1][1]·(B[2]·F[3][1]-F[2][1]·B[3])+B[1]·(F[2][1]·F[3][2]-F[2][2]·F[3][1])＝h[3]

公式(17)

所述第一测量面的表达式为：A₁x+B₁y+C₁z＝d_y 公式(18)

常数d_y＝W_y[1]·(P_average[1])+(P_average[2])+W_y[2]·(P_average[3])-W_y[3] 公式(22)

所述第二测量面上的所述测定点包括：第六测定点P₁₁、第七测定点P₁₂、第八测定点P₁₃、第九测定点P₁₄、第十测定点P₁₅，所述第六测定点P₁₁、所述第七测定点P₁₂、所述第八测定点P₁₃、所述第九测定点P₁₄、所述第十测定点P₁₅的平均值

所述第六测定点P₁₁与所述平均值P_average2之间的差值P₁＝<P₁₁[1]-P_average2[1]|P₁₁[2]-P_average2[2]|P₁₁[3]-P_average2[3]>

公式(24)

所述第七测定点P₁₂与所述平均值P_average2之间的差值P₂＝<P₁₂[1]-P_average2[1]|P₁₂[2]-P_average2[2]|P₁₂[3]-P_average2[3]>

公式(25)

所述第八测定点P₁₃与所述平均值P_average2之间的差值P₃＝<P₁₃[1]-P_average2[1]|P₁₃[2]-P_average2[2]|P₁₃[3]-P_average2[3]>

公式(26)

所述第九测定点P₁₄与所述平均值P_average2之间的差值P₄＝<P₁₄[1]-P_average2[1]|P₁₄[2]-P_average2[2]|P₁₄[3]-P_average2[3]>

公式(27)

所述第十测定点P₁₅与所述平均值P_average2之间的差值P₅＝<P₁₅[1]-P_average2[1]|P₁₅[2]-P_average2[2]|P₁₅[3]-P_average2[3]>

公式(28)

矩阵C_x＝<<P₁[2]|P₁[3]|-1>，<P₂[2]|P₂[3]|-1>，

<P₃[2]|P₃[3]|-1><P₄[2]|P₄[3]|-1>，<P₅[2]|P₅[3]|-1>>

公式(29)

矩阵E_x＝<-P₁[1]，-P₂[1]，-P₃[1]，-P₄[1]，-P₅[1]>

公式(30)

矩阵A_x＝C_x ^T 公式(31)

矩阵B_x＝A_x·E_x 公式(32)

矩阵B_x＝<<-(P₁[1]·P₁[2]+P₂[1]·P₂[2]+P₃[1]·P₃[12]+P₄[1]·P₄[2]+P₅[1]·P₅[2])>，<-(P₁[2]·P₁[3]+P₂[2]·P₂[3]+P₃[2]·P₃[3]+P₄[2]·P₄[3]+P₅[2]·P₅[3])>，<P₁[2]+P₂[2]+P₃[2]+P₄[2]+P₅[2]>>

公式(33)

矩阵F_x＝A_x·C_x 公式(34)

F_x＝<<(P₁[1]·P₁[1]+P₂[1]·P₂[1]+P₃[1]·P₃[1]+P₄[1]·P₄[1]+P₅[1]·P₅[1])|(P₁[1]·P₁[3]+P₂[1]·P₂[3]+P₃[1]·P₃[3]+P₄[1]·P₄[3]+P₅[1]·P₅[3])|P₁[1]+P₂[1]+P₃[1]+P₄[1]+P₅[1]>，<(P₁[3]·P₁[1]+P₂[3]·P₂[1]+P₃[3]·P₃[1]+P₄[3]·P₄[1]+P₅[3]·P₅[1])|(P₁[3]·P₁[3]+P₂[3]·P₂[3]+P₃[3]·P₃[3]+P₄[3]·P₄[3]+P₅[3]·P₅[3])|-(P₁[3]+P₂[3]+P₃[3]+P₄[4]+P₅[5])>，<-(P₁[3]+P₂[3]+P₃[3]+P₄[4]+P₅[5])|-(P₁[3]+P₂[3]+P₃[3]+P₄[3]+P₅[3])|(1+1+1+1+1)>> 公式(35)

F_x·X＝B_x 公式(36)

所述公式(36)中的X的求解函数L_x＝F_x[1][1]·(F_x[2][2]·F_x[3][3]+F_x[2][3]·F_x[3][2])+F_x[1][2]·(F_x[2][3]·F_x[3][1]-F_x[2][1]·F_x[3][3])+F_x[1][3]·(F_x[2][1]·F_x[3][2]-F_x[2][2]·F_x[3][1])

公式(37)

令B_x[1]·(F_x[2][2]·F_x[3][3]+F_x[2][3]·F_x[3][2])+F_x[1][2]·(F_x[2][3]·B[3]-B_x[2]·F_x[3][3])+F_x[1][3]·(B_x[2]·F_x[3][2]-F_x[2][2]·F_x[2][2]·B_x[3])＝h_x[1]

公式(38)

令F_x[1][1]·(B_x[2]·F_x[3][3]+F_x[2][3]·B_x[3])+B_x[1]·(F_x[2][3]·F_x[3][1]-F_x[2][1]·F[3][3])+F_x[1][3]·(F_x[2][1]·B_x[3]-B[2]·F_x[3][1])＝h_x[2] 公式(39)

令F_x[1][1]·(B_x[2]·F_x[3][3]+F_x[2][3]·B_x[3])+F_x[1][1]·(B_x[2]·F_x[3][1]-F_x[2][1]·B_x[3])+B_x[1]·(F_x[2][1]·F_x[3][2]-F_x[2][2]·F_x[3][1])＝h_x[3] 公式(40)

所述第二测量面的表达式为：A₂x+B₂y+C₃z＝d_x 公式(41)

常数d_x＝W_x[1]·(P_average2[2])+(P_average2[1])+W_x[2]·(P_average2[3])-W_x[3] 公式(45)

所述第三测量面上的所述测定点包括：第十一测定点P₂₁、第十二测定点P₂₂、第十三测定点P₂₃、第十四测定点P₂₄和第十五测定点P₂₅，所述第十一测定点P₂₁、所述第十二测定点P₂₂、所述第十三测定点P₂₃、所述第十四测定点P₂₄和所述第十五测定点P₂₅的平均值

所述第十一测定点P₂₁与所述平均值P_average3之间的差值P₆＝<P₂₁[1]-P_average3[1]|P₂₁[2]-P_average3[2]|P₂₁[3]-P_average3[3]>

公式(47)

所述第十二测定点P₂₂与所述平均值P_average3之间的差值P₇＝<P₂₂[1]-P_average3[1]|P₂₂[2]-P_average3[2]|P₂₂[3]-P_average3[3]>

公式(48)

所述第十三测定点P₂₃与所述平均值P_average3之间的差值P₈＝<P₂₃[1]-P_average3[1]|P₂₃[2]-P_average3[2]|P₂₃[3]-P_average3[3]>

公式(49)

所述第十四测定点P₂₄与所述平均值P_average3之间的差值P₉＝<P₂₄[1]-P_average3[1]|P₂₄[2]-P_average3[2]|P₂₄[3]-P_average3[3]>

公式(50)

所述第十五测定点P₂₅与所述平均值P_average3之间的差值P₁₀＝<P₂₅[1]-P_average3[1]|P₂₅[2]-P_average3[2]|P₂₅[3]-P_average3[3]>

公式(51)

矩阵C_z＝<<P₆[1]|P₆[2]|-1>，<P₇[1]|P₇[2]|-1>，<P₈[1]|P₈[2]|-1><P₉[1]|P₉[2]|-1>，<P₁₀[1]|P₁₀[2]|-1>>

公式(52)

矩阵E_z＝<-P₆[3]，-P₇[3]，-P₈[3]，-P₉[3]，-P₁₀[3]> 公式(53)

矩阵A_z＝C_z ^T 公式(54)

矩阵B_z＝A_z·E_z 公式(55)

矩阵B_z＝<<-(P₅[1]·P₅[2]+P₇[1]·P₇[2]+P₈[1]·P₈[12]+P₉[1]·P₉[2]+P₁₀[1]·P₁₀[2])>，<-(P₆[2]·P₆[3]+P₇[2]·P₇[3]+P₈[2]·P₈[3]+P₉[2]·P₉[3]+P₁₀[2]·P₁₀[3])>，<P₅[2]+P₇[2]+P₈[2]+P₉[2]+P₁₀[2]>>

公式(56)

矩阵F_z＝A_z·C_z 公式(57)

矩阵F_z＝<<(P₅[1]·P₅[1]+P₇[1]·P₇[1]+P₈[1]·P₈[1]+P₉[1]·P₉[1]+P₁₀[1]·P₁₀[1])|(P₅[1]·P₆[3]+P₇[1]·P₇[3]+P₈[1]·P₈[3]+P₉[1]·P₉[3]+P₁₀[1]·P₁₀[3])|P₅[1]+P₇[1]+P₈[1]+P₉[1]+P₁₀[1]>，<(P₅[3]·P₅[1]+P₇[3]·P₇[1]+P₈[3]·P₈[1]+P₉[3]·P₉[1]+P₁₀[3]·P₁₀[1])|(P₅[3]·P₅[3]+P₇[3]·P₇[3]+P₈[3]·P₈[3]+P₉[3]·P₉[3]+P₁₀[3]·P₁₀[3])|-(P₆[3]+P₇[3]+P₈[3]+P₉[4]+P₁₀[5])>，<-(P₆[3]+P₇[3]+P₈[3]+P₉[4]+P₁₀[5])|-(P₆[3]+P₇[3]+P₈[3]+P₉[3]+P₁₀[3])|(1+1+1+1+1)>>

公式(58)

F_z·X＝B_z 公式(59)

所述公式(59)中的X的求解函数L_z＝F_z[1][1]·(F_z[2][2]·F_z[3][3]+F_z[2][3]·F_z[3][2])+F_z[1][2]·(F_z[2][3]·F_z[3][1]-F_z[2][1]·F_z[3][3])+F_z[1][3]·(F_z[2][1]·F_z[3][2]-F_z[2][2]·F_z[3][1])

公式(60)

令B_z[1]·(F_z[2][2]·F_z[3][3]+F_z[2][3]·F_z[3][2])+F_z[1][2]·(F_z[2][3]·B_z[3]-B_z[2]·F_z[3][3])+F_z[1][3]·(B_z[2]·F_z[3][2]-F_z[2][2]·F_z[2][2]·B_z[3])＝h_z[1]

公式(61)

令F_z[1][1]·(B_z[2]·F_z[3][3]+F_z[2][3]·B_z[3])+B_z[1]·(F_z[2][3]·F_z[3][1]-F_z[2][1]·F_z[3][3])+F_z[1][3]·(F_z[2][1]·B_z[3]-B_z[2]·F_z[3][1])＝h_z[2] 公式(62)

令F_z[1][1]·(B_z[2]·F_z[3][3]+F_z[2][3]·B_z[3])+F_z[1][1]·(B_z[2]·F_z[3][1]-F_z[2][1]·B_z[3])+B_z[1]·(F_z[2][1]·F_z[3][2]-F_z[2][2]·F_z[3][1])＝h_z[3] 公式(63)

所述第三测量面的表达式为：A₃x+B₃y+C₃z＝d_z 公式(64)

常数d_z＝W_z[1]·(P_average3[1])+W_z[2]·(P_average3[2])+(P_average3[3])-W_z[3] 公式(68)

所述第一测量面、所述第二测量面和所述第三测量面的交点

H：＝<<d_x>，<d_y>，<d_z>> 公式(69)。

进一步地，步骤S4包括：所述标准工件坐标系W_obj的Z轴的方向向量Z_wobj为所述第二测量面的法向量A₁和所述第三测量面的法向量A₂的向量积的方向，所述第三测量面的法向量A₂的方向为所述标准工件坐标系W_obj的X轴的方向向量，所述标准工件坐标系W_obj的Y轴的方向向量

Y_wobj＝Z_wobj×A₂ 公式(70)

所述交点H为所述标准工件坐标系W_obj的原点。

进一步地，所述测距传感器为激光传感器，所述测距传感器与所述辙叉之间的距离不大于100mm。

进一步地，所述标准向量的方向与所述测距传感器的校准激光中心点的方向相同。

进一步地，所述检测所述辙叉在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量包括：

S21：通过工装夹具固定所述辙叉；

S21：选取所述辙叉相邻的两个平面，且两个所述平面均与所述辙叉的底面相邻；

S22：测量两个所述平面在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量。

分析可知，本发明公开一种利用机器人定位辙叉的测量方法，机器人可以准确测量辙叉上料后的空间位置，从而可以根据该空间位置对机器人程序进行补偿，保证能够准确对辙叉进行打磨。本发明的技术方案降低铁路辙叉的测量时间，提高工作效率，减少工作强度。

附图说明

构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。其中：

图1本发明一个实施例的一种利用机器人定位辙叉的测量方法第二次定位的示意图。

图2本发明一个实施例的一种利用机器人定位辙叉的测量方法第一次定位的示意图。

图3本发明一个实施例的一种利用机器人定位辙叉的测量方法的标准工件坐标系的位置示意图。

图4本发明一个实施例的一种利用机器人定位辙叉的测量方法的流程图

附图标记说明：1-辙叉；2-第二测量面；3-第一测量面；4-第三测量面；5-平面。

具体实施方式

下面将参考附图并结合实施例来详细说明本发明。各个示例通过本发明的解释的方式提供而非限制本发明。实际上，本领域的技术人员将清楚，在不脱离本发明的范围或精神的情况下，可在本发明中进行修改和变型。例如，示为或描述为一个实施例的一部分的特征可用于另一个实施例，以产生又一个实施例。因此，所期望的是，本发明包含归入所附权利要求及其等同物的范围内的此类修改和变型。

在本发明的描述中，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明而不是要求本发明必须以特定的方位构造和操作，因此不能理解为对本发明的限制。本发明中使用的术语“相连”、“连接”、“设置”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是直接相连，也可以通过中间部件间接相连；可以是有线电连接、无线电连接，也可以是无线通信信号连接，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

所附附图中示出了本发明的一个或多个示例。详细描述使用了数字和字母标记来指代附图中的特征。附图和描述中的相似或类似标记的已经用于指代本发明的相似或类似的部分。如本文所用的那样，用语“第一”、“第二”、“第三”以及“第四”可互换地使用，以将一个构件与另一个区分开，且不旨在表示单独构件的位置或重要性。

如图1至图4所示，根据本发明的实施例，提供了一种利用机器人定位辙叉的测量方法，机器人的输出端上设有测距传感器，辙叉1通过工装夹具固定，包括：

S1：基于测距传感器的工具坐标系生成标准向量，标准向量的方向与测距传感器的校准激光中心点的方向相同；

S2：对辙叉1进行第一次定位，第一次定位为粗定位，由于铁路辙叉1底部已经经过精加工，所以只要检测铁路辙叉1在世界坐标系中X方向和Y方向的偏移量即可，检测辙叉1在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量，测距传感器为激光传感器，测距传感器与辙叉1之间的距离不大于100mm；

检测辙叉1在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量包括：

S21：通过工装夹具固定辙叉1；

S21：选取辙叉1相邻的两个平面5，且两个平面5均与辙叉1的底面相邻；

S22：测量两个平面5在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量。

S3：对辙叉1进行第二次定位，第二次定位为精定位，并将定位后的数据传输至工业机器人的控制器中，选取辙叉1的一个端点和与端点相邻的三个测量面，通过测距传感器对三个测量面和三个测量面之间的交点进行测量，通过计算交点在世界坐标系中的坐标位置；

通过测距传感器对三个测量面和三个测量面之间的交点进行测量包括：

S31：通过测距传感器测量每个测量面上的至少5个测定点；

S32：通过每个测量面上被测量的至少5个测定点计算出测量面在世界坐标系内的坐标；

S33：基于测量面在世界坐标系中的位置计算交点在世界坐标系的坐标。

S4：根据交点在世界坐标系中的坐标位置计算出辙叉1的标准工件坐标系W_obj。

标准工件坐标系W_obj的Z轴的方向向量Z_wobj为第二测量面2的法向量A₁和第三测量面4的法向量A₂的向量积的方向，第三测量面4的法向量A₂的方向为标准工件坐标系W_obj的X轴的方向向量，标准工件坐标系W_obj的Y轴的方向向量

Y_wobj＝Z_wobj×A₂ 公式(70)

交点H为标准工件坐标系W_obj的原点。

S5：在辙叉1被加工后重复S3，并根据交点在世界坐标系中的坐标位置计算出辙叉1的补偿工件坐标系，计算补偿工件坐标系与标准工件坐标系Wobj之间的差值，并将差值补充到标准工件坐标系Wobj中

优选地，测量面包括第一测量面3、第二测量面2和第三测量面4，述第一测量面3上的测定点包括：第一测定点P₃₁、第二测定点P₃₂、第三测定点P₃₃、第四测定点P₃₄和第五测定点P₃₅，第一测定点P₃₁、第二测定点P₃₂、第三测定点P₃₃、第四测定点P₃₄和第五测定点P₃₅的平均值

/>

所述第一测定点P₃₁与所述平均值P_average之间的差值P₁₀₁＝<P₃₁[1]-P_average[1]|P₃₁[2]-P_average[2]|P₃₁[3]-P_average[3]>

公式(2)

所述第二测定点P₃₂与所述平均值P_average之间的差值P₁₀₂＝<P₃₂[1]-P_average[1]|P₃₂[2]-P_average[2]|P₃₂[3]-P_average1[3]>

公式(3)

所述第三测定点P₃₃与所述平均值P_average之间的差值P₁₀₃＝<P₃₃[1]-P_average[1]|P₃₃[2]-P_average[2]|P₃₃[3]-P_average[3]>

公式(4)

所述第四测定点P₃₄与所述平均值P_average之间的差值P₁₀₄＝<P₃₄[1]-P_average[1]|P₃₄[2]-P_average[2]|P₃₄[3]-P_average[3]>

公式(5)

所述第五测定点P₃₅与所述平均值P_average之间的差值P₁₀₅＝<P₃₅[1]-P_average[1]|P₃₅[2]-P_average[2]|P₃₅[3]-P_average[3]>

公式(6)

矩阵F＝A·C 公式(7)

矩阵C＝<<P₁₀₁[1]|P₁₀₁[3]|-1>，<P₁₀₂[1]|P₁₀₂[3]|-1>，<P₁₀₃[1]|P₁₀₃[3]|-1><P₁₀₄[1]|P₁₀₄[3]|-1>，<P₁₀₅[1]|P₁₀₅[3]|-1>>

公式(8)

矩阵E＝<-P₁₀₁[2]，-P₁₀₂[2]，-P₁₀₃[2]，-P₁₀₄[2]，-P₁₀₅[2]>

公式(9)

矩阵A＝C^T 公式(10)

矩阵B＝A·E 公式(11)

B＝<<(P₁₀₁[1]·P₁₀₁[1]+P₁₀₂[1]·P₁₀₂[1]+P₁₀₃[1]·P₁₀₃[1]+P₁₀₄[1]·P₁₀₄[1]+P₁₀₅[1]·P₁₀₅[1])|(P₁₀₁[1]·P₁₀₁[3]+P₁₀₂[1]·P₁₀₂[3]+P₁₀₃[1]·P₁₀₃[3]+P₁₀₄[1]·P₁₀₄[3]+P₁₀₅[1]·P₁₀₅[3])|P₁₀₁[1]+P₁₀₂[1]+P₁₀₃[1]+P₁₀₄[1]+P₁₀₅[1]>，<(P₁₀₁[3]·P₁₀₁[1]+P₁₀₂[3]·P₁₀₂[1]+P₁₀₃[3]·P₁₀₃[1]+P₁₀₄[3]·P₁₀₄[1]+P₁₀₅[3]·P₁₀₅[1])|(P₁₀₁[3]·P₁₀₁[3]+P₁₀₂[3]·P₁₀₂[3]+P₁₀₃[3]·P₁₀₃[3]+P₁₀₄[3]·P₁₀₄[3]+P₁₀₅[3]·P₁₀₅[3])|-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[4]+P₁₀₅[5])>，<-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[4]+P₁₀₅[5])|-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[3]+P₁₀₅[3])|(1+1+1+1+1)>> 公式(12)

F·X＝B 公式(13)

所述公式(14)中的X的求解函数L＝F[1][1]·(F[2][2]·F[3][3]+F[2][3]·F[3][2])+F[1][1]·(F[2][3]·F[3][1]-F[2][1]·F[3][3])+F[1][3]·(F[2][1]·F[3][2]-F[2][2]·F[3][1]) 公式(14)

令B[1]·(F[2][2]·F[3][3]+F[2][3]·F[3][2])+F[1][2]·(F[2][3]·B[3]-B[2]·F[3][3])+F[1][3]·(B[2]·F[3][2]-F[2][2]·F[2][2]·B[3])＝h[1]

公式(15)

令F[1][1]·(B[2]·F[3][3]+F[2][3]·B[3])+B[1]·(F[2][3]·F[3][1]-F[2][1]·F[3][3])+F[1][3]·(F[2][1]·B[3]-B[2]·F[3][1])＝h[2]

公式(16)

令F[1][1]·(B[2]·F[3][3]+F[2][3]·B[3])+F[1][1]·(B[2]·F[3][1]-F[2][1]·B[3])+B[1]·(F[2][1]·F[3][2]-F[2][2]·F[3][1])＝h[3]

公式(17)

所述第一测量面的表达式为：A₁x+B₁y+C₁z＝d_y 公式(18)

常数d_y＝W_y[1]·(P_average[1])+(P_average[2])+W_y[2]·(P_average[3])-W_y[3] 公式(22)

所述第二测量面上的所述测定点包括：第六测定点P₁₁、第七测定点P₁₂、第八测定点P₁₃、第九测定点P₁₄、第十测定点P₁₅，所述第六测定点P₁₁、所述第七测定点P₁₂、所述第八测定点P₁₃、所述第九测定点P₁₄、所述第十测定点P₁₅的平均值

所述第六测定点P₁₁与所述平均值P_average2之间的差值P₁＝<P₁₁[1]-P_average2[1]|P₁₁[2]-P_average2[2]|P₁₁[3]-P_average2[3]>

公式(24)

所述第七测定点P₁₂与所述平均值P_average2之间的差值P₂＝<P₁₂[1]-P_average2[1]|P₁₂[2]-P_average2[2]|P₁₂[3]-P_average2[3]>

公式(25)

所述第八测定点P₁₃与所述平均值P_average2之间的差值P₃＝<P₁₃[1]-P_average2[1]|P₁₃[2]-P_average2[2]|P₁₃[3]-P_average2[3]>

公式(26)

所述第九测定点P₁₄与所述平均值P_average2之间的差值P₄＝<P₁₄[1]-P_average2[1]|P₁₄[2]-P_average2[2]|P₁₄[3]-P_average2[3]>

公式(27)

所述第十测定点P₁₅与所述平均值P_average2之间的差值P₅＝<P₁₅[1]-P_average2[1]|P₁₅[2]-P_average2[2]|P₁₅[3]-P_average2[3]>

公式(28)

矩阵C_x＝<<P₁[2]|P₁[3]|-1>，<P₂[2]|P₂[3]|-1>，<P₃[2]|P₃[3]|-1><P₄[2]|P₄[3]|-1>，<P₅[2]|P₅[3]|-1>>

公式(29)

矩阵E_x＝<-P₁[1]，-P₂[1]，-P₃[1]，-P₄[1]，-P₅[1]>

公式(30)

矩阵A_x＝C_x ^T 公式(31)

矩阵B_x＝A_x·E_x 公式(32)

矩阵B_x＝<<-(P₁[1]·P₁[2]+P₂[1]·P₂[2]+P₃[1]·P₃[12]+P₄[1]·P₄[2]+P₅[1]·P₅[2])>，<-(P₁[2]·P₁[3]+P₂[2]·P₂[3]+P₃[2]·P₃[3]+P₄[2]·P₄[3]+P₅[2]·P₅[3])>，<P₁[2]+P₂[2]+P₃[2]+P₄[2]+P₅[2]>>

公式(33)

矩阵F_x＝A_x·C_x 公式(34)

F_x＝<<(P₁[1]·P₁[1]+P₂[1]·P₂[1]+P₃[1]·P₃[1]+P₄[1]·P₄[1]+P₅[1]·P₅[1])|(P₁[1]·P₁[3]+P₂[1]·P₂[3]+P₃[1]·P₃[3]+P₄[1]·P₄[3]+P₅[1]·P₅[3])|P₁[1]+P₂[1]+P₃[1]+P₄[1]+P₅[1]>，<(P₁[3]·P₁[1]+P₂[3]·P₂[1]+P₃[3]·P₃[1]+P₄[3]·P₄[1]+P₅[3]·P₅[1])|(P₁[3]·P₁[3]+P₂[3]·P₂[3]+P₃[3]·P₃[3]+P₄[3]·P₄[3]+P₅[3]·P₅[3])|-(P₁[3]+P₂[3]+P₃[3]+P₄[4]+P₅[5])>，<-(P₁[3]+P₂[3]+P₃[3]+P₄[4]+P₅[5])|-(P₁[3]+P₂[3]+P₃[3]+P₄[3]+P₅[3])|(1+1+1+1+1)>> 公式(35)

F_x·X＝B_x 公式(36)

所述公式(36)中的X的求解函数L_x＝F_x[1][1]·(F_x[2][2]·F_x[3][3]+F_x[2][3]·F_x[3][2])+F_x[1][2]·(F_x[2][3]·F_x[3][1]-F_x[2][1]·F_x[3][3])+F_x[1][3]·(F_x[2][1]·F_x[3][2]-F_x[2][2]·F_x[3][1])

公式(37)

令B_x[1]·(F_x[2][2]·F_x[3][3]+F_x[2][3]·F_x[3][2])+F_x[1][2]·(F_x[2][3]·B[3]-B_x[2]·F_x[3][3])+F_x[1][3]·(B_x[2]·F_x[3][2]-F_x[2][2]·F_x[2][2]·B_x[3])＝h_x[1]

公式(38)

令F_x[1][1]·(B_x[2]·F_x[3][3]+F_x[2][3]·B_x[3])+B_x[1]·(F_x[2][3]·F_x[3][1]-F_x[2][1]·F[3][3])+F_x[1][3]·(F_x[2][1]·B_x[3]-B[2]·F_x[3][1])＝h_x[2] 公式(39)

令F_x[1][1]·(B_x[2]·F_x[3][3]+F_x[2][3]·B_x[3])+F_x[1][1]·(B_x[2]·F_x[3][1]-F_x[2][1]·B_x[3])+B_x[1]·(F_x[2][1]·F_x[3][2]-F_x[2][2]·F_x[3][1])＝h_x[3] 公式(40)

所述第二测量面的表达式为：A₂x+B₂y+C₃z＝d_x 公式(41)

/>

常数d_x＝W_x[1]·(P_average2[2])+(P_average2[1])+W_x[2]·(P_average2[3])-W_x[3] 公式(45)

所述第三测量面上的所述测定点包括：第十一测定点P₂₁[1]、第十二测定点P₂₂[1]、第十三测定点P₂₃[1]、第十四测定点P₂₄[1]和第十五测定点P₂₅[1]，所述第十一测定点P₂₁[1]、所述第十二测定点P₂₂[1]、所述第十三测定点P₂₃[1]、所述第十四测定点P₂₄[1]和所述第十五测定点P₂₅[1]的平均值

所述第十一测定点P₂₁[1]与所述平均值P_average3之间的差值P₆＝<P₂₁[1]-P_average3[1]|P₂₁[2]-P_average3[2]|P₂₁[3]-P_average3[3]>

公式(47)

所述第十二测定点P₂₂[1]与所述平均值P_average3之间的差值P₇＝<P₂₂[1]-P_average3[1]|P₂₂[2]-P_average3[2]|P₂₂[3]-P_average3[3]>

公式(48)

所述第十三测定点P₂₃[1]与所述平均值P_average3之间的差值P₈＝<P₂₃[1]-P_average3[1]|P₂₃[2]-P_average3[2]|P₂₃[3]-P_average3[3]>

公式(49)

所述第十四测定点P₂₄[1]与所述平均值P_average3之间的差值P₉＝<P₂₄[1]-P_average3[1]|P₂₄[2]-P_average3[2]|P₂₄[3]-P_average3[3]>

公式(50)

所述第十五测定点P₂₅[1]与所述平均值P_average3之间的差值P₁₀＝<P₂₅[1]-P_average3[1]|P₂₅[2]-P_average3[2]|P₂₅[3]-P_average3[3]>

公式(51)

矩阵C_z＝<<P₆[1]|P₆[2]|-1>，<P₇[1]|P₇[2]|-1>，<P₈[1]|P₈[2]|-1><P₉[1]|P₉[2]|-1>，<P₁₀[1]|P₁₀[2]|-1>>

公式(52)

矩阵E_z＝<-P₆[2]，-P₇[2]，-P₈[2]，-P₉[2]，-P₁₀[2]> 公式(53)

矩阵A_z＝C_z ^T 公式(54)

矩阵B_z＝A_z·E_z 公式(55)

矩阵B_z＝<<-(P₆[1]·P₆[2]+P₇[1]·P₇[2]+P₈[1]·P₈[12]+P₉[1]·P₉[2]+P₁₀[1]·P₁₀[2])>，<-(P₆[2]·P₆[3]+P₇[2]·P₇[3]+P₈[2]·P₈[3]+P₉[2]·P₉[3]+P₁₀[2]·P₁₀[3])>，<P₆[2]+P₇[2]+P₈[2]+P₉[2]+P₁₀[2]>>

公式(56)

矩阵F_z＝A_z·C_z 公式(57)

矩阵F_z＝<<(P₆[1]·P₆[1]+P₇[1]·P₇[1]+P₈[1]·P₈[1]+P₉[1]·P₉[1]+P₁₀[1]·P₁₀[1])|(P₆[1]·P₆[3]+P₇[1]·P₇[3]+P₈[1]·P₈[3]+P₉[1]·P₉[3]+P₁₀[1]·P₁₀[3])|P₆[1]+P₇[1]+P₈[1]+P₉[1]+P₁₀[1]>，<(P₆[3]·P₆[1]+P₇[3]·P₇[1]+P₈[3]·P₈[1]+P₉[3]·P₉[1]+P₁₀[3]·P₁₀[1])|(P₆[3]·P₆[3]+P₇[3]·P₇[3]+P₈[3]·P₈[3]+P₉[3]·P₉[3]+P₁₀[3]·P₁₀[3])|-(P₆[3]+P₇[3]+P₈[3]+P₉[4]+P₁₀[5])>，<-(P₆[3]+P₇[3]+P₈[3]+P₉[4]+P₁₀[5])|-(P₆[3]+P₇[3]+P₈[3]+P₉[3]+P₁₀[3])|(1+1+1+1+1)>>

公式(58)

F_z·X＝B_z 公式(59)

所述公式(59)中的X的求解函数L_z＝F_z[1][1]·(F_z[2][2]·F_z[3][3]+F_z[2][3]·F_z[3][2])+F_z[1][2]·(F_z[2][3]·F_z[3][1]-F_z[2][1]·F_z[3][3])+F_z[1][3]·(F_z[2][1]·F_z[3][2]-F_z[2][2]·F_z[3][1])

公式(60)

令B_z[1]·(F_z[2][2]·F_z[3][3]+F_z[2][3]·F_z[3][2])+F_z[1][2]·(F_z[2][3]·B_z[3]-B_z[2]·F_z[3][3])+F_z[1][3]·(B_z[2]·F_z[3][2]-F_z[2][2]·F_z[2][2]·B_z[3])＝h_z[1]

公式(61)

令F_z[1][1]·(B_z[2]·F_z[3][3]+F_z[2][3]·B_z[3])+B_z[1]·(F_z[2][3]·F_z[3][1]-F_z[2][1]·F_z[3][3])+F_z[1][3]·(F_z[2][1]·B_z[3]-B_z[2]·F_z[3][1])＝h_z[2] 公式(62)

令F_z[1][1]·(B_z[2]·F_z[3][3]+F_z[2][3]·B_z[3])+F_z[1][1]·(B_z[2]·F_z[3][1]-F_z[2][1]·B_z[3])+B_z[1]·(F_z[2][1]·F_z[3][2]-F_z[2][2]·F_z[3][1])＝h_z[3] 公式(63)

所述第三测量面的表达式为：A₃x+B₃y+C₃z＝d_z 公式(64)

常数d_z＝W_z[1]·(P_average3[1])+W_z[2]·(P_average3[2])+(P_average3[3])-W_z[3] 公式(68)

所述第一测量面、所述第二测量面和所述第三测量面的交点

H：＝<<d_x>，<d_y>，<d_z>> 公式(69)。

在实际使用时的验证如下，首先验证第一测量面3的差值，结果如下：

W_y[1]·x+y+W_y[2]·z-d_y＝0

W_y[1]·P₃₁[1]+P₃₁[2]+W_y[2]·P₃₁[3]-d_y＝0.0577568133091404

W_y[1]·P₃₂[1]+P₃₂[2]+W_y[2]·P₃₂[3]-d_y＝-0.0687316767096604

W_y[1]·P₃₃[1]+P₃₃[2]+W_y[2]·P₃₃[3]-d_y＝0.225713130039082

W_y[1]·P₃₄[1]+P₃₄[2]+W_y[2]·P₃₄[3]-d_y＝-0.0690957233220786

W_y[1]·P₃₅[1]+P₃₅[2]+W_y[2]·P₃₅[3]-d_y＝0.057498705137255

接着，验证第二测量面2的差值，结果如下：

x+W_x[1]·y+W_x[2]·z-d_x＝0

P₁₁[1]+W_x[1]·P₁₁[2]+W_x[2]·P₁₁[3]-d_x＝0.116537297318473

P₁₂[1]+W_x[1]·P₁₂[2]+W_x[2]·P₁₂[3]-d_x＝0.155312333241000

P₁₃[1]+W_x[1]·P₁₃[2]+W_x[2]·P₁₃[3]-d_x＝-0.462872419254609

P₁₄[1]+W₁[1]·P₁₄[2]+W_x[2]·P₁₄[3]-d_x＝0.147472809564761

P₁₅[1]+W_x[1]·P₁₅[2]+W_x[2]·P₁₅[3]-d_x＝0.0435518592230437

最后，验证第三测量面4的差值，结果如下：

W_z[1]·x+W_z[2]·y+z-d_z＝0

W_z[1]·P₂₁[1]+P₂₁[2]·W_z[2]+P₂₁[3]-d_z＝-0.252114583240200

W_z[1]·P₂₂[1]+P₂₂[2]·W_z[2]+P₂₂[3]-d_z＝-0.0267218586602667

W_z[1]·P₂₃[1]+P₂₃[2]·W_z[2]+P₂₃[3]-d_z＝0.0567166752875892

W_z[1]·P₂₄[1]+P₂₄[2]·W_z[2]+P₂₄[3]-d_z＝-0.0297226693094217

W_z[1]·P₂₅[1]+P₂₅[2]·W_z[2]+P₂₅[3]-d_z＝0.251842419044124

从以上的描述中，可以看出，本发明上述的实施例实现了如下技术效果：利用工业机器人采用距离检测传感器进行多面检测，检测辙叉不同面在机器人世界坐标系中的位置关系，采用特殊算法进行计算从而最终确定其空间位置。机器人可以准确测量辙叉上料后的空间位置，从而可以根据该空间位置对机器人程序进行补偿，保证能够准确对辙叉进行打磨。与现有技术相比，本发明的对辙叉的测量速度更快，测量操作更简单，提高了工作效率，降低了工作强度。

以上仅为本发明的优选实施例，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种利用机器人定位辙叉的测量方法，所述机器人的输出端上设有测距传感器，其特征在于，包括如下步骤：

S1：基于所述测距传感器的工具坐标系生成标准向量；

S2：对所述辙叉进行第一次定位，检测所述辙叉在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量；

S3：对所述辙叉进行第二次定位，选取所述辙叉的一个端点和与所述端点相邻的三个测量面，通过所述测距传感器对三个所述测量面和三个所述测量面之间的交点进行测量，计算所述交点在世界坐标系中的坐标位置；

S4：根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的标准工件坐标系W_obj；

S5：在所述辙叉被加工后重复步骤S3，并根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的补偿工件坐标系，计算所述补偿工件坐标系与所述标准工件坐标系W_obj之间的差值，并将所述差值补充到所述标准工件坐标系W_obj中；

在所述步骤S3中，通过所述测距传感器对三个所述测量面和三个所述测量面之间的交点进行测量包括：

S31：通过所述测距传感器测量每个所述测量面上的至少5个测定点；

S32：通过每个所述测量面上被测量的至少5个测定点计算出所述测量面在所述世界坐标系内的坐标；

S33：基于所述测量面在所述世界坐标系中的位置计算所述交点在所述世界坐标系的坐标；

在所述步骤S3中，所述测量面包括第一测量面、第二测量面和第三测量面，所述第一测量面上的所述测定点包括：第一测定点P₃₁、第二测定点P₃₂、第三测定点P₃₃、第四测定点P₃₄和第五测定点P₃₅，所述第一测定点P₃₁、所述第二测定点P₃₂、所述第三测定点P₃₃、所述第四测定点P₃₄和所述第五测定点P₃₅的平均值

所述第一测定点P₃₁与所述平均值P_average之间的差值P₁₀₁＝<P₃₁[1]-P_average[1]|P₃₁[2]-P_average[2]|P₃₁[3]-P_average[3]>

公式(2)

所述第二测定点P₃₂与所述平均值P_average之间的差值P₁₀₂＝<P₃₂[1]-P_average[1]|P₃₂[2]-P_average[2]|P₃₂[3]-P_average1[3]>

公式(3)

所述第三测定点P₃₃与所述平均值P_average之间的差值P₁₀₃＝<P₃₃[1]-P_average[1]|P₃₃[2]-P_average[2]|P₃₃[3]-P_average[3]>

公式(4)

所述第四测定点P₃₄与所述平均值P_average之间的差值P₁₀₄＝<P₃₄[1]-P_average[1]|P₃₄[2]-P_average[2]|P₃₄[3]-P_average[3]>

公式(5)

所述第五测定点P₃₅与所述平均值P_average之间的差值P₁₀₅＝<P₃₅[1]-P_average[1]|P₃₅[2]-P_average[2]|P₃₅[3]-P_average[3]>

公式(6)

矩阵F＝A·C 公式(7)

矩阵C＝<<P₁₀₁[1]|P₁₀₁[3]|-1>,<P₁₀₂[1]|P₁₀₂[3]|-1>,

<P₁₀₃[1]|P₁₀₃[3]|-1><P₁₀₄[1]|P₁₀₄[3]|-1>,<P₁₀₅[1]|P₁₀₅[3]|-1>>

公式(8)

矩阵E＝<-P₁₀₁[2],-P₁₀₂[2],-P₁₀₃[2],-P₁₀₄[2],-P₁₀₅[2]>

公式(9)

矩阵A＝C^T 公式(10)

矩阵B＝A·E 公式(11)

B＝<<(P₁₀₁[1]·P₁₀₁[1]+P₁₀₂[1]·P₁₀₂[1]+P₁₀₃[1]·P₁₀₃[1]+P₁₀₄[1]·P₁₀₄[1]+P₁₀₅[1]·P₁₀₅[1])|(P₁₀₁[1]·P₁₀₁[3]+P₁₀₂[1]·P₁₀₂[3]+P₁₀₃[1]·P₁₀₃[3]+P₁₀₄[1]·P₁₀₄[3]+P₁₀₅[1]·P₁₀₅[3])|P₁₀₁[1]+P₁₀₂[1]+P₁₀₃[1]+P₁₀₄[1]+P₁₀₅[1]>,<(P₁₀₁[3]·P₁₀₁[1]+P₁₀₂[3]·P₁₀₂[1]+P₁₀₃[3]·P₁₀₃[1]+P₁₀₄[3]·P₁₀₄[1]+P₁₀₅[3]·P₁₀₅[1])|(P₁₀₁[3]·P₁₀₁[3]+P₁₀₂[3]·P₁₀₂[3]+P₁₀₃[3]·P₁₀₃[3]+P₁₀₄[3]·P₁₀₄[3]+P₁₀₅[3]·P₁₀₅[3])|-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[4]+P₁₀₅[5])>,<-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[4]+P₁₀₅[5])|-(P₁₀₁[3]+P₁₀₂[3]+P₁₀₃[3]+P₁₀₄[3]+P₁₀₅[3])|(1+1+1+1+1)>> 公式(12)

F·X＝B 公式(13)

所述公式(14)中的X的求解函数L＝F[1][1]·(F[2][2]·F[3][3]+F[2][3]·F[3][2])+F[1][1]·(F[2][3]·F[3][1]-F[2][1]·F[3][3])+F[1][3]·(F[2][1]·F[3][2]-F[2][2]·F[3][1]) 公式(14)

令B[1]·(F[2][2]·F[3][3]+F[2][3]·F[3][2])+F[1][2]·(F[2][3]·B[3]-B[2]·F[3][3])+F[1][3]·(B[2]·F[3][2]-F[2][2]·F[2][2]·B[3])＝h[1]

公式(15)

令F[1][1]·(B[2]·F[3][3]+F[2][3]·B[3])+B[1]·(F[2][3]·F[3][1]-F[2][1]·F[3][3])+F[1][3]·(F[2][1]·B[3]-B[2]·F[3][1])＝h[2]

公式(16)

令F[1][1]·(B[2]·F[3][3]+F[2][3]·B[3])+F[1][1]·(B[2]·F[3][1]-F[2][1]·B[3])+B[1]·(F[2][1]·F[3][2]-F[2][2]·F[3][1])＝h[3]

公式(17)

所述第一测量面的表达式为：A₁x+B₁y+C₁z＝d_y 公式(18)

系数

系数

系数

常数d_y＝W_y[1]·(P_average[1])+(P_average[2])+W_y[2]·(P_average[3])-W_y[3] 公式(22)

所述第二测量面上的所述测定点包括：第六测定点P₁₁、第七测定点P₁₂、第八测定点P₁₃、第九测定点P₁₄、第十测定点P₁₅，所述第六测定点P₁₁、所述第七测定点P₁₂、所述第八测定点P₁₃、所述第九测定点P₁₄、所述第十测定点P₁₅的平均值

所述第六测定点P₁₁与所述平均值P_average2之间的差值P₁＝<P₁₁[1]-P_average2[1]|P₁₁[2]-P_average2[2]|P₁₁[3]-P_average2[3]>

公式(24)

所述第七测定点P₁₂与所述平均值P_average2之间的差值P₂＝<P₁₂[1]-P_average2[1]|P₁₂[2]-P_average2[2]|P₁₂[3]-P_average2[3]>

公式(25)

所述第八测定点P₁₃与所述平均值P_average2之间的差值P₃＝<P₁₃[1]-P_average2[1]|P₁₃[2]-P_average2[2]|P₁₃[3]-P_average2[3]>

公式(26)

所述第九测定点P₁₄与所述平均值P_average2之间的差值P₄＝<P₁₄[1]-P_average2[1]|P₁₄[2]-P_average2[2]|P₁₄[3]-P_average2[3]>

公式(27)

所述第十测定点P₁₅与所述平均值P_average2之间的差值P₅＝<P₁₅[1]-P_average2[1]|P₁₅[2]-P_average2[2]|P₁₅[3]-P_average2[3]>

公式(28)

矩阵C_x＝<<P₁[2]|P₁[3]|-1>，<P₂[2]|P₂[3]|-1>，

<P₃[2]|P₃[3]|-1><P₄[2]|P₄[3]|-1>，<P₅[2]|P₅[3]|-1>>

公式(29)

矩阵E_x＝<-P₁[1]，-P₂[1]，-P₃[1]，-P₄[1]，-P₅[1]>

公式(30)

矩阵A_x＝C_x ^T 公式(31)

矩阵B_x＝A_x·E_x 公式(32)

矩阵B_x＝<<-(P₁[1]·P₁[2]+P₂[1]·P₂[2]+P₃[1]·P₃[2]+P₄[1]·P₄[2]+P₅[1]·P₅[2])>，<-(P₁[2]·P₁[3]+P₂[2]·P₂[3]+P₃[2]·P₃[3]+P₄[2]·P₄[3]+P₅[2]·P₅[3])>，<P₁[2]+P₂[2]+P₃[2]+P₄[2]+P₅[2]>>

公式(33)

矩阵F_x＝A_x·C_x 公式(34)

F_x＝<<(P₁[1]·P₁[1]+P₂[1]·P₂[1]+P₃[1]·P₃[1]+P₄[1]·P₄[1]+P₅[1]·P₅[1])|(P₁[1]·P₁[3]+P₂[1]·P₂[3]+P₃[1]·P₃[3]+P₄[1]·P₄[3]+P₅[1]·P₅[3])|P₁[1]+P₂[1]+P₃[1]+P₄[1]+P₅[1]>，<(P₁[3]·P₁[1]+P₂[3]·P₂[1]+P₃[3]·P₃[1]+P₄[3]·P₄[1]+P₅[3]·P₅[1])|(P₁[3]·P₁[3]+P₂[3]·P₂[3]+P₃[3]·P₃[3]+P₄[3]·P₄[3]+P₅[3]·P₅[3])|-(P₁[3]+P₂[3]+P₃[3]+P₄[4]+P₅[5])>，<-(P₁[3]+P₂[3]+P₃[3]+P₄[4]+P₅[5])|-(P₁[3]+P₂[3]+P₃[3]+P₄[3]+P₅[3])|(1+1+1+1+1)>> 公式(35)

F_x·X＝B_x 公式(36)

所述公式(36)中的X的求解函数L_x＝F_x[1][1]·(F_x[2][2]·F_x[3][3]+F_x[2][3]·F_x[3][2])+F_x[1][2]·(F_x[2][3]·F_x[3][1]-F_x[2][1]·F_x[3][3])+F_x[1][3]·(F_x[2][1]·F_x[3][2]-F_x[2][2]·F_x[3][1])

公式(37)

令B_x[1]·(F_x[2][2]·F_x[3][3]+F_x[2][3]·F_x[3][2])+F_x[1][2]·(F_x[2][3]·B[3]-B_x[2]·F_x[3][3])+F_x[1][3]·(B_x[2]·F_x[3][2]-F_x[2][2]·F_x[2][2]·B_x[3])＝h_x[1]

公式(38)

令F_x[1][1]·(B_x[2]·F_x[3][3]+F_x[2][3]·B_x[3])+B_x[1]·(F_x[2][3]·F_x[3][1]-F_x[2][1]·F[3][3])+F_x[1][3]·(F_x[2][1]·B_x[3]-B[2]·F_x[3][1])＝h_x[2] 公式(39)

令F_x[1][1]·(B_x[2]·F_x[3][3]+F_x[2][3]·B_x[3])+F_x[1][1]·(B_x[2]·F_x[3][1]-F_x[2][1]·B_x[3])+B_x[1]·(F_x[2][1]·F_x[3][2]-F_x[2][2]·F_x[3][1])＝h_x[3] 公式(40)

所述第二测量面的表达式为：A₂x+B₂y+C₃z＝d_x 公式(41)

系数

系数

系数

常数d_x＝W_x[1]·(P_average2[2])+(P_average2[1])+W_x[2]·(P_average2[3])-W_x[3] 公式(45)

所述第三测量面上的所述测定点包括：第十一测定点P₂₁、第十二测定点P₂₂、第十三测定点P₂₃、第十四测定点P₂₄和第十五测定点P₂₅，所述第十一测定点P₂₁、所述第十二测定点P₂₂、所述第十三测定点P₂₃、所述第十四测定点P₂₄和所述第十五测定点P₂₅的平均值

所述第十一测定点P₂₁与所述平均值P_average3之间的差值P₆＝<P₂₁[1]-P_average3[1]|P₂₁[2]-P_average3[2]|P₂₁[3]-P_average3[3]>

公式(47)

所述第十二测定点P₂₂与所述平均值P_average3之间的差值P₇＝<P₂₂[1]-P_average3[1]|P₂₂[2]-P_average3[2]|P₂₂[3]-P_average3[3]>

公式(48)

所述第十三测定点P₂₃与所述平均值P_average3之间的差值P₈＝<P₂₃[1]-P_average3[1]|P₂₃[2]-P_average3[2]|P₂₃[3]-P_average3[3]>

公式(49)

所述第十四测定点P₂₄与所述平均值P_average3之间的差值P₉＝<P₂₄[1]-P_average3[1]|P₂₄[2]-P_average3[2]|P₂₄[3]-P_average3[3]>

公式(50)

所述第十五测定点P₂₅与所述平均值P_average3之间的差值P₁₀＝<P₂₅[1]-P_average3[1]|P₂₅[2]-P_average3[2]|P₂₅[3]-P_average3[3]>

公式(51)

矩阵C_z＝<<P₆[1]|P₆[2]|-1>，<P₇[1]|P₇[2]|-1>，<P₈[1]|P₈[2]|-1><P₉[1]|P₉[2]|-1>，<P₁₀[1]|P₁₀[2]|-1>>

公式(52)

矩阵E_z＝<-P₆[3]，-P₇[3]，-P₈[3]，-P₉[3]，-P₁₀[3]> 公式(53)

矩阵A_z＝C_z ^T 公式(54)

矩阵B_z＝A_z·E_z 公式(55)

矩阵B_z＝<<-(P₆[1]·P₆[2]+P₇[1]·P₇[2]+P₈[1]·P₈[2]+P₉[1]·P₉[2]+P₁₀[1]·P₁₀[2])>，<-(P₆[2]·P₆[3]+P₇[2]·P₇[3]+P₈[2]·P₈[3]+P₉[2]·P₉[3]+P₁₀[2]·P₁₀[3])>，<P₆[2]+P₇[2]+P₈[2]+P₉[2]+P₁₀[2]>>

公式(56)

矩阵F_z＝A_z·C_z 公式(57)

矩阵F_z＝<<(P₆[1]·P₆[1]+P₇[1]·P₇[1]+P₈[1]·P₈[1]+P₉[1]·P₉[1]+P₁₀[1]·P₁₀[1])|(P₆[1]·P₆[3]+P₇[1]·P₇[3]+P₈[1]·P₈[3]+P₉[1]·P₉[3]+P₁₀[1]·P₁₀[3])|P₆[1]+P₇[1]+P₈[1]+P₉[1]+P₁₀[1]>，<(P₆[3]·P₆[1]+P₇[3]·P₇[1]+P₈[3]·P₈[1]+P₉[3]·P₉[1]+P₁₀[3]·P₁₀[1])|(P₆[3]·P₆[3]+P₇[3]·P₇[3]+P₈[3]·P₈[3]+P₉[3]·P₉[3]+P₁₀[3]·P₁₀[3])|-(P₆[3]+P₇[3]+P₈[3]+P₉[4]+P₁₀[5])>，<-(P₆[3]+P₇[3]+P₈[3]+P₉[4]+P₁₀[5])|-(P₆[3]+P₇[3]+P₈[3]+P₉[3]+P₁₀[3])|(1+1+1+1+1)>>

公式(58)

F_z·X＝B_z 公式(59)

所述公式(59)中的X的求解函数L_z＝F_z[1][1]·(F_z[2][2]·F_z[3][3]+F_z[2][3]·F_z[3][2])+F_z[1][2]·(F_z[2][3]·F_z[3][1]-F_z[2][1]·F_z[3][3])+F_z[1][3]·(F_z[2][1]·F_z[3][2]-F_z[2][2]·F_z[3][1])

公式(60)

令B_z[1]·(F_z[2][2]·F_z[3][3]+F_z[2][3]·F_z[3][2])+F_z[1][2]·(F_z[2][3]·B_z[3]-B_z[2]·F_z[3][3])+F_z[1][3]·(B_z[2]·F_z[3][2]-F_z[2][2]·F_z[2][2]·B_z[3])＝h_z[1]

公式(61)

令F_z[1][1]·(B_z[2]·F_z[3][3]+F_z[2][3]·B_z[3])+B_z[1]·(F_z[2][3]·F_z[3][1]-F_z[2][1]·F_z[3][3])+F_z[1][3]·(F_z[2][1]·B_z[3]-B_z[2]·F_z[3][1])＝h_z[2] 公式(62)

令F_z[1][1]·(B_z[2]·F_z[3][3]+F_z[2][3]·B_z[3])+F_z[1][1]·(B_z[2]·F_z[3][1]-F_z[2][1]·B_z[3])+B_z[1]·(F_z[2][1]·F_z[3][2]-F_z[2][2]·F_z[3][1])＝h_z[3] 公式(63)

所述第三测量面的表达式为：A₃x+B₃y+C₃z＝d_z 公式(64)

系数

系数

系数

常数d_z＝W_z[1]·(P_average3[1])+W_z[2]·(P_average3[2])+(P_average3[3])-W_z[3] 公式(68)

所述第一测量面、所述第二测量面和所述第三测量面的交点

H：＝<<d_x>，<d_y>，<d_z>> 公式(69)

所述交点H为所述标准工件坐标系W_obj的原点。

2.根据权利要求1所述的一种利用机器人定位辙叉的测量方法，其特征在于，步骤S4包括：

所述标准工件坐标系W_obj的Z轴的方向向量Z_wobj为所述第二测量面的法向量A₁和所述第三测量面的法向量A₂的向量积的方向，所述第三测量面的法向量A₂的方向为所述标准工件坐标系W_obj的X轴的方向向量，所述标准工件坐标系W_obj的Y轴的方向向量

Y_wobj＝Z_wobj×A₂ 公式(70)。

3.根据权利要求1所述的一种利用机器人定位辙叉的测量方法，其特征在于，所述测距传感器为激光传感器，所述测距传感器与所述辙叉之间的距离不大于100mm。

4.根据权利要求3所述的一种利用机器人定位辙叉的测量方法，其特征在于，所述标准向量的方向与所述测距传感器的校准激光中心点的方向相同。

5.根据权利要求1所述的一种利用机器人定位辙叉的测量方法，其特征在于，所述检测所述辙叉在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量包括：

S21：通过工装夹具固定所述辙叉；

S21：选取所述辙叉相邻的两个平面，且两个所述平面均与所述辙叉的底面相邻；

S22：测量两个所述平面在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量。

Images