CN112643674A - 机器人跟随加工工件表面补偿方法、机器人及存储装置 - Google Patents

Abstract

本申请公开了一种机器人跟随加工工件表面补偿方法、机器人及存储装置，机器人上设有围设形成直角三角形的三个激光测距仪，直角三角形的两条直角边分别记为第一距离和第二距离，该方法包括：采集工具中心点与加工工件表面之间的距离，记为第三距离；根据第一距离、第二距离以及第三距离计算工具中心点在工具坐标系中从当前插补点旋转偏移到下一个插补点的姿态偏移量；将当前插补点的姿态从加工工件坐标系变换至工具坐标系中，并根据姿态偏移量进行旋转变换，获得下一个插补点在工具坐标系下的姿态，将下一个插补点的姿态变换为在加工工件坐标系中的姿态。通过上述方式，本申请能够实现机器人跟随加工工件表面进行加工，提高加工精度和效率。

Description

机器人跟随加工工件表面补偿方法、机器人及存储装置

技术领域

本发明涉及机器人技术领域，特别是涉及一种机器人跟随加工工件表面补偿方法、机器人及存储装置。

背景技术

在机器人的工业应用中，有一种应用会要求机器人的工具中心点沿不规则的表面移动，并尽量与该表面相切，例如打磨应用中，要求机器人的末端执行器磨轮与加工表面平行，以得到最好的打磨效果。这类应用实现的一种方式是获得加工工件的建模，使用导入的建模自动生成路径，完成加工，但是在工业上下游环节，加工工件的建模往往不易获得。另一种方式是进行手动示教，但是如果工件表面过于复杂，示教的时长和示教点的数量会急剧增长，而且一旦更换工件需要重新示教。

因此，需要找到一种方式，跟随工件的不规则表面进行加工，并且工具中心点的姿态根据该表面变化。

发明内容

本申请提供一种机器人跟随加工工件表面补偿方法、机器人及存储装置，能够通过改变插补点轨迹的方式使机器人跟随加工工件表面进行加工，提高了加工精度和效率。

为解决上述技术问题，本申请采用的一个技术方案是：提供一种机器人跟随加工工件表面补偿方法，所述机器人上设有三个用于获取所述加工工件表面信息的激光测距仪，所述激光测距仪包括第一激光测距仪、第二激光测距仪以及第三激光测距仪，三个所述激光测距仪围设形成直角三角形，其中，所述第一激光测距仪与所述第二激光测距仪之间的连线垂直于所述第一激光测距仪与所述第三激光测距仪之间的连线，所述第一激光测距仪与所述第二激光测距仪之间的连线为第一距离，所述第一激光测距仪与所述第三激光测距仪之间的连线为第二距离，，所述补偿方法包括：

获取所述加工工件表面的当前插补点在加工工件坐标系中的姿态；

采集工具中心点与所述加工工件表面之间的距离，记为第三距离；

根据所述第一距离、所述第二距离以及所述第三距离计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量；

将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点的姿态变换为在所述加工工件坐标系中的姿态。

根据本申请的一个实施例，所述根据所述第一距离、所述第二距离以及所述第三距离计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量包括：

根据所述第一距离和所述第三距离计算所述工具中心点绕X轴旋转的角度以及根据所述第二距离和所述第三距离计算所述工具中心点绕Y轴旋转的角度；

根据所述工具中心点绕X轴旋转的角度以及所述工具中心点绕Y轴旋转的角度计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到所述下一个插补点的姿态偏移量；

所述姿态偏移量按照如下公式计算：O_Tool＝α_X×α_Y，其中，O_Tool为姿态偏移量，α_X为所述工具中心点绕X轴旋转的角度，α_Y为所述工具中心点绕Y轴旋转的角度。

根据本申请的一个实施例，所述将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点的姿态变换为在所述加工工件坐标系中的姿态，按照如下公式进行：

