[go: up one dir, main page]

CN110464469A - 手术机器人及末端器械的控制方法、控制装置、存储介质 - Google Patents

手术机器人及末端器械的控制方法、控制装置、存储介质 Download PDF

Info

Publication number
CN110464469A
CN110464469A CN201910854104.1A CN201910854104A CN110464469A CN 110464469 A CN110464469 A CN 110464469A CN 201910854104 A CN201910854104 A CN 201910854104A CN 110464469 A CN110464469 A CN 110464469A
Authority
CN
China
Prior art keywords
pose information
target pose
instrument
target
end instrument
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.)
Granted
Application number
CN201910854104.1A
Other languages
English (en)
Other versions
CN110464469B (zh
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.)
Shenzhen Edge Medical Co Ltd
Original Assignee
Shenzhen Edge Medical 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 Shenzhen Edge Medical Co Ltd filed Critical Shenzhen Edge Medical Co Ltd
Priority to CN201910854104.1A priority Critical patent/CN110464469B/zh
Publication of CN110464469A publication Critical patent/CN110464469A/zh
Priority to PCT/CN2020/114113 priority patent/WO2021047520A1/zh
Application granted granted Critical
Publication of CN110464469B publication Critical patent/CN110464469B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B17/00—Surgical instruments, devices or methods
    • A61B17/00234—Surgical instruments, devices or methods for minimally invasive 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/30—Surgical robots
    • A61B34/37—Leader-follower 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/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/74—Manipulators with manual electric input means
    • 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
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B17/00—Surgical instruments, devices or methods
    • A61B2017/00017—Electrical control of surgical instruments
    • 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

Landscapes

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

Abstract

本发明涉及一种手术机器人及末端器械的控制方法、控制装置、存储介质。该方法包括：结合分解获得的各组位姿信息集合计算机械臂远端的第一目标位姿信息、图像末端器械的第二目标位姿信息、各受控操作末端器械的第三目标位姿信息及各受控操作末端器械的第四目标位姿信息；在各目标位姿信息均有效时，根据第一目标位姿信息控制机械臂运动，根据第二目标位姿信息控制图像末端器械对应的操作臂运动以使图像末端器械保持于当前位姿，根据各第三目标位姿信息控制对应受控操作末端器械的操作臂运动，根据各第四目标位姿信息控制对应非受控操作末端器械的操作臂运动。本发明有利于术中在保持视野不变的前提下，扩大操作末端器械的操作空间。

Description

