CN116135169A - 定位方法、装置、电子设备及计算机可读存储介质 - Google Patents

定位方法、装置、电子设备及计算机可读存储介质 Download PDF

Info

Publication number
CN116135169A
CN116135169A CN202310135853.5A CN202310135853A CN116135169A CN 116135169 A CN116135169 A CN 116135169A CN 202310135853 A CN202310135853 A CN 202310135853A CN 116135169 A CN116135169 A CN 116135169A
Authority
CN
China
Prior art keywords
surgical tool
surgical
pose
image
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
CN202310135853.5A
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.)
Guangzhou Aimuyi Technology Co ltd
Original Assignee
Guangzhou Aimuyi 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 Guangzhou Aimuyi Technology Co ltd filed Critical Guangzhou Aimuyi Technology Co ltd
Priority to CN202310135853.5A priority Critical patent/CN116135169A/zh
Publication of CN116135169A publication Critical patent/CN116135169A/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/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
    • 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/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • 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
    • 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
    • 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/2046—Tracking techniques
    • A61B2034/2065—Tracking using image or pattern recognition

Abstract

本公开提供一种定位方法、装置、电子设备及计算机可读存储介质，涉及手术机器人技术领域，该方法包括：获取当前时刻手术工具的当前图像特征，根据世界坐标系下的手术工具的导航路径以及由导航路径的终点建立的导航路径坐标系计算手术工具到达终点处的期望图像特征，基于当前图像特征计算手术机器人下一时刻的位姿控制量，通过下一时刻的位姿控制量调整手术机器人的位姿，获取手术工具的下一时刻图像特征，当下一时刻图像特征与期望图像特征间的实际图像特征误差小于预设误差阈值时，确定手术工具定位完成，否则，重新计算上述的期望图像特征，直至实际图像特征误差小于预设误差阈值。本公开提高了手术机器人的定位速度。

Description

定位方法、装置、电子设备及计算机可读存储介质
技术领域
本公开涉及手术机器人技术领域，尤其涉及一种定位方法、装置、电子设备及计算机可读存储介质。
背景技术
手术机器人学是一门集临床医学、生物医学工程、计算机、机器人学等学科为一体的新兴交叉学科。现有的手术机器人实现定位所设计和使用的图像特征主要包括手术工具上的标记球在光学定位系统中的三维坐标，没有考虑手术工具的方向，从而导致计算的图像特征误差收敛速度慢，降低了手术机器人的定位速度。
需要说明的是，在上述背景技术部分公开的信息仅用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。
发明内容
本公开提供了一种定位方法、装置、电子设备及计算机可读存储介质，至少能够提高手术机器人的定位速度。
本公开的其他特性和优点将通过下面的详细描述变得显然，或部分地通过本公开的实践而习得。
根据本公开的一个方面，提供一种定位方法，应用于手术机器人系统，上述手术机器人系统包括手术机器人、手术工具和光学定位系统，上述手术工具安装于上述手术机器人，上述定位方法包括：
获取当前时刻上述手术工具在第一位置下的当前图像特征；
根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征；其中，上述导航路径在上述光学定位系统所在的第二坐标系下；
基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量，并根据上述下一时刻的位姿控制量调整上述手术机器人的位姿，以使上述手术工具运动至第二位置；
获取上述第二位置下上述手术工具的下一时刻图像特征；
返回执行上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤，直至上述下一时刻图像特征与上述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定上述手术工具定位完成。
可选地，上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤包括：获取上述第二坐标系下关于上述终点和上述导航路径的起点各自的位置坐标；根据上述第一坐标系的原点和上述位置坐标，确定上述期望图像特征。
可选地，上述获取当前时刻上述手术工具在第一位置下的当前图像特征的步骤之前，上述定位方法还包括：构建上述手术工具的初始图像雅可比矩阵；将上述初始图像雅可比矩阵重排为列向量，得到上述手术机器人系统的初始系统状态向量。
可选地，上述构建上述手术工具的初始图像雅可比矩阵的步骤包括：控制上述手术机器人到达初始设定位姿参数对应的初始位姿，并获取上述初始位姿下的上述手术工具的初始图像特征；其中，上述初始设定位姿参数包括i个元素，i大于或者等于6；分别为上述初始设定位姿参数中的每个元素增加预设偏移量，得到i个偏移位姿参数；控制上述手术机器人到达第i个偏移位姿参数对应的偏移位姿；基于上述第i个偏移位姿参数对应的偏移位姿，获取上述手术工具的第i个中间图像特征；确定上述第i个中间图像特征与上述初始图像特征之间的第i个特征差值，得到i个特征差值；根据上述i个特征差值和上述预设偏移量构建上述手术工具的初始图像雅可比矩阵。
可选地，上述基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量的步骤包括：确定上述当前图像特征与上述期望图像特征之间的差值，得到上述手术工具对应的当前图像特征误差；基于上述当前时刻，确定上述手术机器人的当前位姿以及位姿变化量；获取上述手术工具在上述上一时刻的图像雅可比矩阵；基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵；根据上述位姿变化量、上述上一时刻的图像雅可比矩阵、上述当前时刻的图像雅可比矩阵、上述当前图像特征误差以及上述当前位姿，确定上述下一时刻的位姿控制量。
可选地，上述基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵的步骤包括：根据上述位姿变化量确定上述手术机器人系统的当前系统观测矩阵；按照行顺序将上述上一时刻的图像雅可比矩阵重排为列向量，得到上述手术机器人系统在上述上一时刻的系统状态向量；获取上述手术机器人系统的系统观测量、过程噪声协方差矩阵、测量噪声协方差矩阵、上述上一时刻的误差协方差矩阵的第一平方根以及上述当前时刻的误差协方差矩阵的第二平方根；基于平方根容积卡尔曼滤波器、上述初始系统状态向量、上述上一时刻的系统状态向量、上述第一平方根、上述当前系统观测矩阵、上述系统观测量、上述过程噪声协方差矩阵、上述测量噪声协方差矩阵以及上述第二平方根，确定上述手术机器人系统在上述当前时刻的系统状态向量；对上述当前时刻的系统状态向量进行重排，得到上述当前时刻的图像雅可比矩阵。
可选地，获取上述第二位置下上述手术工具的下一时刻图像特征的步骤包括：基于上述手术工具的针尖位置构建第四坐标系；获取上述手术工具的针尖在上述第二坐标系下的针尖坐标；确定上述第四坐标系各个坐标轴上的单位点的单位坐标，并对每个上述单位坐标进行转换，得到各个上述单位点在上述第二坐标系下的转换坐标；根据上述针尖坐标和得到的的上述转换坐标，确定上述下一时刻图像特征。
可选地，上述基于上述手术工具的针尖位置构建第四坐标系的步骤包括：将上述手术工具的针尖位置作为上述第四坐标系的原点；将上述手术工具的针尖的方向作为上述第四坐标系的X轴；将上述手术工具的平面的法向量作为上述第四坐标系的Z轴；将上述X轴与上述Z轴的叉积作为上述第四坐标系的Y轴。
根据本公开的另一个方面，提供一种定位装置，配置于手术机器人系统，上述手术机器人系统包括手术机器人、手术工具和光学定位系统，上述手术工具安装于上述手术机器人上；上述定位装置包括：
第一计算模块，用于获取当前时刻上述手术工具在第一位置下的当前图像特征；
第二计算模块，用于根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征；其中，上述导航路径在上述光学定位系统所在的第二坐标系下；
位姿调整模块，用于基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量，并根据上述下一时刻的位姿控制量调整上述手术机器人的位姿，以使上述手术工具运动至第二位置；
第三计算模块，用于获取上述第二位置下上述手术工具的下一时刻图像特征；
误差比较模块，用于返回执行上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤，直至上述下一时刻图像特征与上述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定上述手术工具定位完成。
根据本公开的再一个方面，提供一种电子设备，上述电子设备包括存储器、处理器以及存储在上述存储器中并可在上述处理器上运行的计算机程序，上述处理器执行上述计算机程序时实现如上述实施例中上述的定位方法。
根据本公开的又一个方面，提供一种计算机可读存储介质，其上存储有计算机程序，上述计算机程序被处理器执行时实现如上述实施例中上述的定位方法。
本公开的实施例所提供的定位方法、装置、电子设备及计算机可读存储介质，具备以下技术效果：
本公开通过采用获取当前时刻手术工具在第一位置下的当前图像特征，根据光学定位系统所在的第二坐标系下的手术工具的导航路径、由导航路径的终点所建立的第一坐标系，确定手术工具到达终点处的期望图像特征，基于当前图像特征确定手术机器人下一时刻的位姿控制量，并根据下一时刻的位姿控制量调整手术机器人的位姿，以使手术工具运动至第二位置，获取第二位置下手术工具的下一时刻图像特征，返回执行根据光学定位系统所在的第二坐标系下的手术工具的导航路径、由导航路径的终点所建立的第一坐标系，确定手术工具到达终点处的期望图像特征，直至下一时刻图像特征与期望图像特征之间的实际图像特征误差小于预设误差阈值，确定手术工具定位完成的技术方案，通过图像特征误差判断手术工具是否到达导航路径的终点，加快了图像特征误差的收敛速度，使得手术机器人的定位更加精准且耗时更短，定位时的运动轨迹更加理想，接近直线，降低了随机性。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1示出了本公开实施例提供的手术机器人系统；
图2示出了本公开定位方法一示例性实施例的流程示意图；
图3示出了手术工具和导航路径的示意图；
图4示出了本公开实施例提供的定位方法中S120的一示例性流程示意图；
图5示出了本公开实施例提供的定位方法中计算手术机器人位姿控制量的一示例性流程示意图；
图6示出了本公开定位方法中S140对应的一示例性的流程示意图；
图7示出了本公开实施例提供的定位装置的结构示意图；
图8示出了本公开实施例提供的电子设备的结构示意图。
具体实施方式
为使本公开的目的、技术方案和优点更加清楚，下面将结合附图对本公开实施例方式作进一步地详细描述。
下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本公开相一致的所有实施方式。相反，它们仅是如所附权利要求书中所详述的、本公开的一些方面相一致的装置和方法的例子。
需要注意的是，上述附图仅是根据本公开示例性实施例的方法所包括的处理的示意性说明，而不是限制目的。易于理解，上述附图所示的处理并不表明或限制这些处理的时间顺序。另外，也易于理解，这些处理可以是例如在多个模块中同步或异步执行的。
手术机器人学是一门集临床医学、生物医学工程、计算机、机器人学等学科为一体的新兴交叉学科。一般来说，基于视觉的机器人系统分为两类，分别是“eye-in-hand”(EIH)系统和“eye-to-hand”(ETH)系统。光学信息引导的手术机器人通常采用ETH模式，可获得机器人及工作环境的全局视觉信息。目前光学信息引导的机器人控制主要有以下两种：其一是手眼标定方法，获取光学定位系统与机械臂之间的空间转换关系是手眼协同工作的前提，通过手眼标定可以得到机器人基座与光学定位系统间、机器人末端与手术器械间的空间关系，手术机器人靶点定位的精度主要取决于手眼标定的精度。另一种是无标定视觉伺服方法，无需进行手眼标定，通过控制机器人往视觉特征逼近、收敛的方向运动，从而完成目标定位。
现有技术中通过手术机器人实现手术工具的目标定位存在以下问题：
1、现有技术中所设计和使用的图像特征为手术工具上的四个标记球在光学定位系统中的三维坐标，由于这四个点同处在一个平面上，特征稳定性不高，导致在线估计算法可能发生图像特征误差不收敛的情况，继而手术机器人偏离目标，定位失败。
2、图像特征误差收敛速度慢，实际手术机器人目标定位速度慢、运动轨迹随机性相对较强。
3、使用CKF(cubature Kalman filter，容积卡尔曼滤波器)算法在线图像雅可比矩阵，然而，CKF中的状态量误差协方差矩阵需要具备对称性和正定性，非线性动态系统状态量滤波问题中的变量可能是病态的，CKF每一次迭代都可能破坏协方差矩阵的对称性和正定性，而正定性的丢失会导致CKF算法运行终止。
基于现有技术中存在的相关技术问题，本公开提供了一种定位方法、装置、电子设备及计算机可读存储介质，以解决上述的相关技术问题。
对于该定位方法，一方面，在手术工具的手术工具的路径上分别计算出当前图像特征和期望图像特征，相较现有技术，该定位方法具有更好的稳定性，图像特征误差收敛更快。另外，在计算手术工具和路径上的特征时，考虑了手术工具的方向约束，保证工具上的标记球始终正对着光学定位系统，提高了系统安全性。
另一方面，将平方根滤波引入CKF，得到平方根容积卡尔曼滤波器(Square-rootCubature Kalman Filter，SCKF)。SCKF使用最小二乘法计算卡尔曼增益，通过将矩阵三角分解或三角化实现协方差的更新，避免了显式的矩阵求逆运算，算法循环中始终保证协方差的对称性和非负定性，从而避免可能最终导致算法不稳定，甚至发散的情况。
以下为本公开提供的定位方法的一实施例。
本公开一实施例提供的定位方法，应用于手术机器人系统。如图1所示，图1示出了本公开实施例提供的手术机器人系统。该手术机器人系统包括手术机器人100、工具执持器200、手术工具300、若干标记球201和光学定位系统400。其中，手术机器人100为六自由度机械臂，用于执持手术器械运动，替代人手完成手术操作。标记球201为表面覆盖有特殊反射层的小球体，能反射近红外光，通常被安装在手术工具或目标物上，用于光学位置定位。光学定位系统400为一种双目相机，能发射和接收近红外光，与标记球一同工作，用于目标位置定位。工具执持器200上设置有4个不共线的标记球，用于夹持手术工具300，手术工具300安装于手术机器人100上，即工具执持器200安装在六自由度机械臂的末端，手术机器人100通过工具执持器200夹持手术工具300，手术工具300例如是手术导航工具。
上述定位方法基于图像的无标定视觉伺服(Image-Based Visual Servoing，IBVS)实现，基于图像的无标定视觉伺服不需要精确的相机校准参数或机器人运动学模型，控制律以图像特征误差为基础，随着手术机器人的运动逐渐接近零，最终完成视觉伺服任务。
基于图像的无标定视觉伺服中的手眼关系可通过图像雅可比矩阵描述：
Figure BDA0004085743080000071
Figure BDA0004085743080000072
上述式中，f为图像特征，p为机械臂任务空间中的末端位姿，机械臂任务空间是指六自由度机械臂的基座所在的空间坐标系，也可以理解为手术机器人基座所在的空间坐标系，Jp(p)是图像雅可比矩阵。图像雅可比矩阵反映了从机器人任务空间到图像特征空间的差异性映射关系，图像雅可比矩阵的在线估计是IBVS(视觉伺服)系统的一个关键问题。
图像特征的选择和提取尤为重要，在很大程度上影响着视觉伺服系统的性能。大多数视觉伺服系统使用目标物体的几何特征，如点、线、收缩角、圆，或它们的组合。还有一些使用全局特征，如傅里叶描述符和图像矩。与传统的视觉伺服系统相比，本公开的手术机器人系统主要应用于手术场景，在光学定位系统和标记球的帮助下可以更准确有效地获得目标的位置信息。基于此，本公开的手术机器人系统使用的图像特征：分别建立手术工具的坐标系和导航路径的坐标系，将坐标系上四个点的空间位置作为实际图像特征或期望图像特征，坐标系上四个点包括了三个坐标轴以及手术工具的针尖，f∈R12。
如图2所示，图2示出了本公开定位方法一示例性实施例的流程示意图，该定位方法包括以下方案：
S110：获取当前时刻上述手术工具在第一位置下的当前图像特征。
在一示例性实施例中，获取当前时刻手术工具在第一位置下的当前图像特征，表示为fk，第一位置为当前时刻手术工具所在的位置，当前图像特征为当前时刻手术工具所在的位置下的图像特征。其中，当前图像特征的计算过程与下述的下一时刻图像特征的计算过程同理，具体参见下一时刻图像特征的计算过程。
S120：根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征；其中，上述导航路径在上述光学定位系统所在的第二坐标系下。
如图3所示，图3示出了手术工具和导航路径的示意图。手术工具的导航路径在光学定位系统所在的第二坐标系下，第二坐标系是指真实世界坐标系，表示为Sw。导航路径由终点和起点决定，并从起点指向终点，即导航路径是由起点指向终点的路径，起点为人体体外的某一位置，起点可以理解为安全点，起点可以根据实际情况选取；终点可以可以理解为目标点，终点为所要抵达到人体的某一位置。
以导航路径的终点(目标点)为原点建立第一坐标系，第一坐标系可以理解为导航路径坐标系。在第一坐标系构建完成之后，根据第一坐标系和导航路径计算手术工具到达导航路径的终点处的期望图像特征，期望图像特征表示为f*。期望图像特征可以理解为手术工具的针尖与导航路径的终点重合时，手术工具的图像特征。图3中，e表示手术工具的针尾，t表示手术工具的针尖。
S130：基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量，并根据上述下一时刻的位姿控制量调整上述手术机器人的位姿，以使上述手术工具运动至第二位置。
得到手术工具的当前图像特征之后，通过当前图像特征计算手术机器人下一时刻的位姿控制量，下一时刻的位姿控制量表示为uk+1。其中，位姿控制量用于控制手术机器人进行位姿调整，从而使得手术工具运动，从而实现手术工具导航。例如，当前时刻，手术机器人握持手术工具处于人体体外的P位置，导航路径为由上到下的竖直路径，P位置为导航路径的起点，起点为导航路径的入口，通过当前图像特征计算出手术机器人下一时刻的位姿控制量之后，通过下一时刻的位姿控制量调整手术机器人的位姿之后，手术工具会运动至起点下方的某一个位置，例如该位置为W位置，此时手术工具所在的W位置为第二位置。
S140：获取上述第二位置下上述手术工具的下一时刻图像特征。
在手术工具运动至第二位置之后，获取第二位置下的手术工具的下一时刻图像特征，下一时刻图像特征表示为fk+1。其中，第二位置与第一位置不同，第二位置可能是终点，也可能不是终点。对于第二位置是否是终点，需要通过下一时刻图像特征与期望图像特征之间的实际图像特征误差确定。
S150：判断上述下一时刻图像特征与上述期望图像特征之间的实际图像特征误差是否小于预设误差阈值；如果实际图像特征误差小于预设误差阈值，则执行S160；如果实际图像特征误差大于或者等于预设误差阈值，则返回执行S120。
S160：上述手术工具定位完成。
得到期望图像特征和下一时刻图像特征之后，计算下一时刻图像特征与期望图像特征之间的差值，得到下一时刻图像特征与期望图像特征之间的实际图像特征误差，实际图像特征误差＝fk+1-f*。进而判断实际图像特征误差是否小于预设误差阈值；如果实际图像特征误差小于预设误差阈值，认为手术工具所在的第二位置为终点，手术工具到达终点，完成了导航任务，即确定手术工具定位完成；如果实际图像特征误差大于或者等于预设误差阈值，认为手术工具所在的第二位置不是终点，手术工具还没有完成导航任务，进而返回执行S120，直至计算得到的实际图像特征误差小于预设误差阈值。
本实施例根据上述技术方案，由于采用获取当前时刻手术工具在第一位置下的当前图像特征，根据光学定位系统所在的第二坐标系下的手术工具的导航路径、由导航路径的终点所建立的第一坐标系，确定手术工具到达终点处的期望图像特征，基于当前图像特征确定手术机器人下一时刻的位姿控制量，并根据下一时刻的位姿控制量调整手术机器人的位姿，以使手术工具运动至第二位置，获取第二位置下手术工具的下一时刻图像特征，返回执行根据光学定位系统所在的第二坐标系下的手术工具的导航路径、由导航路径的终点所建立的第一坐标系，确定手术工具到达终点处的期望图像特征，直至下一时刻图像特征与期望图像特征之间的实际图像特征误差小于预设误差阈值，确定手术工具定位完成的技术方案，通过图像特征误差判断手术工具是否到达导航路径的终点，加快了图像特征误差的收敛速度，使得手术机器人的定位更加精准且耗时更短，定位时的运动轨迹更加理想，接近直线，降低了随机性。
如图4所示，图4示出了本公开实施例提供的定位方法中S120的一示例性流程示意图。可选的，基于上述方法实施例，S120包括以下方案：
S122：获取上述第二坐标系下关于上述终点和上述导航路径的起点各自的位置坐标；
S124：根据上述第一坐标系的原点和上述位置坐标，确定上述期望图像特征。
在一示例性实施例中，第二坐标系Sw下，导航路径的起点的位置坐标表示为
Figure BDA0004085743080000091
导航路径的终点的位置坐标表示为/>
Figure BDA0004085743080000092
在第一坐标系构建完成之后，获取第一坐标系的原点的坐标，即第一坐标系的原点的坐标与终点的位置坐标相同。然后根据第一坐标系的原点的坐标、起点的位置坐标以及终点的位置坐标，计算第一坐标系中X轴上距离原点单位长度的点的坐标，表示为/>
Figure BDA0004085743080000093
第一坐标系中Y轴上距离原点单位长度的点的坐标，表示为/>
Figure BDA0004085743080000101
以及第一坐标系中Z轴上距离原点单位长度的点的坐标，表示为/>
Figure BDA0004085743080000102
其中，/>
Figure BDA0004085743080000103
和/>
Figure BDA0004085743080000104
的计算公式为：
Figure BDA0004085743080000105
得到
Figure BDA0004085743080000106
和/>
Figure BDA0004085743080000107
之后，根据/>
Figure BDA0004085743080000108
以及/>
Figure BDA0004085743080000109
计算手术工具到达终点时，手术工具所在位置的期望图像特征f*，即：
Figure BDA00040857430800001010
通过将导航路径坐标系的原点以及导航路径的起点和终点的位置坐标作为计算手术工具图像特征的依据，使得图像特征更稳定。由于图像特征包括了手术工具的针尖位置，即图像特征的构造方法中考虑到了手术工具的朝向问题，能够避免手术导航过程中因标记球被遮挡而丢失手术工具的引导信息，从而引发手术导航危险的情况发生。
可选的，基于上述方法实施例，S110之前，上述定位方法还包括以下方案：
构建上述手术工具的初始图像雅可比矩阵；
将上述初始图像雅可比矩阵重排为列向量，得到上述手术机器人系统的初始系统状态向量。
在一示例性实施例中，在手术工具位于起点时，构建手术工具在起点所在位置的初始图像雅可比矩阵，即初始化图像雅可比矩阵的过程，得到的初始图像雅可比矩阵表示为J0。得到初始图像雅可比矩阵之后，将初始图像雅可比矩阵重排为列向量，得到手术机器人系统的初始系统状态向量，表示为X0。例如，初始图像雅可比矩阵为12×6的矩阵，该矩阵重排为列向量之后，得到72×1的矩阵，72×1的矩阵就是手术机器人系统的初始系统状态向量。
可选的，基于上述方法实施例，构建上述手术工具的初始图像雅可比矩阵包括以下方案：
控制上述手术机器人到达初始设定位姿参数对应的初始位姿，并获取上述初始位姿下的上述手术工具的初始图像特征；其中，上述初始设定位姿参数包括i个元素，i大于或者等于6；
分别为上述初始设定位姿参数中的每个元素增加预设偏移量，得到i个偏移位姿参数；
控制上述手术机器人到达第i个偏移位姿参数对应的偏移位姿；
基于上述第i个偏移位姿参数对应的偏移位姿，获取上述手术工具的第i个中间图像特征；
确定上述第i个中间图像特征与上述初始图像特征之间的第i个特征差值，得到i个特征差值；
根据上述i个特征差值和上述预设偏移量构建上述手术工具的初始图像雅可比矩阵。
在一示例性实施例中，设定一个位姿参数，通过初始设定位姿参数表示，采用该初始设定位姿参数对手术机器人进行控制，从而调整手术机器人的位姿，手术机器人调整后的位置称为初始位姿，然后获取初始位姿下的上述手术工具的初始图像特征，表示为f0。其中，初始设定位姿参数包括i个元素，i大于或者等于6。本实施例以i＝6为例进行说明。初始设定位姿参数表示为p0，例如p0＝[X0,Y0,Z0,Rx0,Ry0,Rz0]T。
分别为初始设定位姿参数中的每个元素增加预设偏移量Δp，得到6个线性无关的偏移位姿参数，也可以理解为每次在p0中的一个元素上增加偏移量Δp，并保持其他所有元素的数值不变，得到6个线性无关的偏移位姿，第i个偏移位姿参数表示为pi，即：
Figure BDA0004085743080000111
其中，
Figure BDA0004085743080000112
为6阶单位矩阵的第i列。
在初始位姿的基础时上，控制手术机器人到达第i个偏移位姿参数对应的偏移位姿，也就是采用p1至p6依次调整手术机器人的位姿，即手术机器人先到达p1对应的偏移位姿，然后是p2对应的偏移位姿，接着是p3对应的偏移位姿，...，最后是p6对应的偏移位姿。
对于手术机器人达到第i个偏移位姿参数对应的偏移位姿时，获取手术工具的第i个中间图像特征，表示为fi。由于有6个偏移位姿，即p1对应的偏移位姿至p6对应的偏移位姿，那么存在6个中间图像特征，即f1至f6。
计算第i个中间图像特征与初始图像特征之间的第i个特征差值，表示为Δfi，Δfi＝fi-f0，从而得到i个特征差值。得到i个特征差值之后，根据i个特征差值和预设偏移量Δp计算手术工具的初始图像雅可比矩阵，即：
Figure BDA0004085743080000113
当i＝6时，可以得到6个特征差值，即Δf1至Δf6，手术工具的初始图像雅可比矩阵为：
Figure BDA0004085743080000121
可选的，在得到手术机器人系统的初始系统状态向量之后，还初始化手术机器人系统的系统相关参数，即初始化误差协方差矩阵P0，并计算其的初始化平方根S0；设置过程噪声协方差矩阵Q和测量噪声协方差矩阵R，设置当前时刻，当前时刻表示为k，k＝1；设置图像特征误差阈值e0，图像特征误差阈值即为上述的预设误差阈值。
如图5所示，图5示出了本公开实施例提供的定位方法中计算手术机器人位姿控制量的一示例性流程示意图。可选的，基于上述方法实施例，上述确定上述手术机器人的位姿控制量包括以下方案：
S131：确定上述当前图像特征与上述期望图像特征之间的差值，得到上述手术工具对应的当前图像特征误差；
S132：基于上述当前时刻，确定上述手术机器人的当前位姿以及位姿变化量；
S133：获取上述手术工具在上述上一时刻的图像雅可比矩阵；
S134：基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵；
S135：根据上述位姿变化量、上述上一时刻的图像雅可比矩阵、上述当前时刻的图像雅可比矩阵、上述当前图像特征误差以及上述当前位姿，确定上述下一时刻的位姿控制量。
在一示例性实施例中，当前时刻表示为k，手术工具对应的当前图像特征误差表示为ek，ek＝fk-f*，fk表示当前图像特征。手术机器人的当前位姿是指手术机器人的任务空间下的当前位姿，表示为uk，任务空间是指手术机器人的基座所在的坐标系，即基座坐标系；位姿变化量表示为Δuk，Δuk＝uk-uk-1，uk-1表示任务空间下手术机器人上一时刻的位姿。手术工具在上一时刻的图像雅可比矩阵表示为Jk-1，手术工具在当前时刻的图像雅可比矩阵表示为Jk。手术机器人下一时刻的位姿控制量uk+1的计算公式如下：
Figure BDA0004085743080000122
Figure BDA0004085743080000123
/>
k←k+1；
其中，Tk表示中间量，Kp表示控制器参数，手术机器人下一时刻的位姿控制量也是手术机器人下一时刻的位姿变化量。
可选的，基于上述实施例，上述基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵包括以下方案：
根据上述位姿变化量确定上述手术机器人系统的当前系统观测矩阵；
按照行顺序将上述上一时刻的图像雅可比矩阵重排为列向量，得到上述手术机器人系统在上述上一时刻的系统状态向量；
获取上述手术机器人系统的系统观测量、过程噪声协方差矩阵、测量噪声协方差矩阵、上述上一时刻的误差协方差矩阵的第一平方根以及上述当前时刻的误差协方差矩阵的第二平方根；
基于平方根容积卡尔曼滤波器、上述初始系统状态向量、上述上一时刻的系统状态向量、上述第一平方根、上述当前系统观测矩阵、上述系统观测量、上述过程噪声协方差矩阵、上述测量噪声协方差矩阵以及上述第二平方根，确定上述手术机器人系统在上述当前时刻的系统状态向量；
对上述当前时刻的系统状态向量进行重排，得到上述当前时刻的图像雅可比矩阵。
在一示例性实施例中，手术机器人系统的当前系统观测矩阵是指当前时刻的系统观测矩阵，表示为Ck，即：
Figure BDA0004085743080000131
其中，n表示手术机器人的任务空间的维数，手术机器人的任务空间对应的是手术机器人的基座坐标系；m表示图像特征空间的维数，图像特征空间对应的是第二坐标系(真实世界坐标系)。
手术工具在上一时刻的图像雅可比矩阵为Jk-1，按照行顺序将Jk-1重排为列向量，得到手术机器人系统在上一时刻的系统状态向量，表示为Xk-1。
平方根容积卡尔曼滤波器表示为SCKF，手术机器人系统的系统观测量表示为Yk，Yk＝fk-fk-1，fk-1表示手术工具在上一时刻所在位置的图像特征，即上一时刻图像特征；过程噪声协方差矩阵表示为Q、测量噪声协方差矩阵表示为R、上一时刻的误差协方差矩阵表示为Pk-1，上一时刻的误差协方差矩阵的第一平方根表示为Sk-1，当前时刻的误差协方差矩阵表示为Pk，当前时刻的误差协方差矩阵的第二平方根表示为Sk；手术机器人系统当前时刻的系统状态向量表示为Xk，即：
Figure BDA0004085743080000132
其中，当k＝1时，Sk-1＝S0，Sk-1为初始化误差协方差矩阵P0的初始化平方根S0；Xk-1＝X0，Xk-1为初始系统状态向量。由于手术机器人系统当前时刻的系统状态向量是使用SCKF方法在线估计得到的，对运算过程中矩阵的正定性和对称性不作要求，增强了无标定机器人目标定位方法的鲁棒性。
得到手术机器人系统当前时刻的系统状态向量之后，对当前时刻的系统状态向量进行重排，即：Jk←Xk，得到手术工具在当前时刻的图像雅可比矩阵，手术工具在当前时刻的图像雅可比矩阵为Jk。
如图6所示，图6示出了本公开定位方法中S140对应的一示例性的流程示意图。可选的，基于上述实施例，S140包括以下方案：
S142：基于上述手术工具的针尖位置构建第四坐标系；
S144：获取上述手术工具的针尖在上述第二坐标系下的针尖坐标；
S146：确定上述第四坐标系各个坐标轴上的单位点的单位坐标，并对每个上述单位坐标进行转换，得到各个上述单位点在上述第二坐标系下的转换坐标；
S148：根据上述针尖坐标和得到的的上述转换坐标，确定上述下一时刻图像特征。
在一示例性实施例中，如图3所示，对于图像特征的构造，通过工具注册在手术工具上建立相应的坐标系，该坐标系可以理解为中间坐标系，表示为St，同时可得到手术工具的针尖和针尾各自在St中的针尖t的针尖位置和针尾e的针尾位置。
基于手术工具的针尖t的针尖位置构建第四坐标系，第四坐标系表示为Sn，第四坐标系可以理解为针尖坐标系。其中，基于手术工具的针尖t的针尖位置构建第四坐标系包括以下方案：
将上述手术工具的针尖位置作为上述第四坐标系的原点；
将上述手术工具的针尖的方向作为上述第四坐标系的X轴；
将上述手术工具的平面的法向量作为上述第四坐标系的Z轴；
将上述X轴与上述Z轴的叉积作为上述第四坐标系的Y轴。
应理解的是，对于第四坐标系的构建，将针尖t的针尖位置作为第四坐标系的原点，针尖的方向就是第四坐标系的X轴，也可以理解为针尖的朝向所在的直线为第四坐标系的X轴，手术工具所在的平面的法向量
Figure BDA0004085743080000141
为第四坐标系的Z轴，X轴与Z轴的叉积就是第四坐标系的Y轴，即第四坐标系构建完成。
第四坐标系构建完成之后，获取第二坐标系到中间坐标系的转换矩阵以及手术工具的针尖在第二坐标系下的针尖坐标，转换矩阵包括旋转矩阵和平移矩阵，旋转矩阵表示为Rwt，表示为平移矩阵Twt，针尖坐标表示为tw。然后，确定第四坐标系Y轴上单位点的单位坐标，表示为xt、第四坐标系X轴上单位点的单位坐标，表示为yt、第四坐标系Z轴上单位点的单位坐标，表示为zt。其中，xt、yt以及zt的计算公式如下：
Figure BDA0004085743080000151
通过旋转矩阵和平移矩阵对xt、yt以及zt进行转换，得到xt、yt以及zt各自在第二坐标系下对应的转换坐标，即xt在第二坐标系下对应的转换坐标为xw，yt在第二坐标系下对应的转换坐标为yw，zt在第二坐标系下对应的转换坐标为zw。其中，xw、yw和zw的转换公式如下：
Figure BDA0004085743080000152
进而通过tw、xw、yw和zw一起构成j维的列向量，例如j＝12，然后将该j维的列向量作为下一时刻图像特征，下一时刻图像特征表示为f，即：
f＝[tw,xw,yw,zw]T。
以下为本公开提供的定位方法的另一实施例。
本公开一实施例提供的定位方法，应用于手术机器人系统，该定位方法包括以下方案：
步骤A：构建手术工具的初始图像雅可比矩阵J0，也可以理解为初始化建手术工具的图像雅可比矩阵，得到初始图像雅可比矩阵J0。
步骤B：将上述初始图像雅可比矩阵J0重排为列向量，得到上述手术机器人系统的初始系统状态向量X0。
步骤C：初始化手术机器人系统的相关参数，即初始化误差协方差矩阵P0，并计算其平方根S0；设置过程噪声协方差矩阵Q和测量噪声协方差矩阵R，设置当前时刻k＝1；设置图像特征误差阈值e0。
步骤D：计算当前时刻手术工具在第一位置下的当前图像特征fk。
步骤E：基于导航路径的终点建立第一坐标系，并根据第一坐标系的原点的坐标、导航路径的起点的位置坐标和终点的位置坐标，计算手术工具到达终点所在位置的期望图像特征f*。
步骤F：计算手术机器人系统的系统观测量Yk，手术机器人在任务空间下的位姿变化量Δuk，手术工具的当前图像特征误差ek，即：
Yk＝fk-fk-1；
Δuk＝uk-uk-1；
ek＝fk-f*；
步骤G：计算手术机器人系统的当前系统观测矩阵Ck以及手术机器人系统在上一时刻的系统状态向量Xk-1，即：
Figure BDA0004085743080000161
按行顺序重排手术工具在上一时刻的图像雅可比矩阵Jk-1，得到列向量，该列向量即为系统状态向量Xk-1。
步骤H：计算手术工具当前时刻的图像雅可比矩阵Jk，由手术机器人系统在上一时刻的系统状态向量Xk-1通过SCKF计算手术机器人系统当前时刻的系统状态向量Xk，将系统状态向量Xk进行重排，得到图像雅可比矩阵Jk，即：
Figure BDA0004085743080000162
Jk←Xk；
步骤I：计算手术机器人下一时刻的位姿控制量uk+1，即：
Figure BDA0004085743080000163
Figure BDA0004085743080000164
k←k+1；
步骤J：比较当前图像特征误差ek与预设误差阈值e0，如果ek<e0，则手术工具定位完成，并退出算法；如果ek≥e0，则返回步骤E继续执行，直至ek<e0。
本实施例加快了图像特征误差的收敛速度，使得手术机器人的定位更加精准且耗时更短，定位时的运动轨迹更加理想，接近直线，降低了随机性。在图像特征的构造方法中考虑到了手术工具的朝向问题，能够避免手术导航过程中因标记球被遮挡而丢失手术工具的引导信息，从而引发手术导航危险的情况发生。另外，手术机器人系统当前时刻的系统状态向量是使用SCKF方法在线估计得到的，对运算过程中矩阵的正定性和对称性不作要求，增强了无标定机器人目标定位方法的鲁棒性。
下述为本公开装置实施例，可以用于执行本公开方法实施例。对于本公开装置实施例中未披露的细节，请参照本公开方法实施例。
其中，图7示出了可以应用本公开一实施例的定位装置的结构示意图。请参见图7，该图所示的定位装置可以通过软件、硬件或者两者的结合实现成为电子设备的全部或一部分，还可以作为独立的模块集成于电子设备中或服务器上。
本公开实施例中的定位装置700，配置于手术机器人系统，上述手术机器人系统包括手术机器人、手术工具和光学定位系统，上述手术工具安装于上述手术机器人上，上述定位装置700包括：
第一计算模块710，用于获取当前时刻上述手术工具在第一位置下的当前图像特征；
第二计算模块720，用于根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征；其中，上述导航路径在上述光学定位系统所在的第二坐标系下；
位姿调整模块730，用于基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量，并根据上述下一时刻的位姿控制量调整上述手术机器人的位姿，以使上述手术工具运动至第二位置；
第三计算模块740，用于获取上述第二位置下上述手术工具的下一时刻图像特征；
误差比较模块750，用于返回执行上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤，直至上述下一时刻图像特征与上述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定上述手术工具定位完成。
在示例性的实施例中，基于前述方案，上述第二计算模块720包括：
位置坐标获取单元，用于获取上述第二坐标系下关于上述终点和上述导航路径的起点各自的位置坐标；
期望特征计算单元，用于根据上述第一坐标系的原点和上述位置坐标，确定上述期望图像特征。
在示例性的实施例中，基于前述方案，上述定位装置700还包括：
矩阵初始化单元，用于构建上述手术工具的初始图像雅可比矩阵；
状态向量计算单元，用于将上述初始图像雅可比矩阵重排为列向量，得到上述手术机器人系统的初始系统状态向量。
在示例性的实施例中，基于前述方案，上述矩阵初始化单元包括：
第一控制子单元，用于控制上述手术机器人到达初始设定位姿参数对应的初始位姿，并获取上述初始位姿下的上述手术工具的初始图像特征；其中，上述初始设定位姿参数包括i个元素，i大于或者等于6；
参数调整子单元，用于分别为上述初始设定位姿参数中的每个元素增加预设偏移量，得到i个偏移位姿参数；
第二控制子单元，用于控制上述手术机器人到达第i个偏移位姿参数对应的偏移位姿；
中间特征获取子单元，用于基于上述第i个偏移位姿参数对应的偏移位姿，获取上述手术工具的第i个中间图像特征；
差值计算子单元，用于确定上述第i个中间图像特征与上述初始图像特征之间的第i个特征差值，得到i个特征差值；
初始矩阵计算子单元，用于根据上述i个特征差值和上述预设偏移量构建上述手术工具的初始图像雅可比矩阵。
在示例性的实施例中，基于前述方案，上述位姿调整模块740在基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量方面包括：
特征误差计算单元，用于确定上述当前图像特征与上述期望图像特征之间的差值，得到上述手术工具对应的当前图像特征误差；
位姿数据计算单元，用于基于上述当前时刻，确定上述手术机器人的当前位姿以及位姿变化量；
第一矩阵获取单元，用于获取上述手术工具在上述上一时刻的图像雅可比矩阵；
第二矩阵获取单元，用于基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵；
控制量计算单元，用于根据上述位姿变化量、上述上一时刻的图像雅可比矩阵、上述当前时刻的图像雅可比矩阵、上述当前图像特征误差以及上述当前位姿，确定上述下一时刻的位姿控制量。
在示例性的实施例中，基于前述方案，上述第二矩阵获取单元包括：
观测矩阵计算子单元，用于根据上述位姿变化量确定上述手术机器人系统的当前系统观测矩阵；
第一向量计算子单元，用于按照行顺序将上述上一时刻的图像雅可比矩阵重排为列向量，得到上述手术机器人系统在上述上一时刻的系统状态向量；
相关数据获取子单元，用于获取上述手术机器人系统的系统观测量、过程噪声协方差矩阵、测量噪声协方差矩阵、上述上一时刻的误差协方差矩阵的第一平方根以及上述当前时刻的误差协方差矩阵的第二平方根；
第二向量计算子单元，用于基于平方根容积卡尔曼滤波器、上述初始系统状态向量、上述上一时刻的系统状态向量、上述第一平方根、上述当前系统观测矩阵、上述系统观测量、上述过程噪声协方差矩阵、上述测量噪声协方差矩阵以及上述第二平方根，确定上述手术机器人系统在上述当前时刻的系统状态向量；
向量重排子单元，用于对上述当前时刻的系统状态向量进行重排，得到上述当前时刻的图像雅可比矩阵。
在示例性的实施例中，基于前述方案，上述第三计算模块740包括：
坐标系构建单元，用于基于上述手术工具的针尖位置构建第四坐标系；
坐标数据获取单元，用于获取上述手术工具的针尖在上述第二坐标系下的针尖坐标；
转换单元，用于确定上述第四坐标系各个坐标轴上的单位点的单位坐标，并对每个上述单位坐标进行转换，得到各个上述单位点在上述第二坐标系下的转换坐标；
图像特征计算单元，用于根据上述针尖坐标和得到的的上述转换坐标，确定上述下一时刻图像特征。
在示例性的实施例中，基于前述方案，上述坐标系构建单元包括：
原点确定单元，用于将上述手术工具的针尖位置作为上述第四坐标系的原点；
坐标轴确定单元，用于将上述手术工具的针尖的方向作为上述第四坐标系的X轴；将上述手术工具的平面的法向量作为上述第四坐标系的Z轴；将上述X轴与上述Z轴的叉积作为上述第四坐标系的Y轴。
需要说明的是，上述实施例提供的定位装置在执行定位方法时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将设备的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。另外，上述实施例提供的定位装置与定位方法实施例属于同一构思，因此对于本公开装置实施例中未披露的细节，请参照本公开上述的定位方法的实施例，这里不再赘述。
上述本公开实施例序号仅仅为了描述，不代表实施例的优劣。
本公开实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现前述任一实施例方法的步骤。其中，计算机可读存储介质可以包括但不限于任何类型的盘，包括软盘、光盘、DVD、CD-ROM、微型驱动器以及磁光盘、ROM、RAM、EPROM、EEPROM、DRAM、VRAM、闪速存储器设备、磁卡或光卡、纳米系统(包括分子存储器IC)，或适合于存储指令和/或数据的任何类型的媒介或设备。
本公开实施例还提供了一种电子设备，上述电子设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现上述任一实施例方法的步骤。
图8示意性示出了电子设备的结构示意图。请参见图8所示，电子设备800包括有：处理器801和存储器802。
本公开实施例中，处理器801为计算机系统的控制中心，可以是实体机的处理器，也可以是虚拟机的处理器。处理器801可以包括一个或多个处理核心，比如4核心处理器、8核心处理器等。处理器801可以采用DSP(Digital Signal Processing，数字信号处理)、FPGA(Field－Programmable Gate Array，现场可编程门阵列)、PLA(Programmable LogicArray，可编程逻辑阵列)中的至少一种硬件形式来实现。处理器801也可以包括主处理器和协处理器，主处理器是用于对在唤醒状态下的数据进行处理的处理器，也称CPU(CentralProcessing Unit，中央处理器)；协处理器是用于对在待机状态下的数据进行处理的低功耗处理器。
在本公开实施例中，上述处理器801具体用于：获取当前时刻上述手术工具在第一位置下的当前图像特征；根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征；其中，上述导航路径在上述光学定位系统所在的第二坐标系下；基于上述当前图像特征确定上述手术机器人下一时刻的位姿控制量，并根据上述下一时刻的位姿控制量调整上述手术机器人的位姿，以使上述手术工具运动至第二位置；获取上述第二位置下上述手术工具的下一时刻图像特征；返回执行上述根据上述手术工具的导航路径、由上述导航路径的终点所建立的第一坐标系，确定上述手术工具到达上述终点处的期望图像特征的步骤，直至上述下一时刻图像特征与上述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定上述手术工具定位完成。
进一步地，上述处理器801还用于：获取上述第二坐标系下关于上述终点和上述导航路径的起点各自的位置坐标；根据上述第一坐标系的原点和上述位置坐标，确定上述期望图像特征。
进一步地，上述处理器801还用于：构建上述手术工具的初始图像雅可比矩阵；将上述初始图像雅可比矩阵重排为列向量，得到上述手术机器人系统的初始系统状态向量。
进一步地，上述处理器801还用于：控制上述手术机器人到达初始设定位姿参数对应的初始位姿，并获取上述初始位姿下的上述手术工具的初始图像特征；其中，上述初始设定位姿参数包括i个元素，i大于或者等于6；分别为上述初始设定位姿参数中的每个元素增加预设偏移量，得到i个偏移位姿参数；控制上述手术机器人到达第i个偏移位姿参数对应的偏移位姿；基于上述第i个偏移位姿参数对应的偏移位姿，获取上述手术工具的第i个中间图像特征；确定上述第i个中间图像特征与上述初始图像特征之间的第i个特征差值，得到i个特征差值；根据上述i个特征差值和上述预设偏移量构建上述手术工具的初始图像雅可比矩阵。
进一步地，上述处理器801还用于：确定上述当前图像特征与上述期望图像特征之间的差值，得到上述手术工具对应的当前图像特征误差；基于上述当前时刻，确定上述手术机器人的当前位姿以及位姿变化量；获取上述手术工具在上述上一时刻的图像雅可比矩阵；基于上述初始系统状态向量确定上述手术工具在上述当前时刻的图像雅可比矩阵；根据上述位姿变化量、上述上一时刻的图像雅可比矩阵、上述当前时刻的图像雅可比矩阵、上述当前图像特征误差以及上述当前位姿，确定上述下一时刻的位姿控制量。
进一步地，上述处理器801还用于：根据上述位姿变化量确定上述手术机器人系统的当前系统观测矩阵；按照行顺序将上述上一时刻的图像雅可比矩阵重排为列向量，得到上述手术机器人系统在上述上一时刻的系统状态向量；获取上述手术机器人系统的系统观测量、过程噪声协方差矩阵、测量噪声协方差矩阵、上述上一时刻的误差协方差矩阵的第一平方根以及上述当前时刻的误差协方差矩阵的第二平方根；基于平方根容积卡尔曼滤波器、上述初始系统状态向量、上述上一时刻的系统状态向量、上述第一平方根、上述当前系统观测矩阵、上述系统观测量、上述过程噪声协方差矩阵、上述测量噪声协方差矩阵以及上述第二平方根，确定上述手术机器人系统在上述当前时刻的系统状态向量；对上述当前时刻的系统状态向量进行重排，得到上述当前时刻的图像雅可比矩阵。
进一步地，上述处理器801还用于：基于上述手术工具的针尖位置构建第四坐标系；获取上述手术工具的针尖在上述第二坐标系下的针尖坐标；确定上述第四坐标系各个坐标轴上的单位点的单位坐标，并对每个上述单位坐标进行转换，得到各个上述单位点在上述第二坐标系下的转换坐标；根据上述针尖坐标和得到的的上述转换坐标，确定上述下一时刻图像特征。
进一步地，上述处理器801还用于：将上述手术工具的针尖位置作为上述第四坐标系的原点；将上述手术工具的针尖的方向作为上述第四坐标系的X轴；将上述手术工具的平面的法向量作为上述第四坐标系的Z轴；将上述X轴与上述Z轴的叉积作为上述第四坐标系的Y轴。
存储器802可以包括一个或多个计算机可读存储介质，该计算机可读存储介质可以是非暂态的。存储器802还可包括高速随机存取存储器，以及非易失性存储器，比如一个或多个磁盘存储设备、闪存存储设备。在本公开的一些实施例中，存储器802中的非暂态的计算机可读存储介质用于存储至少一个指令，该至少一个指令用于被处理器801所执行以实现本公开实施例中的方法。
一些实施例中，电子设备800还包括有：外围设备接口803和至少一个外围设备。处理器801、存储器802和外围设备接口803之间可以通过总线或信号线相连。各个外围设备可以通过总线、信号线或电路板与外围设备接口803相连。具体地，外围设备包括：显示屏804、摄像头805和音频电路806中的至少一种。
外围设备接口803可被用于将I/O(Input/Output，输入/输出)相关的至少一个外围设备连接到处理器801和存储器802。在本公开的一些实施例中，处理器801、存储器802和外围设备接口803被集成在同一芯片或电路板上；在本公开的一些其他实施例中，处理器801、存储器802和外围设备接口803中的任意一个或两个可以在单独的芯片或电路板上实现。本公开实施例对此不作具体限定。
显示屏804用于显示UI(User Interface，用户界面)。该UI可以包括图形、文本、图标、视频及其它们的任意组合。当显示屏804是触摸显示屏时，显示屏804还具有采集在显示屏804的表面或表面上方的触摸信号的能力。该触摸信号可以作为控制信号输入至处理器801进行处理。此时，显示屏804还可以用于提供虚拟按钮和/或虚拟键盘，也称软按钮和/或软键盘。在本公开的一些实施例中，显示屏804可以为一个，设置电子设备800的前面板；在本公开的另一些实施例中，显示屏804可以为至少两个，分别设置在电子设备800的不同表面或呈折叠设计；在本公开的再一些实施例中，显示屏804可以是柔性显示屏，设置在电子设备800的弯曲表面上或折叠面上。甚至，显示屏804还可以设置成非矩形的不规则图形，也即异形屏。显示屏804可以采用LCD(Liquid Crystal Display，液晶显示屏)、OLED(OrganicLight-Emitting Diode,有机发光二极管)等材质制备。
摄像头805用于采集图像或视频。可选地，摄像头805包括前置摄像头和后置摄像头。通常，前置摄像头设置在电子设备800的前面板，后置摄像头设置在电子设备800的背面。在一些实施例中，后置摄像头为至少两个，分别为主摄像头、景深摄像头、广角摄像头、长焦摄像头中的任意一种，以实现主摄像头和景深摄像头融合实现背景虚化功能、主摄像头和广角摄像头融合实现全景拍摄以及VR(Virtual Reality，虚拟现实)拍摄功能或者其它融合拍摄功能。在本公开的一些实施例中，摄像头805还可以包括闪光灯。闪光灯可以是单色温闪光灯，也可以是双色温闪光灯。双色温闪光灯是指暖光闪光灯和冷光闪光灯的组合，可以用于不同色温下的光线补偿。
音频电路806可以包括麦克风和扬声器。麦克风用于采集用户及环境的声波，并将声波转换为电信号输入至处理器801进行处理。出于立体声采集或降噪的目的，麦克风可以为多个，分别设置在电子设备800的不同部位。麦克风还可以是阵列麦克风或全向采集型麦克风。
电源807用于为电子设备800中的各个组件进行供电。电源807可以是交流电、直流电、一次性电池或可充电电池。当电源807包括可充电电池时，该可充电电池可以是有线充电电池或无线充电电池。有线充电电池是通过有线线路充电的电池，无线充电电池是通过无线线圈充电的电池。该可充电电池还可以用于支持快充技术。
本公开实施例中示出的电子设备800结构框图并不构成对电子设备800的限定，电子设备800可以包括比图示更多或更少的组件，或者组合某些组件，或者采用不同的组件布置。
需要说明的是，本说明书实施例所涉及的信息(包括但不限于用户设备信息、用户个人信息等)、数据(包括但不限于用于分析的数据、存储的数据、展示的数据等)以及信号，均为经用户授权或者经过各方充分授权的，且相关数据的收集、使用和处理需要遵守相关国家和地区的相关法律法规和标准。例如，本说明书中涉及的对象特征、交互行为特征以及用户信息等都是在充分授权的情况下获取的。
在本公开的描述中，需要理解的是，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本公开中的具体含义。此外，在本公开的描述中，除非另有说明，“多个”是指两个或两个以上。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。
以上上述，仅为本公开的具体实施方式，但本公开的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本公开揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本公开的保护范围之内。因此，依本公开权利要求所作的等同变化，仍属本公开所涵盖的范围。

