CN116370088B - 机械臂的运动控制方法及相关装置 - Google Patents
机械臂的运动控制方法及相关装置 Download PDFInfo
- Publication number
- CN116370088B CN116370088B CN202310554106.5A CN202310554106A CN116370088B CN 116370088 B CN116370088 B CN 116370088B CN 202310554106 A CN202310554106 A CN 202310554106A CN 116370088 B CN116370088 B CN 116370088B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- mechanical arm
- point
- reference point
- matrixes
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 239000011159 matrix material Substances 0.000 claims abstract description 197
- 238000006243 chemical reaction Methods 0.000 claims abstract description 82
- 238000003780 insertion Methods 0.000 claims abstract description 70
- 230000037431 insertion Effects 0.000 claims abstract description 70
- 238000013519 translation Methods 0.000 claims abstract description 24
- 230000009466 transformation Effects 0.000 claims description 107
- 230000003287 optical effect Effects 0.000 claims description 71
- 239000013598 vector Substances 0.000 claims description 64
- 239000000700 radioactive tracer Substances 0.000 claims description 38
- 210000000056 organ Anatomy 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 14
- 238000002591 computed tomography Methods 0.000 claims description 6
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 16
- 238000004891 communication Methods 0.000 description 14
- 238000012545 processing Methods 0.000 description 14
- 230000008569 process Effects 0.000 description 10
- 239000003550 marker Substances 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 5
- 208000012260 Accidental injury Diseases 0.000 description 2
- 125000003275 alpha amino acid group Chemical group 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 208000014674 injury Diseases 0.000 description 2
- 230000035515 penetration Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 206010028980 Neoplasm Diseases 0.000 description 1
- 238000010171 animal model Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 239000011248 coating agent Substances 0.000 description 1
- 238000000576 coating method Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 229940079593 drug Drugs 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 210000003734 kidney Anatomy 0.000 description 1
- 230000003902 lesion Effects 0.000 description 1
- 210000004072 lung Anatomy 0.000 description 1
- 230000002685 pulmonary effect Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 210000000952 spleen Anatomy 0.000 description 1
- 239000004575 stone Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- 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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
- A61B17/34—Trocars; Puncturing needles
- A61B17/3403—Needle locating or guiding means
-
- 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/30—Surgical 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
-
- 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/1602—Programme controls characterised by the control system, structure, architecture
-
- 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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
- A61B17/34—Trocars; Puncturing needles
- A61B17/3403—Needle locating or guiding means
- A61B2017/3405—Needle locating or guiding means using mechanical guide means
- A61B2017/3409—Needle locating or guiding means using mechanical guide means including needle or instrument drives
-
- 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/2055—Optical tracking systems
-
- 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/2068—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis using pointers, e.g. pointers having reference marks for determining coordinates of body points
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Life Sciences & Earth Sciences (AREA)
- Surgery (AREA)
- Robotics (AREA)
- Animal Behavior & Ethology (AREA)
- Biomedical Technology (AREA)
- Veterinary Medicine (AREA)
- Public Health (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Medical Informatics (AREA)
- Heart & Thoracic Surgery (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Software Systems (AREA)
- Mathematical Physics (AREA)
- Fuzzy Systems (AREA)
- Pathology (AREA)
- Manipulator (AREA)
- Apparatus For Radiation Diagnosis (AREA)
Abstract
本申请实施例提供了机械臂的运动控制方法及相关装置,本申请通过对末端参考点坐标系与进针点坐标系之间的第一转换矩阵的平移分量进行插值得到M个候选矩阵,且基于M个候选矩阵得到的末端参考点到所述进针点之间的目标路径在第一方向上的曲率大于0,所述第一方向为手术床指向目标对象的方向,一方面可以让机械臂沿着远离目标对象的方向从起点运动到目标位置,安全性高。另一方面通过M个候选矩阵确定M种位姿来控制机械臂的移动,相比其他方案中控制末端参考点走直线的方式,本申请可以有效降低出现奇异点问题的概率。
Description
技术领域
本申请实施例涉及手术导航技术领域,具体涉及机械臂的运动控制方法及相关装置。
背景技术
随着计算机科学技术的不断发展,手术机器人在的应用越来越广泛。示例性地,在医学领域,可以控制手术机器人的机械臂从初始位置移动到目标位置,比如规划的入针点,以便于医生等操作者对靶点进行穿刺等。
由于机械限位或软限位的限制,机械臂运动空间中会出现逆运动无解的情况,这些逆运动无解的点可以称为奇异点。在从初始位置移动到目标位置的过程中进入或接近奇异点时,机械臂可能按照无法意料的方式移动,带来安全隐患。
发明内容
本申请实施例提供了机械臂的运动控制方法及相关装置,通过本申请可以提高机械臂从初始位置运动到目标位置过程中的安全性。
第一方面,本申请实施例提供了机械臂的运动控制方法,上述方法应用于手术导航系统中的电子设备,上述手术导航系统包括上述电子设备、光学追踪设备以及机械臂;上述机械臂包括基座、关节部件以及法兰,上述机械臂通过上述法兰与穿刺导引器固定连接,上述穿刺导引器上设置有示踪器和穿刺导轨,上述示踪器上设置有N个标记物;上述示踪器与上述穿刺导轨固定连接,上述穿刺导轨用于夹持穿刺针;上述光学追踪设备通过追踪上述N个标记物对上述示踪器进行定位,上述N为大于或等于3的整数;上述方法包括:
获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵;上述末端参考点坐标系为在上述机械臂运动之前基于上述穿刺导轨的末端参考点和导轨方向确定的坐标系,上述进针点坐标系为基于进针点和靶点确定的坐标系,上述进针点和靶点是根据重建的目标器官得到的;
对上述第一转换矩阵进行插值,得到M个候选矩阵,上述M个候选矩阵包括上述第一转换矩阵的平移分量和旋转分量的插值结果,基于上述M个候选矩阵得到的上述末端参考点到上述进针点之间的目标路径在第一方向上的曲率大于0,上述第一方向为手术床指向目标对象的方向,上述M为大于或等于2的整数;
基于上述M个候选矩阵计算上述法兰在基座坐标系下的M种位姿,上述基座坐标系为基于上述基座确定的坐标系;上述M个候选矩阵与上述M种位姿一一对应,上述位姿包括空间位置和姿态,上述M种位姿中任意两种位姿包括的空间位置不同;
向上述机械臂发送控制指令,上述控制指令用于控制上述机械臂的上述关节部件基于上述M种位姿分别进行M次运动,上述M次运动中末端参考点的运动轨迹为目标路径。
结合第一方面,在一种可能的实施方式中,上述对上述第一转换矩阵进行插值,得到M个候选矩阵,包括:
对上述第一转换矩阵进行线性插值,得到M个参考矩阵;
获取上述末端参考点坐标系与计算机断层扫描(computed tomography,CT)坐标系之间的第二转换矩阵,上述CT坐标系是基于CT设备确定的坐标系;
确定上述CT坐标系中上述第一方向上的M个偏移量,得到M个第一偏移矩阵,上述M个偏移量与上述M个参考矩阵一一对应;
基于上述第二转换矩阵对上述M个第一偏移矩阵进行转换,得到M个第二偏移矩阵;
将上述M个参考矩阵中的每个参考矩阵和对应的第二偏移矩阵相乘,得到上述M个候选矩阵。
结合第一方面,在一种可能的实施方式中,上述M个候选矩阵对应目标路径上的M个中间点;上述基于上述第二转换矩阵,将上述M个偏移矩阵转换到上述末端参考点坐标系,得到M个第二偏移矩阵,包括:
基于上述第二转换矩阵,计算M个中间点坐标系与上述CT坐标系之间的M个第三转换矩阵;上述M个中间点坐标系为分别基于上述M个中间点确定的坐标系;
基于上述M个第三转换矩阵中每个第三转换矩阵包括的旋转矩阵和对应的第一偏移矩阵,得到上述M个第二偏移矩阵。
结合第一方面,在一种可能的实施方式中,上述获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵,包括:
获取光学坐标系下上述穿刺导轨的出针方向的第一方向向量,以及,获取上述光学坐标系下进针路径方向的第二方向向量;上述光学坐标系为上述光学追踪设备确定的坐标系,上述进针路径方向为上述进针点指向靶点的方向;
计算上述第一方向向量和上述第二方向向量之间的第一法向量;
基于上述光学坐标系与上述末端参考点坐标系之间的第四转换矩阵,将上述第一法向量转换至上述末端参考点坐标系下,得到第二法向量;
基于上述第一方向向量和上述第二方向向量之间的第一夹角和上述第二法向量计算得到上述第一转换矩阵的旋转分量;
获取上述光学坐标系下上述末端参考点指向上述进针点方向的第三方向向量;
基于上述第四转换矩阵对上述第三方向向量进行变换,得到上述第一转换矩阵的平移分量;
基于上述第一转换矩阵的旋转分量和上述第一转换矩阵的平移分量,得到上述第一转换矩阵。
结合第一方面,在一种可能的实施方式中,上述基于上述M个候选矩阵计算上述法兰在基座坐标系下的M种位姿,包括:
在上述机械臂运动之前,获取法兰坐标系与上述基座坐标系之间的第五转换矩阵、以及上述末端参考点坐标系与上述法兰坐标系之间的第六转换矩阵;上述法兰坐标系是基于上述法兰确定的坐标系;
基于上述第五转换矩阵、上述第六转换矩阵以及上述M个候选矩阵,得到上述法兰在基座坐标系下的M种位姿。
结合第一方面,在一种可能的实施方式中,上述M个候选矩阵还包括上述第一转换矩阵的旋转分量的插值结果。
第二方面,本申请实施例提供了一种机械臂的运动控制装置,包括用于执行第一方面或第一方面的任意可能的实现方式中的方法的单元。
第三方面,本申请实施例提供了一种手术导航系统,上述手术导航系统包括电子设备、光学追踪设备以及机械臂;上述机械臂包括基座、关节部件以及法兰,上述机械臂通过上述法兰与穿刺导引器固定连接,上述穿刺导引器上设置有示踪器和穿刺导轨,上述示踪器上设置有N个标记物;上述示踪器与上述穿刺导轨固定连接,上述穿刺导轨用于夹持穿刺针;上述N为大于或等于3的整数;
上述光学追踪设备,用于通过追踪上述N个标记物对上述示踪器进行定位;
上述电子设备,用于获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵;上述末端参考点坐标系为在上述机械臂运动之前基于上述穿刺导轨的末端参考点确定的坐标系,上述进针点坐标系为基于进针点确定的坐标系,上述进针点是根据重建的目标器官得到的;
上述电子设备,用于对上述第一转换矩阵进行插值,得到M个候选矩阵,上述M个候选矩阵包括上述第一转换矩阵的平移分量的插值结果,基于上述M个候选矩阵得到的上述末端参考点到上述进针点之间的目标路径在第一方向上的曲率大于0,上述第一方向为手术床指向目标对象的方向,上述M为大于或等于2的整数;
上述电子设备,用于基于上述M个候选矩阵计算上述法兰在基座坐标系下的M种位姿,上述基座坐标系为基于上述基座确定的坐标系;上述M个候选矩阵与上述M种位姿一一对应,上述位姿包括空间位置和姿态,上述M种位姿中任意两种位姿包括的空间位置不同;
上述电子设备,用于向上述机械臂发送控制指令,上述控制指令用于控制上述机械臂的上述关节部件基于上述M种位姿分别进行M次运动;在相邻两次运动中,后一次运动对应的空间位置与上述末端参考点之间的距离大于前一次运动对应的空间位置与上述末端参考点之间的距离。
第四方面,本申请实施例公开了一种电子设备,包括:处理器和存储器,其中,该存储器中存储有计算机程序,该处理器调用该存储器中存储的计算机程序,用于执行如第一方面或者第一方面的任意一种可能的实施方式中的方法。
第五方面,本申请还提供了另一种电子设备,包括:处理器、发送装置、输入装置、输出装置和存储器,所述存储器用于存储计算机程序代码,所述计算机程序代码包括计算机指令,在所述处理器执行所述计算机指令的情况下,所述电子设备执行如第一方面或者第一方面的任意一种可能的实施方式中的方法。
第六方面,本申请实施例提供了一种计算机可读存储介质,该计算机可读存储介质中存储有计算机程序,当该计算机程序在一个或多个处理器上运行时,使得如第一方面或者第一方面的任意一种可能的实施方式中的方法被执行。
第七方面,本申请实施例提供了一种计算机程序产品,该计算机程序产品包括程序指令,该程序指令当被处理器执行时使该处理器执行如第一方面或者第一方面的任意一种可能的实施方式中的方法。
本申请实施例中,通过对末端参考点坐标系与进针点坐标系之间的第一转换矩阵的平移分量进行插值得到M个候选矩阵,且基于M个候选矩阵得到的末端参考点到所述进针点之间的目标路径在第一方向上的曲率大于0,所述第一方向为手术床指向目标对象的方向,一方面可以让机械臂沿着远离目标对象的方向从起点运动到目标位置,安全性高。另一方面通过M个候选矩阵确定M种位姿来控制机械臂的移动,相比其他方案中控制末端参考点走直线的方式,本申请可以有效降低出现奇异点问题的概率。再者,本申请中机械臂的运动路径是基于已知的M种位姿进行的,即机械臂的各个运动状态和方向都是已知的,可以减少机械臂的非预期运动,从而提高机械臂从初始位置运动到目标位置过程中的安全性。
附图说明
图1是本申请实施例提供的一种手术导航系统的示意图;
图2是本申请实施例提供的一种手术导航系统中的各个坐标系的示意图;
图3是本申请实施例提供的一种机械臂的运动控制方法的流程示意图;
图4是本申请实施例提供的一种CT坐标系与中间点坐标系的关系示意图;
图5是本申请实施例提供的一种机械臂运动路径的示意图;
图6是本申请实施例提供的另一种机械臂运动路径的示意图;
图7是本申请实施例提供的又一种机械臂运动路径的示意图;
图8是本申请实施例提供的一种电子设备的结构示意图;
图9是本申请实施例提供的另一种电子设备的结构示意图。
具体实施方式
本申请以下实施例中所使用的术语只是为了描述特定实施例的目的,而并非旨在作为对本申请的限制。如在本申请的说明书和所附权利要求书中所使用的那样,单数表达形式“一个”、“一种”、“上述”、“该”和“这一”旨在也包括复数表达形式,除非其上下文中明确地有相反指示。还应当理解,本申请中使用的术语“和/或”是指并包含一个或多个所列出项目的任何或所有可能组合。本申请的说明书、权利要求书及附图中的术语“第一”和“第二”等是用于区别不同对象,而不是用于描述特定顺序。
需要说明的是,本申请的说明书、权利要求书及附图中的术语“第一”、“第二”、“第三”以及“第四”等是用于区别不同对象,而不是用于描述特定顺序。还应理解,本申请实施例中,步骤前的编号是为了便于理解和描述方案而作出的,而不应该理解为对步骤执行顺序的限定。
目前,在手术导航系统中,为了提高手术效率,可以让机械臂自动运动至目标位置。在一种可能的实现方式中,可以向机械臂发送包含目标位置的运动指令,由机械臂自主规划初始位置到目标位置之间的运动路径。一般地,机械臂会给末端参考点规划一条从初始位置到目标位置的直线路径,但是直线路径往往存在奇异点问题,即逆运动无解导致机械臂的关节无法达到目标角度。奇异点问题会导致机械臂停止移动或按照意料之外的方式移动,进而导致机械臂无法到达目标位置或对目标对象造成误伤等。
基于上述问题,本申请实施例提供了机械臂的运动控制方法及相关装置,通过本申请可以提高机械臂从初始位置运动到目标位置过程中的安全性。本申请实施例提供的方法适用于手术导航系统中的电子设备,上述电子设备可以是任意一种可执行本申请方法实施例所公开的技术方案的电子设备,电子设备可以是平板电脑、掌上电脑、笔记本电脑等,还可以是终端设备、服务器或者多个服务器组成的服务器集群等,在此不做限定。可选地,本申请方法实施例还可以通过处理器执行计算机程序代码的方式实现。
为便于理解,请参阅图1,图1是本申请实施例提供的一种手术导航系统的示意图。如图1所示,手术导航系统包括电子设备101、光学追踪设备102以及机械臂103。
本申请实施例中,光学追踪设备102可以理解为用于测量标记物的空间位置信息(比如空间坐标)的设备。示例性地,光学追踪设备102可以包括第一传感器1021和第二传感器1022。第一传感器1021可以包括第一红外发光二极管以及第一红外接收器,第二传感器1022可以包括第二红外发光二极管以及第二红外接收器。在对追踪范围内的标记物进行定位时,第一红外发光二极管和第二红外发光二极管可以产生红外光线,并照射到标记物上,标记物表面的反光涂层将红外光线反射回第一红外接收器和第二红外接收器上,光学追踪设备102利用红外光线对标记物进行定位。
本申请实施例中,机械臂103包括基座1031、关节部件1032和法兰1033,机械臂103通过法兰1033与穿刺导引器104固定连接,穿刺导引器104上设置有示踪器1041和穿刺导轨1042,示踪器1041上设置有N个标记物。本申请实施例中,示踪器1041可以理解为用于支撑上述N个标记物的结构。
本申请实施例中,N为大于或等于3的整数,如图1所示的示踪器1041上示例性地包括4个标记物。可以理解的是,在三维空间中,3个点可以确定一个平面,基于上述N个标记物对应的N个坐标可以确定至少一个平面,结合穿刺导引器104的设计图可以确定出穿刺导引器104上其他位置在光学追踪设备102下的空间位置。本申请实施例中,穿刺导轨1042可以用于夹持穿刺针,穿刺针可以理解为现实空间中用于操作者进行穿刺的实体针。
电子设备101可以包括第一通信单元、第一处理器和第一电源。其中,第一电源可以为电子设备101供电,第一通信单元用于电子设备101与机械臂103通信,和/或,用于电子设备101与光学追踪设备102通信。示例性地,电子设备101通过第一通信单元与机械臂103通信时,可以向机械臂发送控制指令,控制指令用于控制机械臂的关节部件基于M种位姿分别进行M次运动;又示例性地,电子设备101通过第一通信单元与光学追踪设备102通信时,电子设备可以通过获取上述N个标记物的坐标信息等。
示例性地,电子设备101可以包括显示屏,显示屏用于实现显示功能,比如可以显示重建的目标器官轮廓,以及用于显示靶点和进针点等。
机械臂103可以包括第二通信单元、第二处理器和第二电源。其中,第二电源为机械臂103供电,第二通信单元用于机械臂103与电子设备101通信,第二处理器用于执行电子设备101发送的控制指令。
为便于理解本申请实施例提供的方法,接下来先介绍本申请实施例中涉及到的坐标系,可以理解的是,本申请实施例中的坐标系均满足右手定则,可以是设备自动设定的,也可以是采用其他方式自定义的坐标系。
为便于理解,示例性地,请参阅图2,图2是本申请实施例提供的一种手术导航系统中的各个坐标系的示意图。
光学坐标系可以理解为基于光学追踪设备确定的坐标系。可以以光学追踪设备两个传感器(比如上述第一传感器和上述第二传感器)之间的中线上的点为原点设定光学坐标系,例如图2中的坐标系(1)。
示踪器坐标系可以理解为基于示踪器确定的坐标系。可以以示踪器上的任一个标记物的中心为原点设定示踪器坐标系,如图2所示的坐标系(3)。
基座坐标系可以理解为基于机械臂的基座确定的坐标系。示例性地,可以以机械臂基座的关节中心点为原点设定基座坐标系,如图2所示的坐标系(5)。
法兰坐标系可以理解为基于机械臂的法兰确定的坐标系。示例性地,可以以机械臂法兰的圆盘中心点为原点设定法兰坐标系,如图2所示的坐标系(2)。
CT坐标系可以理解为基于CT设备确定的坐标系。示例性地,可以以CT机扫描架中心点为原点确定CT坐标系,如图2所示的坐标系(7)。
末端参考点坐标系可以理解为基于末端参考点和导轨方向确定的坐标系。其中,末端参考点可以基于穿刺导轨的结构选定,比如可以将沿着穿刺导轨方向且距离穿刺导轨末端向外长度为L的位置设定为末端参考点,上述情况下,机械臂移动到目标位置后,穿刺导轨末端距离进针点的距离为L。本申请实施例中,距离L可以理解为安全距离,通过预留安全距离可以降低机械臂移动失误情况下对患者误伤的概率。
本申请实施例中,可以以末端参考点为原点设定末端参考点坐标系,如图2所示的坐标系(4)。可选地,可以以导轨指向末端参考点的方向作为末端参考点坐标系的z轴正方向。
进针点坐标系可以理解为基于进针点和靶点确定的坐标系。示例性地,进针点可以理解为从CT三维重建的皮肤轮廓上确定的进针点。本申请实施例中,还可以从CT三维重建的目标器官上确定靶点,进针点指向靶点的路径可以理解为穿刺路径。可选地,电子设备可以包括显示屏,该显示屏可以用于显示目标器官的重建轮廓,例如图2所示的点A可以理解为进针点,点B可以理解为靶点。
本申请实施例中,可以以进针点为原点设定进针点坐标系,如图2所示的坐标系(6)。示例性地,可以以进针点指向靶点的方向为进针点坐标系的z轴正方向。
可以理解的是,不同坐标系下的对象可以通过转换矩阵对空间位置或姿态进行转换,示例性地,转换矩阵中可以包括旋转分量和平移分量,用于表示两个不同坐标系之间的旋转关系和平移关系。基两个坐标系之前的转换矩阵,可以将其中一个坐标系下的任一对象(比如点或向量等)转换到另一个坐标系进行表示。可选地,转换矩阵也可以称为配准矩阵。
以CT坐标系与光学坐标系下的转换矩阵为例,示例性地,可以将CT坐标系与光学坐标系之间的转换矩阵称为转换矩阵M1。在一种可能的实现方式中,可以在目标对象的体表粘贴至少3个标记物(比如5个标记物),然后对目标对象进行CT扫描以得到CT图像,上述CT图像中包括上述5个标记物,如图2所示中201区域所示的5个标记物。对上述CT图像进行分割处理,可以得到上述5个标记物在CT坐标系下的坐标组Ma。然后,利用光学追踪设备对上述5个标记物进行定位追踪,可以得到上述5标记物在光学坐标系下的坐标组Mb。可以理解,上述坐标组Ma和上述坐标组Mb是同一组标记物在不同坐标系下的坐标,对上述坐标组Ma和上述坐标组Mb进行刚性配准即可得到光学坐标系和CT坐标系之间的转换矩阵M1。示例性地,光学坐标系和CT坐标系之间的转换矩阵M1可以用式(1)表示:
(1)
式(1)中,R1可以理解为转换矩阵M1中的旋转分量,P1可以理解为转换矩阵M1中的平移分量。基于转换矩阵M1,CT坐标系下的点和向量可以通过该转换矩阵转换到光学坐标系下进行表示。
本申请实施例中,目标对象可以是能够被CT扫描采集CT图像的任一对象,比如可以是人(例如患者),也可以是动物(比如实验动物)等,本申请对此不作限定。
示例性地,请参阅图3,图3是本申请实施例提供的一种机械臂的运动控制方法的流程示意图。如图3所示,上述方法包括:
301:获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵;末端参考点坐标系为在机械臂运动之前基于穿刺导轨的末端参考点和导轨方向确定的坐标系,进针点坐标系为基于进针点和靶点确定的坐标系,进针点和靶点是根据重建的目标器官得到的。
本步骤中,末端参考点坐标系与进针点坐标系可以参阅前文图2的相关描述,这里不再赘述。本申请实施例中,目标器官可以理解为靶点(比如病灶)所在的器官,且该器官可以进行计算机断层扫描。示例性地,在靶点为结石的情况下,目标器官可以是肾脏、脾脏等;又示例性地,在靶点为肺部结节或肿瘤的情况下,目标器官可以是肺脏。可以理解,在基于重建的目标器官确定靶点后,可以基于靶点确定进针点,一般地,进针点位于皮肤上,可以从CT三维重建的皮肤轮廓上确定进针点。
可以理解的是,在机械臂运动过程中,穿刺导轨的位置是不断变化的。但是,本申请实施例中,末端参考点是在机械臂运动之前基于穿刺导轨确定的,即在机械臂处于静止态(或者可以理解为初始位置)时确定的,应该理解为相对穿刺导轨固定的点。类似地,在目标对象准备就位后,位置基本不会发生变化,因此,进针点也应该理解为固定的点。因此,可以形象地将末端参考点理解为机械臂运动的起点,将进针点理解为机械臂运动的终点。
在一些实施例中,图3所示的步骤301,获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵,包括:
3011:获取光学坐标系下穿刺导轨出针方向的第一方向向量,以及,获取光学坐标系下进针路径方向的第二方向向量;光学坐标系为所述光学追踪设备确定的坐标系,进针路径方向为进针点指向靶点的方向。
本步骤中,光学坐标系可以参阅前文图2的相关描述,这里不再赘述。穿刺导轨出针方向用于表征穿刺针从沿着穿刺导轨进行穿刺的方向,示例性地,在末端参考点坐标系以穿刺导轨出针方向为z轴正方向(例如图2的坐标系(4)所示)的情况下,第一方向向量可以由末端参考点z轴正方向的方向向量旋转到光学坐标系得到。
为便于理解,可以将第一方向向量称为V1,将末端参考点坐标系与光学坐标系之间的转换矩阵称为转换矩阵M3,其中,转换矩阵M3中的旋转矩阵可以由R3表示。那么,光学坐标系下的V1可以表示为:
可以理解,在穿刺导轨出针方向不为z轴正方向而是其他方向时,对式(2)进行适应性修改即可。比如穿刺导轨出针方向为y方向,那么式(2)可以修改为。
本申请实施例中,进针路径可以理解为上述穿刺路径,即进针点指向靶点的路径。示例性地,可以先得到CT坐标系下进针路径的方向向量,然后利用CT坐标系与光学坐标系之间的转换矩阵M1进行转换,得到光学坐标系下的第二方向向量。为便于描述,可以将第二方向向量称为V2。
接下来介绍末端参考点坐标系与光学坐标系之间的转换矩阵M3的计算方式:
复用图2,如图2所示,末端参考点在光学追踪设备的追踪内,虽然无法直接计算出末端参考点坐标系与光学坐标系之间的转换矩阵M3,但是,可以借助示踪器坐标系作为过渡得到末端参考点坐标系与光学坐标系之间的转换矩阵M3。
示例性地,可以将示踪器坐标系与光学坐标系之间的转换矩阵称为转换矩阵M2。由于示踪器坐标系是基于示踪器上的标记物设定的,标记物原本就在光学追踪设备的追踪范围内,因此,光学追踪设备可以基于示踪器上的标记物自动确定出上述转换矩阵M2,可选地,上述转换矩阵M2可以理解为示踪器在光学坐标系下的位姿。示例性地,转换矩阵M2可以用式(2)表示:
(2)
式(2)中,R2可以理解为转换矩阵M2中的旋转分量,P2可以理解为转换矩阵M2中的平移分量。
示例性地,可以将末端参考点坐标系与示踪器坐标系之间的转换矩阵称为转换矩阵T2,由于穿刺导轨与示踪器之间的结构关系是固定的,可以基于两者的相对位置关系得到转换矩阵T2。因此,可以基于转换矩阵T2和转换矩阵M2得到转换矩阵M3,比如可以用式(3)表示:
(3)
式(3)中,R3可以理解为转换矩阵M3中的旋转分量,P3可以理解为转换矩阵M3中的平移分量。
3012:计算第一方向向量和第二方向向量之间的第一法向量。
3013:基于光学坐标系与末端参考点坐标系之间的第四转换矩阵,将第一法向量转换至末端参考点坐标系下,得到第二法向量。
本步骤中,光学坐标系与末端参考点坐标系之间的第四转换矩阵可以理解为上述转换矩阵M3,具体获取方式可以参阅前文步骤3011,这里不再赘述。
为便于描述,可以将第一法向量称为V3,第二法向量称为V4。那么。
3014:基于第一方向向量和第二方向向量之间的第一夹角和第二法向量计算得到第一转换矩阵的旋转分量。
为便于描述,可以将第一方向向量和第二方向向量之间的第一夹角称为,将第一转换矩阵的旋转分量称为/>,假设第二法向量的单位向量为/>。那么,电子设备可以基于、/>、以及第一公式得到/>。其中,第一公式可以是罗德里格斯公式,表达式可以为:
(4)
式(4)中,可以理解为三阶单位矩阵。
3015:获取光学坐标系下末端参考点指向进针点方向的第三方向向量。
3016:基于第四转换矩阵对第三方向向量进行变换,得到第一转换矩阵的平移分量。
本步骤中,第四转换矩阵可以理解为上述转换矩阵M3,转换矩阵M3的旋转分量为R3。为便于描述,可以将光学坐标系下的第三方向向量称为V5,将第一转换矩阵的平移分量称为P4。那么,。
3017:基于第一转换矩阵的旋转分量和第一转换矩阵的平移分量,得到第一转换矩阵。
本步骤中,将第一转换矩阵的旋转分量和第一转换矩阵的平移分量/>合并即可得到第一转换矩阵,示例性地,可以将末端参考点坐标系与进针点坐标系之间的第一转换矩阵称为转换矩阵T4,转换矩阵T4可以用式(5)表示:
(5)
302:对第一转换矩阵进行插值,得到M个候选矩阵;M个候选矩阵包括第一转换矩阵的平移分量的插值结果,基于M个候选矩阵得到的末端参考点到进针点之间的目标路径在第一方向上的曲率大于0,第一方向为手术床指向目标对象的方向,M为大于或等于2的整数。
本步骤中,电子设备对第一转换矩阵进行插值,可以理解为在末端参考点与进针点之间增加M个中间点,每个中间点对应一个候选矩阵,可以得到M个候选矩阵。示例性地,M为大于或等于2的整数,具体取值可以根据路径长度确定,比如可以每间隔2厘米设置一个中间点。例如,假设路径长度为,那么中间点的数量可以是/>。
本申请实施例中,M个候选矩阵包括第一转换矩阵的平移分量的插值结果,即包括对的插值结果。示例性地,电子设备对第一转换矩阵进行插值时,可以直接采用三维曲线进行插值得到满足上述条件的M个候选矩阵;也可以先进行线性插值,然后进行非线性插值得到满足上述条件的M个候选矩阵。
可以理解,上述M个中间点是离散的,在离散点的数量达到一定程度可以将M个中间点对应的路径理解成连续的,因此,末端参考点到进针点之间的目标路径(简称为运动路径)可以理解为连续的曲线,可以计算出运动路径的曲率。
本申请实施例中,第一方向为手术床指向目标对象的方向,运动路径在第一方向上的曲率大于0,用于表征运动路径在远离目标对象的方向上是凸的。可以理解,本申请实施例中,运动路径在第一方向上的曲率大于0即可,与空间中坐标系的建立无关。
示例性地,在对第一转换矩阵插值时,得到的运动路径可以是抛物线,可以是贝塞尔曲线,还可以是B样条曲线、埃尔米特(hermite)曲线等,本申请对此不作限定。
303:基于M个候选矩阵计算法兰在基座坐标系下的M种位姿,基座坐标系为基于基座确定的坐标系;M个候选矩阵与M种位姿一一对应,位姿包括空间位置和姿态,M种位姿中任意两种位姿包括的空间位置不同。
可以理解的是,M个候选矩阵是末端参考点坐标系下的,在控制机械臂运动时,需要将各个位置的空间信息转换到基座坐标系,因此,电子设备基于M个候选矩阵计算法兰在基座坐标系下的M种位姿。其中,对于基座坐标系的描述可以参阅前文图2的相关描述,这里不再赘述。
本申请实施例中,一个候选矩阵对应一种位姿,即M个候选矩阵与M种位姿一一对应。位姿可以包括空间位置和方向,M种位姿中任意两种位姿包括的空间位置不同,因此,上述M种位姿中任意两种位姿是不同的。
示例性地,可以基于基座、法兰以及末端参考点之间的相对位置关系确定上述M种位姿。
在一些实施例中,上述步骤303,基于M个候选矩阵计算法兰在基座坐标系下的M种位姿,包括:
3031:在机械臂运动之前,获取法兰坐标系与基座坐标系之间的第五转换矩阵、以及末端参考点坐标系与法兰坐标系之间的第六转换矩阵;法兰坐标系是基于法兰确定的坐标系。
3032:基于第五转换矩阵、第六转换矩阵以及M个候选矩阵,得到法兰在基座坐标系下的M种位姿。
复用图2,法兰坐标系与基座坐标系之间的第五转换矩阵可以称为T0,末端参考点坐标系与法兰坐标系之间的第六转换矩阵可以称为T3。示例性地,可以由示踪器坐标系与法兰坐标系之间的转换矩阵T1,以及,末端参考点坐标系与示踪器坐标系之间的转换矩阵T2得到T3。示例性地,。
假设第个候选矩阵为/>,第/>个中间点坐标系与基座坐标系之间的转换矩阵为/>,/>取0至M中的任一数值,/>可以由第二公式得到:
(6)
电子设备可以将转换成平移向量和旋转向量的形式,即/>。然后通过逆运动学计算转换成位姿/>。
304:向机械臂发送控制指令,控制指令用于控制机械臂的关节部件基于M种位姿分别进行M次运动,M次运动中末端参考点的的运动轨迹为目标路径。
可以理解的是,M种位姿包括不同的空间位置和姿态。本申请实施例中,机械臂将按照M种位姿进行M次变换,一次变换可以称为一次运动,因此,不同运动采用的位姿可以理解为是不同的,某次运动对应的空间位置可以理解为本次运动采用的位姿包括的空间位置。可以理解,在运动过程中位姿是不断变化的,但是,上述末端参考点是固定的(具体参阅步骤301的介绍),那么,不同运动对应的空间位置与末端参考点之间的距离是不同的。
本申请实施例中,机械臂进行M次运动的运动轨迹可以通过多种方式表征,可以理解为以同一对象为基准得到的运动轨迹,比如可以是末端参考点的运动轨迹,也可以是机械臂法兰的圆盘中心点为基准得到运动轨迹。本申请实施例中,机械臂的M次运动中末端参考点的的运动轨迹为目标路径,可以理解为机械臂以末端参考点为起点,进针点为终点,沿着已经规划好的目标路径进行运动,运动过程可以理解为起点与终点之间的单向运动。
在一些实施例中,在相邻两次运动中,后一次运动对应的空间位置与末端参考点之间的距离大于前一次运动对应的空间位置与末端参考点之间的距离。
示例性地,电子设备可以基于生成指令/>发送给机械臂的控制器,以实现末端参考点沿着运动路径进行运动。
本申请实施例中,通过对末端参考点坐标系与进针点坐标系之间的第一转换矩阵的平移分量进行插值得到M个候选矩阵,且基于M个候选矩阵得到的末端参考点到所述进针点之间的目标路径在第一方向上的曲率大于0,所述第一方向为手术床指向目标对象的方向,一方面可以让机械臂沿着远离目标对象的方向从起点运动到目标位置,安全性高。另一方面通过M个候选矩阵确定M种位姿来控制机械臂的移动,相比其他方案中直接控制末端参考点走直线的方式,本申请可以有效降低出现奇异点问题的概率。再者,本申请中机械臂的运动路径是基于已知的M种位姿进行的,即机械臂的各个运动状态和方向都是已知的,可以减少机械臂的非预期运动,从而提高机械臂从初始位置运动到目标位置过程中的安全性。
在一些实施例中,步骤302,对第一转换矩阵进行插值,得到M个候选矩阵,包括:
3021:对第一转换矩阵进行线性插值,得到M个参考矩阵。
本申请实施例中,M个候选矩阵中包括第一转换矩阵中的平移分量的插值结果。为便于理解,以第一转换矩阵中的平移分量为P4为例介绍插值过程。
示例性地,假设第个参考矩阵的平移分量为/>,其中,/>取0至M中的任一数值。当/>为0(可以理解为起点)时,/>;当/>为M时(可以理解为目标点),;则中间点/>。
上述情况下,第个参考矩阵/>可以表示为:
(7)
式(7)表示不同的参考矩阵的平移分量是不同,为;不同的参考矩阵的旋转分量是相同的,均为R4。
可选地,M个参考矩阵还包括第一转换矩阵的旋转分量的插值结果。为便于理解,以第一转换矩阵中的旋转分量为R4为例介绍插值过程。
在其他方案中,如果只对平移分量进行插值,需要在机械臂到达目标位置后再进行角度变换以对准穿刺路径。采用上述方式过程繁琐,且在机械臂到达目标位置后通过关节部件的变换来调整穿刺导轨的角度可能造成机械臂大幅运动的情况。本申请实施例中,在对第一转换矩阵平移分量和旋转分量分别进行插值后,再进一步对平移分量增加偏移量进行运动路径规划,可以让机械臂到达目标位置后,穿刺导轨有效地对准穿刺路径,效率更高。
示例性地,可以基于第三公式将旋转矩阵R4转换成旋转向量r4,然后对旋转向量r4进行线性插值。其中,上述第三公式示例性地的可以是罗德里格斯公式。
假设第个参考矩阵的旋转分量为/>,其中,/>取0至M中的任一数值。当/>为0时,;当/>为M时,/>;则中间点/>。然后,可以基于式(8)将旋转向量/>转换成旋转矩阵/>。
在另一种可能的实现方式中,也可以通过四元数插值法对旋转矩阵进行插值。
在M个参考矩阵还包括第一转换矩阵的平移分量的插值结果和第一转换矩阵的旋转分量的插值结果的情况下,第个参考矩阵/>可以表示为:
(8)
式(8)表示不同的参考矩阵的平移分量和旋转分量均是不同的。
3022:获取末端参考点坐标系与CT坐标系之间的第二转换矩阵,CT坐标系是基于CT设备确定的坐标系。
复用图2,末端参考点坐标系与CT坐标系之间可以通过光学坐标系建立连接。末端参考点坐标系与光学坐标系之间的转换矩阵M3的计算方式可以参阅前文步骤3011,光学坐标系和CT坐标系之间的转换矩阵M1的计算方式可以参阅前文式(1)的相关描述,这里不再赘述。可以理解,在得到转换矩阵M3和转换矩阵M1的情况下,第二转换矩阵(称为转换矩阵M4)可以表示为。
3033:确定CT坐标系中第一方向上的M个偏移量,得到M个第一偏移矩阵,M个偏移量与所述M个参考矩阵一一对应。
3034:基于第二转换矩阵对M个第一偏移矩阵进行转换,得到M个第二偏移矩阵。
3035:将M个参考矩阵中的每个参考矩阵和对应的第二偏移矩阵相乘,得到M个候选矩阵。
通过上述步骤3021的线性插值,可以理解为基于M个参考矩阵得到的末端参考点到进针点之间的路径(初始路径)为直线运动路径,即在第一方向上的曲率等于0。在此基础上,经过步骤3033-步骤3035的非线性插值,让原本的直线路径变换为曲线运动路径,即变换为在第一方向上曲率大于0的运动路径。
本申请实施例中,由于运动路径在第一方向上的曲率大于0,因此,电子设备计算的偏移量是针对第一方向上的偏移量。第一方向在CT坐标系中的指向正好为y轴正方向,因此,先计算得到CT坐标系下的M个第一偏移矩阵,然后通过末端参考点坐标系与CT坐标系之间的第二转换矩阵对M个第一偏移量进行转换,转换到末端参考点坐标系下,以得到M个第二偏移矩阵,计算量小,效率高。
本申请实施例中,每个参考矩阵都对应一个偏移量,即M个偏移量与所述M个参考矩阵一一对应,因此,一个参考矩阵对应一个偏移量、对应一个第一偏移矩阵,对应一个第二偏移矩阵。
正如步骤303的介绍,电子设备可以通过多种方式得到每个参考矩阵(或者可以理解为每个中间点)对应的偏移量。为便于理解,以使用贝塞尔曲线计算偏移矩阵为例对上述实施例进行介绍。
示例性地,可以基于第四公式得到贝塞尔曲线;然后基于贝塞尔曲线和第一参数得到CT坐标系中第一方向上的M个第一偏移矩阵,其中,第四公式的表达式可以为:
(9)
式(9)中取0至/>之间的任一值,/>表示贝塞尔曲线的阶数,/>表示控制点个数,比如/>表示第/>个控制点。
第一参数(称为)用于对贝塞尔曲线进行缩放,可以理解为为缩放因子,/>越大,贝塞尔曲线离目标对象的距离越远;/>越小,贝塞尔曲线离目标对象的距离越近。
基于此,第个中间点的贝塞尔曲线值为/>,则y方向上的第一偏移矩阵(简称为/>)可以表示为式(10):
(10)
可以理解,M个候选矩阵对应运动路径上的M个中间点,在将CT坐标系中的M个第一偏移矩阵转换到末端参考点坐标系时,可以通过以下方式实现:
(1)基于第二转换矩阵,计算M个中间点坐标系与CT坐标系之间的M个第三转换矩阵;M个中间点坐标系为分别基于M个中间点确定的坐标系。
(2)基于M个第三转换矩阵中每个第三转换矩阵包括的旋转矩阵和对应的第一偏移矩阵,得到M个第二偏移矩阵。
为便于理解,请参阅图4,图4是本申请实施例提供的一种CT坐标系与中间点坐标系的关系示意图。
在机械臂运动之前,CT坐标系与末端参考点坐标系(也可以理解为机械臂运动前位于起点时的末端参考点坐标系)之间的转换矩阵为第二转换矩阵,也即转换矩阵M4,具体计算方式可以参阅前文步骤3022。
在机械臂运动过程中,由于是按照M个候选矩阵进行M次运动,那么,如果以第个候选矩阵/>为例,第/>个中间点与末端参考点坐标系之间的转换矩阵为/>。因此,CT坐标系与第/>个中间点坐标系之间的第三转换矩阵(简称为/>)可以用式(11)表示:
(11)
式(11)中,表示/>的旋转分量,/>表示/>的平移分量。
基于第三转换矩阵包括的旋转矩阵(即)对第一偏移矩阵进行转换,让原本属于CT坐标系的第一偏移矩阵(/>)转换到对应的中间点坐标系下,得到第二偏移矩阵(简称为/>),具体可以通过式(12)实现:/>
(12)
可以将第二偏移矩阵转换成齐次矩阵的形式,即式(13):
(13)
然后,将原来的参考矩阵()乘以转换后的第二偏移矩阵(/>),得到候选矩阵(/>),例如式(14):
(14)
可以理解的是,贝塞尔曲线控制点个数可以设置为3个或多个,对应二阶或更高阶贝塞尔曲线。可以通过设置控制点的位置来改变曲线的形状,以适应更多场景。
为便于理解,请参阅图5-图7,图5是本申请实施例提供的一种机械臂运动路径的示意图,图6是本申请实施例提供的另一种机械臂运动路径的示意图,图7是本申请实施例提供的又一种机械臂运动路径的示意图。
当使用二阶贝塞尔曲线(对应3个控制点),控制点设置为P0为(0,0)、P1为(0.5,1)、P2为(1,0)时,机械臂的运动轨迹如图5所示。
可以通过改变控制点的位置来改变运动路径的朝向,比如同样是二阶贝塞尔曲线,控制点设置为P0为(0,0)、P1为(1,1)、P2为(1,0)时,机械臂的运动轨迹如图6所示。
当使用三阶贝塞尔曲线(对应4个控制点),控制点设置为P0为(0,0)、P1为(0,1)、P2为(1,1)、P3为(1,0)时,机械臂的运动轨迹如图7所示。
以上详细阐述了本申请实施例提供的方法,下面介绍本申请实施例提供的装置。
请参阅图8,图8是本申请实施例提供的一种电子设备的结构示意图,如图8所示的电子设备80应用于手术导航系统,上述手术导航系统包括上述电子设备80、光学追踪设备以及机械臂;上述机械臂包括基座、关节部件以及法兰,上述机械臂通过上述法兰与穿刺导引器固定连接,上述穿刺导引器包括示踪器和穿刺导轨,上述示踪器上设置有N个标记物;上述示踪器与穿刺导轨固定连接,上述穿刺导轨用于夹持穿刺针;上述光学追踪设备通过追踪上述N个标记物对上述示踪器进行定位,上述N为大于或等于3的整数。
电子设备80包括获取单元801、插值单元802、处理单元803以及通信单元804,可选地,电子设备80还可以包括确定单元805、转换单元806,其中,各个单元的描述如下:
获取单元801,用于获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵;上述末端参考点坐标系为在上述机械臂运动之前基于上述穿刺导轨的末端参考点和导轨方向确定的坐标系,上述进针点坐标系为基于进针点和靶点确定的坐标系,上述进针点和靶点是根据重建的目标器官得到的;
插值单元802,用于对上述第一转换矩阵进行插值,得到M个候选矩阵,上述M个候选矩阵包括上述第一转换矩阵的平移分量的插值结果,基于上述M个候选矩阵得到的上述末端参考点到上述进针点之间的目标路径在第一方向上的曲率大于0,上述第一方向为手术床指向目标对象的方向,上述M为大于或等于2的整数;
处理单元803,用于基于上述M个候选矩阵计算上述法兰在基座坐标系下的M种位姿,上述基座坐标系为基于上述基座确定的坐标系;上述M个候选矩阵与上述M种位姿一一对应,上述位姿包括空间位置和姿态,上述M种位姿中任意两种位姿包括的空间位置不同;
通信单元804,用于向上述机械臂发送控制指令,上述控制指令用于控制上述机械臂的上述关节部件基于上述M种位姿分别进行M次运动,M次运动中末端参考点的的运动轨迹为目标路径。
在一种可能的实施方式中,插值单元802,具体用于对上述第一转换矩阵进行线性插值,得到M个参考矩阵;
获取单元801,具体用于获取上述末端参考点坐标系与CT坐标系之间的第二转换矩阵,上述CT坐标系是基于CT设备确定的坐标系;
确定单元805,用于确定上述CT坐标系中上述第一方向上的M个偏移量,得到M个第一偏移矩阵,上述M个偏移量与上述M个参考矩阵一一对应;
转换单元806,用于基于上述第二转换矩阵对上述M个第一偏移矩阵进行转换,得到M个第二偏移矩阵;
处理单元803,用于将上述M个参考矩阵中的每个参考矩阵和对应的第二偏移矩阵相乘,得到上述M个候选矩阵。
在一种可能的实施方式中,上述M个候选矩阵对应上述目标路径上的M个中间点;处理单元803,用于基于上述第二转换矩阵,计算M个中间点坐标系与上述CT坐标系之间的M个第三转换矩阵;上述M个中间点坐标系为分别基于上述M个中间点确定的坐标系;
处理单元803,用于基于上述M个第三转换矩阵中每个第三转换矩阵包括的旋转矩阵和对应的第一偏移矩阵,得到上述M个第二偏移矩阵。
在一种可能的实施方式中,获取单元801,具体用于获取光学坐标系下上述穿刺导轨的出针方向的第一方向向量,以及,获取上述光学坐标系下进针路径方向的第二方向向量;上述光学坐标系为上述光学追踪设备确定的坐标系,上述进针路径方向为上述进针点指向靶点的方向;
处理单元803,用于计算上述第一方向向量和上述第二方向向量之间的第一法向量;
转换单元806,用于基于上述光学坐标系与上述末端参考点坐标系之间的第四转换矩阵,将上述第一法向量转换至上述末端参考点坐标系下,得到第二法向量;
处理单元803,用于基于上述第一方向向量和上述第二方向向量之间的第一夹角和上述第二法向量计算得到上述第一转换矩阵的旋转分量;
获取单元801,具体用于获取上述光学坐标系下上述末端参考点指向上述进针点方向的第三方向向量;
转换单元806,用于基于上述第四转换矩阵对上述第三方向向量进行变换,得到上述第一转换矩阵的平移分量;
处理单元803,用于基于上述第一转换矩阵的旋转分量和上述第一转换矩阵的平移分量,得到上述第一转换矩阵。
在一种可能的实施方式中,获取单元801,具体用于在上述机械臂运动之前,获取法兰坐标系与上述基座坐标系之间的第五转换矩阵、以及上述末端参考点坐标系与上述法兰坐标系之间的第六转换矩阵;上述法兰坐标系是基于上述法兰确定的坐标系;
处理单元803,用于基于上述第五转换矩阵、上述第六转换矩阵以及上述M个候选矩阵,得到上述法兰在基座坐标系下的M种位姿。
在一种可能的实施方式中,上述M个候选矩阵还包括上述第一转换矩阵的旋转分量的插值结果。
请参阅图9,图9是本申请实施例提供的另一种电子设备的结构示意图。如图9所示的电子设备90包括存储器901、处理器902。可选地,该电子设备90还可以包含通信接口903以及总线904。其中,存储器901、处理器902以及通信接口903通过总线904实现彼此之间的通信连接。
其中,存储器901用于提供存储空间,存储空间中可以存储操作系统和计算机程序等数据。存储器901包括但不限于是随机存储记忆体(random access memory,RAM)、只读存储器(read-only memory,ROM)、可擦除可编程只读存储器(erasable programmable readonly memory,EPROM)、或便携式只读存储器(compact disc read-only memory,CD-ROM)。
处理器902是进行算术运算和逻辑运算的模块,可以是中央处理器(centralprocessing unit,CPU)、显卡处理器(graphics processing unit,GPU)或微处理器(microprocessor unit,MPU)等处理模块中的一种或者多种的组合。另外,存储器901中存储有计算机程序,处理器902可以调用存储器901中存储的计算机程序以执行相应的方法。
示例性地,在一些实施例中,处理器902,用于获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵;
处理器902,还用于对上述第一转换矩阵进行插值,得到M个候选矩阵;
处理器902,还用于基于上述M个候选矩阵计算上述法兰在基座坐标系下的M种位姿;
处理器902,还用于控制通信接口向上述机械臂发送控制指令,上述控制指令用于控制上述机械臂的上述关节部件基于上述M种位姿分别进行M次运动。
在另一些实施例中,处理器902可以用于实现电子设备80中获取单元801、插值单元802、处理单元803、通信单元804、确定单元805、转换单元806的功能。可选地,电子设备80中获取单元801获取的数据也可以通过通信接口903获取得到。
本申请还提供一种计算机可读存储介质,该计算机可读存储介质中存储有计算机代码,当计算机代码在计算机上运行时,使得计算机执行上述实施例的方法。
本申请还提供一种计算机程序产品,该计算机程序产品包括计算机代码或计算机程序,当该计算机代码或计算机程序在计算机上运行时,使得上述实施例中的方法被执行。
以上所述,仅为本申请的具体实施方式,但本申请的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本申请揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本申请的保护范围之内。因此,本申请的保护范围应以上述权利要求的保护范围为准。
Claims (10)
1.一种机械臂的运动控制方法,其特征在于,所述方法应用于手术导航系统中的电子设备,所述手术导航系统包括所述电子设备、光学追踪设备以及机械臂;所述机械臂包括基座、关节部件以及法兰,所述机械臂通过所述法兰与穿刺导引器固定连接,所述穿刺导引器上设置有示踪器和穿刺导轨,所述示踪器上设置有N个标记物;所述示踪器与所述穿刺导轨固定连接,所述穿刺导轨用于夹持穿刺针;所述光学追踪设备通过追踪所述N个标记物对所述示踪器进行定位,所述N为大于或等于3的整数;所述方法包括:
获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵;所述末端参考点坐标系为在所述机械臂运动之前基于所述穿刺导轨的末端参考点和导轨方向确定的坐标系,所述进针点坐标系为基于进针点和靶点确定的坐标系,所述进针点和所述靶点是根据重建的目标器官得到的;所述末端参考点为距离穿刺导轨末端向外长度为L的点,所述L为大于0的数;
对所述第一转换矩阵进行插值,得到M个候选矩阵;所述M个候选矩阵包括所述第一转换矩阵的平移分量的插值结果,基于所述M个候选矩阵得到的所述末端参考点到所述进针点之间的目标路径在第一方向上的曲率大于0,所述第一方向为手术床指向目标对象的方向,所述M为大于或等于2的整数;
基于所述M个候选矩阵计算所述法兰在基座坐标系下的M种位姿,所述基座坐标系为基于所述基座确定的坐标系;所述M个候选矩阵与所述M种位姿一一对应,所述位姿包括空间位置和姿态,所述M种位姿中任意两种位姿包括的空间位置不同;
向所述机械臂发送控制指令,所述控制指令用于控制所述机械臂的所述关节部件基于所述M种位姿分别进行M次运动,所述M次运动中所述末端参考点的运动轨迹为所述目标路径。
2.根据权利要求1所述的方法,其特征在于,所述对所述第一转换矩阵进行插值,得到M个候选矩阵,包括:
对所述第一转换矩阵进行线性插值,得到M个参考矩阵;
获取所述末端参考点坐标系与计算机断层扫描CT坐标系之间的第二转换矩阵,所述CT坐标系是基于CT设备确定的坐标系;
确定所述CT坐标系中所述第一方向上的M个偏移量,得到M个第一偏移矩阵,所述M个偏移量与所述M个参考矩阵一一对应;
基于所述第二转换矩阵对所述M个第一偏移矩阵进行转换,得到M个第二偏移矩阵;
将所述M个参考矩阵中的每个参考矩阵和对应的第二偏移矩阵相乘,得到所述M个候选矩阵。
3.根据权利要求2所述的方法,其特征在于,所述M个候选矩阵对应所述目标路径上的M个中间点;所述基于所述第二转换矩阵对所述M个第一偏移矩阵进行转换,得到M个第二偏移矩阵,包括:
基于所述第二转换矩阵,计算M个中间点坐标系与所述CT坐标系之间的M个第三转换矩阵;所述M个中间点坐标系为分别基于所述M个中间点确定的坐标系;
基于所述M个第三转换矩阵中每个第三转换矩阵包括的旋转矩阵和对应的第一偏移矩阵,得到所述M个第二偏移矩阵。
4.根据权利要求1-3中任一项所述的方法,其特征在于,所述获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵,包括:
获取光学坐标系下所述穿刺导轨的出针方向的第一方向向量,以及,获取所述光学坐标系下进针路径方向的第二方向向量;所述光学坐标系为所述光学追踪设备确定的坐标系,所述进针路径方向为所述进针点指向所述靶点的方向;
计算所述第一方向向量和所述第二方向向量之间的第一法向量;
基于所述光学坐标系与所述末端参考点坐标系之间的第四转换矩阵,将所述第一法向量转换至所述末端参考点坐标系下,得到第二法向量;
基于所述第一方向向量和所述第二方向向量之间的第一夹角和所述第二法向量计算得到所述第一转换矩阵的旋转分量;
获取所述光学坐标系下所述末端参考点指向所述进针点方的第三方向向量;
基于所述第四转换矩阵对所述第三方向向量进行变换,得到所述第一转换矩阵的平移分量;
基于所述第一转换矩阵的旋转分量和所述第一转换矩阵的平移分量,得到所述第一转换矩阵。
5.根据权利要求4所述的方法,其特征在于,所述基于所述M个候选矩阵计算所述法兰在基座坐标系下的M种位姿,包括:
在所述机械臂运动之前,获取法兰坐标系与所述基座坐标系之间的第五转换矩阵、以及所述末端参考点坐标系与所述法兰坐标系之间的第六转换矩阵;所述法兰坐标系是基于所述法兰确定的坐标系;
基于所述第五转换矩阵、所述第六转换矩阵以及所述M个候选矩阵,得到所述法兰在基座坐标系下的M种位姿。
6.根据权利要求5所述的方法,其特征在于,所述M个候选矩阵还包括所述第一转换矩阵的旋转分量的插值结果。
7.一种手术导航系统,其特征在于,所述手术导航系统包括电子设备、光学追踪设备以及机械臂;所述机械臂包括基座、关节部件以及法兰,所述机械臂通过所述法兰与穿刺导引器固定连接,所述穿刺导引器上设置有示踪器和穿刺导轨,所述示踪器上设置有N个标记物;所述示踪器与所述穿刺导轨固定连接,所述穿刺导轨用于夹持穿刺针;所述N为大于或等于3的整数;
所述光学追踪设备,用于通过追踪所述N个标记物对所述示踪器进行定位;
所述电子设备,用于获取末端参考点坐标系与进针点坐标系之间的第一转换矩阵;所述末端参考点坐标系为在所述机械臂运动之前基于所述穿刺导轨的末端参考点和导轨方向确定的坐标系,所述进针点坐标系为基于进针点和靶点确定的坐标系,所述进针点和所述靶点是根据重建的目标器官得到的;所述末端参考点为距离穿刺导轨末端向外长度为L的点,所述L为大于0的数;
所述电子设备,用于对所述第一转换矩阵进行插值,得到M个候选矩阵,所述M个候选矩阵包括所述第一转换矩阵的平移分量的插值结果,基于所述M个候选矩阵得到的所述末端参考点到所述进针点之间的目标路径在第一方向上的曲率大于0,所述第一方向为手术床指向目标对象的方向,所述M为大于或等于2的整数;
所述电子设备,用于基于所述M个候选矩阵计算所述法兰在基座坐标系下的M种位姿,所述基座坐标系为基于所述基座确定的坐标系;所述M个候选矩阵与所述M种位姿一一对应,所述位姿包括空间位置和姿态,所述M种位姿中任意两种位姿包括的空间位置不同;
所述电子设备,用于向所述机械臂发送控制指令,所述控制指令用于控制所述机械臂的所述关节部件基于所述M种位姿分别进行M次运动,所述M次运动中所述末端参考点的运动轨迹为所述目标路径。
8.一种机械臂的运动控制装置,其特征在于,包括用于执行如权利要求1-6中任一项所述方法的单元。
9.一种电子设备,其特征在于,包括:处理器和存储器,其中,所述存储器中存储有计算机程序,所述处理器调用所述存储器中存储的计算机程序,用于执行如权利要求1-6中任一项所述的方法。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质中存储有计算机程序,当所述计算机程序在一个或多个处理器上运行时,使得如权利要求1-6中任一项所述的方法被执行。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310554106.5A CN116370088B (zh) | 2023-05-17 | 2023-05-17 | 机械臂的运动控制方法及相关装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310554106.5A CN116370088B (zh) | 2023-05-17 | 2023-05-17 | 机械臂的运动控制方法及相关装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116370088A CN116370088A (zh) | 2023-07-04 |
CN116370088B true CN116370088B (zh) | 2023-08-11 |
Family
ID=86978943
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310554106.5A Active CN116370088B (zh) | 2023-05-17 | 2023-05-17 | 机械臂的运动控制方法及相关装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116370088B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117444987B (zh) * | 2023-12-22 | 2024-03-15 | 北京衔微医疗科技有限公司 | 一种应用于辅助机器人的器械控制方法、系统、终端及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109998682A (zh) * | 2019-04-28 | 2019-07-12 | 北京天智航医疗科技股份有限公司 | 探针装置、精度检测方法、精度检测系统及定位系统 |
CN110496301A (zh) * | 2019-08-03 | 2019-11-26 | 芜湖安普机器人产业技术研究院有限公司 | 一种适用于临床人体截石位靶向粒子植入机器人 |
CN218220310U (zh) * | 2022-07-12 | 2023-01-06 | 深圳惟德精准医疗科技有限公司 | 一种用于穿刺手术的机械臂 |
WO2023005979A1 (zh) * | 2021-07-30 | 2023-02-02 | 武汉联影智融医疗科技有限公司 | 一种机器人手眼标定方法、系统和存储介质 |
CN116077155A (zh) * | 2023-04-06 | 2023-05-09 | 深圳惟德精准医疗科技有限公司 | 基于光学追踪设备和机械臂的穿刺方法及相关装置 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11896440B2 (en) * | 2020-11-30 | 2024-02-13 | Medtronic, Inc. | Medical delivery system |
-
2023
- 2023-05-17 CN CN202310554106.5A patent/CN116370088B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109998682A (zh) * | 2019-04-28 | 2019-07-12 | 北京天智航医疗科技股份有限公司 | 探针装置、精度检测方法、精度检测系统及定位系统 |
CN110496301A (zh) * | 2019-08-03 | 2019-11-26 | 芜湖安普机器人产业技术研究院有限公司 | 一种适用于临床人体截石位靶向粒子植入机器人 |
WO2023005979A1 (zh) * | 2021-07-30 | 2023-02-02 | 武汉联影智融医疗科技有限公司 | 一种机器人手眼标定方法、系统和存储介质 |
CN218220310U (zh) * | 2022-07-12 | 2023-01-06 | 深圳惟德精准医疗科技有限公司 | 一种用于穿刺手术的机械臂 |
CN116077155A (zh) * | 2023-04-06 | 2023-05-09 | 深圳惟德精准医疗科技有限公司 | 基于光学追踪设备和机械臂的穿刺方法及相关装置 |
Also Published As
Publication number | Publication date |
---|---|
CN116370088A (zh) | 2023-07-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7273880B2 (ja) | 仮想オブジェクト駆動方法、装置、電子機器及び可読記憶媒体 | |
CN116370088B (zh) | 机械臂的运动控制方法及相关装置 | |
CN116077155B (zh) | 基于光学追踪设备和机械臂的手术导航方法及相关装置 | |
CN113545851B (zh) | 重建器械术野中心的控制方法、系统、设备和存储介质 | |
EP4090254A1 (en) | Systems and methods for autonomous suturing | |
CN111300384B (zh) | 一种基于标识卡运动的机器人增强现实示教的注册系统及方法 | |
CN111152218A (zh) | 一种异构仿人机械臂的动作映射方法及系统 | |
CN112276914B (zh) | 一种基于ar技术的工业机器人及其人机交互方法 | |
CN113362452A (zh) | 一种手部姿态三维重建方法、装置及存储介质 | |
CN113524201B (zh) | 机械臂位姿主动调节方法、装置、机械臂和可读存储介质 | |
CN115500940A (zh) | 手术针的定位显示方法及相关装置 | |
CN113742992B (zh) | 基于深度学习的主从控制方法及应用 | |
CN113814988A (zh) | 7自由度srs型机械臂逆解的解析方法、装置及电子设备 | |
CN113012268A (zh) | 静态行人图像的运动动态化方法、系统、装置及介质 | |
US20220015844A1 (en) | Kinematics of wristed laparoscopic instruments | |
CN113601504A (zh) | 机器人肢体动作的控制方法及装置、电子设备、存储介质 | |
Trinh et al. | Preliminary design and evaluation of an interfacing mechanism for maneuvering virtual minimally invasive surgical instruments | |
CN112419415B (zh) | 一种基于crs曲线拟合实现位姿规划的超声扫查方法 | |
CN117562661B (zh) | 检测机械臂碰撞的方法及相关产品 | |
JP2676721B2 (ja) | 多関節型ロボットの制御装置 | |
CN117084791B (zh) | 一种穿刺方位解算方法以及穿刺作业执行系统 | |
CN117549328B (zh) | 手术机器人的摆位系统、方法和手术机器人系统 | |
CN117506965B (zh) | 手术机器人的摆位系统、方法、计算机设备和存储介质 | |
CN117084790B (zh) | 一种穿刺方位控制方法、装置、计算机设备、存储介质 | |
US20230341932A1 (en) | Two-way communication between head-mounted display and electroanatomic system |
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 |