P_{ORI_witch_o}＝CF×(O_TOOL×(CF^-1×P_ORI))，其中，P_{ORI_witch_o}为下一个插补点在所述加工工件坐标系下的姿态，CF为将所述工具坐标系变换为所述加工工件坐标系的变换矩阵，O_TOOL为所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量，P_ORI为当前插补点在所述加工工件坐标系中的姿态，CF^-1为将所述加工工件变换为所述工具坐标系下的变换矩阵。

根据本申请的一个实施例，所述补偿方法还包括：

获取所述当前插补点在所述加工工件坐标系中的初始位置；

获取所述工具中心点在所述工具坐标系中从所述当前插补点平行于Z轴平移到下一个插补点的位置偏移量；

将所述位置偏移量从所述工具坐标系中变换至所述工件坐标系中并根据所述初始位置和位置偏移量计算所述下一个插补点在所述加工工件坐标系中的目标位置。

根据本申请的一个实施例，所述将所述位置偏移量从所述工具坐标系中变换至所述工件坐标系中并根据所述初始位置计算所述下一个插补点在所述加工工件坐标系中的目标位置，按照如下公式进行：

P_{TCP_witch_o}＝P_TCP+O_TOOL×C，其中，P_{TCP_witch_o}为下一个插补点在所述工件坐标系中的目标位置，C为将所述工具坐标系变换为所述加工工件坐标系的变换矩阵，O_TOOL为位置偏移量，P_TCP为当前插补点在所述工件坐标系中的初始位置。

根据本申请的一个实施例，所述当前插补点和所述下一个插补点之间还可以具有至少一个中间插补点；在所述根据所述第一距离、所述第二距离以及所述第三距离计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量之后以及所述将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点的姿态变换为在所述加工工件坐标系中的姿态之前，还可以包括：

获取所述工具中心点的起点姿态偏移量、起点速度、起点加速度、终点姿态偏移量、终点速度和终点加速度，所述起点姿态偏移量为上一个插补点偏移到所述当前插补点所发生的姿态偏移量，所述终点姿态偏移量为所述当前插补点偏移到下一个插补点的姿态偏移量；

根据所述起点姿态偏移量、所述起点速度、所述起点加速度、所述终点姿态偏移量、所述终点速度、所述终点加速度以及预设的补偿时间按照预设的补偿速度规划策略进行补偿速度规划，获得所述中间插补点的姿态偏移量、偏移速度以及偏移加速度。

根据本申请的一个实施例，所述补偿速度规划策略包括：

P(t)＝at⁵+bt⁴+ct³+dt²+et+f，

V(t)＝5at⁴+4bt³+3ct²+2dt+e，

A(t)＝20at³+12bt²+6ct+2d，

其中，P(t)为中间插补点的姿态偏移量，V(t)为偏移速度，A(t)为偏移加速度，a、b、c、d、e、f为系数；其中，

a＝6P_e/T⁵-6P_s/T⁵-3V_e/T₄-3V_s/T⁴+0.5a_e/T³+0.5a_s/T³；

b＝-15P_e/T⁴+15P_s/T⁴+7V_e/T³+8V_s/T³-a_e/T²+1.5a_s/T²；

c＝10P_e/T³-10P_s/T³-4V_e/T²-6V_s/T²+0.5a_e/T-1.5a_s/T；

d＝a_s/2；

e＝V_s；

f＝P_s；

其中，P_e为终点姿态偏移量，P_s为起点姿态偏移量，V_e为终点速度，V_s为起点速度，a_e为终点加速度，a_s为起点加速度，T为补偿时间，t∈[0，T]。

根据本申请的一个实施例，所述将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点的姿态变换为在所述加工工件坐标系中的姿态包括：

将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述中间插补点的姿态偏移量、偏移速度以及偏移加速度进行旋转变化，获得所述中间插补点在所述工具坐标系下的姿态，将所述中间插补点的姿态变换为在所述加工工件坐标系中的姿态；

