CN113752263B - 一种基于激光传感器的工具标定方法、装置和电子设备 - Google Patents

Abstract

本申请提供一种基于激光传感器的工具标定方法、装置和电子设备，该方法包括：通过机械臂控制工具在激光传感器的传感器坐标系XOY平面做两次圆周运动；通过两次圆周运动的圆心坐标和传感器坐标系相对于基座坐标系的第一位姿关系，确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵；通过机械臂控制工具在离开传感器坐标系的XOY平面时停止运动，读取机械臂当前的多个关节角和工具坐标系相对于传感器坐标系的第二相对位置；基于多个关节角和第二相对位置确定工具坐标系相对于法兰盘末端坐标系的第一相对位置；基于第一旋转矩阵和第一相对位置，确定工具坐标系相对于法兰盘末端坐标系的目标位姿关系。

Description

一种基于激光传感器的工具标定方法、装置和电子设备

技术领域

本申请涉及机械臂领域，具体而言，涉及一种基于激光传感器的工具标定方法、装置和电子设备。

背景技术

现有技术中，机械手臂类型的机器人在作业之前，人们通常使用四点标定法或六点标定法来标定工具相对于机械臂法兰盘末端坐标系的位姿关系。所谓四(六)点标定即：人工操作机械臂使其携带的工具末端以四(六)种不同的姿态到达空间中同一个位置。而判断工具末端是否到达了同一个位置，通常是靠人眼来判断的，因此会产生较大的误差。而且每次人工手动操作机械臂使得工具到达同一个位置往往费时费力、效率不高。

发明内容

本申请实施例提供了一种基于激光传感器的工具标定方法、装置、电子设备和存储介质，目的在于提升工具标定的准确性，本申请可以自动标定工具相对于机械臂法兰盘末端的坐标系的位姿关系。

本申请实施例第一方面提供了一种基于激光传感器的工具标定方法。该方法应用于机械臂，机械臂包括基座和法兰盘，法兰盘末端的设有工具，该方法包括：

通过机械臂控制工具在激光传感器的传感器坐标系XOY平面做两次圆周运动；其中，传感器坐标系XOY平面平行于基座的基座坐标系XOY平面，法兰盘的法兰盘末端坐标系XOY平面平行于传感器坐标系XOY平面；通过两次圆周运动的圆心坐标和传感器坐标系相对于基座坐标系的第一位姿关系，确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵；通过机械臂控制工具在离开传感器坐标系的XOY平面时停止运动，读取机械臂当前的多个关节角和工具坐标系相对于传感器坐标系的第二相对位置；基于多个关节角和第二相对位置确定工具坐标系相对于法兰盘末端坐标系的第一相对位置；基于第一旋转矩阵和第一相对位置，确定工具坐标系相对于法兰盘末端坐标系的目标位姿关系。

于一实施例中，控制工具在激光传感器的传感器坐标系XOY平面做两次圆周运动，包括：依据指定运动半径和指定角速度，控制工具在激光传感器的传感器坐标系XOY平面做第一次圆周运动；控制法兰盘沿法兰盘末端坐标系的Z轴下降指定高度；在下降完成之后，依据指定运动半径和指定角速度，控制工具在激光传感器的传感器坐标系XOY平面做第二次圆周运动。

于一实施例中，在确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵之前，还包括：基于工具在第一次圆周运动时，多次挡住激光传感器的激光射线的时刻，以及指定角速度，确定每两个相邻时刻之间形成的第一圆心角；基于多个第一圆心角和指定运动半径，确定第一次圆周运动的圆心坐标；基于工具在第二次圆周运动时，多次挡住激光传感器的激光射线的时刻，以及指定角速度，确定每两个相邻时刻之间形成的第二圆心角；基于多个第二圆心角和指定运动半径，确定第二次圆周运动的圆心坐标。

于一实施例中，确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵，包括：基于指定高度和两次圆周运动的圆心坐标，确定工具坐标系的Z轴在传感器坐标系下的第一方向向量；基于第一方向向量确定工具坐标系相对于传感器坐标系的第二旋转矩阵；基于工具在做两次圆周运动时，任一时刻读取的多个关节角，确定基座坐标系相对于法兰盘末端坐标系的第三旋转矩阵；基于第一位姿关系、第二旋转矩阵和第三旋转矩阵，确定工具坐标系相对于法兰盘末端坐标系的第一旋转矩阵。

于一实施例中，通过机械臂控制工具在离开传感器坐标系的XOY平面时停止运动，包括：控制机械臂调整姿态，使工具坐标系的Z轴垂直于传感器坐标系的XOY平面；通过机械臂控制工具平移，当工具挡住激光传感器的任一条激光射线时，控制工具沿工具坐标系的Z轴反向移动；控制工具在离开传感器坐标系的XOY平面时停止运动。

于一实施例中，基于多个关节角和第二相对位置确定工具坐标系相对于法兰盘末端坐标系的第一相对位置，包括：基于多个关节角，确定基座坐标系相对于法兰盘末端坐标系的第二位姿关系；基于第一位姿关系、第二位姿关系与第二相对位置，确定工具坐标系相对于法兰盘末端坐标系的第一相对位置。

于一实施例中，传感器坐标系由激光传感器发出的两束相互垂直的激光射线作为X轴和Y轴。