手术机器人及末端器械的控制方法、控制装置、存储介质
技术领域
本发明涉及医疗器械领域，特别是涉及一种手术机器人及末端器械的控制方法、控制装置、存储介质。
背景技术
微创手术是指利用腹腔镜、胸腔镜等现代医疗器械及相关设备在人体腔体内部施行手术的一种手术方式。相比传统手术方式微创手术具有创伤小、疼痛轻、恢复快等优势。
随着科技的进步，微创手术机器人技术逐渐成熟，并被广泛应用。微创手术机器人通常包括主操作台及从操作设备，主操作台包括手柄，医生通过操作手柄向从操作设备发送控制命令，从操作设备包括机械臂及安装于机械臂远端的多个操作臂，操作臂具有末端器械，在工作状态时，末端器械跟随手柄移动，以实现远程手术操作。
末端器械包括提供手术视野的图像末端器械及执行手术操作的操作末端器械，常期望手术时在固定视野下为操作末端器械提供更大的运动范围(即操作空间，亦可理解为灵活性)，但由于受限于操作臂自身运动范围较小，可以考虑结合机械臂的运动来扩大其运动范围。然而，机械臂远端位姿的改变又容易导致不期望的视野发生改变的问题，可能影响手术安全性。
发明内容
基于此，有必要提供一种可扩大操作末端器械运动范围的同时保持视野不变的手术机器人及末端器械的控制方法、控制装置、存储介质。
一方面，提供一种手术机器人中末端器械的控制方法，所述控制方法包括：获取步骤，获取各所述受控操作末端器械的初始目标位姿信息；分解步骤，分解各所述初始目标位姿信息分别获得一组位姿信息集合，每组所述位姿信息集合包括所述机械臂远端在第一坐标系的第一分量目标位姿信息及所述受控操作末端器械在第二坐标系的第二分量目标位姿信息，第一坐标系指所述机械臂的基坐标系，第二坐标系指所述机械臂的工具坐标系；第一判断步骤，对各组所述位姿信息集合的有效性进行判断；计算步骤，在至少有一组所述位姿信息集合有效、且在所述图像末端器械及各所述非受控操作末端器械均保持于当前位姿的条件下，结合各组所述位姿信息集合计算所述机械臂远端在第一坐标系的第一目标位姿信息、所述图像末端器械在第二坐标系的第二目标位姿信息、各所述受控操作末端器械分别在第二坐标系的第三目标位姿信息及各所述非受控操作末端器械分别在第二坐标系的第四目标位姿信息；第二判断步骤，对所述第一至第四目标位姿信息的有效性进行判断；控制步骤，在所述第一至第四目标位姿信息均有效时，根据所述第一目标位姿信息控制所述机械臂运动以使所述机械臂远端到达对应的目标位姿，根据所述第二目标位姿信息控制所述图像末端器械对应的操作臂运动以使所述图像末端器械保持于当前位姿，根据各所述第三目标位姿信息控制对应所述受控操作末端器械的操作臂运动以使所述受控操作末端器械到达对应的目标位姿，并根据各所述第四目标位姿信息控制对应于所述非受控操作末端器械的操作臂运动以使各所述非受控操作末端器械保持于当前位姿。
其中，所述受控操作末端器械为一个，且所述位姿信息集合有效时，所述计算步骤包括：在所述机械臂远端到达与所述第一分量目标位姿信息对应的目标位姿的条件下，换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息；将所述第一分量目标位姿信息赋值成所述第一目标位姿信息，将所述图像末端器械在第二坐标系的目标位姿信息赋值成所述第二目标位姿信息，将所述第二分量目标位姿信息赋值成所述第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息。
其中，所述受控操作末端器械为两个以上，且一个所述位姿信息集合有效、其余所述位姿信息集合无效时，所述计算步骤包括：在所述机械臂远端到达与有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算与各无效的所述位姿信息关联的所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息；将有效的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将所述图像末端器械在第二坐标系的目标位姿信息赋值成所述第二目标位姿信息，将有效的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将各所述第二期望目标位姿分别赋值成相应所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息。
其中，所述受控操作末端器械为多个，且两个以上所述位姿信息集合有效、其余所述位姿信息集合无效时，所述计算步骤包括：在所述机械臂远端到达与相应于各有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，分别换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息；对所述图像末端器械在第二坐标系的各目标位姿信息的有效性进行判断；在所述图像末端器械的各目标位姿信息中仅一个有效时，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的所述目标位姿信息集合关联的各所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息，换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算与其余有效的所述目标位姿信息集合关联的各所述受控操作末端器械的初始目标位姿信息获得其在第二坐标系的第一期望目标位姿信息；对各所述第一期望目标位姿信息的有效性进行判断；如果各所述第一期望目标位姿信息均有效，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将各所述第一期望目标位姿信息及各所述第二期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；如果各所述第一期望目标位姿信息中至少部分无效，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的各所述第一期望目标位姿信息关联的所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息；将关联于有效的所述图像末端器械的目标位姿信息所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将有效的各所述第一期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述第二期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；在所述图像末端器械的各目标位姿信息中两个以上有效时，选择将有效的所述图像末端器械的目标位姿信息中的一个作为有效、将其余作为无效，进入在所述图像末端器械的各目标位姿信息中仅一个有效时的步骤。
其中，所述受控操作末端器械为两个以上，且各所述位姿信息集合均有效时，所述计算步骤包括：在所述机械臂远端到达与相应于各有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，分别换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息；对所述图像末端器械在第二坐标系的各目标位姿信息的有效性进行判断；在所述图像末端器械的各目标位姿信息中仅一个有效时，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，换算与其余有效的所述目标位姿信息集合关联的各所述受控操作末端器械的初始目标位姿信息获得其在第二坐标系的第一期望目标位姿信息；对各所述第一期望目标位姿信息的有效性进行判断；如果各所述第一期望目标位姿信息均有效，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将各所述第一期望目标位姿信息赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；如果各所述第一期望目标位姿信息中至少部分无效，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的各所述第一期望目标位姿信息关联的所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息；将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将有效的各所述第一期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述第二期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；在所述图像末端器械的各目标位姿信息中两个以上有效时，选择将有效的所述图像末端器械的目标位姿信息中的一个作为有效、将其余作为无效，进入在所述图像末端器械的各目标位姿信息中仅一个有效时的步骤。
其中，所述分解步骤包括：获取输入的关联于所述机械臂远端的任务自由度的操作命令；结合所述任务自由度分别分解各所述初始目标位姿信息获得包括所述机械臂远端在第一坐标系的第一分量目标位姿信息及所述受控操作末端器械在第二坐标系的第二分量目标位姿信息的一组位姿信息集合。
其中，所述操作命令包括第一操作命令和第二操作命令；所述第一操作命令同所述机械臂远端的任务自由度与所述机械臂的有效自由度完全匹配的情况相关联；所述第二操作命令同所述机械臂远端的任务自由度与所述机械臂的有效自由度中的姿态自由度完全匹配的情况相关联。
其中，所述分解步骤包括：获取所述机械臂远端在第一坐标系的当前位姿信息；在所述机械臂远端保持于与其当前位姿信息对应的当前位姿的条件下，换算所述初始目标位姿信息获得所述第二分量目标位姿信息；对所述第二分量目标位姿信息的有效性进行判断；如果有效，在所述受控操作末端器械到达与所述第二分量目标位姿信息对应的目标位姿的条件下，换算所述初始目标位姿信息获得所述第一分量目标位姿信息；如果无效，调整所述第二分量目标位姿信息为有效并更新所述第二分量目标位姿信息，并在所述受控操作末端器械到达与更新后的所述第二分量目标位姿信息对应的目标位姿的条件下，换算所述初始目标位姿信息获得所述第一分量目标位姿信息。
其中，所述第一判断步骤包括：对换算所述初始目标位姿信息获得的所述第一分量目标位姿信息的有效性进行判断；如果所述第一分量目标位姿信息有效，判断所述位姿信息集合为有效；如果所述第一分量目标位姿信息无效，判断所述位姿信息集合为无效。
其中，所述对目标位姿信息的有效性进行判断的步骤包括：将所述目标位姿信息解析为相应臂体中各关节组件的目标运动状态参数，所述臂体是所述机械臂或所述操作臂；将所述臂体中各关节组件的目标运动状态参数与所述臂体中各关节组件的运动状态阈值进行对比；如果所述臂体中各关节组件的目标运动状态参数有一个以上超过相应所述关节组件的运动状态阈值，则判断所述目标位姿信息无效；如果所述臂体中各关节组件的目标运动状态参数均未超过相应所述关节组件的运动状态阈值，则判断所述目标位姿信息有效。
其中，获取由运动输入设备输入遥操作所述受控操作末端器械运动的运动信息，所述运动信息是所述运动输入设备的位姿信息；解析所述运动信息为所述受控操作末端器械的初始目标位姿信息。
又一方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被配置为由一个以上的处理器执行实现如上述任一项实施例所述的控制方法的步骤。
又一方面，提供一种手术机器人的控制装置，包括：存储器，用于存储计算机程序；及处理器，用于加载并执行所述计算机程序；其中，所述计算机程序被配置为由所述处理器加载并执行实现如上述任一项实施例所述的控制方法的步骤。
又一方面，提供一种手术机器人，包括：机械臂；装设于所述机械臂远端的一个以上具有末端器械的操作臂，所述末端器械包括一个图像末端器械和一个以上操作末端器械，所述操作末端器械均被配置为受控操作末端器械；及分别与所述机械臂和所述操作臂连接的控制装置；所述控制装置用于执行实现如上述任一项实施例所述的控制方法的步骤。
本发明具有如下有益效果：
通过控制机械臂运动使操作末端器械到达目标位姿的同时，控制图像末端器械运动以保持当前位姿，进而能够在保持视野不变的前提下，借助机械臂的运动扩大操作末端器械的操作空间，其使用方便、安全。
附图说明
图1为本发明手术机器人一实施例的结构示意图；
图2为图1所示手术机器人的局部示意图；
图3为图1所示手术机器人的局部示意图；
图4为本发明手术机器人中末端器械的控制方法一实施例的流程图；
图5为本发明手术机器人中末端器械的控制方法又一实施例的流程图；
图6为本发明手术机器人一实施例使用状态下的简易示意图；
图7为本发明手术机器人中末端器械的控制方法又一实施例的流程图；
图8为本发明手术机器人又一实施例使用状态下的简易示意图；
图9～图12为本发明手术机器人中末端器械的控制方法不同实施例的流程图；
图13为图1所示手术机器人臂体机构中机械臂的原理示意图；
图14为本发明手术机器人的控制方法中对空间运动角度的解析示意图；
图15为本发明手术机器人中末端器械的控制方法又一实施例的流程图；
图16为本发明手术机器人中末端器械的控制方法一实施例在二对一操作模式下的流程图；
图17为本发明手术机器人中末端器械的控制方法一实施例在二对一操作模式下的操作示意图；
图18～图19为本发明手术机器人中末端器械的控制方法另一实施例在二对一操作模式下的流程图；
图20为本发明手术机器人中末端器械的控制方法另一实施例的二对一操作模式下的操作示意图；
图21为本发明手术机器人中末端器械的控制方法一实施例在一对一操作模式下的流程图；
图22为本发明手术机器人中末端器械的控制方法一实施例在一对一操作模式下的操作示意图；
图23～图25为本发明手术机器人中末端器械的控制方法不同实施例的流程图；
图26为本发明手术机器人另一实施例的结构示意图。
具体实施方式
为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的较佳实施方式。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施方式。相反地，提供这些实施方式的目的是使对本发明的公开内容理解的更加透彻全面。
需要说明的是，当元件被称为“设置于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。当一个元件被认为是“耦合”另一个元件，它可以是直接耦合到另一个元件或者可能同时存在居中元件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的，并不表示是唯一的实施方式。本文所使用的术语“远端”、“近端”作为方位词，该方位词为介入医疗器械领域惯用术语，其中“远端”表示手术过程中远离操作者的一端，“近端”表示手术过程中靠近操作者的一端。
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。本文所使用的术语“各”包括一个及以上。本文所使用的术语“多个”包括两个及以上。
如图1至图3所示，其分别为本发明手术机器人一实施例的结构示意图，及其局部示意图。
手术机器人包括主操作台1及从操作设备2。主操作台1具有运动输入设备11及显示器12，医生通过操作运动输入设备11向从操作设备2发送控制命令，以令从操作设备2根据医生操作运动输入设备11的控制命令执行相应操作，并通过显示器12观察手术区域。其中，从操作设备2具有臂体，臂体具有机械臂21及可拆卸地装设于机械臂21远端的操作臂31。机械臂21包括依次连接的基座及连接组件，连接组件具有多个关节组件，在图1所示意的构型中具有关节组件210～214。操作臂31包括依次连接的连杆32、连接组件33及末端器械34，其中，连接组件33具有多个关节组件，操作臂31通过调节关节组件调节末端器械34的位姿；末端器械34具有图像末端器械34A及操作末端器械34B。更具体的，操作臂31安装于机械臂21远端的动力机构22上并受动力机构22中的驱动部进行驱动。其中，机械臂21及/或操作臂31可跟随运动输入设备11运动，且机械臂21也可被施加外力拖动而运动。
例如，运动输入设备11可以通过连线与主操作台1连接，或通过旋转的连杆与主操作台1相连接。运动输入设备11可以被配置为手持式或穿戴式(常佩戴于手腕远端如手指或手掌处)，其具有多个有效自由度。示例性的，该运动输入设备11被配置为图3所示的手柄形式。一种情况下，运动输入设备11的有效自由度的数量被配置为低于定义于臂体远端任务自由度的数量；另一种情况下，运动输入设备11的有效自由度的数量被配置为不低于臂体远端的任务自由度的数量。运动输入设备11的有效自由度的数量至多为6个，为了能够灵活地对臂体在笛卡尔空间内的运动进行控制，运动输入设备11示例性地被配置为具有6个有效自由度，其中，运动输入设备11的有效自由度指可跟随手部运动的有效自由度，令医生具有较大的操作空间，且可以通过对各有效自由度的解析产生更多有意义的数据，满足对几乎所有构型的机械臂21的控制。
该运动输入设备11跟随医生手部运动，实时采集手部运动造成的运动输入设备自身运动的运动信息。这些运动信息可以解析出位置信息、姿态信息、速度信息及加速度信息等。运动输入设备11包括但不限于磁导航定位传感器、光学定位传感器、或者连杆式主操作手等。
一实施例中，提供一种手术机器人中末端器械的控制方法。如图4所示，该控制方法包括：
步骤S1，获取步骤，即获取各受控操作末端器械的初始目标位姿信息。
安装于动力机构22上的操作末端器械34B包括被配置为受控操作末端器械(需通过运动输入设备控制的操作末端器械)和未被配置的非受控操作末端器械(不需通过运动输入设备控制的操作末端器械)。一个操作人员至多可以同时对两个受控操作末端器械34B进行控制，当受控操作末端器械34B为两个以上时，可以由两个以上的操作人员进行协同控制。
步骤S2，分解步骤，即分解各初始目标位姿信息分别获得一组位姿信息集合。
其中，每组位姿信息集合均包括机械臂远端在第一坐标系的第一分量目标位姿信息及受控操作末端器械在第二坐标系的第二分量目标位姿信息。第一坐标系指机械臂的基坐标系，第二坐标系指机械臂的工具坐标系。
步骤S3，第一判断步骤，即对各组位姿信息集合的有效性进行判断。
具体对每组位姿信息集合中包括的两个分量位姿信息的有效性进行判断，该两个分量位姿信息均有效时，该组位姿信息集合才被认定为有效，否则，该组位姿信息集合被认定为无效。
步骤S4，计算步骤，即在至少有一组位姿信息集合有效、且在图像末端器械及各非受控操作末端器械保持于当前位姿的条件下，结合各组位姿信息集合计算机械臂远端在第一坐标系的第一目标位姿信息、图像末端器械在第二坐标系的第二目标位姿信息、各受控操作末端器械分别在第二坐标系的第三目标位姿信息及各非受控操作末端器械分别在第二坐标系的第四目标位姿信息。
在该步骤中，期望非受控操作末端器械34B能够保持于当前位姿；并优先期望各受控操作末端器械34B能够到达第一期望位姿，如果若干受控操作末端器械34B不能够到达第一期望位姿，则期望这些不能够到达第一期望位姿的受控操作末端器械34B能够到达第二期望位姿。该第一期望位姿指对应于初始目标位姿信息的目标位姿(包括初始目标位姿(其与受控运动输入设备输入的运动信息相关联)与当前位姿一致或不一致的两种情况)。该第二期望位姿指当前位姿，当若干受控操作末端器械34B不能到达第一期望位姿的情况下，旨在确保它/它们能够到达第二期望位姿，以保证手术安全。
步骤S5，第二判断步骤，即对第一目标位姿信息、第二目标位姿信息、各第三目标位姿信息及各第四目标位姿信息的有效性进行判断。
步骤S6，控制步骤，即在第一目标位姿信息、第二目标位姿信息、各第三目标位姿信息及各第四目标位姿信息均有效时，根据第一目标位姿信息控制机械臂运动以使机械臂远端到达对应的目标位姿，根据第二目标位姿信息控制图像末端器械对应的操作臂运动以使图像末端器械保持于当前位姿，根据各第三目标位姿信息控制对应于受控操作末端器械的操作臂运动以使受控操作末端器械到达对应的目标位姿，并根据各第四目标位姿信息控制对应于非受控操作末端器械的操作臂运动以使各非受控操作末端器械保持于当前位姿。
通过上述步骤S1～步骤S6，在获取的第一目标位姿信息、第二目标位姿信息、各第三目标位姿信息及各第四目标位姿信息均有效时，可以在保持图像末端器械34A及非受控操作末端器械34B于当前位姿以提供稳定视野的同时，使受控操作末端器械34B到达第一期望位姿或第二期望位姿；此外，可以在某些场景下结合机械臂及对应的操作臂一起运动，扩展受控操作末端器械34B的运动范围，以利于更容易地施展手术。
一实施例中，具体在上述步骤S3中，若经判断获得的各组位姿信息集合均无效时，表示各受控操作末端器械34B(包括一个及以上的受控操作末端器械)都不具备可调性，因而不再进入后续步骤，即结束控制，重新回到步骤S1。
一实施例中，结合图5和图6参阅，在受控操作末端器械被配置为一个时，且该位姿信息集合有效时，上述步骤S4，即计算步骤包括：
步骤S411，在机械臂远端到达与第一分量目标位姿信息对应的目标位姿的条件下，换算图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算各非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息。
各末端器械34包括图像末端器械34A和受控操作末端器械34B的当前位姿信息可以是第一坐标系或第二坐标系，还可以是其它参考坐标系，这些坐标系之间实质是可以相互转换的。本文中示例性的，“当前位姿信息”指在第二坐标系的当前位姿信息，当然也可以是其它坐标系的当前位姿信息。
步骤S412，将第一分量目标位姿信息赋值成第一目标位姿信息，将换算获得的图像末端器械在第二坐标系的目标位姿信息赋值成第二目标位姿信息，将第二分量目标位姿信息赋值成第三目标位姿信息，将各非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应非受控操作末端器械的第四目标位姿信息。
上述步骤S5即第二判断步骤中，由于该位姿信息集合已经确定是有效的，因而第一目标位姿信息和第三目标位姿信息是有效的。因此实际只需判断第二目标位姿信息及各第四目标位姿信息是否有效即可。
若第一至第四目标位姿信息并非均有效，结束控制。
若第一至第四目标位姿信息均有效，进入步骤S6即控制步骤。如图6所示，操作末端器械34B中包括受控操作末端器械34B1和非受控操作末端器械34B2。根据第一目标位姿信息控制机械臂21运动以使其远端的动力机构22到达对应的目标位姿，根据第二目标位姿信息控制操作臂31A运动以使图像末端器械34A保持于当前位姿，根据第三目标位姿信息控制操作臂31B运动以使受控操作末端器械34B1到达对应的目标位姿(第一期望位姿)，根据第四目标位姿信息控制操作臂31C运动以使非受控操作末端器械34B2保持于当前位姿。
一实施例中，结合图7和图8参阅，在受控操作末端器械被配置为多个，且在上述步骤S3经判断获得仅一个位姿信息集合有效、而其余位姿信息集合无效时，上述步骤S4，即计算步骤包括：
步骤S421，在机械臂远端到达与有效的位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，换算各非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算与各无效的位姿信息关联的受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息。
步骤S422，将有效的位姿信息集合中的第一分量目标位姿信息赋值成第一目标位姿信息，将图像末端器械在第二坐标系的目标位姿信息赋值成第二目标位姿信息，将有效的位姿信息集合中的第二分量目标位姿信息赋值成关联的受控操作末端器械的第三目标位姿信息，将各第二期望目标位姿分别赋值成相应受控操作末端器械的第三目标位姿信息，将各非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应非受控操作末端器械的第四目标位姿信息。
上述步骤S5即第二判断步骤中，实际只需要判断图像末端器械34A的第二目标位姿信息及与各无效的位姿信息集合关联的受控操作末端器械34B的第三目标位姿信息是否有效即可。
若第一目标位姿信息、第二目标位姿信息、各第三目标位姿信息及各第四目标位姿信息并非均有效，结束控制。
若第一目标位姿信息、第二目标位姿信息、各第三目标位姿信息及各第四目标位姿信息均有效，进入步骤S6即控制步骤。如图8所示，操作末端器械34B中包括受控操作末端器械34B1～34B3和非受控操作末端器械34B4。若受控操作末端器械34B1关联的一组位姿信息集合有效，而受控操作末端器械34B2～34B3关联的两组位姿信息集合无效：
根据第一目标位姿信息控制机械臂21运动以使其远端的动力机构22到达对应的目标位姿，根据第二目标位姿信息控制操作臂31A运动以使图像末端器械34A保持当前位姿，根据受控操作末端器械34B1的第三目标位姿信息控制操作臂31B运动以使受控操作末端器械34B到达对应的目标位姿(第一期望位姿)，并根据受控操作末端器械34B2～34B3各自的第三目标位姿信息控制操作臂31C～31D运动以使受控操作末端器械34B2～34B3到达对应的目标位姿(第二期望位姿，即保持当前位姿)，并根据第四目标位姿信息控制操作臂31E运动以使非受控操作末端器械34B4保持于当前位姿。
含有步骤S421～步骤S422的步骤S1～S6除适用于图8所示同一机械臂21具有三个受控操作末端器械34B之外，根据其原理同样适用于同一机械臂21具有两个、及四个以上的受控操作末端器械34B的情况。
一实施例中，结合图8和图9参阅，受控操作末端器械为多个，且两个以上位姿信息集合有效、其余位姿信息集合无效时，上述步骤S4，即计算步骤包括：
步骤S431，在机械臂远端到达与相应于各有效的位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，分别换算图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息。
步骤S432，对图像末端器械在第二坐标系的各目标位姿信息的有效性进行判断。
该步骤中，如果图像末端器械的各目标位姿信息中仅一个有效时，进入步骤S433；如果图像末端器械的各目标位姿信息中两个以上有效时，进入步骤S438。
步骤S433，在机械臂远端到达与有效的图像末端器械的目标位姿信息关联的、有效的位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的目标位姿信息集合关联的各受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息，换算各非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算与其余有效的目标位姿信息集合关联的各受控操作末端器械的初始目标位姿信息获得其在第二坐标系的第一期望目标位姿信息。
步骤S434，对各第一期望目标位姿信息的有效性进行判断。
该步骤中，如果各第一期望目标位姿信息均有效，进入步骤S435；如果各第一期望目标位姿信息中至少部分无效，进入步骤S436。
步骤S435，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第一分量目标位姿信息赋值成第一目标位姿信息，将有效的图像末端器械的目标位姿信息赋值成第二目标位姿信息，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第二分量目标位姿信息赋值成关联的受控操作末端器械的第三目标位姿信息，将各第一期望目标位姿信息及各第二期望目标位姿信息分别赋值成对应的受控操作末端器械的第三目标位姿信息，将各非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应非受控操作末端器械的第四目标位姿信息。
其中，将有效的第一期望目标位姿信息对应赋值成关联的受控操作末端器械的第三目标位姿信息；将第二期望目标位姿信息对应赋值成关联于无效的目标位姿信息集合的受控操作末端器械的第三目标位姿信息。
步骤S436，在机械臂远端到达与有效的图像末端器械的目标位姿信息关联的、有效的位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的各第一期望目标位姿信息关联的受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息。
步骤S437，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第一分量目标位姿信息赋值成第一目标位姿信息，将有效的图像末端器械的目标位姿信息赋值成第二目标位姿信息，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第二分量目标位姿信息赋值成关联的受控操作末端器械的第三目标位姿信息，将有效的各第一期望目标位姿信息分别赋值成对应的受控操作末端器械的第三目标位姿信息，将不同阶段(即条件)获得的各第二期望目标位姿信息分别赋值成对应的受控操作末端器械的第三目标位姿信息，将各非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应非受控操作末端器械的第四目标位姿信息。
其中，将有效的第一期望目标位姿信息对应赋值成关联的受控操作末端器械的第三目标位姿信息；各第二期望目标位姿信息关联于两种情况，一种情况关联于无效的目标位姿信息集合，另一种情况关联于无效的第一期望目标位姿信息(转换第一期望位姿为第二期望位姿)，因此需要将其对应赋值成关联于相应情况的受控操作末端器械。步骤S438，选择将有效的图像末端器械的目标位姿信息中的一个作为有效、将其余作为无效，并进入在图像末端器械的各目标位姿信息中仅一个有效时的步骤S433。
该步骤S438中，可以配置形成多种将多个有效的图像末端器械的目标位姿信息视为有效及视为无效的组合，并分别利用上述步骤S433～步骤S437进行计算，并将计算出的第一目标位姿信息、第二目标位姿信息及各第三目标位姿信息均有效的一组来进行步骤S6的控制步骤。
在一些实施例中，如果不同组合中对应计算出的第一目标位姿信息、第二目标位姿信息、各第三目标位姿信息及各第四目标位姿信息有两组以上均有效，那么可以根据一些衡量指标来确定选择第一目标位姿信息、第二目标位姿信息、各第三目标位姿信息及各第四目标位姿信息均有效的某一组来进行步骤S6的控制步骤。这些衡量指标包括但不限于臂体(机械臂或操作臂)的运动范围、运动速度及运动加速度中的一种以上，可以采用如臂体运动范围更小、运动速度更小及运动加速度更小中的一种以上来选择出用于步骤S6的第一目标位姿信息、第二目标位姿信息及各第三目标位姿信息。这些衡量指标包括但不限于能实现第一预期位姿、第二预期位姿的受控操作末端器械的数量。优先选择能够到达第一预期位姿的数量更多的来进行控制。这些衡量指标也可以相互结合以选取出最优的一组来进行步骤S6的控制。
在一些实施例中，可以预先或者在操作受控操作末端器械的过程中实时(如通过语音指令)为各受控操作末端器械设置优先级，在步骤S438中依次选择优先级更高的受控操作末端器械所关联的有效的图像末端器械的目标位姿信息视为有效、将其余视为无效，以尽量保障较高优先级的控制对象能够实现第一期望位姿。对于优先级的设置，可以根据操作人员的权限来设置，也可以根据具体的控制对象来设置，可以进行灵活配置。
如图8所示，操作末端器械34B中包括受控操作末端器械34B1～34B3和非受控操作末端器械34B4。假设受控操作末端器械34B1关联的一组位姿信息集合无效、受控操作末端器械34B2～34B3关联的两组位姿信息集合均有效，根据上述步骤S431算出的关联于受控操作末端器械34B2、34B3的图像末端器械在第二坐标系的目标位姿信息分别为C2、C3。
情况(1.1)：假设经步骤S432判断出C2、C3均无效，结束控制。
情况(1.2)：假设经步骤S432判断出C2有效、C3无效。
基于受控操作末端器械34B2关联的一组位姿信息集合的第一分量位姿信息，根据步骤S433算出受控操作末端器械34B1在第二坐标系的目标位姿信息(第二期望目标位姿信息)、受控操作末端器械34B3在第二坐标系的目标位姿信息(第一期望目标位姿信息)、及非受控操作末端器械34B4在第二坐标系的目标位姿信息。
假设根据步骤S434判断出受控操作末端器械34B3在第二坐标系的第一期望目标位姿信息有效：
通过步骤S435进行赋值后，进入上述步骤S5，并在第一至第四目标位姿信息均有效时，进入步骤S6。在步骤S6中，根据第一目标位姿信息控制机械臂21运动以使其远端的动力机构22到达对应的目标位姿，根据第二目标位姿信息控制操作臂31A运动以使图像末端器械34A保持当前位姿，根据受控操作末端器械34B1的第三目标位姿信息控制操作臂31B运动以使受控操作末端器械34B到达对应的目标位姿(第二期望位姿，即保持当前位姿)，根据受控操作末端器械34B2～34B3各自的第三目标位姿信息控制操作臂31C～31D运动以使受控操作末端器械34B2～34B3到达对应的目标位姿(第一期望位姿)，并根据第四目标位姿信息控制操作臂31E运动以使非受控操作末端器械34B4保持于当前位姿。
假设根据步骤S434判断出受控操作末端器械34B3在第二坐标系的第一期望目标位姿信息无效：
基于受控操作末端器械34B2关联的一组位姿信息集合的第一分量位姿信息，根据步骤S436计算出受控操作末端器械34B3在第二坐标系的第二期望目标位姿信息，并通过步骤S437进行赋值后，进入上述步骤S5，并在第一至第四目标位姿信息均有效时，进入步骤S6。在步骤S6中，根据第一目标位姿信息控制机械臂21运动以使其远端的动力机构22到达对应的目标位姿，根据第二目标位姿信息控制操作臂31A运动以使图像末端器械34A保持当前位姿，根据受控操作末端器械34B1、34B3各自的第三目标位姿信息控制操作臂31B、34D运动以使受控操作末端器械34B1、34B3到达对应的目标位姿(第二期望位姿，即保持当前位姿)，并根据受控操作末端器械34B2的第三目标位姿信息控制操作臂31C运动以使受控操作末端器械34B2到达对应的目标位姿(第一期望位姿)，并根据第四目标位姿信息控制操作臂31E运动以使非受控操作末端器械34B4保持于当前位姿。
情况(1.3)：假设经步骤S432判断出C2、C3均有效。
选择将C2视为有效、C3视为无效及/或将C2视为无效、C3视为有效，进入上述情况(1.2)。
含有步骤S431～步骤S438的步骤S1～S6除适用于图8所示同一机械臂21具有三个受控操作末端器械34B之外，根据其原理同样适用于同一机械臂21具有四个以上的受控操作末端器械34B的情况。
一实施例中，结合图8和图10参阅，受控操作末端器械为两个以上(包括两个及以上)，且各位姿信息集合均有效时，上述步骤S4，即计算步骤包括：
步骤S441，在机械臂远端到达与相应于各有效的位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，分别换算图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息。
步骤S442，对图像末端器械在第二坐标系的各目标位姿信息的有效性进行判断。
该步骤中，如果图像末端器械的各目标位姿信息中仅一个有效，进入步骤S443；如果图像末端器械的各目标位姿信息中两个以上有效，进入步骤S448。
步骤S443，在机械臂远端到达与有效的图像末端器械的目标位姿信息关联的、有效的位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算各非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，换算与其余有效的目标位姿信息集合关联的各受控操作末端器械的初始目标位姿信息获得其在第二坐标系的第一期望目标位姿信息。
步骤S444，对各第一期望目标位姿信息的有效性进行判断。
该步骤中，如果各第一期望目标位姿信息均有效，进入步骤S445；如果各第一期望目标位姿信息中至少部分无效，进入步骤S446。
步骤S445，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第一分量目标位姿信息赋值成第一目标位姿信息，将有效的图像末端器械的目标位姿信息赋值成第二目标位姿信息，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第二分量目标位姿信息赋值成关联的受控操作末端器械的第三目标位姿信息，将各第一期望目标位姿信息赋值成对应的受控操作末端器械的第三目标位姿信息，将各非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应非受控操作末端器械的第四目标位姿信息。
步骤S446，在机械臂远端到达与有效的图像末端器械的目标位姿信息关联的、有效的位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的各第一期望目标位姿信息关联的受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息。
步骤S447，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第一分量目标位姿信息赋值成第一目标位姿信息，将有效的图像末端器械的目标位姿信息赋值成第二目标位姿信息，将关联于有效的图像末端器械的目标位姿信息的位姿信息集合中的第二分量目标位姿信息赋值成关联的受控操作末端器械的第三目标位姿信息，将有效的各第一期望目标位姿信息分别赋值成对应的受控操作末端器械的第三目标位姿信息，将各第二期望目标位姿信息分别赋值成对应的受控操作末端器械的第三目标位姿信息，将各非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应非受控操作末端器械的第四目标位姿信息。
这里的第二期望目标位姿信息仅关联于无效的第一期望目标位姿信息(转换第一期望位姿为第二期望位姿)这一种情况。
步骤S448，选择将有效的图像末端器械的目标位姿信息中的一个作为有效、将其余作为无效，并进入在图像末端器械的各目标位姿信息中仅一个有效时的步骤S443。
如图8所示，操作末端器械34B中包括受控操作末端器械34B1～34B3和非受控操作末端器械34B4。假设受控操作末端器械34B1～34B3关联的三组位姿信息集合均有效，根据上述步骤S441算出的关联于受控操作末端器械34B1、34B2、34B3的图像末端器械在第二坐标系的目标位姿信息分别为C1、C2、C3。
情况(2.1)：假设经步骤S442判断出C1～C3均无效，结束控制。
情况(2.2)：假设经步骤S442判断出仅C1有效、C2和C3无效。
基于受控操作末端器械34B1关联的一组位姿信息集合的第一分量位姿信息，根据步骤S433算出受控操作末端器械34B2、34B3在第二坐标系的目标位姿信息(第一期望目标位姿信息)，并根据步骤S433算出非受控操作末端器械34B4在第二坐标系的目标位姿信息(同理于第二期望目标位姿)。
假设根据步骤S444判断出受控操作末端器械34B2、34B3在第二坐标系的第一期望目标位姿信息均有效：
通过步骤S445进行赋值后，进入上述步骤S5，并在第一至第四目标位姿信息均有效时，进入步骤S6。在步骤S6中，根据第一目标位姿信息控制机械臂21运动以使其远端的动力机构22到达对应的目标位姿，根据第二目标位姿信息控制操作臂31A运动以使图像末端器械34A保持当前位姿，分别根据受控操作末端器械34B1～34B3的第三目标位姿信息对应控制操作臂31B～31D运动到达对应的目标位姿(第一期望位姿)，并根据第四目标位姿信息控制操作臂31E运动以使非受控操作末端器械34B4保持于当前位姿。
假设根据步骤S444判断出受控操作末端器械34B2、34B3在第二坐标系的第一期望目标位姿信息至少部分无效，如受控操作末端器械34B2在第二坐标系的第一期望目标位姿信息有效、受控操作末端器械34B3在第二坐标系的第一期望目标位姿信息无效：
基于受控操作末端器械34B1关联的一组位姿信息集合的第一分量位姿信息，根据步骤S446计算出受控操作末端器械34B3在第二坐标系的第二期望目标位姿信息，并通过步骤S447进行赋值后，进入上述步骤S5，并在第一目标位姿信息、第二目标位姿信息及各第三目标位姿信息均有效时，进入步骤S6。在步骤S6中，根据第一目标位姿信息控制机械臂21运动以使其远端的动力机构22到达对应的目标位姿，根据第二目标位姿信息控制操作臂31A运动以使图像末端器械34A保持当前位姿，根据受控操作末端器械34B1～34B2的第三目标位姿信息对应控制操作臂31B～31C运动到达对应的目标位姿(第一期望位姿)，并根据受控操作末端器械34B2的第三目标位姿信息对应控制操作臂31D运动到达对应的目标位姿(第二期望位姿)，并根据第四目标位姿信息控制操作臂31E运动以使非受控操作末端器械34B4保持于当前位姿。
假设根据步骤S444判断出受控操作末端器械34B2、34B3在第二坐标系的第一期望目标位姿信息均无效：
基于受控操作末端器械34B1关联的一组位姿信息集合的第一分量位姿信息，根据步骤S446计算出受控操作末端器械34B2、34B3在第二坐标系的第二期望目标位姿信息，并通过步骤S447进行赋值后，进入上述步骤S5，并在第一目标位姿信息、第二目标位姿信息及各第三目标位姿信息均有效时，进入步骤S6。在步骤S6中，根据第一目标位姿信息控制机械臂21运动以使其远端的动力机构22到达对应的目标位姿，根据第二目标位姿信息控制操作臂31A运动以使图像末端器械34A保持当前位姿，根据受控操作末端器械34B1的第三目标位姿信息对应控制操作臂31B运动到达对应的目标位姿(第一期望位姿)，并根据受控操作末端器械34B2、34B3的第三目标位姿信息对应控制操作臂31C、31D运动到达对应的目标位姿(第二期望位姿)，并根据第四目标位姿信息控制操作臂31E运动以使非受控操作末端器械34B4保持于当前位姿。
含有步骤S441～步骤S448的步骤S1～S6除适用于图8所示同一机械臂21具有三个受控操作末端器械34B之外，根据其原理同样适用于同一机械臂21具有两个、或四个以上的受控操作末端器械34B的情况。
上述实施例中，不对非受控操作末端器械的数量进行限制。
通过上述实施例，通过结合机械臂21和操作臂31的运动，能够在图像末端器械34A及非受控操作末端器械保持于当前位姿的条件下，尽量确保受控操作末端器械34B能够到达第一期望位姿实现手术目的，如果不能到达第一期望位姿也会尽量确保受控操作末端器械34B保持于当前位姿降低手术风险。
一实施例中，如图11所示，上述步骤S1，即获取步骤包括：
步骤S11，获取由运动输入设备输入遥操作受控对象运动的运动信息。
该受控对象在本文中具体指受控操作末端器械。
步骤S12，解析运动信息为受控对象的初始目标位姿信息。
通常，该运动信息可以是运动输入设备的位姿信息。
一实施例中，如图12所示，该步骤S12包括：
步骤S121，将运动信息解析映射为受控对象的位姿增量信息。
其中，“映射”为一种转换关系，它可以包括自然映射关系和非自然映射关系。
自然映射关系为一一对应的关系，指受控运动输入设备与受控对象之间的水平移动增量信息到水平移动增量信息、竖直移动增量信息到竖直移动增量信息、前后移动增量信息到前后移动增量信息、偏航角转动增量信息到偏航角转动增量信息、俯仰角转动增量信息到俯仰角转动增量信息及滚动角转动增量信息到滚动角转动增量信息。
非自然映射关系为自然映射关系之外的映射关系。一示例中，该非自然映射关系包括但不限于转换映射关系，其包括但不限于前述的将固定坐标系的水平移动增量信息、竖直移动增量信息及转动增量信息一对一映射为受控对象的偏航角增量信息、俯仰角增量信息及滚动角增量信息。配置成非自然映射关系能够在如二对一操作模式等某些情况下较容易地对受控对象进行控制。
步骤S122，获取受控对象中各关节组件的位置信息。
具体可通过装设于受控对象中各关节组件处的位置传感器如编码器获取相应的位置信息。在以图1及图13示意的实施例中，该机械臂21具有5个自由度，借助各位置传感器能够采集到这样一组位置信息(d1,θ2,θ3,θ4,θ5)。
步骤S123，根据各关节组件的位置信息计算受控对象在第一坐标系的当前位姿信息。
其中，通常可以结合正运动学来进行计算。建立机械臂21的不动点(即C点处，机械臂21的工具坐标系的原点在不动点上)至机械臂21的基座的运动学模型，输出C点与基座的模型转换矩阵计算方法为
步骤S124，结合增量位姿信息及当前位姿信息计算受控对象在第一坐标系的初始目标位姿信息。
其中，根据C点与基座的模型转换矩阵获取C点在固定坐标系的位姿信息。假设在不改变C点位置的情况下，旋转C点的坐标系，使其达到模型转换矩阵所描述的姿态，可得到旋转轴角度[θx0,θy0,θz0]，如图14所示。θx0为滚动角，θy0为偏航角，θz0为俯仰角，事实上在图13所示的机械臂21中，缺乏滚动角的自由度进而实际上θx0不可调。该固定坐标系举例可以定义在显示器处，当然，也可以定义在至少在工作时不可动的部位。
进一步地，具体在上述步骤S6即控制步骤中，对于控制对象即机械臂、图像末端器械及操作末端器械的控制可以包括如下步骤：
根据控制对象远端的目标位姿信息计算其相应各关节组件的目标位置信息。如可通过逆运动学来计算。
根据各关节组件的目标位置信息控制控制对象中各关节组件联动到达对应的目标位姿。
一实施例中，如图15所示，该步骤S12包括：
步骤S125，获取针对受控对象输入的关联于操作模式类型的选择指令。
操作模式包括二对一操作模式和一对一操作模式，二对一操作模式指用两个受控运动输入设备来控制一个受控对象，一对一操作模式指用一个受控运动输入设备来控制一个受控对象。在控制一个受控对象运动时，可以选择采用一对一操作模式或二对一操作模式。对于一对一操作模式而言，可以进一步选择采用哪一个运动输入设备作为受控运动输入设备来进行控制。举例而言，同一操作人员在双手动作时，根据配置，其可以是对一个受控对象进行的二对一操作模式的控制，也可以是对两个受控对象分别进行的一对一操作模式的控制。当手术机器人提供足够多的运动输入设备时，这对于两个以上的操作人员仍然是适用的。
步骤S126，结合操作模式的类型获取受控运动输入设备输入的运动信息并将运动信息解析映射为受控对象远端在第一坐标系的增量位姿信息。
一实施例中，对于一对一操作模式，举例可以通过公式Pn＝KPn获得相应一个受控运动输入设备11在第n时刻的位姿信息P，其中，K是比例系数，通常，K>0，更佳的，1≥K>0，以实现对位姿的缩放，便于控制。
一实施例中，对于二对一操作模式，举例可以通过公式Pn＝K1PnL+K2PnR获得相应两个受控运动输入设备11在第n时刻的位姿信息P，其中，K1和K2分别表示不同运动输入设备11的比例系数，通常，K1>0，K2>0；更佳的，1≥K1>0，1≥K2>0。
计算某前后时刻一对一操作模式或二对一操作模式对应的受控运动输入设备11的增量位姿信息Δpn_n-1时，根据如下公式计算即可：
Δpn_n-1＝Pn-Pn-1
一实施例中，如图16和图17所示，在步骤S125获取到的选择指令关联于二对一操作模式时，步骤S126包括：
步骤S1261，分别获取前一时刻两个受控运动输入设备各自的第一位姿信息。
步骤S1262，分别获取后一时刻两个受控运动输入设备各自的第二位姿信息。
步骤S1263，结合第一比例系数及两个受控运动输入设备各自的第一位姿信息、第二位姿信息计算获取两个受控运动输入设备在固定坐标系的增量位姿信息。
在该步骤S1263中，具体可通过如下步骤实现：
计算一受控运动输入设备的第一位姿信息和第二位姿信息在固定坐标系的增量位姿信息，并计算另一运动输入设备的第一位姿信息和第二位姿信息在固定坐标系的增量位姿信息。
结合第一比例系数计算一运动输入设备在固定坐标系的增量位姿信息和另一运动输入设备在固定坐标系的增量位姿信息分别得到该两个运动输入设备在固定坐标系的增量位姿信息。
其中，在二对一操作模式中，示例性的，该第一比例系数取值为0.5，即将K1和K2均取值为0.5，则获取的增量位姿信息表示的是该两个受控运动输入设备之间连线的中心点的增量位姿信息。根据实际情况，也可以对K1和K2进行另外的赋值。K1和K2可以相同或不同。
步骤S1264，将两个受控运动输入设备在固定坐标系的增量位姿信息映射为受控对象远端在第一坐标系的增量位姿信息。
一实施例中，如图18所示，在步骤S125获取到的选择指令关联于二对一操作模式时，步骤S126也可以包括：
步骤S1265，分别获取前一时刻两个受控运动输入设备各自在固定坐标系的第一位置信息。
步骤S1266，分别获取后一时刻两个受控运动输入设备各自在固定坐标系的第二位置信息。
步骤S1267，结合第二比例系数及两个受控运动输入设备各自在固定坐标系的第一位置信息、第二位置信息计算获取两个受控运动输入设备在固定坐标系的水平移动增量信息、竖直移动增量信息及转动增量信息。
步骤S1268，将两个受控运动输入设备在固定坐标系的水平移动增量信息、竖直移动增量信息及转动增量信息对应映射为受控对象远端在第一坐标系的偏航角增量信息、俯仰角增量信息及滚动角增量信息。
更进一步地，如图19和图20所示，上述步骤S1268在根据两个受控运动输入设备各自在固定坐标系的第一位置信息、第二位置信息计算获取两个受控运动输入设备在固定坐标系的转动增量信息的步骤之中，包括：
步骤S12681，建立前一时刻两个受控运动输入设备之间的第一位置向量。
步骤S12682，建立后一时刻两个受控运动输入设备之间的第二位置向量。
步骤S12683，结合第三比例系数及第一位置向量与第二位置向量之间的夹角获取两个受控运动输入设备在固定坐标系的转动增量信息。
一实施例中，如图21和图22所示，在上述步骤S125获取到的选择指令关联于一对一操作模式时，上述步骤S126可以包括：
步骤S12611，获取前一时刻受控运动输入设备在固定坐标系的第一位姿信息。
步骤S12612，获取后一时刻受控运动输入设备在固定坐标系的第二位姿信息。
步骤S12613，结合第四比例系数及受控运动输入设备在固定坐标系的第一位姿信息、第二位姿信息计算获取受控运动输入设备在固定坐标系的增量位姿信息。
步骤S12614，将受控运动输入设备在固定坐标系的增量位姿信息映射为受控对象远端在第一坐标系的增量位姿信息。
值得注意的是，在某些使用场景下，机械臂21运动时，需要确保机械臂21运动时使机械臂21远端绕不动点(远端运动中心，Remote Center of Motion)运动，即做RCM约束运动，具体可以通过对机械臂远端的任务自由度进行设置来确保实现，该任务自由度仅与姿态自由度相关。臂体远端的任务自由度可被理解为臂体远端在笛卡尔空间允许运动的自由度，其至多为6个。臂体远端在笛卡尔空间实际具有的自由度为有效自由度，臂体远端的有效自由度与其构型(即结构特征)相关，臂体远端的有效自由度可被理解为臂体远端在笛卡尔空间内可实现的自由度。
该不动点与机械臂远端具有一个相对固定的位置关系。根据具体的控制目的，一些实施例中第二坐标系的原点可以是该不动点，其他实施例中第二坐标系的原点也可以是机械臂远端上的某一点。
一实施例中，如图23所示，具体在步骤S2即分解步骤中，可以包括：
步骤S211，获取输入的关联于机械臂远端的任务自由度的操作命令。
步骤S212，结合任务自由度分别分解各初始目标位姿信息获得包括机械臂远端在第一坐标系的第一分量目标位姿信息及受控操作末端器械在第二坐标系的第二分量目标位姿信息的一组位姿信息集合。
其中，该操作命令可以包括第一操作命令和第二操作命令。第一操作命令同机械臂21远端的任务自由度与机械臂21的有效自由度完全匹配的情况相关联，可以令机械臂远端在机械臂的有效自由度内自由运动；第二操作命令同机械臂21远端的任务自由度与机械臂21的有效自由度中的姿态自由度完全匹配的情况相关联，该第二操作命令即对应上述的RCM约束运动，以在机械臂21运动时确保其远端即动力机构22绕不动点运动。当然，还可以定义其它组合的任务自由度以方便控制，此处不再赘述。
例如在步骤S211中获取到第二操作命令时，分解获得的第一分量目标位姿信息中与位置自由度相关的信息保持不变、而只有与姿态自由度相关的信息发生变化。这样机械臂21远端绕不动点运动，而主要依赖受控操作末端器械34B的运动实现期望的位姿，能够确保手术安全。
一实施例中，具体在步骤S2即分解步骤中，可以包括：
步骤S221，获取机械臂远端在第一坐标系的当前位姿信息；
步骤S222，在机械臂远端保持于与其当前位姿信息对应的当前位姿的条件下，换算初始目标位姿信息获得第二分量目标位姿信息；
步骤S223，对第二分量目标位姿信息的有效性进行判断；
该步骤中，如果第二分量目标位姿信息有效，进入步骤S224；否则，进入步骤S225。
步骤S224，在受控操作末端器械到达与第二分量目标位姿信息对应的目标位姿的条件下，换算初始目标位姿信息获得第一分量目标位姿信息；
步骤S225，调整第二分量目标位姿信息为有效并更新第二分量目标位姿信息，并在受控操作末端器械到达与更新后的第二分量目标位姿信息对应的目标位姿的条件下，换算初始目标位姿信息获得第一分量目标位姿信息。
通过上述步骤S221～步骤S225，在对受控操作末端器械的位姿进行调整时，优先调整对应的操作臂，如果操作臂的运动满足对受控操作末端器械的调整，则只需要操作臂运动；如果操作臂的运动不能满足对受控操作末端器械的调整，则可以结合机械臂的运动来进行调整。
进而在上述步骤S3即第一判断步骤中，由于第二分量目标位姿信息本身有效或调整后有效，因此实际只需要对第一分量目标位姿信息进行判断即可，在该第一分量目标位姿信息有效时，则可以判断对应的位姿信息集合为有效，否则，则判断位姿信息集合为无效。
结合图6参阅，上述步骤S221～步骤S222可以通过如下方式实现，具体通过如下公式(1)实现：
其中，是受控操作末端器械34B在第一坐标系的初始目标位姿信息，是机械臂远端在第一坐标系的当前位姿信息，是受控操作末端器械34B在第二坐标系的目标位姿信息。T2是受控操作末端器械34B的工具坐标系，T1是机械臂的工具坐标系，B是机械臂的基坐标系。计算时，由于先将和是已知的，因而可以计算
如果步骤S 223中判断出是无效的，可以将调整成有效，然后由于和是已知的，计算
本领域技术人员可以理解的是，前述实施例中涉及计算(换算)相应臂体远端在相应坐标系的目标位姿信息时，均可以利用上述公式(1)来实现，只是对于等的定义可能根据实际情况发生变化而已，比如，也可以是相应臂体在第一坐标系的当前位姿信息，此处不再一一赘述。
一实施例中，对于上述任意获得的目标位姿信息的有效性进行判断的步骤，包括：
步骤S71，将目标位姿信息解析为相应臂体中各关节组件的目标运动状态参数。
步骤S72，将臂体中各关节组件的目标运动状态参数与臂体中各关节组件的运动状态阈值进行对比。
步骤S73，如果臂体中各关节组件的目标运动状态参数有一个以上超过相应关节组件的运动状态阈值，则判断目标位姿信息无效；如果臂体中各关节组件的目标运动状态参数均未超过相应关节组件的运动状态阈值，则判断目标位姿信息有效。
其中，运动状态参数包括位置参数、速度参数及加速度参数，运动状态阈值包括位置参数阈值、速度参数阈值及加速度参数阈值。比较时，它们同类型一一对比。
进而在前述步骤S225中，具体即在调整第二分量目标位姿信息为有效的步骤中，可以将超过运动状态阈值的臂体中个关节组件的运动状态调整至相应运动状态阈值以内以使其有效。一实施例中，可以将超过运动状态阈值的臂体中个关节组件的运动状态调整至相应运动状态阈值以使其有效，这样可以尽量让操作臂运动到极限，再配合机械臂进行调节。
上述实施例适用于对如图1所示类型的手术机器人中的末端器械进行控制。该类型的手术机器人包括一个机械臂21及装设于该机械臂21远端的一个以上的具有末端器械34的操作臂31，该机械臂21及操作臂31均具有若干自由度。
上述实施例同样适用于对如图26所示类型的手术机器人中的末端器械进行控制。该类型的手术机器人包括一个主臂32’、装设于主臂32’远端的一个以上的调整臂30’及装设于调整臂30’远端的一个以上的具有末端器械的操作臂31’，该主臂32’、调整臂30’及操作臂31’均具有若干自由度。如图26所示，该手术机器人中，调整臂30’可以设置为四个，每个调整臂30’可以仅设置一个操作臂31’。根据实际使用场景，可以将如图26所示类型的手术机器人的三段式臂体结构配置为如图1所示类型的手术机器人的两段式臂体结构从而实现控制。一实施例中，在该两种类型的手术机器人中的操作臂的概念为一致的情况下，例如，根据配置，可以将如图26所示类型的手术机器人中的各调整臂30’视为如图1所示类型的手术机器人中的机械臂21进行控制；又例如，根据配置，也可以将如图26所示类型的手术机器人中的任一调整臂30’及主臂32’的整体视为如图1所示类型的手术机器人中的机械臂21进行控制。一实施例中，可以将如图26所示类型的手术机器人中的主臂32’视为如图1所示类型的手术机器人中的机械臂21、并将如图26所示类型的手术机器人中的调整臂30’及其对应的操作臂31’的整体视为如图1所示类型的手术机器人中的操作臂31进行控制。
本发明的控制方法尤其适用于单孔手术机器人。如果多孔手术机器人在装设有具有图像末端器械的操作臂的机械臂远端还可以装设具有操作末端器械的操作臂的情况下，也适用于上述的方法。
一实施例中，上述手术机器人的控制方法通常被配置为在手术机器人的处理系统中来实现，该处理系统具有一个以上的处理器。
一实施例中，提供一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，该计算机程序被配置为由一个以上的处理器执行实现上述任一项实施例所述的控制方法的步骤。
本发明的手术机器人及其控制方法、计算机可读存储介质，具有如下有益效果：
通过控制机械臂21运动使操作末端器械34B到达目标位姿的同时，控制图像末端器械34A运动以保持当前位姿，进而能够在保持视野不变的前提下，借助机械臂21的运动扩大操作末端器械34B的操作空间，其使用方便、安全。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种手术机器人中末端器械的控制方法，所述手术机器人包括机械臂，所述机械臂远端装设有两个以上具有末端器械的操作臂，所述末端器械包括一个图像末端器械和一个以上操作末端器械，所述操作末端器械包括受控操作末端器械和非受控操作末端器械，其特征在于，所述控制方法包括：
获取步骤，获取各所述受控操作末端器械的初始目标位姿信息；
分解步骤，分解各所述初始目标位姿信息分别获得一组位姿信息集合，每组所述位姿信息集合包括所述机械臂远端在第一坐标系的第一分量目标位姿信息及所述受控操作末端器械在第二坐标系的第二分量目标位姿信息，第一坐标系指所述机械臂的基坐标系，第二坐标系指所述机械臂的工具坐标系；
第一判断步骤，对各组所述位姿信息集合的有效性进行判断；
计算步骤，在至少有一组所述位姿信息集合有效、且在所述图像末端器械及各所述非受控操作末端器械均保持于当前位姿的条件下，结合各组所述位姿信息集合计算所述机械臂远端在第一坐标系的第一目标位姿信息、所述图像末端器械在第二坐标系的第二目标位姿信息、各所述受控操作末端器械分别在第二坐标系的第三目标位姿信息及各所述非受控操作末端器械分别在第二坐标系的第四目标位姿信息；
第二判断步骤，对所述第一至第四目标位姿信息的有效性进行判断；
控制步骤，在所述第一至第四目标位姿信息均有效时，根据所述第一目标位姿信息控制所述机械臂运动以使所述机械臂远端到达对应的目标位姿，根据所述第二目标位姿信息控制所述图像末端器械对应的操作臂运动以使所述图像末端器械保持于当前位姿，根据各所述第三目标位姿信息控制对应所述受控操作末端器械的操作臂运动以使所述受控操作末端器械到达对应的目标位姿，并根据各所述第四目标位姿信息控制对应于所述非受控操作末端器械的操作臂运动以使各所述非受控操作末端器械保持于当前位姿。
2.根据权利要求1所述的控制方法，其特征在于，所述受控操作末端器械为一个，且所述位姿信息集合有效时，所述计算步骤包括：
在所述机械臂远端到达与所述第一分量目标位姿信息对应的目标位姿的条件下，换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息；
将所述第一分量目标位姿信息赋值成所述第一目标位姿信息，将所述图像末端器械在第二坐标系的目标位姿信息赋值成所述第二目标位姿信息，将所述第二分量目标位姿信息赋值成所述第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息。
3.根据权利要求1所述的控制方法，其特征在于，所述受控操作末端器械为两个以上，且一个所述位姿信息集合有效、其余所述位姿信息集合无效时，所述计算步骤包括：
在所述机械臂远端到达与有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算与各无效的所述位姿信息关联的所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息；
将有效的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将所述图像末端器械在第二坐标系的目标位姿信息赋值成所述第二目标位姿信息，将有效的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将各所述第二期望目标位姿分别赋值成相应所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息。
4.根据权利要求1所述的控制方法，其特征在于，所述受控操作末端器械为多个，且两个以上所述位姿信息集合有效、其余所述位姿信息集合无效时，所述计算步骤包括：
在所述机械臂远端到达与相应于各有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，分别换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息；
对所述图像末端器械在第二坐标系的各目标位姿信息的有效性进行判断；
在所述图像末端器械的各目标位姿信息中仅一个有效时，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的所述目标位姿信息集合关联的各所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息，换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，并换算与其余有效的所述目标位姿信息集合关联的各所述受控操作末端器械的初始目标位姿信息获得其在第二坐标系的第一期望目标位姿信息；
对各所述第一期望目标位姿信息的有效性进行判断；
如果各所述第一期望目标位姿信息均有效，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将各所述第一期望目标位姿信息及各所述第二期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；
如果各所述第一期望目标位姿信息中至少部分无效，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的各所述第一期望目标位姿信息关联的所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息；
将关联于有效的所述图像末端器械的目标位姿信息所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将有效的各所述第一期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述第二期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；
在所述图像末端器械的各目标位姿信息中两个以上有效时，选择将有效的所述图像末端器械的目标位姿信息中的一个作为有效、将其余作为无效，进入在所述图像末端器械的各目标位姿信息中仅一个有效时的步骤。
5.根据权利要求1所述的控制方法，其特征在于，所述受控操作末端器械为两个以上，且各所述位姿信息集合均有效时，所述计算步骤包括：
在所述机械臂远端到达与相应于各有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，分别换算所述图像末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息；
对所述图像末端器械在第二坐标系的各目标位姿信息的有效性进行判断；
在所述图像末端器械的各目标位姿信息中仅一个有效时，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算各所述非受控操作末端器械的当前位姿信息获得其在第二坐标系的目标位姿信息，换算与其余有效的所述目标位姿信息集合关联的各所述受控操作末端器械的初始目标位姿信息获得其在第二坐标系的第一期望目标位姿信息；
对各所述第一期望目标位姿信息的有效性进行判断；
如果各所述第一期望目标位姿信息均有效，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将各所述第一期望目标位姿信息赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；
如果各所述第一期望目标位姿信息中至少部分无效，在所述机械臂远端到达与有效的所述图像末端器械的目标位姿信息关联的、有效的所述位姿信息集合中的第一分量目标位姿信息对应的目标位姿的条件下，换算与无效的各所述第一期望目标位姿信息关联的所述受控操作末端器械的当前位姿信息获得其在第二坐标系的第二期望目标位姿信息；
将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第一分量目标位姿信息赋值成所述第一目标位姿信息，将有效的所述图像末端器械的目标位姿信息赋值成所述第二目标位姿信息，将关联于有效的所述图像末端器械的目标位姿信息的所述位姿信息集合中的第二分量目标位姿信息赋值成关联的所述受控操作末端器械的第三目标位姿信息，将有效的各所述第一期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述第二期望目标位姿信息分别赋值成对应的所述受控操作末端器械的第三目标位姿信息，将各所述非受控操作末端器械在第二坐标系的目标位姿信息赋值成相应所述非受控操作末端器械的第四目标位姿信息；
在所述图像末端器械的各目标位姿信息中两个以上有效时，选择将有效的所述图像末端器械的目标位姿信息中的一个作为有效、将其余作为无效，进入在所述图像末端器械的各目标位姿信息中仅一个有效时的步骤。
6.根据权利要求1所述的控制方法，其特征在于，所述分解步骤包括：
获取输入的关联于所述机械臂远端的任务自由度的操作命令；
结合所述任务自由度分别分解各所述初始目标位姿信息获得包括所述机械臂远端在第一坐标系的第一分量目标位姿信息及所述受控操作末端器械在第二坐标系的第二分量目标位姿信息的一组位姿信息集合。
7.根据权利要求6所述的控制方法，其特征在于：
所述操作命令包括第一操作命令和第二操作命令；
所述第一操作命令同所述机械臂远端的任务自由度与所述机械臂的有效自由度完全匹配的情况相关联；
所述第二操作命令同所述机械臂远端的任务自由度与所述机械臂的有效自由度中的姿态自由度完全匹配的情况相关联。
8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被配置为由处理器加载并执行实现如权利要求1～7任一项所述的控制方法的步骤。
9.一种手术机器人的控制装置，其特征在于，包括：
存储器，用于存储计算机程序；
及处理器，用于加载并执行所述计算机程序；
其中，所述计算机程序被配置为由所述处理器加载并执行实现如权利要求1～7任一项所述的控制方法的步骤。
10.一种手术机器人，其特征在于，包括：
机械臂；
装设于所述机械臂远端的一个以上具有末端器械的操作臂，所述末端器械包括一个图像末端器械和一个以上操作末端器械，所述操作末端器械均被配置为受控操作末端器械；
及分别与所述机械臂和所述操作臂连接的控制装置；
所述控制装置用于执行实现如权利要求1～7任一项所述的控制方法的步骤。
CN201910854104.1A 2019-09-10 2019-09-10 手术机器人及末端器械的控制方法、控制装置、存储介质 Active CN110464469B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201910854104.1A CN110464469B (zh) 2019-09-10 2019-09-10 手术机器人及末端器械的控制方法、控制装置、存储介质
PCT/CN2020/114113 WO2021047520A1 (zh) 2019-09-10 2020-09-08 手术机器人及其末端器械的控制方法、控制装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910854104.1A CN110464469B (zh) 2019-09-10 2019-09-10 手术机器人及末端器械的控制方法、控制装置、存储介质