将所述中间插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述中间插补点的姿态偏移量、偏移速度以及偏移加速度进行旋转变化，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点的姿态变换为在所述加工工件坐标系中的姿态。

为解决上述技术问题，本申请采用的另一个技术方案是：提供一种机器人，包括：控制器，所述控制器通过所述的方法实现跟随加工工件表面补偿。

为解决上述技术问题，本申请采用的再一个技术方案是：提供一种存储装置，存储有能够实现上述机器人跟随加工工件表面补偿方法的程序文件。

本申请的有益效果是：通过外部激光测距仪为工具中心点进行补偿偏移计算，实现了机器人在执行原本笛卡尔空间时，通过改变原插补点轨迹的方式跟随加工工件表面进行加工，提高了加工精度和效率。

附图说明

图1是本申请实施例的激光测距仪阵的结构示意图；

图2是本申请第一实施例的机器人跟随加工工件表面补偿方法的流程示意图；

图3是本申请第二实施例的机器人跟随加工工件表面补偿方法的流程示意图；

图4是本申请第三实施例的机器人跟随加工工件表面补偿方法的流程示意图；

图5是本申请第一实施例的机器人跟随加工工件表面补偿装置的结构示意图；

图6是本申请实施例的存储装置的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请中的术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括至少一个该特征。本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。本申请实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

本申请实施例的机器人的磨轮上设有三个用于获取加工工件表面信息的激光测距仪，分别为：第一激光测距仪A、第二激光测距仪B和第三激光测距仪C，如图1所示，三个激光测距仪围设形成一个直角三角形，其中，第一激光测距仪与第二激光测距仪之间的连线与第一激光测距仪和第三激光测距仪之间的连线相互垂直，且第一激光测距仪与第二激光测距仪之间的连线垂直于工具坐标系中的X轴，第一激光测距仪和第三激光测距仪之间的连线垂直于工具坐标系中的Y轴，工具中心点位于直角三角形内。其中，第一激光测距仪与第二激光测距仪之间的距离记为第一距离为AB，第一激光测距仪和第三激光测距仪之间的距离记为第二距离为AC。

图2是本申请第一实施例的机器人跟随加工工件表面补偿方法的流程示意图。需注意的是，若有实质上相同的结果，本申请的方法并不以图2所示的流程顺序为限。如图2所示，该方法包括步骤：

步骤S201：获取加工工件表面的当前插补点在加工工件坐标系中的姿态。

步骤S202：采集工具中心点与加工工件表面之间的距离，记为第三距离。

在步骤S202中，第三距离包括由第一激光测距仪采集的工具中心点与加工工件表面之间的距离l_A、由第二激光测距仪采集的工具中心点与加工工件表面之间的距离l_B、由第三激光测距仪采集的工具中心点与加工工件表面之间的距离l_C。

步骤S203：根据第一距离、第二距离以及第三距离计算工具中心点在工具坐标系中从当前插补点旋转偏移到下一个插补点的姿态偏移量。

在步骤S203中，包括以下步骤，先根据第一距离和第三距离计算工具中心点绕工具坐标系中的X轴旋转的角度以及根据第二距离和第三距离计算工具中心点绕工具坐标系中的Y轴旋转的角度，再根据两个角度，得到工具中心点在工具坐标系中从当前插补点偏移到下一个插补点的姿态偏移量。

具体地，工具中心点绕工具坐标系中的X轴旋转的角度由第一激光测距仪采集的距离l_A和第二激光测距仪采集的距离l_B决定，在工具中心点运动过程中，需要计算工具中心点绕工具坐标系中的X轴旋转的角度，具体可以按照如下公式计算：

α_X＝sign(l_B-l_A)×arcsin(abs(l_A-l_B)/AB)，其中，α_X为工具中心点绕工具坐标系中的X轴旋转的角度，l_A为第一激光测距仪采集的工具中心点与加工工件表面之间的距离，l_B为第二激光测距仪采集的工具中心点与加工工件表面之间的距离，AB为第一激光测距仪与第二激光测距仪之间的距离，sign表示取正负号，abs表示取绝对值。