本申请实施例第二方面提供了一种基于激光传感器的工具标定装置，包括：控制模块、第一确定模块、读取模块、第二确定模块和第三确定模块。

其中，控制模块用于通过机械臂控制工具在激光传感器的传感器坐标系XOY平面做两次圆周运动；传感器坐标系XOY平面平行于基座的基座坐标系XOY平面，法兰盘的法兰盘末端坐标系XOY平面平行于传感器坐标系XOY平面；第一确定模块用于通过两次圆周运动的圆心坐标和传感器坐标系相对于基座坐标系的第一位姿关系，确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵；读取模块用于通过机械臂控制工具在离开传感器坐标系的XOY平面时停止运动，读取机械臂当前的多个关节角和工具坐标系相对于传感器坐标系的第二相对位置；第二确定模块用于基于多个关节角和第二相对位置确定工具坐标系相对于法兰盘末端坐标系的第一相对位置；第三确定模块用于基于第一旋转矩阵和第一相对位置，确定工具坐标系相对于法兰盘末端坐标系的目标位姿关系。

本申请实施例第三方面提供了一种电子设备，包括：处理器和用于存储处理器可执行指令的存储器。其中，处理器被配置为执行成本申请实施例第一方面及其任一实施例的工具标定方法。

本申请实施例第四方面提供了一种计算机可读存储介质，存储介质存储有计算机程序，计算机程序可由处理器执行以完成成本申请实施例第一方面及其任一实施例的工具标定方法。

本申请提供的基于激光传感器的工具标定方法、装置、电子设备和存储介质，通过机械臂控制法兰盘与工具以不同姿态运动并读取相关参数，再通过计算得到工具相对于机械臂法兰盘末端的坐标系自动标定的位姿关系，减小了误差，节省了人力，提高了自动标定工具的工作效率。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图作简单地介绍。

图1为本申请一实施例提供的电子设备的结构示意图；

图2为本申请一实施例提供的工具标定组件的应用场景示意图；

图3为本申请一实施例提供的工具标定方法的流程示意图；

图4为本申请一实施例提供的步骤S200的细节流程示意图；

图5为本申请一实施例提供的圆心角的计算辅助示意图；

图6为本申请一实施例提供的步骤S200的细节流程示意图；

图7为本申请一实施例提供的步骤S300的细节流程示意图；

图8为本申请一实施例提供的第一方向向量的计算辅助示意图；

图9为本申请一实施例提供的步骤S400的细节流程示意图；

图10为本申请一实施例提供的步骤S500的细节流程示意图；

图11为本申请一实施例提供的工具标定装置的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行描述。

相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参看图1，为本申请一实施例提供的电子设备1的结构示意图。请参看图1，电子设备1包括至少一个处理器12和存储器10，图1中以一个处理器12为例。处理器12和存储器10通过总线11连接，存储器10存储有可被至少一个处理器12执行的指令，指令被至少一处理器12执行，以使至少一个处理器12执行如下述实施例中的基于激光传感器的工具标定方法，为表述方便，后续以计算机为执行主体进行描述。

请参看图2,为本申请一实施例提供的工具标定组件2的应用场景示意图。如图2所示，本申请实施例提供了一种基于激光传感器的工具标定组件2，包括：机械臂01、工具04和激光位移传感器06。其中，机械臂01包括基座02和法兰盘03，法兰盘03的末端设有工具04，基座02固定且保持位置不变。另外机械臂01旁还设有激光传感器06的工作台07，工作台07固定且位置保持不变，工作台07上安装有套环05，套环05内设置有激光传感器06，包括第一激光传感器和第二激光传感器，第一激光传感器和第二激光传感器可以发射出两道互相垂直的激光射线，且工具04在套环05内运动时，若恰好遮挡住激光射线，激光传感器06可以自动识别并读取工具04遮挡位置。

其中，机械臂01的基座02固定，以基座02的端面的中心点为原点，在基座02的端面上建立XOY平面，以通过原点且垂直于XOY平面的基座中心轴线为Z轴，得到基座坐标系。为便于表述，该基座坐标系命名为B；X轴命名为

轴，Z轴命名为/>

轴，Y轴命名为/>

轴。

以法兰盘03的末端端面的中心点为原点，在法兰盘03的末端端面上建立XOY平面，以通过原点且垂直于XOY平面的法兰盘中心轴线为Z轴，得到法兰盘末端坐标系。为便于表述，该法兰盘末端坐标系命名为E，X轴命名为

轴，Z轴命名为/>

轴。

激光传感器06包括的第一激光传感器和第二激光传感器发射出两道互相垂直的激光射线，以两道激光射线为X轴和Y轴，以两道激光射线的交点为原点，建立XOY平面，以通过原点且垂直于XOY平面的直线为Z轴，得到传感器坐标系。为便于表述，该传感器坐标系命名为S，X轴命名为

轴，Y轴命名为/>

轴，Z轴命名为/>

轴。

对于尖端型工具04而言，以工具轴末端为原点，以通过原点且与工具轴轴线重合的直线为Z轴，X轴和Y轴只需要保证与Z轴两两互相垂直即可，得到工具坐标系。为便于表述，该工具坐标系命名为B，X轴命名为

