CN114711968A - 一种基于手术机器人系统的无标定靶区定位跟踪方法 - Google Patents

一种基于手术机器人系统的无标定靶区定位跟踪方法 Download PDF

Info

Publication number
CN114711968A
CN114711968A CN202210329570.XA CN202210329570A CN114711968A CN 114711968 A CN114711968 A CN 114711968A CN 202210329570 A CN202210329570 A CN 202210329570A CN 114711968 A CN114711968 A CN 114711968A
Authority
CN
China
Prior art keywords
target
mechanical arm
surgical
positioning
marker
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
CN202210329570.XA
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.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202210329570.XA priority Critical patent/CN114711968A/zh
Publication of CN114711968A publication Critical patent/CN114711968A/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/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/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/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/70—Manipulators specially adapted for use in surgery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/0002—Inspection of images, e.g. flaw detection
    • G06T7/0012—Biomedical image inspection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • 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
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • 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/107—Visualisation of planned trajectories or target regions
    • 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
    • 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
    • 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/10—Image acquisition modality
    • G06T2207/10048—Infrared image
    • 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

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

Abstract

本发明实施例公开了一种基于手术机器人系统的无标定靶区定位跟踪方法，包括以下步骤：S1：由两个标记球构建模拟穿刺路径；S2：构建图像雅可比矩阵；S3：由容积卡尔曼滤波器滤波得到下一时刻的机械臂控制量估计值；S4：目标关节角度通过PID算法转换为机械臂关节速度域控制量；S5：重复S3～S4，直至滤波误差小于预设阈值，此时滤波停止，目标定位完成。本发明采用无标定视觉伺服的技术路线，术前无需进行手眼标定操作，既能大幅节省术前准备时间，同时也能避免因“手眼”位置改变导致标定好的系统坐标系转换关系失效，增强手术机器人系统的抗干扰性，提升系统整体效率。

Description

