CN113876436A - 主从运动的控制方法、机器人系统、设备及存储介质 - Google Patents

主从运动的控制方法、机器人系统、设备及存储介质 Download PDF

Info

Publication number
CN113876436A
CN113876436A CN202110744664.9A CN202110744664A CN113876436A CN 113876436 A CN113876436 A CN 113876436A CN 202110744664 A CN202110744664 A CN 202110744664A CN 113876436 A CN113876436 A CN 113876436A
Authority
CN
China
Prior art keywords
coordinate system
operator
pose
master
current
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110744664.9A
Other languages
English (en)
Inventor
徐凯
杨皓哲
吴百波
王翔
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Surgerii Technology Co Ltd
Original Assignee
Beijing Surgerii Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Surgerii Technology Co Ltd filed Critical Beijing Surgerii Technology Co Ltd
Publication of CN113876436A publication Critical patent/CN113876436A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B34/37—Master-slave robots
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/70—Manipulators specially adapted for use in surgery
    • A61B34/77—Manipulators with motion or force scaling
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1689—Teleoperation
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A61B2034/2046—Tracking techniques
    • A61B2034/2059—Mechanical position encoders
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A61B2034/2046—Tracking techniques
    • A61B2034/2065—Tracking using image or pattern recognition
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A61B2034/2072—Reference field transducer attached to an instrument or patient
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • A61B2034/302—Surgical robots specifically adapted for manipulations within body cavities, e.g. within abdominal or thoracic cavities
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B90/00—Instruments, implements or accessories specially adapted for surgery or diagnosis and not covered by any of the groups A61B1/00 - A61B50/00, e.g. for luxation treatment or for protecting wound edges
    • A61B90/36—Image-producing devices or illumination devices not otherwise provided for
    • A61B90/361—Image-producing devices, e.g. surgical cameras
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40188—Position control with scaling, master small movement, slave large movement
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40195—Tele-operation, computer assisted manual operation

