CN114886567B - 一种面向具有远心不动点约束的手术机器人手眼标定方法 - Google Patents

一种面向具有远心不动点约束的手术机器人手眼标定方法 Download PDF

Info

Publication number
CN114886567B
CN114886567B CN202210532505.7A CN202210532505A CN114886567B CN 114886567 B CN114886567 B CN 114886567B CN 202210532505 A CN202210532505 A CN 202210532505A CN 114886567 B CN114886567 B CN 114886567B
Authority
CN
China
Prior art keywords
surgical instrument
positioning
surgical
hand
under
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
Application number
CN202210532505.7A
Other languages
English (en)
Other versions
CN114886567A (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.)
Suzhou University
Original Assignee
Suzhou University
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 Suzhou University filed Critical Suzhou University
Priority to CN202210532505.7A priority Critical patent/CN114886567B/zh
Publication of CN114886567A publication Critical patent/CN114886567A/zh
Application granted granted Critical
Publication of CN114886567B publication Critical patent/CN114886567B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots
    • 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/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

本发明公开了一种面向具有远心不动点约束的手术机器人手眼标定方法。通过相机图像计算手术器械的三维位姿，并根据远心不动点结构约束构建损失函数，实现对多种摆位下手术器械空间姿态的同步式优化，利用其独有的远心不动点结构特征，通过手术器械在机器坐标系下的姿态和优化后的手术器械中心线的公垂点位置实现手眼关系的计算，与传统方法相比能够大幅提升手术机器人手眼标定的精度，解决了运用传统方法进行手眼标定时因腹腔镜手术机器人运动空间狭窄、姿态变化较小而导致的病态矩阵问题，从而为图像引导的手术任务自主化发展提供了技术基础。

Description