轴，Z轴命名为/>

轴。

于本申请其他实施例中，对于上述各个坐标系的X轴、Y轴和Z轴，三者命名可以互相交换或另作命名，对应的各个坐标系XOY平面也可以推广为YOZ平面、XOZ平面或其他。

请参看图3,为本申请一实施例提供的工具标定方法的流程示意图。如图3所示，本申请实施例提供了一种基于激光传感器的工具标定方法，包括：

S200：通过机械臂控制工具在激光传感器的传感器坐标系XOY平面做两次圆周运动。

在该步骤中，计算机通过机械臂控制法兰盘移动，在两次圆周运动过程中，使法兰盘末端坐标系XOY平面平行于传感器坐标系XOY平面，即法兰盘末端坐标系的Z轴与传感器坐标系的Z轴平行。

S300：通过两次圆周运动的圆心坐标和传感器坐标系相对于基座坐标系的第一位姿关系，确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵。

其中，第一位姿关系包括传感器坐标系相对于基座坐标系的相对位置关系与旋转变换关系，因为套环与机械臂基座位置固定，传感器坐标系与基座坐标系位置始终不变，故第一位姿关系始终不变。

第一旋转矩阵用于指示工具坐标系相对于法兰盘末端坐标系的旋转变换关系。

S400：通过机械臂控制工具在离开传感器坐标系的XOY平面时停止运动，读取机械臂当前的多个关节角和工具坐标系相对于传感器坐标系的第二相对位置。

其中，第二相对位置为工具末端恰好离开传感器坐标系的XOY平面时，工具坐标系相对于传感器坐标系的位置关系。

以图2为例，机械臂在控制工具做圆周运动时穿过套环发出激光射线的水平位置，即工具穿过传感器坐标系XOY平面。做完两次圆周运动后移动离开套环，工具末端恰好离开传感器坐标系XOY平面时停止运动，此时，读取机械臂当前的多个关节角。通过此时工具末端在传感器坐标系中的坐标，得到第二相对位置，即为工具坐标系相对于传感器坐标系的位置关系。

S500：基于多个关节角和第二相对位置确定工具坐标系相对于法兰盘末端坐标系的第一相对位置。

其中，第一相对位置为工具坐标系相对于法兰盘末端坐标系的位置关系。计算机基于当前自动读取的机械臂的多个关节角，可以根据正运动学知识进行计算，求得此时基座坐标系相对于法兰盘末端坐标系的位姿关系。在获得该位姿关系之后，基于该位姿关系、第一位姿关系与第二相对位置计算机求解出工具坐标系相对于法兰盘末端坐标系的第一相对位置。

S600：基于第一旋转矩阵和第一相对位置，确定工具坐标系相对于法兰盘末端坐标系的目标位姿关系。

根据第一旋转矩阵和第一相对位置，构建目标位姿关系。该目标位姿关系可以通过如下公式(1)表示：

其中，

为工具坐标系相对于法兰盘末端坐标系的目标位姿关系；/>

为工具坐标系相对于法兰盘末端坐标系的第一旋转矩阵；^EP_tool-ORG为工具坐标系相对于法兰盘末端坐标系的第一相对位置。

通过上述措施，即完成工具坐标系相对于法兰盘末端坐标系的目标位姿关系标定。

请参看图4，为本申请一实施例提供的步骤S200的细节流程示意图。如图4所示，控制工具在激光传感器的传感器坐标系XOY平面做两次圆周运动包括：

S211：依据指定运动半径和指定角速度，控制工具在激光传感器的传感器坐标系XOY平面做第一次圆周运动。

在该步骤中，根据提前预设好的指定运动半径和指定角速度，计算机通过机械臂控制法兰盘，使法兰盘末端坐标系XOY平面始终平行于传感器坐标系XOY平面，并带动工具穿过套环做第一次圆周运动。其中，工具坐标系的Z轴需要穿过传感器坐标系XOY平面。

请参见图5，为本申请一实施例提供的圆心角的计算辅助示意图，工具穿过传感器坐标系XOY平面内第一次圆周运动，进而形成如图5所示的，在传感器坐标系XOY平面下的圆周运动轨迹。在运动过程中工具轴到达激光射线所在位置时，会遮挡住激光射线，此时传感器可以识别到激光射线被遮挡的位置。于本申请实施例中，工具所做的圆周运动穿过传感器坐标系XOY平面的四个象限，与传感器坐标系的X轴、Y轴各有两个交点。

于本申请其他实施例中，工具所做的圆周运动可以穿过传感器XOY平面的四个象限，也可以不穿过传感器XOY平面的四个象限，但必须与传感器坐标系的X轴、Y轴有交点。

S212：控制法兰盘沿法兰盘末端坐标系的Z轴下降指定高度。

在该步骤中，计算机控制机械臂使法兰盘下降指定高度时，法兰盘末端坐标系XOY平面始终平行于传感器坐标系XOY平面，即保持法兰盘末端坐标系中的Z轴与传感器坐标系的Z轴平行。

其中，指定高度为预设的法兰盘末端端面下降的高度，即此时沿法兰盘末端坐标系中的Z轴下降的高度。

S213：在下降完成之后，依据指定运动半径和指定角速度，控制工具在激光传感器的传感器坐标系XOY平面做第二次圆周运动。

