标定实现

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

TCP位置标定

机械臂眼在手外标定python_机器人


标定步骤

  1. 控制机械臂移动工具从不同方位触碰空间中某个固定点，记录N组数据（机械臂眼在手外标定python_TCP_02）；
  2. 计算获得工具末端点相对机械臂末端点的位置变换；

TCF姿态标定

机械臂眼在手外标定python_机械臂_03


标定步骤

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

原理分析

末端连杆坐标系{E}到基坐标系{B}的位姿关系为：
机械臂眼在手外标定python_机械臂眼在手外标定python_04

机械臂眼在手外标定python_机械臂眼在手外标定python_05可通过运动学正解获得，机械臂眼在手外标定python_机器人_06为末端连杆坐标系{E}相对于基坐标系{B}的旋转矩阵，机械臂眼在手外标定python_TCP_07为末端连杆坐标系{E}原点相对于基坐标系{B}的位置矢量。

工具坐标系{T}到基坐标系{B}的位姿关系为：
机械臂眼在手外标定python_原理分析_08

TCP位置标定分析

在TCP位置标定过程中，第i个标定点的末端连杆坐标系到基坐标系的变换矩阵机械臂眼在手外标定python_机械臂眼在手外标定python_05，可由机器人正解得到，代入\式(2)可得：
机械臂眼在手外标定python_机械臂眼在手外标定python_10
写成分块形式为：
机械臂眼在手外标定python_机械臂眼在手外标定python_11
令等式两边第4列对应相等，则得：
机械臂眼在手外标定python_机械臂眼在手外标定python_12
因工作坐标系{T}相对末端连杆坐标系{E}的位置关系为固定，因此机械臂眼在手外标定python_TCP_13为确定值。所以，对于所有标定点有如下关系：

机械臂眼在手外标定python_原理分析_14

写成矩阵形式：

机械臂眼在手外标定python_机械臂_15

上式的未知量机械臂眼在手外标定python_机械臂_16含三个元素，包含机械臂眼在手外标定python_TCP_17个方程，系数矩阵为机械臂眼在手外标定python_机械臂眼在手外标定python_18矩阵。

当机械臂眼在手外标定python_TCP_19时系数矩阵秩一般等于3，为列满秩矩阵，式(4)为不相容方程组，只能求最佳最小二乘解。

机械臂眼在手外标定python_TCP_20

非奇异的"方阵"才能叫"可逆矩阵"，奇异的"方阵"叫"不可逆矩阵"。

现代应用中为了解决各种线性方程组(系数矩阵是非方阵和方阵为奇异)，将逆矩阵的概念推广到"不可逆方阵"和"长方形矩阵"上，从而产生了"广义逆矩阵"的概念

若A为行满秩，则机械臂眼在手外标定python_原理分析_21

若A为列满秩，则机械臂眼在手外标定python_原理分析_22

机械臂眼在手外标定python_机械臂眼在手外标定python_23为系数矩阵的加号广义逆，因系数矩阵为列满秩，满足机械臂眼在手外标定python_TCP_24，则：

机械臂眼在手外标定python_TCP_25

根据求得的机械臂眼在手外标定python_机械臂_16可计算标定误差
机械臂眼在手外标定python_机械臂眼在手外标定python_27

TCF姿态标定分析

已知TCF状态标定２个标定点的末端连杆坐标系到基坐标系的变换矩阵，可求得工具中心点相对于基坐标系的位置：

  • 中心点和x方向偏移点

机械臂眼在手外标定python_TCP_28

  • 中心点和z方向偏移点

机械臂眼在手外标定python_原理分析_29

这两点所决定的向量为：
机械臂眼在手外标定python_机械臂_30
机械臂眼在手外标定python_机械臂_31为x方向标定点处工具坐标系在+X方向上的向量，即：
机械臂眼在手外标定python_机器人_32
x轴相对于末端连杆坐标系{E}的方向余弦为：
机械臂眼在手外标定python_机械臂眼在手外标定python_33
机械臂眼在手外标定python_TCP_34为z方向标定点处工具坐标系在+Z方向上的向量，z轴相对于末端连杆坐标系{E}的方向余弦为：
机械臂眼在手外标定python_机械臂_35

工具坐标系{T}的Y轴相对于末端连杆坐标系{E}的方向余弦为：
机械臂眼在手外标定python_TCP_36
因x方向和z方向为手动移动确定，无法保证两个方向相互垂直，因此需要对其中一个轴（x或者z）进行修正，确保坐标系主矢量的正交性，此处调整z轴：
机械臂眼在手外标定python_机械臂_37

程序实现

import numpy as np
import pandas as pd
import pysnooper
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_matrix())

            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_matrix())

            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)

            # change into quat
            q = R.from_matrix(tool_rot)
            print(q.as_quat())

    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])
        # print(np.linalg.inv(L).dot(R))
        # tool_tran = np.linalg.inv(L).dot(R)
		
        # 构建Ax=B
        A = self.tran_rotm[0] - self.tran_rotm[1]
        B = self.tran_tran[1] - self.tran_tran[0]
        for i in range(1,len(self.tran_tran)-1):
            A = np.vstack((A,self.tran_rotm[i] - self.tran_rotm[i+1]))
            B = np.vstack((B,self.tran_tran[i+1] - self.tran_tran[i]))
        
        # 广义逆
        tool_tran = np.linalg.pinv(A).dot(B)
        
        # svd
        u,s,v = np.linalg.svd(A,0)
        c = np.dot(u.T,B)
        w = np.linalg.solve(np.diag(s),c)
        tool_tran = np.dot(v.T,w)

        return tool_tran

    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()