Publications (2)

Publication Number Publication Date
CN110464469A true CN110464469A (zh) 2019-11-19
CN110464469B CN110464469B (zh) 2020-12-01

Family

ID=68515401

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910854104.1A Active CN110464469B (zh) 2019-09-10 2019-09-10 手术机器人及末端器械的控制方法、控制装置、存储介质

Country Status (1)

Country Link
CN (1) CN110464469B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111805540A (zh) * 2020-08-20 2020-10-23 北京迁移科技有限公司 工件抓取位姿的确定方法、装置、设备及存储介质
WO2021047520A1 (zh) * 2019-09-10 2021-03-18 深圳市精锋医疗科技有限公司 手术机器人及其末端器械的控制方法、控制装置
CN115227407A (zh) * 2022-09-19 2022-10-25 杭州三坛医疗科技有限公司 一种手术机器人控制方法、装置、系统、设备及存储介质

Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080071291A1 (en) * 2006-06-13 2008-03-20 Intuitive Surgical, Inc. Minimally invasive surgical system
EP1464024B1 (en) * 2001-12-07 2012-02-01 Computer Motion, Inc. Microwrist system for surgical procedures
CN102341055A (zh) * 2008-12-31 2012-02-01 直观外科手术操作公司 用于定位图像中手术器械的基准标记设计和探测
US20120083801A1 (en) * 2004-05-04 2012-04-05 Intuitive Surgical, Inc. Tool grip calibration for robotic surgery
CN102665588A (zh) * 2009-11-13 2012-09-12 直观外科手术操作公司 用于微创手术系统中的手存在性探测的方法和系统
CN103097086A (zh) * 2010-09-07 2013-05-08 奥林巴斯株式会社 主-从机械手
CN105101905A (zh) * 2013-03-29 2015-11-25 奥林巴斯株式会社 主从系统
CN105101903A (zh) * 2013-02-04 2015-11-25 儿童国家医疗中心 混合控制外科机器人系统
CN106553195A (zh) * 2016-11-25 2017-04-05 中国科学技术大学 工业机器人抓取过程中的物体6自由度定位方法及系统
CN107106249A (zh) * 2015-01-16 2017-08-29 奥林巴斯株式会社 操作输入装置和医疗用机械手系统
CN107468293A (zh) * 2017-08-31 2017-12-15 中国科学院深圳先进技术研究院 微创手术机器人及应用其的外科手术设备
CN108883541A (zh) * 2016-03-31 2018-11-23 索尼公司 控制装置和控制方法
CN109048890A (zh) * 2018-07-13 2018-12-21 哈尔滨工业大学（深圳） 基于机器人的协调轨迹控制方法、系统、设备及存储介质
CN109965980A (zh) * 2018-09-30 2019-07-05 深圳市精锋医疗科技有限公司 手术机器人的启动方法、可读取存储器及手术机器人
CN110051436A (zh) * 2018-01-18 2019-07-26 上海舍成医疗器械有限公司 自动化协同工作组件及其在手术器械中的应用
JP2019126450A (ja) * 2018-01-23 2019-08-01 国立大学法人名古屋大学 手術用器具制御装置および手術用器具制御方法
CN110116407A (zh) * 2019-04-26 2019-08-13 哈尔滨工业大学（深圳） 柔性机器人位姿测量方法及装置