一种基于手术机器人系统的无标定靶区定位跟踪方法
技术领域
本发明涉及手术机器人技术领域，尤其涉及一种基于手术机器人系统的无标定靶区定位跟踪方法。
背景技术
近年来，手术机器人领域发展迅速。手术机器人是一种整合计算机技术、影像技术和机器人技术的新兴手术方案，能较好地克服传统徒手操作手术精度不足、辐射过多、操作疲劳等问题。手术导航系统一般包括图像工作站、定位系统、手术器械等。从定位系统层面又可分为机械定位、超声定位、电磁定位和光学定位等。
在光学信息引导的机器人系统中，摄像机有两种典型的安装方式：
(1)眼固定构型(FixedCamera或Eye-to-Hand)。摄像机安装在机器人的正前方或斜侧方，视场能完全覆盖机器人的工作空间。
(2)眼在手上构型(Eye-in-Hand)。摄像机安装在机器人的末端执行器上，随机器人末端的运动而运动，扩大了摄像机的工作范围。
手术机器人系统通常采用眼固定构型安装摄像头，可同时获得机器人及工作环境的全局视觉信息。
对光学信息引导的手术机器人，靶点定位的精度主要依赖于术前手眼标定的精度。通过手眼标定可以得到机器人和光学定位系统之间的位置和方向关系，如图1所示。这需要准确计算机器人基座与光学定位系统间、机器人末端与手术器械间的空间关系。
现有的手术机器人存在以下缺点：
(1)手眼标定首先需要控制机器人在光学定位系统的视场范围内运动，采集后续标定计算所需数据，耗时长。
(2)由于采集数据耗时长，为使所采集数据正确，需要保证在整个采集的过程中环境不受干扰，因而对环境要求较为苛刻，容错率低。
(3)手眼标定需要做一系列复杂的矩阵运算，验证标定结果的过程也较为复杂，且可能引入机器人运动误差。
(4)标定完成后，如果光学定位系统位置发生改变，其与机器人之间的相对位置关系也将改变，计算目标位姿时会出现不可预测的错误，导致定位失败。
(5)手术场景中存在的诸多仪器设备与医务人员使得上述情况发生的概率增大，重复计算和更新标定结果将引入新的机器人运动误差并降低机器人运动跟随与靶区定位的实时性。
(6)标定完成后，控制机器人运动需要进行一系列的坐标系转换，运算量大，难以对运动目标进行实时跟踪。
发明内容
本发明实施例所要解决的技术问题在于，提供一种基于手术机器人系统的无标定靶区定位跟踪方法，以提升手术过程的效率和安全性。
为了解决上述技术问题，本发明实施例提出了一种基于手术机器人系统的无标定靶区定位跟踪方法，所述手术机器人系统包括6DOF机械臂、光学定位系统、手术工具、反光标记球若干，反光标记球是表面涂有反射涂层的小球，能有效反射近红外光；手术工具上安装4个反光标记球，标记球的位置信息可被光学定位系统所捕捉；手术工具安装于6DOF机械臂末端，通过控制机械臂末端带动手术工具进行手术操作；光学定位系统安装有两台呈一定夹角的相机，构成双目立体视觉系统，通过采集视场内标记球所反射的近红外光，对反光标记球进行定位；所述定位跟踪方法包括以下步骤：
S1：由两个标记球构建模拟穿刺路径；
S2：构建图像雅可比矩阵，包括以下子步骤：S21：增加约束条件，确定手术工具为达到穿刺目标时的位姿；S22：手术工具上4个标记球的坐标作为图像特征，6DOF机械臂的关节角度向量作为控制量，由图像特征对控制量微分得到雅可比矩阵；
S3：由容积卡尔曼滤波器滤波得到下一时刻的机械臂控制量估计值；
S4：目标关节角度通过PID算法转换为机械臂关节速度域控制量；
S5：重复S3～S4，直至滤波误差小于预设阈值，此时滤波停止，目标定位完成。
进一步地，步骤S1的具体过程为：
在手术机器人系统的工作空间中，上下放置两个标记球Markertop和Markerbottom，由光学定位系统获取标记球的坐标Ptop与Pbottom，位置高的标记球表示穿刺的目标点，位置高的标记球指向位置低的标记球的向量表示穿刺的方向，即Vectarget＝Pbottom-Ptop，Ptarget＝Ptop，由此确定手术穿刺的方向和位置。
进一步地，子步骤S21的具体过程为：
光学定位系统坐标系Soptical到手术工具坐标系Stool的旋转平移矩阵Rot,Tot由光学定位系统得到，光学定位系统坐标系Soptical中的目标向量Vectarget通过Rot,Tot转换到手术工具坐标系Stool中；根据手术工具上针的信息得到，手术工具坐标系Stool中的针向量Vecnipple与目标向量Vectar get；将两向量叉乘得到旋转轴，两向量点乘再计算反余弦得到向量间的夹角，由此可得Vecnipple到Vectar get的旋转向量，再通过四元数法转换成旋转矩阵Rnipple-target；计算平移矩阵，Tnipple-target＝Ptip-Ptarget；至此针向量到目标向量的旋转矩阵和平移矩阵都已得到；手术工具上的4个标记球可分别通过旋转平移矩阵Rnipple-target,Tnipple-target计算得到到达穿刺目标位置后在光学定位系统坐标系Soptical中的坐标。
进一步地，子步骤S22的具体过程为：
在光学定位系统坐标系中，记
Figure BDA0003574718350000031
其中
Figure BDA0003574718350000032
表示标记球a的x轴坐标变化量，
Δθ＝[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6]，
其中Δθ1表示机械臂第一个关节的角度变化量，则手术工具上标记球的位置变化量Δf对机械臂关节角度的变化量Δθ的微分构成图像雅可比矩阵Jθ：
Figure BDA0003574718350000033
设置图像特征的误差阈值fthreshold，当图像特征的误差小于阈值即认为机械臂已到达目标位置。
进一步地，步骤S3包括以下子步骤：
子步骤S31，系统初始化：
手术机器人无标定定位的非线性系统模型为
Figure BDA0003574718350000034
其中k表示当前时刻，xk和yk分别表示时刻k系统的状态量和观测量，F和H分别表示系统状态方程和系统观测方程，ωk和υk分别为过程噪声和观测噪声；
控制机械臂各关节做几次线性无关的运动，记录关节变化量和手术工具上标记球坐标的变化量，以此初始化图像雅可比矩阵，将图像雅可比矩阵Jθ展开成一维列向量作为系统状态量：
Figure BDA0003574718350000041
初始化误差协方差矩阵Pk，过程噪声协方差矩阵Q和测量噪声协方差矩阵R；
子步骤S32，时间更新：
a)计算容积点
Figure BDA0003574718350000042
Figure BDA0003574718350000043
式中，n为状态量的维数；ξi为容积点集：
Figure BDA0003574718350000044
在应用三阶球面径向准则时，容积点个数为状态向量维数n的2倍，式中[1]i为第i个容积点：
Figure BDA0003574718350000045
b)传播容积点
Figure BDA0003574718350000046
c)计算状态量预测值及误差协方差预测值
Figure BDA0003574718350000047
Figure BDA0003574718350000048
子步骤S33，测量更新：
a)计算容积点
Figure BDA0003574718350000049
Figure BDA00035747183500000410
b)传播容积点
Figure BDA00035747183500000411
c)计算测量预测值
Figure BDA00035747183500000412
d)计算测量误差协方差，互协方差
Figure BDA00035747183500000413
Figure BDA00035747183500000414
子步骤S34，状态估计：
a)计算卡尔曼增益
Figure BDA0003574718350000051
b)更新状态量，误差协方差
Figure BDA0003574718350000052
Figure BDA0003574718350000053
子步骤S34，计算下一时刻机械臂控制量：
将时刻k状态量估计值Xk转换为雅可比矩阵Jk，计算下一时刻的控制量θk+1：
Figure BDA0003574718350000054
Figure BDA0003574718350000055
式中Kp为控制器参数，fk为当前系统测量值与目标状态值的差值。
进一步地，步骤S4的具体过程为：
记t为当前时刻，θg(t)＝[θ1,θ2,θ3,θ4,θ5,θ6]为机械臂目标关节角度，θr(t)＝[θr1,θr2,θr3,θr4,θr5,θr6]为机械臂实时关节角度，θd(t)＝[θd1,θd2,θd3,θd4,θd5,θd6]为机械臂关节目标角度与实时角度的差值，θd(t)＝θg(t)-θr(t)，根据上述信息输出机械臂目标关节速度θgv(t)＝[θgv1,θgv2,θgv3,θgv4,θgv5,θgv6]：
Figure BDA0003574718350000056
将θgv(t)转换成机械臂运动指令，达到机械臂实时控制的目的。
进一步地，步骤S5的具体过程为：
在每次机械臂执行完运动指令后，获取当前机械臂末端手术工具的图像特征f(t)，计算其与目标特征fgoal(t)的差值，即fe(t)＝|f(t)-fgoal(t)|；若fe(t)>fthreshold，则重复步骤S3～S4；若fe(t)≤fthreshold，则认为机械臂已到达设定的目标位置，此时滤波停止，目标定位完成。
本发明的有益效果为：本发明采用无标定视觉伺服的技术路线，术前无需进行手眼标定操作，既能大幅节省术前准备时间，同时也能避免因“手眼”位置改变导致标定好的系统坐标系转换关系失效，增强手术机器人系统的抗干扰性，提升系统整体效率。就计算过程而言，现有基于标定的方法在标定完成后，每次运动都需要将光学定位系统坐标系中的目标点坐标进行一系列的旋转平移变换，转换到机械臂基座坐标系，才能得到机械臂的运动指令。其中包含了大量的矩阵运算，时间复杂度高，生成机械臂控制指令的时间间隔大，无法做到对运动目标的有效跟踪。本发明包括容积卡尔曼滤波和PID控制算法，时间复杂度较低，算法每次迭代可得到新的速度域控制指令，机械臂控制空窗小，能够对运动的目标进行平稳准确的定位和跟踪。
附图说明
图1是手术机器人系统的结构示意图。
图2是本发明实施例的基于手术机器人系统的无标定靶区定位跟踪方法的流程示意图。
图3是本发明实施例的手术工具与模拟穿刺路径示意图。
图4是本发明实施例的角度控制量转速度控制量流程图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互结合，下面结合附图和具体实施例对本发明作进一步详细说明。
本发明的基于手术机器人系统采用眼固定构型(Eye-to-Hand)，光学定位系统放置在机械臂前方，需保证光学定位系统的视场能够完全覆盖机械臂的工作空间。本手术机器人系统包括6DOF机械臂、近红外光学定位仪、手术工具、反光标记球若干。反光标记球是表面涂有反射涂层的小球，能有效反射近红外光；手术工具上安装4个反光标记球，标记球的位置信息可被光学定位系统所捕捉；手术工具安装于6DOF机械臂末端，通过控制机械臂末端带动手术工具进行手术操作；近红外光学定位系统安装有两台呈一定夹角的相机，构成双目立体视觉系统，通过采集视场内标记球所反射的近红外光，对反光标记球进行定位。
请参照图2，本发明实施例的基于手术机器人系统的无标定靶区定位跟踪方法包括以下步骤：
S1：由两个标记球构建模拟穿刺路径；
S2：构建图像雅可比矩阵，包括以下步骤：S21：增加约束条件，确定手术工具为达到穿刺目标时的位姿；S22：手术工具上4个标记球的坐标作为图像特征，6DOF机械臂的关节角度向量作为控制量，由图像特征对控制量微分得到雅可比矩阵；
S3：由容积卡尔曼滤波器(CKF)滤波得到下一时刻的机械臂控制量估计值；
S4：目标关节角度通过PID算法转换为机械臂关节速度域控制量；
S5：重复S3～S4，直至滤波误差小于阈值，此时滤波停止，目标定位完成。
以下详细阐述本发明技术方案：
S1，两个标记球构建穿刺路径：
在手术机器人工作空间中，上下放置两个标记球Markertop和Markerbottom，由光学定位系统获取标记球的坐标ptop与Pbottom，位置高的标记球表示穿刺的目标点，位置高的标记球指向位置低的标记球的向量表示穿刺的方向，即Vectarget＝Pbottom-Ptop，Ptarget＝Ptop，由此确定手术穿刺的方向和位置。
S2，构建图像雅可比矩阵：
(1)确定手术工具为到达穿刺路径时的目标位姿
手术工具与模拟的穿刺路径如图3所示。手术工具与穿刺路径同处于光学定位系统的视场内。光学定位系统坐标系Soptical到手术工具坐标系Stool的旋转平移矩阵Rot,Tot可由光学定位系统得到，光学定位系统坐标系Soptical中的目标向量Vectarget可通过Rot,Tot转换到手术工具坐标系Stool中。手术工具上针的信息可由工具设计文件得到，此时知道手术工具坐标系Stool中的针向量Vecnipple与目标向量Vectar get。将两向量叉乘得到旋转轴，两向量点乘再计算反余弦得到向量间的夹角，由此可得Vecnipple到Vectar get的旋转向量，再通过四元数法转换成旋转矩阵Rnipple-target；计算平移矩阵，Tnipple-target＝Ptip-Ptarget。至此针向量到目标向量的旋转矩阵和平移矩阵都已得到。手术工具上的4个标记球可分别通过旋转平移矩阵Rnipple-target,Tnipple-target计算得到到达穿刺目标位置后在光学定位系统坐标系Soptical中的坐标。
(2)由图像特征与机械臂控制量构建雅可比矩阵
在光学定位系统坐标系中，记
Figure BDA0003574718350000071
其中
Figure BDA0003574718350000072
表示标记球a的x轴坐标变化量，
Δθ＝[Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6]， (2)
其中Δθ1表示机械臂第一个关节的角度变化量。则手术工具上标记球的位置变化量Δf对机械臂关节角度的变化量Δθ的微分构成图像雅可比矩阵Jθ：
Figure BDA0003574718350000081
设置图像特征的误差阈值fthreshold，当图像特征的误差小于阈值即认为机械臂已到达目标位置。
S3，容积卡尔曼滤波得到下一时刻的机械臂控制量估计值：
(1)系统初始化
手术机器人无标定定位的非线性系统模型为
Figure BDA0003574718350000082
其中k表示当前时刻，xk和yk分别表示时刻k系统的状态量和观测量，F和H分别表示系统状态方程和系统观测方程。ωk和υk分别为过程噪声和观测噪声。
控制机械臂各关节做几次线性无关的运动，记录关节变化量和手术工具上标记球坐标的变化量，以此初始化图像雅可比矩阵。将图像雅可比矩阵Jθ展开成一维列向量作为系统状态量
Figure BDA0003574718350000083
作为系统状态量。
初始化误差协方差矩阵Pk，过程噪声协方差矩阵Q和测量噪声协方差矩阵R。
(2)时间更新
a)计算容积点
Figure BDA0003574718350000084
Figure BDA0003574718350000085
式中，n为状态量的维数；ξi为容积点集：
Figure BDA0003574718350000086
在应用三阶球面径向准则时，容积点个数为状态向量维数n的2倍。式中[1]i为第i个容积点：
Figure BDA0003574718350000091
b)传播容积点
Figure BDA0003574718350000092
c)计算状态量预测值及误差协方差预测值
Figure BDA0003574718350000093
Figure BDA0003574718350000094
(3)测量更新
a)计算容积点
Figure BDA0003574718350000095
Figure BDA0003574718350000096
b)传播容积点
Figure BDA0003574718350000097
c)计算测量预测值
Figure BDA0003574718350000098
d)计算测量误差协方差，互协方差
Figure BDA0003574718350000099
Figure BDA00035747183500000910
(4)状态估计
a)计算卡尔曼增益
Figure BDA00035747183500000911
b)更新状态量，误差协方差
Figure BDA00035747183500000912
Figure BDA0003574718350000101
(5)计算下一时刻机械臂控制量
将时刻k状态量估计值Xk转换为雅可比矩阵Jk，计算下一时刻的控制量θk+1：
Figure BDA0003574718350000102
Figure BDA0003574718350000103
式中Kp为控制器参数，fk为当前系统测量值与目标状态值的差值。
S4，目标关节角度通过PID算法转换为机械臂关节速度域控制量：
记t为当前时刻，θg(t)＝[θ1,θ2,θ3,θ4,θ5,θ6]为机械臂目标关节角度，θr(t)＝[θr1,θr2,θr3,θr4,θr5,θr6]为机械臂实时关节角度，θd(t)＝[θd1,θd2,θd3,θd4,θd5,θd6]为机械臂关节目标角度与实时角度的差值，θd(t)＝θg(t)-θr(t)。机器人伺服控制方法如图4所示。将上述信息输入机械臂PID控制系统，输出机械臂目标关节速度θgv(t)＝[θgv1,θgv2,θgv3,θgv4,θgv5,θgv6]：
Figure BDA0003574718350000104
将θgv(t)转换成机械臂运动指令，达到机械臂实时控制的目的。
S5，判断定位误差
在每次机械臂执行完运动指令后，获取当前机械臂末端手术工具的图像特征f(t)，计算其与目标特征fgoal(t)的差值，即fe(t)＝|f(t)-fgoal(t)|。若fe(t)>fthreshold，则重复步骤3～4；若fe(t)≤fthreshold，则认为机械臂已到达设定的目标位置，此时滤波停止，目标定位完成。
本发明采用无标定视觉伺服的方法，省去冗长复杂的标定步骤，大幅节省手术机器人术前准备时间。
本发明允许机器人与光学定位系统的相对位置发生偏移，提升手术过程的效率和安全性。
本发明实时采集标记点的数据，更新目标的位置信息，实现对运动目标的精准跟随。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同范围限定。

