[发明专利]基于激光跟踪仪的机器人末端执行器坐标系标定方法

权利要求书

1.一种基于激光跟踪仪的机器人末端执行器坐标系标定方法，其特征在于：包括以下步骤：

步骤1：在机器人附近放置激光跟踪仪，预热激光跟踪仪，激光跟踪仪建立默认的激光跟踪仪坐标系x_my_mz_m；进给电机调至零位，缩回机器人末端执行器上的压力鼻，将标定板固定在机器人工作台上，调整机器人使压力鼻距离标定板平面为标准值l_tp，并对标定板调平，使机器人末端执行器的主轴轴线垂直于标定板；机器人末端执行器的相机能够同时拍到标定板上呈梅花状分布的5个孔，且安装在主轴刀柄上的激光跟踪仪靶球能够接收到激光跟踪仪的激光束；示教当前点为机器人HOME1点，之后标定过程中机器人都是从HOME1点出发，保证激光跟踪仪在标定坐标系过程中有固定的参考点；所述机器人为六轴关节机器人；

步骤2：测量法兰坐标系：

步骤2.1：机器人从HOME1点出发，按一个方向绕第六轴旋转，用测量软件记录旋转过程中靶球的坐标，直至靶球接收不到激光跟踪仪的激光束；机器人返回HOME1点，再绕第六轴反方向旋转，用测量软件记录旋转过程中靶球的坐标，直至靶球接收不到激光跟踪仪的激光束；根据记录的靶球坐标拟合出第六轴旋转圆和第六轴旋转平面；机器人返回HOME1点，按一个方向绕第五轴旋转，用测量软件记录旋转过程中靶球的坐标，直至靶球接收不到激光跟踪仪的激光束；机器人返回HOME1点，再绕第五轴反方向旋转，用测量软件记录旋转过程中靶球的坐标，直至靶球接收不到激光跟踪仪的激光束；根据记录的靶球坐标拟合出第五轴旋转圆和第五轴旋转平面；

步骤2.2：利用测量软件测得第五轴旋转圆的圆心O₅点到第六轴旋转平面的距离D₅₆，进而得到法兰坐标系x_Fy_Fz_F原点O_F到第六轴旋转面的距离D_F6为：

D_F6＝D₅₆-H

其中H为第五轴旋转圆的圆心O₅点到法兰坐标系的原点距离；以第六轴旋转圆的圆心O₆为中心，以第六轴旋转面的法向方向为偏移方向，向上偏移D_F6，得到的点即为法兰坐标系x_Fy_Fz_F的原点O_F(x_mF,y_mF,z_mF)；

步骤2.3：机器人从HOME1点出发，使机器人沿着法兰坐标系x_Fy_Fz_F的x_F方向移动，拟合向量再沿着法兰坐标系的y_F方向移动，拟合向量将第六轴旋转平面方向向下的法线作为法兰坐标系x_Fy_Fz_F的z_F正方向，分别比较z_F正方向和向量向量的夹角：

若上述两个夹角均在89.95°～90.05°范围内，则取靠近90°的夹角对应的向量作为准确方向；若只有一个夹角在89.95°～90.05°范围内，则取该夹角对应的向量为准确方向；若两个夹角均不在89.95°～90.05°范围内，则重新进行本步骤；

步骤2.4：采用z_F正方向和步骤2.3确定的准确方向组建平面，并得到所建平面的法向，从而得到法兰坐标系y_F正方向和x_F正方向；在激光跟踪仪控制软件中以原点O_F、x_F正方向、y_F正方向构建法兰坐标系x_Fy_Fz_F，得到其在激光跟踪仪坐标系x_my_mz_m下的位姿矩阵T_mF：

TmF=nmFomFamFTransmF0001]]>

其中o_mF为单位向量，

n_mF＝o_mF×a_mF

Trans_mF＝(x_mF,y_mF,z_mF)^T

步骤3：标定工具坐标系：

