目标跟随控制方法、持镜机器人及计算机可读介质

技术领域

本申请涉及医疗设备辅助控制技术领域，特别是涉及一种目标跟随控制方法、持镜机器人及计算机可读介质。

背景技术

在内窥镜术中，持镜机器人能够辅助外科医生进行手术视野的自动调整。持镜机器人通过视觉反馈实现内窥镜的快速定位，进而自主地调整手术视野，辅助医生完成手术操作。

持镜机器人的目标跟随，即根据视觉定位程序给出的目标位姿，控制持镜机器人连续运动，在RCM约束的情况下，使内窥镜连续跟随目标的运动而运动，从而使目标在视野图像中始终处于合适位置。

现有技术中，持镜机器人控制方式多基于持镜臂的逆运动学实现，需要在内窥镜连接件处、RCM点处、内窥镜末端各自建立了一个坐标系，在求解内窥镜镜头速度与持镜臂速度间的关系时，需要同时考虑位置、速度层面的RCM约束，方可得到维持RCM点的内窥镜运动规律；且使用解析方式求解逆运动学解过程繁琐，部分构型难以求出准确的解析解。另外采用数值解迭代的方式进行求解则算法实时性难以保证，且存在误差。在RCM约束的情况下，跟踪目标轨迹的规划无法像自由空间内规划。

发明内容

基于此，有必要针对用解析方式求解逆运动学解过程繁琐，且部分构型难以求出准确的解析解，而且采用数值解迭代的方式进行求解则算法实时性难以保证，且存在误差，在RCM约束的情况下，跟踪目标轨迹的规划无法像自由空间内规划的问题，提供一种目标跟随控制方法、持镜机器人及计算机可读介质。

本申请提供一种目标跟随控制方法，应用于持镜机器人，所述持镜机器人包括持镜臂，用于操控内窥镜围绕一个RCM点运动，所述持镜臂具有通过转动关节连接的多个连杆；其特征在于，所述目标跟随控制方法包括：

接收内窥镜输入的图像数据并根据所述图像数据生成视野图像；

获取所述视野图像的图像中心点的坐标和视野图像中待跟随目标的目标中心点的坐标；

判断所述目标中心点是否进入以所述图像中心点为中心的设定阈值范围内；

若目标点未进入以所述图像中心点为中心的设定阈值范围内，则以图像中心点为原点建立图像坐标系，以RCM点为原点建立RCM坐标系，建立图像坐标系到RCM坐标系的映射关系；

根据目标中心点在RCM坐标系上的投影，计算一个控制周期中内窥镜的微分运动；

将所述一个控制周期中内窥镜的微分运动输入至一个运动学模型，以使所述运动学模型输出此控制周期内持镜臂的各转动关节角度变化量；

根据所述控制周期内持镜臂的各转动关节角度变化量输出控制信号，以控制图像中心点向目标中心点运动；