在该步骤中，控制法兰盘沿法兰盘末端坐标系的Z轴下降指定高度以后，通过机械臂控制法兰盘，指定运动半径和指定角速度与第一次圆周运动相同，使法兰盘末端坐标系XOY平面始终平行于传感器坐标系XOY平面，并带动工具穿过套环做第二次圆周运动。其中，工具坐标系的Z轴可以穿过传感器坐标系XOY平面。

请参看图5，工具穿过传感器坐标系XOY平面内第二次圆周运动，进而形成在传感器坐标系XOY平面下的圆周运动轨迹，其过程与S213基本相同，在此不再赘述。于本申请实施例中，工具所做的圆周运动穿过传感器坐标系XOY平面的四个象限，与传感器坐标系的X轴、Y轴各有两个交点。

于本申请其他实施例中，工具所做的圆周运动可以穿过传感器XOY平面的四个象限，也可以不穿过传感器XOY平面的四个象限，但必须与传感器坐标系的X轴、Y轴有交点。

其中，第二次圆周运动形成的圆周运动轨迹，与第一次圆周运动形成的圆周运动轨迹不重合。

于一实施例中，请参看图6，为本申请一实施例提供的步骤S200的细节流程示意图。如图6所示，在确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵之前，还包括：

S221：基于工具在第一次圆周运动时，多次挡住激光传感器的激光射线的时刻，以及指定角速度，确定每两个相邻时刻之间形成的第一圆心角。

工具在做第一次圆周运动过程中，会多次挡住激光传感器。激光传感器可以自动记录工具多次遮挡激光射线的时刻。计算机可以根据预设的指定角速度，以及多个遮挡激光射线的时刻，计算得到相邻时刻之间形成的多个第一圆心角。

示例性的，如图5所示，激光传感器自动记录工具在做第一次圆周运动过程，前五次遮挡激光射线的时刻，工具做第一次圆周运动第一次挡住激光射线的时刻为t₀，第二次挡住激光射线的时刻为t₁，则相邻时刻运动形成的第一圆心角为图5所示的α角；同样地，第二次、第三次挡住激光射线的时刻t₁、t₂之间运动形成的第一圆心角为β，依次推类至其他相邻时刻之间运动形成的第一圆心角γ和θ。多个第一圆心角求解公式(2)如下：

其中，ω为指定角速度；t₀为工具做第一次圆周运动中第一次挡住激光射线的时刻；t₁为工具做第一次圆周运动中第二次挡住激光射线的时刻；t₂为工具做第一次圆周运动中第三次挡住激光射线的时刻；t₃为工具做第一次圆周运动中第四次挡住激光射线的时刻；t₄为工具做第一次圆周运动中第五次挡住激光射线的时刻；α为t₀、t₁时刻之间运动形成的第一圆心角；β为t₁、t₂时刻之间运动形成的第一圆心角；γ为t₂、t₃时刻之间运动形成的第一圆心角；θ为t₃、t₄时刻之间运动形成的第一圆心角。

S222：基于多个第一圆心角和指定运动半径，确定第一次圆周运动的圆心坐标。

计算机可以根据激光传感器记录第一次圆周运动过程，工具多次遮挡激光射线的位置，判断运动形成的圆形轨迹的圆心所在象限。计算机可以根据圆心所在象限、指定运动半径与步骤S221中确定的多个第一圆心角，求解第一次圆周运动的圆心坐标。

示例性的，如图5所示，根据三角公式和步骤S221中确定的多个第一圆心角确定第一次圆周运动的圆心在传感器坐标系中的坐标的求解公式(3)如下：

其中，^Sx₁为第一次圆周运动圆心的x轴坐标；^Sy₁为第一次圆周运动圆心的y轴坐标；^Sz₁为第一次圆周运动圆心的z轴坐标；α为t₀、t₁时刻之间运动形成的第一圆心角；β为t₁、t₂时刻之间运动形成的第一圆心角；γ为t₂、t₃时刻之间运动形成的第一圆心角。

于本申请其他实施例中，针对工具所做的第一次圆周运动、初始位置等不同的情况，上述公式不能针对所有情况单一套用，仅作为本申请图5中所示的实施例对圆心坐标进行求解，对其他实施例中圆心运动的求解过程仅供参考。

S223：基于工具在第二次圆周运动时，多次挡住激光传感器的激光射线的时刻，以及指定角速度，确定每两个相邻时刻之间形成的第二圆心角。

在该步骤中，激光传感器自动记录工具在做第二次圆周运动过程，多次遮挡激光射线的时刻，同时根据预设的指定角速度，计算机可以计算得到多次遮挡激光射线过程中，相邻时刻之间形成的第二圆心角，第二圆心角可以为多个。

示例性的，激光传感器自动记录工具在做第二次圆周运动过程，前五次遮挡激光射线的时刻，工具做第二次圆周运动第一次挡住激光射线的时刻为t₅，第二次挡住激光射线的时刻为t₆，则相邻时刻运动形成的第二圆心角为图5所示的α′角；同样地，第二次、第三次挡住激光射线的时刻t₆、t₇之间运动形成的第二圆心角为β′，依次推类至其他相邻时刻之间运动形成的第二圆心角γ′和θ′。该步骤中第二圆心角的求解方法与步骤S221中第一圆心角的求解方法相同，具体请参见步骤S221，在此不再赘述，多个第二圆心角的求解公式(4)如下：

