CN108789363B - 一种基于力传感器的直接拖动示教系统及方法 - Google Patents

Abstract

本发明提供了一种基于力传感器的外科手术中的直接拖动示教系统及方法，系统包括：工控机、机器人控制器、机器人、六维力传感器和末端执行器；六维力传感器用于在医生拖动末端执行器进行运动的过程中采集末端执行器的受力信息；工控机对六维力传感器采集的末端执行器的受力信息进行重力补偿，获取医生拖动末端执行器进行运动时的外力信息；工控机根据医生拖动末端执行器进行运动时的外力信息生成机器人运动指令，机器人控制器根据运动指令控制机器人进行运动。本发明在机器人末端加装多维力传感器传达医生的示教意图，引导机器人运动并通过重力补偿消除末端执行器重力的影响，可较为准确地感知医生手部作用力，实现手术过程中灵活准确的拖动示教。

Description

一种基于力传感器的直接拖动示教系统及方法

技术领域

本发明涉及医疗机器人技术领域，具体涉及一种基于力传感器的直接拖动示教系统及方法。

背景技术

口腔种牙手术是在狭小空间内局部麻醉下的精密操作，在使用机器人进行种牙操作的过程中一个关键的步骤是要控制机器人从患者口腔外开始运动到患者口腔内种植初始位置，这一过程要避免机器人与外界环境发生碰撞，保证手术的安全性。如何规划出一条无碰撞的路径有多种方法，可由采用合适的算法由计算机自动计算出路径，也可由医生进行人工示教，考虑到手术的安全性与可靠性，人工示教的方法更合理。

目前，传统工业机器人的示教主要通过示教盒操作实现，但这种方法示教过程繁琐、效率低、且对医生的技术水平要求较高。人机协作型机器人的示教，就是医生直接拖拽机器人的末端移动机器人，实现示教。较之采用示教盒的示教，直接拖拽示教更为灵活、直观、对医生的要求也降低很多。但是一般的人机协作型机器人主要应用于工业生产领域，直接示教需要的拖拽力较大，不够灵活，无法满足医生临床操作需要。

发明内容

针对现有技术中的缺陷，本发明提供一种基于力传感器的直接拖动示教系统及方法，本发明在机器人末端加装多维力传感器传达医生的示教意图，引导机器人运动。

为实现上述目的，本发明提供以下技术方案：

第一方面，本发明提供了一种基于力传感器的直接拖动示教系统，包括：工控机、机器人控制器、机器人、六维力传感器和末端执行器；

所述六维力传感器设置在所述机器人与所述末端执行器之间，用于在医生拖动所述末端执行器进行运动的过程中采集所述末端执行器的受力信息；

所述工控机对所述六维力传感器采集的所述末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息；

所述工控机根据医生拖动所述末端执行器进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动。

进一步地，所述工控机对所述六维力传感器采集的所述末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息，具体包括：

获取预先存储的所述末端执行器在没有受到其他外力作用状况下其处于不同空间姿态时的多组第一测量数据；所述多组第一测量数据由所述六维力传感器采集得到；

根据所述多组第一测量数据获取所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)以及所述末端执行器重力G的大小；

根据所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)，所述末端执行器重力G的大小，以及所述六维力传感器采集的所述末端执行器的受力信息，利用下面公式一获取医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量：

以及，利用下面公式二获取医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量：

其中，F_ex、F_ey、F_ez是医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量；M_ex、M_ey、M_ez是医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量；F_x、F_y、F_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的受力分量，M_x、M_y、M_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的力矩分量；G_x、G_y、G_z是所述六维力传感器采集的力分量F_x、F_y、F_z中由于所述末端执行器重力产生的部分，M_gx、M_gy、M_gz是所述六维力传感器采集的力矩分量M_x、M_y、M_z中由于所述末端执行器重力产生的部分；

其中，上述G_x、G_y、G_z采用下面公式三获取：

其中，上述M_gx、M_gy、M_gz采用下面公式四获取：

其中，所述α、β、γ为在对所述末端执行器安装姿态标定后实时获取到重力方向与六维力传感器坐标系X、Y、Z轴间的夹角。

进一步地，所述工控机根据医生拖动所述末端执行器进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动，具体包括：