Patent Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1464024B1 (en) * 2001-12-07 2012-02-01 Computer Motion, Inc. Microwrist system for surgical procedures
US20120083801A1 (en) * 2004-05-04 2012-04-05 Intuitive Surgical, Inc. Tool grip calibration for robotic surgery
US20080071291A1 (en) * 2006-06-13 2008-03-20 Intuitive Surgical, Inc. Minimally invasive surgical system
CN102341055A (zh) * 2008-12-31 2012-02-01 直观外科手术操作公司 用于定位图像中手术器械的基准标记设计和探测
CN102665588A (zh) * 2009-11-13 2012-09-12 直观外科手术操作公司 用于微创手术系统中的手存在性探测的方法和系统
CN103097086A (zh) * 2010-09-07 2013-05-08 奥林巴斯株式会社 主-从机械手
CN105101903A (zh) * 2013-02-04 2015-11-25 儿童国家医疗中心 混合控制外科机器人系统
CN105101905A (zh) * 2013-03-29 2015-11-25 奥林巴斯株式会社 主从系统
CN107106249A (zh) * 2015-01-16 2017-08-29 奥林巴斯株式会社 操作输入装置和医疗用机械手系统
CN108883541A (zh) * 2016-03-31 2018-11-23 索尼公司 控制装置和控制方法
CN106553195A (zh) * 2016-11-25 2017-04-05 中国科学技术大学 工业机器人抓取过程中的物体6自由度定位方法及系统
CN107468293A (zh) * 2017-08-31 2017-12-15 中国科学院深圳先进技术研究院 微创手术机器人及应用其的外科手术设备
CN110051436A (zh) * 2018-01-18 2019-07-26 上海舍成医疗器械有限公司 自动化协同工作组件及其在手术器械中的应用
JP2019126450A (ja) * 2018-01-23 2019-08-01 国立大学法人名古屋大学 手術用器具制御装置および手術用器具制御方法
CN109048890A (zh) * 2018-07-13 2018-12-21 哈尔滨工业大学（深圳） 基于机器人的协调轨迹控制方法、系统、设备及存储介质
CN109965980A (zh) * 2018-09-30 2019-07-05 深圳市精锋医疗科技有限公司 手术机器人的启动方法、可读取存储器及手术机器人
CN110116407A (zh) * 2019-04-26 2019-08-13 哈尔滨工业大学（深圳） 柔性机器人位姿测量方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
修立刚: "腹腔介入式手术机器人机械结构设计及运动仿真", 《CNKI硕士论文全文数据库》 *
陈鹏: "腹腔镜微创手术机器人的控制系统研究", 《CNKI硕士论文全文数据库》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021047520A1 (zh) * 2019-09-10 2021-03-18 深圳市精锋医疗科技有限公司 手术机器人及其末端器械的控制方法、控制装置
CN111805540A (zh) * 2020-08-20 2020-10-23 北京迁移科技有限公司 工件抓取位姿的确定方法、装置、设备及存储介质
CN115227407A (zh) * 2022-09-19 2022-10-25 杭州三坛医疗科技有限公司 一种手术机器人控制方法、装置、系统、设备及存储介质
CN115227407B (zh) * 2022-09-19 2023-03-03 杭州三坛医疗科技有限公司 一种手术机器人控制方法、装置、系统、设备及存储介质

