基于平面的TCP标定

详细后台留言

通常机器人TCP标定时采用4点法，即机器人的针尖以不同姿态指向空间同一个点，记录对应4个姿态时的法兰盘位姿，并计算。此方法也可采用Probe自动碰撞计算（Probe测头TCP自动标定）

机器人的TCP标定，也可采用基于平面的约束。即假设针尖均在一个平面上，例如下图的RoboDK的自动标定套件。并记录对应不同姿态时的法兰盘位姿，最后基于最小二乘拟合。

import numpy as npfrom scipy.optimize import least_squaresimport math
def quaternion_to_rotation_matrix(q):      w, x, y, z = q      R = np.array([          [1 - 2*y**2 - 2*z**2, 2*x*y - 2*w*z, 2*x*z + 2*w*y],          [2*x*y + 2*w*z, 1 - 2*x**2 - 2*z**2, 2*y*z - 2*w*x],          [2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x**2 - 2*y**2]      ])      return R    
def calibrate_tcp():    # 定义触碰点的法兰盘旋转矩阵和位置        poses =[[[1017.247,-206.968,863.6606],[0.2836881,0.2493498,0.9162623,0.1334507]],    [[926.5267,83.23212,877.0396],[0.2690141,-0.1729892,0.9473225,-0.01691395]],    [[807.8264,229.0596,822.119],[0.1867633,-0.2596407,0.8750229,-0.3633745]],    [[824.1174,256.7666,822.1188],[0.07701796,-0.003043244,0.9127262,-0.4012354]],    [[622.1634,-39.08593,768.3094],[0.5818022,0.01229214,0.8123776,0.037384]],    [[622.1636,-183.8943,742.1432],[0.5509588,0.1873216,0.7632076,0.2808368]],    [[765.6844,-284.185,876.4497],[0.1541128,0.2926626,0.9285961,0.168247]],    [[809.2202,-157.2588,876.4498],[0.2272622,0.842452,0.4880745,0.02024099]],    [[964.7901,-157.2588,886.0762],[0.09513868,0.8197071,0.5299184,-0.1954876]],    [[932.6337,-180.5196,896.2783],[0.04710102,0.8328494,0.5454924,-0.08112613]],    [[965.7423,-185.5846,896.2784],[0.02291511,0.9509443,0.2947965,-0.09096605]],    [[970.7756,-105.8971,891.1408],[0.1075797,0.957977,0.2349058,-0.1246032]],    [[896.0595,-70.11932,880.5201],[0.2107622,0.8035958,0.556383,0.0158417]],    [[856.0594,-2.259757,863.201],[0.3071248,0.7719206,0.5541663,-0.05208224]],    [[792.7153,155.7455,819.4674],[0.4183114,0.6904843,0.5802991,0.107238]],    [[843.2375,-6.758245,863.2011],[0.3112417,0.6398342,0.7025483,0.01291077]],    [[835.915,170.8602,801.0045],[0.4734927,0.6562263,0.5800706,0.09321946]],    [[1002.685,81.56886,861.8849],[0.3151748,0.6958594,0.64531,-0.004405599]],    [[1098.957,45.56094,884.5965],[0.1619111,0.6770209,0.6996878,-0.1608242]],    [[1017.247,42.63607,859.4042],[0.335639,0.173207,0.921301,-0.0924677]]]
    # 初始猜测：TCP 偏移 (d_x, d_y, d_z) 和平面方程参数 (A, B, C, D)    initial_guess = [30, 0, 250, 1, 0, 0, 0]      # 此处测试的TCP值为 31.792,0,229.64    # [d_x, d_y, d_z, A, B, C, D]
    # 使用非线性最小二乘法进行优化    result = least_squares(objective_function, initial_guess, args=(poses,))
    # 解析优化结果    TCP = result.x[0:3]  # [d_x, d_y, d_z]    plane = result.x[3:7]  # [A, B, C, D]
    # 显示结果    print(f"标定后的TCP位置: d_x = {TCP[0]:.2f}, d_y = {TCP[1]:.2f}, d_z = {TCP[2]:.2f}")    print(f"标定后的平面方程: {plane[0]:.2f} * x + {plane[1]:.2f} * y + {plane[2]:.2f} * z + {plane[3]:.2f} = 0")
    return TCP, plane
# 目标函数，计算每个触碰点的误差def objective_function(params, poses):    # 提取当前优化参数    d = np.array(params[0:3])  # 针尖TCP的偏移量 [d_x, d_y, d_z]    A = params[3]  # 平面方程 A    B = params[4]  # 平面方程 B    C = params[5]  # 平面方程 C    D = params[6]  # 平面方程 D
    # 初始化误差列表    errors = []    # 计算每个触碰点的误差    for pose in poses:        R = quaternion_to_rotation_matrix(pose[1]) # 当前触碰点的旋转矩阵        t = pose[0]  # 当前触碰点的平移向量
        # 计算针尖在基坐标系中的位置 P = R * TCP + t        P = np.dot(R, d) + t
        # 计算点 P 到平面的距离        error = abs(A * P[0] + B * P[1] + C * P[2] + D)/(math.sqrt(A*A+B*B+C*C))        # 目标即这些点到平面的距离最小        errors.append(error)    return errors
if __name__ == "__main__":    TCP, plane = calibrate_tcp()

********************************

如何获取更多经典文章？