一种面向具有远心不动点约束的手术机器人手眼标定方法
技术领域
本发明涉及医疗手术机器人的手眼标定领域，特别是涉及一种面向具有远心不动点约束的手术机器人手眼标定方法。
背景技术
随着现代科学技术的进步，以直觉手术公司达芬奇机器人为代表的手术机器人平台在过去二十年间得到了长足的发展。随着硬件平台与软件系统的日趋成熟，近年来，利用视觉信息实现手术机器人任务操作自主化正在成为一个新兴的研究方向。在此类研究中，需要基于相机的反馈信息对手术视野中的关键器械与组织进行识别、分割与重建，并将相机坐标系下的感知信息转换至机器人坐标系，进而引导机器人进行自主化的任务规划与操作。获取相机三维坐标系与机器人坐标系间转换关系的这一过程，便称为机器人的手眼标定。由于腹腔镜手术机器人在结构上具有远心不动点的运动约束，现有的手眼标定技术难以有效且准确的获取手术机器人的手眼关系。
现有的手眼标定技术主要包括以下几类方法：
1、基于AX＝XB模型的手眼标定方法：还包括由传统AX＝XB模型中衍生处的标定方法，这种模式下的校准方法需要广泛使用棋盘格来进行手眼标定，而且基于AX＝XB原理进行手眼关系求解时，需要采集的机器人正向运动学信息之间存在明显差异，因腹腔镜手术机器人存在远心不动点的结构限制，手眼标定过程中手术器械的位姿变化小、移动范围受限，求解时极易出现病态矩阵的问题，难以保证手术机器人的校准精度；
2、基于CAD模型的标定方法：将手术工具的CAD模型与量化梯度方向检测到的图像特征相结合，可以动态校正手眼标定误差。然而CAD模型的设计、加载与计算会导致整体的手眼校准时间变长，从而降低了手眼标定效率。除此之外，此类方法中模型追踪误差也会给手眼标定的结果带来不确定性。
3、基于深度学习的标定方法：该方法需要收集大量校准数据，会产生手眼关系过拟合的问题，使得算法的通用性降低，并且此方法的手眼标定耗时长、效率低。
4、使用外部传感器的标定方法：使用外部传感器(如光学追踪传感器)连接到机器人的关键节点上，得到各关节之间的转换关系。但外部传感器需要频繁的自我校准，以确保其自身的传感可靠性。因此，运用此方式会增加系统的复杂性，同时整体设备的成本会大幅提高，实用性较差。
综上所述可以看出，针对于腹腔镜手术机器人，现有的手眼标定方法的精度较低。
发明内容
本发明的目的是提供一种面向具有远心不动点约束的手术机器人手眼标定方法，以解决现有手眼标定方法对于腹腔镜手术机器人手眼标定精度低的问题。
为解决上述技术问题，本发明提供一种面向具有远心不动点约束的手术机器人手眼标定方法，包括：
获取多次摆位下手术器械在机器坐标系下的姿态；根据相机拍摄到的图像数据确定多次摆位下手术器械在相机坐标系下的三维位姿；
利用所述手术器械在相机坐标系下的三维位姿，计算相机坐标系下每次摆位与其他各摆位的手术器械中心线的公垂点位置；
根据远心不动点结构约束，利用所述多次摆位下手术器械在相机坐标系下的三维位姿和所述每次摆位与其他各摆位的手术器械中心线的公垂点位置构建损失函数，对所述损失函数进行迭代优化，当相邻两次迭代优化后的损失函数差值满足预设值时确定每次摆位下手术器械在相机坐标系下的最优三维位姿；
根据所述最优三维位姿计算每次摆位与其它各摆位的手术器械中心线的最优公垂点位置；
根据所述多次摆位下手术器械在机器坐标系下的姿态和所述每次摆位下与各摆位的手术器械中心线的最优公垂点位置，确定手术器械的手眼关系。
优选地，所述根据相机拍摄到的图像数据确定多次摆位下手术器械在相机坐标系下的三维位姿包括：
确定第i次摆位下的手术器械在相机拍摄图像上的上下边界，并计算第i次摆位下手术器械的边界矩阵Cbi：
Figure BDA0003640833330000031
式中，
Figure BDA0003640833330000032
和/>
Figure BDA0003640833330000033
分别为第i次摆位下手术器械在相机拍摄的图像上的上边界和下边界，i＝1,2,…,k，k为手术器械的摆位总次数；
对第i次摆位下手术器械的边界矩阵Cbi根据相机内参构建矩阵
Figure BDA0003640833330000034
并进行奇异值分解：
Figure BDA0003640833330000035
式中，Kc为相机内参，
Figure BDA0003640833330000036
为第i次摆位下矩阵/>
Figure BDA0003640833330000037
奇异值分解后的对角矩阵，/>
Figure BDA0003640833330000038
主对角线上的每个值为分解后的奇异值，/>
Figure BDA0003640833330000039
为第i次摆位下矩阵/>
Figure BDA00036408333300000310
奇异值分解后的左奇异矩阵，/>
Figure BDA00036408333300000311
为第i次摆位下矩阵/>
Figure BDA00036408333300000312
奇异值分解后的右奇异矩阵；
选择
Figure BDA00036408333300000313
中的两个最大值∑i1与∑i2，计算第i次摆位手术器械在相机坐标系下的三维位姿/>
Figure BDA00036408333300000314
Figure BDA00036408333300000315
Figure BDA00036408333300000316
式中，
Figure BDA00036408333300000317
为第i次摆位下手术器械在相机坐标系下的三维位姿即普吕克坐标，/>
Figure BDA00036408333300000318
表示取/>
Figure BDA00036408333300000319
的最后一列，rC为手术器械末端执行器的半径长度，
Figure BDA00036408333300000320
为取/>
Figure BDA00036408333300000321
的第一列，∑i1与∑i2分别为/>
Figure BDA00036408333300000322
对角线上的前两个元素且∑i1>∑i2。
优选地，所述计算相机坐标系下每次摆位下与其他各摆位的手术器械中心线的公垂点位置包括：
Figure BDA00036408333300000323
Figure BDA00036408333300000324
式中，
Figure BDA0003640833330000041
为手术器械第i次摆位与第j次摆位的中心线在第i次摆位中心线上的公垂点位置，/>
Figure BDA0003640833330000042
为手术器械第j次摆位与第i次摆位的中心线在第j次摆位中心线的公垂点位置，/>
Figure BDA0003640833330000043
为手术器械第i次摆位的普吕克坐标，/>
Figure BDA0003640833330000044
为手术器械第j次摆位的普吕克坐标，i＝1,2,…,k，k为手术器械的摆位总次数。
优选地，所述确定每次摆位下手术器械在相机坐标系下的最优三维位姿包括：
利用多次摆位下手术器械在相机坐标系下的三维位姿和每次摆位与其它各摆位的手术器械中心线的公垂点位置构建损失函数LSAS：
Figure BDA0003640833330000045
式中，
Figure BDA0003640833330000046
为相机坐标系下第i次摆位的手术器械进行优化后的普吕克坐标，/>
Figure BDA0003640833330000047
为第i次摆位手术器械在相机坐标系下的普吕克坐标，[*]×代表矩阵*的斜对称矩阵，/>
Figure BDA0003640833330000048
为手术器械第i次摆位与第j次摆位的中心线在第i次摆位中心线上的公垂点位置，/>
Figure BDA0003640833330000049
为手术器械第j次摆位与第i次摆位的中心线在第j次摆位中心线的公垂点位置，i＝1,2,…,k，k为手术器械的摆位总次数；
对LSAS进行迭代式优化，当相邻两次迭代后的LSAS的差值小于预设值时，确定最后一次迭代时优化后的普吕克坐标
Figure BDA00036408333300000410
为相机坐标系下多次摆位的手术器械的最优三维位姿。
优选地，所述确定手术器械的手眼关系包括计算手眼关系的平移矩阵和计算手眼关系的旋转矩阵。
优选地，所述计算手眼关系的平移矩阵为：
Figure BDA00036408333300000411
式中，Ctr为手眼关系的平移矩阵，n＝i·j，i,j∈[1,k]，Cpn为第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置，Cpn,n＝
Figure BDA0003640833330000051
为利用卡尔曼滤波得到。
优选地，所述第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置为：
利用所述相机坐标系下第i次摆位的手术器械的最优普吕克坐标，计算第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置
Figure BDA0003640833330000052
与/>
Figure BDA0003640833330000053
根据所述第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置
Figure BDA0003640833330000054
与
Figure BDA0003640833330000055
求解第i次摆位与第j次摆的手术器械中心线的最优公垂点的中间点位置Cpn：
Figure BDA0003640833330000056
式中，n＝i·j，i,j∈[1,k]，k为手术器械的摆位总次数。
优选地，所述计算手眼关系的旋转矩阵包括：
根据获取的多次摆位下手术器械在机器坐标系下的姿态rRe(i)，提取每次摆位下的纵向轴信息
Figure BDA0003640833330000057
并利用纵向轴信息/>
Figure BDA0003640833330000058
和最优普吕克坐标中的/>
Figure BDA0003640833330000059
计算Ur：
Figure BDA00036408333300000510
对Ur进行奇异值分解：
Figure BDA00036408333300000511
式中，∑ro为Ur奇异值分解后的对角矩阵，∑ro主对角线上的每个值为分解后的奇异值，Uro为Ur奇异值分解后的左奇异矩阵，
Figure BDA00036408333300000512
为Ur奇异值分解后的右奇异矩阵；
根据Ur奇异值分解后的左奇异矩阵和右奇异矩阵计算手眼关系中的旋转矩阵CRr：
Figure BDA00036408333300000513
优选地，所述确定第i次摆位下的手术器械在相机拍摄图像上的上下边界包括：
在第i次摆位下的手术器械边界在相机拍摄图像上随机采样多个关键点，其中上下边界随机采样关键点的个数均不小于2个；
根据拟合算法分别对上边界和下边界的关键点进行拟合，得到拟合的上边界线和下边界线作为手术器械在相机拍摄图像上的上下边界。
本发明还提供了一种手术机器人，所述手术机器人采用如上所述的面向具有远心不动点约束的手术机器人手眼标定方法的步骤实现手眼标定。
本发明相比于现有技术的有益效果为：
本发明通过相机图像计算手术器械的三维位姿，并根据远心不动点结构约束构建损失函数，实现对多种摆位下手术器械空间姿态的同步式优化，利用其独有的远心不动点结构特征，通过手术器械在机器坐标系下的姿态和优化后的手术器械中心线的公垂点位置实现手眼关系的计算，与传统方法相比能够大幅提升手术机器人手眼标定的精度，解决了运用传统方法进行手眼标定时因腹腔镜手术机器人运动空间狭窄、姿态变化较小而导致的病态矩阵问题，从而为图像引导的手术任务自主化发展提供了技术基础。
附图说明
为了更清楚的说明本发明实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的面向具有远心不动点约束的手术机器人手眼标定方法的一种具体实施例的流程图；
图2为本发明实施例中基于边界采样关键点的手术器械普吕克坐标确定示意图；
图3为本发明实施例中基于手术器械普吕克坐标的手眼关系校准示意图。
具体实施方式
本发明的核心是提供一种面向具有远心不动点约束的手术机器人手眼标定方法，主要应用于具有远心不动点的各类腹腔镜手术机器人。本发明确定出相机坐标系下器械的三维位姿，考虑到腹腔镜手术机器人存在运动空间狭窄、姿态变化较小等约束，利用远心不动点特性构建损失函数，通过计算器械在多种摆位下的公垂点，可得到手眼关系中的平移矩阵，结合器械末端执行器的正运动学信息可计算出对应的旋转矩阵。相比于与传统的AX＝XB方法相比，可避免受限空间下手眼标定的病态矩阵问题，并且比现有的腹腔镜手术机器人手眼标定方法具有更高的精度，可为精细化的手术自主化操作提供精准化的信息引导。
为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参考图1，图1为本发明所提供的面向具有远心不动点约束的手术机器人手眼标定方法的一种具体实施例的流程图；本实施例以腹腔镜手术机器人的手眼标定为例，具体操作过程如下：
步骤1：确定任意k次摆位下手术器械的三维位姿；
腹腔镜手术机器人在结构上具有远心不动点的运动约束，手术器械绕远心不动点进行任意摆位，利用相机采集腹腔镜的图像数据，计算手术器械的末端执行器在相机坐标系下的三维位姿。基于机器人正运动学，记录下末端执行器在机器人坐标系下的姿态，如图2所示，手术器械的三维姿态可以用其中心线的普吕克坐标表示：
Figure BDA0003640833330000071
Figure BDA0003640833330000072
式中，
Figure BDA0003640833330000081
代表手术器械的空间方向单位向量，/>
Figure BDA0003640833330000082
代表相机原点至手术执行器中心线的垂足。
基于腹腔镜的单目图像反馈手术器械的空间姿态
Figure BDA00036408333300000822
的确定过程如下：
S101：在手术器械边界随时采样6个关键点，快速拟合出器械在图像层面的上边界
Figure BDA00036408333300000820
与下边界/>
Figure BDA00036408333300000821
作为其他实施方式，采样关键点的个数可以根据实际情况进行设定，只需要满足上边界和下边界各至少有两个采样点即可；当某一边界只有两个采样关键点时，直接两点共线确定边界线，当某一边界的采样关键点大于等于3个时，利用拟合算法拟合出边界线，例如最小二乘算法、多项式拟合、线性拟合等。
S102：根据拟合出的上下边界线计算手术器械的边界矩阵Cbi：
Figure BDA0003640833330000083
其中，i＝1,2,…,k，k为手术器械的摆位总次数；
S103：构建矩阵
Figure BDA0003640833330000084
并进行奇异值分解：
Figure BDA0003640833330000085
式中，Kc为相机内参，
Figure BDA0003640833330000086
为第i次摆位下矩阵/>
Figure BDA0003640833330000087
奇异值分解后的对角矩阵，/>
Figure BDA0003640833330000088
主对角线上的每个值为分解后的奇异值，/>
Figure BDA0003640833330000089
为第i次摆位下矩阵/>
Figure BDA00036408333300000810
奇异值分解后的左奇异矩阵，/>
Figure BDA00036408333300000811
为第i次摆位下矩阵/>
Figure BDA00036408333300000812
奇异值分解后的右奇异矩阵；
S104：计算手术器械在相机坐标系下的普吕克坐标：
Figure BDA00036408333300000813
Figure BDA00036408333300000814
式中，
Figure BDA00036408333300000823
为第i次摆位下手术器械在相机坐标系下的普吕克坐标/>
Figure BDA00036408333300000815
表示取/>
Figure BDA00036408333300000816
的最后一列，rC为手术器械末端执行器的半径长度，/>
Figure BDA00036408333300000817
为取/>
Figure BDA00036408333300000818
的第一列，∑i1与∑i2分别为/>
Figure BDA00036408333300000819
对角线上的前两个元素且∑i1>∑i2；
通过上述过程可以确定k次摆动下手术器械在相机左边系下的普吕克坐标。在本实施例中，k取值为8，作为其他实施方式，k值可以根据实际情况确定。
步骤S2：确定任意两次摆位手术器械中心线间的公垂点；
Figure BDA0003640833330000091
Figure BDA0003640833330000092
式中，
Figure BDA0003640833330000093
为手术器械第i次摆位与第j次摆位的中心线的公垂线在第i次摆位中心线上的垂足位置，即公垂点位置，/>
Figure BDA0003640833330000094
为手术器械第j次摆位与第i次摆位的中心线的公垂线在第j次摆位中心线的公垂足位置，即公垂点位置。
步骤S3：构建损失函数，确定任意摆位下的最优位姿；
理想情况下，因腹腔镜手术机器人具有远心不动点的结构约束，所得到的多种摆位下器械的中心线应交于同一点，原则上说，每两次摆位下计算得到的
Figure BDA0003640833330000095
和/>
Figure BDA0003640833330000096
应该是位于同一点，所以在构建损失函数时，/>
Figure BDA0003640833330000097
和/>
Figure BDA0003640833330000098
的距离越小越好，但如果仅利用这一原理去优化不同摆位下手术器械的普吕克坐标，可能会使手术器械的三维位姿和方向发生改变，因此构建损失函数时要尽量保证不改变手术器械的三维位姿和方向，所以构建的损失函数为：
Figure BDA0003640833330000099
式中，
Figure BDA00036408333300000910
与/>
Figure BDA00036408333300000911
代表相机坐标系下的第i次摆位下手术器械的优化后的普吕克坐标；[*]×代表矩阵*的斜对称矩阵，/>
Figure BDA00036408333300000912
与/>
Figure BDA00036408333300000913
运用梯度下降法进行优化；
对LSAS进行迭代式优化，当相邻两次迭代后的LSAS的差值小于预设值时，确定最后一次迭代时优化后的普吕克坐标
Figure BDA0003640833330000101
为相机坐标系下多次摆位的手术器械的最优三维位姿，在本实施例中预设值为1％-3％，作为其他实施方式，可以根据实际情况进行设定。
本实施例基于手术器械远心不动点的机械结构特征建立约束函数，考虑到腹腔手术机器人存在运动空间狭窄、姿态变化较小等约束，充分利用手术器械的远心不动点特征，优化手术器械不同摆位下的三维位姿，以提高后续校准精度。
步骤S4：计算每次摆位下手术器械中心线的最优公垂点位置，求解中间点；
利用最优普吕克坐标值计算每次摆位下与其它各摆位的中心线公垂点在相机坐标系下的最优位置
Figure BDA0003640833330000102
和/>
Figure BDA0003640833330000103
根据第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置，求解第i次和第j次两次摆位下最优公垂点的中间点位置Cpn：
Figure BDA0003640833330000104
式中，i,j∈[1,k],n＝i·j，
Figure BDA0003640833330000105
与/>
Figure BDA0003640833330000106
代表运用最优普吕克坐标计算的中心线间互相关垂足在相机坐标系下的三维位置。
步骤S5：计算手眼关系；
根据手术器械在机器坐标系下的姿态、优化后的各摆位下手术器械中心线的公垂点位置，确定手术器械的手眼关系，包括平移矩阵和旋转矩阵；本实施例基于手术器械远心不动点的机械结构特征建立约束函数，将手眼关系分解为独立式的旋转矩阵与平移矩阵计算，以有效且准确的获取手术机器人的手眼关系。
步骤S501：计算平移矩阵Ctr；
Figure BDA0003640833330000107
式中，运用卡尔曼滤波可以得出
Figure BDA0003640833330000108
Figure BDA0003640833330000109
步骤S502：计算旋转矩阵CRr；
基于获取的机器人正运动学信息rRe(i)，通过提取出纵向轴(此处记作Z方向)的信息
Figure BDA0003640833330000111
计算Ur:
Figure BDA0003640833330000112
对Ur进行奇异值分解：
Figure BDA0003640833330000113
式中，∑ro为Ur奇异值分解后的对角矩阵，∑ro主对角线上的每个值为分解后的奇异值，Uro为Ur奇异值分解后的左奇异矩阵，
Figure BDA0003640833330000114
为Ur奇异值分解后的右奇异矩阵；
计算手眼关系的旋转矩阵CRr：
Figure BDA0003640833330000115
最终计算出的平移矩阵和旋转矩阵为最终手术器械的手眼关系即手术机器人的手眼标定结果。
本发明还提出一种手术机器人，该机器人采用如上述面向具有远心不动点约束的手术机器人手眼标定方法的步骤实现精确的手眼标定，该手术机器人为具有远心不动点约束结构的机器人。
为了更好地展示上述手眼标定方法的准确性，通过下述试验进一步说明，本实施例采用蔡式手眼标定方法和本发明提出的手眼标定方法得到的结果进行对比，在手术腹腔镜的相机坐标系下规划不同轨迹，包括空间圆形轨迹、椭圆轨迹、三角形轨迹、螺旋线轨迹；利用本发明方法和蔡氏标定方法得到手术机器人的手眼关系，利用手眼转换关系，将相机坐标系下的空间轨迹映射到手术机器人的从手坐标系下；驱动手术机器人从手，沿规划好的轨迹进行心动；通过相机观测手术机器人的实际轨迹和规划好的轨迹做对比，判别本发明手眼标定方法和蔡氏标定方法的精度，得到在空间圆形轨迹、椭圆轨迹、三角形轨迹、螺旋线轨迹下本发明手眼标定方法和蔡氏标定方法的误差对比结果如表1所示，表1中方法误差的单位为像素：
表1：
Figure BDA0003640833330000121
从表1中可以看出无论在哪一种轨迹下，本发明的手眼标定方法的误差均在14像素以下，而蔡氏标定方法的误差高达70像素-110像素，很明显可以看出本发明方法手眼标定的结果精度更高。说明运用蔡式手眼标定方法进行坐标转换后，手术机器人末端执行器的轨迹会大幅度偏离规划的轨迹，此结果说明，如运用视觉进行手术机器人的自主化操作引导，即使感知的结果十分准确，但因为手眼关系校准的较大误差，会引起机器人实际执行任务时会出现极大的位姿偏差，导致任务的失败甚至会给病人造成二次伤害。而利用本发明提出的手眼标定方法，手术器械末端的实际轨迹与规划的轨迹基本重合，证明了本方法在多种工作条件下的准确性与鲁棒性。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上对本发明所提供的面向具有远心不动点约束的手术机器人手眼标定方法及手术机器人进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (10)