Also Published As

Publication number Publication date
CN110464469B (zh) 2020-12-01

Similar Documents

Publication Publication Date Title
CN110464471B (zh) 手术机器人及其末端器械的控制方法、控制装置
CN110559083B (zh) 手术机器人及其末端器械的控制方法、控制装置
CN110559082B (zh) 手术机器人及其机械臂的控制方法、控制装置
CN111315309B (zh) 用于控制机器人操纵器或相关工具的系统和方法
CN110464469B (zh) 手术机器人及末端器械的控制方法、控制装置、存储介质
US11541551B2 (en) Robotic arm
CN110464470B (zh) 手术机器人及其臂体的控制方法、控制装置
CN110464473B (zh) 手术机器人及其控制方法、控制装置
Stroppa et al. Human interface for teleoperated object manipulation with a soft growing robot
CN113876434A (zh) 主从运动的控制方法、机器人系统、设备及存储介质
JP7079899B2 (ja) ロボットジョイント制御
CN116766151A (zh) 包括弯曲工具的机器人系统及其主从控制方法
WO2021047520A1 (zh) 手术机器人及其末端器械的控制方法、控制装置
CN118105174A (zh) 手术机器人系统的控制方法、装置及存储介质
WO2020209165A1 (ja) 外科手術システム及び外科手術システムの制御方法
JP7079881B2 (ja) ロボットジョイント制御
CN118288303A (zh) 一种基于手势的冗余软体机器人用户友好型实时控制方法
WO2022212524A1 (en) Systems and methods for controlling a robotic manipulator or associated tool
CN117442347A (zh) 欠驱机构遥操控制方法
CN118493375A (zh) 手术机器人的控制方法、装置、电子设备及存储介质
KR102284639B1 (ko) 펜 형태의 인터페이스를 이용한 수술 도구 제어 시스템
EP4243719A1 (en) Adaptive robot-assisted system and method for evaluating the position of the trocar in a robot-assisted laparoscopic surgery intervention

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
GR01 Patent grant
GR01 Patent grant