运动臂系统以及控制方法与流程

1.本公开涉及机器人领域，尤其涉及运动臂系统以及控制方法。


背景技术：

2.现有的机器人系统的运动臂控制方法采用的闭环到位校正方式，主要通过从运动臂运动关节的到位情况进行校正数据的获取，以确定闭环补偿的补偿情况。这种补偿方式不能解决设备自身的误差导致的到位精度问题，尤其是针对精密的运动臂闭环控制。
3.现有空间定位设备通常采用对既有定位标识的识别来确定定位标识所在物的空间位置。目前实现这样的识别和控制方式有许多方案，但各有局限，如对于标记小球的空间定位需要双目摄像头，对设备要求较多。还有一种方案采用明暗或黑白对比的标记作为标记物，采用对图像内每个像素点进行标记物可能性计算或者采用既有模板对像素内的所有像素在一定范围内进行模板匹配。这样的方式或者容易将不是标记物的部分误识为标记物或者具有非常巨大的计算量，不适合高精度实时闭环校正算法的应用。
4.综上，在运动臂的闭环控制领域还有许多课题和技术亟待处理。


技术实现要素：

5.在一些实施例中，本公开提供了一种运动臂的控制方法，包括：获得由图像采集设备采集的图像，所述图像包括设置在所述运动臂末端上的标记物的图像，所述标记物包括多个标记物角点；识别所述采集的图像中的标记物角点；以及基于所述识别的标记物角点，确定所述运动臂末端相对于所述图像采集设备的当前相对位姿。
6.在一些实施例中，本公开提供了一种运动臂系统，包括：图像采集设备，用于采集图像；至少一个运动臂，包括运动臂末端，所述运动臂末端上设有标记物，所述标记物包括多个标记物角点；控制装置，被配置成执行根据本公开一些实施例的控制方法。
7.在一些实施例中，本公开提供了一种计算机可读存储介质，包括一个或多个其上存储有计算机可执行指令，所述计算机可执行指令由处理器执行已将处理器配置为执行根据本公开一些实施例的控制方法。
附图说明
8.为了清楚地说明本公开实施例中的技术方案，下面将对本公开实施例描述中所需要使用的附图作简单的介绍。下面描述中的附图仅仅示出本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据本公开实施例的内容和这些附图获得其他的实施例。
9.图1示出根据本公开一些实施例中的运动臂系统的结构示意图；
10.图2(a)示出根据本公开一些实施例中的运动臂的结构示意图；
11.图2(b)示出根据本公开一些实施例中的标记物的结构示意图；
12.图3示出根据本公开一些实施例中的运动状态下的运动臂末端的放大示意图；
13.图4示出根据本公开一些实施例中的运动臂系统的控制方法的流程图；
14.图5示出根据本公开一些实施例中的用于识别图像中的标记物角点的控制方法的流程图；
15.图6示出根据本公开一些实施例中的疑似标记物角点搜索示意图；
16.图7示出根据本公开一些实施例中的用于确定运动臂末端在世界坐标系中的位置的方法的流程图；
17.图8示出根据本公开一些实施例中的连续体柔性臂的结构示意图；
18.图9示出根据本公开一些实施例中的连续体柔性臂的单个连续体段的结构示意图。
具体实施方式
19.为使本公开解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图对本公开实施例的技术方案作进一步的详细描述，显然，所描述的实施例仅仅是本公开示例性实施例，而不是全部的实施例。
20.在本公开的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本公开和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本公开的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。在本公开的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“耦合”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连；可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本公开中的具体含义。在本公开中，在手术机器人系统中，定义靠近用户(例如医生)的一端为近端、近部或后端、后部，靠近手术患者的一端为远端、远部或前端、前部。
21.在本公开中，术语“位置”指对象或对象的一部分在三维空间中的定位(例如，可使用笛卡尔x、y和z坐标方面的变化描述三个平移自由度，例如分别沿笛卡尔x轴、y轴和z轴的三个平移自由度)。在本公开中，术语“姿态”指对象或对象的一部分的旋转设置(例如三个旋转自由度，可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指对象或对象的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。在本公开中，运动臂或其一部分的位姿是指运动臂或其一部分定义的坐标系相对于运动臂所在的支架、基座定义的坐标系或世界坐标系的位姿。
22.图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还可以应用于其他领域(例如，制造、机械等等)的专用或通用机器人系统。
23.在本公开中，控制装置30可以与至少一个运动臂20的电机通信连接，从而使电机基于驱动信号控制至少一个运动臂20运动到相应的目标位姿。例如，控制运动臂运动的电机可以为伺服电机，可以接受控制装置的指令以控制运动臂运动。控制装置30还可例如通过通信接口与电机耦合的传感器通信连接，以接收运动臂20的运动数据，实现对运动臂20的运动状态监控。在本公开的一个示例中，该通信接口可以为can(controller area network)总线通信接口，其使得控制装置30能够通过can总线与电机以及传感器连接通信。
24.在一些实施例中，运动臂20可以包括连续体柔性臂或者由多个关节构成的具有多自由度的运动臂，例如可以实现6个自由度运动的运动臂。图像采集设备10可以包括但不限于双镜头图像采集设备或单镜头图像采集设备，例如双目或单目相机。
25.图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可以为例如棋盘格标记物中的两个线段的交点。
26.图3示出根据本公开一些实施例中的运动状态下的运动臂末端的放大示意图。如图3所示，至少一个运动臂可以包括但不限于两个运动臂，每个运动臂末端都设有标记物。
27.本公开的一些实施例提供了一种运动臂的控制方法。图4示出了根据本公开一些实施例的运动臂系统(例如运动臂系统100)的控制方法400的流程图。如图4所示，该方法400可以由运动臂系统100的控制装置(例如控制装置30)来执行。控制装置30可以配置在计算设备上。方法400可以由软件、固件和/或硬件来实现。
28.在步骤401，获得由图像采集设备采集的图像。例如，可以由双镜头图像采集设备或单镜头图像采集设备采集运动臂末端的图像，采集的图像中包括运动臂末端上的标记物的图像。
29.在步骤403，识别采集的图像中的标记物角点。在一些实施例中，可以通过控制装置对采集的图像进行预处理，以识别图像中的标记物角点。识别图像中的标记物角点的示例性方法如图5所示方法详述。
30.在步骤405，基于识别的标记物角点，确定运动臂末端相对于图像采集设备的当前相对位姿。在一些实施例中，可以基于识别的标记物角点，确定标记物相对于图像采集设备的当前位姿，基于标记物的当前位姿以及标记物相对于运动臂末端的相对位姿，可以确定运动臂末端的当前相对位姿。应当理解，由于标记物固定设置于运动臂末端，标记物相对于
运动臂末端的相对位姿是已知的。
31.图5示出了根据本公开一些实施例的用于识别采集的图像中的标记物角点的方法500的流程图。如图5所示，该方法500可以由运动臂系统(例如运动臂系统100)的控制装置(例如控制装置30)来执行。控制装置30可以配置在计算设备上。方法500可以由软件、固件和/或硬件来实现。
32.在步骤中501，在采集的图像中确定感兴趣区域(roi)。在一些实施例中，确定roi为全图像，或者基于上一帧图像(例如，上一个运动控制循环中所处理的图像)中的疑似标记物角点位置，确定roi为局部图像，以及将roi转为相应的灰度图像。例如，可以基于采集的图像，截取全图像或者局部图像作为roi，并将roi转为相应的灰度图像，以将每个像素点的灰度信息量化。在一些实施例中，可以基于采集的图像为第一帧图像，确定第一帧图像的roi为全图像。在一些实施例中，基于采集的图像为非第一帧图像，可以基于上一帧图像中的疑似标记物角点位置，确定roi为局部图像。
33.在步骤中503，在roi中识别疑似标记物角点。例如可以基于每个像素点的灰度信息，确定每个像素点为标记物角点的可能性，将其中可能性高的像素点，确定为疑似标记物角点。
34.在一些实施例中，局部图像可以包括以上一帧图像中的疑似标记物角点的平均坐标构成的虚点为中心的设定距离范围，设定距离可以包括多个疑似标记物角点平均间隔距离的预定倍数。应理解，例如，预定倍数可以包括但不限于多个疑似标记物角点平均间隔距离的固定倍数，例如两倍。应理解，预定倍数还可以是多个疑似标记物角点平均间隔距离的可变倍数。
35.在一些实施例中，可以确定roi中的每个像素点的角点似然值(cl)。将roi划分成多个子roi，确定每个子roi中cl值最大的像素点。基于多个子roi的多个cl值最大的像素点，确定cl值大于第一阈值的像素点集合。确定像素点集合中cl值最大的像素点，作为第一疑似标记物角点。基于第一疑似标记物角点，以设定步长沿多个边缘方向搜索第二疑似标记物角点。
36.在一些实施例中，确定roi中的每个像素点的角点似然值(cl)。例如，对roi图像范围内每一个像素点进行卷积操作，得到每个像素点的一阶和/或二阶导数。利用前述roi图像范围内每个像素点的一阶和/或二阶导数求出每个像素点的角点似然值(cl)。
37.在一些实施例中，roi图像内的每个像素点的cl可以根据如下公式计算：
38.cl＝max(c
xy
,c
45
)
39.c
xy
＝ε2·
|i
xy
|-1.5
·
ε
·
(|i
45
| |i
n45
|)
40.c
45
＝ε2·
|i
45_45
|-1.5
·
ε
·
(|i
x
| |iy|)
ꢀꢀꢀꢀꢀ
(1)
41.其中，ε为设定常数，i
x
、i
45
、iy、i
n45
分别是每个像素点在0、π/4、π/2和-π/4四个方向的一阶导数；i
xy
和i
45_45
，分别是每个像素点在0,π/2和在π/4，-π/4方向的二阶导数；c
xy
是每个像素点在0,π/2方向的cl值，c
45
是每个像素点在π/4方向的cl值。
42.在一些实施例中，将roi划分成多个子roi。例如，可以采用非极大抑制法在一个roi图像范围中平均分割出多个子roi。在一些实施例中，可以将roi图像平均分割成5
×
5像素的多个子roi。上述实施例为示例性的，并非限制性的，应当理解，还可以将roi图像分割成其他尺寸大小的多个子roi。可以确定每个子roi中的cl值最大的像素点，将每个子roi中
的cl值最大的像素点与第一阈值进行比较，确定cl值大于第一阈值的像素点集合。在一些实施例中，第一阈值可以设定为0.06。应当理解，第一阈值还可以设定为其他值。
43.在一些实施例中，确定像素点集合中cl值最大的像素点，作为第一疑似标记物角点。例如，可以将该像素点集合中的所有像素点让cl值从大到小的顺序排序，并将cl值最大的像素点作为第一疑似标记物角点。基于所述第一疑似标记物角点，以设定步长沿多个边缘方向搜索第二疑似标记物角点。图6示出根据本公开一些实施例中的疑似标记物角点搜索示意图。如图6所示，以第一疑似标记物角点为起点，以设定步长向四个边缘方向依次搜索第二疑似标记物角点。在一些实施例中，设定步长可以为10个像素点。例如，以第一疑似标记物角点为起点，以10个像素点为步长，在边缘方向上依次在设定范围内搜索cl值最大的像素点，作为第二疑似标记物角点。例如，设定范围可以为10
×
10正方形区域。应理解，上述实施例为示例，并非限制，设定步长还可以为其他数量的像素点，设定范围也可以为其他尺寸大小的区域。例如，可以确定第一疑似标记物角点的四个边缘方向，基于每个边缘方向，以设定步长进行移动搜索，以在设定范围内寻找cl值最大的像素点，作为第二疑似标记物角点。
44.在一些实施例中，可以通过确定以第一疑似标记物角点为中心的第一预定范围中的各像素点的梯度方向和梯度权重，基于第一预定范围中的各像素点的梯度方向和梯度权重，确定多个边缘方向。例如，第一预定范围可以为10
×
10正方形区域。以第一疑似标记物角点为中心的10
×
10正方形像素邻域内，确定各像素点的梯度方向和梯度权重，以确定第一疑似标记物角点的多个边缘方向。应当理解，第二疑似标记物角点也可以通过类似方法确定边缘方向。通过基于疑似标记物角点调整边缘方向，可以对标记物角点进行针对性搜索，以降低运算量。
45.在一些实施例中，第一预定范围中的各像素点的梯度方向和梯度权重可以通过如下公式计算：
[0046][0047]
其中，i
angle
为对应像素点的梯度方向，i
weight
为对应像素点的梯度权重；i
x
、iy分别是对应像素点在0、π/2方向的一阶导数。将第一预定范围内的每个像素点的i
angle
和i
weight
进行聚类方法计算，以获得对应像素点的边缘方向。
[0048]
在一些实施例中，可以基于第二疑似标记物角点，调整边缘方向，并且沿经调整的边缘方向，搜索第三疑似标记物角点。例如，与以上类似，可以通过确定以第二疑似标记物角点为中心的预定范围中的各像素点的梯度方向和梯度权重，基于预定范围中的各像素点的梯度方向和梯度权重，确定更新的边缘方向，如图6所示。
[0049]
在步骤中505，基于标准角点模型，对roi中的疑似标记物角点进行相关性检验，以判断roi图像中的疑似标记物角点是否为标记物角点。例如，可以将第一疑似标记物角点以及搜索到的疑似标记物角点(例如第二疑似标记物角点)的像素点代入标准角点模型进行相关性判断，以确定疑似标记物角点是否为标记物角点。可以基于角点似然估计(corner likelihood)和模板匹配(template matching)相结合的角点检测方法，判断图像(例如在图像采集设备为双目图像采集设备时，提取图像包括左镜头图像和右镜头图像)中的标记物角点。
[0050]
在一些实施例中，确定标准角点模型的灰度分布与以疑似标记物角点的像素点为中心的第二预定范围内像素灰度分布之间的相关性系数，响应于相关性系数大于第三阈值，确定疑似标记物角点为标记物角点。在一些实施例中，标记物可以为黑白相间的棋盘格图形，标准角点模型可以是双曲线正切模型(htm)。例如，可以基于标准角点模型(例如htm)的灰度分布与以疑似标记物角点的像素点为中心的预定范围像素邻域灰度分布之间的相关性系数cc，确定疑似标记物角点是否为标记物角点。例如，以疑似标记物角点的像素点为中心的第二预定范围可以包括但不限于10
×
10正方形像素邻域。相关系系数cc可以如下定义：
[0051][0052]
其中，g
image
为以疑似标记物角点的像素点为中心的预定范围像素邻域灰度分布，g
htm
为标准角点模型的灰度分布，var为方差函数，cov为协方差函数。
[0053]
在判断相关系系数cc值大于阈值，表示该像素点为中心的预定范围内的灰度分布与双曲线正切模型相关系较高，可以确定该疑似标记物角点为标记物角点。反之，确定该疑似标记物角点不是标记物角点。在一些实施例中，可以设定阈值为0.8。应当理解，阈值还可以为其他设定值。例如，以第一疑似标记物角点为起点，搜索第二疑似标记物角点，对搜索到的第二疑似标记物角点进行相关性验证，判断相关系系数cc值大于0.8，确定第二疑似标记物角点为标记物角点。
[0054]
应当理解，可以在每次搜索到疑似标记物角点后或者搜索到多个疑似标记物角点后，将对应的像素点代入标准角点模型进行相关性判断，以确定识别的疑似标记物角点是否为标记物角点。
[0055]
在一些实施例中，可以基于第一疑似标记物角点和第二疑似标记物角点，向相反边缘方向搜索，以确定其他的疑似标记物角点。如图6所示，以第一疑似标记物角点和第二疑似标记物角点为起点，分别向两个相反边缘方向进行搜索。例如，以第一疑似标记物角点为起点沿远离第二疑似标记物角点的方向搜索，以及以第二疑似标记物角点为起点沿远离第一疑似标记物角点的方向搜索。
[0056]
在一些实施例中，响应于疑似标记物角点数量大于或等于第二阈值，可以停止搜索。在一些实施例中，第二阈值可以设定为4。应当理解，上述实施例为示例，并非限制，第二阈值还可以设定为5或者其他数值。例如，确定疑似标记物角点的总数量大于或等于4个并且通过相关性验证，表示成功找到标记物，将确定的多个疑似标记物角点作为标记物角点。
[0057]
在一些实施例中，响应于搜索的距离大于第一疑似标记物角点和第二疑似标记物角点之间的距离的预定倍数或者响应于确定的疑似标记物角点数量小于第二阈值，基于像素点集合中cl值从大到小排序的下一个像素点作为第一疑似标记物角点，以设定步长沿边缘方向搜索疑似标记物角点，直到疑似标记物角点数量大于或等于第二阈值。在一些实施例中，预定倍数可以是第一疑似标记物角点和第二疑似标记物角点之间的距离的两倍，搜索距离大于该预定倍数，当前搜索循环结束。可以基于像素点集合中cl值从大到小排序的下一个像素点作为第一疑似标记物角点，重新开始下一个搜索循环。或者，基于上述方式搜索，确定疑似标记物角点的总数量小于4个，表示未成功识别标记物，基于像素点集合中cl值从大到小排序的下一个像素点作为第一疑似标记物角点，开始下一搜索循环，搜索其他
的疑似标记物角点，直到确定疑似标记物角点的总数量大于或等于第二阈值(例如4)并且通过相关性验证。
[0058]
在一些实施例中，响应于roi中的疑似标记物角点为标记物角点，对标记物角点进行亚像素定位。通过亚像素定位可以对标记物角点的坐标精度进行优化。在一些实施例中，可以对每个子roi中的每个像素点的cl值基于模型进行拟合，以确定经亚像素定位后的标记物角点的坐标。例如，每个子roi中的每个像素点的cl值的拟合函数可以为二次曲面函数，该函数的极值点为亚像素点。拟合函数可以如下：
[0059]
s(x,y)＝ax2 by2 cx dy exy f
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0060]
其中，s(x，y)为每个子roi中的所有像素点的cl值拟合函数，a、b、c、d、e、f为系数；
[0061][0062]
其中，xc为标记物角点的x坐标，yc为标记物角点的y坐标。
[0063]
在一些实施例中，可以确定标记物角点相对于标记物坐标系的三维坐标，确定标记物角点在采集的图像中的二维坐标，确定二维坐标和三维坐标的单应性矩阵的解析解，基于单应性矩阵的解析解，确定标记物相对于图像采集设备的当前相对位姿。应当理解，可以基于标记物角点的已知参数，确定图像识别的标记物角点的三维坐标。应当理解，基于双镜头图像采集设备，可以将左镜头图像、右镜头图像中的标记物角点对齐后，进行以下处理。例如，标记物角点相对于标记物坐标系的三维坐标可以如下：
[0064][0065]
其中，为第i个标记物角点对应的三维坐标，r
wm
和β可以是基于标记物的已知特征确定的参数。例如，对于黑白相间的棋盘格标记物呈圆筒设置在运动臂末端，棋盘格标记物角点均匀分布在圆筒标记物周向(如图2(b)所示)，其中，r
wm
为标记物形成的圆筒的半径，β为标记物角点的分布角。
[0066]
在一些实施例中，基于单镜头图像采集设备，可以直接基于标记物的已知特征确定的参数(例如形成的圆筒的半径以及标记物角点的分布角)，计算标记物角点在标记物坐标系的三维坐标。基于双镜头图像采集设备，可以对齐左、右图像中各自检测到的标记物角点，以使双目立体矫正后同一空间点在y方向坐标一致，然后基于标记物的已知特征确定的参数，计算标记物角点在标记物坐标系的三维坐标。标记物坐标系可以如下建立：其中，标记物坐标系的原点与标记物中心重合，x轴可以从标记物中心指向第一个标记物角点(例如最先确定的标记物角点)。
[0067]
在一些实施例中，确定二维坐标和三维坐标的单应性矩阵的解析解可以包括基于如下公式计算：
[0068][0069]hleft
＝[h
1 h
2 h3]＝η
·a·
[r
1 r
2 ll
p
wm
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0070]
式(7)中，为第i个标记物角点的扩展坐标，[ui,vi]为第i个标记物角点的二维坐标，
left
si为任意的非零标量，
left
si为比例因子，以使式(7)左右两边最后一列数字相同，h
left
为单应性矩阵；
[0071]
式(8)中，a为图像采集设备的已知内部参数矩阵，r1,r2是
llrwm
矩阵的前两列，ll表示图像采集设备坐标系{ll}(此处以左镜头坐标系为例)，η为任意的非零标量。应当理解，通过引入扩展坐标，可以简化计算过程。
[0072]
在一些实施例中，单应性矩阵的解析解还可以包括基于如下公式计算：
[0073][0074]
其中，xh＝[h
1t h
2t h
3t
]
t
，l是2n
×
9矩阵，n为采集的图像中确定的标记物角点数量，xh为矩阵l的奇异值分解(svd)后最小奇异值对应的右奇异向量。
[0075]
在一些实施例中，可以基于以下公式，确定标记物相对于图像采集设备的当前相对位姿：
[0076]
r1＝κ
·
a-1
·
h1[0077]
r2＝κ
·
a-1
·
h2[0078]
llrwm
＝[r
1 r
2 r1×
r2]
[0079]
ll
p
wm
＝κ
·
a-1
h3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0080]
其中，κ＝1/||a-1
·
h1||＝1/||a-1
·
h2||。
[0081]
在一些实施例中，可以基于每个标记物角点的二维坐标和三维坐标之间的单应性矩阵的解析解，执行非线性优化处理以获得标记物相对于图像采集设备的当前相对位姿。例如，建立非线性优化处理，通过代数法计算标记物相对于图像采集设备的当前相对位姿。例如，非线性优化处理可以包括使三维坐标在采集的图像上的投影与标记物角点之间的平均几何误差最小化。
[0082]
在一些实施例中，图像采集设备是单镜头图像采集设备，采集的图像是单镜头(此处以左镜头为例，表示为“left”)图像，非线性优化处理可以包括基于如下公式优化：
[0083][0084]
在一些实施例中，图像采集设备为双镜头图像采集设备，采集的图像包括左镜头(表示为“left”)图像和右镜头(表示为“right”)图像，非线性优化处理可以包括基于如下公式优化：
[0085][0086]
其中，
llrwm
和
ll
p
wm
，分别为标记物在图像采集设备坐标系(或者左镜头坐标系){ll}中的姿态和位置；h
left
和h
right
分别为与左镜头图像、右镜头图像相关的单应性矩阵；
left
si和
right
si，分别为与左镜头图像、右镜头图像相关的单应性矩阵的解析解相关的比例因子，和分别为与左镜头图像、右镜头图像相关的第i个标记物角点的扩展坐标。应当理解，h
left
为左镜头图像采集的标记物角点的二维坐标与三维坐标对应的单应性矩阵，h
right
为右镜头图像采集的标记物角点的二维坐标与三维坐标对应的单应性矩阵，
left
si为
与h
left
的解析解相关的非零标量，为
right
si与h
right
的解析解相关的非零标量。例如，在图像采集设备为双镜头图像采集设备时，可以通过左镜头图像、右镜头图像中标记物角点的二维坐标及三维坐标的对应关系，建立非线性优化处理。其中标记物相对于图像采集设备坐标系的位姿[
llrwm ll
p
wm
]为优化变量，三维标记物角点重投影到左镜头图像、右镜头图像上的标记物角点与实际检测标记物角点间的平均几何误差最小作为优化目标，以获得最优化的左镜头外参(例如标记物与双镜头图像采集设备的相对位姿)。应当理解，以双镜头图像采集设备中的右镜头作为主镜头时，将左右镜像调整，算法与前述流程基本类似，本公开不再详述。
[0087]
通过识别图像采集设备采集的图像中的标记物角点，优化估计标记物相对于图像采集设备的位姿矩阵[
llrwm ll
p
wm
]，并基于公式计算运动臂末端相对于世界坐标系的位姿，并基于运动臂末端的位姿信息，进行运动臂末端位姿的实时闭环控制，以提高系统的运动臂运动精度。
[0088]
图7示出了根据本公开一些实施例的用于确定运动臂末端在世界坐标系中的位置的方法700的流程图。如图7所示，该方法700可以由运动臂系统(例如运动臂系统100)的控制装置(例如控制装置10)来执行。控制装置30可以配置在计算设备上。方法700可以由软件、固件和/或硬件来实现。
[0089]
在步骤701，基于运动臂末端的当前相对位姿，确定运动臂在世界坐标系中的当前位姿。在一些实施例中，可以基于识别的标记物角点，确定标记物相对于图像采集设备的当前相对位姿(例如姿态
llrwm
和位置
ll
p
wm
)。基于标记物与运动臂末端的已知相对位姿关系，以及图像采集设备在世界坐标系中的位姿，可以确定标记物所在的运动臂末端在世界坐标系中的位置。例如，确定运动臂末端在世界坐标系中的位置可以基于如下公式：
[0090]wp
tip
＝
wrll
(
llrwmwm
p
tip

ll
p
wm
) wp
ll
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0091]
其中，wp
tip
为运动臂末端在世界坐标系{w}中的位置，
wm
p
tip
为运动臂末端在标记物坐标系{wm}中的位置；
wrll
、wp
ll
分别为图像采集设备在世界坐标系{w}中的姿态和位置；
llrwm
、
ll
p
wm
分别为标记物在图像采集设备坐标系{ll}中的姿态和位置。应当理解，图像采集设备的位姿确定，
wrll
、wp
ll
可以为已知矩阵。
[0092]
其中，为运动臂末端在标记物坐标系{wm}中的位置坐标。
[0093]
在本公开中，世界坐标系其原点可以位于固定基座(例如鞘套)的中心处(如图2(a)或图8所示)。标记物坐标系其原点与运动臂标记物中心重合，x轴从标记物中心指向第一个标记物角点(如图2(a)、图2(b)所示)。左镜头坐标系其原点与双镜头图像采集设备左镜头的中心重合，x轴从双镜头图像采集设备左镜头的中心指向双镜头图像采集设备右镜头的中心。右镜头坐标系其原点与双镜头图像采集设备右镜头的中心重合，x轴与双镜头图像采集设备左镜头坐标系的x轴重合，指向相同。
[0094]
在步骤703，基于运动臂末端在世界坐标系中的目标位姿和当前位姿，确定运动臂末端的目标位姿和当前位姿的差值。在一些实施例中，运动臂末端在世界坐标系中的目标位姿可以由用户通过输入装置输入。通过比较计算，可以确定运动臂末端的目标位姿和当
前位姿的差值。例如，可以基于运动臂末端在世界坐标系中的目标位置和当前位置，计算目标位置和当前位置的差值δp，δp可以如下：
[0095]
δp＝p
d-pcꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0096]
其中，pd为运动臂末端在世界坐标系的目标位置，pc为运动臂末端在世界坐标系的当前位置(例如前述方法步骤701计算得到的wp
tip
)。
[0097]
基于差值δp得到运动臂末端的空间速度v及参数分别为：
[0098]
v＝v
xlim
δp/||δp||
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0099][0100]
其中，v
xlim
为设定的最大允许空间速度；j