所述工控机根据医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量F_ex、F_ey和F_ez判断有效外力F＝(F_ex ²、F_ey ²、F_ez ²)^1/2是否大于第一预设阈值F_s1，若是，则根据预设的第一V-F关系确定所述机器人的运动线速度V₁，并根据所述线速度V₁确定所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z，然后将所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z发送给所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动。

进一步地，所述预设的第一V-F关系为：当F_s1<F<F_s2时，V随F的增大线性增加，V＝k_F*F；当F≥F_s2时，V＝V_max；其中，k_F为预设的比例系数，V_max为预设的速度上限。

进一步地，当医生拖拽所述末端执行器到达指定的操作位置后，若需要进行钻孔操作，则按照如下方式进行处理：

获取医生拖动所述末端执行器进行运动时的有效外力F在钻孔方向上的投影

其中，钻孔方向的方向向量为

作用力为

判断在钻孔方向上的有效外力

是否大于第二预设阈值F_s2，若是，则根据预设的第二V-F关系确定所述机器人在钻孔方向上的钻孔速度V₂，并将所述钻孔速度V₂发送给所述机器人控制器，以使所述机器人控制器控制所述机器人进行钻孔操作。

第二方面，本发明还提供了一种基于力传感器的直接拖动示教方法，包括：

实时获取在医生拖动末端执行器进行运动的过程中六维力传感器采集的末端执行器的受力信息；

对所述六维力传感器采集的末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息；

根据获取的所述医生拖动所述末端执行器进行运动时的外力信息控制所述末端执行器进行运动。

进一步地，所述对所述六维力传感器采集的末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息，具体包括：

获取预先存储的所述末端执行器在没有受到其他外力作用状况下其处于不同空间姿态时的多组第一测量数据；所述多组第一测量数据由所述六维力传感器采集得到；

根据所述多组第一测量数据获取所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)以及所述末端执行器重力G的大小；

根据所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)，所述末端执行器重力G的大小，以及所述六维力传感器采集的所述末端执行器的受力信息，利用下面公式一获取医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量：

以及，利用下面公式二获取医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量：

其中，F_ex、F_ey、F_ez是医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量；M_ex、M_ey、M_ez是医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量；F_x、F_y、F_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的受力分量，M_x、M_y、M_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的力矩分量；G_x、G_y、G_z是所述六维力传感器采集的力分量F_x、F_y、F_z中由于所述末端执行器重力产生的部分，M_gx、M_gy、M_gz是所述六维力传感器采集的力矩分量M_x、M_y、M_z中由于所述末端执行器重力产生的部分；

其中，上述G_x、G_y、G_z采用下面公式三获取：

其中，上述M_gx、M_gy、M_gz采用下面公式四获取：

其中，所述α、β、γ为在对所述末端执行器安装姿态标定后实时获取到重力方向与六维力传感器坐标系X、Y、Z轴间的夹角。

进一步地，所述根据获取的所述医生拖动所述末端执行器进行运动时的外力信息控制所述末端执行器进行运动，包括：

根据医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量F_ex、F_ey和F_ez判断有效外力F＝(F_ex ²、F_ey ²、F_ez ²)^1/2是否大于第一预设阈值F_s1，若是，则根据预设的第一V-F关系确定用于带动所述末端执行器运动的机器人的运动线速度V₁，并根据所述线速度V₁确定所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z，然后将所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z发送给与所述机器人对应的机器人控制器，以使所述机器人控制器控制所述机器人带动所述末端执行器进行运动。

第三方面，本发明还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如第二方面所述基于力传感器的直接拖动示教方法的步骤。

第四方面，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如第二方面所述基于力传感器的直接拖动示教方法的步骤。

由上述技术方案可知，本发明提供的基于力传感器的直接拖动示教系统，包括：工控机、机器人控制器、机器人、六维力传感器和末端执行器；所述六维力传感器设置在所述机器人与所述末端执行器之间，用于在医生拖动所述末端执行器进行运动的过程中采集所述末端执行器的受力信息；所述工控机对所述六维力传感器采集的所述末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息；所述工控机根据医生拖动所述末端执行器进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动。本发明在机器人末端加装多维力传感器传达医生的示教意图，引导机器人运动，并通过重力补偿，消除末端执行器重力的影响，故可较为准确地感知医生手部作用力信息，实现自动准确的拖动示教。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明一实施例提供的基于力传感器的直接拖动示教系统的结构示意图；