步骤3.1：机器人从HOME1点出发，进给电机直线运动，拟合进给电机的进给直线，方向指向加工方向作为工具坐标系x_ty_tz_t的x_t正方向；机器人保持在HOME1点不动，用激光跟踪仪标定压力鼻平面，将压力鼻平面沿工具坐标系x_ty_tz_t的x_t正方向偏移，偏移量为压力鼻长度l_tp，模拟出理论工件平面，理论工件平面和拟合的进给电机进给直线交点O_t(x_mt,y_mt,z_mt)为工具中心点；

步骤3.2：将法兰坐标系x_Fy_Fz_F的x_F正方向投影到工具坐标系x_ty_tz_t的x_t正方向的切面上，得到工具坐标系x_ty_tz_t的z_t正方向，再根据右手定则，得到工具坐标系x_ty_tz_t的y_t正方向；以工具坐标系x_ty_tz_t的工具中心点、x_t正方向、y_t正方向、z_t正方向构建工具坐标系x_ty_tz_t，得到其在激光跟踪仪坐标系x_my_mz_m下的位姿矩阵T_mt：

Tmt=nmtomtamtTransmt0001]]>

nmt=xt→|xt→|]]>

amt=yF→×xt→|yF→×xt→|]]>

o_mt＝n_mt×a_mt

Trans_mt＝(x_mt,y_mt,z_mt)^T

步骤4：构建标定板坐标系x_by_bz_b，并得到标定板坐标系x_by_bz_b在激光跟踪仪坐标系x_my_mz_m下的位姿矩阵T_mb：

步骤4.1：将靶球放在靶标座上，分别将靶标座放入到标定板中的五个孔中，保持靶标座下表面和标定板平面贴紧，测量五个孔在激光跟踪仪坐标系x_my_mz_m下的坐标值P_mb10(x_mb10,y_mb10,z_mb10)、P_mb20(x_mb20,y_mb20,z_mb20)、P_mb30(x_mb30,y_mb30,z_mb30)、P_mb40(x_mb40,y_mb40,z_mb40)、P_mb50(x_mb50,y_mb50,z_mb50)；

步骤4.2：将标定板固定在机器人工作平台上，用靶球在标定板平面上均匀采集若干个不共线的点，排除靶球的半径尺寸，在激光跟踪仪坐标系下构建标定板平面；利用测量软件将测量的五个孔投影到标定板平面上，得到五个孔的投影点分别为P_mb1(x_mb1,y_mb1,z_mb1)、P_mb2(x_mb2,y_mb2,z_mb2)、P_mb3(x_mb3,y_mb3,z_mb3)、P_mb4(x_mb4,y_mb4,z_mb4)和P_mb5(x_mb5,y_mb5,z_mb5)；

步骤4.3：以五个孔中的中心孔的投影P_mb1为原点，以P_mb1指向孔2的投影P_mb2方向为标定板坐标系x_by_bz_b的x_b轴正方向，得到单位向量为：

以标定板的法向且垂直标定板向下为标定板坐标系x_by_bz_b的z_b轴正方向，其单位向量为：

由右手定则得到标定板坐标系x_by_bz_b的y_b轴正方向，其单位向量为：

o_mb0＝a_mb0×n_mb0

标定板坐标系x_by_bz_b在激光跟踪仪坐标系x_my_mz_m下的位姿矩阵为：

Tmb=nmbombambTransmb0001]]>

其中：

n_mb＝n_mb0

o_mb＝o_mb0

a_mb＝a_mb0

Trans_mb＝(x_mb1 y_mb1 z_mb1)^T

步骤5：标定相机坐标系：

步骤5.1：机器人保持在HOME1点不动，打开机器人末端执行器的相机并调节相机焦距，直至在相机中清晰看到标定板上呈梅花状分布的5个孔，记录在相机自带坐标系x_p0Oy_p0下五个孔中心的坐标值，P_pb01(x_pb01,y_pb01)、P_pb02(x_pb02,y_pb02)、P_pb03(x_pb03,y_pb03)、P_pb04(x_pb04,y_pb04)、P_pb05(x_pb05,y_pb05)；