Landscapes

  • Health & Medical Sciences (AREA)
  • Engineering & Computer Science (AREA)
  • Surgery (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Robotics (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Biomedical Technology (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本公开涉及机器人领域，公开一种主从运动的控制方法、机器人系统、设备及存储介质，其中，主从运动的控制方法，包括：确定主操作器的当前位姿，当前位姿包括当前位置和当前姿态；基于主操作器的当前位姿及主操作器与从动工具的位姿关系，确定从动工具的目标位姿；以及基于从动工具的目标位姿，生成从动工具的控制信号。可以实现主操作器对从动工具的遥操作，提高主操作器对从动工具遥操作的控制精度。

Description

主从运动的控制方法、机器人系统、设备及存储介质
技术领域
本公开涉及机器人领域，尤其涉及一种主从运动的控制方法、机器人系统、设备及存储介质。
背景技术
随着科技的发展，通过医疗机器人辅助医疗工作人员进行手术得到了快速的发展，医疗机器人不仅可以帮助医疗工作人员进行一系列的医疗诊断和辅助治疗，还能有效缓解医疗资源的紧张问题。
通常，医疗机器人包括用于执行操作的从动工具及用于控制从动工具运动的主操作器。在实际场景中，从动工具被设置成能够进入操作区域，医疗工作人员通过遥操作主操作器进而控制从动工具在操作区域中的运动，实现医疗操作。一般医疗机器人通过主操作器与从动工具之间的运动转换实现主操作器对从动工具的运动控制。
手术机器人对操作精度和人机交互体验要求很高。例如，手术机器人可以通过相机采集从动工具的图像，并通过显示器进行显示。操作者(例如手术医生)通过显示器中的图像获取从动工具的位置和姿态，并通过遥操作主操作器控制从动工具的位置和姿态。这样，相机和显示器的位置和姿态会影响操作者下一步的操作方向和距离。因此，需要提高手术机器人的主从运动控制的精度，以实现主操作器与从动工具之间主从运动控制的目标结果与操作者的感官意愿的一致性。
发明内容
在一些实施例中，本公开提供了一种主从运动的控制方法，包括：确定主操作器的当前位姿，当前位姿包括当前位置和当前姿态；基于主操作器的当前位姿及主操作器与从动工具的位姿关系，确定从动工具的目标位姿；以及基于从动工具的目标位姿，生成从动工具的控制信号。
在一些实施例中，本公开提供了一种机器人系统，包括：主操作器，包括机械臂、设置在机械臂上的手柄以及设置在机械臂上的至少一个关节处的至少一个主操作器传感器，至少一个主操作器传感器用于获得至少一个关节的关节信息；从动工具，包括柔性臂体和末端器械；至少一个驱动装置，用于驱动从动工具的柔性臂体；至少一个驱动装置传感器，与至少一个驱动装置耦合并且用于获得驱动信息；以及控制装置，与主操作器和至少一个驱动装置通信连接，控制装置被配置成用于执行上述任一个实施例中提供的主从运动的控制方法。
在一些实施例中，本公开提供了一种计算机设备，计算机设备包括：存储器，用于存储至少一条指令；以及处理器，与存储器耦合并且用于执行至少一条指令以执行上述任一个实施例中提供的主从运动的控制方法。
在一些实施例中，本公开提供了一种计算机可读存储介质，用于存储至少一条指令，至少一条指令由计算机执行时致使机器人系统实现上述任一个实施例中提供的主从运动的控制方法。
附图说明
为了更清楚地说明本公开实施例中的技术方案，下面将对本公开实施例描述中所需要使用的附图作简单的介绍。下面描述中的附图仅仅示出本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据本公开实施例的内容和这些附图获得其他的实施例。
图1示出根据本公开一些实施例的主从运动的控制方法流程图；
图2示出根据本公开一些实施例的机器人系统的结构示意图；
图3示出根据本公开一些实施例的主从运动映射中的坐标系示意图；
图4示出根据本公开一些实施例的机器人系统的示意图。
具体实施方式
为使本公开解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图对本公开实施例的技术方案作进一步的详细描述，显然，所描述的实施例仅仅是本公开示例性实施例，而不是全部的实施例。
在本公开的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本公开和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本公开的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。在本公开的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“耦合”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连；可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本公开中的具体含义。在本公开中，定义靠近操作者(例如医生)的一端为近端、近部或后端、后部，靠近手术患者的一端为远端、远部或前端、前部。本领域技术人员可以理解，本公开的实施例可以用于医疗器械或手术机器人，也可以用于其他非医疗装置。
在本公开中，术语“位置”指对象或对象的一部分在三维空间中的定位(例如，可使用笛卡尔X、Y和Z坐标方面的变化描述三个平移自由度，例如分别沿笛卡尔X轴、Y轴和Z轴的三个平移自由度)。在本公开中，术语“姿态”指对象或对象的一部分的旋转设置(例如，三个旋转自由度，可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指对象或对象的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。在本公开中，主操作器的手柄的位姿可由主操作器关节的关节信息的集合(例如由这些关节信息组成的一维矩阵)来表示。从动工具的位姿可由从动工具的驱动信息(例如，从动工具的柔性臂体的驱动信息)来确定。在本公开中，关节的关节信息可以包括相应关节相对于相应的关节轴所旋转的角度或者相对于初始位置移动的距离。
图1示出根据本公开一些实施例的主从运动的控制方法流程图100，图2示出根据本公开一些实施例的机器人系统的结构示意图200。方法100可以由硬件、软件或者固件实现或执行。在一些实施例中，方法100可以由机器人系统(例如，图2所示的机器人系统200)执行。在一些实施例中，方法100可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图2所示的控制装置220)读取并执行。例如，用于机器人系统200的控制装置可以包括处理器，被配置为执行方法100。在一些实施例中，这些指令可以存储在计算机可读介质上。
在一些实施例中，如图2所示，机器人系统200可以包括主控台车210、手术台车230及控制装置220。控制装置220可以与主控台车210和手术台车230通信连接，例如可通过线缆连接、也可以通过无线连接，以实现主控台车210和手术台车230之间的通信。主控台车210包括供操作者遥操作的主操作器，手术台车230包括用于执行手术的从动工具。通过控制装置220实现主控台车中的主操作器与手术台车中的从动工具之间的主从映射，实现主操作器对从动工具的运动控制。在一些实施例中，手术台车包括设置在手术台车上的至少一个从动工具(如手术工具或视觉工具)。并且从动工具被设置成能够通过鞘套进入操作区域，其中，鞘套可以固定在患者的手术口(例如切口或自然开口)处，操作区域可以是进行手术的区域。从动工具可以包括臂体和末端器械。从动工具的臂体可以是柔性臂体，末端器械可以设置在柔性臂体远端。手术工具的末端器械可以包括但不限于手术钳、电刀、电勾等。视觉工具的末端器械可以包括但不限于成像装置或照明装置等。在一些实施例中，主控台车包括主操作器、显示器及脚踏板。本领域技术人员可以理解，主控台车210和手术台车230可以采用其他结构或者形式，例如基座、支架或建筑物等。
图3示出根据本公开一些实施例的主从运动映射中的坐标系示意图。在图3中各坐标系的定义如下：
{Eb}：相机基坐标系，原点位于视觉工具的基座或者入腹鞘套出口，
Figure BDA0003142360170000051
与基座延长线或者入腹鞘套轴向方向一致，
Figure BDA0003142360170000052
方向如图3所示。
{Tb}：从动工具基坐标系，原点位于从动工具的基座或者入腹鞘套出口，
Figure BDA0003142360170000053
与基座延长线或者入腹鞘套轴向方向一致，
Figure BDA0003142360170000054
方向如图3所示。
{Cam}：相机坐标系，原点位于相机中心，相机轴线方向为
Figure BDA0003142360170000055
方向，视野摆正后上方为
Figure BDA0003142360170000056
方向。
{Tt}：从动工具的末端器械的坐标系，原点位于从动工具的末端，
Figure BDA0003142360170000057
与末端器械的轴线方向一致，
Figure BDA0003142360170000058
方向如图3所示。
{ITt}：从动工具的末端器械的图像坐标系，与显示器中所显示的末端器械的图像相关联。
{W}：参考坐标系，可以是主操作器所在空间的坐标系或者世界坐标系，可以以操作者的体感为基准，当操作者端坐于主控台之前时，体感向上为
Figure BDA0003142360170000059
方向，体感向前方向为
Figure BDA00031423601700000510
方向。
{Screen}：显示器坐标系，原点位于显示器中心，垂直屏幕画面向内方向为
Figure BDA00031423601700000511
正方向，屏幕画面上方为
Figure BDA0003142360170000061
方向。
{CombX}：主操作器基坐标系，坐标轴方向如图3所示。
{H}：主操作器的手柄坐标系，坐标轴方向如图3所示。
下文中，以图3所示的坐标系为例，对主从运动的控制方法100进行描述。但是，本领域技术人员可以理解，可以采用其他坐标系定义实现主从运动的控制方法100。
如图1所示，在步骤101，可以确定主操作器的当前位姿，当前位姿包括当前位置和当前姿态。在一些实施例中，主操作器的当前位姿是相对于主操作器基坐标系的位姿。例如，主操作器的位姿是主操作器的手柄或其一部分定义的坐标系相对于主操作器基坐标系(例如，主操作器所在的支架或基座定义的坐标系、或世界坐标系)的位姿。在一些实施例中，确定主操作器的当前位置包括确定主操作器的手柄相对于主操作器基坐标系的当前位置，确定主操作器的当前姿态包括确定主操作器的手柄相对于主操作器基坐标系的当前姿态。
在一些实施例中，可以基于坐标变换确定主操作器的当前位姿。例如，可以基于主操作器的手柄的坐标系与主操作器基坐标系之间的变换关系确定手柄的当前姿态。通常情况下，主操作器基坐标系可以设置在主操作器所在的支架或基座上，且在遥操作过程中，主操作器基坐标系保持不变。
在一些实施例中，可以基于主操作器传感器确定主操作器的当前位姿。在一些实施例中，接收主操作器的至少一个关节的当前关节信息，并且基于至少一个关节的当前关节信息，确定主操作器的当前位姿。例如，主操作器的当前位姿是基于主操作器传感器获得的至少一个关节的当前关节信息确定的。主操作器传感器设置在主操作器的至少一个关节位置处。例如，主操作器包括至少一个的关节，在至少一个关节处设置至少一个主操作器传感器。基于主操作传感器获取对应关节的关节信息(位置或者角度)，计算主操作器的当前位姿。例如，基于正向运动学算法计算主操作器的当前位置和当前姿态。
在一些实施例中，主操作器包括用于控制手柄的姿态的至少一个姿态关节。确定主操作器的手柄的当前姿态包括：获得至少一个姿态关节的关节信息，并且基于至少一个姿态关节的关节信息，确定主操作器的当前姿态。主操作器包括机械臂，机械臂包括位置关节以及姿态关节。姿态关节调整主操作器的姿态，通过一个或者多个姿态关节控制主操作器达到目标姿态。位置关节调整主操作器的位置，通过一个或者多个的位置关节控制主操作器达到目标位置。主操作器传感器设置在度机械臂的姿态关节以及位置关节处，用于获取姿态关节以及位置关节对应的关节信息(位置或者角度)。根据获取到的关节信息，可以确定主操作器的手柄相对于主操作器的基坐标系的当前位姿。例如，主操作器包括7个关节，其中，关节5、关节6和关节7为姿态关节，用于调整主操作器的手柄的姿态。基于姿态关节的主操作器传感器获取的关节信息(如角度)以及正向运动学算法，计算主操作器的当前姿态。关节1、关节2、关节3为位置关节，用于调整主操作器的手柄的位置。基于位置关节的主操作器传感器获取的关节信息(如位置)以及正向运动学算法，计算主操作器的当前位置。
在步骤103，可以基于主操作器的当前位姿及主操作器与从动工具的位姿关系，确定从动工具的目标位姿。例如，建立主操作器与从动工具的主从映射关系，通过遥操作主操作器以控制从动工具的位姿。位姿关系包括从动工具或者从动工具在显示器中的图像相对于参考坐标系的位姿与主操作器相对于参考坐标系的位姿之间的关系。参考坐标系包括主操作器所在空间的坐标系或者世界坐标系。在一些实施例中，参考坐标系可以以操作者的体感为基准，因此考虑从动工具在显示器中的图像相对于参考坐标系的位姿。
在一些实施例中，主操作器与从动工具之间的位姿关系可以包括主操作器的位姿变化量与从动工具的位姿变化量之间的关系，例如相等或者成比例。确定从动工具的目标位姿包括：确定主操作器的先前位姿，确定从动工具的当前位姿，并且基于主操作器的先前位姿、当前位姿以及从动工具的当前位姿，确定从动工具的目标位姿。主操作器的先前位姿以及当前位姿可以是主操作器的手柄相对于主操作器基坐标系的位姿。从动工具的当前位姿以及目标位姿可以是从动工具的末端器械相对于从动工具基坐标系的位姿。
从动工具包括柔性臂体和设置在柔性臂体末端的末端器械，从动工具的位姿包括从动工具的末端器械相对于从动工具的基坐标系的姿态。从动工具的基坐标系可以是从动工具所安装的基座(例如，手术机器人的运动臂末端)的坐标系、从动工具所穿过的鞘管的坐标系(例如，鞘管出口的坐标系)、从动工具的远端运动中心点(Remote Center ofMotion，RCM)的坐标系等。例如，从动工具的基坐标系可以设置在鞘管出口位置处，且在遥操作过程中，从动工具的基坐标系保持不变。可以对末端器械的当前姿态进行坐标系变换，得到相对于其他坐标系的姿态。
在一些实施例中，可以接收主操作器的至少一个关节的先前关节信息，并且基于至少一个关节的先前关节信息，确定主操作器的先前位姿。例如，基于主操作器传感器读取主操作器在先前时间以及当前时间的关节信息，确定主操作器的手柄的先前位姿和当前位姿。基于手柄相对于主操作器基坐标系的先前位置以及当前位置，确定主操作器的手柄的位置变化量。基于手柄相对于主操作器基坐标系的先前姿态以及当前姿态，确定主操作器的手柄的姿态变化量。
在一些实施例中，可以接收从动工具的至少一个驱动装置的当前驱动信息，其中至少一个驱动装置用于驱动从动工具的柔性臂体。基于至少一个驱动装置的当前驱动信息，确定从动工具的当前位姿。在一些实施例中，通过驱动装置传感器获得从动工具的当前驱动信息(如角度)，基于当前驱动信息确定从动工具的当前姿态。例如，可以通过正向运动学算法，计算从动工具的当前姿态。
驱动装置传感器可以设置在驱动装置上，驱动装置用于驱动从动工具的柔性臂体。驱动装置传感器用于获得驱动信息。可以基于驱动信息，确定从动工具的当前姿态。例如，驱动装置可以包括至少一个驱动电机，驱动装置传感器与驱动电机耦合以记录和输出电机数据。例如，电机数据可以包括二进制或者十六进制数字，经换算可以获得从动工具的当前姿态。驱动装置传感器可以包括电位计或者编码器。通过电位计或者编码器获取角度等信息，进而确定从动工具的当前姿态。在一些实施例中，可以采用位姿传感器获得从动工具的姿态。例如，位姿传感器也可以是光纤传感器，贯穿设置在从动工具的臂体上，用于感测从动工具的位置和姿态。
在一些实施例中，可以基于主操作器的先前位姿以及当前位姿，确定主操作器的位姿变化量。可以基于主操作器的位姿变化量及主操作器与从动工具的位姿关系，确定从动工具的位姿变化量。可以基于从动工具的当前位姿以及从动工具的位姿变化量，确定从动工具的目标位姿。
位姿关系可以包括位置关系以及姿态关系。主操作器与从动工具之间的位置关系可以包括主操作器的位置变化量与从动工具的位置变化量之间的关系，主操作器与从动工具之间的姿态关系可以包括主操作器的姿态变化量与从动工具的姿态变化量之间的关系。
在一些实施例中，方法100还包括：确定主操作器的手柄相对于主操作器基坐标系的当前位置，确定手柄相对于主操作器基坐标系的先前位置，确定从动工具的末端器械相对于从动工具基坐标系的当前位置，以及基于手柄相对于主操作器基坐标系的先前位置和当前位置、末端器械相对于从动工具基坐标系的当前位置，确定末端器械相对于从动工具基坐标系的目标位置。例如，基于主操作器传感器读取的主操作器在先前时间对应的关节信息确定主操作器的先前位置，基于主操作器传感器读取的主操作器在当前时间对应的关节信息确定主操作器的当前位置。基于手柄相对于主操作器基坐标系的先前位置以及当前位置确定主操作器的位置变化量。基于从动工具传感器读取的从动工具在当前时间的关节信息确定从动工具的当前位置。基于主操作器的位置变化量及主操作器与从动工具的位姿关系确定从动工具的位置变化量。基于从动工具的当前位置以及从动工具的位置变化量确定从动工具的目标位置。
在一些实施例中，位姿关系包括从动工具在显示器中的图像相对于参考坐标系的位姿与主操作器相对于参考坐标系的位姿之间的关系。从动工具包括手术工具和视觉工具。在手术过程中,手术工具在患者体内执行手术，视觉工具采集患者体内图像，并将采集到的图像传输至手术台车。图像经手术台车中的视频处理模块处理后，显示在主控台车的显示器上。操作者通过显示器中的图像实时获得从动工具相对于参考坐标系的位姿。主操作器相对于参考坐标系的位姿是操作者真实感知到的姿态。操作者通过遥操作主操作器所感受到的位姿变化与操作者在显示器中感知到的从动工具的姿态变化之间符合预设的位姿关系，这样，通过远程遥操作主操作器，基于预设的位姿关系将主操作器的位姿变换转换为从动工具的位姿变化，进而实现对从动工具的位姿控制。
在一些实施例中，位姿关系包括：从动工具在显示器中的图像相对于参考坐标系的位置变化量与主操作器相对于参考坐标系的位置变化量成比例。这样，当操作者握住主操作器的手柄操作时，基于直观性操作的原则，操作者感受到的从动工具的末端器械的图像的位置变化量与操作者感受到的主操作器的手柄的位置变化量保持一定的比例关系，以提高遥操作的精度。
从动工具在显示器中的图像相对于参考坐标系的位置变化量与主操作器相对于参考坐标系的位置变化量成比例，可以表示为：
WΔpITt＝k·WΔpH (1)
在公式(1)中，左侧的WΔpITt表示从动工具的末端器械在显示器中的图像相对于参考坐标系的位置变化量，右侧的WΔpH表示主操作器的手柄相对于参考坐标系的位置变化量。WΔpITt与WΔpH成比例关系，比例系数为k。
在一些实施例中，可以基于从动工具在显示器中的图像相对于参考坐标系的当前位置WpITt(t0)及目标位置WpITt，确定从动工具的位置变化量WΔpITt。可以基于主操作器相对于参考坐标系的先前位置WpH(t0)及当前位置WpH，确定主操作器的位置变化量WΔpH。例如，遥操作指令被触发时或者上一控制循环中，表示为t0时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的先前位置WpH(t0)。当前控制循环中，表示为t1时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的当前位置WpH。基于主操作器在t0时刻的先前位置WpH(t0)以及主操作器在t1时刻的当前位置WpH，得到主操作器的位置变化量WΔpH。此外，在t0时刻，可以基于从动工具的驱动信息，确定从动工具在显示器中的图像的当前位置WpITt(t0)。可以基于手柄的位置变化量WΔpH以及从动工具在显示器中的图像的当前位置WpITt(t0)，确定从动工具在显示器中的图像的目标位置WpITt。
在一些实施例中，公式(1)左侧，从动工具的末端器械在显示器中的图像相对于参考坐标系的位置变化量WΔpITt可以基于从动工具在显示器中的图像相对于显示器坐标系的位置变化量ScreenΔpITt以及显示器坐标系与世界坐标系的变换关系WRScreen确定。
具体如公式(2)，
WΔpITt＝WRScreen ScreenΔpITt (2)
在一些实施例中，显示器坐标系Screen与相机坐标系Cam对于视野方向的定义一致。因此从动工具在显示器中的图像相对于显示器坐标系的位置变化量ScreenΔpITt与从动工具相对于相机坐标系的位置变化量CamΔpTt一致。具体参见公式(3)，
ScreenΔpITt＝CamΔpTt (3)
基于公式(2)及公式(3)，得到公式(4)，
WΔpITt＝WRScreen CamΔpTt (4)
在公式(4)中，从动工具在显示器中的图像相对于参考坐标系的位置变化量WΔpITt可以基于从动工具的末端器械相对于相机坐标系的位置变化量CamΔpTt以及显示器坐标系与世界坐标系的变换关系WRScreen确定。其中，显示器坐标系与参考坐标系具有预定的变换关系WRScreen。
在公式(4)中，从动工具的末端器械相对于相机坐标系的位置变化量CamΔpTt可以基于末端器械在t0时刻相对于相机坐标系的位置CampTt(t0)以及末端器械在t时刻相对于相机坐标系的位置CamΔpTt的差值确定，如公式(5)，
CamΔpTt＝CampTt-CampTt(t0) (5)
在公式(4)以及公式(5)中，从动工具的末端器械在相机坐标系下的位置变化量CamΔpTt可以基于相机基坐标系与相机坐标系之间的变化关系CamREb、相机基坐标系与从动工具基坐标之间的变化关系EbRTb及从动工具的末端器械相对于从动工具基坐标系的位置变化量TbΔpTt确定。具体参见公式(6)，
CamΔpTt＝CamREb EbRTb TbΔpTt (6)
其中，TbΔpTt可以基于从动工具的驱动信息来确定。
在一些实施例中，公式(1)右侧，主操作器的手柄相对于参考坐标系的位置变化量WΔpH可以基于主操作器基坐标系与参考坐标系的变换关系WRCombX及手柄相对于主操作器基坐标系的位置变化量CombXΔpH确定，如公式(7)，
WΔpH＝WRCombX CombXΔpH (7)
在公式(7)中，WRCombX由主操作器的摆放位置关系所确定。在一些实施例中，参考坐标系与主操作器基坐标系具有预定的变换关系。
在公式(7)中，手柄相对于主操作器基坐标系的位置变化量CombXΔpH可以基于主操作器的手柄相对于主操作器基坐标系的当前姿态CombXpH，以及手柄相对于主操作器基坐标系的先前姿态CombXpH(t0)确定，具体参见公式(8)，
CombXΔpH＝CombXpH-CombXpH(t0) (8)
综合公式(1)至公式(8)，可得出从动工具在遥操作过程中的目标位置TbpTt表达式如公式(9)，
TbpTt＝TbpTt(t0)+k·TbRCam ScreenRCombX(CombXpH-CombXpH(t0)) (9)
基于公式(9)，在一些实施例中，可以基于手柄相对于主操作器基坐标系的先前位置CombXpH(t0)和当前位置CombXpH、末端器械相对于从动工具基坐标系的当前位置TbpTt(t0)、主操作器基坐标系与显示器坐标系的变换关系ScreenRCombX以及相机坐标系与从动工具基坐标系的变换关系TbRCam，确定末端器械相对于从动工具基坐标系的目标位置TbpTt。
在一些实施例中，相机坐标系与从动工具基坐标系的变换关系TbRCam可以基于相机坐标系与相机基坐标系的变换关系EbRCam以及相机基坐标系与从动工具基坐标系的变换关系TbREb确定。
在一些实施例中，主操作器基坐标系与显示器坐标系的变换关系ScreenRCombX可以基于主操作器基坐标系与参考坐标系的变换关系WRCombX、参考坐标系与显示器坐标系的变换关系ScreenRW确定。
在一些实施例中，从动工具在显示器中的图像相对于参考坐标系的姿态变化量与主操作器相对于参考坐标系的姿态变化量一致。这样，当操作者握住主操作器手柄操作时，基于直观性操作的原则，操作者感受到的手术工具的末端器械的图像的姿态变化量与操作者感受到的主操作器手柄的姿态变化量保持一致，以提高遥操作的精度。
从动工具在显示器中的图像相对于参考坐标系的姿态变化量WRITt(t0)ITt与主操作器相对于参考坐标系的姿态变化量WRH(t0)H一致，可以表示如下：
WRITt(t0)ITt＝WRH(t0)H (10)
在一些实施例中，可以基于主操作器相对于参考坐标系在t0时刻的先前姿态WRH(t0)及在t1时刻的当前姿态WRH确定主操作器的姿态变化量WRH(t0)H。而且，可以基于姿态变化量WRH(t0)H以及从动工具在显示器中的图像相对于参考坐标系在t0时刻的当前姿态WRITt(t0)，确定从动工具在显示器中的图像的目标姿态WRITt。
例如，遥操作指令被触发时或者上一控制循环中，表示为t0时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的先前姿态WRH(t0)。当前控制循环中，表示为t1时刻，基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的当前姿态WRH。基于主操作器在t0时刻的先前姿态WRH(t0)以及主操作器在t1时刻的当前姿态WRH，得到主操作器的姿态变化量WRH(t0)H。此外，在t0时刻，可以基于从动工具的驱动信息，确定从动工具在显示器中的图像的当前姿态WRITt(t0)。可以基于手柄的姿态变化量WRH(t0)H以及从动工具在显示器中的图像的当前姿态WRITt(t0)，确定从动工具在显示器中的图像的目标姿态WRITt。
在一些实施例中，关于公式(10)的左侧，从动工具在显示器中的图像相对于参考坐标系的姿态变化量WRITt(t0)ITt可以基于参考坐标系与显示器坐标系之间的变换关系WRScreen、显示器坐标系与相机坐标系之间的变换关系ScreenRCam、从动工具的末端器械相对于相机坐标系的先前姿态CamRTt(t0)及末端器械相对于相机坐标系的当前姿态CamRTt确定。具体参见公式(11)，
WRITt(t0)ITt＝(WRScreen ScreenRCam CamRTt)(WRScreen ScreenRCam CamRTt(t0))T (11)
在一些实施例中，关于公式(10)的右侧，主操作器相对于参考坐标系的姿态变化量WRH(t0)H可以基于参考坐标系与主操作器基坐标系之间的变换关系WRCombX、主操作器的手柄相对于主操作器基坐标系的先前姿态CombXRH(t0)及主操作器的手柄相对于主操作器基坐标系的当前姿态CombXRH确定。具体参见公式(12)，
WRH(t0)H＝WRCombX CombXRH(WRCombX CombXRH(t0))T (12)
综合公式(10)至公式(12)，可得出从动工具在遥操作过程中的目标姿态表达式如公式(13)，
TbRTt＝TbRCombX(CombXRH(CombXRH(t0))T)CombXRTb TbRTt(t0) (13)
基于公式(13)，在一些实施例中，可以基于手柄相对于主操作器基坐标系的先前姿态CombXRH(t0)和当前姿态CombXRH、从动工具的末端执行器相对于从动工具基坐标系的当前姿态TbRTt(t0)及从动工具基坐标系与主操作器基坐标系的变换关系CombXRTb，确定末端器械相对于从动工具基坐标系的目标姿态TbRTt。
在一些实施例中，从动工具基坐标系与主操作器基坐标系的变换关系CombXRTb可以是基于从动工具基坐标系与相机坐标系的变换关系CamRTb、相机坐标系与显示器坐标系的变换关系ScreenRCam、显示器坐标系与主操作器基坐标系的变换关系CombXRScreen确定。
在一些实施例中，从动工具基坐标系与相机坐标系的变换关系CamRTb可以是基于从动工具基坐标系与相机基坐标系的变换关系EbRTb和相机坐标系与相机基坐标系的变换关系CamREb确定的。显示器坐标系与主操作器基坐标系的变换关系CombXRScreen可以是基于显示器坐标系与参考坐标系的变换关系WRScreen和参考坐标系与主操作器基坐标系的变换关系CombXRW确定的。
在一些实施例中，相机可以设置在可被驱动的柔性臂体的末端，相机坐标系与相机基坐标系的变换关系是基于柔性臂体的驱动信息确定的。
在一些实施例中，从动工具基坐标系与相机基坐标系具有预定的变换关系。在一些实施例中，在进入主从运动控制阶段前，视觉工具已经运动结束，相机的当前姿态在相机基坐标系下的表示
Figure BDA0003142360170000151
不再变化。
在一些实施例中，显示器坐标系与参考坐标系具有预定的变换关系。在一些实施例中，参考坐标系与主操作器基坐标系具有预定的变换关系。在一些实施例中，显示器坐标系与主操作器基坐标系具有预定的变换关系。
在一些实施例中，从动工具相对于参考坐标系的姿态变化量与主操作器相对于参考坐标系的姿态变化量一致。在一些实施例中，从动工具相对于参考坐标系的位置变化量与主操作器相对于参考坐标系的位置变化量成比例。这样，当操作者握住主操作器的手柄运动以操作从动工具时，基于直观性操作的原则，操作者感受到的从动工具的末端器械的位置变化量与操作者感受到的主操作器的位置变化量保持一定的比例关系，以提高遥操作的精度。
从动工具相对于参考坐标系的位置变化量与主操作器相对于参考坐标系的位置变化量成比例，可以表示为：
WΔpTt＝k·WΔpH (14)
在公式(14)中，左侧的WΔpTt表示从动工具相对于参考坐标系的位置变化量，右侧的WΔpH表示主操作器相对于参考坐标系的位置变化量。并且，WΔpTt与WΔpH呈比例关系，比例系数为k。
在一些实施例中，可以基于从动工具相对于参考坐标系的先前位置WpTt(t0)及当前位置WpTt，确定从动工具的位置变化量WΔpTt。可以基于主操作器相对于参考坐标系的先前位置WpH(t0)及当前位置WpH确定主操作器的位置变化量WΔpH。例如，遥操作指令被触发时或者上一控制循环中，表示为t0时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的先前位置WpH(t0)。当前控制循环中，表示为t1时刻，基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的当前位置WpH。基于主操作器在t0时刻的先前位置WpH(t0)以及主操作器在t1时刻的当前位置WpH，得到主操作器的位置变化量WΔpH。此外，在t0时刻，可以基于从动工具的驱动信息，确定从动工具相对于参考坐标系的当前位置WpTt(t0)。可以基于手柄的位置变化量WΔpH以及从动工具相对于参考坐标系的当前位置WpTt(t0)，确定从动工具相对于参考坐标系的目标位置WpTt。
在公式(14)中，从动工具的末端器械相对于参考坐标系的位置变化量WΔpTt可以由末端器械在t0时刻相对于参考坐标系的位置WpTt以及末端器械在t时刻相对于参考坐标系的位置WpTt(t0)的差值表示，如公式(15)所示，
WΔpTt＝WpTt-WpTt(t0) (15)
在公式(14)中，主操作器相对于世界坐标系的位置变化量WΔpH可以由主操作器在t0时刻相对于参考坐标系的位置WpH(t0)以及主操作器在t时刻相对于参考坐标系的位置WpH的差值表示，如公式(16)所示，
WΔpH＝WpH-WpH(t0) (16)
在一些实施例中，在公式(14)的左右两侧分别乘以相同的矩阵TbRW，基于公式(14)至公式(16)得到公式(17)，
TbRW(WpTt-WpTt(t0))＝k·TbRW(WpH-WpH(t0)) (17)
基于公式(17)的左侧得到公式(18)，
TbRW(WpTt-WpTt(t0))＝TbpTt(t0) (18)
基于公式(17)的右侧得到公式(19)，
k·TbRW(WpH-WpH(t0))＝k·TbRCombX(CombXpH-CombXpH(t0)) (19)
基于公式(18)及公式(19)得到公式(20)，
TbpTt＝k·TbRCombX(CombXpH-CombXpH(t0))+TbpTt(t0) (20)
基于公式(20)，在一些实施例中，可以基于手柄相对于主操作器基坐标系的先前位置CombXpH(t0)和当前位置CombXpH、末端器械相对于从动工具基坐标系的当前位置TbpTt(t0)、主操作器基坐标系与从动工具基坐标系的变换关系TbRCombX，确定末端器械相对于从动工具基坐标系的目标位置TbpTt。
在一些实施例中，从动工具相对于参考坐标系的姿态变化量与主操作器相对于参考坐标系的姿态变化量一致。这样，当操作者握住主操作器的手柄运动以操作从动工具时，基于直观性操作的原则，操作者感受到的手术工具的末端器械的姿态变化量与操作者感受到的主操作器的姿态变化量保持一致，以提高遥操作的精度。
从动工具相对于参考坐标系的姿态变化量与主操作器相对于参考坐标系的姿态变化量一致，可以表示为：
Figure BDA0003142360170000181
在公式(21)中，左侧的
Figure BDA0003142360170000182
表示从动工具的姿态相对于参考坐标系的姿态变化量，右侧的WRH(t0)H表示主操作器相对于参考坐标系的姿态变化量。
在一些实施例中，可以基于从动工具相对于参考坐标系的当前姿态
Figure BDA0003142360170000183
及目标姿态
Figure BDA0003142360170000189
确定从动工具的姿态变化量
Figure BDA0003142360170000184
基于主操作器相对于参考坐标系的先前姿态WRH(t0)及当前姿态WRH确定主操作器的姿态变化量WRH(t0)H。例如，遥操作指令被触发时或者上一控制循环中，表示为t0时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的先前姿态WRH(t0)H。当前控制循环中，表示为t1时刻，基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对于参考坐标系的当前姿态WRH。可以基于主操作器在t0时刻的先前姿态WRH(t0)H以及主操作器在t1时刻的当前姿态WRH，得到主操作器的姿态变化量WΔpH。此外，在t0时刻，可以基于从动工具的驱动信息，确定从动工具相对于参考坐标系的当前姿态
Figure BDA0003142360170000185
可以基于手柄的姿态变化量WRH(t0)H以及从动工具相对于参考坐标系的当前姿态
Figure BDA0003142360170000186
确定从动工具相对于参考坐标系的目标姿态
Figure BDA0003142360170000187
在公式(21)中，从动工具的姿态相对于参考坐标系的姿态变化量
Figure BDA0003142360170000188
可以基于末端器械在t0时刻相对于参考坐标系的当前姿态
Figure BDA0003142360170000191
以及末端器械在t时刻相对于参考坐标系的目标姿态
Figure BDA0003142360170000192
确定。主操作器相对于参考坐标系的姿态变化量WRH(t0)H可以基于手柄在t0时刻相对于参考坐标系的先前姿态
Figure BDA0003142360170000193
以及手柄在t时刻相对于参考坐标系的当前姿态WRH确定。具体参见公式(22)，
Figure BDA0003142360170000194
在一些实施例中，在公式(22)的左右两侧分别乘以相同的矩阵TbRW(TbRW)T，基于公式(22)得到公式(23)，
Figure BDA0003142360170000195
基于公式(23)的左侧得到公式(24)，
Figure BDA0003142360170000196
基于公式(23)的右侧得到公式(25)，
Figure BDA0003142360170000197
综合公式(21)至公式(25)，可得出从动工具在遥操作过程中的目标姿态
Figure BDA0003142360170000198
表达式如公式(26)，
Figure BDA0003142360170000199
基于公式(26)，在一些实施例中，可以基于手柄相对于主操作器基坐标系的先前姿态CombXRH(t0)和当前姿态CombXRH、从动工具的末端执行器相对于从动工具基坐标系的当前姿态TbRTt(t0)及从动工具基坐标系与主操作器基坐标系的变换关系CombXRTb，确定末端器械相对于从动工具基坐标系的目标姿态TbRTt。
在步骤105，可以基于从动工具的目标位姿，生成从动工具的控制信号。在一些实施例中，基于从动工具的目标位姿，生成用于驱动从动工具的至少一个驱动装置的驱动信号。在一些实施例中，基于从动工具的当前位姿和目标位姿，生成用于驱动从动工具的至少一个驱动装置的驱动信号。例如，基于逆向运动学算法计算用于驱动从动工具的至少一个驱动装置的驱动信号。
在一些实施例中，主从运动的控制方法还包括：以预定周期执行主从运动的控制。例如，在主从运动控制过程中，以预定周期读取主操作器的当前位姿，根据主操作器的当前位姿及主从间的位姿关系确定从动工具的目标位姿，并控制从动工具运动至目标位姿，实现主操作器对从动工具的运动控制。
在一些实施例中，对于每一个当前周期，主操作器的先前位姿包括主操作器与从动工具初次建立主从映射关系时或者上次周期中的位姿。主操作器的当前位姿包括主操作器在当前周期的位姿(例如，在遥操作后的位姿)。从动工具的当前位姿包括当前周期的位姿(例如，当前周期未进行主从控制之前的位姿)，目标位姿包括在当前周期中主从控制的目标位姿。主操作器的当前位姿与从动工具的目标位姿匹配。
手术机器人对操作精度和人机交互体验要求很高。手术机器人通常通过视觉系统进行手术操作。视觉系统中相机的位姿通常是可以调整的，以便调整视场的角度和范围。但是，相机位姿的调整可能导致操作者(例如手术医生)的观察体验发生变化。本公开的一些实施例考虑了相机和显示器的位置和姿态，优化了操作者的使用体验，可以准确、快速地控制从动工具运动到预想位置，且可以突破主操作器的运动限度，达到更大的操作范围。因此，本公开的一些实施例可以减轻、甚至避免主操作器的位姿对从动工具的位姿的直接映射带来的视觉和实际差异以及主操作器控制从动工具的位姿限制带来的从动工具运动范围限制。
图4示出根据本公开一些实施例的机器人系统的示意图400。如图4所示，机器人系统400，包括：主操作器410、从动工具420、驱动装置430和控制装置440。主操作器410包括机械臂、设置在机械臂上的手柄以及设置在机械臂上的至少一个关节处的至少一个主操作器传感器，至少一个主操作器传感器用于获得至少一个关节的关节信息。从动工具420包括柔性臂体和末端器械。驱动装置430用于驱动从动工具的柔性臂体，至少一个驱动装置传感器，与至少一个驱动装置耦合并且用于获得驱动信息。控制装置440与主操作器410和至少一个驱动装置430通信连接。控制装置440被配置成用于执行根据本公开一些实施例的主从运动的控制方法。
在一些实施例中，主操作器410包括六自由度机械臂，六自由度机械臂上的每一个关节处设置一个主操作器传感器，通过每个关节的主操作器传感器生成关节信息(如关节角度数据)。在一些实施例中，主操作器传感器采用电位计和/或编码器。
在一些实施例中，从动工具420包括多构节六自由度柔性臂体。
在一些实施例中，驱动装置430用于驱动从动工具420的柔性臂体，并通过驱动装置传感器获得从动工具对应的驱动信息。
在一些实施例中，控制装置440与主操作器410和驱动装置430通信连接。例如，主操作器410、驱动装置430及控制装置440可以是通过数据传输总线连接，包括但不限于无线数据传输，有线数据连接，或多种数据通讯方式混合使用。数据传输总线可以采用通讯协议总线，例如控制器局域网总线(CAN，Controller Area Network)。
控制装置440被配置成用于执行本公开一些实施例中的主从运动的控制方法。例如，控制装置用于接收主操作器传感器和驱动装置传感器发送的网络数据包(如关节信息)。控制装置根据从动工具的关节信息和主操作器的关节信息计算出主操作器的手柄到达与从动工具的当前姿态一致的目标姿态的关节目标值，并转化为驱动信号发送至驱动装置430。驱动装置430通过网络数据包接收到驱动信号，通过CAN总线发送至各Epos控制工具，驱动主操作器的各个电机运动使得主操作器运动到位，实现主操作器的手柄与从动工具的姿态匹配。
在一些实施例中，主操作器中可以设置有控制器，控制器可以根据各个主操作器传感器获得到的关节信息计算主操作器的姿态数据，并将计算得到的姿态数据发送到控制装置。在另外一些实施例中，控制装置也可以根据主操作器传感器发送的关节数据计算得到主操作器的姿态数据。
1.一种主从运动的控制方法，包括：
确定主操作器的当前位姿，所述当前位姿包括当前位置和当前姿态；
基于所述主操作器的所述当前位姿及所述主操作器与从动工具的位姿关系，确定所述从动工具的目标位姿；以及
基于所述从动工具的所述目标位姿，生成所述从动工具的控制信号。
2.根据第1项所述的控制方法，所述位姿关系包括所述从动工具或者所述从动工具在显示器中的图像相对于参考坐标系的位姿与所述主操作器相对于所述参考坐标系的位姿之间的关系。
3.根据第2项所述的控制方法，所述位姿关系包括以下中的至少一项：
所述从动工具或者所述从动工具在显示器中的图像相对于所述参考坐标系的位置变化量与所述主操作器相对于所述参考坐标系的位置变化量成比例；或
所述从动工具或者所述从动工具在所述显示器中的图像相对于所述参考坐标系的姿态变化量与所述主操作器相对于所述参考坐标系的姿态变化量一致。
4.根据第2-3中任一项所述的控制方法，所述参考坐标系包括所述主操作器所在空间的坐标系或者世界坐标系。
5.根据第1-4中任一项所述的控制方法，还包括：
确定所述主操作器的先前位姿；
确定所述从动工具的当前位姿；
基于所述主操作器的先前位姿和当前位姿以及所述从动工具的当前位姿，确定所述从动工具的目标位姿。
6.根据第1-5中任一项所述的控制方法，还包括：
确定所述主操作器的手柄相对于主操作器基坐标系的当前位置；
确定所述手柄相对于主操作器基坐标系的先前位置；
确定所述从动工具的末端器械相对于从动工具基坐标系的当前位置；以及
基于所述手柄相对于所述主操作器基坐标系的先前位置和当前位置、所述末端器械相对于所述从动工具基坐标系的当前位置，确定所述末端器械相对于所述从动工具基坐标系的目标位置。
7.根据第6项所述的控制方法，确定所述末端器械相对于所述从动工具基坐标系的目标位置包括：
基于所述手柄相对于所述主操作器基坐标系的先前位置和当前位置、所述主操作器基坐标系与显示器坐标系的变换关系、相机坐标系与所述从动工具基坐标系的变换关系、以及所述末端器械相对于所述从动工具基坐标系的当前位置，确定所述末端器械相对于所述从动工具基坐标系的目标位置。
8.根据第7项所述的控制方法，所述显示器坐标系与所述相机坐标系对于视野方向的定义一致。
9.根据第6-8中任一项所述的控制方法，确定所述末端器械相对于所述从动工具基坐标系的目标位置包括：
基于所述手柄相对于所述主操作器基坐标系的先前位置和当前位置、所述主操作器基坐标系与所述从动工具基坐标系的变换关系、以及所述末端器械相对于所述从动工具基坐标系的当前位置，确定所述末端器械相对于所述从动工具基坐标系的目标位置。
10.根据第1-9中任一项所述的控制方法，还包括：
确定所述主操作器的手柄相对于主操作器基坐标系的当前姿态；
确定所述手柄相对于主操作器基坐标系的先前姿态；
确定所述从动工具的末端器械相对于从动工具基坐标系的当前姿态；以及
基于所述手柄相对于所述主操作器基坐标系的先前姿态和当前姿态、所述从动工具基坐标系与所述主操作器基坐标系的变换关系、以及所述从动工具的末端执行器相对于所述从动工具基坐标系的当前姿态，确定所述末端器械相对于所述从动工具基坐标系的目标姿态。
11.根据第10所述的控制方法，所述从动工具基坐标系与所述主操作器基坐标系的变换关系是基于所述从动工具基坐标系与相机坐标系的变换关系、所述相机坐标系与显示器坐标系的变换关系、所述显示器坐标系与所述主操作器基坐标系的变换关系确定的。
12.根据第7或第11项所述的控制方法，所述从动工具基坐标系与相机坐标系的变换关系是基于所述从动工具基坐标系与相机基坐标系的变换关系和相机坐标系与所述相机基坐标系的变换关系确定的。
13.根据第7或第11项所述的控制方法，所述显示器坐标系与所述主操作器基坐标系的变换关系是基于所述显示器坐标系与参考坐标系的变换关系和所述参考坐标系与所述主操作器基坐标系的变换关系确定的。
14.根据第11-13中任一项所述的控制方法，从动工具基坐标系与所述相机基坐标系具有预定的变换关系。
15.根据第13或14项所述的控制方法，所述显示器坐标系与所述参考坐标系具有预定的变换关系。
16.根据第13-15中任一项所述的控制方法，所述参考坐标系与所述主操作器基坐标系具有预定的变换关系。
17.根据第12-16中任一项所述的控制方法，所述相机设置在可被驱动的柔性臂体的末端，所述相机坐标系与所述相机基坐标系的变换关系是基于所述柔性臂体的驱动信息确定的。
18.根据第1-17中任一项所述的控制方法，还包括：
接收所述主操作器的至少一个关节的当前关节信息；以及
基于所述至少一个关节的关节信息，确定所述主操作器的当前位姿。
19.根据第1-18中任一项所述的控制方法，生成所述从动工具的控制信号包括：
基于所述从动工具的所述目标位姿，生成用于驱动所述从动工具的至少一个驱动装置的驱动信号。
20.根据第5-19中任一项所述的控制方法，还包括：
接收所述主操作器的至少一个关节的先前关节信息；
基于所述至少一个关节的先前关节信息，确定所述主操作器的先前位姿，所述先前位姿包括先前位置和先前姿态；
接收所述从动工具的至少一个驱动装置的当前驱动信息，其中所述至少一个驱动装置用于驱动所述从动工具的柔性臂体；以及
基于所述至少一个驱动装置的当前驱动信息，确定所述从动工具的当前位姿，所述当前位姿包括当前位置和当前姿态。
21.根据第1-20中任一项所述的控制方法，还包括：以预定周期执行主从运动的控制。
22.一种机器人系统，包括：
主操作器，包括机械臂、设置在所述机械臂上的手柄以及设置在所述机械臂上的至少一个关节处的至少一个主操作器传感器，所述至少一个主操作器传感器用于获得所述至少一个关节的关节信息；
从动工具，包括柔性臂体和末端器械；
至少一个驱动装置，用于驱动所述从动工具的柔性臂体；
至少一个驱动装置传感器，与所述至少一个驱动装置耦合并且用于获得驱动信息；以及
控制装置，与所述主操作器和所述至少一个驱动装置通信连接，所述控制装置被配置成用于执行如权利要求1-21中任一项所述的主从运动的控制方法。
23.一种计算机设备，所述计算机设备包括：
存储器，用于存储至少一条指令；以及
处理器，与所述存储器耦合并且用于执行所述至少一条指令以执行如权利要求1-21中任一项所述的主从运动的控制方法。
24.一种计算机可读存储介质，用于存储至少一条指令，所述至少一条指令由计算机执行时致使机器人系统实现如权利要求1-21中任一项所述的主从运动的控制方法。
注意，上述仅为本公开的示例性实施例及所运用技术原理。本领域技术人员会理解，本公开不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本公开的保护范围。因此，虽然通过以上实施例对本公开进行了较为详细的说明，但是本公开不仅仅限于以上实施例，在不脱离本公开构思的情况下，还可以包括更多其他等效实施例，而本公开的范围由所附的权利要求范围决定。

Claims (22)

1.一种主从运动的控制方法，包括：
确定主操作器的当前位姿，所述当前位姿包括当前位置和当前姿态；
基于所述主操作器的所述当前位姿及所述主操作器与从动工具的位姿关系，确定所述从动工具的目标位姿；以及
基于所述从动工具的所述目标位姿，生成所述从动工具的控制信号。
2.根据权利要求1所述的控制方法，其特征在于，所述位姿关系包括所述从动工具或者所述从动工具在显示器中的图像相对于参考坐标系的位姿与所述主操作器相对于所述参考坐标系的位姿之间的关系。
3.根据权利要求2所述的控制方法，其特征在于，所述位姿关系包括以下中的至少一项：
所述从动工具或者所述从动工具在显示器中的图像相对于所述参考坐标系的位置变化量与所述主操作器相对于所述参考坐标系的位置变化量成比例；或
所述从动工具或者所述从动工具在所述显示器中的图像相对于所述参考坐标系的姿态变化量与所述主操作器相对于所述参考坐标系的姿态变化量一致。
4.根据权利要求2所述的控制方法，其特征在于，所述参考坐标系包括所述主操作器所在空间的坐标系或者世界坐标系。
5.根据权利要求1所述的控制方法，其特征在于，还包括：
确定所述主操作器的先前位姿；
确定所述从动工具的当前位姿；
基于所述主操作器的先前位姿和当前位姿以及所述从动工具的当前位姿，确定所述从动工具的目标位姿。
6.根据权利要求1所述的控制方法，其特征在于，还包括：
确定所述主操作器的手柄相对于主操作器基坐标系的当前位置；
确定所述手柄相对于主操作器基坐标系的先前位置；
确定所述从动工具的末端器械相对于从动工具基坐标系的当前位置；以及
基于所述手柄相对于所述主操作器基坐标系的先前位置和当前位置、所述末端器械相对于所述从动工具基坐标系的当前位置，确定所述末端器械相对于所述从动工具基坐标系的目标位置。
7.根据权利要求6所述的控制方法，其特征在于，确定所述末端器械相对于所述从动工具基坐标系的目标位置包括：
基于所述手柄相对于所述主操作器基坐标系的先前位置和当前位置、所述主操作器基坐标系与显示器坐标系的变换关系、相机坐标系与所述从动工具基坐标系的变换关系、以及所述末端器械相对于所述从动工具基坐标系的当前位置，确定所述末端器械相对于所述从动工具基坐标系的目标位置。
8.根据权利要求7所述的控制方法，其特征在于，所述显示器坐标系与所述相机坐标系对于视野方向的定义一致。
9.根据权利要求6所述的控制方法，其特征在于，确定所述末端器械相对于所述从动工具基坐标系的目标位置包括：
基于所述手柄相对于所述主操作器基坐标系的先前位置和当前位置、所述主操作器基坐标系与所述从动工具基坐标系的变换关系、以及所述末端器械相对于所述从动工具基坐标系的当前位置，确定所述末端器械相对于所述从动工具基坐标系的目标位置。
10.根据权利要求1所述的控制方法，其特征在于，还包括：
确定所述主操作器的手柄相对于主操作器基坐标系的当前姿态；
确定所述手柄相对于主操作器基坐标系的先前姿态；
确定所述从动工具的末端器械相对于从动工具基坐标系的当前姿态；以及
基于所述手柄相对于所述主操作器基坐标系的先前姿态和当前姿态、所述从动工具基坐标系与所述主操作器基坐标系的变换关系、以及所述从动工具的末端执行器相对于所述从动工具基坐标系的当前姿态，确定所述末端器械相对于所述从动工具基坐标系的目标姿态。
11.根据权利要求10所述的控制方法，其特征在于，所述从动工具基坐标系与所述主操作器基坐标系的变换关系是基于所述从动工具基坐标系与相机坐标系的变换关系、所述相机坐标系与显示器坐标系的变换关系、所述显示器坐标系与所述主操作器基坐标系的变换关系确定的。
12.根据权利要求7或11所述的控制方法，其特征在于，所述从动工具基坐标系与相机坐标系的变换关系是基于所述从动工具基坐标系与相机基坐标系的变换关系和相机坐标系与所述相机基坐标系的变换关系确定的。
13.根据权利要求7或11所述的控制方法，其特征在于，所述显示器坐标系与所述主操作器基坐标系的变换关系是基于所述显示器坐标系与参考坐标系的变换关系和所述参考坐标系与所述主操作器基坐标系的变换关系确定的。
14.根据权利要求12所述的控制方法，其特征在于，从动工具基坐标系与所述相机基坐标系具有预定的变换关系。
15.根据权利要求12所述的控制方法，其特征在于，所述相机设置在可被驱动的柔性臂体的末端，所述相机坐标系与所述相机基坐标系的变换关系是基于所述柔性臂体的驱动信息确定的。
16.根据权利要求1所述的控制方法，其特征在于，还包括：
接收所述主操作器的至少一个关节的当前关节信息；以及
基于所述至少一个关节的关节信息，确定所述主操作器的当前位姿。
17.根据权利要求1所述的控制方法，其特征在于，生成所述从动工具的控制信号包括：
基于所述从动工具的所述目标位姿，生成用于驱动所述从动工具的至少一个驱动装置的驱动信号。
18.根据权利要求6所述的控制方法，其特征在于，还包括：
接收所述主操作器的至少一个关节的先前关节信息；
基于所述至少一个关节的先前关节信息，确定所述主操作器的先前位姿，所述先前位姿包括先前位置和先前姿态；
接收所述从动工具的至少一个驱动装置的当前驱动信息，其中所述至少一个驱动装置用于驱动所述从动工具的柔性臂体；以及
基于所述至少一个驱动装置的当前驱动信息，确定所述从动工具的当前位姿，所述当前位姿包括当前位置和当前姿态。
19.根据权利要求1所述的控制方法，其特征在于，还包括：以预定周期执行主从运动的控制。
20.一种机器人系统，包括：
主操作器，包括机械臂、设置在所述机械臂上的手柄以及设置在所述机械臂上的至少一个关节处的至少一个主操作器传感器，所述至少一个主操作器传感器用于获得所述至少一个关节的关节信息；
从动工具，包括柔性臂体和末端器械；
至少一个驱动装置，用于驱动所述从动工具的柔性臂体；
至少一个驱动装置传感器，与所述至少一个驱动装置耦合并且用于获得驱动信息；以及
控制装置，与所述主操作器和所述至少一个驱动装置通信连接，所述控制装置被配置成用于执行如权利要求1-19中任一项所述的主从运动的控制方法。
21.一种计算机设备，所述计算机设备包括：
存储器，用于存储至少一条指令；以及
处理器，与所述存储器耦合并且用于执行所述至少一条指令以执行如权利要求1-19中任一项所述的主从运动的控制方法。
22.一种计算机可读存储介质，用于存储至少一条指令，所述至少一条指令由计算机执行时致使机器人系统实现如权利要求1-19中任一项所述的主从运动的控制方法。
CN202110744664.9A 2020-07-01 2021-06-30 主从运动的控制方法、机器人系统、设备及存储介质 Pending CN113876436A (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN2020106274778 2020-07-01
CN202010627477 2020-07-01

Publications (1)

Publication Number Publication Date
CN113876436A true CN113876436A (zh) 2022-01-04

Family

ID=79010448

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110744664.9A Pending CN113876436A (zh) 2020-07-01 2021-06-30 主从运动的控制方法、机器人系统、设备及存储介质

Country Status (7)

Country Link
US (1) US20230293252A1 (zh)
EP (1) EP4176835A1 (zh)
JP (1) JP2023533922A (zh)
KR (1) KR20230003003A (zh)
CN (1) CN113876436A (zh)
CA (1) CA3173703A1 (zh)
WO (1) WO2022002155A1 (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114523471A (zh) * 2022-01-07 2022-05-24 中国人民解放军海军军医大学第一附属医院 基于关联标识的误差检测方法及机器人系统
CN114536402A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于关联标识的机器人系统故障检测处理方法及机器人系统
CN114536399A (zh) * 2022-01-07 2022-05-27 中国人民解放军海军军医大学第一附属医院 基于多个位姿标识的误差检测方法及机器人系统
CN114714349A (zh) * 2022-03-23 2022-07-08 武汉库柏特科技有限公司 一种遥操作超声扫描机器人姿态测试方法、装置及设备
CN115300110A (zh) * 2022-09-02 2022-11-08 易达医(北京)健康科技有限公司 内窥镜手术控制系统

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102079945B1 (ko) * 2012-11-22 2020-02-21 삼성전자주식회사 수술 로봇 및 수술 로봇 제어 방법
US9457469B2 (en) * 2014-08-14 2016-10-04 Siemens Industry Software Ltd. Method and apparatus for automatic and efficient location generation for cooperative motion
CN107662195A (zh) * 2017-09-22 2018-02-06 中国东方电气集团有限公司 一种具有临场感的机器手主从异构遥操作控制系统及控制方法
CN109171978B (zh) * 2018-09-18 2024-02-13 上海健康医学院 主从式胃部手术机器人系统
CN109955254B (zh) * 2019-04-30 2020-10-09 齐鲁工业大学 移动机器人控制系统及机器人末端位姿的遥操作控制方法
CN110421558B (zh) * 2019-06-21 2023-04-28 中国科学技术大学 面向配电网作业机器人的通用型遥操作系统及方法
CN110559083B (zh) * 2019-09-10 2020-08-25 深圳市精锋医疗科技有限公司 手术机器人及其末端器械的控制方法、控制装置
CN110464471B (zh) * 2019-09-10 2020-12-01 深圳市精锋医疗科技有限公司 手术机器人及其末端器械的控制方法、控制装置

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114523471A (zh) * 2022-01-07 2022-05-24 中国人民解放军海军军医大学第一附属医院 基于关联标识的误差检测方法及机器人系统
CN114536399A (zh) * 2022-01-07 2022-05-27 中国人民解放军海军军医大学第一附属医院 基于多个位姿标识的误差检测方法及机器人系统
CN114536402A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于关联标识的机器人系统故障检测处理方法及机器人系统
CN114536402B (zh) * 2022-02-16 2024-04-09 中国医学科学院北京协和医院 基于关联标识的机器人系统故障检测处理方法及机器人系统
CN114714349A (zh) * 2022-03-23 2022-07-08 武汉库柏特科技有限公司 一种遥操作超声扫描机器人姿态测试方法、装置及设备
CN114714349B (zh) * 2022-03-23 2024-04-16 武汉库柏特科技有限公司 一种遥操作超声扫描机器人姿态测试方法、装置及设备
CN115300110A (zh) * 2022-09-02 2022-11-08 易达医(北京)健康科技有限公司 内窥镜手术控制系统

Also Published As

Publication number Publication date
CA3173703A1 (en) 2022-10-06
EP4176835A1 (en) 2023-05-10
US20230293252A1 (en) 2023-09-21
JP2023533922A (ja) 2023-08-07
KR20230003003A (ko) 2023-01-05
WO2022002155A1 (zh) 2022-01-06

Similar Documents

Publication Publication Date Title
CN113876436A (zh) 主从运动的控制方法、机器人系统、设备及存储介质
JP7003985B2 (ja) 医療用支持アームシステムおよび制御装置
CN111315309B (zh) 用于控制机器人操纵器或相关工具的系统和方法
JP6026515B2 (ja) ツールの動作を制御するのに使用されるフレームの位置及び向きの推定
WO2018214840A1 (zh) 手术机器人系统及手术器械位置的显示方法
WO2022002159A1 (zh) 主从运动的控制方法、机器人系统、设备及存储介质
US8374723B2 (en) Obtaining force information in a minimally invasive surgical procedure
WO2018159328A1 (ja) 医療用アームシステム、制御装置及び制御方法
US8335590B2 (en) System and method for adjusting an image capturing device attribute using an unused degree-of-freedom of a master control device
JP5737796B2 (ja) 内視鏡操作システムおよび内視鏡操作プログラム
US20140058564A1 (en) Visual force feedback in a minimally invasive surgical procedure
WO2018159336A1 (ja) 医療用支持アームシステムおよび制御装置
KR20200047737A (ko) 입력 장치의 오퍼레이터가 볼 수 있는 디스플레이 영역으로 기구가 진입할 때 기구의 제어를 입력 장치로 전환하는 방법
KR20140139840A (ko) 디스플레이 장치 및 그 제어방법
WO2023083077A1 (zh) 保持rc点不变的方法、机械臂、设备、机器人和介质
CN113645919A (zh) 医疗臂系统、控制装置和控制方法
WO2023083078A1 (zh) 机械臂、从操作设备和手术机器人
CN113271883A (zh) 用于机器人手术系统的手眼协调系统
WO2023040817A1 (zh) 医生控制台的控制方法、医生控制台、机器人系统和介质
WO2018150489A1 (ja) 手術器具の動作方法、ロボット手術システム、及びカメラ座標とロボットに関する座標との関係の推定プログラム
CN113876433A (zh) 机器人系统以及控制方法
CN219629776U (zh) 手术机器人的显示系统及手术机器人系统
KR20230085465A (ko) 내시경 수술 로봇 시스템
US20230139425A1 (en) Systems and methods for optimizing configurations of a computer-assisted surgical system for reachability of target objects
CN118161269A (zh) 主从匹配的控制方法、机器人系统、设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 100192 Tiandi Adjacent to Feng2 Building 106, No. 1 North Yongtaizhuang Road, Haidian District, Beijing

Applicant after: Beijing Shurui Robot Co.,Ltd.

Address before: 100192 Tiandi Adjacent to Feng2 Building 106, No. 1 North Yongtaizhuang Road, Haidian District, Beijing

Applicant before: BEIJING SURGERII TECHNOLOGY Co.,Ltd.