[發(fā)明專利]一種利用機(jī)器人定位轍叉的測(cè)量方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011593542.6
• 申請(qǐng)日: 2020-12-29
• 公開（公告）號(hào): CN112779830B
• 公開（公告）日: 2023-03-24
• 發(fā)明（設(shè)計(jì)）人: 崔洪亮;王瑞成;李超;宋志偉;李天偉;夏武強(qiáng)
• 申請(qǐng)（專利權(quán)）人: 廊坊市億創(chuàng)科技有限公司
• 主分類號(hào): E01B35/00
• 分類號(hào): E01B35/00;E01B29/16
• 代理公司: 北京五洲洋和知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11387
• 代理人: 劉春成;徐麗娜

1.一種利用機(jī)器人定位轍叉的測(cè)量方法，所述機(jī)器人的輸出端上設(shè)有測(cè)距傳感器，其特征在于，包括如下步驟：

S1：基于所述測(cè)距傳感器的工具坐標(biāo)系生成標(biāo)準(zhǔn)向量；

S2：對(duì)所述轍叉進(jìn)行第一次定位，檢測(cè)所述轍叉在世界坐標(biāo)系中的X軸方向的偏移量和Y軸方向的偏移量；

S3：對(duì)所述轍叉進(jìn)行第二次定位，選取所述轍叉的一個(gè)端點(diǎn)和與所述端點(diǎn)相鄰的三個(gè)測(cè)量面，通過所述測(cè)距傳感器對(duì)三個(gè)所述測(cè)量面和三個(gè)所述測(cè)量面之間的交點(diǎn)進(jìn)行測(cè)量，計(jì)算所述交點(diǎn)在世界坐標(biāo)系中的坐標(biāo)位置；

S4：根據(jù)所述交點(diǎn)在世界坐標(biāo)系中的坐標(biāo)位置計(jì)算出所述轍叉的標(biāo)準(zhǔn)工件坐標(biāo)系W_obj；

S5：在所述轍叉被加工后重復(fù)步驟S3，并根據(jù)所述交點(diǎn)在世界坐標(biāo)系中的坐標(biāo)位置計(jì)算出所述轍叉的補(bǔ)償工件坐標(biāo)系，計(jì)算所述補(bǔ)償工件坐標(biāo)系與所述標(biāo)準(zhǔn)工件坐標(biāo)系W_obj之間的差值，并將所述差值補(bǔ)充到所述標(biāo)準(zhǔn)工件坐標(biāo)系W_obj中；

在所述步驟S3中，通過所述測(cè)距傳感器對(duì)三個(gè)所述測(cè)量面和三個(gè)所述測(cè)量面之間的交點(diǎn)進(jìn)行測(cè)量包括：

S31：通過所述測(cè)距傳感器測(cè)量每個(gè)所述測(cè)量面上的至少5個(gè)測(cè)定點(diǎn)；

S32：通過每個(gè)所述測(cè)量面上被測(cè)量的至少5個(gè)測(cè)定點(diǎn)計(jì)算出所述測(cè)量面在所述世界坐標(biāo)系內(nèi)的坐標(biāo)；

S33：基于所述測(cè)量面在所述世界坐標(biāo)系中的位置計(jì)算所述交點(diǎn)在所述世界坐標(biāo)系的坐標(biāo)；

在所述步驟S3中，所述測(cè)量面包括第一測(cè)量面、第二測(cè)量面和第三測(cè)量面，所述第一測(cè)量面上的所述測(cè)定點(diǎn)包括：第一測(cè)定點(diǎn)P₃₁、第二測(cè)定點(diǎn)P₃₂、第三測(cè)定點(diǎn)P₃₃、第四測(cè)定點(diǎn)P₃₄和第五測(cè)定點(diǎn)P₃₅，所述第一測(cè)定點(diǎn)P₃₁、所述第二測(cè)定點(diǎn)P₃₂、所述第三測(cè)定點(diǎn)P₃₃、所述第四測(cè)定點(diǎn)P₃₄和所述第五測(cè)定點(diǎn)P₃₅的平均值

所述第一測(cè)定點(diǎn)P₃₁與所述平均值P_average之間的差值P₁₀₁＝P₃₁[1]-P_average[1]|P₃₁[2]-P_average[2]|P₃₁[3]-P_average[3]

公式(2)

所述第二測(cè)定點(diǎn)P₃₂與所述平均值P_average之間的差值P₁₀₂＝P₃₂[1]-P_average[1]|P₃₂[2]-P_average[2]|P₃₂[3]-P_average1[3]

公式(3)

所述第三測(cè)定點(diǎn)P₃₃與所述平均值P_average之間的差值P₁₀₃＝P₃₃[1]-P_average[1]|P₃₃[2]-P_average[2]|P₃₃[3]-P_average[3]