1.一种面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，包括：
获取多次摆位下手术器械在机器坐标系下的姿态；根据相机拍摄到的图像数据确定多次摆位下手术器械在相机坐标系下的三维位姿；
利用所述手术器械在相机坐标系下的三维位姿，计算相机坐标系下每次摆位与其他各摆位的手术器械中心线的公垂点位置；
根据远心不动点结构约束，利用所述多次摆位下手术器械在相机坐标系下的三维位姿和所述每次摆位与其他各摆位的手术器械中心线的公垂点位置构建损失函数，对所述损失函数进行迭代优化，当相邻两次迭代优化后的损失函数差值满足预设值时确定每次摆位下手术器械在相机坐标系下的最优三维位姿；
根据所述最优三维位姿计算每次摆位与其它各摆位的手术器械中心线的最优公垂点位置；
根据所述多次摆位下手术器械在机器坐标系下的姿态和所述每次摆位下与各摆位的手术器械中心线的最优公垂点位置，确定手术器械的手眼关系。
2.根据权利要求1所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述根据相机拍摄到的图像数据确定多次摆位下手术器械在相机坐标系下的三维位姿包括：
确定第i次摆位下的手术器械在相机拍摄图像上的上下边界，并计算第i次摆位下手术器械的边界矩阵Cbi：
Figure FDA0004176121820000011
式中，
Figure FDA0004176121820000012
和/>
Figure FDA0004176121820000013
分别为第i次摆位下手术器械在相机拍摄的图像上的上边界和下边界，i＝1,2,…,k，k为手术器械的摆位总次数；
对第i次摆位下手术器械的边界矩阵Cbi根据相机内参构建矩阵
Figure FDA0004176121820000014
并进行奇异值分解：
Figure FDA0004176121820000015
式中，Kc为相机内参，
Figure FDA00041761218200000219
为第i次摆位下矩阵/>
Figure FDA0004176121820000021
奇异值分解后的对角矩阵，
Figure FDA00041761218200000220
主对角线上的每个值为分解后的奇异值，/>
Figure FDA00041761218200000221
为第i次摆位下矩阵/>
Figure FDA0004176121820000022
奇异值分解后的左奇异矩阵，/>
Figure FDA0004176121820000023
为第i次摆位下矩阵/>
Figure FDA0004176121820000024
奇异值分解后的右奇异矩阵；
选择
Figure FDA00041761218200000222
中的两个最大值∑i1与∑i2，计算第i次摆位手术器械在相机坐标系下的三维位姿/>
Figure FDA0004176121820000025
Figure FDA0004176121820000026
Figure FDA0004176121820000027
式中，
Figure FDA0004176121820000028
为第i次摆位下手术器械在相机坐标系下的三维位姿即普吕克坐标，/>
Figure FDA0004176121820000029
表示取/>
Figure FDA00041761218200000210
的最后一列，rC为手术器械末端执行器的半径长度，
Figure FDA00041761218200000211
为取/>
Figure FDA00041761218200000223
的第一列，∑i1与∑i2分别为/>
Figure FDA00041761218200000212
对角线上的前两个元素且∑i1>∑i2。
3.根据权利要求1所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述计算相机坐标系下每次摆位下与其他各摆位的手术器械中心线的公垂点位置包括：
Figure FDA00041761218200000213
Figure FDA00041761218200000214
式中，
Figure FDA00041761218200000215
为手术器械第i次摆位与第j次摆位的中心线在第i次摆位中心线上的公垂点位置，/>
Figure FDA00041761218200000216
为手术器械第j次摆位与第i次摆位的中心线在第j次摆位中心线的公垂点位置，/>
Figure FDA00041761218200000217
为手术器械第i次摆位的普吕克坐标，/>
Figure FDA00041761218200000218
为手术器械第j次摆位的普吕克坐标，i＝1,2,…,k，k为手术器械的摆位总次数。
4.根据权利要求1所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述确定每次摆位下手术器械在相机坐标系下的最优三维位姿包括：
利用多次摆位下手术器械在相机坐标系下的三维位姿和每次摆位与其它各摆位的手术器械中心线的公垂点位置构建损失函数LSAS：
Figure FDA0004176121820000031
式中，
Figure FDA0004176121820000032
为相机坐标系下第i次摆位的手术器械进行优化后的普吕克坐标，
Figure FDA0004176121820000033
为第i次摆位手术器械在相机坐标系下的普吕克坐标，[*]×代表矩阵*的斜对称矩阵，/>
Figure FDA0004176121820000034
为手术器械第i次摆位与第j次摆位的中心线在第i次摆位中心线上的公垂点位置，/>
Figure FDA0004176121820000035
为手术器械第j次摆位与第i次摆位的中心线在第j次摆位中心线的公垂点位置，i＝1,2,…,k，k为手术器械的摆位总次数；
对LSAS进行迭代式优化，当相邻两次迭代后的LSAS的差值小于预设值时，确定最后一次迭代时优化后的普吕克坐标
Figure FDA0004176121820000036
为相机坐标系下多次摆位的手术器械的最优三维位姿。
5.根据权利要求1所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述确定手术器械的手眼关系包括计算手眼关系的平移矩阵和计算手眼关系的旋转矩阵。
6.根据权利要求5所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述计算手眼关系的平移矩阵为：
Figure FDA0004176121820000037
式中，Ctr为手眼关系的平移矩阵，n＝i·j，i,j∈[1,k]，Cpn为第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置，
Figure FDA0004176121820000041
为利用卡尔曼滤波得到。
7.根据权利要求6所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置为：
利用所述相机坐标系下第i次摆位的手术器械的最优普吕克坐标，计算第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置
Figure FDA0004176121820000042
与/>
Figure FDA0004176121820000043
根据所述第i次摆位与第j次摆位的手术器械中心线的最优公垂点位置
Figure FDA0004176121820000044
与/>
Figure FDA0004176121820000045
求解第i次摆位与第j次摆的手术器械中心线的最优公垂点的中间点位置Cpn：
Figure FDA0004176121820000046
式中，n＝i·j，i,j∈[1,k]，k为手术器械的摆位总次数。
8.根据权利要求5所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述计算手眼关系的旋转矩阵包括：
根据获取的多次摆位下手术器械在机器坐标系下的姿态rRe(i)，提取每次摆位下的纵向轴信息
Figure FDA0004176121820000047
并利用纵向轴信息/>
Figure FDA0004176121820000048
和最优普吕克坐标中的/>
Figure FDA0004176121820000049
计算Ur：
Figure FDA00041761218200000410
对Ur进行奇异值分解：
Figure FDA00041761218200000411
式中，∑ro为Ur奇异值分解后的对角矩阵，∑ro主对角线上的每个值为分解后的奇异值，Uro为Ur奇异值分解后的左奇异矩阵，
Figure FDA00041761218200000412
为Ur奇异值分解后的右奇异矩阵；
根据Ur奇异值分解后的左奇异矩阵和右奇异矩阵计算手眼关系中的旋转矩阵CRr：
Figure FDA0004176121820000051
9.根据权利要求2所述的面向具有远心不动点约束的手术机器人手眼标定方法，其特征在于，所述确定第i次摆位下的手术器械在相机拍摄图像上的上下边界包括：
在第i次摆位下的手术器械边界在相机拍摄图像上随机采样多个关键点，其中上下边界随机采样关键点的个数均不小于2个；
根据拟合算法分别对上边界和下边界的关键点进行拟合，得到拟合的上边界线和下边界线作为手术器械在相机拍摄图像上的上下边界。
10.一种手术机器人，其特征在于，所述手术机器人采用如上述权利要求1-9任一项所述的面向具有远心不动点约束的手术机器人手眼标定方法的步骤实现手眼标定。
CN202210532505.7A 2022-05-12 2022-05-12 一种面向具有远心不动点约束的手术机器人手眼标定方法 Active CN114886567B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210532505.7A CN114886567B (zh) 2022-05-12 2022-05-12 一种面向具有远心不动点约束的手术机器人手眼标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210532505.7A CN114886567B (zh) 2022-05-12 2022-05-12 一种面向具有远心不动点约束的手术机器人手眼标定方法