图2是本发明一实施例提供的基于力传感器的直接拖动示教系统的工作原理示意图；

图3是六维力传感器坐标系示意图；

图4是机器人柔性随动控制原理图；

图5是工控机根据医生拖动所述末端执行器进行运动时的外力信息控制机器人进行运动的过程示意图；

图6是预设的第一V-F关系示意图；

图7是外力在钻孔方向上的投影示意图；

图8是本发明另一实施例提供的基于力传感器的直接拖动示教方法的流程图；

图9是本发明又一实施例提供的电子设备的结构示意图。

具体实施方式

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

本发明一实施例提供了一基于力传感器的直接拖动示教系统，参见图1，该系统包括：工控机1、机器人控制器2、机器人3、六维力传感器4和末端执行器5；

所述六维力传感器4安装在所述机器人2与所述末端执行器5之间，用于在医生拖动所述末端执行器5进行运动的过程中采集所述末端执行器5的受力信息；

所述工控机1对所述六维力传感器4采集的所述末端执行器5的受力信息进行所述末端执行器5本身的重力补偿，获取医生拖动所述末端执行器5进行运动时的外力信息；

所述工控机1根据医生拖动所述末端执行器5进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器2，以使所述机器人控制器2控制所述机器人3进行运动。

参见图1，在该系统中，工控机1对六维力传感器4的信号进行采集与处理，根据检测到的外力大小和方向生成机器人运动指令，发送至机器人控制器2中，控制机器人3运动。六维力传感器4可以通过法兰和螺纹连接在机器人3和末端执行器5之间，通过重力补偿算法消除末端执行器重力的影响后，可感知医生手部作用力信息。可以理解的是，在图1中，末端执行器(末端执行器5)重力作用在六维力传感器上，要想准确得到末端执行器(末端执行器5)受到的外部作用力信息，如医生手部作用力等，需要对末端执行器(末端执行器5)重力的影响进行补偿。可以理解的是，当该系统应用于牙齿种植手术时，所述末端执行器5可以为口腔种植机。当该系统应用于其他场景如骨科或心脑手术时，所述末端执行器5可以为其他相配套的执行终端。

图2示出了基于力传感器的直接拖动示教系统的工作原理示意图。由图2可知，本实施例通过在机器人末端加装六维力传感器的方式来达到传达医生示教意图的目的，根据六维力传感器采集的末端执行器的受力情况引导机器人进行运动，并通过重力补偿，消除末端执行器重力的影响，故可较为准确地感知医生的外部用力信息，实现自动准确的拖动示教。

由上面描述可知，本实施例提供的基于力传感器的直接拖动示教系统，包括：工控机、机器人控制器、机器人、六维力传感器和末端执行器；所述六维力传感器设置在所述机器人与所述末端执行器之间，用于在医生拖动所述末端执行器进行运动的过程中采集所述末端执行器的受力信息；所述工控机对所述六维力传感器采集的所述末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息；所述工控机根据医生拖动所述末端执行器进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动。本实施例在机器人末端加装多维力传感器传达医生的示教意图，引导机器人运动，并通过重力补偿，消除末端执行器重力的影响，故可较为准确地感知医生手部作用力信息，实现自动准确的拖动示教。

在一种优选实施方式中，给出了进行重力补偿一种具体实施手段。参见下面介绍可知，所述工控机对所述六维力传感器采集的所述末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息，具体包括：

a、获取预先存储的所述末端执行器在没有受到其他外力作用状况下其处于不同空间姿态时的多组第一测量数据；所述多组第一测量数据由所述六维力传感器采集得到；

b、根据所述多组第一测量数据获取所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)以及所述末端执行器重力G的大小；

参见图3，可以理解的是，若没有其他外力作用在末端执行器上，则六维力传感器测得的力与力矩信息完全由末端执行器重力引起。这种情况下，本实施例控制机器人使末端执行器(末端执行器)处于不同的空间姿态，得到多组六维力传感器的测量数据，然后再利用最小二乘法求解末端执行器质心在六维力传感器坐标系中的坐标(x，y，z)以及末端执行器重力G的大小。需要说明的是，测量多组数据，并根据多组测量数据采用最小二乘法进行计算处理，相对于只测一组数据进行求解的方法来说，可以尽量减少测量误差，使得计算结果的准确度提高。

c、根据所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)，所述末端执行器重力G的大小，以及所述六维力传感器采集的所述末端执行器的受力信息，利用下面公式一获取医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量：

以及，利用下面公式二获取医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量：

其中，F_ex、F_ey、F_ez是医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量；M_ex、M_ey、M_ez是医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量；F_x、F_y、F_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的受力分量(也即F_x、F_y、F_z是在有外力作用于末端执行器的情况下，六维力传感器测得的3个力分量)，M_x、M_y、M_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的力矩分量(也即M_x、M_y、M_z是在有外力作用于末端执行器的情况下，六维力传感器测得的3个力矩分量)；G_x、G_y、G_z是所述六维力传感器采集的力分量F_x、F_y、F_z中由于所述末端执行器重力产生的部分，M_gx、M_gy、M_gz是所述六维力传感器采集的力矩分量M_x、M_y、M_z中由于所述末端执行器重力产生的部分；

其中，上述G_x、G_y、G_z采用下面公式三获取：

其中，在手术操作中末端执行器重力在六维力传感器坐标系中的方向随机器人末端姿态变化而改变。对末端执行器安装姿态标定后可通过机器人控制系统实时获取到重力方向与六维力传感器坐标系X、Y、Z轴间的夹角α、β、γ。

其中，末端执行器重力G对X、Y、Z轴的作用力矩分别为M_gx、M_gy、M_gz，根据力与力矩的关系，参照图3可知M_gx、M_gy、M_gz可根据下面公式四获取：

可以理解的是，为了实现医生手持末端就能用很小的力拖动机械臂随动，需要根据力传感器获得的人手作用力信息制定控制策略控制机器人产生柔性随动效果。末端执行器按人手作用力的方向运动，运动速度与人手作用力的大小相关。因此，在一种优选实施方式中，所述工控机根据医生拖动所述末端执行器进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动，具体包括：

参见图4和图5，所述工控机根据医生(图5中用人手表示)拖动所述末端执行器进行运动时外力在3个坐标轴上的分量F_ex、F_ey和F_ez判断有效外力F＝(F_ex ²、F_ey ²、F_ez ²)^1/2是否大于第一预设阈值F_s1，若是，则根据预设的第一V-F关系确定所述机器人的运动线速度V₁，并根据所述线速度V₁确定所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z，然后将所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z发送给所述机器人控制器，以使所述机器人控制器控制所述机器人(图5中以机械臂代替机器人)进行运动；否则，不控制所述机器人进行运动。

可以理解的是，系统获得的外部作用力为控制输入，由于传感器自身误差或者重力补偿误差，在实际没有外力作用至传感器或末端执行器上的情况下，系统得到的外部作用力并不为0，因此为保证安全性，需要为系统获得的外部作用力大小F设定第一预设阈值F_s1，当F>F_s1时方能对机器人产生作用。其中，第一预设阈值F_s1的值可以根据实际需要进行设定。

在一种优选实施方式中，所述预设的第一V-F关系为：当F_s1<F<F_s2时，V随F的增大线性增加，V＝k_F*F；当F≥F_s2时，V＝V_max；其中，k_F为预设的比例系数，V_max为预设的速度上限。

可以理解的是，所述预设的第一V-F关系表示的是机器人运动速度大小V与外部作用力大小F的关系。参见图6，当F_s1<F<F_s2时，V随F的增大线性增加，当F≥F_s2时，V＝V_max。这样的V-F关系中，V随F连续变化，没有跳变，可以避免外力变化带来的机器人剧烈抖动；当F_s1<F<F_s2时，外部作用力越大，机器人运动速度越快，这符合人的日常习惯，为避免机器人速度过快影响操作安全，为V设置上限V_max。总速度V确定后按照各轴力的分量(F_ex，F_ey，F_ez)将V分配为各轴分速度(V_x，V_y，V_z)，使机器人运动方向与外部作用力方向相同，实现柔性跟随移动。其中，通过设置比例系数k_F的大小可以调节拖拽机器人的灵敏度(图6中的斜率)。k_F值设置的越大运动越灵敏，即医生(如医生)出很小的力就能拖动机器人，但是如果k_F值设置过大，也比较容易导致误操作，因此k_F值的设置要合适。