公式(4)

所述第四測(cè)定點(diǎn)P₃₄與所述平均值P_average之間的差值P₁₀₄＝P₃₄[1]-P_average[1]|P₃₄[2]-P_average[2]|P₃₄[3]-P_average[3]

公式(5)

所述第五測(cè)定點(diǎn)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]|-1P₁₀₄[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的求解函數(shù)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)

所述第一測(cè)量面的表達(dá)式為：A₁x+B₁y+C₁z＝d_y 公式(18)

系數(shù)

系數(shù)

系數(shù)

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

所述第二測(cè)量面上的所述測(cè)定點(diǎn)包括：第六測(cè)定點(diǎn)P₁₁、第七測(cè)定點(diǎn)P₁₂、第八測(cè)定點(diǎn)P₁₃、第九測(cè)定點(diǎn)P₁₄、第十測(cè)定點(diǎn)P₁₅，所述第六測(cè)定點(diǎn)P₁₁、所述第七測(cè)定點(diǎn)P₁₂、所述第八測(cè)定點(diǎn)P₁₃、所述第九測(cè)定點(diǎn)P₁₄、所述第十測(cè)定點(diǎn)P₁₅的平均值

所述第六測(cè)定點(diǎn)P₁₁與所述平均值P_average2之間的差值P₁＝P₁₁[1]-P_average2[1]|P₁₁[2]-P_average2[2]|P₁₁[3]-P_average2[3]

公式(24)

所述第七測(cè)定點(diǎn)P₁₂與所述平均值P_average2之間的差值P₂＝P₁₂[1]-P_average2[1]|P₁₂[2]-P_average2[2]|P₁₂[3]-P_average2[3]

公式(25)

所述第八測(cè)定點(diǎn)P₁₃與所述平均值P_average2之間的差值P₃＝P₁₃[1]-P_average2[1]|P₁₃[2]-P_average2[2]|P₁₃[3]-P_average2[3]

公式(26)

所述第九測(cè)定點(diǎn)P₁₄與所述平均值P_average2之間的差值P₄＝P₁₄[1]-P_average2[1]|P₁₄[2]-P_average2[2]|P₁₄[3]-P_average2[3]

公式(27)

所述第十測(cè)定點(diǎn)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]|-1P₄[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的求解函數(shù)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)

所述第二測(cè)量面的表達(dá)式為：A₂x+B₂y+C₃z＝d_x 公式(41)

系數(shù)

系數(shù)

系數(shù)

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

所述第三測(cè)量面上的所述測(cè)定點(diǎn)包括：第十一測(cè)定點(diǎn)P₂₁、第十二測(cè)定點(diǎn)P₂₂、第十三測(cè)定點(diǎn)P₂₃、第十四測(cè)定點(diǎn)P₂₄和第十五測(cè)定點(diǎn)P₂₅，所述第十一測(cè)定點(diǎn)P₂₁、所述第十二測(cè)定點(diǎn)P₂₂、所述第十三測(cè)定點(diǎn)P₂₃、所述第十四測(cè)定點(diǎn)P₂₄和所述第十五測(cè)定點(diǎn)P₂₅的平均值

所述第十一測(cè)定點(diǎn)P₂₁與所述平均值P_average3之間的差值P₆＝P₂₁[1]-P_average3[1]|P₂₁[2]-P_average3[2]|P₂₁[3]-P_average3[3]

公式(47)

所述第十二測(cè)定點(diǎn)P₂₂與所述平均值P_average3之間的差值P₇＝P₂₂[1]-P_average3[1]|P₂₂[2]-P_average3[2]|P₂₂[3]-P_average3[3]

公式(48)

所述第十三測(cè)定點(diǎn)P₂₃與所述平均值P_average3之間的差值P₈＝P₂₃[1]-P_average3[1]|P₂₃[2]-P_average3[2]|P₂₃[3]-P_average3[3]

公式(49)

所述第十四測(cè)定點(diǎn)P₂₄與所述平均值P_average3之間的差值P₉＝P₂₄[1]-P_average3[1]|P₂₄[2]-P_average3[2]|P₂₄[3]-P_average3[3]

公式(50)

所述第十五測(cè)定點(diǎn)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]|-1P₉[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的求解函數(shù)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)

所述第三測(cè)量面的表達(dá)式為：A₃x+B₃y+C₃z＝d_z 公式(64)

系數(shù)

系數(shù)

系數(shù)

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

所述第一測(cè)量面、所述第二測(cè)量面和所述第三測(cè)量面的交點(diǎn)

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

所述交點(diǎn)H為所述標(biāo)準(zhǔn)工件坐標(biāo)系W_obj的原點(diǎn)。