操作臂的控制方法和手术机器人系统与流程

1.本公开属于控制技术领域，尤其涉及一种操作臂的控制方法和手术机器人系统。


背景技术：

2.随着技术的发展，由人工或者计算机控制相关机器设备执行期望的动作以辅助或者替代操作人员变得越来越普及。例如，使用物流机器人进行快递的分拣，使用手术机器人辅助医生执行手术等。
3.上述应用中，需要对操作臂进行控制，从而实现机器设备的控制。


技术实现要素：

4.在一些实施例中，本公开提供一种操作臂的控制方法，包括：
5.获取定位图像；
6.在定位图像中，识别位于操作臂上的多个位姿标识；
7.基于多个位姿标识，识别位于操作臂上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系；
8.基于角度标识和多个位姿标识，确定操作臂相对参考坐标系的当前相对位姿；以及
9.基于当前相对位姿和操作臂的目标位姿，确定操作臂的驱动信号。
10.在一些实施例中，本公开提供了一种计算机设备，计算机设备包括：
11.存储器，用于存储有至少一条指令；以及
12.处理器，与存储器耦合，用于执行所述至少一条指令以执行本公开的方法。
13.在一些实施例中，本公开提供了一种计算机可读存储介质，存储介质中存储有至少一条指令，至少一条指令由处理器执行以使计算机执行本公开的方法。
14.在一些实施例中，本公开提供了一种手术机器人系统，包括：
15.手术工具，手术工具包括操作臂、设置在操作臂远端的执行器以及设置在操作臂的末端上的至少一个角度标识和多个位姿标识；
16.图像采集器，用于采集操作臂的定位图像；以及
17.处理器，与图像采集器连接，用于执行本公开的方法以确定操作臂的驱动信号。
附图说明
18.图1示出根据本公开一些实施例的操作臂控制系统的示意图；
19.图2示出根据本公开一些实施例的操作臂的构节示意图；
20.图3示出根据本公开一些实施例的操作臂的结构示意图；
21.图4示出包括多个位姿标识与多个角度标识的标签示意图；
22.图5示出标签设置在在操作臂末端周侧形成的圆筒形标签的示意图；
23.图6示出根据本公开一些实施例的实施场景示意图；
24.图7示出根据本公开一些实施例的操作臂控制系统的控制方法的流程图；
25.图8示出根据本公开一些实施例的用于确定驱动信号的方法的流程图；
26.图9示出根据本公开一些实施例的用于确定操作臂坐标系相对参考坐标系的位姿的方法的流程图；
27.图10示出根据本公开另一些实施例的用于确定操作臂坐标系相对参考坐标系的位姿的方法的流程图；
28.图11示出根据本公开一些实施例的多个位姿标识在截面圆的示意图；
29.图12示出根据本公开一些实施例的用于识别位姿标识的方法的流程图；
30.图13示出根据本公开一些实施例的位姿标识图案的示意图；
31.图14示出根据本公开一些实施例的用于搜索位姿标识的方法的流程图；
32.图15示出根据本公开一些实施例的搜索位姿标识的示意图；
33.图16示出根据本公开一些实施例的识别角度标识的方法的流程图；
34.图17示出根据本公开一些实施例的计算机设备的示意框图；
35.图18示出根据本公开一些实施例的手术机器人系统的示意图。
具体实施方式
36.下面结合附图描述本公开的示例性实施例，本领域技术人员可以理解，本公开的范围并不仅限于这些实施方案。本公开可以在下述实施方案的基础上作出各种改进和变化。这些改进和变化都包括在本公开的范围之内。在本公开的附图所示的各个实施方案之间，相似附图标记指示相似部件。
37.在本公开中，术语“位置”指物体或物体的一部分在三维空间中的定位(例如，可使用笛卡尔x、y和z坐标方面的变化描述三个平移自由度，例如分别沿笛卡尔x轴、y轴和z轴的三个平移自由度)。在本公开中，术语“姿态”指物体或物体的一部分的旋转设置(例如三个旋转自由度，可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指物体或物体的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。
38.在本公开中，参考坐标系可以理解为能够描述物体位姿的坐标系。根据实际的定位需求，参考坐标系可以选择以虚拟参照物的原点或实体参照物的原点为坐标系原点。在一些实施例中，参考坐标系可以为世界坐标系或者相机坐标系或者操作人员自身的感知坐标系等。
39.在本公开中，物体可以理解为需要被定位的对象或目标，例如操作臂或者操作臂末端。
40.在本公开中，操作臂或其一部分的位姿是指操作臂或其一部分定义的操作臂坐标系相对于参考坐标系的位姿。
41.图1示出根据本公开一些实施例的操作臂控制系统100的示意图。如图1所示，操作臂控制系统100可以包括图像采集设备110、至少一个操作臂140和控制装置120。图像采集设备110和至少一个操作臂140分别与控制装置120通信连接。在一些实施例中，如图1所示，控制装置120可以用于控制至少一个操作臂140的运动，以调整至少一个操作臂140的位姿、相互协调等。在一些实施例中，至少一个操作臂140在末端或远端处可以包括操作臂末端
130。控制装置120可以控制至少一个操作臂140运动，以使操作臂末端130运动至期望的位置和姿态。本领域的技术人应理解，操作臂控制系统100可以应用于手术机器人系统，例如腔镜手术机器人系统。例如，操作臂末端130的远端处可以设置手术执行器160，如图1所示。应当理解，操作臂控制系统100还可以应用于其他领域(例如，制造、机械等等)的专用或通用机器人系统。
42.在本公开中，控制装置120可以与至少一个操作臂140的驱动单元150(例如电机)通信连接并向驱动单元150发送驱动信号，从而使驱动单元150基于驱动信号控制至少一个操作臂140运动到相应的目标位姿。例如，控制操作臂140运动的驱动单元150可以为伺服电机，可以接受控制装置的指令以控制操作臂140运动。控制装置120还可例如通过通信接口与驱动单元150耦合的传感器通信连接，以接收操作臂140的运动数据，实现对操作臂140的运动状态监控。在本公开的一个示例中，该通信接口可以为can(controller area network)总线通信接口，其使得控制装置120能够通过can总线与驱动单元150以及传感器连接通信。
43.在一些实施例中，操作臂140可以包括连续体可形变臂，例如由多个关节构成的具有多自由度的操作臂，诸如可以实现6个自由度运动的操作臂。图像采集设备110可以包括但不限于双镜头图像采集设备或单镜头图像采集设备，例如双目或单目相机。
44.在一些实施例中，图像采集设备110可以用于采集定位图像。定位图像中可以包括操作臂140的部分或全部的图像。在一些实施例中，图像采集设备110可以用于采集操作臂末端130的图像，操作臂末端130上设置有定位标识。定位标识可以包括位姿标识和角度标识(如下详述)。如图1所示，操作臂末端130处于图像采集设备110的观测视野之内，则采集的定位图像中可以包括操作臂末端130的图像。
45.在一些实施例中，控制装置120可以接收来自图像采集设备110的定位图像，并且对定位图像进行处理。例如，控制装置120可以在定位图像中，识别位于操作臂140上的多个位姿标识和至少一个角度标识，并且确定操作臂140相对参考坐标系(例如世界坐标系)的当前相对位姿。控制装置120还可以基于操作臂140的当前相对位姿和目标位姿，确定操作臂140的驱动信号。驱动信号可以发送到驱动单元150，以对操作臂140进行运动控制。
46.图2示出根据本公开一些实施例的操作臂的构节200示意图。操作臂(例如操作臂140)可以包括至少一个可形变的构节200。如图2所示，可形变的构节200包括固定盘210和多根结构骨220。多根结构骨220的第一端与固定盘210固定连接，第二端与驱动单元(未示出)连接。在一些实施例中，固定盘210可以是包括但不限于环状结构、盘状结构等，而且横截面可以是圆形、矩形、多边形等等各种形状。
47.驱动单元通过驱动结构骨220使构节200发生形变。例如，驱动单元通过驱动结构骨220使构节200处于如图2所示的弯曲状态。在一些实施例中，多根结构骨220的第二端穿过基盘230与驱动单元连接。在一些实施例中，与固定盘210类似，基盘230可以是包括但不限于环状结构、盘状结构等，而且横截面可以是圆形、矩形、多边形等等各种形状。驱动单元可以包括线性运动机构、驱动构节或两者的组合。线性运动机构可以与结构骨220连接，以推动或拉动结构骨220，进而驱动构节200弯曲。驱动构节可以包括固定盘和多根结构骨，其中多根结构骨的一端固定连接到固定盘。驱动构节的多根结构骨的另一端与多根结构骨220连接或一体成型，以通过驱动构节的弯曲来驱动构节200的弯曲。
48.在一些实施例中，在固定盘210与基盘230之间还包括间隔盘240，多根结构骨220穿过间隔盘240。类似地，驱动构节也可以包括间隔盘。
49.图3示出根据本公开一些实施例的操作臂300的结构示意图。如图3所示，操作臂300为可形变的操作臂，操作臂300可以包括操作臂末端310和操作臂主体320。操作臂主体320可以包括一个或多个构节，例如第一构节3201和第二构节3202。在一些实施例中，第一构节3201和第二构节3202的结构可以与图2所示的构节200类似。在一些实施中，如图3所示，操作臂主体320还包括位于第一构节3201和第二构节3202之间的第一直杆段3203。第一直杆段3203的第一端与第二构节3202的基盘连接，第二端与第一构节3201的固定盘连接。在一些实施中，如图3所示，操作臂主体320还包括第二直杆段3204，第二直杆段3204的第一端与第一构节3201的基盘连接。
50.在一些实施例中，操作臂上(例如，操作臂主体320或操作臂末端310上)分布有多个位姿标识和至少一个角度标识。例如，多个位姿标识沿周向分布在操作臂末端310上，多个角度标识沿周向分布在操作臂末端310上。多个位姿标识与多个角度标识在操作臂末端310上沿轴向并列设置。例如，多个位姿标识与多个角度标识设置在操作臂末端310的柱状部分的外表面上。
51.在一些实施例中，每个角度标识与位姿标识之一具有位置关联关系。基于该位置关联关系，通过位姿标识的位置，可以确定角度标识可能分布的区域。或者，通过角度标识的位置，可以确定位姿标识可能分布的区域。位置关联关系可以根据位姿标识和角度标识的具体布置方式确定，并且可以是预先设计的。
52.在一些实施例中，位置关联关系可以包括角度标识与位姿标识在轴向的对应关系。例如，位置关联关系可以包括沿轴向的偏移。基于在轴向的对应关系，在已知一个或多个位姿标识在操作臂末端上的位置的情况下，沿轴向偏移一定的距离可以确定角度标识可能存在的区域。例如，位置关联关系还可以包括沿轴向的斜对齐等等。
53.在一些实施例中，多个位姿标识与多个角度标识可以设置在贴附在操作臂末端周侧的标签上。
54.在一些实施例中，位姿标识可以包括位姿标识图案和位姿标识图案角点，角度标识可以包括角度标识图案和角度标识图案角点。在一些实施例中，位姿标识图案和角度标识图案可以设置在贴附在操作臂末端上的标签上，或者可以印刷在操作臂末端上，或者可以是由操作臂末端自身的物理构造形成的图案，例如，可以包括凹陷或凸起及其组合。在一些实施例中，位姿标识图案或角度标识图案可以包括以亮度、灰度、色彩等形成的图案。在一些实施例中，位姿标识图案和角度标识图案可以包括主动(例如，自发光)或被动(例如，反射光线)提供被图像采集模块探测的信息的图案。本领域技术人员可以理解，在一些实施例中，位姿标识的位姿可以由位姿标识图案角点坐标系的位姿来表示，角度标识的位姿可以由角度标识图案角点坐标系的位姿来表示。
55.在一些实施例中，位姿标识图案或角度标识图案设置在操作臂末端上适于被图像采集设备采集图像的区域，例如，在工作过程中可以被图像采集设备的视场覆盖的区域或者在工作过程中不容易被干扰或遮挡的区域。
56.图4示出根据一些实施例的包括多个位姿标识与多个角度标识的标签400的示意图。图5示出设置在操作臂末端周侧并形成圆筒形的标签500的示意图。可以理解，简单起
见，标签400可以与标签500包括相同的位姿标识图案和角度标识图案。
57.参看图4，多个位姿标识(本公开中以
“○”
符号表示位姿标识图案角点)和多个角度标识(本公开中以
“△”
符号表示角度标识图案角点)并排设置。多个位姿标识图案411可以相同或类似，并且多个位姿标识图案角点位于多个位姿标识图案411中。多个角度标识图案421-426可以不同，并且多个角度标识图案角点位于多个角度标识图案421-426中。
58.每个角度标识与位姿标识之一可以具有位置关联关系。例如，如图4所示，在箭头所示的方向上，部分位姿标识(例如位姿标识图案411)和对应的角度标识(例如，角度标识图案421)沿箭头方向排列并且具有间隔距离d1。参看图5，在沿周向设置状态下，标签400变为空间构造为圆筒形的标签500，每个角度标识与位姿标识之一的位置关联关系可以包括角度标识与位姿标识在轴向(如图5中z轴正方向)的对应关系。基于在轴向的对应关系，在已知一个或多个位姿标识在操作臂末端上的位置的情况下，沿轴向偏移一定的距离(如距离d1)可以确定角度标识可能存在的区域。在一些实施例中，角度标识与位姿标识在轴向的对应关系可以通过角度标识图案角点与位姿标识图案角点在轴向的对应关系表示。在一些实施例中，基于角度标识与位姿标识在轴向的对应关系，角度标识图案角点与位姿标识图案角点之一沿z轴方向的投影重合。
59.在一些实施例中，角度标识或位姿标识的绕轴角度或者滚转角可以通过角度标识图案角点或位姿标识图案角点的绕轴角度表示。角度标识图案角点相对于操作臂坐标系(例如建立在操作臂末端的坐标系，如图5所示的xy坐标系)的角度是已知的或预先确定的，例如图5中的角度标识图案角点r5在xy坐标系下与x轴的角度为θ。基于位置关联关系，可以得到与其位置相关联的位姿标识图案角点p5相对x轴的夹角为角度θ。应该理解，角度标识图案角点r5和位姿标识图案角点p5对应的角度θ可以称为角度标识或位姿标识的绕z轴的绕轴角度或者滚转角。在本公开中，绕轴角度或者滚转角是指绕z轴的角度。可以理解，清楚起见，图5中将角度标识图案角点r5和位姿标识图案角点p5示为分离，但它们是重合的。
60.图6示出根据本公开一些实施例的实施场景600的示意图。如图6所示，操作臂640包括末端630以及远端的执行器660，多个位姿标识和角度标识可以沿周向设置在末端630上。例如，如图4所示的标签400沿周向设置在操作臂末端630上，形成圆筒形的角度标识图案带610和位姿标识图案带620。多个位姿标识图案角点分布在操作臂末端630的位姿标识图案带620的截面圆621上，多个角度标识图案角点分布在操作臂末端630的角度标识图案带610的截面圆611上。
61.在一些实施例中，多个角度标识图案为不同的图案。每个角度标识图案用于指示或标识不同的绕轴角度。在一些实施例中，每个角度标识的图案与所标识的绕轴角度具有一一对应关系，基于角度标识的图案可以确定所标识的绕轴角度。
62.例如，如图6所示，多个不同的角度标识图案(如图4中示出的多个角度标识图案421-426)沿圆柱形结构周向均匀分布，形成角度标识图案角点a-f。设定角度标识图案角点a对应的角度标识图案为参照图案(例如，设定角度标识图案角点a对应的角度标识图案用于标识0
°
绕轴角度)，设立平面坐标系{wm1}，则可以根据其余角度标识图案与角度标识图案角点a对应的角度标识图案的位置关系确定其余角度标识图案包含的角度标识图案角点标识的绕轴角度。例如，参看图6，当识别出角度标识图案角点b对应的角度标识图案，则根据角度标识图案角点b对应的角度标识图案与角度标识图案角点a对应的角度标识图案的
位置关系，可以确定在截面圆611的二维平面坐标系内角度标识图案角点b标识的绕轴角度为60
°
。截面圆611的二维平面坐标系的原点为截面圆611的圆心，x轴方向为原点指向角度标识图案角点a，y轴垂直于x轴。
63.在一些实施例中，执行器660的位姿可以通过将操作臂坐标系{wm}(例如，操作臂末端坐标系)平移预定距离来确定。或者，执行器660的位姿可以近似地等于操作臂末端坐标系{wm}的位姿。
64.在一些实施例中，基于操作臂坐标系相对参考坐标系的位姿，确定执行器660相对参考坐标系(例如，参考坐标系为世界坐标系{w})的位姿。具体计算公式如下：
65.wrtip
＝
wrwmwmrtip
ꢀꢀꢀ
(1)
66.w
p
tip
＝
wrwmwm
p
tip
+wp
wm
67.其中，
wrtip
为执行器相对世界坐标系的姿态，wp
tip
为执行器相对世界坐标系的位置，
wmrtip
为执行器相对世界坐标系的姿态，
wm
p
tip
为执行器相对世界坐标系的位置，
wrwm
为操作臂坐标系相对世界坐标系的姿态，wp
wm
为操作臂坐标系相对世界坐标系的位置。
68.本公开的一些实施例提供了一种操作臂的控制方法。图7示出根据本公开一些实施例的操作臂控制系统(例如操作臂控制系统100)的控制方法700的流程图。如图7所示，该方法700中的部分或全部步骤可以由操作臂控制系统100的控制装置(例如控制装置120)来执行。控制装置120可以配置在计算设备上。方法700可以由软件、固件和/或硬件来实现。在一些实施例中，方法700可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的处理器1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
69.参看图7，在步骤701，获取定位图像。在一些实施例中，定位图像中包含操作臂上的多个位姿标识和至少一个角度标识。在一些实施例中，可以从如图1所示的图像采集设备110接收定位图像。例如，控制装置120可以接收图像采集设备110主动发送的定位图像。或者，控制装置120可以向图像采集设备110发送图像请求指令，图像采集设备110响应图像请求指令向控制装置120发送定位图像。
70.继续参看图7，在步骤703，在定位图像中，识别位于操作臂上的多个位姿标识。例如，识别位于操作臂上的多个位姿标识的示例性方法可以包括如图12和图14所示的方法。在一些实施例中，控制装置120可以通过图像处理算法识别定位图像中的部分或全部的位姿标识。在一些实施例中，图像处理算法可以包括特征识别算法，图像处理算法可以提取或识别位姿标识的特征。例如，图像处理算法可以包括角点检测算法，用于检测位姿标识图案角点。该角点检测算法可以是包括但不限于基于灰度图的角点检测、基于二值图像的角点检测、基于轮廓曲线的角点检测中的一种。例如，图像处理算法可以是颜色特征提取算法，用于检测位姿标识图案中的颜色特征。再例如，图像处理算法可以是轮廓检测算法，用于检测位姿标识图案的轮廓特征。
71.在一些实施例中，控制装置可以通过识别模型识别定位图像中的部分或全部的位姿标识。
72.继续参看图7，在步骤705，基于多个位姿标识，识别位于操作臂上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系。在一些实施例中，在识别出多个位姿标识之后，根据位置关联关系，识别位于操作臂上的角度标识。在一些实施例中，
角度标识与第一位姿标识的位置关联关系可以是如图4或图5中示出的位置关联关系。在一些实施例中，第一位姿标识(例如是第一位姿标识图案或第一位姿标识图案角点)是指多个位姿标识中与角度标识具有位置关联关系的位姿标识。识别角度标识的示例性方法包括如图16所示的方法。
73.继续参看图7，在步骤707，基于角度标识和多个位姿标识，确定操作臂相对参考坐标系的当前相对位姿。确定操作臂相对参考坐标系的相对位姿的示例性方法包括如图9或图10所示的方法。在一些实施例中，可以基于角度标识、第一位姿标识和多个位姿标识，确定操作臂相对参考坐标系的位姿。
74.在一些实施例中，方法700还包括：基于角度标识和多个位姿标识，确定操作臂坐标系和位姿标识坐标系的变换关系。在一些实施例中，根据操作臂坐标系和位姿标识坐标系的变换关系，可以将位姿标识坐标系中的三维坐标转换为在操作臂坐标系中对应的三维坐标。在一些实施例中，根据操作臂坐标系和位姿标识坐标系的变换关系和位姿标识坐标系相对参考坐标系的位姿，得到操作臂坐标系相对参考坐标系的位姿。
75.在一些实施例中，操作臂坐标系和位姿标识坐标系的变换关系可以包括位姿标识坐标系相对操作臂坐标系的滚转角。在一些实施例中，可以基于角度标识和第一位姿标识，确定位姿标识坐标系相对操作臂坐标系的滚转角。应该理解，位姿标识坐标系相对操作臂坐标系的滚转角可以是位姿标识坐标系绕操作臂坐标系的z轴的转动的角度。
76.在一些实施例中，操作臂坐标系可以是基于多个位姿标识或多个角度标识，在物体上设定的固定坐标系。在一些实施例中，操作臂坐标系的z轴平行于操作臂的轴向，操作臂坐标系的xy平面与多个位姿标识图案角点处于同一平面，或者与多个角度标识图案角点处于同一平面。
77.在一些实施例中，可以确定位姿标识坐标系，以方便确定多个位姿标识的位置。在一些实施例中，位姿标识的位置可以通过位姿标识图案角点的位置表示。在一些实施例中，位姿标识坐标系的z轴平行于操作臂的轴向或与轴向重合，位姿标识坐标系的xy平面与多个位姿标识图案角点处于同一平面。
78.示例性地，参看图6，操作臂坐标系{wm}≡[x
wm y
wm z
wm
]
t
的原点为多个位姿标识图案角点所在截面圆621的圆心，x轴方向为原点指向其中一个位姿标识图案角点，z轴的方向平行操作臂末端630的轴向，y轴垂直于xz平面。操作臂坐标系{wm}的x轴与截面圆611的二维平面坐标系{wm1}≡[x
wm1 y
wm1
]
t
的x轴平行，操作臂坐标系的y轴与截面圆611的二维平面坐标系{wm1}的y轴平行。角度标识图案角点在截面圆611的二维平面坐标系{wm1}中标识的绕轴角度可以与其在操作臂坐标系{wm}中标识的绕轴角度相等。位姿标识坐标系{wm0}≡[x
wm0 y
wm0 z
wm0
]
t
的原点为多个位姿标识图案角点所在截面圆621的圆心，x轴方向为原点指向其中一个位姿标识图案角点，z轴的方向平行物体操作臂末端630的轴向，y轴垂直于xz平面。继续参看图6，操作臂坐标系{wm}的z轴与位姿标识坐标系{wm0}的z轴重合。操作臂坐标系{wm}相对位姿标识坐标系{wm0}的变换关系可以通过位姿标识坐标系{wm0}相对操作臂坐标系{wm}的滚转角α0确定。滚转角α0可以是指位姿标识坐标系{wm0}相对操作臂坐标系{wm}绕z轴的转动角度。
[0079]
在一些实施例中，参看图6，滚转角α0通过如下公式计算：
[0080]
α0＝α
1-α2ꢀꢀꢀ
(2)
[0081]
其中α1为第一绕轴角度，α2为第二绕轴角度。第一绕轴角度为角度标识图案角点(例如，角度标识图案角点r6)在操作臂坐标系中标识的绕轴角度。第二绕轴角度为第一位姿标识图案角点(例如，位姿标识图案角点p6)在位姿标识坐标系中标识的绕轴角度。
[0082]
继续参看图7，在步骤709，基于当前相对位姿和操作臂的目标位姿，确定操作臂的驱动信号。在一些实施例中，方法700还可以包括以预定周期，确定操作臂的驱动信号，以通过多个运动控制循环实现实时控制。
[0083]
在一些实施例中，方法700还可以包括：基于操作臂的当前相对位姿和操作臂的目标位姿，确定位姿差；以及基于位姿差和操作臂的逆运动学模型，确定操作臂的驱动信号。例如，基于操作臂末端在世界坐标系中的目标位姿和当前位姿的差值，通过操作臂运动学模型的逆运动学数值迭代算法，可以确定操作臂所包括的多个关节在当前运动控制循环内的驱动值(或者控制操作臂运动的对应多个电机的驱动值)。应当理解，运动学模型可以表示操作臂的关节空间和任务空间的运动关系的数学模型。例如，运动学模型可以通过dh(denavit-hartenberg)参数法和指数积表示法等方法建立。
[0084]
在一些实施例中，操作臂的目标位姿为操作臂在世界坐标系中的目标位姿。方法700还可以包括：基于当前相对位姿，确定操作臂在世界坐标系中的当前位姿；以及基于操作臂的目标位姿和所述操作臂在世界坐标系中的当前位姿，确定位姿差。在一些实施例中，位姿差包括位置差和姿态差。
[0085]
在第k次运动控制循环中，位姿差可以表示如下：
[0086][0087]
其中，为第k次运动控制循环时操作臂的位置差，为第k次运动控制循环时操作臂的角度差，p
tk
为第k次运动控制循环时操作臂的目标位置，r
tk
为第k次运动控制循环时操作臂的目标姿态，为第k次运动控制循环时操作臂的当前位置，r
tk
为第k次运动控制循环时操作臂的当前姿态，表示与r
tk
之间的转角。
[0088]
在一些实施例中，以预定周期，在每个运动控制循环之前或过程中，更新操作臂的目标位姿。在一些实施例中，迭代地执行多个运动控制循环，在每个运动控制循环，可以执行根据本公开一些实施例的方法，例如步骤701-709，以控制操作臂运动到目标位姿。通过迭代地执行多个运动控制循环，可以实现操作臂末端位姿的实时闭环控制，可以提高操作臂的位姿控制精度。应理解，经本公开的方法实现操作臂的位姿控制，能改进操作臂(例如连续体可形变臂)的轨迹跟踪误差。
[0089]
在一些实施例中，方法700还可以包括接收控制命令；以及基于控制命令，确定操作臂的目标位姿。在一些实施例中，操作臂末端在世界坐标系中的目标位姿可以由用户通过输入装置输入。通过比较计算，可以确定操作臂末端的目标位姿和当前位姿的差值。在一些实施例中，可以基于主从运动的控制方式接收控制命令。例如，通过在每个运动控制循环中，获取主操作器的位姿或者主操作器的关节信息，确定操作臂的目标位姿。通过多个运动
控制循环，可以进行实时主从运动的控制。
[0090]
图8示出根据本公开一些实施例的用于确定驱动信号的方法800的流程图。如图8所示，该方法800中的部分或全部步骤可以由操作臂控制系统100的控制装置(例如控制装置120)来执行。控制装置120可以配置在计算设备上。方法800可以由软件、固件和/或硬件来实现。在一些实施例中，方法800可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的处理器1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0091]
参看图8，在步骤801，基于位姿差，确定笛卡尔空间速度。在一些实施例中，笛卡尔空间速度包括笛卡尔空间线速度和笛卡尔空间角速度。方法800还可以包括：基于位置差，确定笛卡尔空间线速度，以及基于姿态差，确定笛卡尔空间角速度。在一些实施例中，可以基于姿态差，通过比例-积分-微分控制器或者比例-微分控制器确定笛卡尔空间角速度。在一些实施例中，第k次运动控制循环的笛卡尔空间速度如下：
[0092][0093]
其中，vk为第k次运动控制循环的笛卡尔空间线速度，ωk为第k次运动控制循环的笛卡尔空间角速度，pv为线速度比例系数，dv为线速度微分系数，p
ω
为角速度比例系数，d
ω
为角速度微分系数，为第k-1次运动控制循环时操作臂的位置差，为第k-1次运动控制循环时操作臂的角度差。
[0094]
参看图8，在步骤803，基于笛卡尔空间速度，确定参数空间速度。第k次运动控制循环的关节参数空间速度如下：
[0095][0096]j+
是操作臂的运动学模型的速度雅克比矩阵j的moore
–
penrose伪逆矩阵，操作臂的运动学模型的速度雅克比矩阵j可以基于操作臂的结构确定。
[0097]
参看图8，在步骤805，基于参数空间速度和当前关节参数，确定目标关节参数。第k次运动控制循环的目标关节参数如下：
[0098][0099]
其中，为第k次运动控制循环的当前关节参数，δt为运动控制循环的周期。
[0100]
应理解，当操作臂具有多个构节(例如，图3所示的操作臂300)，操作臂的目标关节参数可以是全部的构节的目标关节参数，也可以是多个构节中的一个或一些构节的目标关节参数。
[0101]
参看图8，在步骤807，基于目标关节参数，确定驱动信号。例如，基于目标关节参数与驱动量的映射关系，可以确定操作臂所包括的多个关节在当前运动控制循环内的驱动
量，进而基于驱动量确定驱动单元(例如，电机)的驱动信号。在一些实施例中，单个构节的关节参数与驱动量的映射关系可以是如公式(15)示出的映射关系。
[0102]
在一些实施例中，以操作臂为可形变运动臂(例如连续体可形变臂)作为示例。连续体可形变臂可以是如图3所示的操作臂300。如图3所示，每个构节(第一构节3201和第二构节3202)可以包括基座盘、固定盘以及贯穿基座盘和固定盘的多根结构骨，多根结构骨可以与固定盘固定连接，与基座盘滑动连接。连续体可形变臂及其包含的构节可以通过运动学模型描述。在一些实施例中，每个构节的结构具体可如图2所示的构节200。如图2所示，基座盘坐标系附着在第t(t＝1,2,3
…
)节连续体段的基座盘上，其原点位于基座盘中心，xy平面与基座盘平面重合，从基座盘中心指向第一根结构骨(第一根结构骨可以理解为从多根结构骨中任意指定的一根作为参照的结构骨)。弯曲平面坐标系其原点与基座盘坐标系原点重合，xy平面和弯曲平面重合，与重合。固定盘坐标系附着在第t节连续体段的固定盘上，其原点位于固定盘中心，xy平面与固定盘平面重合，从固定盘中心指向第一根结构骨。弯曲平面坐标系其原点位于固定盘中心，xy平面和弯曲平面重合，与重合。
[0103]
如图2所示的单个构节200可以通过运动学模型表示。第t节构节末端(固定盘坐标系{te})相对于基座盘坐标系{tb}的位置
tb
p
te
、姿态
tbrte
可以分别如以下公式(7)、(8)所示：
[0104][0105]
tbrte
＝
tbrt1t1rt2t2rte
ꢀꢀꢀ
(8)
[0106]
其中，l
t
为第t节构节的虚拟结构骨(例如，图2中示出的虚拟结构骨221)的长度，θ
t
为在第t节构节中，关于或者旋转到所需的转动角度，
tbrt1
为第t节构节的弯曲平面坐标系1{t1}相对于基座盘坐标系{tb}的姿态，
t1rt2
为第t节构节的弯曲平面坐标系2{t2}相对于弯曲平面坐标系1{t1}的姿态，
t2rte
为第t节构节的固定盘坐标系{te}相对于弯曲平面坐标系2{t2}的姿态。
[0107]
tbrt1
、
t1rt2
和
t2rte
可以分别如以下公式(9)、公式(10)和公式(11)所示：
[0108]
[0109][0110][0111]
其中，δ
t
为在第t节构节中，弯曲平面和的夹角。
[0112]
如图2所示的单个构节200的关节参数ψ
t
可以如以下公式(12)所示：
[0113]
ψ
t
＝[θ
t
,δ
t
]
t
ꢀꢀꢀ
(12)
[0114]
关节参数ψ
t
的参数空间速度可以如以下公式(13)所示：
[0115][0116]
其中，为θ
t
的一阶导数，为δ
t
的一阶导数。
[0117]
如图2所示的单个构节200的末端笛卡尔空间速度可以如以下公式(14)所示：
[0118][0119]
其中，j
t
是单个构节的的运动学模型的速度雅克比矩阵，j
tv
是单个构节的运动学模型的线速度雅克比矩阵，j
tω
是单个构节的的运动学模型的角速度雅克比矩阵，v
t
是单个构节的末端线速度，ω
t
是单个构节的末端角速度。
[0120]
在一些实施例中，多根结构骨的驱动量与关节参数具有已知的映射关系。基于构节的目标关节参数和映射关系，可以确定多根结构骨的驱动量。多根结构骨的驱动量可以理解为将单个构节从初始状态(例如，θ
t
＝0)弯曲至目标弯曲角度时结构骨受推或受拉的长度。在一些实施例中，多根结构骨的驱动量与关节参数的映射关系可以如以下公式(15)所示：
[0121]
qi≡-r
ti
θ
t
cos(δ
t
+β
ti
)
ꢀꢀꢀ
(15)其中，r
ti
为第t节构节中第i根结构骨到虚拟结构骨的距离，β
ti
为第t节构节中第i根结构骨和第一根结构骨的夹角，qi为第i根结构骨的驱动量，基于第i根结构骨的驱动量可以确定驱动单元的驱动信号。
[0122]
在一些实施例中，基于公式(3)和公式(4)可以确定单个构节的末端笛卡尔空间速度，基于公式(5)可以确定单个构节的参数空间速度，基于公式(6)可以确定单个构节的目标关节参数，基于公式(15)可以确定每根结构骨的驱动量，进而基于驱动量确定驱动单元(例如，电机)的驱动信号。
[0123]
在一些实施例中，整个可形变臂可以通过运动学模型来描述。如图3所示，位于可
形变臂多个位置处的多个坐标系之间可以进行变换。例如，连续体可形变臂的末端执行器在世界坐标系{w}中可表示为：
[0124]wt
tip
＝wt
1b1b
t
1e1e
t
2b2b
t
2e2e
t
tip
ꢀꢀꢀ
(16)
[0125]
其中，wt
tip
表示连续体可形变臂的末端执行器相对于世界坐标系的齐次变换矩阵；wt
1b
表示第一连续体段的基座盘相对于世界坐标系的齐次变换矩阵；
1b
t
1e
表示第一连续体段的固定盘相对于第一连续体段的基座盘的齐次变换矩阵；
1e
t
2b
表示第二连续体段的基座盘相对于第一连续体段的固定盘的齐次变换矩阵；
2b
t
2e
表示第二连续体段的固定盘相对于第二连续体段的基座盘的齐次变换矩阵；
2e
t
tip
表示连续体可形变臂的末端执行器相对于第二连续体段的固定盘的齐次变换矩阵。在一些实施例中，末端执行器固定设置在固定盘上，因此
2e
t
tip
是已知的或者预先确定的。
[0126]
应理解，可变形臂在不同的工作状态具有不同的关节参数。例如，图3所示的操作臂300至少包括四种工作状态。操作臂300的四种工作状态如下所述：
[0127]
第一工作状态：只有第二构节3202参与执行器的位姿控制(例如，只有第二构节3202进入工作空间)，此时操作臂300的关节参数如以下公式(17)所示：
[0128][0129]
其中，ψ
c1
为第一工作状态下操作臂300的关节参数，为操作臂300的绕轴转角，l2、θ2、δ2与如图2所示构节200中l
t
、θ
t
和δ
t
的物理意义相同。
[0130]
第二工作状态：第二构节3202和第一直线段3203参与执行器的位姿控制(例如，第二构节3202全部进入工作空间，第一直线段3203部分进入工作空间)，此时操作臂300的关节参数如以下公式(18)所示：
[0131][0132]
其中，ψ
c2
为第二工作状态下操作臂300的关节参数，lr为第一直线段3203的进给量。
[0133]
第三工作状态：第二构节3202、第一直线段3203和第一构节3201参与执行器的位姿控制(例如，第二构节3202全部进入工作空间，第一直线段3203全部进入工作空间，第一构节3201部分进入工作空间)，此时操作臂300的关节参数如以下公式(19)所示：
[0134][0135]
其中，ψ
c3
为第三工作状态下操作臂300的关节参数，θ1和δ1与如图2所示构节200中θ
t
和δ
t
的物理意义相同。
[0136]
第四工作状态：第二构节3202、第一直线段3203、第一构节3201和第二直线段3204参与执行器的位姿控制(例如，第二构节3202全部进入工作空间，第一直线段3203全部进入工作空间，第一构节3201全部进入工作空间，第二直线段3204部分进入工作空间)，此时操作臂300的关节参数如以下公式(20)所示：
[0137][0138]
其中，ψ
c4
为第四工作状态下操作臂300的关节参数，ls为第二直线段3204的进给量。
[0139]
在一些实施例中，与单个构节类似，基于公式(3)和公式(4)可以确定可形变臂的末端笛卡尔空间速度，基于公式(5)可以确定可形变臂的参数空间速度，基于公式(6)可以确定可形变臂的目标关节参数，其中公式(5)中的参数空间速度和公式(6)中的目标关节参数中包含的具体参数可以基于公式(17)、(18)、(19)或(20)确定，基于公式(15)可以确定每个构节的每根结构骨的驱动量，进而基于驱动量确定驱动单元(例如，电机)的驱动信号。
[0140]
图9示出根据本公开一些实施例的用于确定操作臂坐标系相对参考坐标系的位姿的方法900的流程图。如图3所示的操作臂300，操作臂坐标系可以包括操作臂末端的坐标系。如图9所示，该方法900中的部分或全部步骤可以由控制装置(例如，图1所示的控制装置120)来执行。方法900中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法900可以由机器人系统(例如，图18所示的手术机器人系统1800)执行。在一些实施例中，方法900可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的处理器1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0141]
参看图9，在步骤901，基于角度标识和多个位姿标识，确定位姿标识坐标系相对操作臂坐标系的滚转角。在一些实施例中，确定角度标识在操作臂坐标系中标识的第一绕轴角度。确定第一位姿标识在位姿标识坐标系中标识的第二绕轴角度。基于第一绕轴角度和第二绕轴角度，确定位姿标识坐标系相对操作臂坐标系的滚转角。在一些实施例中，位姿标识坐标系相对操作臂坐标系的滚转角可以基于公式(2)确定。
[0142]
在步骤903，基于多个位姿标识，确定位姿标识坐标系相对参考坐标系的位姿。位姿标识在相应坐标系中的坐标可以通过位姿标识图案角点在相应坐标系中的坐标表示。例如，位姿标识的在定位图像中的二维坐标和在位姿标识坐标系中的三维坐标可以通过位姿标识图案角点的坐标表示。在一些实施例中，基于多个位姿标识图案角点在定位图像中的二维坐标和多个位姿标识图案角点在位姿标识坐标系中的三维坐标，确定位姿标识坐标系相对参考坐标系的位姿。在一些实施例中，基于多个位姿标识图案角点在定位图像中的二维坐标、多个位姿标识图案角点在位姿标识坐标系中的三维坐标和相机坐标系相对参考坐标系的变换关系，确定位姿标识坐标系相对参考坐标系的位姿。
[0143]
在一些实施例中，基于多个位姿标识的分布，确定多个位姿标识图案角点在位姿标识坐标系中的三维坐标。例如，参看图11，每个位姿标识图案角点均位于在截面圆1122的圆周上，截面圆1122的圆心与半径r均是已知的。设定截面圆1122的圆心为位姿标识坐标系的原点，xy平面位于截面圆1122上，x轴可以指定为由原点出发指向任一已确定的位姿标识图案角点(例如，位姿标识图案角点p
11
)，进而可基于多个位姿标识的分布，确定每个位姿标识图案角点在位姿标识坐标系中的三维坐标。例如，如图11所示，位姿标识图案角点p
11
在位姿标识坐标系的三维坐标为(r,0,0)，则其余位姿标识图案角点在位姿标识坐标系中的三维坐标可以根据以下公式计算：
[0144]cm
＝[r
·
cos((m-1)
·
χ)r
·
sin((m-1)
·
χ)0]
t
ꢀꢀꢀ
(21)
[0145]
其中，cm为以位姿标识图案角点p
11
作为起点，第m个位姿标识图案角点在位姿标识坐标系中的三维坐标；χ为相邻的位姿标识图案角点之间的绕轴夹角。
[0146]
在一些实施例中，相机坐标系相对参考坐标系的变换关系可以是已知的。例如，参考坐标系为世界坐标系，相机坐标系相对世界坐标系的变换关系可以根据相机摆放的位姿
确定。另一些实施例中，根据实际的需求，参考坐标系也可以是相机坐标系本身。
[0147]
在一些实施例中，基于相机成像原理与投影模型，基于多个位姿标识图案角点在定位图像中的二维坐标和多个位姿标识图案角点在位姿标识坐标系中的三维坐标，确定位姿标识坐标系相对相机坐标系的位姿。基于位姿标识坐标系相对相机坐标系的位姿和相机坐标系相对参考坐标系的变换关系，可以得到位姿标识坐标系相对参考坐标系的位姿。在一些实施例中，还可以考虑相机的内参。例如，相机的内参可以是如图1所示的图像采集设备110的相机内参。相机的内参可以是已知的或者经过标定而得到的。
[0148]
在一些实施例中，相机坐标系可以理解为以相机原点建立的坐标系。例如，以相机的光心为原点建立的坐标系或者以相机的镜头中心为原点建立的坐标系。当相机为双目相机时，相机坐标系的原点可以是相机左镜头的中心，或者右镜头的中心，或者左右镜头中心连线上的任意一点(例如该连线的中点)。
[0149]
参看图9，在步骤905，基于位姿标识坐标系相对操作臂坐标系的滚转角和位姿标识坐标系相对参考坐标系的位姿，确定操作臂坐标系相对参考坐标系的位姿。在一些实施例中，操作臂坐标系相对参考坐标系的位姿可以作为操作臂相对参考坐标系的当前相对位姿。
[0150]
例如，以参考坐标系为世界坐标系为例，操作臂坐标系相对世界坐标系的位姿具体如下：
[0151][0152]
其中，
wrwm
为操作臂坐标系相对世界坐标系的姿态，wp
wm
为操作臂坐标系相对世界坐标系的位置，
wrwm0
为位姿坐标系相对世界坐标系的姿态，wp
wm0
为位姿坐标系相对世界坐标系的位置，rotz(α0)表示绕操作臂坐标系的z轴转动滚转角α0。
[0153]
在一些实施例中，操作臂坐标系相对世界坐标系的位姿具体计算公式如下：
[0154][0155]
其中，
wrlens
为相机坐标系相对世界坐标系的姿态，wp
lens
为相机坐标系相对世界坐标系的位置，
lensrwm0
为位姿标识坐标系相对相机坐标系的姿态，
lens
p
wm0
为位姿标识坐标系相对相机坐标系的位置，
wm0rwm
为操作臂坐标系相对位姿标识坐标系的姿态，
wm0
p
wm
为操作臂坐标系相对位姿标识坐标系的位置。
[0156]
图10示出根据本公开另一些实施例的用于确定操作臂坐标系相对参考坐标系的位姿的方法1000的流程图。方法1000可以是图9的方法900的替换实施例。如图10所示，该方法1000中的部分或全部步骤可以由数据处理装置(例如，图1所示的控制装置120，图18所示的处理器1820)来执行。方法1000中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1000可以由机器人系统(例如，图18所示的手术机器人系统1800)执行。在一些实施例中，方法1000可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的处理器1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
π/4四个方向的一阶导数；i
xy
和i
45_45
分别是像素点在0,π/2和π/4,-π/4方向的二阶导数。
[0166]
在一些实施例中，将roi划分成多个子图像。例如，可以采用非极大抑制法在一个roi范围中平均分割出多个子图像。在一些实施例中，可以将roi平均分割成5
×
5像素的多个子图像。上述实施例为示例性的，并非限制性的，应当理解，还可以将定位图像或roi分割成其他尺寸大小的多个子图像，例如，分割成9
×
9像素的多个子图像。可以确定每个子图像中的cl值最大的像素点，将每个子图像中的cl值最大的像素点与第一阈值进行比较，确定cl值大于第一阈值的像素点集合。在一些实施例中，第一阈值可以设定为0.06。应当理解，第一阈值还可以设定为其他值。在一些实施例中，cl值大于第一阈值的像素点可以作为候选位姿标识图案角点。
[0167]
参看图12，在步骤1203，基于位姿图案匹配模板，从多个候选位姿标识中识别初始位姿标识。在一些实施例中，通过位姿图案匹配模板与候选位姿标识图案角点之一处的图像进行匹配，确定达到预设位姿图案匹配度标准的候选位姿标识图案角点为初始位姿标识图案角点。
[0168]
在一些实施例中，位姿图案匹配模板与位姿标识图案角点附近区域的图像具有相同或相似的特征。若位姿图案匹配模板与候选位姿标识图案角点附近区域的图像的匹配度达到预设位姿图案匹配度标准(例如，匹配度高于阈值)，则可以认为候选位姿标识图案角点附近区域的图案与位姿图案匹配模板具有相同或相似的特征，进而可认为当前的候选位姿标识图案角点为位姿标识图案角点。
[0169]
在一些实施例中，确定像素点集合中cl值最大的像素点，作为待匹配候选位姿标识图案角点。例如，可以将该像素点集合中的所有像素点让cl值从大到小的顺序排序，并将cl值最大的像素点作为待匹配候选位姿标识图案角点。在确定了待匹配候选位姿标识图案角点之后，使用位姿图案匹配模板与待匹配候选位姿标识图案角点处的图案进行匹配，如果达到了预设位姿图案匹配度标准，则确定待匹配候选位姿标识图案角点为识别出的初始位姿标识图案角点。如果待匹配候选位姿标识图案角点未达到预设的匹配度标准，则选择次级cl值的像素点(cl值第二大的像素点)作为待匹配候选位姿标识图案角点，使用位姿图案匹配模板与该候选位姿标识图案角点处的图像进行匹配，依次类推，直至识别出初始位姿标识图案角点。
[0170]
在一些实施例中，位姿标识图案可以为黑白相间的棋盘格图形，因此位姿图案匹配模板可以为相同的棋盘格图形，利用位姿图案匹配模板的灰度分布gm与候选位姿标识图案角点对应的像素点的像素邻域灰度分布g
image
间的相关性系数(correlation coefficient,cc)来进行匹配。像素点的像素邻域灰度分布g
image
为以该像素点为中心一定范围内(例如，10
×
10像素)像的像素的灰度分布。具体公式如下：
[0171][0172]
其中，var为方差函数，cov为协方差函数。在一些实施例中，当cc值小于0.8时，像素领域内的灰度分布与位姿图案匹配模板相关性较低，则判定该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点，否则认为该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点。
[0173]
在一些实施例中，方法1200包括：确定候选位姿标识图案角点的边缘方向。例如，如图13所示，图13中包括一个位姿标识图案1311，候选位姿标识图案角点为图13中的角点p
13
，那么该角点p
13
的边缘方向可以是指形成角点p
13
的边缘的方向，如图13中虚线箭头示意的方向。
[0174]
在一些实施例中，边缘方向可以通过对以候选位姿标识图案角点为中心的一定范围邻域(例如10
×
10像素)的每个像素在平面坐标系的x方向和y方向的一阶导数值(i
x
和iy)确定。例如，边缘方向可以通过以下公式进行计算：
[0175][0176]
其中，一阶导数(i
x
和iy)可以通过对一定范围邻域范围内每一个像素点进行卷积操作得到。一些实施例中，通过对每个范围邻域内的像素点的边缘方向i
angle
和对应的权重i
weight
进行聚类计算获得该像素点的边缘方向，选择权重i
weight
占比最大的类对应的i
angle
作为边缘方向。需要说明的是，如果存在多个边缘方向，则选择权重i
weight
占比最大的多个类对应的i
angle
作为边缘方向。
[0177]
在一些实施例中，聚类计算所用的方法可以是k均值方法、birch(balanced iterative reducing and clustering using hierarchies，基于层次结构的平衡迭代聚类方法)方法、dbscan(density-based spatial clustering of applications with noise，具有噪声的基于密度的聚类方法)方法、gmm(gaussian mixed model，高斯混合模型)方法中的任意一种。
[0178]
在一些实施例中，方法1200包括：根据边缘方向旋转位姿图案匹配模板。根据边缘方向旋转位姿图案匹配模板，可以将位姿图案匹配模板与候选位姿标识图案角点处的图像对齐。
[0179]
候选位姿标识图案角点的边缘方向可以用于确定该候选标识图案角点处的图像在定位图像中的设置方向。在一些实施例中，根据边缘方向旋转位姿图案匹配模板，可以将位姿图案匹配模板调整至与候选位姿标识图案角点处的图像方向相同或接近相同以便于进行图像匹配。
[0180]
参看图12，在步骤1205，以初始位姿标识作为起点，搜索位姿标识。
[0181]
例如，图14示出根据本公开一些实施例的用于搜索位姿标识的方法1400的流程图。如图14所示，该方法1400中的部分或全部步骤可以由数据处理装置(例如，图1所示的控制装置120，图18所示的处理器1820)来执行。方法1400中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1400可以由机器人系统(例如，图18所示的手术机器人系统1800)执行。在一些实施例中，方法1400可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的处理器1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0182]
参看图14，在步骤1401，以初始位姿标识作为起点，确定第二个位姿标识。在一些实施例中，以初始位姿标识图案角点作为起点，在设定的搜索方向上搜索第二个位姿标识图案角点。在一些实施例中，设定的搜索方向可以包括：初始位姿标识图案角点的正前方(对应0
°
角度方向)、正后方(对应180
°
角度方向)、正上方(90
°
角度方向)、正下方(-90
°
角度方向)和斜向(例如
±
45
°
角度方向)中的至少一个方向。
[0183]
在一些实施例中，设定的搜索方向为n个，例如在8个方向进行搜索，每个搜索方向vsn可以根据如下公式计算：
[0184]vsn
＝[cos(n
·
π/4)sin(n
·
π/4)],(n＝1,2,
…
,8)
ꢀꢀꢀ
(27)
[0185]
在一些实施例中，当前步骤中设定的搜索方向可以根据上一帧确定的多个位姿标识图案角点中相邻位姿标识图案角点之间的偏差角度确定。示例性地，预定的搜索方向可以根据如下公式计算：
[0186][0187]
其中，(xj,yj)为上一帧(或者上个图像处理周期)确定的多个位姿标识图案角点的二维坐标；n
last
为上一帧确定的多个位姿标识图案角点的个数；v
s1
为第一个设定的搜索方向；v
s2
为第二个设定的搜索方向。
[0188]
在一些实施例中，如图15所示，以初始位姿标识图案角点p
151
的坐标位置作为搜索起点，在设定的搜索方向上搜索第二个位姿标识图案角点p
152
的坐标位置具体可以包括：以初始位姿标识图案角点p
151
的坐标位置作为搜索起点，通过搜索框(例如，图15中的虚线框)以一定的搜索步长在设定的搜索方向v
151
上搜索位姿标识图案角点。若搜索框内存在至少一个候选位姿标识图案角点，则优先选择搜索框内角点似然值最大的候选位姿标识图案角点为第二个位姿标识图案角点p
152
。在搜索框限制在合适的大小情况下，以初始位姿标识图案角点p
151
的坐标位置作为搜索起点进行第二位姿标识图案角点p
152
搜索时，搜索框内出现的候选位姿标识图案角点中角点似然值最大的候选位姿标识图案角点为位姿标识图案角点的可能性较大。因此，可以认为搜索框内角点似然值最大的候选位姿标识图案角点为第二个位姿标识图案角点p
152
，以便提高数据处理速度。其他实施方式中，为了提高位姿标识图案角点识别的准确度，在搜索框内存在至少一个候选位姿标识图案角点的情况下，选择搜索框内出现的候选位姿标识图案角点中角点似然值最大的候选位姿标识图案角点进行角点的识别，以确定该角点似然值最大的候选位姿标识图案角点是否为位姿标识图案角点。例如，对位姿图案匹配模板与该角点似然值最大的候选位姿标识图案角点处一定范围内的图像进行匹配，满足预设位姿图案匹配度标准的候选位姿标识图案角点可以认为是搜索到的第二个位姿标识图案角点p
152
。
[0189]
在一些实施例中，继续参看图15，搜索框的大小可以逐步增大，从而搜索范围逐步增大。搜索步长可与搜索框的边长同步变化。其他实施方式中，搜索框的大小也可以为固定的大小。
[0190]
在一些实施例中，位姿标识图案可以为黑白相间的棋盘格图形，可以采用公式(25)中的相关性系数cc来进行图案匹配。如果cc大于阈值，则认为该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点，记为第二个位姿标识图案角点。
[0191]
参看图14，在步骤1403，基于初始位姿标识、第二个位姿标识，确定搜索方向。在一些实施例中，搜索方向包括：第一搜索方向和第二搜索方向。第一搜索方向可以是以初始位姿标识图案角点的坐标位置为起点，且远离第二位姿标识图案角点的方向。第二搜索方向可以是以第二位姿标识图案角点的坐标位置为起点，且远离第一位姿标识图案角点的方
向。例如，图15中所示的搜索方向v
152
。
[0192]
在步骤1405，以初始位姿标识或第二个位姿标识作为起点，在搜索方向上搜索位姿标识。在一些实施例中，若以第一位姿标识图案角点为新的起点，则可以以上述实施例中的第一搜索方向作为搜索方向进行位姿标识图案角点的搜索。若以第二位姿标识图案角点为新的搜索起点，则可以以上述实施例中的第二搜索方向作为搜索方向进行位姿标识图案角点的搜索。在一些实施例中，搜索新的位姿标识图案角点(例如，图15中的第三位姿标识图案角点p
153
)可以与步骤1401类似地执行。在一些实施例中，搜索步长可以是初始位姿标识图案角点和第二个位姿标识图案角点之间的距离l1。
[0193]
在一些实施例中，响应于搜索到的位姿标识图案角点数量大于或等于位姿标识图案角点数量阈值，停止对位姿标识图案角点的搜索。例如，当搜索到(识别到)四个位姿标识图案角点，停止对位姿标识图案角点的搜索。
[0194]
在一些实施例中，响应于搜索的距离大于第n-1个位姿标识图案角点和第n-2个位姿标识图案角点的距离的设定倍数，停止对第n个位姿标识图案角点的搜索，其中n≥3。例如，搜索的结束条件可以是搜索的距离大于两倍的前两个位姿标识图案角点的距离。这样，搜索第三个位姿标识图案角点的最大搜索距离是初始位姿标识图案角点和第二个位姿标识图案角点的距离的两倍。若达到该搜索距离还未搜索到位姿标识图案角点，则认为未找到第三个位姿标识图案角点并且搜索结束。
[0195]
在一些实施例中，若搜索到的位姿标识图案角点的总数大于或等于设定的阈值(例如，设定的阈值为4)，则认为成功识别到了足够的位姿标识图案角点。若找到的位姿标识图案角点总数小于设定的数值，则认为上述步骤中基于初始位姿标识图案角点的搜索不成功。搜索不成功的情况下，从候选位姿标识图案角点中重新确定新的初始位姿标识图案角点，之后基于该重新确定的初始位姿标识图案角点为搜索起点，搜索其余的位姿标识图案角点。与方法1200类似，可以重新确定新的初始位姿标识图案角点，并且与方法1400类似，可以以新的位姿标识图案角点为搜索起点，搜索其余的位姿标识图案角点。
[0196]
在一些实施例中，在搜索到或者识别到位姿标识图案角点之后，还可以对已确定的位姿标识图案角点进行亚像素定位，以提高位姿标识图案角点的位置精度。
[0197]
在一些实施例中，可以对像素点的cl值基于模型进行拟合，以确定经亚像素定位后的位姿标识图案角点的坐标。例如，roi中每个像素点的cl值的拟合函数可以为二次曲面函数，该函数的极值点为亚像素点。拟合函数可以如下：
[0198]
s(x,y)＝ax2+by2+cx+dy+exy+f
ꢀꢀꢀ
(29)
[0199][0200]
其中，s(x，y)为每个roi中的所有像素点的cl值拟合函数，a、b、c、d、e、f为系数；xc为位姿标识的x坐标，yc为位姿标识的y坐标。
[0201]
图16示出根据本公开一些实施例的识别角度标识的方法1600的流程图。如图16所示，该方法1600中的部分或全部步骤可以由数据处理装置(例如，图1所示的控制装置120，图18所示的处理器1820)来执行。方法1600中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1600可以由机器人系统(例如，图18所示的手术机器人系统1800)执行。在一些实施例中，方法1600可以实现为计算机可读的指令。这些指令可以由
通用处理器或专用处理器(例如，图18所示的处理器1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0202]
参看图16，在步骤1601，基于多个位姿标识在定位图像中的二维坐标和多个位姿标识在位姿标识坐标系中的三维坐标，确定成像变换关系。在一些实施例中，位姿标识坐标系可以是在方法700所示实施例中详述的位姿标识坐标系。例如，位姿标识坐标系为如图6所示。在一些实施例中，成像变换关系可以是指位姿标识坐标系中的三维坐标与定位图像中的二维坐标的变换关系。应该理解，基于成像变换关系，也可以将定位图像中的二维坐标变换为位姿标识坐标系中的三维坐标。在一些实施例中，多个位姿标识在位姿标识坐标系中的三维坐标可以基于公式(21)确定。在一些实施例中，多个位姿标识的数量可以大于或等于4个，例如，可以基于4个位姿标识在定位图像中的二维坐标和在位姿标识坐标系中对应的4个三维坐标，得到成像变换关系。
[0203]
参看图16，在步骤1603，基于成像变换关系、多个位姿标识在位姿标识坐标系中的三维坐标和位置关联关系，在定位图像中确定多个角度标识候选区域。在一些实施例中，角度标识候选区域可以表示角度标识图案的候选区域。在一些实施例中，基于多个位姿标识图案角点在位姿标识坐标系中的三维坐标和位置关联关系，在位姿标识坐标系中确定多个角度标识图案角点候选三维坐标。例如，根据多个位姿标识图案角点在位姿标识坐标系中的三维坐标，沿轴向偏移一定的距离可以确定多个在位姿标识坐标系中的三维坐标。这些三维坐标由多个角度标识图案角点候选三维坐标表示。例如，参看图4，位置关联关系为角度标识和对应的位姿标识沿位姿标识坐标系的z轴间隔一定距离。在确定位姿标识图案角点的位置的前提下，沿z轴的正方向或反方向移动一定距离得到的位置可以认为是角度标识图案角点在位姿标识坐标系下的候选位置。
[0204]
在一些实施例中，基于成像变换关系和多个角度标识图案角点候选三维坐标，在定位图像中确定多个角度标识候选区域。例如，基于成像变换关系和多个角度标识图案角点候选三维坐标，在定位图像中得到多个角度标识图案角点候选二维坐标。在一些实施例中，基于多个角度标识图案角点候选二维坐标，确定多个角度标识图案候选区域。例如，以每个角度标识图案角点候选二维坐标为中心，在定位图像中确定一定范围大小(例如，5
×
5像素、10
×
10像素等)的区域作为角度标识候选区域。在一些实施例中，一定范围大小的区域大于或等于角度标识图案成像后的大小。角度标识图案成像后的大小可以基于角度标识图案的实际大小和成像变换关系得到。
[0205]
参看图16，在步骤1605，从多个角度标识候选区域，识别角度标识。在一些实施例中，角度标识包括角度标识图案和角度标识图案角点。在一些实施例中，方法1600可以包括确定每个角度标识候选区域中角点似然值最大的像素点以形成像素集合。在一些实施例中，像素点的角点似然值可以是在执行方法1200时计算的，也可以是基于公式(24)重新计算得到。方法1600还包括确定像素集合中角点似然值最大的像素点对应的角度标识候选区域作为待识别的角度标识候选区域。方法1600还包括使用多个角度图案匹配模板分别与待识别的角度标识候选区域进行匹配，以识别角度标识。
[0206]
在一些实施例中，角度标识图案为图形特征不同的图案。多个角度图案匹配模板可以是指与多个角度标识图案分别对应的具有相同或相似图形特征的标准角度图案模板。
[0207]
在一些实施例中，通过确定多个角度标识候选区域，可以在多个角度标识候选区
域进行角度标识的识别，避免了在整个图像范围内进行角度标识的识别，提高了数据处理的速度。
[0208]
在一些实施例中，可以使用平方差匹配法、归一化平方差匹配法、相关匹配法、归一化相关匹配法、相关系数匹配法和归一化相关系数匹配法中的任意一种模板匹配算法对角度图案匹配模板与角度标识候选区域进行匹配运算。
[0209]
在一些实施例中，由于角度图案匹配模板与角度标识图案具有相同或相似的图形特征，因此角度标识的图案信息可以包括对应的角度图案匹配模板的图案信息。例如，角度图案匹配模板的形状、图像中可被识别的特征等。在一些实施例中，每个角度图案匹配模板与对应的角度标识图案所标识的绕轴角度具有一一对应关系。基于特定的角度图案匹配模板或者识别出的角度标识对应的角度标识图案的图案信息，确定第一绕轴角度。
[0210]
在一些实施例中，方法1600可以包括响应于匹配失败，确定像素集合的剩余像素点中角点似然值最大的像素点对应的角度标识候选区域作为待识别的角度标识候选区域。在一些实施例中，在确定新的待识别的角度标识候选区域之后，使用多个角度图案匹配模板分别与待识别的角度标识候选区域进行匹配，以识别角度标识。
[0211]
在一些实施例中，基于识别出的角度标识所在的角度标识候选区域，确定与角度标识具有位置关联关系的第一位姿标识。在一些实施例中，多个角度标识候选区域分别对应识别出的多个位姿标识图案角点中的至少一个，在确定识别出的角度标识所在的角度标识候选区域之后，可以基于多个角度标识候选区域与多个位姿标识图案角点的对应关系，确定第一位姿标识图案角点。
[0212]
本公开一些实施例中，本公开还提供了一种计算机设备，计算机设备包括存储器和处理器。存储器可以用于存储有至少一条指令，处理器与存储器耦合，用于执行至少一条指令以执行本公开的方法中的部分步骤或全部步骤，如图7、图8、图9、图10、图12、图14和图16中公开的方法中的部分步骤或全部步骤。
[0213]
图17示出根据本公开一些实施例的计算机设备1700的示意框图。参看图17，该计算机设备1700可以包括中央处理单元(cpu)1701、包括随机存取存储器(ram)1702和只读存储器(rom)1703的系统存储器1704，以及连接各部件的系统总线1705。计算机设备1700还可以包括输入/输出系统，和用于存储操作系统1713、应用程序1714和其他程序模块1715的大容量存储设备1707。输入/输出设备包括主要由显示器1708和输入设备1709组成的输入/输出控制器1710。
[0214]
大容量存储设备1707通过连接到系统总线1705的大容量存储控制器(未示出)连接到中央处理单元1701。大容量存储设备1707或者计算机可读介质为计算机设备提供非易失性存储。大容量存储设备1707可以包括诸如硬盘或者光盘只读存储器(compact disc read-only memory，cd-rom)驱动器之类的计算机可读介质(未示出)。
[0215]
不失一般性，计算机可读介质可以包括计算机存储介质和通信介质。计算机存储介质包括以用于存储诸如计算机可读指令、数据结构、程序模块或其他数据等信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动介质。计算机存储介质包括ram、rom、闪存或其他固态存储其技术，cd-rom、或其他光学存储、磁带盒、磁带、磁盘存储或其他磁性存储设备。当然，本领域技术人员可知计算机存储介质不局限于上述几种。上述的系统存储器和大容量存储设备可以统称为存储器。
[0216]
计算机设备1700可以通过连接在系统总线1705上的网络接口单元1711连接到网络1712。
[0217]
系统存储器1704或大容量存储设备1707还用于存储一个或者一个以上的指令。中央处理器1701通过执行该一个或一个以上指令来实现本公开一些实施例中的方法的全部或者部分步骤。
[0218]
本公开一些实施例中，本公开还提供一种计算机可读存储介质，存储介质中存储有至少一条指令，该至少一条指令由处理器执行以使计算机执行本公开一些实施例的方法中的部分步骤或全部步骤，如图7、图8、图9、图10、图12、图14和图16中公开的方法中的部分步骤或全部步骤。计算机可读存储介质的示例包括计算机程序(指令)的存储器，例如，只读存储器(read-only memory，rom)、随机存取存储器(random access memory，ram)、只读光盘(compact disc read-only memory，cd-rom)、磁带、软盘和光数据存储设备等。
[0219]
图18示出根据本公开一些实施例的手术机器人系统1800的示意图。本公开一些实施例中，参看图18，手术机器人系统1800可以包括：手术器械1850、图像采集器1810以及处理器1820。手术器械1850可以包括操作臂1840、设置在操作臂1840远端的末端1830。末端1830可以包括至少一个角度标识和多个位姿标识以及执行器。图像采集器1810可以用于采集操作臂1840的定位图像。处理器1820与图像采集器1810连接，用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图7、图8、图9、图10、图12、图14和图16中公开的方法中的部分步骤或全部步骤。
[0220]
虽然已经举例说明和描述了本公开的具体实施方案，但是对于本领域技术人员来说显而易见的是，在不脱离本公开实质和范围的情况下可以做出多种其他改变和变型。因此，在随附的权利要求书中包括属于本公开范围内的所有这些改变和变型。