点激光原点标定

显示全部楼层 · 2024-9-25 13:53:54

假设在下图工具末端安装一个测距点激光（即能准确返回距离mm）。如果想直接知道点激光末端的点在机器人base下的位置xyz，则需要知道点激光的原点相对于法兰盘的位姿（即可以认为点激光的原点就是一个TCP）。如何计算这个位姿？

在仿真过程中，先要制作一个模拟点激光的测距smart组件。

可以新建一个smart组件，组件内包括一个lineSensor和其他若干组件，具体如下图。

将组件安装到上图的工具上，记得将工具的可由传感器检测的勾去掉（此处工具TCP已知，可以后期用于验证）。

开启仿真后，lineSensor会返回lineSensor检测到的物体表面的位置。由于smart组件安装到了tool末端，positionSensor用于返回这个smart组件的位置（也就是lineSensor的起点位置）。positionSensor内的Object选择这个smart组件。

并通过自定义表达式，计算lineSensor起点到工件表面的距离（默认数据单位是m，建议乘1000）并记录到RAPID。

对于点激光原点的标定，可以假设机器人的法兰盘在不同位姿时，点激光的末端应该在一个平面。基于这个约束，在获取到机器人不同姿态（例如15个）时的法兰盘位姿和对应的点激光测距值。

P = pose_tool0 *TCP *[0,0,sensor]! 点激光末端在平面的上x,y,z表示

基于以上约束，采用最小二乘拟合，可以计算出上文的TCP（x,y,z,rx,ry,rz=0）的值。（由于标定点激光，可以假设TCP的rz为零）