Publications (2)

Publication Number Publication Date
CN114886567A CN114886567A (zh) 2022-08-12
CN114886567B true CN114886567B (zh) 2023-06-23

Family

ID=82723572

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210532505.7A Active CN114886567B (zh) 2022-05-12 2022-05-12 一种面向具有远心不动点约束的手术机器人手眼标定方法

Country Status (1)

Country Link
CN (1) CN114886567B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115229805B (zh) * 2022-09-21 2022-12-09 北京壹点灵动科技有限公司 手术机器人手眼标定方法、装置、存储介质及处理器
CN117103286B (zh) * 2023-10-25 2024-03-19 杭州汇萃智能科技有限公司 一种机械手手眼标定方法、系统和可读存储介质

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104390598A (zh) * 2013-12-30 2015-03-04 北京中天荣泰科技发展有限公司 一种基于机器视觉的异面直线测量方法
CN106777656A (zh) * 2016-12-09 2017-05-31 江南大学 一种基于pmpsd的工业机器人绝对精度校准方法
CN107053216A (zh) * 2017-04-25 2017-08-18 苏州蓝斯视觉系统股份有限公司 机器人和末端执行器的自动标定方法及系统
CN108972558A (zh) * 2018-08-16 2018-12-11 居鹤华 一种基于轴不变量的多轴机器人动力学建模方法
CN109048876A (zh) * 2018-07-03 2018-12-21 上海新时达电气股份有限公司 一种基于激光跟踪仪的机器人标定方法
CN110598590A (zh) * 2019-08-28 2019-12-20 清华大学 基于多视角相机的紧密交互人体姿态估计方法及装置
CN110695991A (zh) * 2019-09-26 2020-01-17 南京理工大学 基于变异粒子群优化的自动手眼标定方法
CN111409077A (zh) * 2020-05-09 2020-07-14 南京工程学院 一种基于关节角代偿的机器人末端多目标位姿逼近方法
CN111986271A (zh) * 2020-09-04 2020-11-24 廊坊和易生活网络科技股份有限公司 一种基于光束平差的机器人方位与手眼关系同时标定方法
WO2021114666A1 (zh) * 2019-12-11 2021-06-17 山东大学 人机协作中人体安全评估方法及系统
CN113288429A (zh) * 2021-05-07 2021-08-24 山东商业职业技术学院 一种乳腺微创介入手术机器人空间配准与实时导航方法
CN113974835A (zh) * 2021-09-29 2022-01-28 李汉忠 一种基于远心不动点约束的手术机器人运动控制方法
CN114343847A (zh) * 2022-01-06 2022-04-15 广东工业大学 基于光学定位系统的手术机器人的手眼标定方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108765498B (zh) * 2018-05-30 2019-08-23 百度在线网络技术（北京）有限公司 单目视觉跟踪方法、装置及存储介质

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104390598A (zh) * 2013-12-30 2015-03-04 北京中天荣泰科技发展有限公司 一种基于机器视觉的异面直线测量方法
CN106777656A (zh) * 2016-12-09 2017-05-31 江南大学 一种基于pmpsd的工业机器人绝对精度校准方法
CN107053216A (zh) * 2017-04-25 2017-08-18 苏州蓝斯视觉系统股份有限公司 机器人和末端执行器的自动标定方法及系统
CN109048876A (zh) * 2018-07-03 2018-12-21 上海新时达电气股份有限公司 一种基于激光跟踪仪的机器人标定方法
CN108972558A (zh) * 2018-08-16 2018-12-11 居鹤华 一种基于轴不变量的多轴机器人动力学建模方法
CN110598590A (zh) * 2019-08-28 2019-12-20 清华大学 基于多视角相机的紧密交互人体姿态估计方法及装置
CN110695991A (zh) * 2019-09-26 2020-01-17 南京理工大学 基于变异粒子群优化的自动手眼标定方法
WO2021114666A1 (zh) * 2019-12-11 2021-06-17 山东大学 人机协作中人体安全评估方法及系统
CN111409077A (zh) * 2020-05-09 2020-07-14 南京工程学院 一种基于关节角代偿的机器人末端多目标位姿逼近方法
CN111986271A (zh) * 2020-09-04 2020-11-24 廊坊和易生活网络科技股份有限公司 一种基于光束平差的机器人方位与手眼关系同时标定方法
CN113288429A (zh) * 2021-05-07 2021-08-24 山东商业职业技术学院 一种乳腺微创介入手术机器人空间配准与实时导航方法
CN113974835A (zh) * 2021-09-29 2022-01-28 李汉忠 一种基于远心不动点约束的手术机器人运动控制方法
CN114343847A (zh) * 2022-01-06 2022-04-15 广东工业大学 基于光学定位系统的手术机器人的手眼标定方法