工具中心点绕工具坐标系中的Y轴旋转的角度由第一激光测距仪采集的距离l_A和第三激光测距仪采集的距离l_C决定，在工具中心点运动过程中，需要计算工具中心点绕工具坐标系中的Y轴旋转的角度，具体按照如下公式计算：

α_Y＝sign(l_C-l_A)×arcsin(abs(l_A-l_C)/AC)，其中，α_Y为工具中心点绕工具坐标系中的Y轴旋转的角度，l_A为第一激光测距仪采集的工具中心点与加工工件表面之间的距离，l_C为第三激光测距仪采集的工具中心点与加工工件表面之间的距离，AC为第一激光测距仪与第三激光测距仪之间的距离，sign表示取正负号，abs表示取绝对值。

工具中心点在工具坐标系中从当前插补点偏移到下一个插补点的姿态偏移量按照如下公式计算：O_Tool＝α_X×α_Y，其中，O_Tool为姿态偏移量。

在本实施例中，工具中心点在工具坐标系中不存在绕Z轴旋转的情况，Z轴的旋转轴心即工件表面的法向，工具中心点绕Z轴旋转对实际应用效果不会产生任何变化，因此，在此不对绕Z轴旋转的情况进行详细说明。

步骤S204：将当前插补点的姿态从加工工件坐标系变换至工具坐标系中，并根据姿态偏移量进行旋转变换，获得下一个插补点在工具坐标系下的姿态并将下一个插补点的姿态变换为在加工工件坐标系中的姿态，将下一个插补点的姿态变换为在加工工件坐标系中的姿态。

在步骤S204中，

下一个插补点在加工工件坐标系中的姿态按照如下公式获得：

P_{ORI_witch_o}＝CF×(O_TOOL×(CF^-1×P_ORI))，其中，P_{ORI_witch_o}为下一个插补点在加工工件坐标系下的姿态，CF为将工具坐标系变换为加工工件坐标系的变换矩阵，O_TOOL为工具中心点在工具坐标系中从当前插补点旋转偏移到下一个插补点的姿态偏移量，P_ORI为当前插补点在加工工件坐标系中的姿态，CF^-1为将加工工件坐标系变换为工具坐标系的变换矩阵。

本申请第一实施例的机器人跟随加工工件表面补偿方法通过外部激光测距仪为工具中心点进行补偿偏移计算，实现了机器人在执行原本笛卡尔空间时，通过改变原插补点轨迹的方式跟随加工工件表面进行加工，提高了加工精度和效率。

机器人跟随加工工件表面变化的速度决定加工精度，因此可以进一步进行速度规划。图3是本申请第二实施例的机器人跟随加工工件表面补偿方法的流程示意图。需注意的是，若有实质上相同的结果，本申请的方法并不以图3所示的流程顺序为限。如图3所示，该方法包括步骤：

步骤S301：获取加工工件表面的当前插补点在加工工件坐标系中的姿态。

在本实施例中，图3中的步骤S301和图2中的步骤S201类似，为简约起见，在此不再赘述。

步骤S302：采集工具中心点与加工工件表面之间的距离，记为第三距离。

在本实施例中，图3中的步骤S302和图2中的步骤S202类似，为简约起见，在此不再赘述。

步骤S303：根据第一距离、第二距离以及第三距离计算工具中心点在工具坐标系中从当前插补点旋转偏移到下一个插补点的姿态偏移量。

在本实施例中，当前插补点和下一个插补点之间还设置至少一个中间插补点，图3中的步骤S303和图2中的步骤S203类似，在此不再赘述。

