一种利用机器人定位辙叉的测量方法
阅读说明：本技术 一种利用机器人定位辙叉的测量方法 (Measuring method for positioning frog by using robot ) 是由 崔洪亮 王瑞成 李超 宋志伟 李天伟 夏武强 于 2020-12-29 设计创作，主要内容包括：本发明提供一种利用机器人定位辙叉的测量方法,所述机器人的输出端上设有测距传感器,其特征在于,包括如下步骤：基于所述测距传感器的工具坐标系生成标准向量；对所述辙叉进行第一次定位,检测所述辙叉在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量；计算所述交点在世界坐标系中的坐标位置；根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的标准工件坐标系W-(obj),通过机器人可以准确测量辙叉上料后的空间位置,从而可以根据该空间位置对机器人程序进行补偿,保证能够准确对辙叉进行打磨。本发明的技术方案降低铁路辙叉的测量时间,提高工作效率,减少工作强度。(The invention provides a measuring method for positioning a frog by using a robot, wherein a distance measuring sensor is arranged on an output end of the robot, and the measuring method is characterized by comprising the following steps of: generating a standard vector based on a tool coordinate system of the ranging sensor; positioning the frog for the first time, and detecting the offset of the frog in the X-axis direction and the offset of the frog in the Y-axis direction in a world coordinate system; calculating the coordinate position of the intersection point in a world coordinate system; calculating a standard workpiece coordinate system W of the frog according to the coordinate positions of the intersection points in the world coordinate system obj The spatial position of the frog after being fed can be accurately measured through the robot, so that the robot program can be compensated according to the spatial position, and the frog can be accurately polished. The technical scheme of the invention reduces the measuring time of the railway frog, improves the working efficiency and reduces the working strength.)
技术领域
本发明涉及工业测量技术领域，特别涉及一种利用机器人定位辙叉的测量方法。
背景技术
铁路辙叉是使火车车轮由一股线路转换到另一股路线的轨线平面交叉部件，该部件为消耗品，当磨损到一定程度后就需要进行更换，因此该部件有大量的生产需求。该铁路辙叉的两端需要与钢轨进行精确装配，与钢轨配合的装配面有较高的尺寸精度要求，传统的方式是通过人工打磨方式来实现尺寸要求。该方式对工人的技术水平要求较高，不仅生产效率低下，打磨产生的粉尘对人体也有很大伤害，因此我们开发了机器人打磨技术，但是由于辙叉为铸造件，外形尺寸最长达4米，重将近1吨，自身尺寸偏差很大，在机器人打磨系统中很难通过装卡的方式保证辙叉的空间位置。
发明内容
本发明的目的在于提供一种利用机器人定位辙叉的测量方法，用于测量人工上料后的辙叉相对于工业机器人的空间位置，以保证机器人对辙叉进行准确打磨。
为了实现上述目的，本发明提供如下技术方案：一种利用机器人定位辙叉的测量方法，所述机器人的输出端上设有测距传感器，其特征在于，包括如下步骤：
S1：基于所述测距传感器的工具坐标系生成标准向量；
S2：对所述辙叉进行第一次定位，检测所述辙叉在世界坐标系中的X轴方向的偏移量和Y轴方向的偏移量；
S3：对所述辙叉进行第二次定位，选取所述辙叉的一个端点和与所述端点相邻的三个测量面，通过所述测距传感器对三个所述测量面和三个所述测量面之间的交点进行测量，通过计算所述交点在世界坐标系中的坐标位置；
S4：根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的标准工件坐标系Wobj。
进一步地，还包括步骤S5：在所述辙叉被加工后重复步骤S3，并根据所述交点在世界坐标系中的坐标位置计算出所述辙叉的补偿工件坐标系，计算所述补偿工件坐标系与所述标准工件坐标系Wobj之间的差值，并将所述差值补充到所述标准工件坐标系Wobj中。
进一步地，在所述步骤S3中，所述通过所述测距传感器对三个所述测量面和三个所述测量面之间的交点进行测量包括：
S31：通过所述测距传感器测量每个所述测量面上的至少5个测定点；
S32：通过每个所述测量面上被测量的至少5个测定点计算出所述测量面在所述世界坐标系内的坐标；
S33：基于所述测量面在所述世界坐标系中的位置计算所述交点在所述世界坐标系的坐标。
进一步地，在所述步骤S3中，所述测量面包括第一测量面、第二测量面和第三测量面，所述第一测量面上的所述测定点包括：第一测定点P31、第二测定点P32、第三测定点P33、第四测定点P34和第五测定点P35，所述第一测定点P31、所述第二测定点P32、所述第三测定点P33、所述第四测定点P34和所述第五测定点P35的平均值
所述第一测定点P31与所述平均值Paverage之间的差值P101＝<P31[1]-Paverage[1]|P31[2]-Paverage[2]|P31[3]-Paverage[3]> 公式(2)
所述第二测定点P32与所述平均值Paverage之间的差值P102＝<P32[1]-Paverage[1]|P32[2]-Paverage[2]|P32[3]-Paverage1[3]> 公式(3)
所述第三测定点P33与所述平均值Paverage之间的差值P103＝<P33[1]-Paverage[1]|P33[2]-Paverage[2]|P33[3]-Paverage[3]> 公式(4)
所述第四测定点P34与所述平均值Paverage之间的差值P104＝<P34[1]-Paverage[1]|P34[2]-Paverage[2]|P34[3]-Paverage[3]> 公式(5)
所述第五测定点P35与所述平均值Paverage之间的差值P105＝<P35[1]-Paverage[1]|P35[2]-Paverage[2]|P35[3]-Paverage[3]> 公式(6)
矩阵F＝A·C 公式(7)
矩阵C＝<<P101[1]|P101[1]|-1>，<P102[1]|P102[1]|-1>，<P103[1]|P103[1]|-1><P104[1]|P104[1]|-1>，<P105[1]|P105[1]|-1>> 公式(8)
矩阵E＝<-P101[2]，-P102[2]，-P103[2]，-P104[2]，-P105[2]> 公式(9)
矩阵A＝CT 公式(10)
矩阵B＝A·E 公式(11)
B＝<<(P101[1]·P101[1]+P102[1]·P102[1]+P103[1]·P103[1]+P104[1]·P104[1]+P105[1]·P105[1])|(P101[1]·P101[3]+P102[1]·P102[3]+P103[1]·P103[3]+P104[1]·P104[3]+P105[1]·P105[3])|P101[1]+P102[1]+P103[1]+P104[1]+P105[1]>，<(P101[3]·P101[1]+P102[3]·P102[1]+P103[3]·P103[1]+P104[3]·P104[1]+P105[3]·P105[1])|(P101[3]·P101[3]+P102[3]·P102[3]+P103[3]·P103[3]+P104[3]·P104[3]+P105[3]·P105[3])|-(P101[3]+P102[3]+P103[3]+P104[4]+P105[5])>，<-(P101[3]+P102[3]+P103[3]+P104[4]+P105[5])|-(P101[3]+P102[3]+P103[3]+P104[3]+P105[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)
所述第一测量面的表达式为：A1x+B1y+C1z＝dy 公式(18)
系数
系数
系数
常数dy＝Wy[1]·(Paverage[1])+(Paverage[2])+Wy[2]·(Paverage[3])-Wy[3] 公式(22)
所述第二测量面上的所述测定点包括：第六测定点P11、第七测定点P12、第八测定点P13、第九测定点P14、第十测定点P15，所述第六测定点P11、所述第七测定点P12、所述第八测定点P13、所述第九测定点P14、所述第十测定点P15的平均值
所述第六测定点P11与所述平均值Paverage2之间的差值P1＝<P11[1]-Paverage2[1]|P11[2]-Paverage2[2]|P11[3]-Paverage2[3]> 公式(24)
所述第七测定点P12与所述平均值Paverage2之间的差值P2＝<P12[1]-Paverage2[1]|P12[2]-Paverage2[2]|P12[3]-Paverage2[3]> 公式(25)
所述第八测定点P13与所述平均值Paverage2之间的差值P3＝<P13[1]-Paverage2[1]|P13[2]-Paverage2[2]|P13[3]-Paverage2[3]> 公式(26)
所述第九测定点P14与所述平均值Paverage2之间的差值P4＝<P14[1]-Paverage2[1]|P14[2]-Paverage2[2]|P14[3]-Paverage2[3]> 公式(27)
所述第十测定点P15与所述平均值Paverage2之间的差值P5＝<P15[1]-Paverage2[1]|P15[2]-Paverage2[2]|P15[3]-Paverage2[3]> 公式(28)
矩阵Cx＝<<P1[1]|P1[1]|-1>，<P2[1]|P2[1]|-1>，<P3[1]|P3[1]|-1><P4[1]|P4[1]|-1>，<P5[1]|P5[1]|-1>> 公式(29)
矩阵Ex＝<-P1[2]，-P2[2]，-P3[2]，-P4[2]，-P5[2]> 公式(30)
矩阵Ax＝Cx T 公式(31)
矩阵Bx＝Ax·Ex 公式(32)
矩阵Bx＝<<-(P1[1]·P1[2]+P2[1]·P2[2]+P3[1]·P3[12]+P4[1]·P4[2]+P5[1]·P5[2])>，<-(P1[2]·P1[3]+P2[2]·P2[3]+P3[2]·P3[3]+P4[2]·P4[3]+P5[2]·P5[3])>，<P1[2]+P2[2]+P3[2]+P4[2]+P5[2]>> 公式(33)
矩阵Fx＝Ax·Cx 公式(34)
Fx＝<<(P1[1]·P1[1]+P2[1]·P2[1]+P3[1]·P3[1]+P4[1]·P4[1]+P5[1]·P5[1])|(P1[1]·P1[3]+P2[1]·P2[3]+P3[1]·P3[3]+P4[1]·P4[3]+P5[1]·P5[3])|P1[1]+P2[1]+P3[1]+P4[1]+P5[1]>，<(P1[3]·P1[1]+P2[3]·P2[1]+P3[3]·P3[1]+P4[3]·P4[1]+P5[3]·P5[1])|(P1[3]·P1[3]+P2[3]·P2[3]+P3[3]·P3[3]+P4[3]·P4[3]+P5[3]·P5[3])|-(P1[3]+P2[3]+P3[3]+P4[4]+P5[5])>，<-(P1[3]+P2[3]+P3[3]+P4[4]+P5[5])|-(P1[3]+P2[3]+P3[3]+P4[3]+P5[3])|(1+1+1+1+1)>> 公式(35)
Fx·X＝Bx 公式(36)
所述公式(36)中的X的求解函数Lx＝Fx[1][1]·(Fx[2][2]·Fx[3][3]+Fx[2][3]·Fx[3][2])+Fx[1][2]·(Fx[2][3]·Fx[3][1]-Fx[2][1]·Fx[3][3])+Fx[1][3]·(Fx[2][1]·Fx[3][2]-Fx[2][2]·Fx[3][1]) 公式(37)
令Bx[1]·(Fx[2][2]·Fx[3][3]+Fx[2][3]·Fx[3][2])+Fx[1][2]·(Fx[2][3]·B[3]-Bx[2]·Fx[3][3])+Fx[1][3]·(Bx[2]·Fx[3][2]-Fx[2][2]·Fx[2][2]·Bx[3])＝hx[1] 公式(38)
令Fx[1][1]·(Bx[2]·Fx[3][3]+Fx[2][3]·Bx[3])+Bx[1]·(Fx[2][3]·Fx[3][1]-Fx[2][1]·F[3][3])+Fx[1][3]·(Fx[2][1]·Bx[3]-B[2]·Fx[3][1])＝hx[2] 公式(39)
令Fx[1][1]·(Bx[2]·Fx[3][3]+Fx[2][3]·Bx[3])+Fx[1][1]·(Bx[2]·Fx[3][1]-Fx[2][1]·Bx[3])+Bx[1]·(Fx[2][1]·Fx[3][2]-Fx[2][2]·Fx[3][1])＝hx[3] 公式(40)
所述第二测量面的表达式为：A2x+B2y+C3z＝dx 公式(41)
系数
系数
系数常数dx＝Wx[1]·(Paverage2[2])+(Paverage2[1])+Wx[2]·(Paverage2[3])-Wx[3] 公式(45)
所述第三测量面上的所述测定点包括：第十一测定点P21、第十二测定点P22、第十三测定点P23、第十四测定点P24和第十五测定点P25，所述第十一测定点P21、所述第十二测定点P22、所述第十三测定点P23、所述第十四测定点P24和所述第十五测定点P25的平均值
所述第十一测定点P21与所述平均值Paverage3之间的差值P6＝<P21[1]-Paverage3[1]|P21[2]-Paverage3[2]|P21[3]-Paverage3[3]> 公式(47)
所述第十二测定点P22与所述平均值Paverage3之间的差值P7＝<P22[1]-Paverage3[1]|P12[2]-Paverage3[2]|P22[3]-Paverage3[3]> 公式(48)
所述第十三测定点P23与所述平均值Paverage3之间的差值P8＝<P23[1]-Paverage3[1]|P23[2]-Paverage3[2]|P23[3]-Paverage3[3]> 公式(49)
所述第十四测定点P24与所述平均值Paverage3之间的差值P9＝<P24[1]-Paverage3[1]|P24[2]-Paverage3[2]|P24[3]-Paverage3[3]> 公式(50)
所述第十五测定点P25与所述平均值Paverage3之间的差值P10＝<P25[1]-Paverage3[1]|P25[2]-Paverage3[2]|P25[3]-Paverage3[3]> 公式(51)
矩阵Cz＝<<P6[1]|P6[1]|-1>，<P7[1]|P7[1]|-1>，<P8[1]|P8[1]|-1><P9[1]|P9[1]|-1>，<P10[1]|P10[1]|-1>> 公式(52)
矩阵Ez＝<-P6[2]，-P7[2]，-P8[2]，-P9[2]，-P10[2]>(53)
矩阵Az＝Cz T 公式(54)
矩阵Bz＝Az·Ez 公式(55)
矩阵Bz＝<<-(P6[1]·P5[2]+P7[1]·P7[2]+P8[1]·P8[12]+P9[1]·P9[2]+P10[1]·P10[2])>，<-(P6[2]·P6[3]+P7[2]·P7[3]+P8[2]·P8[3]+P9[2]·P9[3]+P10[2]·P10[3])>，<P6[2]+P7[2]+P8[2]+P9[2]+P10[2]>> 公式(56)
矩阵Fz＝Az·Cz 公式(57)
矩阵Fz＝<<(P6[1]·P6[1]+P7[1]·P7[1]+P8[1]·P8[1]+P9[1]·P9[1]+P10[1]·P10[1])|(P6[1]·P6[3]+P7[1]·P7[3]+P8[1]·P8[3]+P9[1]·P9[3]+P10[1]·P10[3])|P6[1]+P7[1]+P8[1]+P9[1]+P10[1]>，<(P6[3]·P6[1]+P7[3]·P7[1]+P8[3]·P8[1]+P9[3]·P9[1]+P10[3]·P10[1])|(P6[3]·P6[3]+P7[3]·P7[3]+P8[3]·P8[3]+P9[3]·P9[3]+P10[3]·P10[3])|-(P6[3]+P7[3]+P8[3]+P9[4]+P10[5])>，<-(P6[3]+P7[3]+P8[3]+P9[4]+P10[5])|-(P6[3]+P7[3]+P8[3]+P9[3]+P10[3])|(1+1+1+1+1)>> 公式(58)
Fz·X＝Bz 公式(59)
所述公式(59)中的X的求解函数Lz＝Fz[1][1]·(Fz[2][2]·Fz[3][3]+Fz[2][3]·Fz[3][2])+Fz[1][2]·(Fz[2][3]·Fz[3][1]-Fz[2][1]·Fz[3][3])+Fz[1][3]·(Fz[2][1]·Fz[3][2]-Fz[2][2]·Fz[3][1]) 公式(60)
令Bz[1]·(Fz[2][2]·Fz[3][3]+Fz[2][3]·Fz[3][2])+Fz[1][2]·(Fz[2][3]·Bz[3]-Bz[2]·Fz[3][3])+Fz[1][3]·(Bz[2]·Fz[3][2]-Fz[2][2]·Fz[2][2]·Bz[3])＝hz[1] 公式(61)
令Fz[1][1]·(Bz[2]·Fz[3][3]+Fz[2][3]·Bz[3])+Bz[1]·(Fz[2][3]·Fz[3][1]-Fz[2][1]·Fz[3][3])+Fz[1][3]·(Fz[2][1]·Bz[3]-Bz[2]·Fz[3][1])hz[2]＝ 公式(62)
令hz[3]＝Fz[1][1]·(Bz[2]·Fz[3][3]+Fz[2][3]·Bz[3])+Fz[1][1]·(Bz[2]·Fz[3][1]-Fz[2][1]·Bz[3])+Bz[1]·(Fz[2][1]·Fz[3][2]-Fz[2][2]·Fz[3][1]) 公式(63)
所述第三测量面的表达式为：A3x+B3y+C3z＝dz 公式(64)
系数
系数
系数
常数dz＝Wz[1]·(Paverage3[1])+Wz[2]·(Paverage3[2])+(Paverage3[3])-Wz[3] 公式(68)
所述第一测量面、所述第二测量面和所述第三测量面的交点
H：＝<<dx>，<dy>，<dz>> 公式(69)。
进一步地，步骤S4包括：所述标准工件坐标系Wobj的Z轴的方向向量Zwobj为所述第二测量面的法向量A1和所述第三测量面的法向量A2的向量积的方向，所述第三测量面的法向量A2的方向为所述标准工件坐标系Wobj的X轴的方向向量，所述标准工件坐标系Wobj的Y轴的方向向量
Ywobj＝Zwobj×A2 公式(70)
所述交点H为所述标准工件坐标系Wobj的原点。
进一步地，所述测距传感器为激光传感器，所述测距传感器与所述辙叉之间的距离不大于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的标准工件坐标系Wobj。
标准工件坐标系Wobj的Z轴的方向向量Zwobj为第二测量面2的法向量A1和第三测量面4的法向量A2的向量积的方向，第三测量面4的法向量A2的方向为标准工件坐标系Wobj的X轴的方向向量，标准工件坐标系Wobj的Y轴的方向向量
Ywobj＝Zwobj×A2 公式(70)
交点H为标准工件坐标系Wobj的原点。
S5：在辙叉1被加工后重复S3，并根据交点在世界坐标系中的坐标位置计算出辙叉1的补偿工件坐标系，计算补偿工件坐标系与标准工件坐标系Wobj之间的差值，并将差值补充到标准工件坐标系Wobj中
优选地，测量面包括第一测量面3、第二测量面2和第三测量面4，述第一测量面3上的测定点包括：第一测定点P31、第二测定点P32、第三测定点P33、第四测定点P34和第五测定点P35，第一测定点P31、第二测定点P32、第三测定点P33、第四测定点P34和第五测定点P35的平均值
所述第一测定点P31与所述平均值Paverage之间的差值P101＝<P31[1]-Paverage[1]|P31[2]-Paverage[2]|P31[3]-Paverage[3]> 公式(2)
所述第二测定点P32与所述平均值Paverage之间的差值P102＝<P32[1]-Paverage[1]|P32[2]-Paverage[2]|P32[3]-Paverage1[3]> 公式(3)
所述第三测定点P33与所述平均值Paverage之间的差值P103＝<P33[1]-Paverage[1]|P33[2]-Paverage[2]|P33[3]-Paverage[3]> 公式(4)
所述第四测定点P34与所述平均值Paverage之间的差值P104＝<P34[1]-Paverage[1]|P34[2]-Paverage[2]|P34[3]-Paverage[3]> 公式(5)
所述第五测定点P35与所述平均值Paverage之间的差值P105＝<P35[1]-Paverage[1]|P35[2]-Paverage[2]|P35[3]-Paverage[3]> 公式(6)
矩阵F＝A·C 公式(7)
矩阵C＝<<P101[1]|P101[1]|-1>，<P102[1]|P102[1]|-1>，
<P103[1]|P103[1]|-1><P104[1]|P104[1]|-1>，<P105[1]|P105[1]|-1>> 公式(8)
矩阵E＝<-P101[2]，-P102[2]，-P103[2]，-P104[2]，-P105[2]> 公式(9)
矩阵A＝CT 公式(10)
矩阵B＝A·E 公式(11)
B＝<<(P101[1]·P101[1]+P102[1]·P102[1]+P103[1]·P103[1]+P104[1]·P104[1]+P105[1]·P105[1])|(P101[1]·P101[3]+P102[1]·P102[3]+P103[1]·P103[3]+P104[1]·P104[3]+P105[1]·P105[3])|P101[1]+P102[1]+P103[1]+P104[1]+P105[1]>，<(P101[3]·P101[1]+P102[3]·P102[1]+P103[3]·P103[1]+P104[3]·P104[1]+P105[3]·P105[1])|(P101[3]·P101[3]+P102[3]·P102[3]+P103[3]·P103[3]+P104[3]·P104[3]+P105[3]·P105[3])|-(P101[3]+P102[3]+P103[3]+P104[4]+P105[5])>，<-(P101[3]+P102[3]+P103[3]+P104[4]+P105[5])|-(P101[3]+P102[3]+P103[3]+P104[3]+P105[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)
所述第一测量面的表达式为：A1x+B1y+C1z＝dy 公式(18)
系数
系数
系数
常数dy＝Wy[1]·(Paverage[1])+(Paverage[2])+Wy[2]·(Paverage[3])-Wy[3] 公式(22)
所述第二测量面上的所述测定点包括：第六测定点P11、第七测定点P12、第八测定点P13、第九测定点P14、第十测定点P15，所述第六测定点P11、所述第七测定点P12、所述第八测定点P13、所述第九测定点P14、所述第十测定点P15的平均值
所述第六测定点P11与所述平均值Paverage2之间的差值P1＝<P11[1]-Paverage2[1]|P11[2]-Paverage2[2]|P11[3]-Paverage2[3]> 公式(24)
所述第七测定点P12与所述平均值Paverage2之间的差值P2＝<P12[1]-Paverage2[1]|P12[2]-Paverage2[2]|P12[3]-Paverage2[3]> 公式(25)
所述第八测定点P13与所述平均值Paverage2之间的差值P3＝<P13[1]-Paverage2[1]|P13[2]-Paverage2[2]|P13[3]-Paverage2[3]> 公式(26)
所述第九测定点P14与所述平均值Paverage2之间的差值P4＝<P14[1]-Paverage2[1]|P14[2]-Paverage2[2]|P14[3]-Paverage2[3]> 公式(27)
所述第十测定点P15与所述平均值Paverage2之间的差值P5＝<P15[1]-Paverage2[1]|P15[2]-Paverage2[2]|P15[3]-Paverage2[3]> 公式(28)
矩阵Cx＝<<P1[1]|P1[1]|-1>，<P2[1]|P2[1]|-1>，<P3[1]|P3[1]|-1><P4[1]|P4[1]|-1>，<P5[1]|P5[1]|-1>> 公式(29)
矩阵Ex＝<-P1[2]，-P2[2]，-P3[2]，-P4[2]，-P5[2]> 公式(30)
矩阵Ax＝Cx T 公式(31)
矩阵Bx＝Ax·Ex 公式(32)
矩阵Bx＝<<-(P1[1]·P1[2]+P2[1]·P2[2]+P3[1]·P3[12]+P4[1]·P4[2]+P5[1]·P5[2])>，<-(P1[2]·P1[3]+P2[2]·P2[3]+P3[2]·P3[3]+P4[2]·P4[3]+P5[2]·P5[3])>，<P1[2]+P2[2]+P3[2]+P4[2]+P5[2]>> 公式(33)
矩阵Fx＝Ax·Cx 公式(34)
Fx＝<<(P1[1]·P1[1]+P2[1]·P2[1]+P3[1]·P3[1]+P4[1]·P4[1]+P5[1]·P5[1])|(P1[1]·P1[3]+P2[1]·P2[3]+P3[1]·P3[3]+P4[1]·P4[3]+P5[1]·P5[3])|P1[1]+P2[1]+P3[1]+P4[1]+P5[1]>，<(P1[3]·P1[1]+P2[3]·P2[1]+P3[3]·P3[1]+P4[3]·P4[1]+P5[3]·P5[1])|(P1[3]·P1[3]+P2[3]·P2[3]+P3[3]·P3[3]+P4[3]·P4[3]+P5[3]·P5[3])|-(P1[3]+P2[3]+P3[3]+P4[4]+P5[5])>，<-(P1[3]+P2[3]+P3[3]+P4[4]+P5[5])|-(P1[3]+P2[3]+P3[3]+P4[3]+P5[3])|(1+1+1+1+1)>> 公式(35)
Fx·X＝Bx 公式(36)
所述公式(36)中的X的求解函数Lx＝Fx[1][1]·(Fx[2][2]·Fx[3][3]+Fx[2][3]·Fx[3][2])+Fx[1][2]·(Fx[2][3]·Fx[3][1]-Fx[2][1]·Fx[3][3])+Fx[1][3]·(Fx[2][1]·Fx[3][2]-Fx[2][2]·Fx[3][1]) 公式(37)
令Bx[1]·(Fx[2][2]·Fx[3][3]+Fx[2][3]·Fx[3][2])+Fx[1][2]·(Fx[2][3]·B[3]-Bx[2]·Fx[3][3])+Fx[1][3]·(Bx[2]·Fx[3][2]-Fx[2][2]·Fx[2][2]·Bx[3])＝hx[1] 公式(38)
令Fx[1][1]·(Bx[2]·Fx[3][3]+Fx[2][3]·Bx[3])+Bx[1]·(Fx[2][3]·Fx[3][1]-Fx[2][1]·F[3][3])+Fx[1][3]·(Fx[2][1]·Bx[3]-B[2]·Fx[3][1])＝hx[2] 公式(39)
令Fx[1][1]·(Bx[2]·Fx[3][3]+Fx[2][3]·Bx[3])+Fx[1][1]·(Bx[2]·Fx[3][1]-Fx[2][1]·Bx[3])+Bx[1]·(Fx[2][1]·Fx[3][2]-Fx[2][2]·Fx[3][1])＝hx[3] 公式(40)
所述第二测量面的表达式为：A2x+B2y+C3z＝dx 公式(41)
系数
系数
系数
常数dx＝Wx[1]·(Paverage2[2])+(Paverage2[1])+Wx[2]·(Paverage2[3])-Wx[3] 公式(45)
所述第三测量面上的所述测定点包括：第十一测定点P21[1]、第十二测定点P22[1]、第十三测定点P23[1]、第十四测定点P24[1]和第十五测定点P25[1]，所述第十一测定点P21[1]、所述第十二测定点P22[1]、所述第十三测定点P23[1]、所述第十四测定点P24[1]和所述第十五测定点P25[1]的平均值
所述第十一测定点P21[1]与所述平均值Paverage3之间的差值P6＝<P21[1]-Paverage3[1]|P21[2]-Paverage3[2]|P21[3]-Paverage3[3]> 公式(47)
所述第十二测定点P22[1]与所述平均值Paverage3之间的差值P7＝<P22[1]-Paverage3[1]|P22[2]-Paverage3[2]|P22[3]-Paverage3[3]> 公式(48)
所述第十三测定点P23[1]与所述平均值Paverage3之间的差值P8＝<P23[1]-Paverage3[1]|P23[2]-Paverage3[2]|P23[3]-Paverage3[3]> 公式(49)
所述第十四测定点P24[1]与所述平均值Paverage3之间的差值P9＝<P24[1]-Paverage3[1]|P24[2]-Paverage3[2]|P24[3]-Paverage3[3]> 公式(50)
所述第十五测定点P25[1]与所述平均值Paverage3之间的差值P10＝<P25[1]-Paverage3[1]|P25[2]-Paverage3[2]|P25[3]-Paverage3[3]> 公式(51)
矩阵Cz＝<<P6[1]|P6[1]|-1>，<P7[1]|P7[1]|-1>，<P8[1]|P8[1]|-1><P9[1]|P9[1]|-1>，<P10[1]|P10[1]|-1>> 公式(52)
矩阵Ez＝<-P6[2]，-P7[2]，-P8[2]，-P9[2]，-P10[2]> (53)
矩阵Az＝Cz T 公式(54)
矩阵Bz＝Az·Ez 公式(55)
矩阵Bz＝<<-(P6[1]·P6[2]+P7[1]·P7[2]+P8[1]·P8[12]+P9[1]·P9[2]+P10[1]·P10[2])>，<-(P6[2]·P6[3]+P7[2]·P7[3]+P8[2]·P8[3]+P9[2]·P9[3]+P10[2]·P10[3])>，<P6[2]+P7[2]+P8[2]+P9[2]+P10[2]>> 公式(56)
矩阵Fz＝Az·Cz 公式(57)
矩阵Fz＝<<(P6[1]·P6[1]+P7[1]·P7[1]+P8[1]·P8[1]+P9[1]·P9[1]+P10[1]·P10[1])|(P6[1]·P6[3]+P7[1]·P7[3]+P8[1]·P8[3]+P9[1]·P9[3]+P10[1]·P10[3])|P6[1]+P7[1]+P8[1]+P9[1]+P10[1]>，<(P6[3]·P6[1]+P7[3]·P7[1]+P8[3]·P8[1]+P9[3]·P9[1]+P10[3]·P10[1])|(P6[3]·P6[3]+P7[3]·P7[3]+P8[3]·P8[3]+P9[3]·P9[3]+P10[3]·P10[3])|-(P6[3]+P7[3]+P8[3]+P9[4]+P10[5])>，<-(P6[3]+P7[3]+P8[3]+P9[4]+P10[5])|-(P6[3]+P7[3]+P8[3]+P9[3]+P10[3])|(1+1+1+1+1)>> 公式(58)
Fz·X＝Bz 公式(59)
所述公式(59)中的X的求解函数Lz＝Fz[1][1]·(Fz[2][2]·Fz[3][3]+Fz[2][3]·Fz[3][2])+Fz[1][2]·(Fz[2][3]·Fz[3][1]-Fz[2][1]·Fz[3][3])+Fz[1][3]·(Fz[2][1]·Fz[3][2]-Fz[2][2]·Fz[3][1]) 公式(60)
令Bz[1]·(Fz[2][2]·Fz[3][3]+Fz[2][3]·Fz[3][2])+Fz[1][2]·(Fz[2][3]·Bz[3]-Bz[2]·Fz[3][3])+Fz[1][3]·(Bz[2]·Fz[3][2]-Fz[2][2]·Fz[2][2]·Bz[3])＝hz[1] 公式(61)
令Fz[1][1]·(Bz[2]·Fz[3][3]+Fz[2][3]·Bz[3])+Bz[1]·(Fz[2][3]·Fz[3][1]-Fz[2][1]·Fz[3][3])+Fz[1][3]·(Fz[2][1]·Bz[3]-Bz[2]·Fz[3][1])＝hz[2] 公式(62)
令Fz[1][1]·(Bz[2]·Fz[3][3]+Fz[2][3]·Bz[3])+Fz[1][1]·(Bz[2]·Fz[3][1]-Fz[2][1]·Bz[3])+Bz[1]·(Fz[2][1]·Fz[3][2]-Fz[2][2]·Fz[3][1])＝hz[3] 公式(63)
所述第三测量面的表达式为：A3x+B3y+C3z＝dz 公式(64)
系数
系数
系券
常数dz＝Wz[1]·(Paverage3[1])+Wz[2]·(Paverage3[2])+(Paverage3[3])-Wz[3] 公式(68)
所述第一测量面、所述第二测量面和所述第三测量面的交点
H：＝<<dx>，<dy>，<dz>> 公式(69)。
在实际使用时的验证如下，首先验证第一测量面3的差值，结果如下：
Wy[1]·x+y+Wy[2]·z-dy＝0
Wy[1]·P31[1]+P31[2]+Wy[2]·P31[3]-dy＝0.0577568133091404
Wy[1]·P32[1]+P32[2]+Wy[2]·P32[3]-dy＝-0.0687316767096604
Wy[1]·P33[1]+P33[2]+Wy[2]·P33[3]-dy＝0.225713130039082
Wy[1]·P34[1]+P34[2]+Wy[2]·P34[3]-dy＝-0.0690957233220786
Wy[1]·P35[1]+P35[2]+Wy[2]·P35[3]-dy＝0.057498705137255
接着，验证第二测量面2的差值，结果如下：
x+Wx[1]·y+Wx[2]·z-dx＝0
P11[1]+Wx[1]·P11[2]+Wx[2]·P11[3]-dx＝0.116537297318473
P12[1]+Wx[1]·P12[2]+Wx[2]·P12[3]-dx＝0.155312333241000
P13[1]+Wx[1]·P13[2]+Wx[2]·P13[3]-dx＝-0.462872419254609
P14[1]+W1[1]·P14[2]+Wx[2]·P14[3]-dx＝0.147472809564761
P15[1]+Wx[1]·P15[2]+Wx[2]·P15[3]-dx＝0.0435518592230437
最后，验证第三测量面4的差值，结果如下：
Wz[1]·x+Wz[2]·y+z-dz＝0
Wz[1]·P21[1]+P21[2]·Wz[2]+P21[3]-dz＝-0.252114583240200
Wz[1]·P22[1]+P22[2]·Wz[2]+P22[3]-dz＝-0.0267218586602667
Wz[1]·P23[1]+P23[2]·Wz[2]+P23[3]-dz＝0.0567166752875892
Wz[1]·P24[1]+P24[2]·Wz[2]+P24[3]-dz＝-0.0297226693094217
Wz[1]·P25[1]+P25[2]·Wz[2]+P25[3]-dz＝0.251842419044124
从以上的描述中，可以看出，本发明上述的实施例实现了如下技术效果：利用工业机器人采用距离检测传感器进行多面检测，检测辙叉不同面在机器人世界坐标系中的位置关系，采用特殊算法进行计算从而最终确定其空间位置。机器人可以准确测量辙叉上料后的空间位置，从而可以根据该空间位置对机器人程序进行补偿，保证能够准确对辙叉进行打磨。与现有技术相比，本发明的对辙叉的测量速度更快，测量操作更简单，提高了工作效率，降低了工作强度。
以上仅为本发明的优选实施例，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种多功能轨道检测车