Also Published As

Publication number Publication date
CN114886567A (zh) 2022-08-12

Similar Documents

Publication Publication Date Title
CN114886567B (zh) 一种面向具有远心不动点约束的手术机器人手眼标定方法
CN109567942B (zh) 采用人工智能技术的颅颌面外科手术机器人辅助系统
CN112006779B (zh) 一种手术导航系统精度检测方法
CN110000787B (zh) 一种超冗余机械臂的控制方法
CN112105484B (zh) 机器人运动学参数自标定方法、系统及存储装置
CN103459102A (zh) 机器人控制装置、机器人控制方法、程序和记录介质
Liu et al. Intelligent Robotics and Applications: 14th International Conference, ICIRA 2021, Yantai, China, October 22–25, 2021, Proceedings, Part III
CN114343847B (zh) 基于光学定位系统的手术机器人的手眼标定方法
CN115179294A (zh) 机器人控制方法、系统、计算机设备、存储介质
WO2022199296A1 (zh) 手术导航机器人误差消除方法、装置及电子设备
CN116277035B (zh) 机器人的控制方法、装置、处理器及电子设备
CN113910218A (zh) 基于运动学与深度神经网络融合的机器人标定方法和装置
CN113524201B (zh) 机械臂位姿主动调节方法、装置、机械臂和可读存储介质
CN115553945A (zh) 一种导航式牙科手机钻针的配准方法
CN113448246B (zh) 一种口腔种植机器人自进化姿态调整方法及系统
CN114521962A (zh) 手术机器人轨迹跟踪方法、装置、机器人及存储介质
Lu et al. A unified monocular camera-based and pattern-free hand-to-eye calibration algorithm for surgical robots with RCM constraints
CN110900608B (zh) 基于最优测量构型选择的机器人运动学标定方法
CN117017495A (zh) 基于ckf的手术机器人高精度自动手眼标定方法及系统
CN116942314A (zh) 光学定位和机械定位混合的定位方法及系统
CN115424701A (zh) 一种关于最优路径规划的骨表面随动技术
CN114559432B (zh) 手术机械臂自动定位寻路方法、装置、机器人及存储介质
CN115813556A (zh) 手术机器人标定方法、装置、手术机器人及存储介质
CN114310873B (zh) 位姿转换模型的生成方法、控制方法、系统、设备和介质
CN114654466A (zh) 自动标定方法、装置、系统、电子设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant