CN119632692B - 手术系统的主从控制方法、电子设备及存储介质 - Google Patents

Abstract

本申请提供了一种手术系统的主从控制方法，握持结构上设置有位姿传感器和开合角传感器，控制方法包括：获取位姿传感器在第一预设时刻的第一位置信息和第一姿态信息以及第二预设时刻的第二位置信息和第二姿态信息；根据第一姿态信息和第二姿态信息，确定末端器械的目标姿态信息；根据第一位置信息和第二位置信息，确定握持结构的位移变化量；根据位移变化量，通过放缩系数映射得到末端器械的目标位移信息；将目标位移信息、目标姿态信息和开合角传感器获取的握持结构的开合角，发送至末端器械，控制末端器械运动。能够解决现有的手术系统主从映射因通过旋转矩阵解算存在奇点的问题。

Description

手术系统的主从控制方法、电子设备及存储介质

技术领域

本发明涉及医疗器械技术领域，具体而言，涉及一种手术系统的主从控制方法、电子设备及存储介质。

背景技术

目前手术系统通过设置手术操作椅，操作者坐在手术操作椅上，通过磁场发生器获取连接于手术操作椅的握持结构的位姿信息，结合脚踏和握持结构上的传感器以及按键等解算得出操作者的控制意图，通过处理器控制实现握持结构和末端器械的空间位姿映射，进而控制末端器械运动，但是其存在以下缺陷：

（1）现有的握持结构使用机械连杆连接于操作椅上，使得握持结构的运动范围受到很大限制，会使得操作者在使用时活动受限。

（2）现有的握持结构采用电位器或编码器等机械式传感器，来获取握持结构的运动参数，在使用过程中有机械性磨损，导致其性能下降、寿命短、性能不稳定的问题；

（3）现有的手术系统中的主从映射是通过旋转矩阵解算，导致存在奇点问题，进而影响末端器械的运动。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种手术系统的主从控制方法，以解决现有的手术系统握持结构活动受限、主从映射存在奇点的问题以及握持结构性能不稳定、寿命短的问题。

本发明的上述技术目的是通过以下技术方案得以实现的：

一种手术系统的主从控制方法，手术系统包括末端器械以及控制末端器械运动的握持结构，握持结构上设置有位姿传感器和开合角传感器，控制方法包括：获取位姿传感器在第一预设时刻的第一位置信息和第一姿态信息以及第二预设时刻的第二位置信息和第二姿态信息；根据第一姿态信息和第二姿态信息，确定末端器械的目标姿态信息；根据第一位置信息和第二位置信息，确定握持结构的位移变化量；根据位移变化量，通过放缩系数映射得到末端器械的目标位移信息；将目标位移信息、目标姿态信息和开合角传感器获取的握持结构的开合角，发送至末端器械，控制末端器械运动。

进一步的，根据第一位置信息和第二位置信息，确定握持结构的位移变化量，包括：根据第一位置信息，确定握持结构在第一预设时刻的第三位置信息；根据第二位置信息，确定握持结构在第二预设时刻的第四位置信息；根据第三位置信息和第四位置信息，确定握持结构的位移变化量。

进一步的，手术系统还包括磁场发生器，与位姿传感器连接，用于接收位姿传感器传输的位置信息，并传输至处理器，根据第一位置信息，确定握持结构在第一预设时刻的第三位置信息，包括：根据第一位置信息以及第一预设时刻握持结构的坐标系内位姿传感器相对于握持结构的基点的位置向量以及握持结构的坐标系变换至磁场发生器的坐标系的旋转变换矩阵，确定握持结构的基点在第一预设时刻的第三位置信息。

进一步的，手术系统还包括磁场发生器，与位姿传感器连接，用于接收位姿传感器传输的位置信息，并传输至处理器，根据第二位置信息，确定握持结构在第二预设时刻的第四位置信息，包括：根据第二位置信息以及第二预设时刻握持结构的坐标系内位姿传感器相对于握持结构的基点的位置向量以及握持结构的坐标系变换至磁场发生器的坐标系的旋转变换矩阵，确定握持结构的基点在第二预设时刻的第四位置信息。

进一步的，第一姿态信息包括第一自转角度，第二姿态信息包括第二自转角度，目标姿态信息包括目标自转角度，根据第一姿态信息和第二姿态信息，确定末端器械的目标姿态信息，包括：通过计算第二自转角度和第一自转角度的差值，得到目标自转角度。

进一步的，手术系统还包括磁场发生器，与位姿传感器连接，用于接收位姿传感器传输的姿态信息，并传输至处理器，目标姿态信息包括目标俯仰角，根据第一姿态信息和第二姿态信息，确定末端器械的目标姿态信息，包括：根据末端器械在第一预设时刻相对于磁场发生器的旋转矩阵、预设的握持结构绕第一方向旋转的旋转矩阵以及第一列矩阵，计算得到第二方向上握持结构在第一预设时刻进行自转后的第一平面法向量；根据末端器械在第二预设时刻相对于磁场发生器的旋转矩阵和第一列矩阵，计算得到第二方向上握持结构在第二预设时刻的第二平面法向量；通过计算第一平面法向量与第二平面法向量的夹角，得到目标俯仰角。