步骤S304：获取工具中心点的起点姿态偏移量、起点速度、起点加速度、终点姿态偏移量、终点速度和终点加速度，起点姿态偏移量为上一个插补点偏移到当前插补点所发生的姿态偏移量，终点姿态偏移量为当前插补点偏移到下一个插补点的姿态偏移量。

步骤S305：根据起点姿态偏移量、起点速度、起点加速度、终点姿态偏移量、终点速度、终点加速度以及预设的补偿时间按照预设的补偿速度规划策略进行补偿速度规划，获得中间插补点的姿态偏移量、偏移速度以及偏移加速度。

在步骤S305中，补偿时间由用户根据激光测距仪更新采集数据的时间设定，补偿速度规划可以在工具坐标系下完成，补偿速度规划策略可以采取5次多项式，使速度更平稳。具体地，补偿速度规划策略包括：

P(t)＝at⁵+bt⁴+ct³+dt²+et+f，

V(t)＝5at⁴+4bt³+3ct²+2dt+e，

A(t)＝20at³+12bt²+6ct+2d，

其中，P(t)为中间插补点的姿态偏移量，单位为rad，V(t)为偏移速度，单位为rad/s，A(t)为偏移加速度，单位为rad/s²，a、b、c、d、e、f为系数，其中，

a＝6P_e/T⁵-6P_s/T⁵-3V_e/T⁴-3V_s/T⁴+0.5a_e/T³+0.5a_s/T³；

b＝-15P_e/T⁴+15P_s/T⁴+7V_e/T³+8V_s/T³-a_e/T²+1.5a_s/T²；

c＝10P_e/T³-10P_s/T³-4V_e/T²-6V_s/T²+0.5a_e/T-1.5a_s/T；

d＝a_s/2；

e＝V_s；

f＝P_s；

其中，P_e为终点姿态偏移量，单位为弧度(rad)，P_s为起点姿态偏移量，单位为rad，V_e为终点速度，单位为rad/s，V_s为起点速度，单位为rad/s，a_e为终点加速度，单位为rad/s²，a_s为起点加速度，单位为rad/s²，T为补偿时间，单位为s，t∈[0，T]。

在步骤S305中，如果上一次偏移完成，起点速度和起点加速度为0，如果上一次偏移未完成，在0-T时间内，起点速度为e，起点加速度为2d，本实施例的T为预设的补偿时间，每次偏移的终点均到达期望的偏移姿态，终点速度和终点加速度均等于0。若用户输入的补偿时间为T，则t∈[0，T]，输出t时刻的姿态偏移量、偏移速度以及偏移加速度。

步骤S306：将当前插补点的姿态从加工工件坐标系变换至工具坐标系中，并根据中间插补点的姿态偏移量、偏移速度以及偏移加速度进行旋转变化，获得中间插补点在工具坐标系下的姿态，将中间插补点的姿态变换为在加工工件坐标系中的姿态。

在本实施例中，图3中的步骤S306和图2中的步骤S204类似，为简约起见，在此不再赘述。

步骤S307：将中间插补点的姿态从加工工件坐标系变换至工具坐标系中，并根据中间插补点的姿态偏移量、偏移速度以及偏移加速度进行旋转变化，获得下一个插补点在工具坐标系下的姿态，将下一个插补点的姿态变换为在加工工件坐标系中的姿态。

在本实施例中，图3中的步骤S307和图2中的步骤S204类似，为简约起见，在此不再赘述。当前插补点和下一个插补点之间存在多个中间插补点时，重复步骤S306和步骤S307获得每个中间插补点的姿态，直至完成计算下一个插补点的姿态。

本申请第二实施例的机器人跟随加工工件表面补偿方法在第一实施例的基础上，通过补偿速度规划进一步提高机器人跟随加工工件表面的精度和柔顺性。

图4是本发明第三实施例的机器人跟随加工工件表面补偿方法的流程示意图。需注意的是，若有实质上相同的结果，本发明的方法并不以图4所示的流程顺序为限。如图4所示，该方法包括步骤：

步骤S401：获取加工工件表面的当前插补点在加工工件坐标系中的姿态。

在本实施例中，图4中的步骤S401和图2中的步骤S201类似，为简约起见，在此不再赘述。

步骤S402：采集工具中心点与加工工件表面之间的距离，记为第三距离。

在本实施例中，图4中的步骤S402和图2中的步骤S202类似，为简约起见，在此不再赘述。

步骤S403：根据第一距离、第二距离以及第三距离计算工具中心点在工具坐标系中从当前插补点旋转偏移到下一个插补点的姿态偏移量。

在本实施例中，图4中的步骤S403和图2中的步骤S203类似，为简约起见，在此不再赘述。

步骤S404：将当前插补点的姿态从加工工件坐标系变换至工具坐标系中，并根据姿态偏移量进行旋转变换，获得下一个插补点在工具坐标系下的姿态并将下一个插补点的姿态变换为在加工工件坐标系中的姿态，将下一个插补点的姿态变换为在加工工件坐标系中的姿态。

在本实施例中，图4中的步骤S404和图2中的步骤S204类似，为简约起见，在此不再赘述。

步骤S405：获取当前插补点在加工工件坐标系中的初始位置。

步骤S406：获取工具中心点在工具坐标系中从当前插补点平行于Z轴平移到下一个插补点的位置偏移量。

在步骤S406中，位置偏移量为第一激光测距仪采集的工具中心点与加工工件表面之间的距离l_A、第二激光测距仪采集的工具中心点与加工工件表面之间的距离l_B以及第三激光测距仪采集的工具中心点与加工工件表面之间的距离l_C的平均值。

本实施例的步骤S405和步骤S406在执行过程中不分先后顺序，在步骤S406之后，当前插补点和下一个插补点之间存在多个中间插补点时，可以按照图3的步骤S304-305的类似方法进行速度规划获得中间插补点的位置偏移量，在此不再一一赘述。

步骤S407：将位置偏移量从工具坐标系中变换至工件坐标系中并根据初始位置和位置偏移量计算下一个插补点在加工工件坐标系中的目标位置。

在步骤S407中，计算下一个插补点在加工工件坐标系中的目标位置按照如下公式进行：

P_{TCP_witch_o}＝P_TCP+O_TOOL×C，其中，P_{TCP_witch_o}为下一个插补点在工件坐标系中的目标位置，C为将工具坐标系变换为加工工件坐标系的变换矩阵，O_TOOL为位置偏移量，P_TCP为当前插补点在工件坐标系中的初始位置。

当前插补点和下一个插补点之间存在多个中间插补点时，中间插补点在加工工件坐标系中的目标位置可以按照图3的步骤中的计算公式获得。

本申请第三实施例的机器人跟随加工工件表面补偿方法在第一实施例的基础上，机器人跟随加工工件表面同时进行位置和姿态补偿，进一步提高了加工精度和效率，在本实施例中，位置和姿态补偿不分补偿先后顺序。

在本申请中，偏移坐标系与工具坐标系重合，姿态补偿在工具坐标系中进行，在其它方案中，偏移坐标系与工具坐标系也可以不重合，姿态补偿在偏移坐标系下进行，当偏移坐标系与工具坐标系不重合时，偏移坐标系可以选择参考加工工件坐标系或者世界坐标系。如果偏移坐标系选择参考加工工件坐标系，则偏移坐标系变换矩阵为3×3的单位矩阵，如果偏移坐标系选择参考世界坐标系，则偏移坐标系中的姿态等于加工工件在世界坐标系下的姿态。

图5是本申请实施例的机器人跟随加工工件表面补偿装置的结构示意图。如图5所示，该装置50包括获取模块51、采集模块52、第一计算模块53和第二计算模块54。

获取模块51用于获取加工工件表面的当前插补点在加工工件坐标系中的姿态。

采集模块52用于采集工具中心点与加工工件表面之间的距离，记为第三距离。

第一计算模块53与采集模块52耦接，用于根据第一距离、第二距离以及第三距离计算工具中心点在工具坐标系中从当前插补点旋转偏移到下一个插补点的姿态偏移量。

第二计算模块54分别与获取模块51、第一计算模块53耦接，用于将当前插补点的姿态从加工工件坐标系变换至工具坐标系中，并根据姿态偏移量进行旋转变换，获得下一个插补点在工具坐标系下的姿态，将下一个插补点的姿态变换为在加工工件坐标系中的姿态。

本申请实施例提供一种机器人，机器人包括控制器，控制器通过上述方法实现跟随加工工件表面补偿。

参阅图6，图6为本申请实施例的存储装置的结构示意图。本申请实施例的存储装置存储有能够实现上述所有方法的程序文件61，其中，该程序文件61可以以软件产品的形式存储在上述存储装置中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储装置包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等终端设备。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

以上仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (10)

1.一种机器人跟随加工工件表面补偿方法，所述机器人上设有三个用于获取所述加工工件表面信息的激光测距仪，其特征在于，所述激光测距仪包括第一激光测距仪、第二激光测距仪以及第三激光测距仪，三个所述激光测距仪围设形成直角三角形，其中，所述第一激光测距仪与所述第二激光测距仪之间的连线垂直于所述第一激光测距仪与所述第三激光测距仪之间的连线，所述第一激光测距仪与所述第二激光测距仪之间的连线为第一距离，所述第一激光测距仪与所述第三激光测距仪之间的连线为第二距离，所述补偿方法包括：

获取所述加工工件表面的当前插补点在加工工件坐标系中的姿态；

采集工具中心点与所述加工工件表面之间的距离，记为第三距离；

根据所述第一距离、所述第二距离以及所述第三距离计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量；

将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态；

将所述下一个插补点在所述工具坐标系下的姿态变换为在所述加工工件坐标系中的姿态。

2.根据权利要求1所述的补偿方法，其特征在于，所述根据所述第一距离、所述第二距离以及第三距离计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量包括：

根据所述第一距离和所述第三距离计算所述工具中心点绕X轴旋转的角度以及根据所述第二距离和所述第三距离计算所述工具中心点绕Y轴旋转的角度；

根据所述工具中心点绕X轴旋转的角度以及所述工具中心点绕Y轴旋转的角度计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到所述下一个插补点的姿态偏移量；

所述姿态偏移量按照如下公式计算：O_Tool＝α_X×α_Y，其中，O_Tool为姿态偏移量，α_X为所述工具中心点绕X轴旋转的角度，α_Y为所述工具中心点绕Y轴旋转的角度。

3.根据权利要求2所述的补偿方法，其特征在于，所述将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点在所述工具坐标系下的姿态变换为在所述加工工件坐标系中的姿态，按照如下公式进行：

P_{ORI_witch_o}＝CF×(O_TOOL×(CF^-1×P_ORI))，其中，P_{ORI_witch_o}为下一个插补点在所述加工工件坐标系下的姿态，CF为将所述工具坐标系变换为所述加工工件坐标系的变换矩阵，O_TOOL为所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量，P_ORI为当前插补点在所述加工工件坐标系中的姿态，CF^-1为将所述加工工件坐标系变换为所述工具坐标系的变换矩阵。

4.根据权利要求1所述的补偿方法，其特征在于，所述补偿方法还包括：

获取所述当前插补点在所述加工工件坐标系中的初始位置；

获取所述工具中心点在所述工具坐标系中从所述当前插补点平行于Z轴平移到下一个插补点的位置偏移量；

将所述位置偏移量从所述工具坐标系中变换至所述工件坐标系中，并根据所述初始位置和所述位置偏移量计算所述下一个插补点在所述加工工件坐标系中的目标位置。