是j的moore
–
penrose伪逆矩阵；ω为运动臂末端角速度；ψ可以表示运动臂的弯转角度或者运动臂关节的旋转角度参数向量，可以为ψ的导数。例如在运动臂为柔性运动臂时，ψ为柔性运动臂臂体的弯转角度参数向量，在运动臂为普通关节运动臂结构时，ψ为运动臂关节的旋转角度参数向量。
[0101]
在每个运动控制循环，可以更新运动臂末端的参数空间向量：
[0102][0103]
其中，δt为运动控制循环的周期。
[0104]
在步骤705，基于差值和运动臂的逆运动学数值迭代算法，确定运动臂的驱动信号。例如，基于运动臂末端在世界坐标系中的目标位姿和当前位姿的差值，通过运动臂运动学模型的逆运动学数值迭代算法，可以确定运动臂所包括的多个关节在当前运动控制循环内的驱动值(或者控制运动臂运动的对应多个电机的驱动值)。应当理解，运动学模型可以表示运动臂的关节空间和任务空间的运动关系的数学模型。例如，运动学模型可以通过dh参数法和指数积表示法等方法建立。
[0105]
在一些实施例中，方法700还可以包括：以预定周期，确定运动臂的驱动信号以实现多个运动控制循环。例如，迭代地执行多个运动控制循环，在每个运动控制循环，可以执行根据本公开一些实施例的控制方法，以控制运动臂运动到目标位姿。通过迭代地执行多个运动控制循环，可以实现运动臂末端位置的实时闭环控制，可以提高运动臂的位置控制精度，避免在运动臂末端具有负载时，通过开环控制算法导致的运动学建模不准确。应理解，经本公开的方法实现运动臂的位置控制，能改进运动臂(例如连续体柔性臂)的轨迹跟踪误差。例如，在一些实施例中，运动臂的轨迹跟踪误差可以降低至开环控制误差的25.23％。
[0106]
在一些实施例中，以运动臂为柔性运动臂(例如连续体柔性臂)作为示例。图8和图9分别示出根据本公开一些实施例中的连续体柔性臂的结构示意图、连续体柔性臂的单个连续体段的结构示意图。如图8和图9所示，连续体柔性臂可包括两段连续体段和两个直杆段，每个连续体段(如图9)包括2个自由度结构，其中一个直杆段位于两个连续体之间，另一个直杆段含1个进给自由度和1个整体绕自身轴线旋转自由度。
[0107]
如图9所示，每个连续体段可以包括基座环、末端环以及贯穿基座环和末端环的多根并列的结构骨，多根结构骨可以与末端环固定连接，与基座环滑动连接。连续体柔性臂的运动过程为平面弯转，连续体柔性臂及其包含的连续体段运动学描述如下：
[0108]
在本公开中，应当理解，基座环坐标系附着在第t节连续体段的
基座环上，其原点位于基座环中心，xy平面与基座环平面重合，从基座环中心指向第一根结构骨。
[0109]
弯曲平面坐标系1其原点与基座环坐标系原点重合，xz平面和弯曲平面重合，与重合。
[0110]
弯曲平面坐标系2其原点位于末端环中心，xy平面和弯曲平面重合，与重合。
[0111]
末端环坐标系附着在第t节连续体段的末端环上，其原点位于末端环中心，xy平面与末端环平面重合，从末端环中心指向第一根结构骨。
[0112]
在一些实施例中，连续柔性臂以及连续体段运动学建模的各符号的定义可以如表1所示。
[0113]
表1
[0114][0115]
如图9所示，单个连续体段的运动学如下：
[0116]
单个连续体段末端的位置
tb
p
te
、姿态
tbrte
可以如以下公式所示：
[0117][0118]
tbrte
＝
tbrt1 t1rt2 t2rte
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0119]
单个连续体段末端的角速度ω
t
、线速度v
t
和θ
t
、δ
t
速度的关系，可以如以下公式所示：
[0120][0121]
在一些实施例中，整个连续体柔性臂的运动学描述建立如下：
[0122]
根据图8中所示的各坐标系间的变换关系，连续体柔性臂的末端位置姿态在世界坐标系{w}中可表示为：
[0123]wt
tip
＝wt
1b1b
t
1e1e
t
2b2b
t
2e2e
t
tip
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0124]
其中，wt
tip
表示连续体的末端相对于世界坐标系的齐次变换矩阵；wt
1b
表示第一个连续体的基座环相对于世界坐标系的齐次变换矩阵；
1b
t
1e
表示第一个连续体的末端环相对于第一个连续体的基座环的齐次变换矩阵；
1e
t
2b
表示第二个连续体的基座环相对于第一个连续体的末端环的齐次变换矩阵；
2b
t
2e
表示第二个连续体的末端环相对于第二个连续体的基座环的齐次变换矩阵；
2e
t
tip
表示连续体的末端相对于第二个连续体的末端环的齐次变换矩阵。
[0125]
应理解，连续体柔性臂的末端位置在世界坐标系{w}中的齐次变换矩阵可表示为：
[0126]wt
tip
＝wt
wmwm
t
tip
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0127]
应理解，整个连续体柔性臂的末端的速度，可以如以下公式所示：
[0128][0129]
其中，为速度雅可比矩阵；矩阵中w1，w2和w3分别可以如以下公式所示：
[0130][0131][0132]
w3＝j
2v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0133]
为在坐标系a下，从坐标系b到坐标系c的向量的表达；为所述向量的反对称矩阵。
[0134]
该雅克比矩阵中：
[0135]
本领域技术人员可以理解，基于连续体运动臂运动学模的逆运动学解算，例如基于公式(15)、(16)和(23)，可以确定控制连续体运动臂运动的对应多个电机在当前运动控制循环内的驱动值。
[0136]
在一些实施例中，本公开提供了一种计算机可读存储介质，计算机可读存储介质可以包括至少一个指令，至少一个指令由处理器执行以将处理器配置为执行以上任何实施例中的控制方法。
[0137]
在一些实施例中，本公开提供了一种计算机系统，可以包括非易失性存储介质和至少一个处理器。非易失性存储介质可以包括至少一个指令。处理器被配置为执行至少一个指令以将处理器配置为执行以上任何实施例中的控制方法。
[0138]
在一些实施例中，计算机可读存储介质可以是可以保持和存储由指令执行设备使
用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意组合。
[0139]
在一些实施例中，计算机可读取存储介质可以包括但不限于：便携式计算机盘、硬盘、只读存储器(rom)、随机存取存储器(ram)、可擦除可编程只读存储器(eprom)、电可擦可编程只读存储器(eeprom)、闪存或其他固态存储器技术、cd-rom、数字多功能盘(dvd)、hd-dvd、蓝光(blue-ray)或其他光存储设备、磁带、磁盘存储或其他磁性存储设备、或能用于存储所需信息且可以由计算机访问的任何其他介质，其上存储有计算机可执行指令，计算机可执行指令在机器(例如计算机设备)中运行时，使得机器执行本公开的控制方法。应当理解，计算机设备可以包括个人计算机、服务器或者网络设备等。
[0140]
应理解，可由计算机可执行指令实现流程图和方框图中的每一流程和方框、以及流程图和方框图中的流程和方框的结合。可提供这些计算机可执行指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和方框图一个方框或多个方框中指定的功能的装置。
[0141]
本公开的一些实施例，能够基于视觉跟踪的闭环控制方法及设置的标记物，实时检测运动臂末端的姿态，有效降低运动臂的轨迹跟踪误差，可降低运动控制误差。而且，根据本公开一些实施例的控制方法可以以预定周期执行运动控制循环，每次运动控制循环中，确定运动臂末端的姿态，并对运动臂的运动进行控制，以实现目标位姿。
[0142]
本公开的一些实施例，基于有针对性的标记物角点搜索策略，大大降低了运算量，可以迭代实现对于运动臂末端的轨迹跟踪和误差校正。
[0143]
本公开的一些实施例，通过在不同步骤使用不同的方法，以对标记物角点进行识别和确定，可以避免由于各种因素导致的标记物角点的误识和错过，可以在短时间内实现高精确的运动臂末端的跟踪和实时校正。且本公开的技术方案具有广泛的适应性，可以适用于多种标记物和标记物识别设备，也可以广泛地应用于机器人，例如可以用于对手术机器人、工业机器人等的运动末端的跟踪及校正。
[0144]
注意，上述仅为本公开的示例性实施例及所运用技术原理。本领域技术人员会理解，本公开不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本公开的保护范围。因此，虽然通过以上实施例对本公开进行了较为详细的说明，但是本公开不仅仅限于以上实施例，在不脱离本公开构思的情况下，还可以包括更多其他等效实施例，而本公开的范围由所附的权利要求范围决定。