其中，ω为指定角速度；t₅为工具做第二次圆周运动中第一次挡住激光射线的时刻；t₆为工具做第二次圆周运动中第二次挡住激光射线的时刻；t₇为工具做第二次圆周运动中第三次挡住激光射线的时刻；t₈为工具做第二次圆周运动中第四次挡住激光射线的时刻；t₉为工具做第二次圆周运动中第五次挡住激光射线的时刻；α′为t₅、t₆时刻之间运动形成的第二圆心角；β′为t₆、t₇时刻之间运动形成的第二圆心角；γ′为t₇、t₈时刻之间运动形成的第二圆心角；θ′为t₈、t₉时刻之间运动形成的第二圆心角。

S224：基于多个第二圆心角和指定运动半径，确定第二次圆周运动的圆心坐标。

计算机可以根据激光传感器记录第二次圆周运动过程，工具多次遮挡激光射线的位置，判断运动形成的圆形轨迹的圆心所在象限。同时根据圆心所在象限、指定运动半径与步骤S223中确定的多个第二圆心角，求解第二次圆周运动的圆心坐标。

示例性的，该步骤中，第二次圆周运动的圆心坐标求解方法与步骤S222中第二次圆周运动的圆心坐标求解方法相同，根据三角公式和步骤S223中确定的多个第二圆心角确定第二次圆周运动的圆心在传感器坐标系中的坐标的求解公式(5)如下：

其中，^Sx₂为第二次圆周运动圆心的x轴坐标；^Sy₂为第二次圆周运动圆心的y轴坐标；^Sz₂为第二次圆周运动圆心的z轴坐标；α′为t₅、t₆时刻之间运动形成的第二圆心角；β′为t₆、t₇时刻之间运动形成的第二圆心角；γ′为t₇、t₈时刻之间运动形成的第二圆心角。

在一实施例中，参见图7，为本申请一实施例提供的步骤S300的细节流程示意图。如图7所示，确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵，包括：

S321：基于指定高度和两次圆周运动的圆心坐标，确定工具坐标系的Z轴在传感器坐标系下的第一方向向量。

其中，第一方向向量是指工具坐标系的Z轴在传感器坐标系下的向量关系，第一方向向量由工具坐标系的Z轴，在传感器坐标系各坐标轴方向的单位向量关系构成。

在第一次圆周运动工具坐标系的Z轴上取一点作为第一参考点，由第一参考点做多个辅助线，得到相同Z轴上的第二参考点和多个辅助向量。将第一参考点与第二参考点之间的距离定义为第一辅助向量的模长，并基于上述步骤中求解得到的两次圆心坐标与指定高度求得第一方向向量。

示例性地，请参见图8，为本申请一实施例提供的第一方向向量的计算辅助示意图。如图8所示，在第一次圆周运动过程中，法兰盘101带动工具坐标系的Z轴102形成第一圆形轨迹100，其圆心为O₁。在第二次圆周运动过程中，法兰盘201带动工具坐标系的Z轴202形成第二圆形轨迹200，其圆心为O₂。在第一次圆周运动过程中的工具坐标系的Z轴102上任意取一点P₂作为第一参考点，沿传感器坐标系的Z轴方向向下延伸与第二次圆周运动过程中工具坐标系的Z轴202相交于P′₂点；在工具坐标系的Z轴102上再取一P₁点作为第二参考点,使第二辅助向量

平行于传感器坐标系XOY平面。/>

其中，第一辅助向量

的模长L可通过两次圆周运动的圆心坐标与指定高度求解得到，参考公式(6)如下：

其中，L为第一辅助向量

的模长；^Sx₁为第一次圆周运动圆心的x轴坐标；^Sy₁为第一次圆周运动圆心的y轴坐标；^Sx₂为第二次圆周运动圆心的x轴坐标；^Sy₂为第二次圆周运动圆心的y轴坐标；ΔZ为指定高度。

请参看图8，因为在法兰盘下降过程中始终保持法兰盘末端坐标系的XOY平面与传感器坐标系XOY平面平行，根据向量公式与平行四边形法则，可以得到辅助向量之间的关系：

由此可以通过下述公式(7)求解出工具坐标系的Z轴在传感器坐标系下的第一方向向量

其中，

为工具坐标系的Z轴在传感器坐标系下的第一方向向量；/>

为第一辅助向量/>

的模长；L也为第一辅助向量/>

的模长；^Sx₁为第一次圆周运动圆心的x轴坐标；^Sy₁为第一次圆周运动圆心的y轴坐标；^Sx₂为第二次圆周运动圆心的x轴坐标；^Sy₂为第二次圆周运动圆心的y轴坐标；ΔZ为指定高度，其他向量为求解第一方向向量而构建的各辅助向量。

S322：基于第一方向向量确定工具坐标系相对于传感器坐标系的第二旋转矩阵。

于本申请实施例中，对于尖端型工具而言，工具坐标系的Z轴为实际工作时自动标定或接触工件的重要部分，工具坐标系的Z轴在传感器坐标系下的X轴方向向量、Y轴方向向量只需要保证与工具坐标系的Z轴在传感器坐标系下的Z轴方向向量三个向量两两互相垂直，各自模长为1即可，故通过三个乘积为0的方程组，即可求解得到工具坐标系的Z轴在传感器坐标系下的X轴方向向量、Y轴方向向量，方程组公式(8)如下：

其中，

为工具坐标系的Z轴在传感器坐标系下的X轴方向向量；/>

为工具坐标系的Z轴在传感器坐标系下的Y轴方向向量；/>

为工具坐标系的Z轴在传感器坐标系下的第一方向向量。

则通过下述公式(9)可以得到工具坐标系相对于法兰盘末端坐标系的第一旋转矩阵：

其中，

是工具坐标系相对于法兰盘末端坐标系的第一旋转矩阵；/>

为工具坐标系的Z轴在传感器坐标系下的X轴方向向量；/>

为工具坐标系的Z轴在传感器坐标系下的Y轴方向向量；/>

为工具坐标系的Z轴在传感器坐标系下的第一方向向量。

S323：基于工具在做两次圆周运动时，任一时刻读取的多个关节角，确定基座坐标系相对于法兰盘末端坐标系的第三旋转矩阵。

在该步骤中，因为工具在做两次圆周运动时，控制机械臂始终保持法兰盘末端坐标系的XOY平面与传感器坐标系XOY平面平行，即始终保持法兰盘末端坐标系的XOY平面与基座坐标系的XOY平面平行，因此基座坐标系相对于法兰盘末端坐标的旋转变换关系保持不变，基座坐标系相对于法兰盘末端坐标系的第三旋转矩阵始终不变。

通过工具在做两次圆周运动时，任一时刻读取的多个机械臂关节角以及正运动学知识可以求解得到基座坐标系相对于法兰盘末端坐标系的位姿关系，进而求解得到该位姿关系的子集，即为第三旋转矩阵。

S324：基于第一位姿关系、第二旋转矩阵和第三旋转矩阵，确定工具坐标系相对于法兰盘末端坐标系的第一旋转矩阵。

在该步骤中，第一位姿关系在上述步骤S300中通过测量工具直接得到，传感器坐标系相对于基座坐标系的旋转矩阵，是第一位姿关系的子集，也就是第一位姿关系前三行三列元素组成的矩阵；基于传感器坐标系相对于基座坐标系的旋转矩阵、第二旋转矩阵和第三旋转矩阵，计算机可求解得到工具坐标系相对于法兰盘末端坐标系的第一旋转矩阵，求解公式(10)如下。

其中，

是传感器坐标系相对于基座坐标系的旋转矩阵，/>

是/>

的子集，也就是

前三行三列元素组成的矩阵；/>

是基座坐标系相对于法兰盘末端坐标系的第三旋转矩阵；/>

是工具坐标系相对于法兰盘末端坐标系的第一旋转矩阵。

在一实施例中，参见图9，为本申请一实施例提供的步骤S400的细节流程示意图。如图9所示，通过机械臂控制工具在离开传感器坐标系的XOY平面时停止运动，包括：

S411：控制机械臂调整姿态，使工具坐标系的Z轴垂直于传感器坐标系的XOY平面。

在该步骤中，在控制机械臂带动工具完成前两次圆周运动后，控制机械臂调整姿态，因为工具设于法兰盘上，因此也可以控制工具调整姿态，使工具坐标系的Z轴垂直于传感器坐标系的XOY平面。

S412：通过机械臂控制工具平移，当工具挡住激光传感器的任一条激光射线时，控制工具沿工具坐标系的Z轴反向移动。

在该步骤中，计算机通过机械臂控制工具做第三次圆周运动，当工具运动中恰好挡住任一条激光射线时，即控制工具立刻停止圆周运动，通过激光传感器自动读取此时工具在传感器坐标系下的X轴和Y轴坐标，然后控制工具沿工具坐标系的Z轴反向移动，反向移动使工具坐标系的Z轴逐渐离开传感器坐标系的XOY平面。

S413：控制工具在离开传感器坐标系的XOY平面时停止运动。

在该步骤中，当工具末端恰好离开传感器坐标系的XOY平面时，即激光传感器恰好不再识别到遮挡住激光射线的工具时，控制工具停止运动。

在一实施例中，参见图10，为本申请一实施例提供的步骤S500的细节流程示意图。如图10所示，基于多个关节角和第二相对位置确定工具坐标系相对于法兰盘末端坐标系的第一相对位置，该流程可以包括步骤S511-S512:

S511：基于多个关节角，确定基座坐标系相对于法兰盘末端坐标系的第二位姿关系；

当工具末端恰好离开传感器坐标系的XOY平面时，即激光传感器恰好不再识别到遮挡住激光射线的工具时，控制工具停止运动。此时再次读取机械臂的各个关节角，基于此时读取到的多个机械臂关节角，可以通过正运动学知识求取基座坐标系相对于法兰盘末端坐标系的第二位姿关系。

S512：基于第一位姿关系、第二位姿关系与第二相对位置，确定工具坐标系相对于法兰盘末端坐标系的第一相对位置。

通过机械臂控制工具做第三次圆周运动时，当工具运动中恰好挡住任一条激光射线时，即控制工具立刻停止圆周运动，通过激光传感器自动读取此时工具在传感器坐标系下的X轴和Y轴坐标，计算机控制工具沿工具坐标系的Z轴反向移动至恰好离开传感器坐标系的XOY平面，因为此时工具坐标系的Z轴平行于传感器坐标系的Z轴，因此工具末端在传感器坐标系中的X轴、Y轴坐标不变，工具末端在传感器坐标系的Z轴坐标为0，又因为工具坐标系以工具末端为原点，由此可以得到工具坐标系相对于传感器坐标系的第二相对位置。

通过工具末端恰好离开传感器坐标系的XOY平面时，基座坐标系相对于法兰盘末端坐标系的第二位姿关系，计算机可以求解到第二位姿关系对应的子集，即为基座坐标系相对于法兰盘末端坐标系的旋转矩阵与相对位置；同样地，通过第一位姿关系可求解得到对应的子集，即为传感器坐标系相对于基座坐标系的旋转矩阵与相对位置。

通过工具坐标系相对于传感器坐标系的第二相对位置、基座坐标系相对于法兰盘末端坐标系的旋转矩阵与相对位置、传感器坐标系相对于基座坐标系的旋转矩阵与相对位置，计算机可以求解得到工具坐标系相对于法兰盘末端坐标系的第一相对位置。示例性的，求解公式(11)如下：

其中，^EP_tool-ORG为工具坐标系相对于法兰盘末端坐标系的第一相对位置；

为基座坐标系相对于法兰盘末端坐标系的旋转矩阵；/>

为传感器坐标系相对于基座坐标系的旋转矩阵；^SP_tool-ORG为工具坐标系相对于传感器坐标系的第二相对位置；^BP_S-ORG为传感器坐标系相对于基座坐标系的相对位置；^EP'_B-ORG为基座坐标系相对于法兰盘末端坐标系的相对位置。

最后进入上述步骤S600,计算机求解得到工具坐标系相对于法兰盘末端坐标系的目标位姿关系，完成工具位姿关系的标定。

请参看图11，于一实施例中，图11为本申请一实施例提供的基于激光传感器的工具标定装置700。如图11所示，工具标定装置700包括：控制模块710、第一确定模块720、读取模块730、第二确定模块740和第三确定模块750。

其中，控制模块710用于通过机械臂控制工具在激光传感器的传感器坐标系XOY平面做两次圆周运动；传感器坐标系XOY平面平行于基座的基座坐标系XOY平面，法兰盘的法兰盘末端坐标系XOY平面平行于传感器坐标系XOY平面；第一确定模块720用于通过两次圆周运动的圆心坐标和传感器坐标系相对于基座坐标系的第一位姿关系，确定工具的工具坐标系相对于法兰盘末端的法兰盘末端坐标系的第一旋转矩阵；读取模块730用于通过机械臂控制工具在离开传感器坐标系的XOY平面时停止运动，读取机械臂当前的多个关节角和工具坐标系相对于传感器坐标系的第二相对位置；第二确定模块740用于基于多个关节角和第二相对位置确定工具坐标系相对于法兰盘末端坐标系的第一相对位置；第三确定模块750用于基于第一旋转矩阵和第一相对位置，确定工具坐标系相对于法兰盘末端坐标系的目标位姿关系。

上述装置中各个模块的功能和作用的实现过程具体详见上述基于激光传感器的工具标定方法中对应步骤的实现过程，在此不再赘述。

在本申请所提供的几个实施例中，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

本申请实施例提供了一种非暂态电子设备可读存储介质，包括程序，当其藉由电子设备运行时，使得电子设备执行本申请任一实施例的工具标定方法的全部或部分流程。其中，存储介质可为磁盘、光盘、只读存储记忆体(Read-OnlyMemory，ROM)、随机存储记忆体(RandomAccessMemory，RAM)、快闪存储器(FlashMemory)、硬盘(HardDiskDrive，缩写：HDD)或固态硬盘(Solid-StateDrive，SSD)等。存储介质还可以包括上述种类的存储器的组合。

功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

本申请提供的基于激光传感器的工具标定方法、装置、电子设备和存储介质，通过机械臂控制法兰盘与工具以不同姿态运动并读取相关参数，该工具自动标定方法通过计算得到工具相对于机械臂法兰盘末端坐标系的位姿关系，减小了误差，节省了人力，提高了自动标定工具的工作效率。

Claims (8)

1.一种基于激光传感器的工具标定方法，应用于机械臂，所述机械臂包括基座和法兰盘，所述法兰盘末端设有工具，其特征在于，包括：

通过所述机械臂控制所述工具在激光传感器的传感器坐标系XOY平面做两次圆周运动；其中，所述传感器坐标系XOY平面平行于所述基座的基座坐标系XOY平面，所述法兰盘的法兰盘末端坐标系XOY平面平行于所述传感器坐标系XOY平面；

通过两次圆周运动的圆心坐标和所述传感器坐标系相对于基座坐标系的第一位姿关系，确定所述工具的工具坐标系相对于所述法兰盘末端的法兰盘末端坐标系的第一旋转矩阵；