进一步的，手术系统还包括磁场发生器，与位姿传感器连接，用于接收位姿传感器传输的姿态信息，并传输至处理器，目标姿态信息包括目标偏转角，根据第一姿态信息和第二姿态信息，确定末端器械的目标姿态信息，包括：根据末端器械在第一预设时刻相对于磁场发生器的旋转矩阵、预设的握持结构绕第一方向旋转的旋转矩阵以及第二列矩阵，计算得到第三方向上握持结构在第一预设时刻进行自转后的第一方向向量；根据末端器械在第二预设时刻相对于磁场发生器的旋转矩阵和第二列矩阵，计算得到第三方向上握持结构在第二预设时刻的第二方向向量；通过计算第一方向向量和第二方向向量的夹角，得到目标偏转角。

进一步的，握持结构上设置有握持传感器，用于检测握持结构的电容值，控制方法包括：获取地平面电容值，接收电容值；计算电容值与地平面电容值的电容差值；根据电容差值与预设电容值的关系，判断握持结构是否处于正常握持状态；在判断握持结构未处于正常握持状态时，根据位姿传感器检测到的握持结构在第二方向的n个位置信息，计算并输出加速度；根据加速度与预设加速度的关系，判断握持结构是否跌落，并根据判断结果，控制手术系统继续工作或进入待机状态。

进一步的，根据电容差值与预设电容值的关系，判断握持结构是否处于正常握持状态，包括：若电容差值大于预设电容值，判定握持结构处于正常握持状态；若电容差值小于预设电容值，判定握持结构未处于正常握持状态。

进一步的，根据加速度与预设加速度的关系，判断握持结构是否跌落，包括：若加速度大于预设加速度，判定握持结构跌落，则控制手术系统进入待机状态；若加速度小于等于预设加速度，判定握持结构未跌落，则控制手术系统继续工作。

进一步的，手术系统还包括设置于末端器械的第一电机和第二电机，握持结构上设置有夹持指示灯，控制方法包括：获取第一电机内第一编码器值和第二电机内第二编码器值；计算第一编码器值和第二编码器值之间的第一差值；根据第一差值与预设的电机编码器值的关系，判断末端器械是否进行夹持操作；根据判断结果控制夹持指示灯亮起或保持熄灭。

进一步的，根据第一差值与预设的电机编码器值的关系，判断末端器械是否进行夹持操作，包括：若第一差值小于电机编码器值，确定末端器械未进行夹持操作；若第一差值不小于电机编码器值，接收第一电机发送的第一输出力矩和第二电机发送的第二输出力矩；计算第一输出力矩和第二输出力矩之间的第二差值；根据第二差值与预设输出力矩值之间的关系，判断末端器械是否进行夹持操作。

进一步的，根据第二差值与预设输出力矩值之间的关系，判断末端器械是否进行夹持操作，包括：若第二差值小于预设输出力矩值，确定末端器械未进行夹持操作。若第二差值不小于预设输出力矩值，确定末端器械进行夹持操作。

本申请实施例还提供一种电子设备，电子设备包括：处理器，用于存储处理器可执行指令的存储器；其中，处理器被配置为执行上述任意一项的手术系统的主从控制方法。

本申请实施例还提供一种计算机可读存储介质，存储介质存储有计算机程序，计算机程序可由处理器执行以完成上述任意一项的手术系统的主从控制方法。

本申请实施例的有益效果是：

首先，本申请实施例提供一种手术系统的主从控制方法，通过在握持结构上设置位姿传感器，通过位姿传感器检测位置信息和姿态信息，且根据位置信息和姿态信息，计算得到目标姿态信息和位移变化量，并通过放缩系数映射得到末端器械的目标位移信息，将上述位置信息和姿态信息通过空间几何解算的方式，即第一预设时刻的位置信息和第二预设时刻的位置信息之间、第一预设时刻的姿态信息和第二预设时刻的姿态信息之间在空间上的位置关系进行解算，得到相对应目标位置信息和目标姿态信息，能够避免手术系统的主从映射因旋转矩阵解算的方式会存在旋转角度的重合出现奇点的问题，即因出现特殊点位导致失去一个或多个自由度的问题。

其次，本申请实施例采用位姿传感器和开合角传感器进行位置和姿态的检测，能够避免传感器长时间使用后容易磨损、导致性能下降的问题，延长了传感器的使用寿命，即延长了手术系统的使用寿命。

