CN113659879A - 一种手术机器人的电机闭环控制装置及方法 - Google Patents

Abstract

本发明涉及一种手术机器人的电机闭环控制装置及方法，包括与基座连接的执行侧，执行侧位于驱动侧顶部，且执行侧与驱动侧可拆卸连接，驱动侧输出机构设有第一磁铁，基座内与第一磁铁对应的位置设有第一传感器；执行侧从动轮轴心处设有第二磁铁，基座内与第二磁铁对应的位置安装第二传感器，两组传感器分别获取驱动侧输出的转速和执行侧输出的转速。两传感器均位于驱动侧的基座内部，在磁场的作用下获取转速数据的过程不需要执行侧与传感器本体发生直接接触，有利于执行侧的灭菌操作和无菌隔离，能够利用基座上的串口将两组传感器获取的数据直接导出，从而使得获取执行侧转速的过程不再需要导线，能够满足手术器械的无菌隔离要求。

Description

一种手术机器人的电机闭环控制装置及方法

技术领域

本发明涉及手术机器人领域，具体为一种手术机器人的电机闭环控制装置及方法。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

采用机器人技术的辅助手术设备和辅助手术系统，在实施手术时要将非灭菌设备和已灭菌设备通过无菌隔离罩隔开，非灭菌的设备/系统部件被罩在无菌隔离罩内，已灭菌的设备/系统部件位于无菌隔离罩外与患者接触。此类手术机器人通常包括非灭菌的驱动侧，和已灭菌的执行侧，执行侧末端携带不同的执行器(例如持针钳、施夹钳，剥离钳，单极钩等)，驱动侧和执行侧需要通过机械耦合机构连接，以传导驱动侧电机的旋转动力以驱动执行侧末端的执行器。

手术机器人对末端执行器的控制精度要求较高，而驱动侧电机输出的旋转运动和执行侧接最终输出的运动之间存在误差，从而影响末端执行器的控制精度，这里的误差来源于执行侧内部多个机械零件之间的间隙(例如齿轮啮合时，相邻两组齿轮的齿面间隙)，此类运动误差较小，对于一般的机械设备可以忽略，但对于手术机器人而言，该结构产生的运动误差使得针对驱动侧发出的运动控制指令，由执行侧输出时存在微小的位置偏差或是角度偏差，最终影响手术机器人动作的精准度，位姿准确度和重复性，甚至主从控制的延迟时间。

对于上述问题，现有技术通过驱动侧和执行侧的闭环速度检测来解决，而现有的电机驱动闭环方式常规的有两种方式，第一是在电机输出轴安装速度检测传感器，通过系统设计的传动比例计算执行侧输出的转速；第二是在电机输出轴安装速度检测传感器的同时，在执行侧安装有线的传感器检测输出转速。

第一种方式只能对电机输出轴检测转速，而未对执行侧输出的转速进行检测，仅通过系统设计的传动比例计算输出转速仍然是一个理论值，会引入较大的误差，无法最大限度的减小因传动机构的机械间隙带来的误差，因此这种方式精度还不能满足手术器械的驱动控制要求。

第二种方式虽然同时检测了驱动侧和执行侧的转速，但需要有连接线缆，无法实现驱动侧和执行侧的无菌隔离，无法满足手术环境的要求。

发明内容

为了解决上述背景技术中存在的技术问题，本发明提供一种手术机器人的电机闭环控制装置及方法，通过在驱动侧输出轴和执行侧从动齿轮轴轴心处同时安装磁铁，在基座内与磁铁对应的位置处分别安装非接触式磁性旋转编码器，同时检测驱动侧输出的转速和执行侧输出的转速，通过软件算法最大限度的减小因传动机构机械空程间隙引发的误差，提升运动控制精度，从而满足手术机器人的使用要求。

为了实现上述目的，本发明采用如下技术方案：

本发明的第一个方面提供一种手术机器人的电机闭环控制装置，包括与基座连接的执行侧，执行侧位于驱动侧顶部，且执行侧与驱动侧可拆卸连接，驱动侧输出机构设有第一磁铁，基座内与第一磁铁对应的位置设有第一传感器；执行侧从动轮轴心处设有第二磁铁，基座内与第二磁铁对应的位置安装第二传感器，两组传感器分别获取驱动侧输出的转速和执行侧输出的转速。

