CN117562657A - 针对手术机器人的地图构建方法、装置和设备 - Google Patents

针对手术机器人的地图构建方法、装置和设备 Download PDF

Info

Publication number
CN117562657A
CN117562657A CN202311586359.7A CN202311586359A CN117562657A CN 117562657 A CN117562657 A CN 117562657A CN 202311586359 A CN202311586359 A CN 202311586359A CN 117562657 A CN117562657 A CN 117562657A
Authority
CN
China
Prior art keywords
observation
surgical robot
target object
equation
target
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
CN202311586359.7A
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.)
Way Of Technology Medical Technology Suzhou Co ltd
Original Assignee
Way Of Technology Medical Technology Suzhou 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 Way Of Technology Medical Technology Suzhou Co ltd filed Critical Way Of Technology Medical Technology Suzhou Co ltd
Priority to CN202311586359.7A priority Critical patent/CN117562657A/zh
Publication of CN117562657A publication Critical patent/CN117562657A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
    • 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/10—Computer-aided planning, simulation or modelling of surgical operations
    • 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
    • A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
    • A61B2017/00743—Type of operation; Specification of treatment sites
    • A61B2017/00747—Dermatology
    • A61B2017/00752—Hair removal or transplantation
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
    • A61B2017/00969—Surgical instruments, devices or methods, e.g. tourniquets used for transplantation
    • 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/10—Computer-aided planning, simulation or modelling of surgical operations
    • A61B2034/101—Computer-aided simulation of surgical operations
    • 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/10—Computer-aided planning, simulation or modelling of surgical operations
    • A61B2034/108—Computer aided selection or customisation of medical implants or cutting guides
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30004—Biomedical image processing

Landscapes

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

Abstract

本申请提供了一种针对手术机器人的地图构建方法、装置和设备，涉及SLAM技术领域，该方法包括：获取目标对象的多个时刻的观测结果组；其中，每个观测组包括同一时刻针对目标场景中目标对象的多角度观测结果；对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息和目标对象的位置信息；基于批量优化结果所对应的目标对象的位置信息构建目标场景的场景地图。本申请缓解了传统SLAM精度低于手术机器人所需精度的技术问题，提升了手术机器人的地图构建精度。

Description