通过所述机械臂控制所述工具在离开所述传感器坐标系的XOY平面时停止运动，读取所述机械臂当前的多个关节角和所述工具坐标系相对于所述传感器坐标系的第二相对位置；

基于所述多个关节角和所述第二相对位置确定所述工具坐标系相对于所述法兰盘末端坐标系的第一相对位置；

基于所述第一旋转矩阵和所述第一相对位置，确定所述工具坐标系相对于所述法兰盘末端坐标系的目标位姿关系；

所述控制所述工具在激光传感器的传感器坐标系XOY平面做两次圆周运动包括：

依据指定运动半径和指定角速度，控制所述工具在所述激光传感器的传感器坐标系XOY平面做第一次圆周运动；

控制所述法兰盘沿法兰盘末端坐标系的Z轴下降指定高度；

在下降完成之后，依据所述指定运动半径和所述指定角速度，控制所述工具在所述激光传感器的传感器坐标系XOY平面做第二次圆周运动；

所述确定所述工具的工具坐标系相对于所述法兰盘末端的法兰盘末端坐标系的第一旋转矩阵包括：

基于所述指定高度和所述两次圆周运动的圆心坐标，确定所述工具坐标系的Z轴在传感器坐标系下的第一方向向量；

基于所述第一方向向量确定工具坐标系相对于传感器坐标系的第二旋转矩阵；

基于所述工具在做两次圆周运动时，任一时刻读取的多个关节角，确定所述基座坐标系相对于所述法兰盘末端坐标系的第三旋转矩阵；

基于所述第一位姿关系、所述第二旋转矩阵和所述第三旋转矩阵，确定所述工具坐标系相对于所述法兰盘末端坐标系的第一旋转矩阵。

2.根据权利要求1所述的方法，其特征在于，在所述确定所述工具的工具坐标系相对于所述法兰盘末端的法兰盘末端坐标系的第一旋转矩阵之前，所述方法还包括：

基于所述工具在所述第一次圆周运动时，多次挡住所述激光传感器的激光射线的时刻，以及所述指定角速度，确定每两个相邻时刻之间形成的第一圆心角；

基于多个第一圆心角和所述指定运动半径，确定所述第一次圆周运动的圆心坐标；

基于所述工具在所述第二次圆周运动时，多次挡住所述激光传感器的激光射线的时刻，以及所述指定角速度，确定每两个相邻时刻之间形成的第二圆心角；

基于多个第二圆心角和所述指定运动半径，确定所述第二次圆周运动的圆心坐标。

3.根据权利要求1所述的方法，其特征在于，所述通过所述机械臂控制所述工具在离开所述传感器坐标系的XOY平面时停止运动，包括：

控制所述机械臂调整姿态，使所述工具坐标系的Z轴垂直于所述传感器坐标系的XOY平面；

通过所述机械臂控制所述工具平移，当所述工具挡住所述激光传感器的任一条激光射线时，控制所述工具沿工具坐标系的Z轴反向移动；

控制所述工具在离开所述传感器坐标系的XOY平面时停止运动。

4.根据权利要求3所述的方法，其特征在于，所述基于所述多个关节角和所述第二相对位置确定所述工具坐标系相对于所述法兰盘末端坐标系的第一相对位置，包括：

基于所述多个关节角，确定所述基座坐标系相对于所述法兰盘末端坐标系的第二位姿关系；

基于所述第一位姿关系、所述第二位姿关系与所述第二相对位置，确定所述工具坐标系相对于所述法兰盘末端坐标系的第一相对位置。

5.根据权利要求1所述的方法，其特征在于，所述传感器坐标系，由所述激光传感器发出的两束相互垂直的激光射线作为X轴和Y轴。

6.一种基于激光传感器的工具标定装置，应用于机械臂，所述机械臂包括基座和法兰盘，所述法兰盘末端的设有工具，其特征在于，包括：

控制模块，用于通过所述机械臂控制所述工具在激光传感器的传感器坐标系XOY平面做两次圆周运动；其中，所述传感器坐标系XOY平面平行于所述基座的基座坐标系XOY平面，所述法兰盘的法兰盘末端坐标系XOY平面平行于所述传感器坐标系XOY平面；

第一确定模块，用于通过两次圆周运动的圆心坐标和所述传感器坐标系相对于基座坐标系的第一位姿关系，确定所述工具的工具坐标系相对于所述法兰盘末端的法兰盘末端坐标系的第一旋转矩阵；

读取模块，用于通过所述机械臂控制所述工具在离开所述传感器坐标系的XOY平面时停止运动，读取所述机械臂当前的多个关节角和所述工具坐标系相对于所述传感器坐标系的第二相对位置；

第二确定模块，用于基于所述多个关节角和所述第二相对位置确定所述工具坐标系相对于所述法兰盘末端坐标系的第一相对位置；

第三确定模块，用于基于所述第一旋转矩阵和所述第一相对位置，确定所述工具坐标系相对于所述法兰盘末端坐标系的目标位姿关系。

7.一种电子设备，其特征在于，所述电子设备包括：

处理器；

用于存储处理器可执行指令的存储器；

其中，所述处理器被配置为执行权利要求1－5任意一项所述的工具标定方法。

8.一种计算机可读存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序可由处理器执行以完成权利要求1－5任意一项所述的工具标定方法。

Images