5.根据权利要求4所述的补偿方法，其特征在于，所述将所述位置偏移量从所述工具坐标系中变换至所述工件坐标系中并根据所述初始位置计算所述下一个插补点在所述加工工件坐标系中的目标位置，按照如下公式进行：

P_{TCP_witch_o}＝P_TCP+O_TOOL×C，其中，P_{TCP_witch_o}为下一个插补点在所述工件坐标系中的目标位置，C为将所述工具坐标系变换为所述加工工件坐标系的变换矩阵，O_TOOL为位置偏移量，P_TCP为当前插补点在所述工件坐标系中的初始位置。

6.根据权利要求1所述的补偿方法，其特征在于，所述当前插补点和所述下一个插补点之间具有至少一个中间插补点；在所述根据所述第一距离、所述第二距离以及所述第三距离计算所述工具中心点在工具坐标系中从所述当前插补点旋转偏移到下一个插补点的姿态偏移量之后以及所述将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点的姿态变换为在所述加工工件坐标系中的姿态之前，还包括：

获取所述工具中心点的起点姿态偏移量、起点速度、起点加速度、终点姿态偏移量、终点速度和终点加速度，所述起点姿态偏移量为上一个插补点偏移到所述当前插补点所发生的姿态偏移量，所述终点姿态偏移量为所述当前插补点偏移到下一个插补点的姿态偏移量；

根据所述起点姿态偏移量、所述起点速度、所述起点加速度、所述终点姿态偏移量、所述终点速度、所述终点加速度以及预设的补偿时间按照预设的补偿速度规划策略进行补偿速度规划，获得所述中间插补点的姿态偏移量、偏移速度以及偏移加速度。

7.根据权利要求6所述的补偿方法，其特征在于，所述补偿速度规划策略包括：

P(t)＝at⁵+bt⁴+ct³+dt²+et+f，

V(t)＝5at⁴+4bt³+3ct²+2dt+e，

A(t)＝20at³+12bt²+6ct+2d，

其中，P(t)为中间插补点的姿态偏移量，V(t)为偏移速度，A(t)为偏移加速度，a、b、c、d、e、f为系数；其中，

a＝6P_e/T⁵-6P_s/T⁵-3V_e/T⁴-3V_s/T⁴+0.5a_e/T³+0.5a_s/T³；

b＝-15P_e/T⁴+15P_s/T⁴+7V_e/T³+8V_s/T³-a_e/T²+1.5a_s/T²；

c＝10P_e/T³-10P_s/T³-4V_e/T²-6V_s/T²+0.5a_e/T-1.5a_s/T；

d＝a_s/2；

e＝V_s；

f＝P_s；

其中，P_e为终点姿态偏移量，P_s为起点姿态偏移量，V_e为终点速度，V_s为起点速度，a_e为终点加速度，a_s为起点加速度，T为补偿时间，t∈[0，T]。

8.根据权利要求7所述的补偿方法，其特征在于，所述将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述姿态偏移量进行旋转变换，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点在所述工具坐标系下的姿态变换为在所述加工工件坐标系中的姿态包括：

将所述当前插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述中间插补点的姿态偏移量、偏移速度以及偏移加速度进行旋转变化，获得所述中间插补点在所述工具坐标系下的姿态，将所述中间插补点的姿态变换为在所述加工工件坐标系中的姿态；

将所述中间插补点的姿态从所述加工工件坐标系变换至所述工具坐标系中，并根据所述中间插补点的姿态偏移量、偏移速度以及偏移加速度进行旋转变化，获得所述下一个插补点在所述工具坐标系下的姿态，将所述下一个插补点在所述工具坐标系下的姿态变换为在所述加工工件坐标系中的姿态。

9.一种机器人，其特征在于，所述机器人包括控制器，所述控制器通过权利要求1至8任意一项所述的方法实现跟随加工工件表面补偿。

10.一种存储装置，其特征在于，存储有能够实现如权利要求1-8中任一项所述的机器人跟随加工工件表面补偿方法的程序文件。

Images