第一传感器和第二传感器均为非接触式磁性旋转编码器。

驱动侧包括与基座连接的电机和驱动侧输出机构，电机连接驱动侧输出机构输出驱动手术机器人末端执行器动作的动力，驱动侧输出机构的输出轴上安装第一磁铁。

第一传感器位于基座内与第一磁铁对应的位置，第一传感器在第一磁铁的影响下获取驱动侧输出的转速。

执行侧包括安装在外壳内的主动轮和从动轮，主动轮连接转轴，驱动侧输出机构的输出轴与转轴可拆卸连接，传递驱动侧的动力，转轴带动主动轮运动，主动轮与从动轮啮合，从动轮输出驱动侧的动力。

执行侧还具有末端执行器，从动轮与末端执行器连接。

从动轮的轴心处设有第二磁铁，执行侧位于驱动侧顶部时，第二传感器位于基座内与第二磁铁对应的位置。

第二传感器在第二磁铁的影响下获取执行侧从动轮输出的转速。

本发明的第二个方面提供基于上述装置实现电机闭环控制的方法，包括以下步骤：

获取驱动侧输出的转速数据和执行侧从动轮的转速数据；

通过滤波判断转速是否超出阈值，若超出阈值则经间隙消除算法后，控制电机旋转至设定位置。

间隙消除算法的步骤为：

处理器以设定周期，循环获取驱动侧和执行侧经过滤波处理的旋转编码器数据；

对比驱动侧和执行侧的传感器数据，获得电机传动间隙的机械空程；

利用机械空程作为本次运行动作的修正因子，执行间隙消除；

间隙消除的过程中，驱动侧电机旋转角度设定值与修正因子的和，作为为执行侧目标转动角度。

与现有技术相比，以上一个或多个技术方案存在以下有益效果；

1、两处传感器均位于驱动侧的基座内部，且均为非接触式磁性旋转编码器，在磁场的作用下获取转速数据的过程不需要执行侧与传感器本体发生直接接触，有利于执行侧的灭菌操作和无菌隔离，能够利用基座上的串口将两组传感器获取的数据直接导出，使得获取执行侧转速的过程不再需要导线，能够满足手术器械的无菌隔离要求。

2、通过磁场和非接触式磁性旋转编码器实现了驱动侧和执行侧隔离，同时检测两者速度，通过软件算法消除传动机构机械空程间隙，以最大限度减小传动机构的误差，提升运动控制精度。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1是本发明一个或多个实施例提供的整体结构示意图；

图2是本发明一个或多个实施例提供的执行侧底部结构示意图；

图3是本发明一个或多个实施例提供的执行侧主动轮和从动轮啮合结构示意图；

图4是本发明一个或多个实施例提供的实现电机闭环控制的流程示意图；

图中：1.基座；2.执行侧；3.驱动侧输出机构；21.转轴；22.主动轮；23.从动轮；24.末端执行器。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

正如背景技术中所描述的，手术机器人驱动侧电机产生的动力通过齿轮减速箱、机械耦合机构到达执行侧的旋转轴，旋转轴再通过齿轮啮合从动轮转化为牵拉末端手术器械(末端执行器)的动力。系统含有齿轮减速箱和传动耦合机构和主从动轮齿轮啮合，存在机械空程间隙，这给系统运行精确控制带来较大误差。

因此以下实施例，在驱动侧电机齿轮减速箱的输出轴安装磁铁，基座内与该磁铁对应的位置安装非接触式磁性旋转编码器，执行侧旋转轴从动齿轮轴心处安装磁铁，在基座内与该磁铁对应的位置安装非接触式磁性旋转编码器，利用磁铁和非接触式磁性旋转编码器获取驱动侧输出的转速和执行侧输出的转速。

通过基座上设置的SPI接口(串行外设接口)，实时读取上述两个位置非接触式磁性旋转编码器获取的数据，依靠基座内主控板上搭载的MCU内置滤波算法、间隙消除补偿算法进一步修正电机的旋转位置，确保电机旋转位置与末端器械运动的精确性，同时依靠执行侧磁铁与驱动侧非接触式磁性旋转编码器的间距可以实时判断执行侧机构是否安装良好，以保证器械运行时的安全性和可靠性。