最后，本申请实施例通过握持结构内安装的各种传感器即可进行操作数据的采集，所以握持结构无需通过机械连杆安装在座椅手托上，将机械连杆的运动转换为末端器械的控制指令，可以解决现有的握持结构运动空间有限的问题，提高握持结构的活动范围，使得操作更加灵活。

附图说明

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

图1是本申请实施例提供的手术系统的示意图；

图2是图1所示手术操作座椅11的结构示意图；

图3是图2中握持结构的示意图；

图4是本申请实施例提供的手术系统的主从控制方法的主流程示意图；

图5是本申请实施例提供的手术系统的主从控制方法的握持结构从第一预设时刻到第二预设时刻进行位置和姿态变化示意图；

图6是本申请一实施例提供的手术系统的主从控制方法的步骤S120的子步骤的流程图；

图7是本申请实施例提供的手术系统的主从控制方法的第一平面法向量和第二平面法向量的结构示意图；

图8是本申请另一实施例提供的手术系统的主从控制方法的步骤S120的子步骤的流程图；

图9是本申请实施例提供的手术系统的主从控制方法的第一方向向量和第二方向向量的结构示意图；

图10是本申请实施例提供的手术系统的主从控制方法的判断握持结构是否处于正常握持状态的流程示意图；

图11为本申请实施例提供的手术系统的主从控制方法的判断握持结构是否进行夹持操作的流程示意图。

附图标记：握持结构1；磁场发生器2；位姿传感器3；开合角传感4；末端器械5；末端云台6；手托7；处理器9；手术操作座椅11；手术执行设备12；握持传感器13；夹持指示灯14。

具体实施方式

在本申请的描述中，需要说明的是，术语“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该申请产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。此外，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

图1是本申请实施例提供的手术系统的示意图。如图1所示，手术系统包括手术操作座椅11和手术执行设备12，手术操作座椅11与手术执行设备12之间采用有线或无线传输。手术执行设备12包括有末端云台6和连接于末端云台6的末端器械5。

图2是图1所示手术操作座椅11的结构示意图，如图2所示，手术操作座椅11包括座椅、握持结构1、位于座椅两侧的手托7、位于座椅的坐垫下侧的磁场发生器2和处理器9。磁场发生器2与握持结构1连接，磁场发生器2与处理器9进行连接，用于接收握持结构1发送的数据信息并传输至处理器9中。

图3是图2中握持结构的示意图。如图3所示，握持结构内设置有位姿传感器3、开合角传感器4、握持传感器13和夹持指示灯14，位姿传感器3用于检测位置信息和握持结构1的姿态信息，开合角传感器4用于检测握持结构的开合角，握持传感器13用于获取握持结构1的电容值，夹持指示灯14用于通过处理器9控制，进行保持熄灭或亮起。

图4是本申请实施例提供的手术系统的主从控制方法的主流程示意图。如图4所示，该控制方法可以包括步骤S110~步骤S150，其中步骤S120和步骤S130~步骤S140之间没有先后顺序，可以同步进行，也可以调整顺序进行。该主从控制方法可以由手术系统的处理器9执行。如图4所示：

步骤S110：获取位姿传感器3在第一预设时刻的第一位置信息和第一姿态信息以及第二预设时刻的第二位置信息和第二姿态信息。

磁场发生器2与握持结构1的位姿传感器3之间可以采用有线或无线连接，位姿传感器3将检测到的第一位置信息和第一姿态信息、第二位置信息和第二姿态信息等数据传输至磁场发生器2中，磁场发生器2将其发送至手术系统的处理器9中。

其中，第一位置信息是第一预设时刻位姿传感器3位于握持结构1的坐标系X、Y、Z轴方向上的位置，第二位置信息是第二预设时刻位姿传感器3位于握持结构1的坐标系X、Y、Z轴方向上的位置，第一姿态信息是第一预设时刻位于握持结构1的坐标系内的第一偏转角度、第一俯仰角度和第一自转角度，第二姿态信息是第二预设时刻位于握持结构1的坐标系的第二偏转角度、第二俯仰角度和第二自转角度。

步骤S120：根据第一姿态信息和第二姿态信息，确定末端器械5的目标姿态信息。

其中，在一实施例中，第一姿态信息包括第一自转角度，第二姿态信息包括第二自转角度，目标姿态信息包括目标自转角度。

在另一实施例中，目标姿态信息包括目标俯仰角。

在其他实施例中，目标姿态信息包括目标偏转角。

在本实施例中，末端器械5的腕式结构与握持结构1的构型不同，因此不能将握持结构1的姿态信息直接作为末端器械5的目标姿态信息，并控制末端器械5进行相应姿态变化。

因此，本申请实施例通过将握持结构1的姿态信息分别计算，得到对应的末端器械5的目标自转角度、目标俯仰角和目标偏转角，能够避免现有的手术系统的主从映射因采用旋转矩阵解算的方式存在出现特殊点位，导致失去一个或多个自由度的问题。