在一种优选实施方式中，当医生拖拽所述末端执行器到达指定的操作位置后，若需要进行钻孔操作，则按照如下方式进行处理：

A、获取医生拖动所述末端执行器进行运动时的有效外力F在钻孔方向上的投影

其中，钻孔方向的方向向量为

作用力为

B、判断在钻孔方向上的有效外力

是否大于第二预设阈值F_s2，若是，则根据预设的第二V-F关系确定所述机器人在钻孔方向上的钻孔速度V₂，并将所述钻孔速度V₂发送给所述机器人控制器，以使所述机器人控制器控制所述机器人进行钻孔操作。

可以理解的是，所述预设的第二V-F关系可以与所述第一V-F关系相同或类似，例如在某个区间范围内呈线性关系，在另一区间范围内成稳定恒值关系。其中，第二预设阈值F_s2的值可以根据实际需要进行设定。

可以理解的是，医生拖拽机器人到达了指定的操作位置，接下来要进行钻孔时也可以根据力传感器信息来进行钻孔路径约束。如图7所示粗虚线为钻孔方向，设其方向向量为

作用力为

则外作用力在钻孔方向上的投影为

将操作外力F投影到钻孔方向上之后，再按照之前的控制流程，机器人就只能沿着规定好的方向进行运动。这样就能避免医生手抖动带来的误操作，增强手术的安全性与可靠性。

基于相同的发明构思，本发明另一实施例提供了一种基于力传感器的直接拖动示教方法，参见图8，该方法包括如下步骤：

步骤101：实时获取在医生拖动末端执行器进行运动的过程中六维力传感器采集的末端执行器的受力信息。

步骤102：对所述六维力传感器采集的末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息。

步骤103：根据获取的所述医生拖动所述末端执行器进行运动时的外力信息控制所述末端执行器进行运动。

在一种优选实施方式中，上述步骤102具体采用如下方式实现：

获取预先存储的所述末端执行器在没有受到其他外力作用状况下其处于不同空间姿态时的多组第一测量数据；所述多组第一测量数据由所述六维力传感器采集得到；

根据所述多组第一测量数据获取所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)以及所述末端执行器重力G的大小；

根据所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)，所述末端执行器重力G的大小，以及所述六维力传感器采集的所述末端执行器的受力信息，利用下面公式一获取医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量：

以及，利用下面公式二获取医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量：

其中，F_ex、F_ey、F_ez是医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量；M_ex、M_ey、M_ez是医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量；F_x、F_y、F_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的受力分量，M_x、M_y、M_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的力矩分量；G_x、G_y、G_z是所述六维力传感器采集的力分量F_x、F_y、F_z中由于所述末端执行器重力产生的部分，M_gx、M_gy、M_gz是所述六维力传感器采集的力矩分量M_x、M_y、M_z中由于所述末端执行器重力产生的部分；

其中，上述G_x、G_y、G_z采用下面公式三获取：

其中，上述M_gx、M_gy、M_gz采用下面公式四获取：

其中，所述α、β、γ为在对所述末端执行器安装姿态标定后实时获取到重力方向与六维力传感器坐标系X、Y、Z轴间的夹角。

在一种优选实施方式中，上述步骤103具体采用如下方式实现：

根据医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量F_ex、F_ey和F_ez判断有效外力F＝(F_ex ²、F_ey ²、F_ez ²)^1/2是否大于第一预设阈值F_s1，若是，则根据预设的第一V-F关系确定用于带动所述末端执行器运动的机器人的运动线速度V₁，并根据所述线速度V₁确定所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z，然后将所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z发送给与所述机器人对应的机器人控制器，以使所述机器人控制器控制所述机器人带动所述末端执行器进行运动。

需要说明的是，本发明实施例提供的基于力传感器的直接拖动示教方法可以单独实现，也可以基于上述实施例所述的系统实现，本发明对此不做限定。

基于相同的发明构思，本发明又一实施例提供了一种电子设备，参见图9，所述电子设备具体包括如下内容：处理器701、存储器702、通信接口703和总线704；