Claims (7)

1.一种基于手术机器人系统的无标定靶区定位跟踪方法，其特征在于，所述手术机器人系统包括6DOF机械臂、光学定位系统、手术工具、反光标记球若干，反光标记球是表面涂有反射涂层的小球，能有效反射近红外光；手术工具上安装4个反光标记球，标记球的位置信息可被光学定位系统所捕捉；手术工具安装于6DOF机械臂末端，通过控制机械臂末端带动手术工具进行手术操作；光学定位系统安装有两台呈一定夹角的相机，构成双目立体视觉系统，通过采集视场内标记球所反射的近红外光，对反光标记球进行定位；所述定位跟踪方法包括以下步骤：
S1：由两个标记球构建模拟穿刺路径；
S2：构建图像雅可比矩阵，包括以下子步骤：S21：增加约束条件，确定手术工具为达到穿刺目标时的位姿；S22：手术工具上4个标记球的坐标作为图像特征，6DOF机械臂的关节角度向量作为控制量，由图像特征对控制量微分得到雅可比矩阵；
S3：由容积卡尔曼滤波器滤波得到下一时刻的机械臂控制量估计值；
S4：目标关节角度通过PID算法转换为机械臂关节速度域控制量；
S5：重复S3～S4，直至滤波误差小于预设阈值，此时滤波停止，目标定位完成。
2.如权利要求1所述的基于手术机器人系统的无标定靶区定位跟踪方法，其特征在于，步骤S1的具体过程为：
在手术机器人系统的工作空间中，上下放置两个标记球Markertop和Markerbottom，由光学定位系统获取标记球的坐标Ptop与Pbottom，位置高的标记球表示穿刺的目标点，位置高的标记球指向位置低的标记球的向量表示穿刺的方向，即Vectarget＝Pbottom-Ptop，Ptarget＝Ptop，由此确定手术穿刺的方向和位置。
3.如权利要求1所述的基于手术机器人系统的无标定靶区定位跟踪方法，其特征在于，子步骤S21的具体过程为：
光学定位系统坐标系Soptical到手术工具坐标系Stool的旋转平移矩阵Rot，Tot由光学定位系统得到，光学定位系统坐标系Soptical中的目标向量Vectarget通过Rot，Tot转换到手术工具坐标系Stool中；根据手术工具上针的信息得到，手术工具坐标系Stool中的针向量Vecnipple与目标向量Vectarget；将两向量叉乘得到旋转轴，两向量点乘再计算反余弦得到向量间的夹角，由此可得Vecnipple到Vectarget的旋转向量，再通过四元数法转换成旋转矩阵Rnipple-target；计算平移矩阵，Tnipple-target＝Ptip-Ptarget；至此针向量到目标向量的旋转矩阵和平移矩阵都已得到；手术工具上的4个标记球可分别通过旋转平移矩阵Rnipple-target，Tnipple-target计算得到到达穿刺目标位置后在光学定位系统坐标系Soptical中的坐标。
4.如权利要求3所述的基于手术机器人系统的无标定靶区定位跟踪方法，其特征在于，子步骤S22的具体过程为：
在光学定位系统坐标系中，记
Figure FDA0003574718340000021
其中
Figure FDA0003574718340000022
表示标记球a的x轴坐标变化量，
Δθ＝[Δθ1，Δθ2，Δθ3，Δθ4，Δθ5，Δθ6]，
其中Δθ1表示机械臂第一个关节的角度变化量，则手术工具上标记球的位置变化量Δf对机械臂关节角度的变化量Δθ的微分构成图像雅可比矩阵Jθ：
Figure FDA0003574718340000023
设置图像特征的误差阈值fthreshold，当图像特征的误差小于阈值即认为机械臂已到达目标位置。
5.如权利要求4所述的基于手术机器人系统的无标定靶区定位跟踪方法，其特征在于，步骤S3包括以下子步骤：
子步骤S31，系统初始化：
手术机器人无标定定位的非线性系统模型为
Figure FDA0003574718340000024
其中k表示当前时刻，xk和yk分别表示时刻k系统的状态量和观测量，F和H分别表示系统状态方程和系统观测方程，ωk和υk分别为过程噪声和观测噪声；
控制机械臂各关节做几次线性无关的运动，记录关节变化量和手术工具上标记球坐标的变化量，以此初始化图像雅可比矩阵，将图像雅可比矩阵Jθ展开成一维列向量作为系统状态量：
Figure FDA0003574718340000025
初始化误差协方差矩阵Pk，过程噪声协方差矩阵Q和测量噪声协方差矩阵R；
子步骤S32，时间更新：
a)计算容积点
Figure FDA0003574718340000031
Figure FDA0003574718340000032
式中，n为状态量的维数；ξi为容积点集：
Figure FDA0003574718340000033
在应用三阶球面径向准则时，容积点个数为状态向量维数n的2倍，式中[1]i为第i个容积点：
Figure FDA0003574718340000034
b)传播容积点
Figure FDA0003574718340000035
c)计算状态量预测值及误差协方差预测值
Figure FDA0003574718340000036
Figure FDA0003574718340000037
子步骤S33，测量更新：
a)计算容积点
Figure FDA0003574718340000038
Figure FDA0003574718340000039
b)传播容积点
Figure FDA00035747183400000310
c)计算测量预测值
Figure FDA00035747183400000311
d)计算测量误差协方差，互协方差
Figure FDA00035747183400000312
Figure FDA00035747183400000313
子步骤S34，状态估计：
a)计算卡尔曼增益
Figure FDA00035747183400000314
b)更新状态量，误差协方差
Figure FDA00035747183400000315
Figure FDA00035747183400000316
子步骤S34，计算下一时刻机械臂控制量：
将时刻k状态量估计值Xk转换为雅可比矩阵Jk，计算下一时刻的控制量θk+1：
Figure FDA0003574718340000041
Figure FDA0003574718340000042
式中Kp为控制器参数，fk为当前系统测量值与目标状态值的差值。
6.如权利要求5所述的基于手术机器人系统的无标定靶区定位跟踪方法，其特征在于，步骤S4的具体过程为：
记t为当前时刻，θg(t)＝[θ1，θ2，θ3，θ4，θ5，θ6]为机械臂目标关节角度，θr(t)＝[θr1，θr2，θr3，θr4，θr5，θr6]为机械臂实时关节角度，θd(t)＝[θd1，θd2，θd3，θd4，θd5，θd6]为机械臂关节目标角度与实时角度的差值，θd(t)＝θg(t)-θr(t)，根据上述信息输出机械臂目标关节速度θgv(t)＝[θgv1，θgv2，θgv3，θgv4，θgv5，θgv6]：
Figure FDA0003574718340000043
将θgv(t)转换成机械臂运动指令，达到机械臂实时控制的目的。
7.如权利要求6所述的基于手术机器人系统的无标定靶区定位跟踪方法，其特征在于，步骤S5的具体过程为：
在每次机械臂执行完运动指令后，获取当前机械臂末端手术工具的图像特征f(t)，计算其与目标特征fgoal(t)的差值，即fe(t)＝|f(t)-fgoal(t)|；若fe(t)＞fthreshold，则重复步骤S3～S4；若fe(t)≤fthreshpld，则认为机械臂已到达设定的目标位置，此时滤波停止，目标定位完成。
CN202210329570.XA 2022-03-31 2022-03-31 一种基于手术机器人系统的无标定靶区定位跟踪方法 Pending CN114711968A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210329570.XA CN114711968A (zh) 2022-03-31 2022-03-31 一种基于手术机器人系统的无标定靶区定位跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210329570.XA CN114711968A (zh) 2022-03-31 2022-03-31 一种基于手术机器人系统的无标定靶区定位跟踪方法

