CN113910219A - 运动臂系统以及控制方法 - Google Patents

运动臂系统以及控制方法 Download PDF

Info

Publication number
CN113910219A
CN113910219A CN202110744106.2A CN202110744106A CN113910219A CN 113910219 A CN113910219 A CN 113910219A CN 202110744106 A CN202110744106 A CN 202110744106A CN 113910219 A CN113910219 A CN 113910219A
Authority
CN
China
Prior art keywords
marker
image
suspected
control method
determining
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.)
Granted
Application number
CN202110744106.2A
Other languages
English (en)
Other versions
CN113910219B (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.)
Beijing Surgerii Technology Co Ltd
Original Assignee
Beijing Surgerii Technology Co Ltd
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 Beijing Surgerii Technology Co Ltd filed Critical Beijing Surgerii Technology Co Ltd
Publication of CN113910219A publication Critical patent/CN113910219A/zh
Application granted granted Critical
Publication of CN113910219B publication Critical patent/CN113910219B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • Surgery (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • General Physics & Mathematics (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Physics & Mathematics (AREA)
  • Biomedical Technology (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)
  • Image Analysis (AREA)

Abstract

本公开涉及机器人器械领域，公开一种运动臂的控制方法，包括获得由图像采集设备采集的图像，图像包括设置在运动臂末端上的标记物的图像，标记物包括多个标记物角点，识别采集的图像中的标记物角点，以及基于识别的标记物角点，确定运动臂末端相对于图像采集设备的当前相对位姿。基于视觉跟踪的闭环控制方法及设置的标记物，实时检测运动臂末端的姿态，有效降低运动臂的轨迹跟踪误差，可降低运动控制误差。

Description

运动臂系统以及控制方法
技术领域
本公开涉及机器人领域，尤其涉及运动臂系统以及控制方法。
背景技术
现有的机器人系统的运动臂控制方法采用的闭环到位校正方式，主要通过从运动臂运动关节的到位情况进行校正数据的获取，以确定闭环补偿的补偿情况。这种补偿方式不能解决设备自身的误差导致的到位精度问题，尤其是针对精密的运动臂闭环控制。
现有空间定位设备通常采用对既有定位标识的识别来确定定位标识所在物的空间位置。目前实现这样的识别和控制方式有许多方案，但各有局限，如对于标记小球的空间定位需要双目摄像头，对设备要求较多。还有一种方案采用明暗或黑白对比的标记作为标记物，采用对图像内每个像素点进行标记物可能性计算或者采用既有模板对像素内的所有像素在一定范围内进行模板匹配。这样的方式或者容易将不是标记物的部分误识为标记物或者具有非常巨大的计算量，不适合高精度实时闭环校正算法的应用。
综上，在运动臂的闭环控制领域还有许多课题和技术亟待处理。
发明内容
在一些实施例中，本公开提供了一种运动臂的控制方法，包括：获得由图像采集设备采集的图像，所述图像包括设置在所述运动臂末端上的标记物的图像，所述标记物包括多个标记物角点；识别所述采集的图像中的标记物角点；以及基于所述识别的标记物角点，确定所述运动臂末端相对于所述图像采集设备的当前相对位姿。
在一些实施例中，本公开提供了一种运动臂系统，包括：图像采集设备，用于采集图像；至少一个运动臂，包括运动臂末端，所述运动臂末端上设有标记物，所述标记物包括多个标记物角点；控制装置，被配置成执行根据本公开一些实施例的控制方法。
在一些实施例中，本公开提供了一种计算机可读存储介质，包括一个或多个其上存储有计算机可执行指令，所述计算机可执行指令由处理器执行已将处理器配置为执行根据本公开一些实施例的控制方法。
附图说明
为了清楚地说明本公开实施例中的技术方案，下面将对本公开实施例描述中所需要使用的附图作简单的介绍。下面描述中的附图仅仅示出本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据本公开实施例的内容和这些附图获得其他的实施例。
图1示出根据本公开一些实施例中的运动臂系统的结构示意图；
图2(a)示出根据本公开一些实施例中的运动臂的结构示意图；
图2(b)示出根据本公开一些实施例中的标记物的结构示意图；
图3示出根据本公开一些实施例中的运动状态下的运动臂末端的放大示意图；
图4示出根据本公开一些实施例中的运动臂系统的控制方法的流程图；
图5示出根据本公开一些实施例中的用于识别图像中的标记物角点的控制方法的流程图；
图6示出根据本公开一些实施例中的疑似标记物角点搜索示意图；
图7示出根据本公开一些实施例中的用于确定运动臂末端在世界坐标系中的位置的方法的流程图；
图8示出根据本公开一些实施例中的连续体柔性臂的结构示意图；
图9示出根据本公开一些实施例中的连续体柔性臂的单个连续体段的结构示意图。
具体实施方式
为使本公开解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图对本公开实施例的技术方案作进一步的详细描述，显然，所描述的实施例仅仅是本公开示例性实施例，而不是全部的实施例。
在本公开的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本公开和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本公开的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。在本公开的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“耦合”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连；可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本公开中的具体含义。在本公开中，在手术机器人系统中，定义靠近用户(例如医生)的一端为近端、近部或后端、后部，靠近手术患者的一端为远端、远部或前端、前部。
在本公开中，术语“位置”指对象或对象的一部分在三维空间中的定位(例如，可使用笛卡尔X、Y和Z坐标方面的变化描述三个平移自由度，例如分别沿笛卡尔X轴、Y轴和Z轴的三个平移自由度)。在本公开中，术语“姿态”指对象或对象的一部分的旋转设置(例如三个旋转自由度，可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指对象或对象的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。在本公开中，运动臂或其一部分的位姿是指运动臂或其一部分定义的坐标系相对于运动臂所在的支架、基座定义的坐标系或世界坐标系的位姿。
图1示出了根据本公开一些实施例的运动臂系统100的结构框图。如图1所示，运动臂系统100可以包括图像采集设备10、至少一个运动臂20和控制装置30。图像采集设备10和至少一个运动臂20分别与控制装置30通信连接。在一些实施例中，如图1所示，控制装置30可以用于控制至少一个运动臂20的运动，以调整至少一个运动臂20的位姿、相互协调等。在一些实施例中，至少一个运动臂20在末端或远端处可以包括运动臂末端21。在一些实施例中，至少一个运动臂20还可以包括运动臂臂体22，运动臂末端21可以为运动臂臂体22的远端部分或者为设置在运动臂臂体22远端的末端执行器。控制装置30可以控制至少一个运动臂20运动，以使运动臂末端21运动至期望的位置和姿态。本领域的技术人应理解，运动臂系统100可以应用于手术机器人系统，例如腔镜手术机器人系统。应当理解，运动臂系统100还可以应用于其他领域(例如，制造、机械等等)的专用或通用机器人系统。
在本公开中，控制装置30可以与至少一个运动臂20的电机通信连接，从而使电机基于驱动信号控制至少一个运动臂20运动到相应的目标位姿。例如，控制运动臂运动的电机可以为伺服电机，可以接受控制装置的指令以控制运动臂运动。控制装置30还可例如通过通信接口与电机耦合的传感器通信连接，以接收运动臂20的运动数据，实现对运动臂20的运动状态监控。在本公开的一个示例中，该通信接口可以为CAN(Controller AreaNetwork)总线通信接口，其使得控制装置30能够通过CAN总线与电机以及传感器连接通信。
在一些实施例中，运动臂20可以包括连续体柔性臂或者由多个关节构成的具有多自由度的运动臂，例如可以实现6个自由度运动的运动臂。图像采集设备10可以包括但不限于双镜头图像采集设备或单镜头图像采集设备，例如双目或单目相机。
图2(a)示出了根据本公开一些实施例的运动臂20的结构示意图。如图2(a)所示，运动臂20可以包括运动臂末端21和运动臂臂体22，运动臂末端21上固定设有标记物211。图2(b)示出了根据本公开一些实施例的标记物211的结构示意图。如图2(b)所示，标记物211可以包括多个标记物角点2111。在一些实施例中，标记物211可以呈筒状，固定包覆于运动臂末端21。标记物211可以包括多个分布在筒状上(例如规律分布)的标记物角点2111。应当理解，标记物可以是在亮度、灰度、色相有明显区别的已知排布的图形。标记物角点可以为标记物上有特殊区别特征的点，标记物角点的周围范围内图形或颜色可以呈点对称分布，以便于图像采集设备采集标记物角点。在一些实施例中，多个标记物角点按规律分布，便于确定标记物角点的参数，例如多个标记物角点之间的分布角度和间隔距离等参数。在一些实施例中，如图2(b)所示，标记物211可以包括但不限于黑白相间的棋盘格标记物，标记物角点2111可以为例如棋盘格标记物中的两个线段的交点。
图3示出根据本公开一些实施例中的运动状态下的运动臂末端的放大示意图。如图3所示，至少一个运动臂可以包括但不限于两个运动臂，每个运动臂末端都设有标记物。
本公开的一些实施例提供了一种运动臂的控制方法。图4示出了根据本公开一些实施例的运动臂系统(例如运动臂系统100)的控制方法400的流程图。如图4所示，该方法400可以由运动臂系统100的控制装置(例如控制装置30)来执行。控制装置30可以配置在计算设备上。方法400可以由软件、固件和/或硬件来实现。
在步骤401，获得由图像采集设备采集的图像。例如，可以由双镜头图像采集设备或单镜头图像采集设备采集运动臂末端的图像，采集的图像中包括运动臂末端上的标记物的图像。
在步骤403，识别采集的图像中的标记物角点。在一些实施例中，可以通过控制装置对采集的图像进行预处理，以识别图像中的标记物角点。识别图像中的标记物角点的示例性方法如图5所示方法详述。
在步骤405，基于识别的标记物角点，确定运动臂末端相对于图像采集设备的当前相对位姿。在一些实施例中，可以基于识别的标记物角点，确定标记物相对于图像采集设备的当前位姿，基于标记物的当前位姿以及标记物相对于运动臂末端的相对位姿，可以确定运动臂末端的当前相对位姿。应当理解，由于标记物固定设置于运动臂末端，标记物相对于运动臂末端的相对位姿是已知的。
图5示出了根据本公开一些实施例的用于识别采集的图像中的标记物角点的方法500的流程图。如图5所示，该方法500可以由运动臂系统(例如运动臂系统100)的控制装置(例如控制装置30)来执行。控制装置30可以配置在计算设备上。方法500可以由软件、固件和/或硬件来实现。
在步骤中501，在采集的图像中确定感兴趣区域(ROI)。在一些实施例中，确定ROI为全图像，或者基于上一帧图像(例如，上一个运动控制循环中所处理的图像)中的疑似标记物角点位置，确定ROI为局部图像，以及将ROI转为相应的灰度图像。例如，可以基于采集的图像，截取全图像或者局部图像作为ROI，并将ROI转为相应的灰度图像，以将每个像素点的灰度信息量化。在一些实施例中，可以基于采集的图像为第一帧图像，确定第一帧图像的ROI为全图像。在一些实施例中，基于采集的图像为非第一帧图像，可以基于上一帧图像中的疑似标记物角点位置，确定ROI为局部图像。
在步骤中503，在ROI中识别疑似标记物角点。例如可以基于每个像素点的灰度信息，确定每个像素点为标记物角点的可能性，将其中可能性高的像素点，确定为疑似标记物角点。
在一些实施例中，局部图像可以包括以上一帧图像中的疑似标记物角点的平均坐标构成的虚点为中心的设定距离范围，设定距离可以包括多个疑似标记物角点平均间隔距离的预定倍数。应理解，例如，预定倍数可以包括但不限于多个疑似标记物角点平均间隔距离的固定倍数，例如两倍。应理解，预定倍数还可以是多个疑似标记物角点平均间隔距离的可变倍数。
在一些实施例中，可以确定ROI中的每个像素点的角点似然值(CL)。将ROI划分成多个子ROI，确定每个子ROI中CL值最大的像素点。基于多个子ROI的多个CL值最大的像素点，确定CL值大于第一阈值的像素点集合。确定像素点集合中CL值最大的像素点，作为第一疑似标记物角点。基于第一疑似标记物角点，以设定步长沿多个边缘方向搜索第二疑似标记物角点。
在一些实施例中，确定ROI中的每个像素点的角点似然值(CL)。例如，对ROI图像范围内每一个像素点进行卷积操作，得到每个像素点的一阶和/或二阶导数。利用前述ROI图像范围内每个像素点的一阶和/或二阶导数求出每个像素点的角点似然值(CL)。
在一些实施例中，ROI图像内的每个像素点的CL可以根据如下公式计算：
CL＝max(cxy,c45)
cxy＝ε2·|Ixy|-1.5·ε·(|I45|+|In45|)
c45＝ε2·|I45_45|-1.5·ε·(|Ix|+|Iy|) (1)
其中，ε为设定常数，Ix、I45、Iy、In45分别是每个像素点在0、π/4、π/2和-π/4四个方向的一阶导数；Ixy和I45_45，分别是每个像素点在0,π/2和在π/4，-π/4方向的二阶导数；cxy是每个像素点在0,π/2方向的CL值，c45是每个像素点在π/4方向的CL值。
在一些实施例中，将ROI划分成多个子ROI。例如，可以采用非极大抑制法在一个ROI图像范围中平均分割出多个子ROI。在一些实施例中，可以将ROI图像平均分割成5×5像素的多个子ROI。上述实施例为示例性的，并非限制性的，应当理解，还可以将ROI图像分割成其他尺寸大小的多个子ROI。可以确定每个子ROI中的CL值最大的像素点，将每个子ROI中的CL值最大的像素点与第一阈值进行比较，确定CL值大于第一阈值的像素点集合。在一些实施例中，第一阈值可以设定为0.06。应当理解，第一阈值还可以设定为其他值。
在一些实施例中，确定像素点集合中CL值最大的像素点，作为第一疑似标记物角点。例如，可以将该像素点集合中的所有像素点让CL值从大到小的顺序排序，并将CL值最大的像素点作为第一疑似标记物角点。基于所述第一疑似标记物角点，以设定步长沿多个边缘方向搜索第二疑似标记物角点。图6示出根据本公开一些实施例中的疑似标记物角点搜索示意图。如图6所示，以第一疑似标记物角点为起点，以设定步长向四个边缘方向依次搜索第二疑似标记物角点。在一些实施例中，设定步长可以为10个像素点。例如，以第一疑似标记物角点为起点，以10个像素点为步长，在边缘方向上依次在设定范围内搜索CL值最大的像素点，作为第二疑似标记物角点。例如，设定范围可以为10×10正方形区域。应理解，上述实施例为示例，并非限制，设定步长还可以为其他数量的像素点，设定范围也可以为其他尺寸大小的区域。例如，可以确定第一疑似标记物角点的四个边缘方向，基于每个边缘方向，以设定步长进行移动搜索，以在设定范围内寻找CL值最大的像素点，作为第二疑似标记物角点。
在一些实施例中，可以通过确定以第一疑似标记物角点为中心的第一预定范围中的各像素点的梯度方向和梯度权重，基于第一预定范围中的各像素点的梯度方向和梯度权重，确定多个边缘方向。例如，第一预定范围可以为10×10正方形区域。以第一疑似标记物角点为中心的10×10正方形像素邻域内，确定各像素点的梯度方向和梯度权重，以确定第一疑似标记物角点的多个边缘方向。应当理解，第二疑似标记物角点也可以通过类似方法确定边缘方向。通过基于疑似标记物角点调整边缘方向，可以对标记物角点进行针对性搜索，以降低运算量。
在一些实施例中，第一预定范围中的各像素点的梯度方向和梯度权重可以通过如下公式计算：
Figure BDA0003142273370000091
其中，Iangle为对应像素点的梯度方向，Iweight为对应像素点的梯度权重；Ix、Iy分别是对应像素点在0、π/2方向的一阶导数。将第一预定范围内的每个像素点的Iangle和Iweight进行聚类方法计算，以获得对应像素点的边缘方向。
在一些实施例中，可以基于第二疑似标记物角点，调整边缘方向，并且沿经调整的边缘方向，搜索第三疑似标记物角点。例如，与以上类似，可以通过确定以第二疑似标记物角点为中心的预定范围中的各像素点的梯度方向和梯度权重，基于预定范围中的各像素点的梯度方向和梯度权重，确定更新的边缘方向，如图6所示。
在步骤中505，基于标准角点模型，对ROI中的疑似标记物角点进行相关性检验，以判断ROI图像中的疑似标记物角点是否为标记物角点。例如，可以将第一疑似标记物角点以及搜索到的疑似标记物角点(例如第二疑似标记物角点)的像素点代入标准角点模型进行相关性判断，以确定疑似标记物角点是否为标记物角点。可以基于角点似然估计(CornerLikelihood)和模板匹配(Template Matching)相结合的角点检测方法，判断图像(例如在图像采集设备为双目图像采集设备时，提取图像包括左镜头图像和右镜头图像)中的标记物角点。
在一些实施例中，确定标准角点模型的灰度分布与以疑似标记物角点的像素点为中心的第二预定范围内像素灰度分布之间的相关性系数，响应于相关性系数大于第三阈值，确定疑似标记物角点为标记物角点。在一些实施例中，标记物可以为黑白相间的棋盘格图形，标准角点模型可以是双曲线正切模型(HTM)。例如，可以基于标准角点模型(例如HTM)的灰度分布与以疑似标记物角点的像素点为中心的预定范围像素邻域灰度分布之间的相关性系数CC，确定疑似标记物角点是否为标记物角点。例如，以疑似标记物角点的像素点为中心的第二预定范围可以包括但不限于10×10正方形像素邻域。相关系系数CC可以如下定义：
Figure BDA0003142273370000101
其中，Gimage为以疑似标记物角点的像素点为中心的预定范围像素邻域灰度分布，GHTM为标准角点模型的灰度分布，Var为方差函数，Cov为协方差函数。
在判断相关系系数CC值大于阈值，表示该像素点为中心的预定范围内的灰度分布与双曲线正切模型相关系较高，可以确定该疑似标记物角点为标记物角点。反之，确定该疑似标记物角点不是标记物角点。在一些实施例中，可以设定阈值为0.8。应当理解，阈值还可以为其他设定值。例如，以第一疑似标记物角点为起点，搜索第二疑似标记物角点，对搜索到的第二疑似标记物角点进行相关性验证，判断相关系系数CC值大于0.8，确定第二疑似标记物角点为标记物角点。
应当理解，可以在每次搜索到疑似标记物角点后或者搜索到多个疑似标记物角点后，将对应的像素点代入标准角点模型进行相关性判断，以确定识别的疑似标记物角点是否为标记物角点。
在一些实施例中，可以基于第一疑似标记物角点和第二疑似标记物角点，向相反边缘方向搜索，以确定其他的疑似标记物角点。如图6所示，以第一疑似标记物角点和第二疑似标记物角点为起点，分别向两个相反边缘方向进行搜索。例如，以第一疑似标记物角点为起点沿远离第二疑似标记物角点的方向搜索，以及以第二疑似标记物角点为起点沿远离第一疑似标记物角点的方向搜索。
在一些实施例中，响应于疑似标记物角点数量大于或等于第二阈值，可以停止搜索。在一些实施例中，第二阈值可以设定为4。应当理解，上述实施例为示例，并非限制，第二阈值还可以设定为5或者其他数值。例如，确定疑似标记物角点的总数量大于或等于4个并且通过相关性验证，表示成功找到标记物，将确定的多个疑似标记物角点作为标记物角点。
在一些实施例中，响应于搜索的距离大于第一疑似标记物角点和第二疑似标记物角点之间的距离的预定倍数或者响应于确定的疑似标记物角点数量小于第二阈值，基于像素点集合中CL值从大到小排序的下一个像素点作为第一疑似标记物角点，以设定步长沿边缘方向搜索疑似标记物角点，直到疑似标记物角点数量大于或等于第二阈值。在一些实施例中，预定倍数可以是第一疑似标记物角点和第二疑似标记物角点之间的距离的两倍，搜索距离大于该预定倍数，当前搜索循环结束。可以基于像素点集合中CL值从大到小排序的下一个像素点作为第一疑似标记物角点，重新开始下一个搜索循环。或者，基于上述方式搜索，确定疑似标记物角点的总数量小于4个，表示未成功识别标记物，基于像素点集合中CL值从大到小排序的下一个像素点作为第一疑似标记物角点，开始下一搜索循环，搜索其他的疑似标记物角点，直到确定疑似标记物角点的总数量大于或等于第二阈值(例如4)并且通过相关性验证。
在一些实施例中，响应于ROI中的疑似标记物角点为标记物角点，对标记物角点进行亚像素定位。通过亚像素定位可以对标记物角点的坐标精度进行优化。在一些实施例中，可以对每个子ROI中的每个像素点的CL值基于模型进行拟合，以确定经亚像素定位后的标记物角点的坐标。例如，每个子ROI中的每个像素点的CL值的拟合函数可以为二次曲面函数，该函数的极值点为亚像素点。拟合函数可以如下：
S(x,y)＝ax2+by2+cx+dy+exy+f (4)
其中，S(x，y)为每个子ROI中的所有像素点的CL值拟合函数，a、b、c、d、e、f为系数；
Figure BDA0003142273370000121
其中，xc为标记物角点的x坐标，yc为标记物角点的y坐标。
在一些实施例中，可以确定标记物角点相对于标记物坐标系的三维坐标，确定标记物角点在采集的图像中的二维坐标，确定二维坐标和三维坐标的单应性矩阵的解析解，基于单应性矩阵的解析解，确定标记物相对于图像采集设备的当前相对位姿。应当理解，可以基于标记物角点的已知参数，确定图像识别的标记物角点的三维坐标。应当理解，基于双镜头图像采集设备，可以将左镜头图像、右镜头图像中的标记物角点对齐后，进行以下处理。例如，标记物角点相对于标记物坐标系的三维坐标可以如下：
Figure BDA0003142273370000122
其中，
Figure BDA0003142273370000123
为第i个标记物角点对应的三维坐标，rwm和β可以是基于标记物的已知特征确定的参数。例如，对于黑白相间的棋盘格标记物呈圆筒设置在运动臂末端，棋盘格标记物角点均匀分布在圆筒标记物周向(如图2(b)所示)，其中，rwm为标记物形成的圆筒的半径，β为标记物角点的分布角。
在一些实施例中，基于单镜头图像采集设备，可以直接基于标记物的已知特征确定的参数(例如形成的圆筒的半径以及标记物角点的分布角)，计算标记物角点在标记物坐标系的三维坐标。基于双镜头图像采集设备，可以对齐左、右图像中各自检测到的标记物角点，以使双目立体矫正后同一空间点在y方向坐标一致，然后基于标记物的已知特征确定的参数，计算标记物角点在标记物坐标系的三维坐标。标记物坐标系可以如下建立：
Figure BDA0003142273370000131
其中，标记物坐标系的原点与标记物中心重合，x轴可以从标记物中心指向第一个标记物角点(例如最先确定的标记物角点)。
在一些实施例中，确定二维坐标和三维坐标的单应性矩阵的解析解可以包括基于如下公式计算：
Figure BDA0003142273370000132
Hleft＝[h1 h2 h3]＝η·A·[r1 r2 llpwm] (8)
式(7)中，
Figure BDA0003142273370000133
为第i个标记物角点的扩展坐标，[ui,vi]为第i个标记物角点的二维坐标，leftsi为任意的非零标量，leftsi为比例因子，以使式(7)左右两边最后一列数字相同，Hleft为单应性矩阵；
式(8)中，A为图像采集设备的已知内部参数矩阵，r1,r2是llRwm矩阵的前两列，ll表示图像采集设备坐标系{ll}(此处以左镜头坐标系为例)，η为任意的非零标量。应当理解，通过引入扩展坐标，可以简化计算过程。
在一些实施例中，单应性矩阵的解析解还可以包括基于如下公式计算：
Figure BDA0003142273370000134
其中，xH＝[h1 T h2 T h3 T]T，L是2n×9矩阵，n为采集的图像中确定的标记物角点数量，xH为矩阵L的奇异值分解(SVD)后最小奇异值对应的右奇异向量。
在一些实施例中，可以基于以下公式，确定标记物相对于图像采集设备的当前相对位姿：
r1＝κ·A-1·h1
r2＝κ·A-1·h2
llRwm＝[r1 r2 r1×r2]
llpwm＝κ·A-1h3 (10)
其中，κ＝1/||A-1·h1||＝1/||A-1·h2||。
在一些实施例中，可以基于每个标记物角点的二维坐标和三维坐标之间的单应性矩阵的解析解，执行非线性优化处理以获得标记物相对于图像采集设备的当前相对位姿。例如，建立非线性优化处理，通过代数法计算标记物相对于图像采集设备的当前相对位姿。例如，非线性优化处理可以包括使三维坐标在采集的图像上的投影与标记物角点之间的平均几何误差最小化。
在一些实施例中，图像采集设备是单镜头图像采集设备，采集的图像是单镜头(此处以左镜头为例，表示为“left”)图像，非线性优化处理可以包括基于如下公式优化：
Figure BDA0003142273370000141
在一些实施例中，图像采集设备为双镜头图像采集设备，采集的图像包括左镜头(表示为“left”)图像和右镜头(表示为“right”)图像，非线性优化处理可以包括基于如下公式优化：
Figure BDA0003142273370000142
其中，llRwm和llpwm，分别为标记物在图像采集设备坐标系(或者左镜头坐标系){ll}中的姿态和位置；Hleft和Hright分别为与左镜头图像、右镜头图像相关的单应性矩阵；leftsi和rightsi，分别为与左镜头图像、右镜头图像相关的单应性矩阵的解析解相关的比例因子，
Figure BDA0003142273370000143
和
Figure BDA0003142273370000144
分别为与左镜头图像、右镜头图像相关的第i个标记物角点的扩展坐标。应当理解，Hleft为左镜头图像采集的标记物角点的二维坐标与三维坐标对应的单应性矩阵，Hright为右镜头图像采集的标记物角点的二维坐标与三维坐标对应的单应性矩阵，leftsi为与Hleft的解析解相关的非零标量，为rightsi与Hright的解析解相关的非零标量。例如，在图像采集设备为双镜头图像采集设备时，可以通过左镜头图像、右镜头图像中标记物角点的二维坐标及三维坐标的对应关系，建立非线性优化处理。其中标记物相对于图像采集设备坐标系的位姿[llRwm llpwm]为优化变量，三维标记物角点重投影到左镜头图像、右镜头图像上的标记物角点与实际检测标记物角点间的平均几何误差最小作为优化目标，以获得最优化的左镜头外参(例如标记物与双镜头图像采集设备的相对位姿)。应当理解，以双镜头图像采集设备中的右镜头作为主镜头时，将左右镜像调整，算法与前述流程基本类似，本公开不再详述。
通过识别图像采集设备采集的图像中的标记物角点，优化估计标记物相对于图像采集设备的位姿矩阵[llRwm llpwm]，并基于公式计算运动臂末端相对于世界坐标系的位姿，并基于运动臂末端的位姿信息，进行运动臂末端位姿的实时闭环控制，以提高系统的运动臂运动精度。
图7示出了根据本公开一些实施例的用于确定运动臂末端在世界坐标系中的位置的方法700的流程图。如图7所示，该方法700可以由运动臂系统(例如运动臂系统100)的控制装置(例如控制装置10)来执行。控制装置30可以配置在计算设备上。方法700可以由软件、固件和/或硬件来实现。
在步骤701，基于运动臂末端的当前相对位姿，确定运动臂在世界坐标系中的当前位姿。在一些实施例中，可以基于识别的标记物角点，确定标记物相对于图像采集设备的当前相对位姿(例如姿态llRwm和位置llpwm)。基于标记物与运动臂末端的已知相对位姿关系，以及图像采集设备在世界坐标系中的位姿，可以确定标记物所在的运动臂末端在世界坐标系中的位置。例如，确定运动臂末端在世界坐标系中的位置可以基于如下公式：
Wptip＝WRll(llRwm wmptip+llpwm)+Wpll (13)
其中，Wptip为运动臂末端在世界坐标系{W}中的位置，wmptip为运动臂末端在标记物坐标系{wm}中的位置；WRll、Wpll分别为图像采集设备在世界坐标系{W}中的姿态和位置；llRwm、llpwm分别为标记物在图像采集设备坐标系{ll}中的姿态和位置。应当理解，图像采集设备的位姿确定，WRll、Wpll可以为已知矩阵。
其中，
Figure BDA0003142273370000161
为运动臂末端在标记物坐标系{wm}中的位置坐标。
在本公开中，世界坐标系
Figure BDA0003142273370000162
其原点可以位于固定基座(例如鞘套)的中心处(如图2(a)或图8所示)。标记物坐标系
Figure BDA0003142273370000163
其原点与运动臂标记物中心重合，x轴从标记物中心指向第一个标记物角点(如图2(a)、图2(b)所示)。左镜头坐标系
Figure BDA0003142273370000164
其原点与双镜头图像采集设备左镜头的中心重合，x轴从双镜头图像采集设备左镜头的中心指向双镜头图像采集设备右镜头的中心。右镜头坐标系
Figure BDA0003142273370000165
其原点与双镜头图像采集设备右镜头的中心重合，x轴与双镜头图像采集设备左镜头坐标系的x轴重合，指向相同。
在步骤703，基于运动臂末端在世界坐标系中的目标位姿和当前位姿，确定运动臂末端的目标位姿和当前位姿的差值。在一些实施例中，运动臂末端在世界坐标系中的目标位姿可以由用户通过输入装置输入。通过比较计算，可以确定运动臂末端的目标位姿和当前位姿的差值。例如，可以基于运动臂末端在世界坐标系中的目标位置和当前位置，计算目标位置和当前位置的差值Δp，Δp可以如下：
Δp＝pd-pc (14)
其中，pd为运动臂末端在世界坐标系的目标位置，pc为运动臂末端在世界坐标系的当前位置(例如前述方法步骤701计算得到的Wptip)。
基于差值Δp得到运动臂末端的空间速度v及参数
Figure BDA0003142273370000171
分别为：
v＝vxlimΔp/||Δp|| (15)
Figure BDA0003142273370000172
其中，vxlim为设定的最大允许空间速度；J+是J的Moore–Penrose伪逆矩阵；ω为运动臂末端角速度；ψ可以表示运动臂的弯转角度或者运动臂关节的旋转角度参数向量，
Figure BDA0003142273370000173
可以为ψ的导数。例如在运动臂为柔性运动臂时，ψ为柔性运动臂臂体的弯转角度参数向量，在运动臂为普通关节运动臂结构时，ψ为运动臂关节的旋转角度参数向量。
在每个运动控制循环，可以更新运动臂末端的参数空间向量：
Figure BDA0003142273370000174
其中，Δt为运动控制循环的周期。
在步骤705，基于差值和运动臂的逆运动学数值迭代算法，确定运动臂的驱动信号。例如，基于运动臂末端在世界坐标系中的目标位姿和当前位姿的差值，通过运动臂运动学模型的逆运动学数值迭代算法，可以确定运动臂所包括的多个关节在当前运动控制循环内的驱动值(或者控制运动臂运动的对应多个电机的驱动值)。应当理解，运动学模型可以表示运动臂的关节空间和任务空间的运动关系的数学模型。例如，运动学模型可以通过DH参数法和指数积表示法等方法建立。
在一些实施例中，方法700还可以包括：以预定周期，确定运动臂的驱动信号以实现多个运动控制循环。例如，迭代地执行多个运动控制循环，在每个运动控制循环，可以执行根据本公开一些实施例的控制方法，以控制运动臂运动到目标位姿。通过迭代地执行多个运动控制循环，可以实现运动臂末端位置的实时闭环控制，可以提高运动臂的位置控制精度，避免在运动臂末端具有负载时，通过开环控制算法导致的运动学建模不准确。应理解，经本公开的方法实现运动臂的位置控制，能改进运动臂(例如连续体柔性臂)的轨迹跟踪误差。例如，在一些实施例中，运动臂的轨迹跟踪误差可以降低至开环控制误差的25.23％。
在一些实施例中，以运动臂为柔性运动臂(例如连续体柔性臂)作为示例。图8和图9分别示出根据本公开一些实施例中的连续体柔性臂的结构示意图、连续体柔性臂的单个连续体段的结构示意图。如图8和图9所示，连续体柔性臂可包括两段连续体段和两个直杆段，每个连续体段(如图9)包括2个自由度结构，其中一个直杆段位于两个连续体之间，另一个直杆段含1个进给自由度和1个整体绕自身轴线旋转自由度。
如图9所示，每个连续体段可以包括基座环、末端环以及贯穿基座环和末端环的多根并列的结构骨，多根结构骨可以与末端环固定连接，与基座环滑动连接。连续体柔性臂的运动过程为平面弯转，连续体柔性臂及其包含的连续体段运动学描述如下：
在本公开中，应当理解，基座环坐标系
Figure BDA0003142273370000181
附着在第t节连续体段的基座环上，其原点位于基座环中心，XY平面与基座环平面重合，
Figure BDA0003142273370000182
从基座环中心指向第一根结构骨。
弯曲平面坐标系1
Figure BDA0003142273370000183
其原点与基座环坐标系原点重合，XZ平面和弯曲平面重合，
Figure BDA0003142273370000184
与
Figure BDA0003142273370000185
重合。
弯曲平面坐标系2
Figure BDA0003142273370000186
其原点位于末端环中心，XY平面和弯曲平面重合，
Figure BDA0003142273370000187
与
Figure BDA0003142273370000188
重合。
末端环坐标系
Figure BDA0003142273370000189
附着在第t节连续体段的末端环上，其原点位于末端环中心，XY平面与末端环平面重合，
Figure BDA00031422733700001810
从末端环中心指向第一根结构骨。
在一些实施例中，连续柔性臂以及连续体段运动学建模的各符号的定义可以如表1所示。
表1
Figure BDA0003142273370000191
如图9所示，单个连续体段的运动学如下：
单个连续体段末端的位置tbpte、姿态tbRte可以如以下公式所示：
Figure BDA0003142273370000192
tbRte＝tbRt1 t1Rt2 t2Rte (19)
单个连续体段末端的角速度ωt、线速度vt和θt、δt速度的关系，可以如以下公式所示：
Figure BDA0003142273370000201
在一些实施例中，整个连续体柔性臂的运动学描述建立如下：
根据图8中所示的各坐标系间的变换关系，连续体柔性臂的末端位置姿态在世界坐标系{w}中可表示为：
WTtip＝WT1b 1bT1e 1eT2b 2bT2e 2eTtip (21)
其中，WTtip表示连续体的末端相对于世界坐标系的齐次变换矩阵；WT1b表示第一个连续体的基座环相对于世界坐标系的齐次变换矩阵；1bT1e表示第一个连续体的末端环相对于第一个连续体的基座环的齐次变换矩阵；1eT2b表示第二个连续体的基座环相对于第一个连续体的末端环的齐次变换矩阵；2bT2e表示第二个连续体的末端环相对于第二个连续体的基座环的齐次变换矩阵；2eTtip表示连续体的末端相对于第二个连续体的末端环的齐次变换矩阵。
应理解，连续体柔性臂的末端位置在世界坐标系{w}中的齐次变换矩阵可表示为：
WTtip＝WTwm wmTtip (22)
应理解，整个连续体柔性臂的末端的速度，可以如以下公式所示：
Figure BDA0003142273370000202
其中，
Figure BDA0003142273370000203
为速度雅可比矩阵；矩阵中W1，W2和W3分别可以如以下公式所示：
Figure BDA0003142273370000211
Figure BDA0003142273370000212
W3＝J2v (26)
Figure BDA0003142273370000213
为在坐标系A下，从坐标系B到坐标系C的向量的表达；
Figure BDA0003142273370000214
为所述向量的反对称矩阵。
该雅克比矩阵中：
Figure BDA0003142273370000215
本领域技术人员可以理解，基于连续体运动臂运动学模的逆运动学解算，例如基于公式(15)、(16)和(23)，可以确定控制连续体运动臂运动的对应多个电机在当前运动控制循环内的驱动值。
在一些实施例中，本公开提供了一种计算机可读存储介质，计算机可读存储介质可以包括至少一个指令，至少一个指令由处理器执行以将处理器配置为执行以上任何实施例中的控制方法。
在一些实施例中，本公开提供了一种计算机系统，可以包括非易失性存储介质和至少一个处理器。非易失性存储介质可以包括至少一个指令。处理器被配置为执行至少一个指令以将处理器配置为执行以上任何实施例中的控制方法。
在一些实施例中，计算机可读存储介质可以是可以保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意组合。
在一些实施例中，计算机可读取存储介质可以包括但不限于：便携式计算机盘、硬盘、只读存储器(ROM)、随机存取存储器(RAM)、可擦除可编程只读存储器(EPROM)、电可擦可编程只读存储器(EEPROM)、闪存或其他固态存储器技术、CD-ROM、数字多功能盘(DVD)、HD-DVD、蓝光(Blue-Ray)或其他光存储设备、磁带、磁盘存储或其他磁性存储设备、或能用于存储所需信息且可以由计算机访问的任何其他介质，其上存储有计算机可执行指令，计算机可执行指令在机器(例如计算机设备)中运行时，使得机器执行本公开的控制方法。应当理解，计算机设备可以包括个人计算机、服务器或者网络设备等。
应理解，可由计算机可执行指令实现流程图和方框图中的每一流程和方框、以及流程图和方框图中的流程和方框的结合。可提供这些计算机可执行指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和方框图一个方框或多个方框中指定的功能的装置。
本公开的一些实施例，能够基于视觉跟踪的闭环控制方法及设置的标记物，实时检测运动臂末端的姿态，有效降低运动臂的轨迹跟踪误差，可降低运动控制误差。而且，根据本公开一些实施例的控制方法可以以预定周期执行运动控制循环，每次运动控制循环中，确定运动臂末端的姿态，并对运动臂的运动进行控制，以实现目标位姿。
本公开的一些实施例，基于有针对性的标记物角点搜索策略，大大降低了运算量，可以迭代实现对于运动臂末端的轨迹跟踪和误差校正。
本公开的一些实施例，通过在不同步骤使用不同的方法，以对标记物角点进行识别和确定，可以避免由于各种因素导致的标记物角点的误识和错过，可以在短时间内实现高精确的运动臂末端的跟踪和实时校正。且本公开的技术方案具有广泛的适应性，可以适用于多种标记物和标记物识别设备，也可以广泛地应用于机器人，例如可以用于对手术机器人、工业机器人等的运动末端的跟踪及校正。
注意，上述仅为本公开的示例性实施例及所运用技术原理。本领域技术人员会理解，本公开不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本公开的保护范围。因此，虽然通过以上实施例对本公开进行了较为详细的说明，但是本公开不仅仅限于以上实施例，在不脱离本公开构思的情况下，还可以包括更多其他等效实施例，而本公开的范围由所附的权利要求范围决定。

Claims (26)

1.一种运动臂的控制方法，包括：
获得由图像采集设备采集的图像，所述图像包括设置在所述运动臂末端上的标记物的图像，所述标记物包括多个标记物角点；
识别所述采集的图像中的标记物角点；以及
基于所述识别的标记物角点，确定所述运动臂末端相对于所述图像采集设备的当前相对位姿。
2.根据权利要求1所述的控制方法，其特征在于，还包括：
在所述采集的图像中确定感兴趣区域(ROI)；
在所述ROI中识别疑似标记物角点；以及
基于标准角点模型，对所述ROI中的疑似标记物角点进行相关性检验，以判断所述ROI图像中的疑似标记物角点是否为标记物角点。
3.根据权利要求2所述的控制方法，其特征在于，还包括：
确定所述ROI为全图像，或者基于上一帧图像中的疑似标记物角点位置，确定所述ROI为局部图像；以及
将所述ROI转为相应的灰度图像。
4.根据权利要求3所述的控制方法，其特征在于，所述局部图像包括以上一帧图像中的疑似标记物角点的平均坐标构成的虚点为中心的设定距离范围，所述设定距离包括多个所述疑似标记物角点平均间隔距离的预定倍数。
5.根据权利要求2所述的控制方法，其特征在于，在所述ROI中识别疑似标记物角点包括：
确定所述ROI中的每个像素点的角点似然值(CL)；
将所述ROI划分成多个子ROI；
确定每个所述子ROI中CL值最大的像素点；
基于所述多个子ROI的多个所述CL值最大的像素点，确定CL值大于第一阈值的像素点集合；
确定所述像素点集合中CL值最大的像素点，作为第一疑似标记物角点；以及
基于所述第一疑似标记物角点，以设定步长沿多个边缘方向搜索第二疑似标记物角点。
6.根据权利要求5所述的控制方法，其特征在于，还包括：
确定以第一疑似标记物角点为中心的第一预定范围中的各像素点的梯度方向和梯度权重；
基于所述第一预定范围中的各像素点的梯度方向和梯度权重，确定所述多个边缘方向。
7.根据权利要求5所述的控制方法，其特征在于，还包括：
基于所述第一疑似标记物角点和所述第二疑似标记物角点，调整边缘方向；以及
沿经调整的边缘方向，搜索第三疑似标记物角点。
8.根据权利要求5-7中任一项所述的控制方法，其特征在于，还包括：
基于所述第一疑似标记物角点和所述第二疑似标记物角点，向相反边缘方向搜索。
9.根据权利要求5-7中任一项所述的控制方法，其特征在于，还包括：
响应于疑似标记物角点数量大于或等于第二阈值，停止所述搜索；或者
响应于搜索的距离大于所述第一疑似标记物角点和所述第二疑似标记物角点之间的距离的预定倍数或者响应于确定的疑似标记物角点数量小于所述第二阈值，基于所述像素点集合中CL值从大到小排序的下一个像素点作为第一疑似标记物角点，以所述设定步长沿边缘方向搜索疑似标记物角点，直到疑似标记物角点数量大于或等于第二阈值。
10.根据权利要求2-5中任一项所述的控制方法，其特征在于，对所述疑似标记物角点进行相关性检验包括：
确定所述标准角点模型的灰度分布与以所述疑似标记物角点的像素点为中心的第二预定范围内像素灰度分布之间的相关性系数；
响应于所述相关性系数大于第三阈值，确定所述疑似标记物角点为标记物角点。
11.根据权利要求2所述的控制方法，其特征在于，还包括：
响应于所述ROI中的疑似标记物角点为标记物角点，对所述标记物角点进行亚像素定位。
12.根据权利要求11所述的控制方法，其特征在于，对所述标记物角点进行亚像素定位包括：
基于以下公式，计算所述标记物角点的坐标：
S(x，y)＝ax2+by2+cx+dy+exy+f
其中，S(x，y)为每个子ROI中的所有像素点的CL值拟合函数，a、b、c、d、e、f为系数；
Figure FDA0003142273360000031
其中，xc为所述标记物角点的x坐标，yc为所述标记物角点的y坐标。
13.根据权利要求1所述的控制方法，其特征在于，还包括：
确定所述标记物角点相对于标记物坐标系的三维坐标；
确定所述标记物角点在所述采集的图像中的二维坐标；
确定所述二维坐标和所述三维坐标的单应性矩阵的解析解；以及
基于所述单应性矩阵的解析解，确定所述标记物相对于所述图像采集设备的当前相对位姿。
14.根据权利要求13所述的控制方法，其特征在于，确定所述标记物角点相对于标记物坐标系的三维坐标包括基于以下公式确定所述标记物角点相对于所述标记物坐标系的三维坐标：
Figure FDA0003142273360000041
其中，
Figure FDA0003142273360000042
为第i个标记物角点对应的三维坐标，rwm为所述标记物形成的圆筒的半径，β为标记物角点的分布角。
15.根据权利要求14所述的控制方法，其特征在于，确定所述二维坐标和所述三维坐标的单应性矩阵的解析解包括：
基于以下公式计算所述解析解：
Figure FDA0003142273360000043
Hleft＝[h1 h2 h3]＝η·A·[r1 r2 llpwm] (2)
式(1)中，
Figure FDA0003142273360000044
为第i个标记物角点的扩展坐标，[ui，vi]为第i个标记物角点的二维坐标，leftsi为任意的非零标量，leftsi为比例因子，以使式(1)左右两边最后一列数字相同，Hleft为单应性矩阵；以及
式(2)中，A为图像采集设备的已知内部参数矩阵，r1，r2是llRwm矩阵的前两列，η为任意的非零标量。
16.根据权利要求15所述的控制方法，其特征在于，确定所述二维坐标和所述三维坐标的单应性矩阵的解析解还包括：
基于以下公式计算所述解析解：
Figure FDA0003142273360000045
其中，xH＝[h1 T h2 T h3 T]T，L是2n×9矩阵，n为所述采集的图像中确定的标记物角点数量，xH为矩阵L的奇异值分解(SVD)后最小奇异值对应的右奇异向量。
17.根据权利要求16所述的控制方法，其特征在于，还包括：
基于以下公式，确定所述标记物相对于所述图像采集设备的当前相对位姿：
r1＝κ·A-1·h1
r2＝κ·A-1·h2
llRwm＝[r1 r2 r1×r2]
llpwm＝κ·A-1h3
其中，κ＝1/||A-1·h1||＝1/||A-1·h2||。
18.根据权利要求13所述的控制方法，还包括：
基于所述单应性矩阵的解析解，执行非线性优化处理以获得所述标记物相对于所述图像采集设备的当前相对位姿。
19.根据权利要求18所述的控制方法，其特征在于，执行所述非线性优化处理包括使所述三维坐标在所述采集的图像上的投影与所述标记物角点之间的平均几何误差最小化。
20.根据权利要求19所述的控制方法，其特征在于，
所述图像采集设备是单镜头图像采集设备，所述采集的图像是单镜头图像，执行所述非线性优化处理包括：
基于以下公式执行优化处理：
Figure FDA0003142273360000051
或者
所述图像采集设备为双镜头图像采集设备，所述采集的图像包括左镜头图像和右镜头图像，执行所述非线性优化处理包括：
基于以下公式执行优化处理：
Figure FDA0003142273360000052
其中，Hleft和Hright分别为与左镜头图像、右镜头图像相关的单应性矩阵；leftsi和rightsi分别为与左镜头图像、右镜头图像相关的单应性矩阵的解析解相关的比例因子，
Figure FDA0003142273360000061
和
Figure FDA0003142273360000062
分别为与左镜头图像、右镜头图像相关的第i个标记物角点的扩展坐标。
21.根据权利要求1所述的控制方法，还包括：
基于所述运动臂末端的当前相对位姿，确定所述运动臂在世界坐标系中的当前位姿；
基于所述运动臂末端在世界坐标系中的目标位姿和当前位姿，确定所述运动臂末端的目标位姿和当前位姿的差值；以及
基于所述差值和所述运动臂的逆运动学数值迭代算法，确定所述运动臂的驱动信号。
22.根据权利要求21所述的控制方法，还包括：
以预定周期，确定所述运动臂的所述驱动信号以实现多个运动控制循环。
23.根据权利要求1所述的控制方法，其特征在于，还包括：
基于所述识别的标记物角点，确定所述标记物相对于所述图像采集设备的当前相对位姿；以及
基于所述标记物的当前相对位姿和以下对应关系，确定所述运动臂末端在世界坐标系中的当前位姿：
Wptip＝WRll(llRwm wmptip+llpwm)+Wpll
其中，Wptip为所述运动臂末端在世界坐标系{W}中的位置，wmptip为所述运动臂末端在标记物坐标系{wm}中的位置；WRll、Wpll分别为所述图像采集设备在世界坐标系{W}中的姿态和位置；llRwm、llpwm分别为所述标记物在图像采集设备坐标系{ll}中的姿态和位置；
Figure FDA0003142273360000063
其中，
Figure FDA0003142273360000071
为所述运动臂末端在所述标记物坐标系{wm}中的位置坐标。
24.根据权利要求1所述的控制方法，其特征在于，所述标记物呈筒状，包括多个分布在筒状上的标记物角点。
25.一种运动臂系统，包括：
图像采集设备，用于采集图像；
至少一个运动臂，包括运动臂末端，所述运动臂末端上设有标记物，所述标记物包括多个标记物角点；
控制装置，被配置成执行根据权利要求1-24中任一项所述的控制方法。
26.一种计算机可读存储介质，包括一个或多个其上存储有计算机可执行指令，所述计算机可执行指令由处理器执行已将处理器配置为执行根据权利要求1-24中任一项所述的控制方法。
CN202110744106.2A 2020-07-11 2021-06-30 运动臂系统以及控制方法 Active CN113910219B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202010665741 2020-07-11
CN2020106657417 2020-07-11

Publications (2)

Publication Number Publication Date
CN113910219A true CN113910219A (zh) 2022-01-11
CN113910219B CN113910219B (zh) 2024-07-05

Family

ID=

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114347037A (zh) * 2022-02-16 2022-04-15 中国医学科学院北京协和医院 基于复合标识的机器人系统故障检测处理方法及机器人系统
CN114536330A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于多个位姿标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536402A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于关联标识的机器人系统故障检测处理方法及机器人系统
CN114536331A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于关联标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536329A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于复合标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536401A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于多个位姿标识的机器人系统故障检测处理方法及机器人系统
CN114742785A (zh) * 2022-03-31 2022-07-12 启东普力马机械有限公司 基于图像处理的液压接头清洁度控制方法
CN115761602A (zh) * 2023-01-07 2023-03-07 深圳市蓝鲸智联科技有限公司 用于车窗控制系统的视频智能识别方法
CN117245651A (zh) * 2023-09-12 2023-12-19 北京小米机器人技术有限公司 机械臂插拔控制方法、装置、设备及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009285778A (ja) * 2008-05-29 2009-12-10 Toyota Industries Corp ロボットハンドの姿勢検知システム
JP2010172986A (ja) * 2009-01-28 2010-08-12 Fuji Electric Holdings Co Ltd ロボットビジョンシステムおよび自動キャリブレーション方法
CN107784672A (zh) * 2016-08-26 2018-03-09 百度在线网络技术（北京）有限公司 用于获取车载相机的外部参数的方法和装置
CN108717709A (zh) * 2018-05-24 2018-10-30 东北大学 图像处理系统及图像处理方法
CN108827316A (zh) * 2018-08-20 2018-11-16 南京理工大学 基于改进的Apriltag标签的移动机器人视觉定位方法
CN109597067A (zh) * 2018-12-21 2019-04-09 济南爱我本克网络科技有限公司 毫米波辐射计线列扫描低识别度目标的分析方法和系统
CN109949366A (zh) * 2019-03-08 2019-06-28 鲁班嫡系机器人（深圳）有限公司 一种定位设备及其方法
CN110959099A (zh) * 2017-06-20 2020-04-03 卡尔蔡司Smt有限责任公司 确定可移动物体在空间中的位置的系统、方法和标记物

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009285778A (ja) * 2008-05-29 2009-12-10 Toyota Industries Corp ロボットハンドの姿勢検知システム
JP2010172986A (ja) * 2009-01-28 2010-08-12 Fuji Electric Holdings Co Ltd ロボットビジョンシステムおよび自動キャリブレーション方法
CN107784672A (zh) * 2016-08-26 2018-03-09 百度在线网络技术（北京）有限公司 用于获取车载相机的外部参数的方法和装置
CN110959099A (zh) * 2017-06-20 2020-04-03 卡尔蔡司Smt有限责任公司 确定可移动物体在空间中的位置的系统、方法和标记物
CN108717709A (zh) * 2018-05-24 2018-10-30 东北大学 图像处理系统及图像处理方法
CN108827316A (zh) * 2018-08-20 2018-11-16 南京理工大学 基于改进的Apriltag标签的移动机器人视觉定位方法
CN109597067A (zh) * 2018-12-21 2019-04-09 济南爱我本克网络科技有限公司 毫米波辐射计线列扫描低识别度目标的分析方法和系统
CN109949366A (zh) * 2019-03-08 2019-06-28 鲁班嫡系机器人（深圳）有限公司 一种定位设备及其方法

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114536330B (zh) * 2022-02-16 2023-10-20 中国医学科学院北京协和医院 基于多个位姿标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536331B (zh) * 2022-02-16 2023-10-20 中国医学科学院北京协和医院 基于关联标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536402A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于关联标识的机器人系统故障检测处理方法及机器人系统
CN114536331A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于关联标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536329A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于复合标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536401A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于多个位姿标识的机器人系统故障检测处理方法及机器人系统
CN114536330A (zh) * 2022-02-16 2022-05-27 中国医学科学院北京协和医院 基于多个位姿标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536329B (zh) * 2022-02-16 2024-05-17 中国医学科学院北京协和医院 基于复合标识确定可形变机械臂的外部受力的方法及机器人系统
CN114536402B (zh) * 2022-02-16 2024-04-09 中国医学科学院北京协和医院 基于关联标识的机器人系统故障检测处理方法及机器人系统
CN114347037A (zh) * 2022-02-16 2022-04-15 中国医学科学院北京协和医院 基于复合标识的机器人系统故障检测处理方法及机器人系统
CN114536401B (zh) * 2022-02-16 2024-03-29 中国医学科学院北京协和医院 基于多个位姿标识的机器人系统故障检测处理方法及机器人系统
CN114347037B (zh) * 2022-02-16 2024-03-29 中国医学科学院北京协和医院 基于复合标识的机器人系统故障检测处理方法及机器人系统
CN114742785A (zh) * 2022-03-31 2022-07-12 启东普力马机械有限公司 基于图像处理的液压接头清洁度控制方法
CN115761602A (zh) * 2023-01-07 2023-03-07 深圳市蓝鲸智联科技有限公司 用于车窗控制系统的视频智能识别方法
CN117245651A (zh) * 2023-09-12 2023-12-19 北京小米机器人技术有限公司 机械臂插拔控制方法、装置、设备及存储介质

Also Published As

Publication number Publication date
WO2022012337A1 (zh) 2022-01-20

Similar Documents

Publication Publication Date Title
CN111775146B (zh) 一种工业机械臂多工位作业下的视觉对准方法
US11911914B2 (en) System and method for automatic hand-eye calibration of vision system for robot motion
CN110116407B (zh) 柔性机器人位姿测量方法及装置
CN109308693B (zh) 由一台ptz相机构建的目标检测和位姿测量单双目视觉系统
CN106457562B (zh) 用于校准机器人的方法和机器人系统
CN109202912B (zh) 一种基于单目深度传感器和机械臂配准目标轮廓点云的方法
CN111801198B (zh) 一种手眼标定方法、系统及计算机存储介质
EP3407088A1 (en) Systems and methods for tracking location of movable target object
CN108621167B (zh) 一种基于轮廓边及内包圆特征的视觉伺服解耦控制方法
WO2014161603A1 (en) A robot system and method for calibration
CN110919658B (zh) 一种基于视觉和多坐标系闭环转换的机器人标定方法
KR20140008262A (ko) 로봇 시스템, 로봇, 로봇 제어 장치, 로봇 제어 방법 및 로봇 제어 프로그램
CN110465946B (zh) 一种像素坐标与机器人坐标关系标定方法
US20220230348A1 (en) Method and apparatus for determining a three-dimensional position and pose of a fiducial marker
WO2022012337A1 (zh) 运动臂系统以及控制方法
WO2023134237A1 (zh) 用于机器人的坐标系标定方法、装置、系统以及介质
CN114536399B (zh) 基于多个位姿标识的误差检测方法及机器人系统
CN116766194A (zh) 基于双目视觉的盘类工件定位与抓取系统和方法
CN112164112A (zh) 一种获取机械臂位姿信息的方法及装置
Yang et al. Autonomous laparoscope control for minimally invasive surgery with intuition and RCM constraints
CN114523471A (zh) 基于关联标识的误差检测方法及机器人系统
Yang et al. A closed-loop controller for a continuum surgical manipulator based on a specially designed wrist marker and stereo tracking
CN113910219B (zh) 运动臂系统以及控制方法
CN115446847A (zh) 用于提高机器人系统的3d眼手协调准确度的系统和方法
CN115246123A (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
CB02 Change of applicant information

Address after: 100192 Tiandi Adjacent to Feng2 Building 106, No. 1 North Yongtaizhuang Road, Haidian District, Beijing

Applicant after: Beijing Shurui Robot Co.,Ltd.

Address before: 100192 Tiandi Adjacent to Feng2 Building 106, No. 1 North Yongtaizhuang Road, Haidian District, Beijing

Applicant before: BEIJING SURGERII TECHNOLOGY Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant