CN113143461B - 人机协同的微创内窥镜持镜机器人系统 - Google Patents

Abstract

本发明提供一种人机协同的微创内窥镜持镜机器人系统，涉及内窥镜领域。通过获取内窥镜视图和机器人位姿；获取机器人各关节状态，并根据机器人正向运动学原理求解内窥镜相机位姿；采用YOLOv3算法检测内窥镜视图中的手术器械尖端位置；基于视图中每一个手术器械尖端位置，获取手术器械尖端到视图中心点的距离；基于手术器械尖端位置、手术器械尖端到视图中心点的距离和相机参数获取视觉追踪向量；获取内窥镜插入体内距离约束向量；基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度。本发明提出了一种新的内窥镜术野自适应调整方法，在不需要估计手术器械深度的情况下通过机器人自动调整内窥镜术野范围，系统稳定，鲁棒性好。

Description

人机协同的微创内窥镜持镜机器人系统

技术领域

本发明涉及内窥镜技术领域，具体涉及一种人机协同的微创内窥镜持镜机器人系统。

背景技术

在微创手术中，内窥镜通常由助手医生扶持，根据外科医生的指示来调整内窥镜的位姿。肌肉疲劳和助手与外科医生之间配合不默契可能会导致视觉反馈不佳，降低手术质量。

在现有技术中，应用视觉伺服控制技术来控制机器人自动扶持内窥镜(即人机协同的微创内窥镜持镜机器人系统)可缓解上述问题。如申请号为2017106021363，名称为一种手术野智能化自动跟踪腔镜系统的专利中，通过摄像头实时采集手术环境的图像信息，并将手术环境的图像信息发送至视频处理器中，视频处理器根据手术环境的图像信息控制全自动云台实现对跟踪目标的自动跟踪，从而实现手术术野的全自动跟踪。视频处理器处理图像信息得到手术器械的深度信息，从而实现对跟踪目标的自动跟踪。

然而，现有的自动视野控制技术需要估计器械的深度信息，这会导致系统不稳定，鲁棒性差。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种人机协同的微创内窥镜持镜机器人系统，解决了现有的人机协同的微创内窥镜持镜机器人系统鲁棒性差的技术问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：

本发明提供了一种人机协同的微创内窥镜持镜机器人系统，所述系统包括：

内窥镜模块用于拍摄内窥镜视图，并将内窥镜视图传输给数据处理模块；

机器人，用于扶持内窥镜，并通过控制机器人每个关节的位姿调整内窥镜术野范围；

数据处理模块，用于对所述内窥镜视图进行图像处理和执行控制算法，完成机器人控制指令的生成和发送；

数据处理模块获得所述内窥镜视图后，采用YOLOv3算法获取内窥镜视图中的手术器械尖端的定位，同时通过机器人正运动学建模求解内窥镜相机位姿；

基于所述手术器械尖端的定位和所述相机位姿在相机参数已知的情况下，追踪控制器输出相机运动向量；

所述追踪控制器根据手术器械尖端和视图中心点的距离来判断是否改变相机的位姿；

如果所述手术器械尖端中心与视图中心之间的距离小于阈值r，则内窥镜不动，反之，内窥镜需要根据手术器械尖端在视图中的位置和手术器械尖端与视图中心的距离进行运动；

通过末端执行器到相机位姿的坐标转换、逆向运动学求解，获得机器人各个关节的速度；

将机器人各个关节的速度发送给所述机器人。

优选的，所述手术器械尖端的定位包括：手术器械尖端位置和手术器械尖端到视图中心点的距离。

优选的，所述基于手术器械尖端的定位和相机位姿在相机参数已知的情况下，追踪控制器输出相机运动向量，包括：

基于所述手术器械尖端位置、手术器械尖端到视图中心点的距离和相机参数获取视觉追踪向量；

获取内窥镜插入体内距离约束向量；

基于所述视觉追踪向量和所述内窥镜插入体内距离约束向量相机运动向量。

优选的，所述基于器械尖端位置、器械尖端到视图中心点的距离和相机参数获取视觉追踪向量，包括：

基于所述相机参数获取手术器械尖端中心点在相机坐标系下三维位置和手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；具体为：

其中：(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置；

(x_cam，y_cam，z_cam)表示内窥镜相机坐标系；

z_i表示手术器械尖端中心在相机坐标系下的深度；

P_i表示手术器械尖端中心点在相机坐标系下三维位置；

P′_i表示手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；

计算视觉追踪向量：

和/>符合以下规则：

V_max表示内窥镜相机所能达到的最大速度；

当d_i≤r时，当d_i≥R时，/>且/>

优选的，所述获取内窥镜插入体内距离约束向量，包括：

其中：

z_EEF表示内窥镜轴所在的z轴单位向量；

l表示内窥镜相机到创口点的距离；

是用球坐标系表示的内窥镜安全操作区域，假设医生设定的插入深度为λ为一个比例系数常量；

符合以下规则：

当时，/>

当时，/>

表示内窥镜插入体内距离约束向量的大小；

和/或

所述基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度，包括：

基于视觉追踪向量和内窥镜插入体内距离约束向量获取内窥镜术野自适应调整时内窥镜相机运动向量：

其中：

表示相机运动向量；

n表示画面中手术器械的个数；

γ_i表示在追踪第i个手术器械时该手术器械所占的权重；

v_i表示追踪第i个手术器械时的视觉追踪向量；

v_c表示内窥镜插入体内距离约束向量；

ξ(.)为一个向量大小约束函数，计算公式为：

V_max表示内窥镜相机所能达到的最大速度；

根据内窥镜相机运动向量计算机器人各关节速度。

本发明还提供一种控制人机协同的微创内窥镜持镜机器人的工作方法，所述工作方法包括：

获取内窥镜视图和机器人位姿；获取机器人各关节状态，并根据机器人正向运动学原理求解内窥镜相机位姿；

采用YOLOv3算法检测内窥镜视图中的手术器械尖端位置；

基于视图中手术器械尖端位置，获取手术器械尖端到视图中心点的距离；

基于手术器械尖端位置、手术器械尖端到视图中心点的距离和相机参数获取视觉追踪向量；

获取内窥镜插入体内距离约束向量；

基于所述视觉追踪向量和所述内窥镜插入体内距离约束向量获取协作型机械臂各关节速度。

优选的，基于视图中每一个手术器械尖端位置，获取手术器械尖端到视图中心点的距离，包括：

其中：

(u₀，v₀)表示内窥镜图像中心点的位置；

(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置。

优选的，基于器械尖端位置、器械尖端到视图中心点的距离和相机参数获取视觉追踪向量，包括：

基于相机参数获取手术器械尖端中心点在相机坐标系下三维位置和手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；具体为：

其中：(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置；

(x_cam，y_cam，z_cam)表示内窥镜相机坐标系；

z_i表示手术器械尖端中心在相机坐标系下的深度；

P_i表示手术器械尖端中心点在相机坐标系下三维位置；

P′_i表示手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；

计算视觉追踪向量：

和/>符合以下规则：

V_max表示内窥镜相机所能达到的最大速度；

当d_i≤r时，当d_i≥R时，/>且/>

优选的，所述获取内窥镜插入体内距离约束向量，包括：

其中：

z_EEF表示内窥镜轴所在的z轴单位向量；

l表示内窥镜相机到创口点的距离；

是用球坐标系表示的内窥镜安全操作区域，假设医生设定的插入深度为λ为一个比例系数常量；

符合以下规则：

当时，/>

当时，/>

表示内窥镜插入体内距离约束向量的大小。

优选的，所述基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度，包括：

基于视觉追踪向量和内窥镜插入体内距离约束向量获取内窥镜术野自适应调整时内窥镜相机运动向量：

其中：

表示相机运动向量；

n表示画面中手术器械的个数；

γ_i表示在追踪第i个手术器械时该手术器械所占的权重；

v_i表示追踪第i个手术器械时的视觉追踪向量；

v_c表示内窥镜插入体内距离约束向量；

ξ(.)为一个向量大小约束函数，计算公式为：

V_max表示内窥镜相机所能达到的最大速度；

根据内窥镜相机运动向量计算机器人各关节速度。

(三)有益效果

本发明提供了一种人机协同的微创内窥镜持镜机器人系统。与现有技术相比，具备以下有益效果：

在本发明中数据处理模块获得内窥镜视图后，采用YOLOv3算法获取内窥镜视图中的手术器械尖端的定位，同时通过机器人正运动学建模求解内窥镜相机的位姿；在相机参数已知的情况下，追踪控制器输出相机运动向量。为了保证手术器械始终内窥镜视图内，追踪控制器根据手术器械尖端和视图中心点的距离来判断是否改变相机的位姿。同时为了保证内窥镜视图的稳定性，如果手术器械尖端中心与视图中心之间的距离小于阈值r，则内窥镜不动。反之，内窥镜需要根据手术器械尖端在视图中的位置和手术器械尖端与视图中心的距离进行运动。最后通过末端执行器到相机位姿的坐标转换、逆向运动学求解，获得机器人各个关节的速度，将机器人各个关节的速度发送给机器人。本发明提出了一种新的内窥镜术野自适应调整方法，在不需要估计手术器械深度的情况下通过机器人自动调整内窥镜术野范围，系统稳定，鲁棒性好。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中一种人机协同的微创内窥镜持镜机器人系统的框图；

图2为本发明实施例中一种人机协同的微创内窥镜持镜机器人系统的工作流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本申请实施例通过提供一种人机协同的微创内窥镜持镜机器人系统，解决了现有的人机协同的微创内窥镜持镜机器人系统鲁棒性差的技术问题，实现在不需要估计手术器械深度的情况下自动调整内窥镜术野范围，提高系统稳定性，具有较好的鲁棒性。

本申请实施例中的技术方案为解决上述技术问题，总体思路如下：

采用视觉伺服的控制方法实现自主持镜可以改善医助配合不默契、长时间持镜肌肉酸痛等带来欠佳视觉反馈的问题。当需要调整相机视角时，控制器可以根据视图中手术器械的位置，驱动机器人移动腹腔镜，使得重要的手术器械投影向视图中央移动。通过这种控制策略可以自动调整相机位姿，以更好的视角给外科医生展示更好的手术场景。上述方法需要两个步骤：(1)手术器械的定位。(2)设计控制器调整内窥镜相机位姿。现有的手术器械定位的方法可分为基于深度学习的方法和基于传统手工特征的方法，采用手工特征检测方法，基于颜色、纹理、形状等特征信息，但鲁棒性差，受光线、气泡、胆汁等环境的干扰，添加标志物时，标志物通常不在手术器械尖端，且容易受污染、遮挡等，使得控制器准确度不高。基于Fast RCNN等深度学习算法计算复杂度高，难以达到实时性。同时，当存在多手术器械时，很难自适应调整算法保证所有手术器械在视图内。且无法自适应的调整内窥镜插入体内的距离，存在与危险部位碰撞的风险。本发明实施例提出了一种人机协同的微创内窥镜持镜机器人系统及工作方法，可以在不需要估计手术器械深度的情况下自动调整内窥镜术野范围，同时可以实时定位和跟踪多个手术器械，保证手术器械都在视图内，并防止内窥镜移出患者身体。

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

本发明实施例提供了一种人机协同的微创内窥镜持镜机器人系统，其包括：内窥镜模块、机器人和数据处理模块。

其中：

内窥镜模块用于拍摄内窥镜视图，并将内窥镜视图传输给数据处理模块；

机器人用于扶持内窥镜，并通过控制机器人每个关节的位姿调整内窥镜术野范围；

数据处理模块用于对内窥镜视图进行图像处理和执行控制算法，完成机器人控制指令的生成和发送。

需要说明的是，在本发明实施例中，数据处理模块是指计算机，机器人是指协作型机械臂，本发明实施例中的人机协同的微创内窥镜持镜机器人系统的框图如图1所示。

数据处理模块获得内窥镜视图后，采用改进的YOLOv3算法获取内窥镜视图中的手术器械尖端的定位，同时通过协作型机械臂正运动学建模求解内窥镜相机的位姿。在相机参数已知的情况下，追踪控制器输出相机运动向量。为了保证手术器械始终内窥镜视图内，追踪控制器根据手术器械尖端和视图中心点的距离来判断是否改变相机的位姿。同时为了保证内窥镜视图的稳定性，如果手术器械尖端中心与视图中心之间的距离小于阈值r(r的设置与手术医生的习惯和手术类型等因素有关。在实际使用过程中，可根据临床需求来定)，则内窥镜不动。反之，内窥镜需要根据手术器械尖端在视图中的位置和手术器械尖端与视图中心的距离进行运动。最后通过末端执行器(末端执行器指的是一个连接在机器人边缘(关节)的用来扶持内窥镜的工具)到相机位姿的坐标转换、逆向运动学求解，获得协作型机械臂各个关节的速度，将协作型机械臂各个关节的速度发送给协作型机械臂。具体流程如图2所示。

基于同一发明构思，本发明实施例还提供了一种控制人机协同的微创内窥镜持镜机器人的工作方法，该工作方法由内窥镜自主控制系统执行，(在本发明实施例中，内窥镜自主控制系统为计算机，机器人是指协作型机械臂)该工作方法包括：

S1、获取内窥镜视图和协作型机械臂位姿；获取机器人各关节状态，并根据机器人正向运动学原理求解内窥镜相机位姿；

S2、采用改进的YOLOv3算法检测内窥镜视图中的手术器械尖端位置；

S3、对于视图中每一个手术器械尖端，获取手术器械尖端到视图中心点的距离；

S4、基于手术器械尖端位置、手术器械尖端到视图中心点的距离、内窥镜相机位姿和相机参数获取视觉追踪向量；

S5、获取内窥镜插入体内距离约束向量；

S6、基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度。

下面对各个步骤进行详细说明：

在步骤S1中，获取内窥镜视图和协作型机械臂位姿；获取机器人各关节状态，并根据机器人正向运动学原理求解内窥镜相机位姿。具体实施过程如下：

计算机中的opencv实时接收和读取内窥镜视图和协作型机械臂每个关节的状态，通过协作型机械臂每个关节的状态判断协作型机械臂位姿。

正向运动学建模是指采用机器人的运动方程，根据机器人各关节的相关状态参数求解末端执行器的位姿，然后通过末端执行器到内窥镜相机的位姿转换，最终求得内窥镜相机的位姿。常用的正向运动学求解方法有D-H参数法。该过程为本领域人员的公知常识，此处不再赘述。

在步骤S2中，采用改进的YOLOv3算法检测内窥镜视图中的手术器械尖端位置。具体实施过程如下：

改进的YOLOv3算法的改进之处包括：修改输入层的大小以自适应内窥镜图像大小(不再压缩内窥镜图像)；修改anchor的值：训练算法时，对各手术器械的尺寸进行聚类，聚类时：特别注意要考虑手术器械在不同闭合程度时的状态，针对不同的闭合状态获取不同的聚类结果，并将聚类结果作为算法的anchors参数；缩减网络的层数，以加快网络的运行时间，保证手术器械定位的实时性。

通过YOLOv3算法检测内窥镜视图中的手术器械尖端位置为本领域人员的公知常识，此处不再赘述。

在步骤S3中，对于视图中每一个手术器械尖端，获取手术器械尖端到视图中心点的距离。具体实施过程如下：

其中：

(u₀，v₀)表示内窥镜图像中心点的位置，由上述步骤S3中的YOLOv3算法检测得到；

(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置。

在步骤S4中，基于手术器械尖端位置、手术器械尖端到视图中心点的距离、内窥镜相机位姿和相机参数获取视觉追踪向量。具体实施过程如下：

相机参数是指的相机的内部参数，是与相机自身特性相关的参数，比如相机的焦距、像素大小等。如公式中的(f_x，f_y，u₀，v₀)这个可以通过相机出厂说明获得，或者自己用标定工具来标定。

基于相机参数获取手术器械尖端中心点在相机坐标系下三维位置和手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；具体为：

其中：

(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置；

(x_cam，y_cam，z_cam)表示内窥镜相机坐标系；

z_i表示手术器械尖端中心在相机坐标系下的深度；

P_i表示手术器械尖端中心点在相机坐标系下三维位置；

P′_i表示手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置。

计算视觉追踪向量：

和/>符合以下规则：

V_max表示内窥镜相机所能达到的最大速度；

当d_i≤r时，当d_i≥R时，/>且/>

在步骤S5中，获取内窥镜插入体内距离约束向量。具体实施过程如下：

其中：

z_EEF表示内窥镜轴所在的z轴单位向量；

l表示内窥镜相机到创口点的距离；

是用球坐标系表示的内窥镜安全操作区域，假设医生设定的插入深度为λ为一个比例系数常量；

符合以下规则：

当时，/>

当时，/>

表示内窥镜插入体内距离约束向量的大小。

在步骤S6中，基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度。具体实施过程如下：

内窥镜术野自适应调整时内窥镜相机运动向量：

其中：

表示相机运动向量；

n表示画面中手术器械的个数；

γ_i表示在追踪第i个手术器械时该手术器械所占的权重；

v_i表示追踪第i个手术器械时的视觉追踪向量；

v_c表示内窥镜插入体内距离约束向量；

ξ(.)为一个向量大小约束函数，计算公式为：

V_max表示内窥镜相机所能达到的最大速度；

根据内窥镜相机运动向量计算协作型机械臂各关节速度。具体为：

根据内窥镜相机运动向量获取内窥镜相机到末端执行器的位姿转换，根据机器人逆向运动学和内窥镜相机到末端执行器的位姿转换求解协作型机械臂的各关节速度。这计算过程在机器人领域较为成熟，为本领域技术人员的公知常识，此处不再赘述。

综上所述，与现有技术相比，具备以下有益效果：

1、本发明实施例提出了一种新的内窥镜术野自适应调整方法，在不需要估计手术器械深度的情况下自动调整内窥镜术野范围，系统稳定，鲁棒性好。

2.本发明实施例逐个跟踪画面中的手术器械，且给手术器械赋予不同的权重，不受手术器械数量限制，在手术器械进入画面和移出画面时都具有很好的鲁棒性。

3.本发明实施例能够自主调整内窥镜插入患者体内的深度，保证安全性。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种人机协同的微创内窥镜持镜机器人系统，其特征在于，所述系统包括：

内窥镜模块，用于拍摄内窥镜视图，并将内窥镜视图传输给数据处理模块；

机器人，用于扶持内窥镜，并通过控制机器人每个关节的位姿调整内窥镜术野范围；

数据处理模块，用于对所述内窥镜视图进行图像处理和执行控制算法，完成机器人控制指令的生成和发送；

数据处理模块获得所述内窥镜视图后，采用YOLOv3算法获取内窥镜视图中的手术器械尖端的定位，同时通过机器人正运动学建模求解内窥镜相机位姿；

基于所述手术器械尖端的定位和所述相机位姿在相机参数已知的情况下，追踪控制器输出相机运动向量；

所述追踪控制器根据手术器械尖端和视图中心点的距离来判断是否改变相机的位姿；

如果所述手术器械尖端中心与视图中心之间的距离小于阈值r，则内窥镜不动，反之，内窥镜需要根据手术器械尖端在视图中的位置和手术器械尖端与视图中心的距离进行运动；

通过末端执行器到相机位姿的坐标转换、逆向运动学求解，获得机器人各个关节的速度；

将机器人各个关节的速度发送给所述机器人。

2.如权利要求1所述的人机协同的微创内窥镜持镜机器人系统，其特征在于，所述手术器械尖端的定位包括：手术器械尖端位置和手术器械尖端到视图中心点的距离。

3.如权利要求2所述的人机协同的微创内窥镜持镜机器人系统，其特征在于，所述基于手术器械尖端的定位和相机位姿在相机参数已知的情况下，追踪控制器输出相机运动向量，包括：

基于所述手术器械尖端位置、手术器械尖端到视图中心点的距离和相机参数获取视觉追踪向量；

获取内窥镜插入体内距离约束向量；

基于所述视觉追踪向量和所述内窥镜插入体内距离约束向量相机运动向量。

4.如权利要求3所述人机协同的微创内窥镜持镜机器人系统，其特征在于，所述基于器械尖端位置、器械尖端到视图中心点的距离和相机参数获取视觉追踪向量，包括：

基于所述相机参数获取手术器械尖端中心点在相机坐标系下三维位置和手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；具体为：

其中：(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置；

(x_cam，y_cam，z_cam)表示内窥镜相机坐标系；

z_i表示手术器械尖端中心在相机坐标系下的深度；

P_i表示手术器械尖端中心点在相机坐标系下三维位置；

P′_i表示手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；

计算视觉追踪向量：

和/>符合以下规则：

V_max表示内窥镜相机所能达到的最大速度；

当d_i≤r时，当d_i≥R时，/>且/>

5.如权利要求3所述人机协同的微创内窥镜持镜机器人系统，其特征在于，所述获取内窥镜插入体内距离约束向量，包括：

其中：

z_EEF表示内窥镜轴所在的z轴单位向量；

l表示内窥镜相机到创口点的距离；

是用球坐标系表示的内窥镜安全操作区域，假设医生设定的插入深度为/>λ为一个比例系数常量；

符合以下规则：

当时，/>

当时，/>

表示内窥镜插入体内距离约束向量的大小；

和/或

所述基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度，包括：

基于视觉追踪向量和内窥镜插入体内距离约束向量获取内窥镜术野自适应调整时内窥镜相机运动向量：

其中：

表示相机运动向量；

n表示画面中手术器械的个数；

γ_i表示在追踪第i个手术器械时该手术器械所占的权重；

v_i表示追踪第i个手术器械时的视觉追踪向量；

v_c表示内窥镜插入体内距离约束向量；

ξ(.)为一个向量大小约束函数，计算公式为：

V_max表示内窥镜相机所能达到的最大速度；

根据内窥镜相机运动向量计算机器人各关节速度。

6.一种控制人机协同的微创内窥镜持镜机器人的工作方法，其特征在于，所述工作方法包括：

获取内窥镜视图和机器人位姿；获取机器人各关节状态，并根据机器人正向运动学原理求解内窥镜相机位姿；

采用YOLOv3算法检测内窥镜视图中的手术器械尖端位置；

基于视图中手术器械尖端位置，获取手术器械尖端到视图中心点的距离；

基于手术器械尖端位置、手术器械尖端到视图中心点的距离和相机参数获取视觉追踪向量；

获取内窥镜插入体内距离约束向量；

基于所述视觉追踪向量和所述内窥镜插入体内距离约束向量获取协作型机械臂各关节速度。

7.如权利要求6所述的控制人机协同的微创内窥镜持镜机器人的工作方法，其特征在于，基于视图中每一个手术器械尖端位置，获取手术器械尖端到视图中心点的距离，包括：

其中：

(u₀，v₀)表示内窥镜图像中心点的位置；

(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置。

8.如权利要求6所述的控制人机协同的微创内窥镜持镜机器人的工作方法，其特征在于，基于器械尖端位置、器械尖端到视图中心点的距离和相机参数获取视觉追踪向量，包括：

基于相机参数获取手术器械尖端中心点在相机坐标系下三维位置和手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；具体为：

其中：(u_i，v_i)表示手术器械尖端中心点在内窥镜图像中的位置；

(x_cam，y_cam，z_cam)表示内窥镜相机坐标系；

z_i表示手术器械尖端中心在相机坐标系下的深度；

P_i表示手术器械尖端中心点在相机坐标系下三维位置；

P′_i表示手术器械尖端中心点在相机坐标系下缩放到单位深度时的三维位置；

计算视觉追踪向量：

和/>符合以下规则：

V_max表示内窥镜相机所能达到的最大速度；

当d_i≤r时，当d_i≥R时，/>且/>

9.如权利要求6所述的控制人机协同的微创内窥镜持镜机器人的工作方法，其特征在于，所述获取内窥镜插入体内距离约束向量，包括：

其中：

z_EEF表示内窥镜轴所在的z轴单位向量；

l表示内窥镜相机到创口点的距离；

是用球坐标系表示的内窥镜安全操作区域，假设医生设定的插入深度为/>λ为一个比例系数常量；

符合以下规则：

当时，/>

当时，/>

表示内窥镜插入体内距离约束向量的大小。

10.如权利要求6所述的控制人机协同的微创内窥镜持镜机器人的工作方法，其特征在于，所述基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度，包括：

基于视觉追踪向量和内窥镜插入体内距离约束向量获取内窥镜术野自适应调整时内窥镜相机运动向量：

其中：

表示相机运动向量；

n表示画面中手术器械的个数；

γ_i表示在追踪第i个手术器械时该手术器械所占的权重；

v_i表示追踪第i个手术器械时的视觉追踪向量；

v_c表示内窥镜插入体内距离约束向量；

ξ(.)为一个向量大小约束函数，计算公式为：

V_max表示内窥镜相机所能达到的最大速度；

根据内窥镜相机运动向量计算机器人各关节速度。