Claims (10)

1.一种定位方法，其特征在于，应用于手术机器人系统，所述手术机器人系统包括手术机器人、手术工具和光学定位系统，所述手术工具安装于所述手术机器人；
所述定位方法包括：
获取当前时刻所述手术工具在第一位置下的当前图像特征；
根据所述手术工具的导航路径、由所述导航路径的终点所建立的第一坐标系，确定所述手术工具到达所述终点处的期望图像特征；其中，所述导航路径在所述光学定位系统所在的第二坐标系下；
基于所述当前图像特征确定所述手术机器人下一时刻的位姿控制量，并根据所述下一时刻的位姿控制量调整所述手术机器人的位姿，以使所述手术工具运动至第二位置；
获取所述第二位置下所述手术工具的下一时刻图像特征；
返回执行所述根据所述手术工具的导航路径、由所述导航路径的终点所建立的第一坐标系，确定所述手术工具到达所述终点处的期望图像特征的步骤，直至所述下一时刻图像特征与所述期望图像特征之间的实际图像特征误差小于预设误差阈值，确定所述手术工具定位完成。
2.如权利要求1所述的定位方法，其特征在于，所述根据所述手术工具的导航路径、由所述导航路径的终点所建立的第一坐标系，确定所述手术工具到达所述终点处的期望图像特征的步骤包括：
获取所述第二坐标系下关于所述终点和所述导航路径的起点各自的位置坐标；
根据所述第一坐标系的原点和所述位置坐标，确定所述期望图像特征。
3.如权利要求1所述的定位方法，其特征在于，所述获取当前时刻所述手术工具在第一位置下的当前图像特征的步骤之前，所述定位方法还包括：
构建所述手术工具的初始图像雅可比矩阵；
将所述初始图像雅可比矩阵重排为列向量，得到所述手术机器人系统的初始系统状态向量。
4.如权利要求3所述的定位方法，其特征在于，所述构建所述手术工具的初始图像雅可比矩阵的步骤包括：
控制所述手术机器人到达初始设定位姿参数对应的初始位姿，并获取所述初始位姿下的所述手术工具的初始图像特征；其中，所述初始设定位姿参数包括i个元素，i大于或者等于6；
分别为所述初始设定位姿参数中的每个元素增加预设偏移量，得到i个偏移位姿参数；
控制所述手术机器人到达第i个偏移位姿参数对应的偏移位姿；
基于所述第i个偏移位姿参数对应的偏移位姿，获取所述手术工具的第i个中间图像特征；
确定所述第i个中间图像特征与所述初始图像特征之间的第i个特征差值，得到i个特征差值；
根据所述i个特征差值和所述预设偏移量构建所述手术工具的初始图像雅可比矩阵。
5.如权利要求3所述的定位方法，其特征在于，所述基于所述当前图像特征确定所述手术机器人下一时刻的位姿控制量的步骤包括：
确定所述当前图像特征与所述期望图像特征之间的差值，得到所述手术工具对应的当前图像特征误差；
基于所述当前时刻，确定所述手术机器人的当前位姿以及位姿变化量；
获取所述手术工具在所述上一时刻的图像雅可比矩阵；
基于所述初始系统状态向量确定所述手术工具在所述当前时刻的图像雅可比矩阵；
根据所述位姿变化量、所述上一时刻的图像雅可比矩阵、所述当前时刻的图像雅可比矩阵、所述当前图像特征误差以及所述当前位姿，确定所述下一时刻的位姿控制量。
6.如权利要求5所述的定位方法，其特征在于，所述基于所述初始系统状态向量确定所述手术工具在所述当前时刻的图像雅可比矩阵的步骤包括：
根据所述位姿变化量确定所述手术机器人系统的当前系统观测矩阵；
按照行顺序将所述上一时刻的图像雅可比矩阵重排为列向量，得到所述手术机器人系统在所述上一时刻的系统状态向量；
获取所述手术机器人系统的系统观测量、过程噪声协方差矩阵、测量噪声协方差矩阵、所述上一时刻的误差协方差矩阵的第一平方根以及所述当前时刻的误差协方差矩阵的第二平方根；
基于平方根容积卡尔曼滤波器、所述初始系统状态向量、所述上一时刻的系统状态向量、所述第一平方根、所述当前系统观测矩阵、所述系统观测量、所述过程噪声协方差矩阵、所述测量噪声协方差矩阵以及所述第二平方根，确定所述手术机器人系统在所述当前时刻的系统状态向量；
对所述当前时刻的系统状态向量进行重排，得到所述当前时刻的图像雅可比矩阵。
7.如权利要求1至6中任意一项所述的定位方法，其特征在于，所述获取所述第二位置下所述手术工具的下一时刻图像特征的步骤包括：
基于所述手术工具的针尖位置构建第四坐标系；
获取所述手术工具的针尖在所述第二坐标系下的针尖坐标；
确定所述第四坐标系各个坐标轴上的单位点的单位坐标，并对每个所述单位坐标进行转换，得到各个所述单位点在所述第二坐标系下的转换坐标；
根据所述针尖坐标和得到的的所述转换坐标，确定所述下一时刻图像特征。
8.如权利要求7所述的定位方法，其特征在于，所述基于所述手术工具的针尖位置构建第四坐标系的步骤包括：
将所述手术工具的针尖位置作为所述第四坐标系的原点；
将所述手术工具的针尖的方向作为所述第四坐标系的X轴；
将所述手术工具的平面的法向量作为所述第四坐标系的Z轴；
将所述X轴与所述Z轴的叉积作为所述第四坐标系的Y轴。
9.一种电子设备，所述电子设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至8中任一项所述的定位方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8中任一项所述的定位方法。
CN202310135853.5A 2023-02-17 2023-02-17 定位方法、装置、电子设备及计算机可读存储介质 Pending CN116135169A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310135853.5A CN116135169A (zh) 2023-02-17 2023-02-17 定位方法、装置、电子设备及计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310135853.5A CN116135169A (zh) 2023-02-17 2023-02-17 定位方法、装置、电子设备及计算机可读存储介质