所述运动学模型根据一个描述持镜臂关节空间速度与笛卡尔空间中RCM点处速度关系雅可比矩阵J建立；雅可比矩阵J基于所述持镜臂的D-H坐标系获得；所述持镜臂的D-H坐标系以{f

在所述持镜臂的D-H坐标系中，{f

本申请还提供一种持镜机器人，包括：

处理器，用于执行上述内容所述的目标跟随控制方法；

内窥镜，与所述处理器电连接；

显示设备，用于显示所述内窥镜的镜头所拍摄的图像，且所述显示设备与所述处理器通信连接。

持镜臂，所述内窥镜设置于所述持镜臂之上；

所述持镜臂与所述处理器电连接，所述处理器根据所述内窥镜的镜头所拍摄的图像控制持镜臂调整所述内窥镜的拍摄图像。

本申请还提供一种计算机可读介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现如上述内容所述的目标跟随控制方法。

本申请涉及一种目标跟随控制方法、持镜机器人及计算机可读介质，其中目标跟随控制方法通过将固结在内窥镜上的坐标系(即{f

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，使得本申请的其它特征、目的和优点变得更明显。本申请的示意性实施例附图及其说明用于解释本申请，并不构成对本申请的不当限定。

图1是根据本申请一种实施例的目标跟随控制方法的流程示意图，

图2是根据本申请一种实施例的目标跟随控制方法中D-H坐标系的示意图，

图3是根据本申请一种实施例的目标跟随控制方法中图像坐标系到RCM坐标系的映射的示意图。

图4是根据本申请一种实施例的目标跟随控制方法中硬镜和软体内窥镜的拍摄角度对比示意图。

图5是根据本申请一种实施例的目标跟随控制方法中腔镜坐标系的示意图。

图6为本申请一实施例提供的持镜机器人的结构框图。

图7为本申请一实施例提供的持镜机器人中持镜机械臂的立体示意图。

图8为本申请一实施例提供的持镜机器人的系统拓扑图。

图9为本申请一实施例提供的持镜机器人中内窥镜的结构示意图。

附图标记：

100-持镜机器人；110-处理器；120-显示设备；130-内窥镜；131-内窥镜本体；132-镜头；133-可弯关节；140-持镜机械臂。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

本申请提供一种目标跟随控制方法。需要说明的是，本申请提供的视野控制方法不限制其执行主体。可选地，本申请提供的视野控制方法应用于一种持镜机器人，应用于持镜机器人，所述持镜机器人包括持镜臂，用于操控内窥镜围绕一个RCM点运动，所述持镜臂具有通过转动关节连接的多个连杆。

如图1所示，在本申请的一实施例中，所述目标跟随控制方法包括如下S100至S700：

S100，接收内窥镜输入的图像数据并根据所述图像数据生成视野图像。

S200，获取所述视野图像的图像中心点的坐标和视野图像中待跟随目标的目标中心点的坐标。

S300，判断所述目标中心点是否进入以所述图像中心点为中心的设定阈值范围内。

S400，若目标点未进入以所述图像中心点为中心的设定阈值范围内，则以图像中心点为原点建立图像坐标系，以RCM点(即远程运动中心)为原点建立RCM坐标系，建立图像坐标系到RCM坐标系的映射关系。

S500，根据目标中心点在RCM坐标系上的投影，计算一个控制周期中内窥镜的微分运动。

S600，将所述一个控制周期中内窥镜的微分运动输入至一个运动学模型，以使所述运动学模型输出此控制周期内持镜臂的各转动关节角度变化量。

S700，根据所述控制周期内持镜臂的各转动关节角度变化量输出控制信号，以控制图像中心点向目标中心点运动。

具体而言，在各控制周期内重复上述S100至S700，直至目标中心点进入以所述图像中心点为中心的设定阈值范围内，实现对待跟随目标的跟踪。

具体的，所述运动学模型根据一个描述持镜臂关节空间速度与笛卡尔空间中RCM点处速度关系雅可比矩阵J建立。雅可比矩阵J基于所述持镜臂的D-H坐标系获得。

如图2所示，采用标准D-H方法，在持镜臂各转动关节处建立固结在其连杆上的坐标系，所述持镜臂的D-H坐标系以{f

在所述持镜臂的D-H坐标系中，{f

在本实施例中，通过将固结在内窥镜上的坐标系(即{f

在持镜机器人的一个控制周期中，首先由视觉系统获取腔镜图像中跟踪目标的图像二维坐标系，在传统方法中，通常利用此二维坐标系及已标定好的相机参数，估计目标相对于相机的三维坐标，但由于腔镜的光学系统在使用过程中需要改变焦距等内参，故上述传统方法无法使用。

如图3所示，在本申请的一实施例中，所述S400包括如下S410和S420。

S410，建立图像坐标系，其以图像中心点为原点，且图像坐标系的x轴定义为与视野图像一边平行的x

S420，定义RCM坐标系，其原点为RCM点，RCM坐标系的x轴定义为与x

如图3所示，在本申请的一实施例中，所述S500包括如下S510和S530。

S510，计算t’分别在RCM坐标系的x

S520，将所述t’分别在RCM坐标系的x

S530，将所述内窥镜绕RCM坐标系旋转在x

在本申请的一实施例中，若n＝6，则表示本申请的持镜臂具有6个转动关节，所述运动学模型具体为：

其中，J

若n≠6，则表示本申请的持镜臂的转动关节数量大于6或小于6，所述运动学模型具体为：

其中，J

在本申请的一实施例中，所述S600包括如下S610。

S610，将一个控制周期中内窥镜绕RCM坐标系旋转在x

具体的，当持镜臂的转动关节数量为6时，一个控制周期中内窥镜绕RCM坐标系旋转在x

具体的，如图2所示，本申请中所述运动学模型中雅可比矩阵J的求解方法包括以下步骤：

1)将D-H坐标系中各个坐标系间的齐次变换矩阵(4×4矩阵)A用n、o、a、p这四个列向量表示，其中A

2)计算齐次变换矩阵

3)

在本申请的一实施例中，当持镜臂的转动关节数量为6时，对于雅可比矩阵J，有:

将公式3的两端同时左乘雅可比矩阵J的逆矩阵J

在本申请的一实施例中，当持镜臂的转动关节数量大于6或小于6时，对于雅可比矩阵J，有:

将公式4的两端同时左乘雅可比矩阵J的广义逆矩阵J

在本申请的一实施例中，对于夹持硬镜的持镜机器人，RCM点的标定包括以下步骤。

1)首先移动机械臂，使其末端带动内窥镜插入RCM点。

2)可采用零力拖动或单独控制各转动关节移动使内窥镜末端达插入RCM点。

3)在将内窥镜插入RCM点且调整完成初始位置后，通过所述上位机界面点击标定按键，则控制系统可通过各转动关节处的角度及已知的连杆杆长计算RCM点相对于持镜机器人的相对位置。

其中零力拖动指建立持镜机器人的动力学模型，并根据动力模型使持镜机器人关节电机实时输出的力矩，能补偿持镜机器人杆件所受重力、关节处的摩擦力等，从而使操作者能直接拖动持镜机器人进行运动

如图4所示，当持镜臂夹持自行设计的软体内窥镜时，当软体内窥镜在曲率较小时可实现类似于硬镜的目标跟踪。当软体内窥镜在曲率较大时，则可实现对目标可实现更多姿态的观察。

利用上述软体内窥镜特点，在本申请的一实施例中，在S300之前，所述目标跟随控制方法还包括以下S210至S240。

S210，计算所述目标中心点与图像中心点之间的直线距离。

S220，判断所述直线距离是否大于设定距离阈值。

S230，若所述直线距离小于或等于设定距离阈值，则输出控制信号驱使内窥镜的可弯关节形变，以控制图像中心点向目标中心点运动。

S240，若所述直线距离大于设定距离阈值，则执行所述判断所述目标中心点是否进入以所述图像中心点为中心的设定阈值范围内。

在本示例中，当待跟随目标在较小范围内移动时候，仅利用内窥镜的可弯关节形变移动内窥镜的镜头观察病灶。而当待跟随目标大范围移动时或观察特殊角度时，采用持镜臂移动内窥镜。采用这样的控制方式，减少持镜臂外部各转动关节的运动，对操作者产生更少的干扰。

如图5所示，在本申请的一实施例中，也可采用分段目标跟踪的方法结合可弯关节和持械臂。所述分段目标跟踪的方法包括以下S251至S253。

S251，在内窥镜末端建立腔镜坐标系，坐标系z轴z

S252，获取腔镜坐标系下目标中心点的位置，驱使内窥镜的可弯关节形变控制镜头镜跟随目标中心点。

S253，若内窥镜的可弯关节曲率已经达到其允许的最大值，但待跟随目标仍移动，则启动腹腔外持镜臂的自由度，使整个软体内窥镜均移动，从而具备重新对准目标的能力。

本申请还提供一种持镜机器人。

如图6和图7所示，在本申请一实施例中，持镜机器人100包括：处理器110，内窥镜120和显示设备130和持镜臂140。

处理器，用于执行前述内容提及到的目标跟随控制方法。内窥镜，与所述处理器电连接。显示设备，用于显示所述内窥镜的镜头所拍摄的图像，且所述显示设备与所述处理器通信连接。

具体的，所述显示设备120包括触摸屏。

所述内窥镜设置于所述持镜臂之上，持镜臂用于操控内窥镜围绕一个RCM点(即远程运动中心)运动，所述持镜臂具有通过转动关节连接的多个连杆。持镜机械臂140安装于可移动式的台车，方便转移位置。所述持镜臂与所述处理器电连接，所述处理器根据所述内窥镜的镜头所拍摄的图像控制持镜臂调整所述内窥镜的拍摄图像。

如图8所示，在本申请一实施例中，持镜机器人还包括控制系统，用于人机交互的上位机界面程序和能获取腔镜图像并根据腔镜图像识别跟踪目标(如手术器械、病灶区域等)并计算目标相对于腔镜位姿的视觉定位程序。对于夹持软体内窥镜，持镜机器人自由度布置在上述描述的基础上，增加了内窥镜本身的弯曲自由度。

如图9所示，在本申请一实施例中，所述内窥镜130包括：内窥镜本体131，镜头132和可弯关节133。

内窥镜本体131设置有驱动装置。镜头132用于拍摄图像。可弯关节133一端与所述内窥镜本体131连接，且另一端连接至所述镜头132。

所述驱动装置与所述处理器110通信连接，用于响应于所述处理器110输出的控制信号，输出驱使所述可弯关节133发生形变以使所述镜头132拍摄的图像中心向目标点移动的力矩。

一种计算机可读介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现如前述内容提及到的目标跟随控制方法。

以上所述实施例的各技术特征可以进行任意的组合，各方法步骤也并不做执行顺序的限制，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请的保护范围应以所附权利要求为准。