其中，所述处理器701、存储器702、通信接口703通过所述总线704完成相互间的通信；所述通信接口703用于实现各建模软件及智能制造装备模块库等相关设备之间的信息传输；

所述处理器701用于调用所述存储器702中的计算机程序，所述处理器执行所述计算机程序时实现上述实施例所述的基于力传感器的直接拖动示教方法中的全部步骤，例如，所述处理器执行所述计算机程序时实现下述步骤：

步骤101：实时获取在医生拖动末端执行器进行运动的过程中六维力传感器采集的末端执行器的受力信息。

步骤102：对所述六维力传感器采集的末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息。

步骤103：根据获取的所述医生拖动所述末端执行器进行运动时的外力信息控制所述末端执行器进行运动。

基于相同的发明构思，本发明又一实施例提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现上述实施例所述的基于力传感器的直接拖动示教方法中的全部步骤，例如，所述处理器执行所述计算机程序时实现下述步骤：

步骤101：实时获取在医生拖动末端执行器进行运动的过程中六维力传感器采集的末端执行器的受力信息。

步骤102：对所述六维力传感器采集的末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息。

步骤103：根据获取的所述医生拖动所述末端执行器进行运动时的外力信息控制所述末端执行器进行运动。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (4)

1.一种基于力传感器的直接拖动示教系统，其特征在于，包括：工控机、机器人控制器、机器人、六维力传感器和末端执行器；

所述六维力传感器设置在所述机器人与所述末端执行器之间，用于在医生拖动所述末端执行器进行运动的过程中采集所述末端执行器的受力信息；

所述工控机对所述六维力传感器采集的所述末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息；

所述工控机根据医生拖动所述末端执行器进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动；

其中，所述工控机对所述六维力传感器采集的所述末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息，具体包括：

获取预先存储的所述末端执行器在没有受到其他外力作用状况下其处于不同空间姿态时的多组第一测量数据；所述多组第一测量数据由所述六维力传感器采集得到；

根据所述多组第一测量数据获取所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)以及所述末端执行器重力G的大小；

根据所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)，所述末端执行器重力G的大小，以及所述六维力传感器采集的所述末端执行器的受力信息，利用下面公式一获取医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量：

以及，利用下面公式二获取医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量：

其中，F_ex、F_ey、F_ez是医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量；M_ex、M_ey、M_ez是医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量；F_x、F_y、F_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的受力分量，M_x、M_y、M_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的力矩分量；G_x、G_y、G_z是所述六维力传感器采集的力分量F_x、F_y、F_z中由于所述末端执行器重力产生的部分，M_gx、M_gy、M_gz是所述六维力传感器采集的力矩分量M_x、M_y、M_z中由于所述末端执行器重力产生的部分；

其中，上述G_x、G_y、G_z采用下面公式三获取：

其中，上述M_gx、M_gy、M_gz采用下面公式四获取：

其中，所述α、β、γ为在对所述末端执行器安装姿态标定后实时获取到重力方向与六维力传感器坐标系X、Y、Z轴间的夹角；

其中，所述工控机根据医生拖动所述末端执行器进行运动时的外力信息生成机器人运动指令，并将生成的机器人运动指令发送至所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动，具体包括：

所述工控机根据医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量F_ex、F_ey和F_ez判断有效外力F＝(F_ex ²、F_ey ²、F_ez ²)^1/2是否大于第一预设阈值F_s1，若是，则根据预设的第一V-F关系确定所述机器人的运动线速度V₁，并根据所述线速度V₁确定所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z，然后将所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z发送给所述机器人控制器，以使所述机器人控制器控制所述机器人进行运动；

其中，所述预设的第一V-F关系为：当F_s1<F<F_s2时，V随F的增大线性增加，V＝k_F*F；当F≥F_s2时，V＝V_max；其中，k_F为预设的比例系数，V_max为预设的速度上限；

当医生拖拽所述末端执行器到达指定的操作位置后，若需要进行钻孔操作，则按照如下方式进行处理：

获取医生拖动所述末端执行器进行运动时的有效外力F在钻孔方向上的投影

其中，钻孔方向的方向向量为

作用力为

判断在钻孔方向上的有效外力

