[發(fā)明專利]一種六軸機器人末端跟隨目標物實時位姿的計算方法

有效

• 申請?zhí)枺?/td>: 202010044804.7
• 申請日: 2020-01-16
• 公開（公告）號: CN111230866B
• 公開（公告）日: 2021-12-28
• 發(fā)明（設(shè)計）人: 李濟甫;陳晶晶;王素鋼;劉海健
• 申請（專利權(quán)）人: 山西萬合智能科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 太原榮信德知識產(chǎn)權(quán)代理事務(wù)所(特殊普通合伙) 14119
• 代理人: 楊凱;連慧敏

1.一種六軸機器人末端跟隨目標物實時位姿的計算方法，其特征在于：包括下列步驟：

S1、以六軸機器人第一軸與工作臺的交點為原點建立世界坐標系；

S2、在各軸、機器人末端以及目標物依次建立各自坐標系；

S3、依據(jù)機器人正運動學(xué)原理得到各個關(guān)節(jié)之間的齊次變換矩陣，將矩陣相乘得到當(dāng)前狀態(tài)下機器人末端相對于世界坐標系的位姿

S4、計算目標物坐標系相對于機器人末端坐標系的齊次變換矩陣記為B；

S5、根據(jù)目標物的參數(shù)求解目標物相對于目標物自身的初始坐標系的齊次變換矩陣為C；

S6、求解當(dāng)前目標物在世界坐標系的位姿：T＝A*B*C；

S7、為了使機器人末端始終正對著要跟隨的目標物體并保持一定距離，那么新的狀態(tài)下機械臂末端相對于世界坐標系的矩陣記為

S8、計算目標物坐標系相對于機器人末端坐標系的變換矩陣記為B'；

S9、計算目標物相對于目標物自身坐標系的旋轉(zhuǎn)矩陣C'；

所述目標物相對于目標物自身坐標系的旋轉(zhuǎn)矩陣C'的求解方法為：由于機器人末端正對著目標物，因此目標物在坐標系下滾動角、俯仰角、偏航角都為0，即目標物相對于目標物自身坐標系的旋轉(zhuǎn)矩陣為單位矩陣

S10、求解目標物在世界坐標系的位姿T＝A'*B'*C'；

S11、求解機器人末端相對于世界坐標系新的位姿A'；

S12、為了使機器人末端的水平軸能夠始終平行于工作臺，將A'矩陣與繞末端軸軸線的旋轉(zhuǎn)的齊次矩陣Rot相乘，得到新的矩陣記為H

當(dāng)機器人末端的水平軸是x軸，那么經(jīng)過矩陣A'與Rot相乘x軸在新坐標系下的z軸的向量分量n_z,H應(yīng)為0，即：

n_z,H＝n_z'*n_x,R+s_z'*n_y,R+a_z'*n_z,R+p_z'*0＝0

根據(jù)上述等式求出末端軸的偏差角，再通過機器人逆運動學(xué)可得到各關(guān)節(jié)應(yīng)轉(zhuǎn)動的角度θ₁、θ₂、θ₃、θ₄、θ₅、θ₆，從而得到六軸需要轉(zhuǎn)動的角度，便可以使得機器人末端水平軸始終平行于工作臺；

S13、重復(fù)S3，通過不斷更新位姿，保持實時的追蹤。

2.根據(jù)權(quán)利要求1所述的一種六軸機器人末端跟隨目標物實時位姿的計算方法，其特征在于：所述S4中計算機器人末端坐標系的齊次變換矩B的方法為：相機在末端坐標系的安裝位置為(a，b，c)，相機采集到目標物相對于相機的位置坐標(x，y，z)，那么目標物在末端坐標系的位置坐標為(a+x，b+y，c+z)，目標物坐標系相對于機器人末端坐標系先繞x軸旋轉(zhuǎn)90度，再繞z軸旋轉(zhuǎn)90度，計算目標物坐標系相對于機器人末端坐標系的變換矩陣

3.根據(jù)權(quán)利要求1所述的一種六軸機器人末端跟隨目標物實時位姿的計算方法，其特征在于：所述S5中求解目標物相對于目標物自身的初始坐標系的齊次變換矩陣C的方法為：設(shè)目標物在坐標系下的滾動角為R、俯仰角為P、偏航角為Y，那么計算其旋轉(zhuǎn)矩陣為

4.根據(jù)權(quán)利要求1所述的一種六軸機器人末端跟隨目標物實時位姿的計算方法，其特征在于：所述S8中機器人末端坐標系的變換矩陣記B'的求解方法為：設(shè)始終保持的距離為Z，坐標系先繞x軸旋轉(zhuǎn)90度，再繞z軸旋轉(zhuǎn)90度，計算目標物相對于機器人末端的轉(zhuǎn)換矩陣為

5.根據(jù)權(quán)利要求1所述的一種六軸機器人末端跟隨目標物實時位姿的計算方法，其特征在于：所述S11中機器人末端相對于世界坐標系新的位姿A'的求解方法為：根據(jù)T＝A*B*C和T＝A'*B'*C'兩個等式，可得到新的狀態(tài)下機械臂末端相對于世界坐標系的矩陣A'。