Publications (1)

Publication Number Publication Date
CN116135169A true CN116135169A (zh) 2023-05-19

Family

ID=86334520

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310135853.5A Pending CN116135169A (zh) 2023-02-17 2023-02-17 定位方法、装置、电子设备及计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN116135169A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116919596A (zh) * 2023-09-14 2023-10-24 武汉联影智融医疗科技有限公司 器械导航方法、系统、装置、设备和存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116919596A (zh) * 2023-09-14 2023-10-24 武汉联影智融医疗科技有限公司 器械导航方法、系统、装置、设备和存储介质
CN116919596B (zh) * 2023-09-14 2024-01-09 武汉联影智融医疗科技有限公司 器械导航方法、系统、装置、设备和存储介质

Similar Documents

Publication Publication Date Title
US10384348B2 (en) Robot apparatus, method for controlling the same, and computer program
US8355816B2 (en) Action teaching system and action teaching method
EP2728548B1 (en) Automated frame of reference calibration for augmented reality
EP1629366B1 (en) Single camera system for gesture-based input and target indication
US6198485B1 (en) Method and apparatus for three-dimensional input entry
WO2022062464A1 (zh) 基于计算机视觉的手眼标定方法及装置、存储介质
KR20180080630A (ko) 핸드-아이 캘리브레이션을 수행하는 로봇 및 전자 장치
TWI677413B (zh) 用於機械手臂系統之校正方法及裝置
EP2932358A1 (en) Direct interaction system for mixed reality environments
US10534426B2 (en) Interactive system, remote controller and operating method thereof
Pachtrachai et al. Hand-eye calibration with a remote centre of motion
CN116135169A (zh) 定位方法、装置、电子设备及计算机可读存储介质
CN112603542B (zh) 手眼标定方法、装置、电子设备和存储介质
CN114343847A (zh) 基于光学定位系统的手术机器人的手眼标定方法
CN113793389A (zh) 一种增强现实系统虚实融合标定方法及装置
Liu et al. Iris feature-based 3-D gaze estimation method using a one-camera-one-light-source system
JP2004265222A (ja) インタフェース方法、装置、およびプログラム
EP3627289A1 (en) Tracking system and tracking method using the same
CN110096134A (zh) 一种vr手柄射线抖动矫正方法、装置、终端和介质
KR20200073031A (ko) 높은 정확도를 갖는 손 동작 추적을 위한 3차원 손 모델 제작 방법
CN113384347B (zh) 一种机器人标定方法、装置、设备及存储介质
CN114926542A (zh) 基于光学定位系统的混合现实固定参考系校准方法
GB2369673A (en) Image processing apparatus calibration
RU2761924C1 (ru) Способ управления манипулятором
WO2022172471A1 (ja) 支援システム、画像処理装置、支援方法およびプログラム

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