步骤S130：根据第一位置信息和第二位置信息，确定握持结构1的位移变化量。

步骤S140：根据位移变化量，通过放缩系数映射得到末端器械5的目标位移信息。

本实施例中，通过设置放缩系数将握持结构1的位移变化量映射出末端器械5的目标位移信息，使得目标位移信息可根据实际情况通过调节放缩系数得到，使得本申请实施例的主从控制方法具有灵活性和实用性。

步骤S150：将目标位移信息、目标姿态信息和开合角传感器4获取的握持结构1的开合角，发送至末端器械5，控制末端器械5运动。

其中，在一实施例中，发送至末端器械5的握持结构1的开合角为第二预设时刻的握持结构1的第二开合角。

本申请实施例通过采用坐标系的方式进行空间几何解算，得到上述的目标位移信息和目标姿态信息，能够避免出现特殊点位，导致失去一个或多个目标姿态信息、目标位移信息的问题，提升手术系统操作的稳定性和精确度。

现有的握持结构1采用电位器或编码器等机械式传感器，来获取握持结构1的运动参数，在使用过程中有机械性磨损，导致其性能下降、寿命短、性能不稳定的问题。本申请实施例中握持结构1内采用位姿传感器3和开合角传感器4进行位置和姿态的检测，能够避免传感器长时间使用后容易磨损、导致性能下降的问题，延长了传感器的使用寿命，即延长了手术系统的使用寿命。

本申请实施例通过握持结构1内安装的各种传感器即可进行操作数据的采集，所以握持结构1无需通过机械连杆安装在座椅手托上，将机械连杆的运动转换为末端器械5的控制指令，可以解决现有的握持结构1运动空间有限的问题，提高握持结构1的活动范围，使得操作更加灵活。

图5是本申请实施例提供的手术系统的主从控制方法的握持结构从第一预设时刻到第二预设时刻进行位置和姿态变化示意图。

如图5所示，为第一预设时刻位姿传感器的第一位置信息，为第二预设时刻位姿传感器的第二位置信息，为第一预设时刻开合角传感器的第一位置信息，为第二预设时刻开合角传感器的第二位置信息。

为第一预设时刻握持结构1的第一偏转角度，为第一预设时刻握持结构1的第一俯仰角度，为第一预设时刻握持结构1的第一自转角度。

且为第二预设时刻握持结构1的第二偏转角度，为第二预设时刻握持结构1的第二俯仰角度，为第二预设时刻握持结构1的第二自转角度。

为第一预设时刻握持结构1的第一开合角，为第二预设时刻握持结构1的第二开合角。

在一实施例中，如图5所示，上述步骤S120具体包括：通过计算第二自转角度和第一自转角度的差值，得到目标自转角度。上述步骤S120可以采用以下公式计算得到：

其中，为目标自转角度。

图6是本申请一实施例提供的手术系统的主从控制方法的步骤S120的子步骤的流程图；图7是本申请实施例提供的手术系统的主从控制方法的第一平面法向量和第二平面法向量的结构示意图。

在一实施例中，结合图5，如图6-7所示，上述步骤S120具体包括子步骤S121~子步骤S123。

子步骤S121：根据末端器械5在第一预设时刻相对于磁场发生器2的旋转矩阵、预设的握持结构1绕第一方向旋转的旋转矩阵以及第一列矩阵，计算得到第二方向上握持结构1在第一预设时刻进行自转后的第一平面法向量。

在一实施例中，上述子步骤S121具体可以采用以下公式计算得到：

其中，为第一平面法向量，为末端器械5相对于磁场发生器2的旋转矩阵，为第一列矩阵。

为预设的第一预设时刻握持结构1绕第一方向旋转的旋转矩阵。

其中，为第一预设时刻握持结构1的第一自转角度。

在上述实施例中，第一方向为握持结构1的坐标系中的X轴方向，第二方向为握持结构1的坐标系中的Z轴方向，握持结构1的坐标系是一种三维坐标系，用于描述握持结构1的位置和姿态。

子步骤S122：根据末端器械5在第二预设时刻相对于磁场发生器2的旋转矩阵和第一列矩阵，计算得到第二方向上握持结构1在第二预设时刻的第二平面法向量。

在一实施例中，子步骤S122具体可以采用以下公式计算得到：

其中，表示握持结构1在第二预设时刻的第二平面法向量，它与握持结构1在经过所有旋转变换后的平面法向量一致，因此为第二预设时刻末端器械5相对于磁场发生器2的旋转矩阵，为第二列矩阵。

子步骤S123：通过计算第一平面法向量与第二平面法向量的夹角，得到上述目标俯仰角。

在一实施例中，子步骤S123具体可以采用以下公式计算得到：

其中，表示目标俯仰角，因为握持结构1在一定预设时间内（例如10ms~20ms），其俯仰角度的变化不会超过90°，因此用于表示第一平面法向量与第二平面法向量之间的夹角小于90°的情况。

图8是本申请另一实施例提供的手术系统的主从控制方法的步骤S120的子步骤的流程图；图9是本申请实施例提供的手术系统的主从控制方法的第一方向向量和第二方向向量的结构示意图。

在一实施例中，结合图5，如图8-9所示，上述步骤S120还包括子步骤S124~子步骤S126，其中，子步骤S124~子步骤S126与上述另一实施例中的子步骤S121~子步骤S123不存在时间先后顺序。

子步骤S124：根据末端器械5在第一预设时刻相对于磁场发生器2的旋转矩阵、预设的握持结构1绕第一方向旋转的旋转矩阵以及第二列矩阵，计算得到第三方向上握持结构1在第一预设时刻进行自转后的第一方向向量。

在一实施例中，上述步骤S124具体可以采用以下公式计算得到：

其中，表示第一方向向量，为末端器械5在第一预设时刻相对于磁场发生器2的旋转矩阵，其可通过查找预设的数据库获得。

为第二列矩阵。

为预设的第一预设时刻握持结构1绕第一方向旋转的旋转矩阵。

其中，为第一预设时刻握持结构1的第一自转角度。

其中，第三方向为握持结构1的坐标系中的Y轴方向。

子步骤S125：根据末端器械5在第二预设时刻相对于磁场发生器2的旋转矩阵和第二列矩阵，计算得到第三方向上握持结构1在第二预设时刻的第二方向向量。

在一实施例中，上述子步骤S125具体可以采用以下公式计算得到：

其中，表示第二方向向量，为末端器械5在第二预设时刻相对于磁场发生器2的旋转矩阵，为第二列矩阵。

子步骤S126：通过计算第一方向向量和第二方向向量的夹角，得到目标偏转角。

在一实施例中，上述子步骤S126具体可以采用以下公式计算得到：

其中，表示第一方向向量和第二方向向量的夹角，即末端器械5的目标偏转角。

因为握持结构1在一定预设时间内（例如10ms~20ms），其偏转角度的变化不会超过90°，因此用于表示第一方向向量与第二方向向量之间的夹角小于90°的情况。

在一实施例中，结合图5，上述步骤S130具体包括子步骤S131~子步骤S133。

子步骤S131：根据第一位置信息，确定握持结构1在第一预设时间的第三位置信息。

具体包括：根据第一位置信息以及第一预设时刻握持结构1坐标系内位姿传感器3相对于握持结构1的基点的位置向量以及握持结构1的坐标系变换至磁场发生器2的坐标系的旋转变换矩阵，确定握持结构1的基点在第一预设时刻的第三位置信息。

在一实施例中，通过计算第一预设时刻握持结构1的坐标系内位姿传感器3相对于握持结构1的基点的位置向量与握持结构1的坐标系变换至磁场发生器2坐标系的旋转变换矩阵的乘积与第一位置信息之和，得到第三位置信息。

在一实施例中，上述子步骤S131具体可以采用以下公式计算：

其中，表示位姿传感器3在第一预设时刻的位置，即第一预设时刻的第一位置信息，为第一预设时刻握持结构1的坐标系变换至磁场发生器2的坐标系的旋转变换矩阵，为第一预设时刻握持结构1坐标系内位姿传感器3相对于握持结构1的基点的位置向量。

其中，开合角传感器4位于人手握住握持结构1时的虎口的位置处，以此位置作为握持结构1的基点的位置。

子步骤S132：根据第二位置信息，确定握持结构1在第二预设时间的第四位置信息。

在一实施例中，根据第二位置信息以及第二预设时刻握持结构1坐标系内位姿传感器3相对于握持结构1的基点的位置向量以及握持结构1的坐标系变换至磁场发生器2的坐标系的旋转变换矩阵，确定握持结构1的基点在第二预设时刻的第四位置信息。

在一实施例中，通过计算第二预设时刻握持结构1的坐标系内位姿传感器3相对于握持结构1的基点的位置向量和握持结构1的坐标系变换至磁场发生器2坐标系的旋转变换矩阵的乘积与第二位置信息之和，得到第四位置信息。

在一实施例中，上述子步骤S122具体可以采用以下公式计算：

其中，为第二位置信息，为握持结构1坐标系变换至磁场发生器2坐标系的旋转变换矩阵，为第二预设时刻握持结构1坐标系内位姿传感器3相对于握持结构1的基点的位置向量。其中，开合角传感器4位于人手握住握持结构1时的虎口的位置处，以此位置作为握持结构1的基点的位置。

子步骤S133：根据第三位置信息和第四位置信息，确定握持结构1的位移变化量。

