• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

机械臂——工具标定

人工智能 white_Learner 1471次浏览 0个评论

一、标定实现

机器人工具坐标系标定就是确定工具坐标系相对于末端连杆坐标系的变换矩阵

 

1.1 TCP位置标定

 
机械臂——工具标定   标定步骤

  1. 控制机械臂移动工具从不同方位触碰空间中某个固定点,记录N组数据(n ⩾ 3 n \geqslant 3n3);
  2. 计算获得工具末端点相对机械臂末端点的位置变换;

1.2 TCF姿态标定

 
机械臂——工具标定   标定步骤

  1. 完成位置标定;
  2. 控制工具末端点分别沿x方向和z方向移动一定距离,工具末端点只在该方向上有移动,其它方向上无位移,同时固定初始姿态保持不变。实际操作上可以设置三个固定点(三个固定点满足上述要求,点2和点3相对点1只有一个方向上的移动),使工具末端点分别触碰这三个点然后记录下机械臂末端位姿;
  3. 计算获得工具坐标系相对机械臂末端坐标系的姿态变换;

 

二、原理分析

末端连杆坐标系{E}到基坐标系{B}的位姿关系为:  
机械臂——工具标定  
机械臂——工具标定
机械臂——工具标定
机械臂——工具标定
机械臂——工具标定
机械臂——工具标定
机械臂——工具标定  

三、程序实现

 

import numpy as np
import pandas as pd
import math
from scipy.spatial.transform import Rotation as R

class tool_cal():
    def __init__(self):
        """
		load data from csv
		tool_points(0~5) : use robot effectors to touch the same points in the world
                      and record the pos 
        tool_poses_tran(0~3):count tanslation
        tool_poses_rot(3~5):count rotation
		"""
        with open("tool_data.csv") as file:
            tool_poses = pd.read_csv(file, header = None)
            tool_poses = np.array(tool_poses)
            
            # cal translation
            self.tran_tran=[]
            self.tran_rotm=[]
            tool_poses_tran = tool_poses[0:4,:]
            for pose in tool_poses_tran:
                # set translation
                self.tran_tran.append(np.array([[pose[0]],[pose[1]],[pose[2]]]))
                
                # set rotation
                r = R.from_euler('xyz', np.array([pose[3], pose[4], pose[5]]), degrees=True)
                self.tran_rotm.append(r.as_dcm())

            tool_tran=self.cal_tran()

            # cal rotation
            self.rot_tran=[]
            self.rot_rotm=[]
            tool_poses_rot = tool_poses[3:6,:]
            for pose in tool_poses_rot:
                # set translation
                self.rot_tran.append(np.array([[pose[0]],[pose[1]],[pose[2]]]))
                
                # set rotation
                r = R.from_euler('xyz', np.array([pose[3], pose[4], pose[5]]), degrees=True)
                self.rot_rotm.append(r.as_dcm())

            tool_rot=self.cal_rotm(tool_tran)

            # get transformation
            tool_T = np.array(np.zeros((4,4)))
            tool_T[0:3,0:3] = tool_rot
            tool_T[0:3,3:] = tool_tran
            tool_T[3:,:] = [0,0,0,1]
            print tool_T

    def cal_tran(self):
        tran_data=[]
        rotm_data=[]
        for i in range(len(self.tran_tran)-1):
            tran_data.append(self.tran_tran[i+1] - self.tran_tran[i])
            rotm_data.append(self.tran_rotm[i] - self.tran_rotm[i+1])
        
        L = np.array(np.zeros((3,3)))
        R = np.array(np.zeros((3,1)))
        for i in range(len(tran_data)):
            L = L + np.dot(rotm_data[i],rotm_data[i])
            R = R + np.dot(rotm_data[i],tran_data[i])

        return np.linalg.inv(L).dot(R)

    def cal_rotm(self, tran):
        # centre
        P_otcp_To_B = np.dot(self.rot_rotm[0],tran)+self.rot_tran[0]

        # cal the dircction vector of x
        P_xtcp_To_B = np.dot(self.rot_rotm[1],tran)+self.rot_tran[1]
        vector_X = P_xtcp_To_B - P_otcp_To_B
        dire_vec_x_o = np.linalg.inv(self.rot_rotm[0]).dot(vector_X) / np.linalg.norm(vector_X)

        # cal the dircction vector of z
        P_ztcp_To_B = np.dot(self.rot_rotm[2],tran)+self.rot_tran[2]
        vector_Z = P_ztcp_To_B - P_otcp_To_B
        dire_vec_z_o = np.linalg.inv(self.rot_rotm[0]).dot(vector_Z) / np.linalg.norm(vector_Z)

        # cal the dircction vector of y
        dire_vec_y_o = np.cross(dire_vec_z_o.T,dire_vec_x_o.T)

        # modify the dircction vector of z 
        dire_vec_z_o = np.cross(dire_vec_x_o.T,dire_vec_y_o)

        # cal rotation matrix
        tool_rot = np.array(np.zeros((3,3)))
        tool_rot[:,0] = dire_vec_x_o.T
        tool_rot[:,1] = dire_vec_y_o
        tool_rot[:,2] = dire_vec_z_o
        return tool_rot

if __name__ == "__main__":
    tool_cal()

   

参考

机器人工具坐标系TCP标定 四点法原理 与 matlab实现 机器人工具坐标系标定算法研究 广义逆矩阵A+


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明机械臂——工具标定
喜欢 (0)

您必须 登录 才能发表评论!

加载中……