import numpy as npfrom scipy.optimize import least_squaresimport mathfrom scipy.spatial.transform import Rotation as R
def calibrate_tcp():    # 15个机器人 tool0的pose    poses = [[[1106.923,-62.67398,928.2547],[0.2302114,0.6685829,0.6685824,-0.2302112]],    [[1106.923,86.77248,851.3452],[0.366503,0.6047113,0.6047111,-0.3665027]],    [[1108.335,-23.01278,944.0456],[0.06131056,0.503774,0.850939,-0.1354827]],    [[1175.693,110.5853,908.7065],[0.06720021,0.693396,0.6310124,-0.3413349]],    [[975.0178,-104.9414,908.7065],[0.3213044,-0.7556739,0.5549161,0.1333739]],    [[1059.605,-107.5823,910.6316],[0.1949158,-0.7095332,0.6108817,0.2922225]],    [[1114.916,-77.83147,879.8255],[0.1018847,-0.6581752,0.6330785,0.394508]],    [[922.5756,-52.07503,876.531],[0.3974437,-0.7669845,0.5031964,0.02380637]],    [[922.5756,-165.3309,843.5357],[0.3974437,-0.7669845,0.5031963,0.02380632]],    [[1029.069,-165.3308,925.215],[0.2418884,-0.7301325,0.5938506,0.2360891]],    [[916.822,-198.3876,930.8027],[0.267173,0.2556136,0.9202346,0.1282515]],    [[909.4986,-244.8393,885.1127],[0.1924895,0.4580627,0.8272752,0.2621871]],    [[851.7618,-75.75834,899.2515],[0.2401697,0.4349622,0.8504897,0.172608]],    [[1073.684,-26.0818,905.7529],[0.1545833,0.5056722,-0.8340939,-0.1571214]],    [[951.1398,-4.736905,926.2714],[0.06837972,0.8789129,-0.4523344,-0.1350182]]]    # 对应点激光测距结果，单位mm    sensor =[[0,0,62.9722],[0,0,57.2877],[0,0,57.8577],[0,0,55.4303],[0,0,55.4302],              [0,0,52.3926],[0,0,44.3778],[0,0,50.9007],[0,0,10.0095],[0,0,63.3876],              [0,0,69.304],[0,0,55.0024],[0,0,45.5043],[0,0,56.6058],[0,0,42.8198]]
    # 假设点激光原点TCP的rz为0    # 初始猜测：TCP 偏移 (d_x, d_y, d_z),旋转 (rx,ry deg) 和平面方程参数 (A, B, C, D)    initial_guess = [30, 0, 250, 0, 20 , 1, 0, 0, 0]      # 猜测值 x=30,y=0,z=250,rx =0, ry =20, A =1 ,B=0, C=0, D=0    # 此处理论值TCP为 [31.79,0,229.64,rx=0,ry =38, rz = 0]    # [d_x, d_y, d_z, A, B, C, D]
    # 使用非线性最小二乘法进行优化    result = least_squares(objective_function, initial_guess, args=(poses,sensor))
    # 解析优化结果    TCP = result.x[0:3]  # [d_x, d_y, d_z]    TCP_ORI = result.x[3:5] # [rx, ry]    plane = result.x[5:9]  # [A, B, C, D]
    # 显示结果    print(f"标定后的TCP位置: d_x = {TCP[0]:.2f}, d_y = {TCP[1]:.2f}, d_z = {TCP[2]:.2f}")    print(f"标定后的TCP姿态: r_x = {TCP_ORI[0]:.2f}, r_y = {TCP_ORI[1]:.2f}, r_z = 0")    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, sensor):    # 提取当前优化参数    d = np.array(params[0:3])  # 针尖TCP的偏移量 [d_x, d_y, d_z]    ori = np.array([0,params[4],params[3]])  # 输入参数是rx,ry,rz=0, 修改为RZ, RY, RX 顺序    A = params[5]  # 平面方程 A    B = params[6]  # 平面方程 B    C = params[7]  # 平面方程 C    D = params[8]  # 平面方程 D
    # 初始化误差列表    errors = []
    # 计算每个触碰点的误差    for i in range(0,15):              R1 = quaternion_to_rotation_matrix(poses[1]) # 当前触碰点的旋转矩阵                # ABB 四元数 w,x,y,z        # scipy 四元数 x,y,z,w        Rotation = R.from_quat([poses[1][1],poses[1][2],poses[1][3],poses[1][0]])        R1 = Rotation.as_matrix()
        t1 = poses[0]  # 当前触碰点的平移向量        # 计算点激光原点在基坐标系中的位置 P = R * TCP + t        P = np.dot(R1, d) + t1        Rotation = R.from_euler('ZYX', ori, degrees=True) #ZYX 动态角        R2 = Rotation.as_matrix()        R2 = np.dot(R1,R2)        t2 = [0,0,sensor[2]]        P = np.dot(R2, t2) + P
        # 计算点 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()

运算结果如下：

理论的TCP位置: d_x = 31.792631019, d_y = 0.00, d_z = 229.638935148理论的TCP姿态: r_x = 0.00, r_y = 38.00, r_z = 0标定后的TCP位置: d_x = 31.79, d_y = 0.00, d_z = 229.64标定后的TCP姿态: r_x = 0.00, r_y = 38.00, r_z = 0

以下为RAPID中用于记录机器人tool0位姿和对应激光测距结果的代码

CONST robtarget p1001:=[[1106.923,-62.67398,928.2547],[0.2302114,0.6685829,0.6685824,-0.2302112],[-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];...CONST robtarget p1015:=[[951.1398,-4.736905,926.2714],[0.06837972,0.8789129,-0.4523344,-0.1350182],[-1,0,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];PROC main()      VAR robtarget ptmp:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];      FOR i FROM 1 TO 15 DO          GetDataVal "p"+ValToStr(1000+i),ptmp;          MoveJ ptmp,v100,fine,tool0\WObj:=wobj0;          waittime 0.5;          PulseDO do_dotLaser;          waittime 0.5;          posArr{i}.z:=dotLaserDis;          ! 将对应线激光距离记录      ENDFOR  ENDPROC

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

点激光原点标定

本帖子中包含更多资源