是否大于第二预设阈值F_s2，若是，则根据预设的第二V-F关系确定所述机器人在钻孔方向上的钻孔速度V₂，并将所述钻孔速度V₂发送给所述机器人控制器，以使所述机器人控制器控制所述机器人进行钻孔操作；

其中，所述第二V-F关系与所述第一V-F关系相同：在某个区间范围内呈线性关系，在另一区间范围内成稳定恒值关系。

2.一种基于力传感器的直接拖动示教方法，其特征在于，包括：

实时获取在医生拖动末端执行器进行运动的过程中六维力传感器采集的末端执行器的受力信息；

对所述六维力传感器采集的末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息；

根据获取的所述医生拖动所述末端执行器进行运动时的外力信息控制所述末端执行器进行运动；

其中，所述对所述六维力传感器采集的末端执行器的受力信息进行所述末端执行器本身的重力补偿，获取医生拖动所述末端执行器进行运动时的外力信息，具体包括：

获取预先存储的所述末端执行器在没有受到其他外力作用状况下其处于不同空间姿态时的多组第一测量数据；所述多组第一测量数据由所述六维力传感器采集得到；

根据所述多组第一测量数据获取所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)以及所述末端执行器重力G的大小；

根据所述末端执行器质心在所述六维力传感器坐标系中的坐标(x，y，z)，所述末端执行器重力G的大小，以及所述六维力传感器采集的所述末端执行器的受力信息，利用下面公式一获取医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量：

以及，利用下面公式二获取医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量：

其中，F_ex、F_ey、F_ez是医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量；M_ex、M_ey、M_ez是医生拖动所述末端执行器进行运动时外力力矩在3个坐标轴上的分量；F_x、F_y、F_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的受力分量，M_x、M_y、M_z是医生拖动所述末端执行器进行运动的过程中所述六维力传感器采集的所述末端执行器在3个方向上的力矩分量；G_x、G_y、G_z是所述六维力传感器采集的力分量F_x、F_y、F_z中由于所述末端执行器重力产生的部分，M_gx、M_gy、M_gz是所述六维力传感器采集的力矩分量M_x、M_y、M_z中由于所述末端执行器重力产生的部分；

其中，上述G_x、G_y、G_z采用下面公式三获取：

其中，上述M_gx、M_gy、M_gz采用下面公式四获取：

其中，所述α、β、γ为在对所述末端执行器安装姿态标定后实时获取到重力方向与六维力传感器坐标系X、Y、Z轴间的夹角；

其中，所述根据获取的所述医生拖动所述末端执行器进行运动时的外力信息控制所述末端执行器进行运动，包括：

根据医生拖动所述末端执行器进行运动时外力在3个坐标轴上的分量F_ex、F_ey和F_ez判断有效外力F＝(F_ex ²、F_ey ²、F_ez ²)^1/2是否大于第一预设阈值F_s1，若是，则根据预设的第一V-F关系确定用于带动所述末端执行器运动的机器人的运动线速度V₁，并根据所述线速度V₁确定所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z，然后将所述机器人在X、Y、Z轴的分速度V_1x、V_1y、V_1z发送给与所述机器人对应的机器人控制器，以使所述机器人控制器控制所述机器人带动所述末端执行器进行运动；

其中，所述预设的第一V-F关系为：当F_s1<F<F_s2时，V随F的增大线性增加，V＝k_F*F；当F≥F_s2时，V＝V_max；其中，k_F为预设的比例系数，V_max为预设的速度上限；

其中，当医生拖拽所述末端执行器到达指定的操作位置后，若需要进行钻孔操作，则按照如下方式进行处理：

获取医生拖动所述末端执行器进行运动时的有效外力F在钻孔方向上的投影

其中，钻孔方向的方向向量为

作用力为

判断在钻孔方向上的有效外力

是否大于第二预设阈值F_s2，若是，则根据预设的第二V-F关系确定所述机器人在钻孔方向上的钻孔速度V₂，并将所述钻孔速度V₂发送给所述机器人控制器，以使所述机器人控制器控制所述机器人进行钻孔操作；

其中，所述第二V-F关系与所述第一V-F关系相同：在某个区间范围内呈线性关系，在另一区间范围内成稳定恒值关系。

3.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求2所述基于力传感器的直接拖动示教方法的步骤。

4.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求2所述基于力传感器的直接拖动示教方法的步骤。

Images