Publications (1)

Publication Number Publication Date
CN114711968A true CN114711968A (zh) 2022-07-08

Family

ID=82239159

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210329570.XA Pending CN114711968A (zh) 2022-03-31 2022-03-31 一种基于手术机器人系统的无标定靶区定位跟踪方法

Country Status (1)

Country Link
CN (1) CN114711968A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116919595A (zh) * 2023-08-17 2023-10-24 哈尔滨工业大学 基于光学和电磁定位及卡尔曼滤波的骨针位置跟踪方法
CN116919595B (zh) * 2023-08-17 2024-06-07 哈尔滨工业大学 基于光学和电磁定位及卡尔曼滤波的骨针位置跟踪装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116919595A (zh) * 2023-08-17 2023-10-24 哈尔滨工业大学 基于光学和电磁定位及卡尔曼滤波的骨针位置跟踪方法
CN116919595B (zh) * 2023-08-17 2024-06-07 哈尔滨工业大学 基于光学和电磁定位及卡尔曼滤波的骨针位置跟踪装置

Similar Documents

Publication Publication Date Title
CN113334393B (zh) 一种机械臂控制方法、系统、机器人及存储介质
CN111775146A (zh) 一种工业机械臂多工位作业下的视觉对准方法
Yoshimi et al. Alignment using an uncalibrated camera system
CN110919658B (zh) 一种基于视觉和多坐标系闭环转换的机器人标定方法
CN114343847B (zh) 基于光学定位系统的手术机器人的手眼标定方法
He et al. Moment-based 2.5-D visual servoing for textureless planar part grasping
CN110345936B (zh) 运动装置的轨迹数据处理方法及其处理系统
CN103878770A (zh) 基于速度估计的空间机器人视觉时延误差补偿方法
CN113843792B (zh) 一种手术机器人的手眼标定方法
JP2004030445A (ja) 移動ロボットの自己位置推定方法及びシステム、並びにプログラム
CN113524201A (zh) 机械臂位姿主动调节方法、装置、机械臂和可读存储介质
Hu et al. 1d-lrf aided visual-inertial odometry for high-altitude mav flight
CN117283528A (zh) 一种并联机器人运动学模型在线学习方法
CN110900608B (zh) 基于最优测量构型选择的机器人运动学标定方法
CN114711968A (zh) 一种基于手术机器人系统的无标定靶区定位跟踪方法
CN112587237A (zh) 一种骨科手术机器人降低操作误差的方法
CN111283664A (zh) 一种面向机器人增强现实示教的注册系统和方法
Taylor et al. Flexible self-calibrated visual servoing for a humanoid robot
CN113689501B (zh) 一种基于收敛点的双机协同目标机定位跟踪控制方法
Yang et al. A closed-loop controller for a continuum surgical manipulator based on a specially designed wrist marker and stereo tracking
Rebello et al. Autonomous active calibration of a dynamic camera cluster using next-best-view
CN114764830A (zh) 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法
CN113799127B (zh) 光学双目定位系统下六自由度机械臂无标定位姿定位方法
CN113298881B (zh) 单目相机-imu-机械臂的空间联合标定方法
Uno et al. Deep Inertial Underwater Odometry 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