[發(fā)明專利]一種礦用鉆桿自動(dòng)加接方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202111546611.2
• 申請(qǐng)日: 2021-12-16
• 公開(kāi)（公告）號(hào): CN114367979B
• 公開(kāi)（公告）日: 2023-07-18
• 發(fā)明（設(shè)計(jì)）人: 李艷;高峰;王楠;安佳樂(lè);張東亞;楊元
• 申請(qǐng)（專利權(quán)）人: 西安理工大學(xué)
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16;B25J15/02
• 代理公司: 北京國(guó)昊天誠(chéng)知識(shí)產(chǎn)權(quán)代理有限公司 11315
• 代理人: 楊洲

1.一種礦用鉆桿自動(dòng)加接方法，其特征在于，其具體步驟如下：

步驟1：首先在加工位置安裝固定工業(yè)機(jī)器人(1.1)、鉆桿安裝箱(3.2)、鉆桿安裝架(3.5)、激光輪廓儀(1.3)；所述鉆桿安裝架(3.5)上設(shè)置有鉆桿夾持器(3.3)、鉆機(jī)(3.6)；所述鉆桿安裝箱(3.2)位于鉆桿安裝架(3.5)的旁邊；

步驟2：利用激光輪廓儀(1.3)掃描待抓取鉆桿(3.1)，獲取鉆桿(3.1)三維表面輪廓數(shù)據(jù)，以點(diǎn)云數(shù)據(jù)P＝{P_i|(x_i,y_i,z_i),i∈Z}描述鉆桿(3.1)三維表面輪廓和坐標(biāo)位姿；其中χi、уi、zi為鉆桿三維數(shù)據(jù)點(diǎn)坐標(biāo)值；

步驟3：設(shè)定工業(yè)機(jī)器人(1.1)處于加接位置為初始狀態(tài)，提取引入的理論虛擬鉆桿(3.4)對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)Q＝{Q_i|(x_i,y_i,z_i),i∈Z}描述理論虛擬鉆桿(3.4)三維表面輪廓和坐標(biāo)位姿；

步驟4：提取點(diǎn)云數(shù)據(jù)幾何特征并采用SAC-IA+ICP三維點(diǎn)云配準(zhǔn)算法，計(jì)算出待抓取鉆桿(3.1)相對(duì)于理論虛擬鉆桿(3.4)的坐標(biāo)變換矩陣，即坐標(biāo)系∑F相對(duì)于坐標(biāo)系∑M的坐標(biāo)變換矩陣即公式(1)：

上式中：

n＝(n_x,n_y,n_z)為∑F的X軸對(duì)∑M的三個(gè)坐標(biāo)軸的方向余弦；

o＝(o_x,o_y,o_z)為∑F的Y軸對(duì)∑M的三個(gè)坐標(biāo)軸的方向余弦；

a＝(a_x,a_y,a_z)為∑F的Z軸對(duì)∑M的三個(gè)坐標(biāo)軸的方向余弦；

p＝(p_x,p_y,p_z)為∑F的原點(diǎn)在∑M中的坐標(biāo)；

為∑F相對(duì)于∑M的旋轉(zhuǎn)矩陣；

為∑F的原點(diǎn)在∑M中的坐標(biāo)；

步驟5：利用公式(1)計(jì)算出的基于機(jī)器人逆運(yùn)動(dòng)學(xué)原理求解機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)量(θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)，即公式(2)：

公式(2)中，θ₁～θ₆為機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)量；

為∑F相對(duì)于∑M的齊次坐標(biāo)變換矩陣；

為∑Y相對(duì)于∑G的齊次坐標(biāo)變換矩陣；

為∑J相對(duì)于∑Y的齊次坐標(biāo)變換矩陣；

為∑Z相對(duì)于∑J的齊次坐標(biāo)變換矩陣；

為∑W₁相對(duì)于∑Z的齊次坐標(biāo)變換矩陣；

為∑W₂相對(duì)于∑W₁的齊次坐標(biāo)變換矩陣；

為∑W₃相對(duì)于∑W₂的齊次坐標(biāo)變換矩陣；

為∑M相對(duì)于∑W₃的齊次坐標(biāo)變換矩陣；

步驟6：利用公式(2)得到機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)量，控制機(jī)器人完成對(duì)鉆桿(3.1)的抓取；

步驟7：對(duì)公式(1)求得的矩陣進(jìn)行求逆操作，得到加接位置處的理論虛擬鉆桿(3.4)相對(duì)于已抓取鉆桿(3.1)的位姿變換矩陣，即坐標(biāo)系∑M相對(duì)于坐標(biāo)系∑F的坐標(biāo)變換矩陣即公式(3)：

上式中：

n＝(n_x,n_y,n_z)為∑M的X軸對(duì)∑F的三個(gè)坐標(biāo)軸的方向余弦；

o＝(o_x,o_y,o_z)為∑M的Y軸對(duì)∑F的三個(gè)坐標(biāo)軸的方向余弦；

a＝(a_x,a_y,a_z)為∑M的Z軸對(duì)∑F的三個(gè)坐標(biāo)軸的方向余弦；

p＝(p_x,p_y,p_z)為∑M的原點(diǎn)在∑F中的坐標(biāo)；

為∑M相對(duì)于∑F的旋轉(zhuǎn)矩陣為∑M的原點(diǎn)在∑F中的坐標(biāo)；

步驟8：利用公式(3)計(jì)算出的基于機(jī)器人逆運(yùn)動(dòng)學(xué)原理求解機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)量(θ₁′，θ₂′，θ₃′，θ₄′，θ₅′，θ₆′)，即公式(4)：

公式(4)中，θ₁′～θ₆′機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)量；

步驟9：利用公式(4)得到機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)量，控制機(jī)器人將已抓取的鉆桿(3.1)移動(dòng)到安裝位置；

步驟10：鉆桿夾持器(3.3)夾緊鉆桿(3.1)，鉆機(jī)(3.6)旋轉(zhuǎn)，實(shí)現(xiàn)鉆機(jī)(3.6)與鉆桿(3.1)頂端螺紋連接，完成鉆桿(3.1)安裝，鉆桿夾持器(3.3)松開(kāi)，鉆機(jī)(3.6)帶動(dòng)鉆桿(3.1)旋轉(zhuǎn)，進(jìn)行作業(yè)；

步驟11：重復(fù)步驟2～步驟10，對(duì)鉆桿(3.1)進(jìn)行連續(xù)加接，直至滿足鉆井深度要求。