针对手术机器人的地图构建方法、装置和设备
技术领域
本申请涉及SLAM技术领域，尤其是涉及一种针对手术机器人的地图构建方法、装置和设备。
背景技术
传统同步定位与建图(Simultaneous Localization and Mapping，简称SLAM)在进行地图构建时，地图通常为固定不变的。例如，区别于传统SLAM场景，在植发SLAM场景下随着植发对象的生理运动(例如呼吸)地图可能发生一定的改变，因此传统SLAM的方式并不适用于手术机器人SLAM场景。并且，手术机器人的运行精度通常比传统SLAM场景下的精度高的多，目前的传统SLAM无法达到手术机器人所需的精度。
发明内容
本申请的目的在于提供一种针对手术机器人的地图构建方法、装置和设备，以缓解了传统SLAM精度低于手术机器人所需精度的技术问题。
第一方面，本发明提供一种针对手术机器人的地图构建方法，方法包括：获取目标对象的多个时刻的观测结果组；其中，每个观测组包括同一时刻针对目标场景中目标对象的多角度观测结果；对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息和目标对象的位置信息；基于批量优化结果所对应的目标对象的位置信息构建目标场景的场景地图。
在可选的实施方式中，观测结果组为目标场景中连续多个目标对象的多角度观测结果；
对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息，包括：获取手术机器人的执行机构的运动方程，以及观测结果组对应的观测方程；基于运动方程确定执行机构的第一运动误差，基于观测方程确定第一观测误差；根据最优化代价函数、第一运动误差和第一观测误差进行批量优化处理，并对批量优化结果进行最优化求解，估计手术机器人的执行机构的位姿信息。
在可选的实施方式中，观测结果组为目标场景中时空邻近的一个或多个目标对象的多角度观测结果；
对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息，包括：确定下一目标对象的时空邻域内的观测结果组，以及执行机构的当前位姿；获取执行机构的运动方程和目标对象的位置信息；基于运动方程确定执行机构的第二运动误差，基于下一目标对象的时空邻域内的观测结果组确定第二观测误差；根据最优化代价函数、第二运动误差和第二观测误差按照时空邻近顺序进行时空滑窗处理，对时空滑窗处理结果进行最优化求解，并根据预先建立的计划方程计算手术机器人的执行机构移动至下一目标对象所对应的目标位姿和执行指令；其中，计划方程通过执行机构的绝对位姿、由于目标对象的运动产生的执行机构的误差偏差、执行机构的移动偏差和修正参数进行表征。
在可选的实施方式中，计划方程为：
其中，uk+1为执行机构移动至下一目标对象所对应的目标位姿，uk+1+δ(uk+1)为执行机构移动至下一目标对象所对应的执行指令，为执行机构的绝对位姿，/>为由于目标对象的运动产生的执行机构的误差偏差，δ(uk+1)为执行机构的移动偏差，ηk+1为修正参数。
在可选的实施方式中，观测方程为手术机器人视觉传感器所对应的观测方程；其中，视觉传感器包括双目相机或多目相机，相应的，观测方程包括双目相机对应的观测方程对或多目相机对应的观测方程集。
在可选的实施方式中，运动方程为通过李代数进行表征的形式：
xk＝f(xk-1,uk)+ωk
其中，xk为观测第k个目标对象时所对应的执行机构的实际位姿；xk-1为观测第k-1个目标对象时所对应的执行机构的实际位姿，uk为每次观测时执行机构的控制信息；ωk为执行机构在实际位置时的运动误差；f为运动方程。
在可选的实施方式中，手术机器人包括植发机器人。
第二方面，本发明提供一种针对手术机器人的地图构建装置，装置包括：观测结果获取模块，用于获取目标对象的多个时刻的观测结果组；其中，每个观测组包括同一时刻针对目标场景中目标对象的多角度观测结果；批量处理模块，用于对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息和目标对象的位置信息；地图构建模块，用于基于批量优化结果所对应的目标对象的位置信息构建目标场景的场景地图。
第三方面，本发明提供一种电子设备，包括处理器和存储器，存储器存储有能够被处理器执行的计算机可执行指令，处理器执行计算机可执行指令以实现前述实施方式任一项的针对手术机器人的地图构建方法。
第四方面，本发明提供一种计算机可读存储介质，计算机可读存储介质存储有计算机可执行指令，计算机可执行指令在被处理器调用和执行时，计算机可执行指令促使处理器实现前述实施方式任一项的针对手术机器人的地图构建方法。
本申请提供的针对手术机器人的地图构建方法、装置和设备，由于多个时刻的观测结果组包含有不同时刻针对目标场景的多角度观测结果，通过对该多个时刻的观测结果组进行批量优化处理，在对执行机构的实际位姿和目标对象的位置信息进行估计时，可以减少因为视差因素所导致的建模错误甚至医疗事故，同时可减少单次观测时的误差所带来的影响，提升了手术机器人的地图构建精度。
附图说明
为了更清楚地说明本申请具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种针对手术机器人的地图构建方法的流程图；
图2为本申请实施例提供的一种全批量式示意图；
图3为本申请实施例提供的一种植发SLAM场景矩阵示意图；
图4为本申请实施例提供的一种时空滑窗式示意图；
图5为本申请实施例提供的一种针对手术机器人的地图构建装置的结构图；
图6为本申请实施例提供的一种电子设备的结构图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
本申请实施例提供了一种针对手术机器人的地图构建方法，参见图1所示，该方法主要包括以下步骤：
步骤S110，获取目标对象的多个时刻的观测结果组；其中，每个观测组包括同一时刻针对目标场景中目标对象的多角度观测结果。
上述目标对象为手术机器人所要执行动作的对象。在一种实施方式中，手术机器人可以包括骨科手术机器人、齿科手术机器人、腹腔镜手术机器人、植发机器人等。
多个时刻的观测组为通过手术机器人所包含的视觉传感器进行观测，多角度观测结果针对同一目标对象的多角度观测结果。视觉传感器包括双目相机或多目相机，当采用双目相机时，多角度观测结果为针对同一目标对象的双角度观测结果；当采用多目相机(例如三目相机)时，多角度观测结果为针对同一目标对象的多角度(三角度-对应三目相机)观测结果。
在实际应用中，上述观测结果是经过畸变校正后的结果。
上述目标场景为手术机器人执行动作所对应的场景，由于应用环境的特殊性，该目标场景通常为非规整的区域，诸如器官、植发区域等。并且，该目标场景会随着生理运动进行一定的变化，例如，在植发场景下，随着呼吸运动，植发区域会产生一定频率的起伏变化，因此本申请实施例所对应的目标场景与传统SLAM场景(诸如自动驾驶、智能家居)存在本质的区别。
步骤S120，对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息和目标对象的位置信息。
在一种实施方式中，批量优化处理可以包含批量处理和优化处理两个处理过程。其中，批量处理可以包括全批量处理和时空滑窗处理；优化处理为最优化处理，诸如可以通过卡尔曼滤波、扩展卡尔曼滤波、深度学习、机器学习等方式。
上述批量优化结果为采用全批量处理或时空滑窗处理后生成的批量结果，其中，全批量处理采用的是多个时刻的观测来估计多个时刻的手术机器人的执行机构的位姿信息，而时空滑窗处理为仅采用最近的几个时刻的观测来估计多个时刻的手术机器人的执行机构的位姿信息。
在一种实施方式中，前述多角度观测结果与目标对象的位置信息具有一定的对应关系。在视觉传感器进行观测时，观测视野中可能包含有一个或多个目标对象，因此通过对观测结果进行优化处理，可以得到每个目标对象的位置坐标以及目标对象之间的相对位置关系。
步骤S130，基于批量优化结果所对应的目标对象的位置信息构建目标场景的场景地图。
在一种实施方式中，当得到目标场景下的批量优化结果所对应的目标对象的位置信息(也即每个目标对象的位置坐标以及目标对象之间的相对位置关系)之后，可以根据该位置信息构建目标场景的场景地图。
由于本申请实施例所对应的目标场景是非规整、且伴随生理运动进行起伏变化的，因此构建的场景地图需要考虑该目标场景的特殊性，通过估计手术机器人执行机构的位姿信息，可以提升手术机器人执行机构在构建的场景地图的执行准确性。
本申请实施例提供的针对手术机器人的地图构建方法，由于多个时刻的观测结果组包含有不同时刻针对目标场景的多角度观测结果，通过对该多个时刻的观测结果组进行批量优化处理，在对执行机构的实际位姿和目标对象的位置信息进行估计时，可以减少因为视差因素所导致的建模错误甚至医疗事故，同时可减少单次观测时的误差所带来的影响，提升了手术机器人的地图构建精度。
为便于理解，以下对本申请所提供的实施例进行详细说明。
本申请实施例在对观测结果进行批量优化时，包括全批量处理和时空滑窗处理，以下分别介绍两种批量处理的具体实施手段。
一、全批量处理
在进行全批量处理时，上述观测结果组为目标场景中连续多个目标对象的多角度观测结果，也即全批量式采用的是多个时刻的观测进行位姿估计。参见图2所示，当采用全批量处理时，待观测的每个目标对象(图2中1、2、3……11、12)均是由全部的其他目标对象确定，也即目标对象1的位姿由1至12的观测确定，目标对象2同样也由1至12的观测确定。该方式可以使得每个目标对象的观测均与其他目标对象之间具有一定的关系，因此得到的位姿信息更加准确，通过对多次观测结果的综合,减少了单次观测时的误差所带来的影响。
上述步骤S120在进行全批量处理时，对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息的具体实施手段包括以下步骤1.1)至步骤1.3)：
步骤1.1)，获取手术机器人的执行机构的运动方程，以及观测结果组对应的观测方程。
步骤1.2)，基于运动方程确定执行机构的第一运动误差，基于观测方程确定第一观测误差。
步骤1.3)，根据最优化代价函数、第一运动误差和第一观测误差进行批量优化处理，并对批量优化结果进行最优化求解，估计手术机器人的执行机构的位姿信息。
针对上述步骤1.1)，假定对某个目标对象进行了n次观测，分别称为ob1,ob2,...,obn，每次观测时执行机构的传感器读数为U1,U2,...,Un，该传感器读数是提前给定的。在一种示例中，上述执行机构可以为手术机器人的机械臂。需要注意的是，机械臂传感器的读数同时也是使机械臂移动时所发出的控制指令，来源于针对执行机构的控制信息。
定义在观测obk时，机械臂末端所在的实际位姿为Xk，则：
Xk＝f(Xk-1,Uk)+ωk
其中，为机械臂传感器在测量末端实际位置时的运动误差，函数f为运动方程。
在批量式SLAM中的运动方程中的运动误差不再是零均了，也即既包含了绝对误差，也包含了重复误差。因此，在此需要对运动误差进行建模，并且在后面的优化中进行求解。
假定在观测obk中观测到的目标对象构成的集合为那么对某一个目标对象/>其观测结果为zk,j，并且记：
zk，j＝h(Xk，yj)+vk，j
其中，为观测误差，上述函数h为观测方程。
可选的，上述观测方程为手术机器人视觉传感器所对应的观测方程；其中，视觉传感器包括双目相机或多目相机，相应的，观测方程包括双目相机对应的观测方程对或多目相机对应的观测方程集。
在一种实施方式中，以手术机器人所使用的视觉传感器为双目相机为例，此时，观测方程则对应为观测方程组，也即：
和
其中，和/>
增量式SLAM的定位场景下，仅考虑连续两次观测之间的关系，也即从obk-1到obk的状态变化，并求解Xk。最终会表示为：
运动方程：Xk＝f(Xk-1,Uk)+ωk。
左相机观测方程：
右相机观测方程：
本申请实施例为了可以进行方便的求导，降低计算的复杂度，提升计算的效率，本申请实施例可以通过李代数的方式进行上述运动方程和观测方程的表示：
通过李代数进行表征运动方程的形式：
xk＝f(xk-1,uk)+ωk
其中，xk为观测第k个目标对象时所对应的执行机构的实际位姿；xk-1为观测第k-1个目标对象时所对应的执行机构的实际位姿，uk为每次观测时执行机构的控制信息，当执行机构为机械臂时，该uk为机械臂末端传感读数；ωk为执行机构在实际位置时的运动误差；f为运动方程。
通过李代数进行表征左相机观测方程的形式：
通过李代数进行表征右相机观测方程的形式：
在另一种实施方式中，以手术机器人所使用的视觉传感器为多目相机为例，此时，观测方程则对应为观测方程集，也即：
……
其中，
优选的，当为多目相机时，观测方程集也可以采用李代数的方式进行表征，以降低计算难度，提升计算速度。
针对上述步骤1.2)，假定当前机械臂末端的实际位姿为xk-1，机械臂传感器的读数为uk，在实际应用中，机械臂传感器的读数可以定义为“为了使末端位姿达到目标的所需机械臂指令(编码器读数/指令)”，也即上述运动方程还可以为下述形式：
xk＝f′(uh)+ωk
其中f′表示机械臂的正向运动模型。
在上述运动方程的基础上，定义运动误差项(也即第一运动误差)：
针对观测误差，为方便说明，以双目相机为例，此时第一观测误差包括左相机观测误差和右相机观测误差，也即，
左相机观测误差：
其中，为左相机观测误差，/>为左相机观测方程，/>为左相机成像平面上的坐标，/>为左相机在机械臂末端中的位姿，yj为目标对象。
右相机观测误差：
其中，为右相机观测误差，/>为右相机观测方程，/>为右相机成像平面上的坐标，/>为右相机在机械臂末端中的位姿，yj为目标对象。
可以理解的是，当采用多目相机时，观测误差则为多个相机的观测误差，计算方式参见上述双目相机的观测误差，此处不再赘述。
针对上述步骤1.3)，所求解的最优化问题的代价函数是：
根据对上述误差项的定义，可知：
1)仅和xk有关，也即仅/>非零，其余的偏导数均为零。
2)仅和xk和yj有关，也即仅/>和/>非零，其余的偏导数均为零。
关于上述问题,有几个补充说明：
1、其中综合了两个相机的观测结果，也即将和/>综合为/>并不是在任何观测k中都可以观测到感兴趣物体j，如果观测不到，则会设定/>
2、虽然使用了来表示观测误差的权重，但是在很多应用场景下并不能准确的设定改权重。例如，在传统自动导航SLAM应用场景下，往往会忽略该权重。此外，在本问题中会将权重/>融入到观测误差项中,也即/>而不是在代价函数中。
在一种实施方式中，在进行最优化求解时，可以构造在某个附近的代价函数的增量形式，参见图3所示，图3中C1、C2、C3、C4为执行机构，t1、t2、t3、t4、t5为目标对象。当应用于植发SLAM场景中，C1、C2、C3、C4为植发机器人机械臂，t1、t2、t3、t4、t5为头发。在线性系统中也可以直接定义雅克比矩阵如下：
如果将待解变量(x和y)合在一起，即ξ＝[x，y]T，则它是一个6n+3m长度的向量。并且观察关于Dk，j、Ek，j和Fk的定义过程，会发现其分别仅与“位姿k和物体j”、“位姿k和物体j”、“位姿k和位姿k-1”相关，并且矩阵形状分别为2×6、2×3和6×6，分别代表其所对应的雅克比矩阵。由此，可以合成雅克比矩阵J为：
其中，Fn＝06×6和q1＝06×6。
在一种实施方式中，可以采用高斯牛顿法对其求解。假设给定的最优化问题的代价函数是并且已知q在ξ的雅克比矩阵为J(ξ)，那么q在ξ处的一阶泰勒展开结果为：
q(ξ+Δξ)≈q(ξ)+J(ξ)TΔξ
那么代价函数Q可以写作是：
对其求导，并令其为零，可以得到：
J(ξ)Tq(ξ)+J(ξ)J(ξ)TΔξ＝0
其结果来自于求解：
HΔξ＝g
其中，H＝J(ξ)J(ξ)T，g＝-J(ξ)Tq(ξ)，其结果为：
Δξ＝H-1g
那么，上述高斯牛顿法的最优化求解过程为：
1.k←0,给定初始值ξk；
2.计算Hk＝J(ξk)J(ξk)T和gk＝-J(ξk)Tq(ξk)；
3.计算
4.更新ξk+1＝ξk+Δξk，k←k+1；
5.如果不收敛,则重复2-4。
对于高斯牛顿法,最后需要计算增量线性方程Δξ＝H-1g，其中H＝JTJ是一个(6n+2m)×(6n+2m)形状的矩阵，并且矩阵具有性质。当其中分块元素(i,j)非零时，对应于i＝j或Jij非零或Jji非零。如此，分为了四种情况：
1.i＝j，要不代表一个位姿，要不代表一次观测结果；
2.i<n且j<n且j＝i,对应于Gi,即i和j分别代表观测的运动方程中的起点位姿和终点位姿；
3.i<n且j>n,对应于Ei,j,即i和j分别代表观测的观测方程中的位姿和物体；
4.i>n且j<n,对应于Di,j,即i和j分别代表观测的观测方程中的物体和位姿。
对Δξ＝H-1g进行求解的过程中，涉及到对高维矩阵的求逆，为了加速该过程，可以使用边缘化(Marginalization)方法来简化。重写其中B和C是分块对角矩阵，E是稀疏矩阵./>
那么，对HΔξ＝g做初等变换后可以得到：
即：
那么，Δx＝[B-EC-1ET]-1(v-EC-1w)以及Δy＝C-1(w-ETΔx)。
其中，B-EC-1ET是一个稠密矩阵，但是其形状为6n×6n，与位置相关，，通常来说远小于目标对象的个数，因此可以直接求逆。
在计算Δy＝C-1(w-ETΔX)中的C-1求逆中，C是一个分块对角矩阵，其求逆过程可以通过实现每一个分块的求逆来实现。
综上，可以通过边缘化来加速Δξ＝H-1g的计算。继而，可以写出对批量式后端的求解过程如下：
1.k←0，给定初始值xk＝f′(uk)，yk为感兴趣物体的大致位置，
2.计算Hk＝J(ξk)J(ξk)T和gk＝-J(ξk)Tq(ξk)；
3.计算其中使用边缘化方法加速计算；
4.更新vk+1＝ξk+Δξk，k←k+1；
5.如果收敛则停止，否则重复2-4步。
通过上述计算可以得到最优化的结果，可以一步步地完成对机械臂末端位置(x)和机械臂控制指令(u)的估计。此外，还可以通过对y的估计，得到对目标对象的估计，也即更新地图。
二、时空滑窗处理
在进行时空滑窗处理时，上述观测结果组为目标场景中时空邻近的一个或多个目标对象的多角度观测结果，时空滑窗处理可以仅采用最近的几个时刻的观测进行位姿估计。当采用时空滑窗处理时，待观测的每个目标对象由其时空邻近的其他目标对象的观测确定。参见图4所示，以目标对象7为例，其可以由时空邻近的目标对象6、2、5、1、3的观测确定。
上述步骤S120在进行全批量处理时，对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息的具体实施手段包括以下步骤2.1)至步骤2.4)：
步骤2.1)，确定下一目标对象的时空邻域内的观测结果组，以及执行机构的当前位姿；
步骤2.2)，获取执行机构的运动方程和目标对象的位置信息；
步骤2.3)，基于运动方程确定执行机构的第二运动误差，基于下一目标对象的时空邻域内的观测结果组确定第二观测误差；
步骤2.4)，根据最优化代价函数、第二运动误差和第二观测误差按照时空邻近顺序进行时空滑窗处理，对时空滑窗处理结果进行最优化求解，并根据预先建立的计划方程计算手术机器人的执行机构移动至下一目标对象所对应的目标位姿和执行指令；其中，计划方程通过执行机构的绝对位姿(该位姿为计算得到的位姿信息，通常具有一定的误差)、由于目标对象的运动产生的执行机构的误差偏差、执行机构的移动偏差和修正参数进行表征。
针对上述步骤2.1)，在植发机器人场景下,所使用的滑动窗口指的是“时空滑窗”，也即，对于下一个下针目标，估计机械臂末端实际位姿，或者说所需的机械臂指令uk时，所使用的观测和机械臂末端实际位姿是和该目标在时间或者空间上临近的，包含：
1.该目标的上一个或者上几个观测结果；
2.之前观测时,与当前目标临近的其他目标的观测结果。
针对上述步骤2.2)和步骤2.3)，其具体操作参见前述全批量处理的操作方式，此处不再赘述。
针对上述步骤2.4)，滑动窗口式后端的使用场景下，会需要对下一目标所对应的机械臂传感器读数(指令)进行估计，该估计的结果用于对下次目标的移动。该时空滑窗处理相对于全批量处理的方式，加入一个计划方程。
在一种实施方式中，机械臂传感器的读数可以定义为“为了使末端位姿达到目标的所需机械臂指令(编码器读数/指令)”，而目标机械臂末端位置由y中的一个目标队形的三维坐标给出(植发场景下对应下一个目标头发)，假定对其下针所需的绝对机械臂位姿为(这一点和传统SLAM下一位置直接由目标对象的空间位置直接给出有很大不同)。则计划方程为：
其中，uk+1为执行机构移动至下一目标对象所对应的目标位姿，uk+1+δ(uk+1)为执行机构移动至下一目标对象所对应的执行指令，为执行机构的绝对位姿，/>为由于目标对象的运动产生的执行机构的误差偏差，δ(uk+1)为执行机构的移动偏差，ηk+1为修正参数。
在可选的实施方式中，对上述δ(uk+1)的处理方式有两种，一种是建立uk+1-δ(uk+1)和uk+1之间的一一对应表，即由标定给出；另一种是将δ(uk+1)近似为δ(uk)+Δδ(uk)，其中δ(uk)由前面给出，Δδ(uk)在完成移动后，根据其与实际目标之间的偏差给出。以下针对第二种方式进行详细说明，具体来说：
对的计算：假定下一目标头发为yk+1，对其下针所需位姿可以通过反解正向运动方程f′给出，此处不再赘述。
对的计算：其表示对感兴趣物体移动的修正项，例如呼吸运动或者患者本身的移动.可以通过呼吸运动补偿算法来计算。实际上，其所估计的是yk+1在下针时的位置，并反解出/>
对δ(uk+1)：将其近似为δ(uk)+Δδ(uk)，其中δ(uk)表示上一次移动时所使用的偏差，并且δ(u0)＝0，Δδ(uk)表示上次移动后的仍存在的误差。在计算此计划方程时已知。
因此，针对时空滑窗式的批量处理，在一种具体实施手段中，无需对之前的运动方程和观测方程修改，具体实施过程如下：
0.确定下一目标的时空邻域内的观测和机械臂末端实际位姿；
1.k←0,给定初始值xk＝f′(uk),yk为感兴趣物体的大致位置,
2.计算Hk＝J(ξk)J(ξk)T和gk＝-J(ξk)Tq(ξk)；
3.计算其中使用边缘化方法加速计算；
4.更新ξk+1＝ξk+Δξk,k←k+1；
5.如果收敛则停止,否则重复2-4步。
6.根据计划方程设计下一目标所需的机械臂指令uk+1+δ(uk+1),其中涉及到对邻域内的偏差δ(u)和Δδ(u)的计算.
本申请提供的上述战队手术机器人的地图构建方法，可以应用于手术机器人为植发机器人的具体实施例，结合上述方法的具体实施流程，前述执行机构为植发机器人机械臂，目标对象为头发(或发根部分)，目标场景为头皮植发区域(该区域通常为非规整的)。
综上，本申请可以通过采用全批量处理和时空滑窗处理进行批量式的估计和优化处理，减少了因为机械臂移动误差和感兴趣物体移动所导致的不准确性，并进一步减少了因为机械臂一次性移动不到位所导致的计算时间延长(即延长下针时间)。
基于上述方法实施例，本申请实施例还提供一种针对手术机器人的地图构建装置，参见图5所示，该装置主要包括以下部分：
观测结果获取模块510，用于获取目标对象的多个时刻的观测结果组；其中，每个观测组包括同一时刻针对目标场景中目标对象的多角度观测结果；
批量处理模块520，用于对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息和目标对象的位置信息；
地图构建模块530，用于基于批量优化结果所对应的目标对象的位置信息构建目标场景的场景地图。
在一可行的实施方式中，观测结果组为目标场景中连续多个目标对象的多角度观测结果；
上述批量处理模块520，还用于：获取手术机器人的执行机构的运动方程，以及观测结果组对应的观测方程；基于运动方程确定执行机构的第一运动误差，基于观测方程确定第一观测误差；根据最优化代价函数、第一运动误差和第一观测误差进行批量优化处理，并对批量优化结果进行最优化求解，估计手术机器人的执行机构的位姿信息。
在一可行的实施方式中，观测结果组为目标场景中时空邻近的一个或多个目标对象的多角度观测结果；
上述批量处理模块520，还用于：
确定下一目标对象的时空邻域内的观测结果组，以及执行机构的当前位姿；获取执行机构的运动方程和目标对象的位置信息；基于运动方程确定执行机构的第二运动误差，基于下一目标对象的时空邻域内的观测结果组确定第二观测误差；根据最优化代价函数、第二运动误差和第二观测误差按照时空邻近顺序进行时空滑窗处理，对时空滑窗处理结果进行最优化求解，并根据预先建立的计划方程计算手术机器人的执行机构移动至下一目标对象所对应的目标位姿和执行指令；其中，计划方程通过执行机构的绝对位姿、由于目标对象的运动产生的执行机构的误差偏差、执行机构的移动偏差和修正参数进行表征。
在一可行的实施方式中，计划方程为：
其中，uk+1为执行机构移动至下一目标对象所对应的目标位姿，uk+1+δ(uk+1)为执行机构移动至下一目标对象所对应的执行指令，为执行机构的绝对位姿，/>为由于目标对象的运动产生的执行机构的误差偏差，δ(uk+1)为执行机构的移动偏差，ηk+1为修正参数。
在一可行的实施方式中，观测方程为手术机器人视觉传感器所对应的观测方程；其中，视觉传感器包括双目相机或多目相机，相应的，观测方程包括双目相机对应的观测方程对或多目相机对应的观测方程集。
在一可行的实施方式中，运动方程为通过李代数进行表征的形式：
xk＝f(xk-1,uk)+ωk
其中，xk为观测第k个目标对象时所对应的执行机构的实际位姿；xk-1为观测第k-1个目标对象时所对应的执行机构的实际位姿，uk为每次观测时执行机构的控制信息；ωk为执行机构在实际位置时的运动误差；f为运动方程。
在一可行的实施方式中，手术机器人包括植发机器人。
本申请实施例提供的针对手术机器人的地图构建装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，针对手术机器人的地图构建装置的实施例部分未提及之处，可参考前述针对手术机器人的地图构建方法实施例中相应内容。
本申请实施例还提供了一种电子设备，如图6所示，为该电子设备的结构示意图，其中，该电子设备100包括处理器61和存储器60，该存储器60存储有能够被该处理器61执行的计算机可执行指令，该处理器61执行该计算机可执行指令以实现上述任一项针对手术机器人的地图构建方法。
在图6示出的实施方式中，该电子设备还包括总线62和通信接口63，其中，处理器61、通信接口63和存储器60通过总线62连接。
其中，存储器60可能包含高速随机存取存储器(RAM，Random Access Memory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。通过至少一个通信接口63(可以是有线或者无线)实现该系统网元与至少一个其他网元之间的通信连接，可以使用互联网，广域网，本地网，城域网等。总线62可以是ISA(IndustryStandard Architecture，工业标准体系结构)总线、PCI(Peripheral ComponentInterconnect，外设部件互连标准)总线或EISA(Extended Industry StandardArchitecture，扩展工业标准结构)总线等。所述总线62可以分为地址总线、数据总线、控制总线等。为便于表示，图6中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。
处理器61可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器61中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器61可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital SignalProcessor，简称DSP)、专用集成电路(Application Specific Integrated Circuit，简称ASIC)、现场可编程门阵列(Field-Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器61读取存储器中的信息，结合其硬件完成前述实施例的针对手术机器人的地图构建方法的步骤。
本申请实施例还提供了一种计算机可读存储介质，该计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令在被处理器调用和执行时，该计算机可执行指令促使处理器实现上述针对手术机器人的地图构建方法，具体实现可参见前述方法实施例，在此不再赘述。
本申请实施例所提供的针对手术机器人的地图构建方法、装置和设备的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见方法实施例，在此不再赘述。
除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对步骤、数字表达式和数值并不限制本申请的范围。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个处理器可执行的非易失的计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在本申请的描述中，需要说明的是，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
最后应说明的是：以上各实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述各实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。

Claims (10)

1.一种针对手术机器人的地图构建方法，其特征在于，所述方法包括：
获取目标对象的多个时刻的观测结果组；其中，每个观测组包括同一时刻针对目标场景中目标对象的多角度观测结果；
对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息和所述目标对象的位置信息；
基于批量优化结果所对应的目标对象的位置信息构建所述目标场景的场景地图。
2.根据权利要求1所述的针对手术机器人的地图构建方法，其特征在于，所述观测结果组为目标场景中连续多个目标对象的多角度观测结果；
对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息，包括：
获取所述手术机器人的执行机构的运动方程，以及所述观测结果组对应的观测方程；
基于所述运动方程确定所述执行机构的第一运动误差，基于所述观测方程确定第一观测误差；
根据最优化代价函数、所述第一运动误差和所述第一观测误差进行批量优化处理，并对批量优化结果进行最优化求解，估计手术机器人的执行机构的位姿信息。
3.根据权利要求2所述的针对手术机器人的地图构建方法，其特征在于，所述观测结果组为目标场景中时空邻近的一个或多个目标对象的多角度观测结果；
对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息，包括：
确定下一目标对象的时空邻域内的观测结果组，以及执行机构的当前位姿；
获取执行机构的运动方程和目标对象的位置信息；
基于所述运动方程确定所述执行机构的第二运动误差，基于下一目标对象的时空邻域内的观测结果组确定第二观测误差；
根据最优化代价函数、所述第二运动误差和所述第二观测误差按照时空邻近顺序进行时空滑窗处理，对时空滑窗处理结果进行最优化求解，并根据预先建立的计划方程计算手术机器人的执行机构移动至下一目标对象所对应的目标位姿和执行指令；其中，所述计划方程通过执行机构的绝对位姿、由于目标对象的运动产生的执行机构的误差偏差、执行机构的移动偏差和修正参数进行表征。
4.根据权利要求3所述的针对手术机器人的地图构建方法，其特征在于，所述计划方程为：
其中，uk+1为执行机构移动至下一目标对象所对应的目标位姿，uk+1+δ(uk+1)为执行机构移动至下一目标对象所对应的执行指令，为执行机构的绝对位姿，/>为由于目标对象的运动产生的执行机构的误差偏差，δ(uk+1)为执行机构的移动偏差，ηk+1为修正参数。
5.根据权利要求3所述的针对手术机器人的地图构建方法，其特征在于，所述观测方程为手术机器人视觉传感器所对应的观测方程；其中，所述视觉传感器包括双目相机或多目相机，相应的，所述观测方程包括双目相机对应的观测方程对或多目相机对应的观测方程集。
6.根据权利要求3所述的针对手术机器人的地图构建方法，其特征在于，所述运动方程为通过李代数进行表征的形式：
xk＝f(xk-1，uk)+ωk
其中，xk为观测第k个目标对象时所对应的执行机构的实际位姿；xk-1为观测第k-1个目标对象时所对应的执行机构的实际位姿，uk为每次观测时所述执行机构的控制信息；ωk为执行机构在实际位置时的运动误差；f为运动方程。
7.根据权利要求1至6任一项所述的针对手术机器人的地图构建方法，其特征在于，所述手术机器人包括植发机器人。
8.一种针对手术机器人的地图构建装置，其特征在于，所述装置包括：
观测结果获取模块，用于获取目标对象的多个时刻的观测结果组；其中，每个观测组包括同一时刻针对目标场景中目标对象的多角度观测结果；
批量处理模块，用于对多个观测结果组进行批量优化处理，基于批量优化结果估计手术机器人的执行机构的位姿信息和所述目标对象的位置信息；
地图构建模块，用于基于批量优化结果所对应的目标对象的位置信息构建所述目标场景的场景地图。
9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机可执行指令，所述处理器执行所述计算机可执行指令以实现权利要求1至7任一项所述的针对手术机器人的地图构建方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，计算机可执行指令促使处理器实现权利要求1至7任一项所述的针对手术机器人的地图构建方法。
CN202311586359.7A 2023-11-24 2023-11-24 针对手术机器人的地图构建方法、装置和设备 Pending CN117562657A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311586359.7A CN117562657A (zh) 2023-11-24 2023-11-24 针对手术机器人的地图构建方法、装置和设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311586359.7A CN117562657A (zh) 2023-11-24 2023-11-24 针对手术机器人的地图构建方法、装置和设备

Publications (1)

Publication Number Publication Date
CN117562657A true CN117562657A (zh) 2024-02-20

Family

ID=89863939

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311586359.7A Pending CN117562657A (zh) 2023-11-24 2023-11-24 针对手术机器人的地图构建方法、装置和设备

Country Status (1)

Country Link
CN (1) CN117562657A (zh)

Similar Documents

Publication Publication Date Title
Zhang et al. Convergence and consistency analysis for a 3-D invariant-EKF SLAM
CN108876815B (zh) 骨骼姿态计算方法、人物虚拟模型驱动方法及存储介质
Cifuentes et al. Probabilistic articulated real-time tracking for robot manipulation
KR102050895B1 (ko) 로봇의 캘리브레이션 및 프로그래밍
JP7515795B2 (ja) 画像処理システム、及び、画像処理方法、並びに、画像処理プログラム
CN108896047B (zh) 分布式传感器网络协同融合与传感器位置修正方法
JP2019166626A (ja) 制御装置及び機械学習装置
CN110118556A (zh) 一种基于协方差交叉融合slam的机器人定位方法及装置
US11415433B2 (en) Method for calibrating a multi-sensor system using an artificial neural network
CN112767493B (zh) Stewart平台运动学参数的机器视觉标定方法
CN113777593B (zh) 基于伺服电机辅助运动的多激光雷达外参标定方法和装置
KR20220159895A (ko) 카메라 가이드된 기구의 핸드-아이 캘리브레이션 방법
CN114310901A (zh) 用于机器人的坐标系标定方法、装置、系统以及介质
CN116672031B (zh) 机器人的控制方法、装置、处理器及电子设备
CN117562657A (zh) 针对手术机器人的地图构建方法、装置和设备
CN108345463B (zh) 基于机器人的三维测量方法、装置、机器人和存储介质
CN114310873B (zh) 位姿转换模型的生成方法、控制方法、系统、设备和介质
CN116533230A (zh) 机器人的标定方法、装置及电子设备
JP7497188B2 (ja) 校正装置、訓練装置、及びコンピュータ・プログラム
CN110007601B (zh) 一种双边遥操作系统的控制装置和设备
JP7423387B2 (ja) 較正システム、情報処理システム、ロボット制御システム、較正方法、情報処理方法、ロボット制御方法、較正プログラム、情報処理プログラム、較正装置、情報処理装置、及びロボット制御装置
CN113439013B (zh) 机器人系统，机器人的控制装置，以及机器人的控制程序
CN116917093A (zh) 模拟装置、控制系统以及建模方法
CN116601670A (zh) 用于对跨医学图像中的变化进行域泛化的系统和方法
JP2022138037A (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