在一实施例中，通过计算第四位置信息和第三位置信息的差值，得到上述握持结构1的位移变化量。

在一实施例中，上述步骤S140根据步骤S130计算得到的位移变化量，通过放缩系数映射得到末端器械5的目标位移信息，具体可以采用以下公式计算：

即。

其中，分别表示第一预设时刻手术系统的末端云台（图中未示出）在X、Y、Z轴方向上相对于磁场发生器2的位置信息，分别表示第二预设时刻手术系统的末端云台在X、Y、Z轴方向上相对于磁场发生器2的位置信息，末端器械5连接于末端云台，且末端云台的位移信息相等于末端器械5的目标位移信息，因此用上述末端云台的位移信息表示目标位移信息。

图10是本申请实施例提供的手术系统的主从控制方法的判断握持结构是否处于正常握持状态的流程示意图。

在一实施例中，手术系统具有掉落检测功能，用于检测握持结构1是否从操作者手中脱落。

握持结构1上设置的握持传感器13可位于位姿传感器3的上方的3~4cm处。握持传感器13检测握持结构1的电容值，并将实时检测的电容值生成电容信号发送至手术系统的处理器9中，由处理器9中的信号处理单元进行滤波后判断握持结构1是否处于正常握持状态。

具体的，该控制方法包括：

步骤S810：获取地平面电容值，接收电容值；

步骤S820：计算电容值与地平面电容值的电容差值。具体计算公式如下：

其中，为握持传感器13检测到的电容值，为地平面电容。

其中，地平面电容为手握住握持结构1时，握持传感器13对地平面的实时电容。

步骤S830：根据电容差值与预设电容值的关系，判断握持结构1是否处于正常握持状态。

在一实施例中，若电容差值大于预设电容值，判定握持结构1处于正常握持状态；若电容差值小于等于预设电容值，判定握持结构1未处于正常握持状态。

即时，握持结构1处于正常握持状态；≤时，握持结构1未处于正常握持状态。

其中，表示预设电容值。

步骤S840：在判断握持结构1未处于正常握持状态时，根据位姿传感器3检测到的握持结构1在第二方向的n个位置信息，计算并输出加速度。

在一实施例中，位姿传感器3对Z轴方向的位置进行检测，并记录n个读取循环内的值，通过计算输出加速度，具体计算公式如下：

其中，为握持结构1的位置信息在Z轴方向的二阶导数，为时刻握持结构1在Z轴方向的加速度。

步骤S850：根据加速度与预设加速度的关系，判断握持结构1是否跌落。

在一实施例中，用表示预设加速度。

若加速度大于等于预设加速度，，判定握持结构1跌落；若加速度小于预设加速度，即，判定握持结构1未跌落。

步骤S860：根据判断结果，控制手术系统继续工作或进入待机状态。

在判定握持结构1跌落时，控制手术系统进入待机状态；在判定握持结构1未跌落时，控制手术系统继续工作。

本申请实施例的手术系统通过设置握持传感器13，通过检测握持结构1是否处于正常握持状态，若握持传感器13识别到操作者手部接触信号，即握持结构1处于正常握持状态，才可以进行手术系统的控制操作，若未检测到手部接触信号，即握持结构1未处于正常握持状态，便无法进行控制操作，可以提高手术系统的控制操作的准确性，避免握持结构1脱落时发生误操作的问题。

图11为本申请实施例提供的手术系统的主从控制方法的判断握持结构是否进行夹持操作的流程示意图。

在一实施例中，手术系统包括设置于末端器械5的第一电机和第二电机，末端器械5设置有至少两个夹钳，包括第一夹钳和第二夹钳，第一电机位于第一夹钳上，第二电机位于第二夹钳上，第一电机和第二电机可以实时反馈输出力矩，通过控制夹持的第一电机和第二电机输出力矩的差值以及第一电机和第二电机内编码器的值的差值可以判断末端器械5是否进行夹持操作。

握持结构1上还设置有连接于处理器9的夹持指示灯14。

该控制方法包括步骤S910~步骤S940。

步骤S910：获取第一电机内第一编码器值和第二电机内第二编码器值。

步骤S920：计算第一编码器值和第二编码器值之间的第一差值。在一实施例中，上述步骤S920具体可以采用以下公式计算：

其中，在一实施例中，表示第一差值，可以表示第一编码器值，可以表示第二编码器值。

步骤S930：根据第一差值与预设的电机编码器值的关系，判断末端器械5是否进行夹持操作。

步骤S940：根据判断结果控制所述夹持指示灯亮起或保持熄灭。

若末端器械5进行夹持操作，控制夹持指示灯14亮起。

若末端器械5未进行夹持操作，控制夹持指示灯14保持熄灭。

在一实施例中，步骤S930具体可以包括子步骤S931~S935。

子步骤S931：判断第一差值是否小于预设的电机编码器值，即。

子步骤S932：若是，判定末端器械5未进行夹持操作，控制夹持指示灯14保持熄灭。

子步骤S933：若否，接收第一电机发送的第一输出力矩和第二电机发送的第二输出力矩。

其中，当其中的时，表明第一钳头和第二钳头正在进行合拢过程。

上述子步骤S933中，因当末端器械5的第一钳头和第二钳头进行合拢的过程时，无法判断末端器械5是否进行了夹持操作，因此需要通过第一输出力矩和第二输出力矩辅助进行判断。

步骤S934：计算第一输出力矩的第二输出力矩之间的第二差值。在一实施例中，上述步骤S934具体可以采用以下公式计算：

其中，在一实施例中，表示第二差值，可以表示第一输出力矩，可以表示第二输出力矩。

步骤S935：根据第二差值与预设输出力矩值之间的关系，判断末端器械5是否进行夹持操作。

在一实施例中，步骤S935具体可以包括子步骤S9351~S9353。

子步骤S9351：判断第二差值是否小于预设的输出力矩值，即。

子步骤S9352：若是，判定末端器械5未进行夹持操作，控制夹持指示灯14保持熄灭。

步骤S9353：若否，时，表明两个钳头中间受到向外张开的阻力，判定末端器械5进行夹持操作，控制夹持指示灯14亮起。

本申请实施例通过设置夹持指示灯14，以及设置涉及夹持指示灯14的控制方法，可以精准判断末端器械5是否进行夹持操作，提高末端器械5工作的准确性，提高操作者的使用体验。

在一实施例中，握持结构1上可以设置有按键（图中未示出），按键连接于处理器9，其可以用于实现使能、开启或关闭手术系统的滤波器的功能以及进行档位切换功能。

本发明还提供一种电子设备，电子设备可以包括处理器、存储器以及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述的手术系统的主从控制方法。

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

在本申请的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。

需要说明的是，在不冲突的情况下，本申请中的实施例中的特征可以相互结合。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (12)

1.一种手术系统的主从控制装置，其特征在于，所述手术系统的主从控制装置包括磁场发生器、末端器械以及控制末端器械运动的握持结构，所述握持结构上设置有位姿传感器和开合角传感器，所述磁场发生器连接所述位姿传感器，用于接收所述位姿传感器传输的姿态信息，并传输至处理器，所述处理器用于：

获取所述位姿传感器在第一预设时刻的第一位置信息和第一姿态信息以及第二预设时刻的第二位置信息和第二姿态信息；

根据所述第一姿态信息和第二姿态信息，确定所述末端器械的目标姿态信息；

根据所述第一位置信息和第二位置信息，确定所述握持结构的位移变化量；

根据所述位移变化量，通过放缩系数映射得到所述末端器械的目标位移信息；

将所述目标位移信息、所述目标姿态信息和所述开合角传感器获取的所述握持结构的开合角，发送至所述末端器械，控制所述末端器械运动；

其中，所述目标姿态信息包括目标俯仰角，所述根据所述第一姿态信息和第二姿态信息，确定所述末端器械的目标姿态信息，包括：

根据所述末端器械在所述第一预设时刻相对于所述磁场发生器的旋转矩阵、预设的所述握持结构绕第一方向旋转的旋转矩阵以及第一列矩阵，计算得到第二方向上所述握持结构在所述第一预设时刻进行自转后的第一平面法向量；

根据所述末端器械在所述第二预设时刻相对于所述磁场发生器的旋转矩阵和第一列矩阵，计算得到第二方向上所述握持结构在所述第二预设时刻的第二平面法向量；

通过计算所述第一平面法向量与所述第二平面法向量的夹角，得到所述目标俯仰角。

2.根据权利要求1所述的手术系统的主从控制装置，其特征在于，所述根据所述第一位置信息和第二位置信息，确定所述握持结构的位移变化量，包括：

根据所述第一位置信息，确定所述握持结构在第一预设时刻的第三位置信息；

根据所述第二位置信息，确定所述握持结构在第二预设时刻的第四位置信息；

根据所述第三位置信息和第四位置信息，确定所述握持结构的位移变化量。

3.根据权利要求2所述的手术系统的主从控制装置，其特征在于，所述手术系统的主从控制装置还包括磁场发生器，与所述位姿传感器连接，用于接收所述位姿传感器传输的位置信息，并传输至所述处理器，所述根据所述第一位置信息，确定所述握持结构在第一预设时刻的第三位置信息，包括：

根据所述第一位置信息以及所述第一预设时刻所述握持结构的坐标系内位姿传感器相对于所述握持结构的基点的位置向量，以及所述握持结构的坐标系变换至所述磁场发生器的坐标系的旋转变换矩阵，确定所述握持结构的基点在第一预设时刻的第三位置信息。

