CN115972208B 目标跟随控制方法、持镜机器人及计算机可读介质 （杭州华匠医学机器人有限公司）

通过将固结在内窥镜上的坐标系(即{fn})的原点恰与RCM点重合，无需位置或速度层面的运动坐标系移动的位移及速度，从而实现维持RCM点位置的内窥镜控制。采用以上D_H坐标系获得的雅可比矩阵J建立的运动学模型，能够直接描述控制周期中内窥镜的微分运动与持镜臂的各转持镜臂逆解，且无需再RCM约束情况下规划空间轨迹，此控制方式引起的运动自然而然地满足2接收内窥镜输入的图像数据并根据所述图像获取所述视野图像的图像中心点的坐标和视野图像中待跟随目标的目标中心点的坐若所述目标中心点未进入以所述图像中心点为中心的设定将所述一个控制周期中内窥镜的微分运动输入至一个运动学模型，以根据所述控制周期内持镜臂的各转动关节角度变化量输出控制所述运动学模型根据一个描述持镜臂关节空间速度与笛卡尔空间中RCM点处速度关系准D_H规则确定。入以所述图像中心点为中心的设定阈值范围内，则以图像中心点为原点建立图像坐标系，3.根据权利要求2所述的目标跟随控制方法，其将所述t’分别在RCM坐标系的xne轴与ynem轴上的投影与定义的速度转换系数相乘，将所述内窥镜绕RCM坐标系旋转在xrem轴与ynea轴上的速度分量乘控制周期，得到一4.根据权利要求3所述的目标跟随控制方法，其特征在于，所述运动学模型具体为：3j-表示雅可比矩阵J的逆矩阵；表示持镜臂夹持内窥镜沿dθ1至dθ6表示为持镜臂各转动关节的角速度；将一个控制周期中内窥镜绕RCM坐标系旋转在xrem轴与yrem轴上的微分运动，分别赋4是否进入以所述图像中心点为中心的设定阈值范围内之前，所述目标跟随控制方法还包若所述直线距离小于或等于设定距离阈值，则输出控制若所述直线距离大于设定距离阈值，则执行所述判断所述显示设备，用于显示所述内窥镜的镜头所拍摄的图像，且所述所述持镜臂与所述处理器电连接，所述处理器根据所述内窥镜的镜头所述驱动装置与所述处理器通信连接，用于响应于所述处理器输出驱使所述可弯关节发生形变以使所述镜头拍摄的图像中心点向所述目标中心点移动的力行时实现如权利要求1至7中任意一项所述的目标跟5采用数值解迭代的方式进行求解则算法实时性难以保证，且存在误差。在RCM约束的情况镜臂，用于操控内窥镜围绕一个RCM点运动，所述持镜臂具有通过转动关节连接的多个连[0008]获取所述视野图像的图像中心点的坐标和视野图像中待跟随目标的目标中心点6[0014]所述运动学模型根据一个描述持镜臂关节空间速度与笛卡尔空间中RCM点处速度随控制方法通过将固结在内窥镜上的坐标系(即{fn})的原点恰与RCM点重合，无需位置或镜的微分运动与持镜臂的各转动关节角度变化量的联系，使得本申请无需求解持镜臂逆[0027]图3是根据本申请一种实施例的目标跟随控制方法中图像坐标系到RCM坐标系的[0028]图4是根据本申请一种实施例的目标跟随控制方法中硬镜和软体内窥镜的拍摄角7[0038]如图1所示，在本申请的一实施例中，所述目标跟随控制方法包括如下S100至[0041]S300，判断所述目标中心点是否进入以所述图像中心点为中心的设定阈值范围述运动学模型输出此控制周期内持镜臂的各转动关[0047]具体的，所述运动学模型根据一个描述持镜臂关节空间速度与笛卡尔空间中RCM8无需位置或速度层面的运动约束，自然而然地会产生{fn}姿态变化而位置不变的运动规满足RCM约束，简化持械臂的转动关节角度的求解，以给操作者提供准确和实时的视觉反得到一个控制周期中内窥镜绕RCM坐标系旋转在xrcm轴与yrcm轴上θ1至d9xyθ1个控制周期中内窥镜绕RCM坐标系旋转在xrcm轴与yrcm轴上的微分运动赋[0071]3)至中各行列元素均可用符号表示，则雅可比矩阵J=az比矩阵J求解完成。[0077]将公式4的两端同时左乘雅可比矩阵J的广义逆矩阵J+得到以上内容所述的公式[0078]在本申请的一实施例中，对于夹持硬镜的持镜机器人，RCM点的标定包括以下步[0081]3)在将内窥镜插入RCM点且调整完成初始位置后，通过所述上位机界面点击标定按键，则控制系统可通过各转动关节处的角度及已知的连杆杆长计算RCM点相对于持镜机窥镜的镜头所拍摄的图像控制持镜臂调整所述内上位机界面程序和能获取腔镜图像并根据腔镜图