实施例一：

如图1-3所示，一种手术机器人的电机闭环控制装置，包括与基座1连接的执行侧2，执行侧2与驱动侧可拆卸连接，驱动侧输出机构设有第一磁铁，基座1内与第一磁铁对应的位置设有第一传感器；执行侧从动轮轴心处设有第二磁铁，基座内与第二磁铁对应的位置安装第二传感器，两组传感器分别获取驱动侧输出的转速和执行侧输出的转速。

本实施例中，第一传感器和第二传感器均为非接触式磁性旋转编码器。

驱动侧包括与基座1连接的电机和驱动侧输出机构3，本实施例中驱动侧输出机构3为齿轮减速箱，电机连接齿轮减速箱输出驱动手术机器人末端执行器动作的动力，驱动侧输出机构3的输出轴上安装第一磁铁。

第一传感器位于基座1内与第一磁铁对应的位置，第一传感器在第一磁铁的影响下获取驱动侧输出的转速。

执行侧包括安装在外壳内的主动轮22和从动轮23，主动轮22连接转轴21，驱动侧输出机构3的输出轴与转轴21可拆卸连接，传递驱动侧的动力，转轴21带动主动轮22运动，主动轮22与从动轮23啮合，从动轮23输出驱动侧的动力。

执行侧还具有末端执行器24，从动轮23与末端执行器24连接。

从动轮23的轴心处设有第二磁铁，执行侧位于驱动侧顶部时，第二传感器位于基座内与第二磁铁对应的位置。

第二传感器在第二磁铁的影响下获取执行侧从动轮输出的转速。

上述结构中，两处传感器均位于驱动侧的基座内部，且均为非接触式磁性旋转编码器，在磁场的作用下获取转速数据的过程不需要执行侧与传感器本体发生直接连接，有利于执行侧的灭菌操作和无菌隔离，能够利用基座上的串口将两组传感器获取的数据直接导出，获取执行侧转速的过程不再需要导线，能够满足手术器械的无菌隔离要求。

本实施例中的非接触式磁性旋转编码器，其核心技术为霍尔传感器，芯片参数如下：

AS5145是一种12位可编程、非接触式磁性旋转编码器，用于360度全回转角度的精确测量。是一种片上系统，将霍尔元件、模拟前端和数字信号处理集成在一个单一的设备中。要测量角度，只需要一个简单的双极磁铁，在芯片的中心旋转。磁铁可以放置在集成电路的两侧。绝对角度测量提供即时指示的磁体的角位置的分辨率0.08790＝每转4096个位置。

实施例二：

本实施例提供了基于上述检测装置实现手术机器人的电机闭环控制方法，包括以下步骤：

获取驱动侧输出的转速数据和执行侧从动轮的转速数据；

通过滤波算法判断转速是否超出阈值，若超出阈值则经间隙消除算法后，控制电机旋转至设定位置。

滤波算法的原理：

非接触式磁性旋转编码器测得的旋转角为12Bit数字量，会存在一定的量化偏差和抖动，本实施例通过过采样和数字滤波进行数据处理，减小误差，使之跟接近真实值，更精确，这些经过处理的传感器数据会作为间隙消除程序的输入数据。

间隙消除算法(程序)的原理：

启动电机时，处理器记录驱动侧和执行侧经过滤波算法处理的旋转编码器数据，处理器以一定周期，循环读取驱动侧和执行侧经过滤波算法处理的旋转编码器数据作比对，获得电机传动间隙的机械空程，作为本次运行动作的修正因子，按如下公式执行间隙消除：

α＝β+θ；

α：执行侧目标转动角度；

β：驱动侧电机旋转角度设定值；

θ：间隙消除修正因子。

上述处理过程中，旋转编码器的数据处理更加准确，利用获得的间隙消除因子，对电机执行实时控制。

编码器在运动过程中的每一个位置都有一个确定的数字码作为位置的对应，本实施例的驱动侧和执行侧获取的传感器数据中，两组编码器的数字码一一对应，代表着驱动侧和执行侧的旋转位置一一对应，如果驱动侧的某一个旋转位置没有与执行侧对应，那么说明该位置下的执行侧出现了“机械空程”，则代表驱动侧位置的一个或几个数字码无法被执行侧对应，需要人为的“修正因子”来消除这部分“机械空程”。