步骤5.2：根据步骤4.1得到五个孔中心的坐标值，计算每两个孔之间的像素点距离l_pij：

lpij=(xpb0i-xpb0j)2+(ypb0i-ypb0j)2]]>

其中i、j＝1～5且i≠j，依次得到l_p12、l_p13、l_p14、l_p15、l_p23、l_p24、l_p25、l_p34、l_p35、l_p45；

步骤5.3：根据P_mb1(x_mb1,y_mb1,z_mb1)、P_mb2(x_mb2,y_mb2,z_mb2)、P_mb3(x_mb3,y_mb3,z_mb3)、P_mb4(x_mb4,y_mb4,z_mb4)和P_mb5(x_mb5,y_mb5,z_mb5)计算对应的两个孔之间的实际距离L_pij：

lpij=(xmbi-xmbj)2+(ymbi-ymbj)2+(zmbi-zmbj)2]]>其中i、j＝1～5且i≠j，得到标定板上五个孔之间的实际距离L_p12、L_p13、L_p14、L_p15、L_p23、L_p24、L_p25、L_p34、L_p35、L_p45；依次得到实际每一毫米的像素点数N_ij

Nij=lpijLpij]]>

求出对应的N₁₂、N₁₃、N₁₄、N₁₅、N₂₃、N₂₄、N₂₅、N₃₄、N₃₅、N₄₅的平均值N；

步骤5.4：得到标定板上五个孔在相机坐标系x_py_pz_p下的坐标为P_pb1(x_pb1,y_pb1,z_pb1)、P_pb2(x_pb2,y_pb2,z_pb2)、P_pb3(x_pb3,y_pb3,z_pb3)、P_pb4(x_pb4,y_pb4,z_pb4)、P_pb5(x_pb5,y_pb5,z_pb5)为：

xpbi=xpb0i-L0/2N]]>

ypbi=ypb0i-B0/2N]]>

z_pbi＝0

其中i＝1～5，L₀*B₀为相机像素点实际大小；

步骤5.5：标定板上五个孔中的中心孔在相机坐标系下的坐标点P_pb1(x_pb1,y_pb1,z_pb1)为原点，为标定板坐标系x_by_bz_b的x_b轴正方向，再构建向量按照右手定则从正方向指向x_b轴的正方向，得到标定板坐标系x_by_bz_b的z_b轴正方向，再以x_b轴正方向和z_b轴正方向按照右手定则得出y_b轴正方向，从而得到标定板坐标系x_by_bz_b在相机坐标系x_py_pz_p中的位姿矩阵T_pb：

Tpb=npbopbapbTranspb0001]]>

其中：

o_pb＝a_pb×n_pb

Trans_pb＝(x_pb1y_pb1z_pb1)^T

步骤6：依据得到的激光跟踪仪坐标系x_my_mz_m下，法兰坐标系x_Fy_Fz_F、工具坐标系x_ty_tz_t、标定板坐标系x_by_bz_b的位姿矩阵T_mF、T_mt、T_mb，和标定板坐标系x_by_bz_b在相机坐标系x_py_pz_p下的位姿矩阵T_pb，利用矩阵转换关系，得到在法兰坐标系x_Fy_Fz_F下，工具坐标系x_ty_tz_t的位姿矩阵T_Ft和相机坐标系x_py_pz_p的位姿矩阵T_Fp：

T_Ft＝(T_mF)^-1T_mt

T_mp＝T_mb(T_pb)^-1

T_Fp＝(T_mF)^-1T_mb(T_pb)^-1

步骤7：将工具坐标系x_ty_tz_t、相机坐标系x_py_pz_p在法兰坐标系x_Fy_Fz_F下的位姿矩阵T_Ft和T_Fp的对应参数输入到机器人对应的工具坐标系Tool1和Tool2参数表中，定义Tool1为工具坐标系x_ty_tz_t，Tool2为相机坐标系x_py_pz_p，机器人识别Tool1和Tool2的参数，从而能够在编程人员指定的坐标系中运行工作。