4.根据权利要求2所述的手术系统的主从控制装置，其特征在于，所述手术系统的主从控制装置还包括磁场发生器，与所述位姿传感器连接，用于接收所述位姿传感器传输的位置信息，并传输至所述处理器，所述根据所述第二位置信息，确定所述握持结构在第二预设时刻的第四位置信息，包括：

根据所述第二位置信息以及所述第二预设时刻所述握持结构的坐标系内位姿传感器相对于所述握持结构的基点的位置向量，以及所述握持结构的坐标系变换至所述磁场发生器的坐标系的旋转变换矩阵，确定所述握持结构的基点在第二预设时刻的第四位置信息。

5.根据权利要求1所述的手术系统的主从控制装置，其特征在于，所述第一姿态信息包括第一自转角度，所述第二姿态信息包括第二自转角度，所述目标姿态信息包括目标自转角度，所述根据所述第一姿态信息和第二姿态信息，确定所述末端器械的目标姿态信息，包括：

通过计算所述第二自转角度和所述第一自转角度的差值，得到所述目标自转角度。

6.根据权利要求1所述的手术系统的主从控制装置，其特征在于，所述目标姿态信息还包括目标偏转角，所述根据所述第一姿态信息和第二姿态信息，确定所述末端器械的目标姿态信息，包括：

根据所述末端器械在所述第一预设时刻相对于所述磁场发生器的旋转矩阵、预设的所述握持结构绕第一方向旋转的旋转矩阵以及第二列矩阵，计算得到第三方向上所述握持结构在所述第一预设时刻进行自转后的第一方向向量；

根据所述末端器械在所述第二预设时刻相对于所述磁场发生器的旋转矩阵和第二列矩阵，计算得到第三方向上所述握持结构在所述第二预设时刻的第二方向向量；

通过计算所述第一方向向量和所述第二方向向量的夹角，得到所述目标偏转角。

7.根据权利要求1所述的手术系统的主从控制装置，其特征在于，所述握持结构上设置有握持传感器，用于检测所述握持结构的电容值，所述处理器还用于：

获取地平面电容值，接收所述电容值；

计算所述电容值与地平面电容值的电容差值；

根据所述电容差值与预设电容值的关系，判断所述握持结构是否处于正常握持状态；

在判断所述握持结构未处于正常握持状态时，根据所述位姿传感器检测到的所述握持结构在第二方向的n个位置信息，计算并输出加速度；

根据所述加速度与预设加速度的关系，判断所述握持结构是否跌落，并根据判断结果，控制所述手术系统继续工作或进入待机状态。

8.根据权利要求7所述的手术系统的主从控制装置，其特征在于，所述根据所述电容差值与预设电容值的关系，判断所述握持结构是否处于正常握持状态，包括：

若所述电容差值大于所述预设电容值，判定所述握持结构处于正常握持状态；

若所述电容差值小于所述预设电容值，判定所述握持结构未处于正常握持状态。

9.根据权利要求7所述的手术系统的主从控制装置，其特征在于，所述根据所述加速度与预设加速度的关系，判断所述握持结构是否跌落，包括：

若所述加速度大于所述预设加速度，判定所述握持结构跌落，则控制所述手术系统进入待机状态；

若所述加速度小于等于所述预设加速度，判定所述握持结构未跌落，则控制所述手术系统继续工作。

10.根据权利要求1所述的手术系统的主从控制装置，其特征在于，所述手术系统还包括设置于所述末端器械的第一电机和第二电机，所述握持结构上设置有夹持指示灯，所述处理器还用于：

获取所述第一电机内第一编码器值和所述第二电机内第二编码器值；

计算所述第一编码器值和所述第二编码器值之间的第一差值；

根据所述第一差值与预设的电机编码器值的关系，判断所述末端器械是否进行夹持操作；

根据判断结果控制所述夹持指示灯亮起或保持熄灭。

11.根据权利要求10所述的手术系统的主从控制装置，其特征在于，所述根据所述第一差值与预设的电机编码器值的关系，判断所述末端器械是否进行夹持操作，包括：

若所述第一差值小于所述电机编码器值，确定所述末端器械未进行夹持操作；

若所述第一差值不小于所述电机编码器值，接收所述第一电机发送的第一输出力矩和所述第二电机发送的第二输出力矩；

计算所述第一输出力矩和所述第二输出力矩之间的第二差值；

根据所述第二差值与预设输出力矩值之间的关系，判断所述末端器械是否进行夹持操作。

12.根据权利要求11所述的手术系统的主从控制装置，其特征在于，所述根据所述第二差值与预设输出力矩值之间的关系，判断所述末端器械是否进行夹持操作，包括：

若所述第二差值小于预设输出力矩值，确定所述末端器械未进行夹持操作；

若所述第二差值不小于预设输出力矩值，确定所述末端器械进行夹持操作。