因此，本实施例中，驱动侧的输出结构(即第一磁铁安装处)与执行侧的第二磁铁安装处的传动比是1:1，驱动侧与执行侧采用相同的非接触式磁性旋转编码器，所以在没有间隙的情况下，驱动侧和执行侧的数字码的增量是一致的，即驱动则变化几个数字量，执行侧也变化同样的数字量，也就是上文中描述的“两组编码器的数字码一一对应”，监测增量的方式不受限于传感器机械安装误差和机械传动误差，只要驱动侧和执行侧增量一致就认为间隙被消除。同样的，由于机械间隙的存在，执行侧可能跟驱动侧的增量不一致，因此引入修正的过程能够最大限度的减小传动机构的误差，从而提升运动控制精度。

处理器可以集成在手术机器人本体中，不限制具体的规格型号；通过磁铁和非接触式磁性旋转编码器实现了驱动侧和执行侧隔离，同时检测两者速度，通过软件算法消除传动机构机械空程间隙，以最大限度减小传动机构的误差，提升运动控制精度。

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

Claims (10)

1.一种手术机器人的电机闭环控制装置，其特征在于：包括与基座连接的执行侧，执行侧位于驱动侧顶部，且执行侧与驱动侧可拆卸连接，驱动侧输出机构设有第一磁铁，基座内与第一磁铁对应的位置设有第一传感器；执行侧从动轮轴心处设有第二磁铁，基座内与第二磁铁对应的位置安装第二传感器，两组传感器分别获取驱动侧输出的转速和执行侧输出的转速。

2.如权利要求1所述的一种手术机器人的电机闭环控制装置，其特征在于：所述第一传感器和第二传感器均为非接触式磁性旋转编码器。

3.如权利要求1所述的一种手术机器人的电机闭环控制装置，其特征在于：所述驱动侧包括与基座连接的电机和驱动侧输出机构，电机连接驱动侧输出机构输出驱动手术机器人末端执行器动作的动力，驱动侧输出机构的输出轴上安装第一磁铁。

4.如权利要求1所述的一种手术机器人的电机闭环控制装置，其特征在于：所述第一传感器位于基座内与第一磁铁对应的位置，第一传感器在第一磁铁的影响下获取驱动侧输出的转速。

5.如权利要求1所述的一种手术机器人的电机闭环控制装置，其特征在于：所述执行侧包括安装在外壳内的主动轮和从动轮，主动轮连接转轴，驱动侧输出机构的输出轴与转轴可拆卸连接，传递驱动侧的动力，转轴带动主动轮运动，主动轮与从动轮啮合，从动轮输出驱动侧的动力。

6.如权利要求5所述的一种手术机器人的电机闭环控制装置，其特征在于：所述执行侧还具有末端执行器，从动轮与末端执行器连接。

7.如权利要求5所述的一种手术机器人的电机闭环控制装置，其特征在于：所述从动轮的轴心处设有第二磁铁，执行侧位于驱动侧顶部时，第二传感器位于基座内与第二磁铁对应的位置；第二传感器在第二磁铁的影响下获取执行侧从动轮输出的转速。

8.基于权利要求1所述装置实现手术机器人的电机闭环控制方法，其特征在于：包括以下步骤：

获取驱动侧输出的转速数据和执行侧从动轮的转速数据；

通过滤波判断转速是否超出阈值，若超出阈值则经间隙消除算法后，控制电机旋转至设定位置。

9.如权利要求8所述的控制方法，其特征在于：所述间隙消除算法，包括以下步骤：

处理器以设定周期，循环获取驱动侧和执行侧经过滤波处理的旋转编码器数据；

对比驱动侧和执行侧的传感器数据，获得电机传动间隙的机械空程；

利用机械空程作为本次运行动作的修正因子，执行间隙消除。

10.如权利要求9所述的控制方法，其特征在于：具体为：所述间隙消除的过程中，驱动侧电机旋转角度设定值与修正因子的和，作为执行侧目标转动角度。

Images