CN116492063A - 基于定位图像的主从运动的控制方法及手术机器人系统 - Google Patents

Abstract

本公开涉及机器人领域，公开一种基于定位图像的主从运动的控制方法，包括：获取定位图像；基于定位图像，确定从动工具相对参考坐标系的当前姿态；基于从动工具的当前姿态，确定主操作器的手柄的目标姿态；以及基于主操作器的手柄的目标姿态，生成用于主操作器的控制信号。

Description

基于定位图像的主从运动的控制方法及手术机器人系统

技术领域

本公开涉及机器人领域，尤其涉及一种基于定位图像的主从运动的控制方法及手术机器人系统。

背景技术

随着科技的发展，通过医疗机器人辅助医疗工作人员进行手术得到了快速的发展，医疗机器人不仅可以帮助医疗工作人员进行一系列的医疗诊断和辅助治疗，还能有效缓解医疗资源的紧张问题。

通常，医疗机器人包括用于执行操作的从动工具及用于控制从动工具运动的主操作器。在实际场景中，从动工具被设置成能够进入操作区域，医疗工作人员通过遥操作主操作器进而控制从动工具在操作区域中的运动，以实现医疗操作。

然而，一般被遥操作的从动工具的数量会大于主操作器的数量，因此在手术中会存在改变主操作器所控制的从动工具的情况。而且，在操作开始时或者操作过程中，主操作器都需要先与从动工具建立映射，然后进行主从控制。由于没有预先将主操作器与对应控制的从动工具进行姿态匹配，会存在主操作器与从动工具之间的姿态(如朝向或者角度)不匹配。如果直接将两者对应匹配进行主从映射，将会降低对从动工具的控制精度，劣化医疗工作人员(例如手术医生)的人机互动体验。因此，在主操作器与从动工具匹配连接后和遥操作前，需要将主操作器的姿态与从动工具的姿态进行对应匹配，以改进主操作器对从动工具的姿态操控精准度。

发明内容

在一些实施例中，本公开提供了一种主从运动的控制方法，包括：获取定位图像；基于定位图像，确定从动工具相对参考坐标系的当前姿态；基于从动工具的当前姿态，确定主操作器的手柄的目标姿态；以及基于主操作器的手柄的目标姿态，生成用于主操作器的控制信号。

在一些实施例中，本公开提供了一种机器人系统，包括：主操作器，包括多自由度机械臂、设置在多自由度机械臂上的手柄以及设置在多自由度机械臂上的至少一个关节处的至少一个电机和至少一个主操作器传感器，至少一个主操作器传感器用于获得至少一个关节的关节信息；从动工具，包括操作臂和设置在操作臂末端的末端器械；图像采集器，用于采集定位图像；以及控制装置，与图像采集器和主操作器通信连接，控制装置被配置成用于执行本公开一些实施例的主从运动的控制方法。

在一些实施例中，本公开提供了一种计算机设备，计算机设备包括：存储器，用于存储至少一条指令；以及处理器，与存储器耦合并且用于执行至少一条指令以执行本公开一些实施例的主从运动的控制方法。

在一些实施例中，本公开提供了一种计算机可读存储介质，用于存储至少一条指令，至少一条指令由计算机执行时致使机器人系统实现本公开一些实施例的主从运动的控制方法。

附图说明

为了更清楚地说明本公开实施例中的技术方案，下面将对本公开实施例描述中所需要使用的附图作简单的介绍。下面描述中的附图仅仅示出本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据本公开实施例的内容和这些附图获得其他的实施例。

图1示出根据本公开一些实施例的机器人系统的示意图；

图2示出根据本公开一些实施例的操作臂的构节示意图；

图3示出根据本公开一些实施例的操作臂的结构示意图；

图4示出根据本公开一些实施例的包括多个位姿标识与多个角度标识的标签的示意图；

图5示出设置在操作臂末端周侧并形成圆筒形的标签的示意图；

图6示出根据本公开一些实施例的实施场景的示意图；

图7示出根据本公开一些实施例的主从运动的控制方法的流程图；

图8示出根据本公开一些实施例的用于确定从动工具坐标系相对参考坐标系的姿态的方法的流程图；

图9示出根据本公开一些实施例的多个位姿标识在截面圆的示意图；

图10示出根据本公开另一些实施例的用于确定从动工具坐标系相对参考坐标系的姿态的方法的流程图；

图11示出根据本公开一些实施例的用于识别位姿标识的方法的流程图；

图12示出根据本公开一些实施例的位姿标识图案的示意图；

图13示出根据本公开一些实施例的用于搜索位姿标识的方法的流程图；

图14示出根据本公开一些实施例的搜索位姿标识的示意图；

图15示出根据本公开一些实施例的识别角度标识的方法的流程图；

图16示出根据本公开一些实施例的主操作器的示意图；

图17示出根据本公开一些实施例的计算机设备的示意框图；

图18示出根据本公开一些实施例的手术机器人系统的示意图；

图19示出根据本公开一些实施例的手术工具的示意图；

图20示出根据本公开一些实施例的主控台车的示意图；

图21示出根据本公开一些实施例的手术台车的示意图。

具体实施方式

为使本公开解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图对本公开实施例的技术方案作进一步的详细描述，显然，所描述的实施例仅仅是本公开示例性实施例，而不是全部的实施例。

在本公开的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本公开和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本公开的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。在本公开的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“耦合”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连；可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本公开中的具体含义。在本公开中，定义靠近操作者(例如医生)的一端为近端、近部或后端、后部，靠近手术患者的一端为远端、远部或前端、前部。本领域技术人员可以理解，本公开的实施例可以用于医疗器械或手术机器人，也可以用于其他非医疗装置。

在本公开中，术语“位置”指物体或物体的一部分在三维空间中的定位(例如，可使用笛卡尔X、Y和Z坐标方面的变化描述三个平移自由度，例如分别沿笛卡尔X轴、Y轴和Z轴的三个平移自由度)。在本公开中，术语“姿态”指物体或物体的一部分的旋转设置(例如三个旋转自由度，可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指物体或物体的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。在本公开中，主操作器的手柄的位姿可由主操作器关节的关节信息的集合(例如由这些关节信息组成的一维矩阵)来表示。在本公开中，关节的关节信息可以包括相应关节相对于相应的关节轴所旋转的角度或者相对于初始位置移动的距离。

在本公开中，参考坐标系可以理解为能够描述物体位姿的坐标系。根据实际的定位需求，参考坐标系可以选择以虚拟参照物的原点或实体参照物的原点为坐标系原点。在一些实施例中，参考坐标系可以为世界坐标系或者相机坐标系或者操作人员自身的感知坐标系等。在本公开中，物体可以理解为需要被定位的对象或目标，例如从动工具的操作臂或者操作臂末端或末端器械。在本公开中，从动工具或其一部分的位姿是指从动工具或其一部分定义的从动工具坐标系相对于参考坐标系的位姿。

图1示出根据本公开一些实施例的机器人系统100的示意图。如图1所示，机器人系统100可以包括：图像采集设备110、控制装置120、至少一个从动工具(150a，150b)和主操作器180。在一些实施例中，图像采集设备110和主操作器180分别与控制装置120通信连接。

在一些实施例中，图像采集设备110可以用于采集定位图像。定位图像中可以包括从动工具(150a，150b)的部分或全部的图像。在一些实施例中，从动工具(150a，150b)上设置有定位标识。在一些实施例中，定位标识包括位姿标识，基于位姿标识可以确定从动工具(150a，150b)的位置或姿态。在一些实施例中，定位标识可以包括位姿标识和角度标识(如下详述)，基于位姿标识和角度标识可以确定从动工具(150a，150b)的位置和姿态。如图1所示，从动工具(150a，150b)处于图像采集设备110的观测视野之内，则采集的定位图像中可以包括从动工具(150a，150b)的部分图像。在一些实施例中，图像采集设备110可以包括但不限于双镜头图像采集设备或单镜头图像采集设备，例如双目或单目相机。根据不同的应用场景，图像采集设备110可以是工业相机、水下相机、微型电子相机、内窥镜相机等。在一些实施例中，图像采集设备110可以是位置固定的或者位置变化的，例如，固定在监控位置的工业相机或者位置或姿态可调节的内窥镜相机。在一些实施例中，图像采集设备110可以实现可见光波段成像、红外波段成像、CT(Computed Tomography，电子计算机断层扫描)成像和声波成像等中的至少一种。在一些实施例中，图像采集设备110例如可以是图21中示出的成像模组2160b。

在一些实施例中，控制装置120被配置成用于执行至少一条指令以执行本公开的方法中的部分步骤或全部步骤，如图7、图8、图10、图11、图13和图15中公开的方法中的部分步骤或全部步骤。在一些实施例中，控制装置120可以接收来自图像采集设备110的定位图像，并且对定位图像进行处理。例如，控制装置120可以在定位图像中，识别位于至少一个从动工具(150a，150b)上的多个定位标识。在一些实施例中，控制装置120可以基于定位图像，确定从动工具(150a，150b)的姿态，例如，基于定位图像，确定从动工具(150a，150b)相对参考坐标系的当前姿态。在一些实施例中，控制装置120还可以基于从动工具(150a，150b)的姿态，确定主操作器180的姿态，例如，基于从动工具(150a，150b)的当前姿态，确定主操作器180的手柄的目标姿态。其中，主操作器180的手柄的目标姿态与从动工具(150a，150b)的当前姿态具有映射关系。在一些实施例中，控制装置120还可以基于主操作器180的手柄的目标姿态，生成主操作器180的控制信号。在一些实施例中，控制装置120可以将主操作器180的控制信号发送至主操作器180的多个关节的驱动电机。

在一些实施例中，主操作器180包括多自由度机械臂(例如，六自由度机械臂)，多自由度机械臂上的部分关节处设置关节传感器，通过关节传感器生成关节信息(如关节角度数据)。在一些实施例中，关节传感器采用电位计和/或编码器。在一些实施例中，主操作器例如可以是图16中示出的主操作器1600。在一些实施例中，主操作器180中可以设置有控制器，控制器可以根据各个关节传感器获得到的关节信息计算主操作器180的姿态数据，并将计算得到的姿态数据发送到控制装置120。在另外一些实施例中，控制装置120也可以根据关节传感器发送的关节信息计算得到主操作器的姿态数据。

在一些实施例中，以从动工具150a为例。如图1所示，从动工具150a包括操作臂140。在一些实施例中，从动工具150a还包括设置在操作臂末端130的末端器械160。在一些实施例中，操作臂140可以是刚性臂或者可形变臂。在一些实施例中，操作臂140可以包括连续体可形变臂。连续体可形变臂例如是如图3所示的操作臂300。在一些实施例中，操作臂140可以包括由多个关节构成的具有多自由度的操作臂。诸如可以实现4至7个自由度运动的操作臂。例如可以实现6个自由度运动的操作臂。在一些实施例中，末端器械160可以包括但不限于手术钳、电刀、电勾等。

图2示出根据本公开一些实施例的操作臂的构节200示意图。在一些实施例中，从动工具的操作臂可以包括至少一个可形变的构节200。如图2所示，可形变的构节200包括固定盘210和多根结构骨220。多根结构骨220的第一端与固定盘210固定连接，第二端与驱动单元(未示出)连接。在一些实施例中，固定盘210可以是包括但不限于环状结构、盘状结构等，而且横截面可以是圆形、矩形、多边形等等各种形状。在一些实施例中，驱动单元通过驱动结构骨220使构节200发生形变。例如，驱动单元通过驱动结构骨220使构节200处于如图2所示的弯曲状态。在一些实施例中，多根结构骨220的第二端穿过基盘230与驱动单元连接。在一些实施例中，与固定盘210类似，基盘230可以是包括但不限于环状结构、盘状结构等，而且横截面可以是圆形、矩形、多边形等等各种形状。驱动单元可以包括线性运动机构、驱动构节或两者的组合。线性运动机构可以与结构骨220连接，以推动或拉动结构骨220，进而驱动构节200弯曲。驱动构节可以包括固定盘和多根结构骨，其中多根结构骨的一端固定连接到固定盘。驱动构节的多根结构骨的另一端与多根结构骨220连接或一体成型，以通过驱动构节的弯曲来驱动构节200的弯曲。在一些实施例中，在固定盘210与基盘230之间还包括间隔盘240，多根结构骨220穿过间隔盘240。类似地，驱动构节也可以包括间隔盘。

图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的基盘连接。如图3所示，每个构节(第一构节3201和第二构节3202)可以包括基座盘、固定盘以及贯穿基座盘和固定盘的多根结构骨，多根结构骨可以与固定盘固定连接，与基座盘滑动连接。连续体可形变臂及其包含的构节可以通过运动学模型描述(如下详述)。

在一些实施例中，操作臂300的每个构节的结构具体可如图2所示的构节200。如图2所示，基座盘坐标系附着在第t(t＝1,2,3…)节连续体段的基座盘上，其原点位于基座盘中心，XY平面与基座盘平面重合，/>从基座盘中心指向第一根结构骨(第一根结构骨可以理解为从多根结构骨中任意指定的一根作为参照的结构骨)。弯曲平面坐标系1/>其原点与基座盘坐标系原点重合，XY平面和弯曲平面重合，/>与/>重合。固定盘坐标系/>附着在第t节连续体段的固定盘上，其原点位于固定盘中心，XY平面与固定盘平面重合，/>从固定盘中心指向第一根结构骨。弯曲平面坐标系2/>其原点位于固定盘中心，XY平面和弯曲平面重合，/>与/>重合。

如图2所示的单个构节200可以通过运动学模型表示。第t节构节末端(固定盘坐标系{te})相对于基座盘坐标系{tb}的位置^tbP_te、姿态^tbR_te可以基于以下公式(1)、(2)确定：

^tbR_te＝^tbR_t1 ^t1R_t2 ^t2R_te (2)

其中，L_t为第t节构节的虚拟结构骨(例如，图2中示出的虚拟结构骨221)的长度，θ_t为在第t节构节中，关于/>或者/>旋转到/>所需的转动角度，^tbR_t1为第t节构节的弯曲平面坐标系1{t1}相对于基座盘坐标系{tb}的姿态，^t1R_t2为第t节构节的弯曲平面坐标系2{t2}相对于弯曲平面坐标系1{t1}的姿态，^t2R_te为第t节构节的固定盘坐标系{te}相对于弯曲平面坐标系2{t2}的姿态。

^tbR_t1、^t1R_t2和^t2R_te可以基于以下公式(3)、公式(4)和公式(5)所示：

其中，δ_t为在第t节构节中，弯曲平面和的夹角。

如图2所示的单个构节200的关节参数Ψ_t可以基于以下公式(6)确定：

ψ_t＝[θ_t，δ_t]^T (6)

在一些实施例中，多根结构骨的驱动量与关节参数具有已知的映射关系。基于构节的目标关节参数和映射关系，可以确定多根结构骨的驱动量。多根结构骨的驱动量可以理解为将单个构节从初始状态(例如，θ_t＝0)弯曲至目标弯曲角度时结构骨受推或受拉的长度。在一些实施例中，多根结构骨的驱动量与关节参数的映射关系可以基于以下公式(7)确定：

q_{i_tool}≡-r_{ti_tool}θ_t cos(δ_t+β_{ti_tool}) (7)

其中，r_{ti_tool}为第t节构节中第i根结构骨到虚拟结构骨的距离，β_{ti_tool}为第t节构节中第i根结构骨和第一根结构骨的夹角，q_{i_tool}为第i根结构骨的驱动量，基于第i根结构骨的驱动量可以确定驱动单元的驱动信号。

在一些实施例中，整个可形变臂可以通过运动学模型来描述。如图3所示，位于可形变臂多个位置处的多个坐标系之间可以进行变换。例如，连续体可形变臂的末端器械在世界坐标系{w}中可基于以下公式(8)确定：

^WT_tip＝^WT_1b ^1bT_1e ^1eT_2b ^2bT_2e ^2eT_tip (8)

其中，^WT_tip表示连续体可形变臂的末端器械相对于世界坐标系的齐次变换矩阵；^WT_1b表示第一连续体段的基座盘相对于世界坐标系的齐次变换矩阵；^1bT_1e表示第一连续体段的固定盘相对于第一连续体段的基座盘的齐次变换矩阵；^1eT_2b表示第二连续体段的基座盘相对于第一连续体段的固定盘的齐次变换矩阵；^2bT_2e表示第二连续体段的固定盘相对于第二连续体段的基座盘的齐次变换矩阵；^2eT_tip表示连续体可形变臂的末端器械相对于第二连续体段的固定盘的齐次变换矩阵。在一些实施例中，末端器械固定设置在固定盘上，因此^2eT_tip是已知的或者预先确定的。

本领域技术人员可以理解，可形变臂在不同的工作状态具有不同的关节参数。例如，图3所示的操作臂300至少包括四种工作状态。操作臂300的四种工作状态如下所述：

第一工作状态：只有第二构节3202参与末端器械的位姿控制(例如，只有第二构节3202进入工作空间)，此时操作臂300的关节参数可以基于以下公式(9)确定：

其中，ψ_c1为第一工作状态下操作臂300的关节参数，为操作臂300的绕轴转角，L₂、θ₂、δ₂与如图2所示构节200中L_t、θ_t和δ_t的物理意义相同。

第二工作状态：第二构节3202和第一直线段3203参与末端器械的位姿控制(例如，第二构节3202全部进入工作空间，第一直线段3203部分进入工作空间)，此时操作臂300的关节参数可以基于以下公式(10)确定：

其中，ψ_c2为第二工作状态下操作臂300的关节参数，L_r为第一直线段3203的进给量。

第三工作状态：第二构节3202、第一直线段3203和第一构节3201参与末端器械的位姿控制(例如，第二构节3202全部进入工作空间，第一直线段3203全部进入工作空间，第一构节3201部分进入工作空间)，此时操作臂300的关节参数可以基于以下公式(11)确定：

其中，ψ_c3为第三工作状态下操作臂300的关节参数，L₁、θ₁和δ₁与如图2所示构节200中L_t、θ_t和δ_t的物理意义相同。

第四工作状态：第二构节3202、第一直线段3203、第一构节3201和第二直线段3204参与末端器械的位姿控制(例如，第二构节3202全部进入工作空间，第一直线段3203全部进入工作空间，第一构节3201全部进入工作空间，第二直线段3204部分进入工作空间)，此时操作臂300的关节参数可以基于以下公式(12)确定：

其中，ψ_c4为第四工作状态下操作臂300的关节参数，L_s为第二直线段3204的进给量。

在一些实施例中，从动工具上设置有多个位姿标识。例如，从动工具的操作臂上分布有多个位姿标识。例如，多个位姿标识沿周向分布在操作臂末端上，或者多个位姿标识沿操作臂的轴向分布。基于多个位姿标识的图像，可以确定从动工具的位姿。在一些实施例中，多个位姿标识可以包括单排或多排的位姿标识。

在一些实施例中，从动工具上设置有多个位姿标识和至少一个角度标识。例如，从动工具的操作臂上分布有多个位姿标识和至少一个角度标识。例如，多个位姿标识沿周向分布在操作臂末端310上，多个角度标识沿周向分布在操作臂末端310上。多个位姿标识与多个角度标识在操作臂末端310上沿轴向并列设置。例如，多个位姿标识与多个角度标识设置在操作臂末端310的柱状部分的外表面上。

在一些实施例中，每个角度标识与位姿标识之一具有位置关联关系。基于该位置关联关系，通过位姿标识的位置，可以确定角度标识可能分布的区域。或者，通过角度标识的位置，可以确定位姿标识可能分布的区域。位置关联关系可以根据位姿标识和角度标识的具体布置方式确定，并且可以是预先设计的。

在一些实施例中，位置关联关系可以包括角度标识与位姿标识在轴向的对应关系。例如，位置关联关系可以包括沿轴向的偏移。基于在轴向的对应关系，在已知一个或多个位姿标识在操作臂末端上的位置的情况下，沿轴向偏移一定的距离可以确定角度标识可能存在的区域。例如，位置关联关系还可以包括沿轴向的斜对齐等等。

在一些实施例中，多个位姿标识与多个角度标识可以设置在标签(例如，图4示出的标签400或图5示出的标签500)上，标签贴附在从动工具上。在一些实施例中，标签贴附在操作臂末端周侧。

在一些实施例中，位姿标识可以包括位姿标识图案和位姿标识图案角点，角度标识可以包括角度标识图案和角度标识图案角点。在一些实施例中，位姿标识图案和角度标识图案可以设置在贴附在操作臂末端上的标签上，或者可以印刷在操作臂末端上，或者可以是由操作臂末端自身的物理构造形成的图案，例如，可以包括凹陷或凸起及其组合。在一些实施例中，位姿标识图案或角度标识图案可以包括以亮度、灰度、色彩等形成的图案。在一些实施例中，位姿标识图案和角度标识图案可以包括主动(例如，自发光)或被动(例如，反射光线)提供被图像采集模块探测的信息的图案。本领域技术人员可以理解，在一些实施例中，位姿标识的位姿可以由位姿标识图案角点坐标系的位姿来表示，角度标识的位姿可以由角度标识图案角点坐标系的位姿来表示。

在一些实施例中，位姿标识图案或角度标识图案设置在操作臂末端上适于被图像采集设备采集图像的区域，例如，在工作过程中可以被图像采集设备的视场覆盖的区域或者在工作过程中不容易被干扰或遮挡的区域。

图4示出根据一些实施例的包括多个位姿标识与多个角度标识的标签400的示意图。图5示出设置在操作臂末端周侧并形成圆筒形的标签500的示意图。可以理解，简单起见，标签400可以与标签500包括相同的位姿标识图案和角度标识图案。

参看图4，多个位姿标识(本公开中以“○”符号表示位姿标识图案角点)和多个角度标识(本公开中以“△”符号表示角度标识图案角点)并排设置。多个位姿标识图案411可以相同或类似，并且多个位姿标识图案角点位于多个位姿标识图案411中。多个角度标识图案421-426可以不同，并且多个角度标识图案角点位于多个角度标识图案421-426中。

每个角度标识与位姿标识之一可以具有位置关联关系。例如，如图4所示，在箭头所示的方向上，部分位姿标识(例如位姿标识图案411)和对应的角度标识(例如，角度标识图案421)沿箭头方向排列并且具有间隔距离d₁。参看图5，在沿周向设置状态下，标签400变为空间构造为圆筒形的标签500，每个角度标识与位姿标识之一的位置关联关系可以包括角度标识与位姿标识在轴向(如图5中Z轴正方向)的对应关系。基于在轴向的对应关系，在已知一个或多个位姿标识在操作臂末端上的位置的情况下，沿轴向偏移一定的距离(如距离d₁)可以确定角度标识可能存在的区域。在一些实施例中，角度标识与位姿标识在轴向的对应关系可以通过角度标识图案角点与位姿标识图案角点在轴向的对应关系表示。在一些实施例中，基于角度标识与位姿标识在轴向的对应关系，角度标识图案角点与位姿标识图案角点之一沿Z轴方向的投影重合。

在一些实施例中，角度标识或位姿标识的绕轴角度或者滚转角可以通过角度标识图案角点或位姿标识图案角点的绕轴角度表示。角度标识图案角点相对于从动工具坐标系(例如建立在操作臂末端的坐标系，如图5所示的XY坐标系)的角度是已知的或预先确定的，例如图5中的角度标识图案角点R₅在XY坐标系下与X轴的角度为θ。基于位置关联关系，可以得到与其位置相关联的位姿标识图案角点P₅相对X轴的夹角为角度θ。应该理解，角度标识图案角点R₅和位姿标识图案角点P₅对应的角度θ可以称为角度标识或位姿标识的绕Z轴的绕轴角度或者滚转角。在本公开中，绕轴角度或者滚转角是指绕Z轴的角度。可以理解，清楚起见，图5中将角度标识图案角点R₅和位姿标识图案角点P₅示为分离，但它们是重合的。

图6示出根据本公开一些实施例的实施场景600的示意图。如图6所示，从动工具包括操作臂和设置在操作臂末端的末端器械660，多个位姿标识和角度标识可以沿周向设置在操作臂末端630上。例如，如图4所示的标签400沿周向设置在操作臂末端630上，形成圆筒形的角度标识图案带610和位姿标识图案带620。多个位姿标识图案角点分布在操作臂末端630的位姿标识图案带620的截面圆621上，多个角度标识图案角点分布在操作臂末端630的角度标识图案带610的截面圆611上。

在一些实施例中，多个角度标识图案为不同的图案。每个角度标识图案用于指示或标识不同的绕轴角度。在一些实施例中，每个角度标识的图案与所标识的绕轴角度具有一一对应关系，基于角度标识的图案可以确定所标识的绕轴角度。

例如，如图6所示，多个不同的角度标识图案(如图4中示出的多个角度标识图案421-426)沿圆柱形结构周向均匀分布，形成角度标识图案角点A-F。设定角度标识图案角点A对应的角度标识图案为参照图案(例如，设定角度标识图案角点A对应的角度标识图案用于标识0°绕轴角度)，设立平面坐标系{wm1}，则可以根据其余角度标识图案与角度标识图案角点A对应的角度标识图案的位置关系确定其余角度标识图案包含的角度标识图案角点标识的绕轴角度。例如，参看图6，当识别出角度标识图案角点B对应的角度标识图案，则根据角度标识图案角点B对应的角度标识图案与角度标识图案角点A对应的角度标识图案的位置关系，可以确定在截面圆611的二维平面坐标系内角度标识图案角点B标识的绕轴角度为60°。截面圆611的二维平面坐标系的原点为截面圆611的圆心，X轴方向为原点指向角度标识图案角点A，Y轴垂直于X轴。

在一些实施例中，末端器械660的位姿可以通过将从动工具坐标系{wm}(例如，操作臂末端坐标系)平移预定距离来确定。或者，末端器械660的位姿可以近似地等于从动工具坐标系{wm}的位姿。

在一些实施例中，基于从动工具坐标系相对参考坐标系的位姿，确定末端器械660相对参考坐标系(例如，参考坐标系为世界坐标系{w})的位姿。具体计算公式如下：

其中，^wR_tip为末端器械相对世界坐标系的姿态，^wP_tip为末端器械相对世界坐标系的位置，^wmR_tip为末端器械相对世界坐标系的姿态，^wmP_tip为末端器械相对世界坐标系的位置，^wR_wm为从动工具坐标系相对世界坐标系的姿态，^wP_wm为从动工具坐标系相对世界坐标系的位置。

本公开的一些实施例提供了一种主从运动的控制方法。图7示出根据本公开一些实施例的主从运动的控制方法700的流程图。方法700可以用于机器人系统，例如图1示出的机器人系统100或者图18示出的手术机器人系统1800。如图7所示，该方法700中的部分或全部步骤可以由机器人系统100的控制装置(例如控制装置120)来执行。控制装置120可以配置在计算设备上。方法700可以由软件、固件和/或硬件来实现。在一些实施例中，方法700可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图7，在步骤701，获取定位图像。在一些实施例中，定位图像中包含从动工具的一部分和从动工具上的多个位姿标识和至少一个角度标识。例如，定位图像中包含操作臂的一部分和操作臂上的多个位姿标识和至少一个角度标识。在一些实施例中，可以从如图1所示的图像采集设备110接收定位图像。例如，控制装置120可以接收图像采集设备110主动发送的定位图像。或者，控制装置120可以向图像采集设备110发送图像请求指令，图像采集设备110响应图像请求指令向控制装置120发送定位图像。

在步骤703，基于定位图像，确定从动工具相对参考坐标系的当前姿态。在一些实施例中，方法700还包括在定位图像中，识别位于从动工具上的多个位姿标识；以及基于多个位姿标识，确定从动工具相对所述参考坐标系的当前姿态。在另一些实施例中，方法700还包括在定位图像中，识别位于从动工具上的多个位姿标识和角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系。在一些实施例中，识别位于从动工具上的多个位姿标识的示例性方法可以包括如图11和图13所示的方法。在一些实施例中，控制装置120可以通过图像处理算法识别定位图像中的部分或全部的位姿标识。在一些实施例中，图像处理算法可以包括特征识别算法，图像处理算法可以提取或识别位姿标识的特征。例如，图像处理算法可以包括角点检测算法，用于检测位姿标识图案角点。该角点检测算法可以是包括但不限于基于灰度图的角点检测、基于二值图像的角点检测、基于轮廓曲线的角点检测中的一种。例如，图像处理算法可以是颜色特征提取算法，用于检测位姿标识图案中的颜色特征。再例如，图像处理算法可以是轮廓检测算法，用于检测位姿标识图案的轮廓特征。在一些实施例中，控制装置可以通过识别模型识别定位图像中的部分或全部的位姿标识。在一些实施例中，方法700还包括基于多个位姿标识，识别位于从动工具上的角度标识。在一些实施例中，在识别出多个位姿标识之后，根据位置关联关系，识别位于从动工具上的角度标识。在一些实施例中，角度标识与第一位姿标识的位置关联关系可以是如图4或图5中示出的位置关联关系。在一些实施例中，第一位姿标识(例如是第一位姿标识图案或第一位姿标识图案角点)是指多个位姿标识中与角度标识具有位置关联关系的位姿标识。在一些实施例中，识别角度标识的示例性方法包括如图15所示的方法。

在一些实施例中，方法700还包括基于角度标识和多个位姿标识，确定从动工具相对参考坐标系的当前姿态。在一些实施例中，确定从动工具相对参考坐标系的姿态的示例性方法包括如图8或图10所示的方法。在一些实施例中，可以基于角度标识、第一位姿标识和多个位姿标识，确定从动工具相对参考坐标系的姿态。在一些实施例中，从动工具的当前姿态是从动工具相对于从动工具的基坐标系的当前姿态。从动工具包括操作臂和设置在操作臂末端的末端器械，从动工具的当前姿态包括末端器械相对于从动工具的基坐标系的姿态或者操作臂末端相对于从动工具的基坐标系的姿态。在一些实施例中，从动工具的基坐标系可以是从动工具所安装的基座(例如，手术机器人的运动臂末端)的坐标系、从动工具所穿过的鞘管的坐标系(例如，鞘管出口的坐标系)、从动工具的远端运动中心点(RemoteCenter of Motion，RCM)的坐标系等。例如，从动工具的基坐标系可以设置在鞘管出口位置处，且在遥操作过程中，从动工具的基坐标系固定不变。可以对末端器械的当前姿态进行坐标系变换，得到相对于其他坐标系的姿态。在一些实施例中，从动工具的当前姿态是从动工具在显示器中的图像相对于世界坐标系的当前姿态。在一些实施例中，世界坐标系可以是操作者或主操作器所在空间的坐标系。因此，从动工具在显示器中的图像相对于世界坐标系的姿态是操作者所感知的姿态。从动工具包括手术工具和视觉工具。在手术过程中，手术工具在患者体内执行手术，视觉工具使用相机采集患者体内图像，并将采集到的图像传输至手术台车。图像经手术台车中的视频处理模块处理后，显示在主控台车的显示器上。操作者通过显示器中的图像获得从动工具的当前姿态。在一些实施例中，从动工具在显示器中的图像相对于世界坐标系的当前姿态可以通过坐标变换得到。例如，基于从动工具的基坐标系、视觉工具的相机的坐标系、视觉工具的基坐标系、显示器的坐标系及世界坐标系，可以获得从动工具在显示器中的图像相对于世界坐标系的当前姿态。

继续参看图7，在步骤705，基于从动工具的当前姿态，确定主操作器的手柄的目标姿态。在一些实施例中，从动工具的当前姿态是相对于从动工具的基坐标系的当前姿态，或者从动工具的当前姿态是从动工具在显示器中的图像相对于世界坐标系的当前姿态。主操作器的手柄的目标姿态是相对于主操作器的基坐标系的姿态。主操作器的基坐标系可以是主操作器所连接的基座的坐标系。在一些实施例中，主操作器的基坐标系与从动工具的基坐标系具有确定的变换关系。

在一些实施例中，从动工具的当前姿态与手柄的目标姿态相匹配，例如相同、成比例或具有固定差值。例如，在遥操作之前，保持从动工具的当前姿态不变并将从动工具的当前姿态作为手柄的目标姿态，将手柄的当前姿态调整至目标姿态，实现手柄与从动工具的姿态匹配。

继续参看图7，在步骤707，基于主操作器的手柄的目标姿态，生成主操作器的控制信号。在一些实施例中，方法700还包括：确定主操作器的手柄的当前姿态；以及基于主操作器的手柄的目标姿态和手柄的当前姿态，生成主操作器的控制信号。主操作器的手柄的当前姿态是主操作器的手柄相对于主操作器的基坐标系的姿态。在一些实施例中，基于手柄的当前姿态以及目标姿态确定手柄从当前姿态达到目标姿态所对应的控制信号。

在一些实施例中，方法700还包括：基于角度标识和多个位姿标识，确定从动工具坐标系和位姿标识坐标系的变换关系。在一些实施例中，根据从动工具坐标系和位姿标识坐标系的变换关系，可以将位姿标识坐标系中的三维坐标转换为在从动工具坐标系中对应的三维坐标。在一些实施例中，根据从动工具坐标系和位姿标识坐标系的变换关系和位姿标识坐标系相对参考坐标系的位姿，得到从动工具坐标系相对参考坐标系的姿态。

在一些实施例中，从动工具坐标系和位姿标识坐标系的变换关系可以包括位姿标识坐标系相对从动工具坐标系的滚转角。在一些实施例中，可以基于角度标识和第一位姿标识，确定位姿标识坐标系相对从动工具坐标系的滚转角。应该理解，位姿标识坐标系相对从动工具坐标系的滚转角可以是位姿标识坐标系绕从动工具坐标系的Z轴的转动的角度。

在一些实施例中，从动工具坐标系可以是基于多个位姿标识或多个角度标识，在从动工具的操作臂上设定的固定坐标系。在一些实施例中，从动工具坐标系的Z轴平行于操作臂的轴向，从动工具坐标系的XY平面与多个位姿标识图案角点处于同一平面，或者与多个角度标识图案角点处于同一平面。

在一些实施例中，可以确定位姿标识坐标系，以方便确定多个位姿标识的位置。在一些实施例中，位姿标识的位置可以通过位姿标识图案角点的位置表示。在一些实施例中，位姿标识坐标系的Z轴平行于操作臂的轴向或与轴向重合，位姿标识坐标系的XY平面与多个位姿标识图案角点处于同一平面。

示例性地，参看图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}的滚转角α₀确定。滚转角α₀可以是指位姿标识坐标系{wm0}相对从动工具坐标系{wm}绕Z轴的转动角度。

在一些实施例中，参看图6，滚转角α₀通过如下公式计算：

α₀＝α₁-α₂ (14)

其中α₁为第一绕轴角度，α₂为第二绕轴角度。第一绕轴角度为角度标识图案角点(例如，角度标识图案角点R₆)在从动工具坐标系中标识的绕轴角度。第二绕轴角度为第一位姿标识图案角点(例如，位姿标识图案角点P₆)在位姿标识坐标系中标识的绕轴角度。

图8示出根据本公开一些实施例的用于确定从动工具坐标系相对参考坐标系的姿态的方法800的流程图。如图3所示的操作臂300，从动工具坐标系可以包括在操作臂末端建立的坐标系。如图8所示，该方法800中的部分或全部步骤可以由控制装置(例如，图1所示的控制装置120)来执行。方法800中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法800可以用于机器人系统，例如，图1示出的机器人系统100或者图18示出的手术机器人系统1800。在一些实施例中，方法800可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图8，在步骤801，基于角度标识和多个位姿标识，确定位姿标识坐标系相对从动工具坐标系的滚转角。在一些实施例中，确定角度标识在从动工具坐标系中标识的第一绕轴角度。确定第一位姿标识在位姿标识坐标系中标识的第二绕轴角度。基于第一绕轴角度和第二绕轴角度，确定位姿标识坐标系相对从动工具坐标系的滚转角。在一些实施例中，位姿标识坐标系相对从动工具坐标系的滚转角可以基于公式(14)确定。

在步骤803，基于多个位姿标识，确定位姿标识坐标系相对参考坐标系的姿态。位姿标识在相应坐标系中的坐标可以通过位姿标识图案角点在相应坐标系中的坐标表示。例如，位姿标识的在定位图像中的二维坐标和在位姿标识坐标系中的三维坐标可以通过位姿标识图案角点的坐标表示。在一些实施例中，基于多个位姿标识图案角点在定位图像中的二维坐标和多个位姿标识图案角点在位姿标识坐标系中的三维坐标，确定位姿标识坐标系相对参考坐标系的姿态。在一些实施例中，基于多个位姿标识图案角点在定位图像中的二维坐标、多个位姿标识图案角点在位姿标识坐标系中的三维坐标和相机坐标系相对参考坐标系的变换关系，确定位姿标识坐标系相对参考坐标系的姿态。

在一些实施例中，基于多个位姿标识的分布，确定多个位姿标识图案角点在位姿标识坐标系中的三维坐标。例如，参看图9，每个位姿标识图案角点均位于在截面圆922的圆周上，截面圆922的圆心与半径r均是已知的。设定截面圆922的圆心为位姿标识坐标系的原点，XY平面位于截面圆922上，X轴可以指定为由原点出发指向任一已确定的位姿标识图案角点(例如，位姿标识图案角点P₉)，进而可基于多个位姿标识的分布，确定每个位姿标识图案角点在位姿标识坐标系中的三维坐标。例如，如图9所示，位姿标识图案角点P₉在位姿标识坐标系的三维坐标为(r,0,0)，则其余位姿标识图案角点在位姿标识坐标系中的三维坐标可以根据以下公式计算：

C_m＝[r·cos((m-1)·χ)r·sin((m-1)·χ)0]^T (15)

其中，C_m为以位姿标识图案角点P₉作为起点，第m个位姿标识图案角点在位姿标识坐标系中的三维坐标；χ为相邻的位姿标识图案角点之间的绕轴夹角。

在一些实施例中，相机坐标系相对参考坐标系的变换关系可以是已知的。例如，参考坐标系为世界坐标系，相机坐标系相对世界坐标系的变换关系可以根据相机摆放的位姿确定。另一些实施例中，根据实际的需求，参考坐标系也可以是相机坐标系本身。

在一些实施例中，基于相机成像原理与投影模型，基于多个位姿标识图案角点在定位图像中的二维坐标和多个位姿标识图案角点在位姿标识坐标系中的三维坐标，确定位姿标识坐标系相对相机坐标系的位姿。基于位姿标识坐标系相对相机坐标系的位姿和相机坐标系相对参考坐标系的变换关系，可以得到位姿标识坐标系相对参考坐标系的位姿。在一些实施例中，还可以考虑相机的内参。例如，相机的内参可以是如图1所示的图像采集设备110或者图21中示出的成像模组2160b的相机内参。相机的内参可以是已知的或者经过标定而得到的。

在一些实施例中，相机坐标系可以理解为以相机原点建立的坐标系。例如，以相机的光心为原点建立的坐标系或者以相机的镜头中心为原点建立的坐标系。当相机为双目相机时，相机坐标系的原点可以是相机左镜头的中心，或者右镜头的中心，或者左右镜头中心连线上的任意一点(例如该连线的中点)。

参看图8，在步骤805，基于位姿标识坐标系相对从动工具坐标系的滚转角和位姿标识坐标系相对参考坐标系的姿态，确定从动工具坐标系相对参考坐标系的姿态。在一些实施例中，从动工具坐标系相对参考坐标系的姿态可以作为从动工具相对参考坐标系的当前姿态。

本领域技术人员可以理解，本公开一些实施例还可以基于位姿标识坐标系相对从动工具坐标系的滚转角和位姿标识坐标系相对参考坐标系的位姿，确定从动工具坐标系相对参考坐标系的位姿。例如，以参考坐标系为世界坐标系为例，从动工具坐标系相对世界坐标系的位姿具体如下：

^wR_wm＝^wR_wm0·rot_z(α₀)

^wP_wm＝^wP_wm0 (16)

其中，^wR_wm为从动工具坐标系相对世界坐标系的姿态，^wP_wm为从动工具坐标系相对世界坐标系的位置，^wR_wm0为位姿坐标系相对世界坐标系的姿态，^wP_wm0为位姿坐标系相对世界坐标系的位置，rot_z(α₀)表示绕从动工具坐标系的Z轴转动滚转角α₀。

在一些实施例中，从动工具坐标系相对世界坐标系的位姿具体计算公式如下：

^wR_wm＝^wR_lens ^lensR_wm0 ^wm0R_wm

^wP_wm＝^wR_lens(^lensR_wm0 ^wm0P_wm+^lensP_wm0)+^wP_lens (17)

其中，^wR_lens为相机坐标系相对世界坐标系的姿态，^wP_lens为相机坐标系相对世界坐标系的位置，^lensR_wm0为位姿标识坐标系相对相机坐标系的姿态，^lensP_wm0为位姿标识坐标系相对相机坐标系的位置，^wm0R_wm为从动工具坐标系相对位姿标识坐标系的姿态，^wm0P_wm为从动工具坐标系相对位姿标识坐标系的位置。

图10示出根据本公开另一些实施例的用于确定从动工具坐标系相对参考坐标系的姿态的方法1000的流程图。方法1000可以是图8的方法800的替换实施例。如图10所示，该方法1000中的部分或全部步骤可以由控制装置(例如，图1所示的控制装置120)来执行。方法1000中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1000可以用于机器人系统，例如，图1示出的机器人系统100或者图18示出的手术机器人系统1800。在一些实施例中，方法1000可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图10，在步骤1001，基于位姿标识坐标系相对从动工具坐标系的滚转角和多个位姿标识在位姿标识坐标系中的三维坐标，确定多个位姿标识在从动工具坐标系中的三维坐标。可以理解，已知位姿标识坐标系相对从动工具坐标系的滚转角，可以根据坐标变换将多个位姿标识图案角点在位姿标识坐标系中的三维坐标变换为在从动工具坐标系中的三维坐标。

在步骤1003，基于多个位姿标识在定位图像中的二维坐标和多个位姿标识在从动工具坐标系中的三维坐标，确定从动工具坐标系相对参考坐标系的姿态。在一些实施例中，步骤1003可以与方法800中步骤803和805类似地实现。

图11示出根据本公开一些实施例的用于识别位姿标识的方法1100的流程图。如图12所示，该方法1100中的部分或全部步骤可以由控制装置(例如，图1所示的控制装置120)来执行。方法1100中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1100可以用于机器人系统，如，图1示出的机器人系统100或者图18示出的手术机器人系统1800。在一些实施例中，方法1100可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图11，在步骤1101，从定位图像中确定多个候选位姿标识。在一些实施例中，候选位姿标识可以通过候选位姿标识图案角点表示。在一些实施例中，候选位姿标识图案角点可以是指经过对定位图像进行初步处理或者初步识别得到的可能的位姿标识图案角点。在一些实施例中，可以先从定位图像中截取ROI(Region of Interest，感兴趣区域)，从ROI中确定多个候选位姿标识。其中，ROI可以是定位图像的全图像，也可以是部分区域。例如，可以基于上一帧图像(例如，上一图像处理周期的定位图像)确定的多个位姿标识图案角点一定范围内的区域截取当前帧的ROI。对非第一帧的定位图像，ROI可以为上一图像处理周期的多个位姿标识图案角点的坐标构成的虚点为中心的一定距离范围内的区域。一定距离范围可以是位姿标识图案角点平均间隔距离的固定倍数，例如两倍。应理解，预定倍数还可以是上一图像处理周期中多个候选位姿标识图案角点平均间隔距离的可变倍数。

在一些实施例中，方法1100可以包括：确定定位图像中各像素点的角点似然值(Corner Likelihood,CL)。在一些实施例中，像素点的角点似然值可以是表征像素点作为特征点(例如，角点)的可能性的数值。在一些实施例中，在计算各像素点的角点似然值之前可以对定位图像进行预处理，之后确定预处理之后的图像中各像素点的角点似然值。图像的预处理例如可以包括：图像灰度化、图像去噪、图像增强中的至少一种。例如，图像预处理可以包括：从定位图像中截取ROI，将ROI转为相应的灰度图像。

在一些实施例中，确定ROI中的每个像素点的角点似然值的方式例如可以包括对ROI范围内每一个像素点进行卷积操作，得到每个像素点的一阶和/或二阶导数。利用ROI范围内每个像素点的一阶和/或二阶导数求出每个像素点的角点似然值。示例性地，各像素点的角点似然值可以根据如下公式计算：

CL＝max(c_xy,c₄₅)

c_xy＝τ²·|I_xy|-1.5·τ·(|I₄₅|+|I_n45|)

c₄₅＝τ²·|I_{45_45}|-1.5·τ·(|I_x|+|I_y|) (18)

其中，τ为设定的常数，例如设定为2；I_x、I₄₅、I_y、I_n45分别是像素点在0、π/4、π/2、-π/4四个方向的一阶导数；I_xy和I_{45_45}分别是像素点在0,π/2和π/4,-π/4方向的二阶导数。

在一些实施例中，将ROI划分成多个子图像。例如，可以采用非极大抑制法在一个ROI范围中平均分割出多个子图像。在一些实施例中，可以将ROI平均分割成5×5像素的多个子图像。上述实施例为示例性的，并非限制性的，应当理解，还可以将定位图像或ROI分割成其他尺寸大小的多个子图像，例如，分割成9×9像素的多个子图像。可以确定每个子图像中的CL值最大的像素点，将每个子图像中的CL值最大的像素点与第一阈值进行比较，确定CL值大于第一阈值的像素点集合。在一些实施例中，第一阈值可以设定为0.06。应当理解，第一阈值还可以设定为其他值。在一些实施例中，CL值大于第一阈值的像素点可以作为候选位姿标识图案角点。

参看图11，在步骤1103，基于位姿图案匹配模板，从多个候选位姿标识中识别初始位姿标识。在一些实施例中，通过位姿图案匹配模板与候选位姿标识图案角点之一处的图像进行匹配，确定达到预设位姿图案匹配度标准的候选位姿标识图案角点为初始位姿标识图案角点。

在一些实施例中，位姿图案匹配模板与位姿标识图案角点附近区域的图像具有相同或相似的特征。若位姿图案匹配模板与候选位姿标识图案角点附近区域的图像的匹配度达到预设位姿图案匹配度标准(例如，匹配度高于阈值)，则可以认为候选位姿标识图案角点附近区域的图案与位姿图案匹配模板具有相同或相似的特征，进而可认为当前的候选位姿标识图案角点为位姿标识图案角点。

在一些实施例中，确定像素点集合中CL值最大的像素点，作为待匹配候选位姿标识图案角点。例如，可以将该像素点集合中的所有像素点让CL值从大到小的顺序排序，并将CL值最大的像素点作为待匹配候选位姿标识图案角点。在确定了待匹配候选位姿标识图案角点之后，使用位姿图案匹配模板与待匹配候选位姿标识图案角点处的图案进行匹配，如果达到了预设位姿图案匹配度标准，则确定待匹配候选位姿标识图案角点为识别出的初始位姿标识图案角点。如果待匹配候选位姿标识图案角点未达到预设的匹配度标准，则选择次级CL值的像素点(CL值第二大的像素点)作为待匹配候选位姿标识图案角点，使用位姿图案匹配模板与该候选位姿标识图案角点处的图像进行匹配，依次类推，直至识别出初始位姿标识图案角点。

在一些实施例中，位姿标识图案可以为黑白相间的棋盘格图形，因此位姿图案匹配模板可以为相同的棋盘格图形，利用位姿图案匹配模板的灰度分布G_M与候选位姿标识图案角点对应的像素点的像素邻域灰度分布G_image间的相关性系数(CorrelationCoefficient,CC)来进行匹配。像素点的像素邻域灰度分布G_image为以该像素点为中心一定范围内(例如，10×10像素)像的像素的灰度分布。具体公式如下：

其中，Var为方差函数，Cov为协方差函数。在一些实施例中，当CC值小于0.8时，像素领域内的灰度分布与位姿图案匹配模板相关性较低，则判定该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点，否则认为该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点。

在一些实施例中，方法1100包括：确定候选位姿标识图案角点的边缘方向。例如，如图12所示，图12中包括一个位姿标识图案1201，候选位姿标识图案角点为图12中的角点P₁₂，那么该角点P₁₂的边缘方向可以是指形成角点P₁₂的边缘的方向，如图12中虚线箭头示意的方向。

在一些实施例中，边缘方向可以通过对以候选位姿标识图案角点为中心的一定范围邻域(例如10×10像素)的每个像素在平面坐标系的X方向和Y方向的一阶导数值(I_x和I_y)确定。例如，边缘方向可以通过以下公式进行计算：

I_angle＝arctan(I_y/I_x),

其中，一阶导数(I_x和I_y)可以通过对一定范围邻域范围内每一个像素点进行卷积操作得到。一些实施例中，通过对每个范围邻域内的像素点的边缘方向I_angle和对应的权重I_weight进行聚类计算获得该像素点的边缘方向，选择权重I_weight占比最大的类对应的I_angle作为边缘方向。需要说明的是，如果存在多个边缘方向，则选择权重I_weight占比最大的多个类对应的I_angle作为边缘方向。

在一些实施例中，聚类计算所用的方法可以是K均值方法、BIRCH(BalancedIterative Reducing and Clustering using Hierarchies，基于层次结构的平衡迭代聚类方法)方法、DBSCAN(Density-Based Spatial Clustering of Applications withNoise，具有噪声的基于密度的聚类方法)方法、GMM(Gaussian Mixed Model，高斯混合模型)方法中的任意一种。

在一些实施例中，方法1100包括：根据边缘方向旋转位姿图案匹配模板。根据边缘方向旋转位姿图案匹配模板，可以将位姿图案匹配模板与候选位姿标识图案角点处的图像对齐。

候选位姿标识图案角点的边缘方向可以用于确定该候选标识图案角点处的图像在定位图像中的设置方向。在一些实施例中，根据边缘方向旋转位姿图案匹配模板，可以将位姿图案匹配模板调整至与候选位姿标识图案角点处的图像方向相同或接近相同以便于进行图像匹配。

参看图11，在步骤1105，以初始位姿标识作为起点，搜索位姿标识。例如，图13示出根据本公开一些实施例的用于搜索位姿标识的方法1300的流程图。如图13所示，该方法1300中的部分或全部步骤可以由控制装置(例如，图1所示的控制装置120)来执行。方法1300中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1300可以用于机器人系统，例如，图1示出的机器人系统100或者图18示出的手术机器人系统1800。在一些实施例中，方法1300可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图13，在步骤1301，以初始位姿标识作为起点，确定第二个位姿标识。在一些实施例中，以初始位姿标识图案角点作为起点，在设定的搜索方向上搜索第二个位姿标识图案角点。在一些实施例中，设定的搜索方向可以包括：初始位姿标识图案角点的正前方(对应0°角度方向)、正后方(对应180°角度方向)、正上方(90°角度方向)、正下方(-90°角度方向)和斜向(例如±45°角度方向)中的至少一个方向。

在一些实施例中，设定的搜索方向为n个，例如在8个方向进行搜索，每个搜索方向v_sn可以根据如下公式计算：

v_sn＝[cos(n·π/4)sin(n·π/4)],(n＝1,2,…,8) (21)

在一些实施例中，当前步骤中设定的搜索方向可以根据上一帧确定的多个位姿标识图案角点中相邻位姿标识图案角点之间的偏差角度确定。示例性地，预定的搜索方向可以根据如下公式计算：

其中，(x_j,y_j)为上一帧(或者上个图像处理周期)确定的多个位姿标识图案角点的二维坐标；n_last为上一帧确定的多个位姿标识图案角点的个数；v_s1为第一个设定的搜索方向；v_s2为第二个设定的搜索方向。

在一些实施例中，如图14所示，以初始位姿标识图案角点P₁₄₀₁的坐标位置作为搜索起点，在设定的搜索方向上搜索第二个位姿标识图案角点P₁₄₀₂的坐标位置具体可以包括：以初始位姿标识图案角点P₁₄₀₁的坐标位置作为搜索起点，通过搜索框(例如，图14中的虚线框)以一定的搜索步长在设定的搜索方向V₁₄₀₁上搜索位姿标识图案角点。若搜索框内存在至少一个候选位姿标识图案角点，则优先选择搜索框内角点似然值最大的候选位姿标识图案角点为第二个位姿标识图案角点P₁₄₀₂。在搜索框限制在合适的大小情况下，以初始位姿标识图案角点P₁₄₀₁的坐标位置作为搜索起点进行第二位姿标识图案角点P₁₄₀₂搜索时，搜索框内出现的候选位姿标识图案角点中角点似然值最大的候选位姿标识图案角点为位姿标识图案角点的可能性较大。因此，可以认为搜索框内角点似然值最大的候选位姿标识图案角点为第二个位姿标识图案角点P₁₄₀₂，以便提高数据处理速度。其他实施方式中，为了提高位姿标识图案角点识别的准确度，在搜索框内存在至少一个候选位姿标识图案角点的情况下，选择搜索框内出现的候选位姿标识图案角点中角点似然值最大的候选位姿标识图案角点进行角点的识别，以确定该角点似然值最大的候选位姿标识图案角点是否为位姿标识图案角点。例如，对位姿图案匹配模板与该角点似然值最大的候选位姿标识图案角点处一定范围内的图像进行匹配，满足预设位姿图案匹配度标准的候选位姿标识图案角点可以认为是搜索到的第二个位姿标识图案角点P₁₄₀₂。

在一些实施例中，继续参看图14，搜索框的大小可以逐步增大，从而搜索范围逐步增大。搜索步长可与搜索框的边长同步变化。其他实施方式中，搜索框的大小也可以为固定的大小。

在一些实施例中，位姿标识图案可以为黑白相间的棋盘格图形，可以采用公式(19)中的相关性系数CC来进行图案匹配。如果CC大于阈值，则认为该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点，记为第二个位姿标识图案角点。

参看图13，在步骤1303，基于初始位姿标识、第二个位姿标识，确定搜索方向。在一些实施例中，搜索方向包括：第一搜索方向和第二搜索方向。第一搜索方向可以是以初始位姿标识图案角点的坐标位置为起点，且远离第二位姿标识图案角点的方向。第二搜索方向可以是以第二位姿标识图案角点的坐标位置为起点，且远离第一位姿标识图案角点的方向。例如，图14中所示的搜索方向V₁₄₀₂。

在步骤1305，以初始位姿标识或第二个位姿标识作为起点，在搜索方向上搜索位姿标识。在一些实施例中，若以第一位姿标识图案角点为新的起点，则可以以上述实施例中的第一搜索方向作为搜索方向进行位姿标识图案角点的搜索。若以第二位姿标识图案角点为新的搜索起点，则可以以上述实施例中的第二搜索方向作为搜索方向进行位姿标识图案角点的搜索。在一些实施例中，搜索新的位姿标识图案角点(例如，图14中的第三位姿标识图案角点P₁₄₀₃)可以与步骤1301类似地执行。在一些实施例中，搜索步长可以是初始位姿标识图案角点和第二个位姿标识图案角点之间的距离L₁。

在一些实施例中，响应于搜索到的位姿标识图案角点数量大于或等于位姿标识图案角点数量阈值，停止对位姿标识图案角点的搜索。例如，当搜索到(识别到)四个位姿标识图案角点，停止对位姿标识图案角点的搜索。

在一些实施例中，响应于搜索的距离大于第N-1个位姿标识图案角点和第N-2个位姿标识图案角点的距离的设定倍数，停止对第N个位姿标识图案角点的搜索，其中N≥3。例如，搜索的结束条件可以是搜索的距离大于两倍的前两个位姿标识图案角点的距离。这样，搜索第三个位姿标识图案角点的最大搜索距离是初始位姿标识图案角点和第二个位姿标识图案角点的距离的两倍。若达到该搜索距离还未搜索到位姿标识图案角点，则认为未找到第三个位姿标识图案角点并且搜索结束。

在一些实施例中，若搜索到的位姿标识图案角点的总数大于或等于设定的阈值(例如，设定的阈值为4)，则认为成功识别到了足够的位姿标识图案角点。若找到的位姿标识图案角点总数小于设定的数值，则认为上述步骤中基于初始位姿标识图案角点的搜索不成功。搜索不成功的情况下，从候选位姿标识图案角点中重新确定新的初始位姿标识图案角点，之后基于该重新确定的初始位姿标识图案角点为搜索起点，搜索其余的位姿标识图案角点。与方法1100类似，可以重新确定新的初始位姿标识图案角点，并且与方法1300类似，可以以新的位姿标识图案角点为搜索起点，搜索其余的位姿标识图案角点。

在一些实施例中，在搜索到或者识别到位姿标识图案角点之后，还可以对已确定的位姿标识图案角点进行亚像素定位，以提高位姿标识图案角点的位置精度。

在一些实施例中，可以对像素点的CL值基于模型进行拟合，以确定经亚像素定位后的位姿标识图案角点的坐标。例如，ROI中每个像素点的CL值的拟合函数可以为二次曲面函数，该函数的极值点为亚像素点。拟合函数可以如下：

S(x,y)＝ax²+by²+cx+dy+exy+f (23)

其中，S(x,y)为每个ROI中的所有像素点的CL值拟合函数，a、b、c、d、e、f为系数；x_c为位姿标识的x坐标，y_c为位姿标识的y坐标。

图15示出根据本公开一些实施例的识别角度标识的方法1500的流程图。如图15所示，该方法1500中的部分或全部步骤可以由控制装置(例如，图1所示的控制装置120)来执行。方法1500中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1500可以用于机器人系统，例如，图1示出的机器人系统100或者图18示出的手术机器人系统1800。在一些实施例中，方法1500可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图15，在步骤1501，基于多个位姿标识在定位图像中的二维坐标和多个位姿标识在位姿标识坐标系中的三维坐标，确定成像变换关系。在一些实施例中，位姿标识坐标系可以是在方法700所示实施例中详述的位姿标识坐标系。例如，位姿标识坐标系为如图6所示。在一些实施例中，成像变换关系可以是指位姿标识坐标系中的三维坐标与定位图像中的二维坐标的变换关系。应该理解，基于成像变换关系，也可以将定位图像中的二维坐标变换为位姿标识坐标系中的三维坐标。在一些实施例中，多个位姿标识在位姿标识坐标系中的三维坐标可以基于公式(15)确定。在一些实施例中，多个位姿标识的数量可以大于或等于4个，例如，可以基于4个位姿标识在定位图像中的二维坐标和在位姿标识坐标系中对应的4个三维坐标，得到成像变换关系。

参看图15，在步骤1503，基于成像变换关系、多个位姿标识在位姿标识坐标系中的三维坐标和位置关联关系，在定位图像中确定多个角度标识候选区域。在一些实施例中，角度标识候选区域可以表示角度标识图案的候选区域。在一些实施例中，基于多个位姿标识图案角点在位姿标识坐标系中的三维坐标和位置关联关系，在位姿标识坐标系中确定多个角度标识图案角点候选三维坐标。例如，根据多个位姿标识图案角点在位姿标识坐标系中的三维坐标，沿轴向偏移一定的距离可以确定多个在位姿标识坐标系中的三维坐标。这些三维坐标由多个角度标识图案角点候选三维坐标表示。例如，参看图4，位置关联关系为角度标识和对应的位姿标识沿位姿标识坐标系的Z轴间隔一定距离。在确定位姿标识图案角点的位置的前提下，沿Z轴的正方向或反方向移动一定距离得到的位置可以认为是角度标识图案角点在位姿标识坐标系下的候选位置。

在一些实施例中，基于成像变换关系和多个角度标识图案角点候选三维坐标，在定位图像中确定多个角度标识候选区域。例如，基于成像变换关系和多个角度标识图案角点候选三维坐标，在定位图像中得到多个角度标识图案角点候选二维坐标。在一些实施例中，基于多个角度标识图案角点候选二维坐标，确定多个角度标识图案候选区域。例如，以每个角度标识图案角点候选二维坐标为中心，在定位图像中确定一定范围大小(例如，5×5像素、10×10像素等)的区域作为角度标识候选区域。在一些实施例中，一定范围大小的区域大于或等于角度标识图案成像后的大小。角度标识图案成像后的大小可以基于角度标识图案的实际大小和成像变换关系得到。

参看图15，在步骤1505，从多个角度标识候选区域，识别角度标识。在一些实施例中，角度标识包括角度标识图案和角度标识图案角点。在一些实施例中，方法1500可以包括确定每个角度标识候选区域中角点似然值最大的像素点以形成像素集合。在一些实施例中，像素点的角点似然值可以是在执行方法1100时计算的，也可以是基于公式(18)重新计算得到。方法1500还包括确定像素集合中角点似然值最大的像素点对应的角度标识候选区域作为待识别的角度标识候选区域。方法1500还包括使用多个角度图案匹配模板分别与待识别的角度标识候选区域进行匹配，以识别角度标识。在一些实施例中，角度标识图案为图形特征不同的图案。多个角度图案匹配模板可以是指与多个角度标识图案分别对应的具有相同或相似图形特征的标准角度图案模板。在一些实施例中，通过确定多个角度标识候选区域，可以在多个角度标识候选区域进行角度标识的识别，避免了在整个图像范围内进行角度标识的识别，提高了数据处理的速度。

在一些实施例中，可以使用平方差匹配法、归一化平方差匹配法、相关匹配法、归一化相关匹配法、相关系数匹配法和归一化相关系数匹配法中的任意一种模板匹配算法对角度图案匹配模板与角度标识候选区域进行匹配运算。

在一些实施例中，由于角度图案匹配模板与角度标识图案具有相同或相似的图形特征，因此角度标识的图案信息可以包括对应的角度图案匹配模板的图案信息。例如，角度图案匹配模板的形状、图像中可被识别的特征等。在一些实施例中，每个角度图案匹配模板与对应的角度标识图案所标识的绕轴角度具有一一对应关系。基于特定的角度图案匹配模板或者识别出的角度标识对应的角度标识图案的图案信息，确定第一绕轴角度。

在一些实施例中，方法1500可以包括响应于匹配失败，确定像素集合的剩余像素点中角点似然值最大的像素点对应的角度标识候选区域作为待识别的角度标识候选区域。在一些实施例中，在确定新的待识别的角度标识候选区域之后，使用多个角度图案匹配模板分别与待识别的角度标识候选区域进行匹配，以识别角度标识。

在一些实施例中，基于识别出的角度标识所在的角度标识候选区域，确定与角度标识具有位置关联关系的第一位姿标识。在一些实施例中，多个角度标识候选区域分别对应识别出的多个位姿标识图案角点中的至少一个，在确定识别出的角度标识所在的角度标识候选区域之后，可以基于多个角度标识候选区域与多个位姿标识图案角点的对应关系，确定第一位姿标识图案角点。

图16示出本公开一些实施例的主操作器1600的示意图。如图16所示，在一些实施例中，主操作器1600包括多自由度机械臂1610和手柄1620，多自由度机械臂1610包括多个关节(16101-16107)。多自由度机械臂1610的关节包括位置关节以及姿态关节，姿态关节作为主操作器1600的定向模块，通过一个或者多个姿态关节控制手柄1620达到目标姿态。位置关节作为主操作器1600的定位模块，通过一个或者多个的位置关节控制手柄1620达到目标位置。

在一些实施例中，确定主操作器的手柄的当前姿态包括：获得至少一个姿态关节的关节信息；以及基于至少一个姿态关节的关节信息，确定主操作器的当前姿态。

在一些实施例中，主操作器传感器设置在多自由度机械臂的姿态关节处，用于获取姿态关节对应的关节信息(如角度)，根据获取到的关节信息确定主操作器的手柄相对于主操作器的基坐标系的当前姿态。在一些实施例中，通过姿态关节的主操作器传感器获取关节信息，基于正向运动学算法计算主操作器的当前姿态。在一些实施例中，主操作器包括用于控制主操作器的手柄的姿态的至少一个姿态关节，并且控制信号包括用于控制至少一个姿态关节中的一个或多个姿态关节的控制信号。通过调整一个或者多个姿态关节实现对主操作器的手柄的姿态调整，实现主操作器的手柄与从动工具的姿态匹配。

在一些实施例中，控制信号包括用于控制至少一个姿态关节中的一个或多个姿态关节的控制信号，其中至少一个姿态关节中的一个或多个姿态关节包括非耦合姿态关节。耦合关节可以是指用于调整主操作器的位置和姿态的关节。非耦合关节可以是指只能够用于调整主操作器的位置(在本公开中，称为非耦合位置关节)或者姿态的关节(在本公开中，称为非耦合姿态关节)。在一些实施例中，主操作器可以包括至少一个耦合关节。例如，图16示出的主操作器1600，其中的第一关节16101、第二关节16102以及第三关节16103为位置关节，第一关节16101、第二关节16102、第五关节16105、第六关节16106以及第七关节16107为姿态关节，第一关节16101以及第二关节16102是既可以调整主操作器1600的位置又可以调整主操作器1600的姿态的耦合关节，第五关节16105以及第六关节16106以及第七关节16107是只能调整主操作器1600的姿态的非耦合姿态关节。在一些实施例中，可以通过计算非耦合姿态关节(例如，第五关节16105、第六关节16106以及第七关节16107)的控制信号实现对主操作器1600的手柄1620的姿态调整，实现主操作器1600的手柄1620与从动工具的姿态匹配，为后续的遥操作提供条件。

在一些实施例中，主操作器的基坐标系为b，手柄的坐标系为d。在一些实施例中，基坐标系b是以基座虚拟为一点而建立的坐标系，其方向可以基于其物理构造而确定。类似地，手柄的坐标系d是以手柄虚拟为一点而建立的坐标系，其方向可以基于其物理构造而确定。在一些实施例中，手柄的坐标系d的原点可以与第五关节、第六关节、第七关节的坐标系的原点重合。本领域技术人员可以理解，手柄的坐标系d相对于主操作器的基坐标系的位置和姿态可以通过第一关节至第七关节的关节信息确定。

在一些实施例中，主操作器传感器获取主操作器的关节信息q_{j_mp}(j是关节的编号)。在一些实施例中，第j个关节信息q_{j_mp}可以包括对应关节的角度值θ_{j_mp}。例如，获取第一关节的关节信息q_{1_mp}、第二关节的关节信息q_{2_mp}、第三关节的关节信息q_{3_mp}、第四关节的关节信息q_{4_mp}、第五关节的关节信息q_{5_mp}、第六关节的关节信息q_{6_mp}、第七关节的关节信息q_{7_mp}。在一些实施例中，第四关节是第三关节的从动关节，第四关节的关节角度与第三关节的关节角度绝对值相同方向相反。因此，主操作器的六个关节的角度表示为6*1的矩阵q_manipulator，第四关节的关节角度可以不在矩阵q_manipulator中体现。每一个关节信息q_{j_mp}可以表示为θ_{j_mp}，主操作器的结构为六个自由度，如公式(25)：

q_manipulator＝(q_{1_mp} q_{2_mp} q_{3_mp} q_{5_mp} q_{6_mp} q_{7_mp})^T (25)

第一关节、第二关节以及第三关节为位置关节，q_{1_mp}、q_{2_mp}、q_{3_mp}决定了主操作器的手柄的位置。第一关节、第二关节、第五关节、第六关节、第七关节为姿态关节，q_{1_mp}、q_{2_mp}、q_{5_mp}、q_{6_mp}、q_{7_mp}决定了手柄的姿态。在一些实施例中，确定主操作器的手柄的姿态，可以不关心由第一关节、第二关节以及第三关节控制的位置，而关心由第一关节、第二关节、第五关节、第六关节以及第七关节确定的姿态(如方向)。在一些实施例中，在电机驱动时，保持第一关节、第二关节以及第三关节不动，基于目标位姿或目标姿态，确定第五关节、第六关节以及第七关节对应的q_{5_mp}、q_{6_mp}、q_{7_mp}，根据q_{5_mp}、q_{6_mp}、q_{7_mp}计算控制信号，以实现对手柄的姿态调整。

本领域技术人员可以理解，多关节的主操作器达到某一个目标姿态可以有很多解。在一些实施例中，可以调整至少一个姿态关节中的一个或多个姿态关节，来调整主操作器手柄的姿态。例如，在一个实施例中，可以保持耦合姿态第一关节、耦合姿态第二关节以及非耦合位置第三关节不变，通过调整非耦合姿态第五关节、第六关节、第七关节，来调整主操作器手柄的姿态。

在一些实施例中，方法700还包括：获得至少一个姿态关节中除待调整的一个或多个姿态关节之外的其他姿态关节的关节信息；以及基于其他姿态关节的关节信息，确定其他姿态关节的转换矩阵。例如，基于主操作器传感器获取其他姿态关节的关节信息，基于其他姿态关节的关节信息确定其他姿态关节的转换矩阵。可以获得耦合姿态第一关节和第二关节的关节信息，并计算转换矩阵。在一些实施例中，可以调整一个或者多个姿态关节中的非耦合姿态关节(例如，第五关节、第六关节以及第七关节)，而不调整其他姿态关节，例如耦合关节(例如，第一关节以及第二关节)。在一些实施例中，可以基于第一关节以及第二关节对应的q_{1_mp}以及q_{2_mp}确定其他姿态关节的转换矩阵(例如，其他姿态关节相对于关节起始点0的转换矩阵⁰R₄)。一些实施例中，方法700还包括基于所述主操作器的手柄的所述目标姿态和所述其他姿态关节的转换矩阵，生成所述主操作器的所述控制信号。例如，基于主操作器的手柄的目标姿态和其他姿态关节的转换矩阵⁰R₄，生成主操作器的控制信号，如公式(26)至公式(28)。

在一些实施例中，第三关节、第四关节是非耦合位置关节，基于q_{1_mp}、q_{2_mp}、q_{3_mp}确定的其他姿态关节的转换矩阵⁰R₄与基于q_{1_mp}、q_{2_mp}确定的其他姿态关节的转换矩阵⁰R₄一致。

⁴R₇＝⁰R₄ ^T·^bR₀ ^T·^bR_d·⁷R_d ^T (26)

在公式(26)中，转换矩阵⁰R₄由输入q_{1_mp}、q_{2_mp}或者q_{1_mp}、q_{2_mp}、q_{3_mp}确定，b为主操作器的基坐标系，d为主操作器的手柄的坐标系，^bR_d为主操作器手柄相对于主操作器基坐标系的姿态，^bR₀为基座与关节起始点的既有角度关系，是结构常数，⁷R_d为第七关节与手柄的既有角度关系，是结构常数。

⁴R₇＝⁴R₅·⁵R₆·⁶R₇ (27)

R(q_{5_mp},q_{6_mp},q_{7_mp})＝⁰R₄ ^T·^bR₀ ^T·R_t·⁷R_d ^T (28)

在公式(28)中，R_t为从动工具的当前姿态并且与^bR_d相同，⁴R₅、⁵R₆和⁶R₇分别对应待求解量q_{5_mp}、q_{6_mp}、q_{7_mp}。基于得到的q_{5_mp}、q_{6_mp}、q_{7_mp}确定控制信号，基于控制信号对主操作器的姿态调整，实现主从姿态的匹配。本领域技术人员可以理解，R_t可以是从动工具的末端器械相对于从动工具的基坐标系的当前姿态，或者从动工具的末端器械在显示器中的图像相对于世界坐标系的当前姿态。R_t可以与^bR_d一致，例如相同或者具有特定比例或差值。在一些实施例中，根据控制信号确定手柄中的一个或者多个姿态关节的关节目标值，将关节目标值转化为驱动量发送至驱动装置。驱动装置驱动主操作器的一个或者多个姿态关节的电机运动使得主操作器的一个或者多个姿态关节运动，实现主操作器的手柄的姿态与从动工具的末端器械的姿态匹配。

在一些实施例中，可以基于D-H参数法或者指数积表示法构建主操作器的数学结构模型。例如，确定主操作器的关节对应的D-H矩阵，基于关节的D-H矩阵确定主操作器的数学结构模型。主操作器的每一个关节的D-H矩阵表示为公式(29)。

D-H矩阵与关节信息的对应关系如表1。

表1 D-H矩阵与关节信息的对应关系

在公式(29)中，Rot(x,α_{j_mp})为绕x轴旋转α_{j_mp}角度，Rot(z,θ_{j_mp})为绕z轴旋转θ_{j_mp}角度，Trans(x,a_{j_mp})在x方向上移动a_{j_mp}，Trans(z,d_{j_mp})在z方向上移动d_{j_mp}。如图16所示的主操作器1600，z轴为该关节的旋转轴，x轴指向下一个关节，可以依据笛卡尔坐标系的左/右手定律确定y轴方向。Rot(x,α_{j_mp})、Trans(x,a_{j_mp})四阶矩阵表示绕一个方向旋转一定角度或延一个方向平移一定距离。

在一些实施例中，主操作器的数学结构模型由所有关节的D-H矩阵相乘描述，如公式(30)：

⁰T_{7_mp}＝⁰T_{1_mp}·^1_mpT_{2_mp}·^2_mpT_{3_mp}·^3_mpT_{4_mp}·^4_mpT_{5_mp}·^5_mpT_{6_mp}·^6_mpT_{7_mp} (30)

在一些实施例中，公式(30)中的关节的D-H矩阵可以基于公式(29)确定。

本领域技术人员可以理解，在启动遥操作时，若手柄的姿态(如朝向或角度)与对应控制的从动工具的姿态(如朝向或角度)不一致，会使得操作者(如手术医生)在操作过程中的人机互动体验较差，影响对从动工具的操作精度。因此，在主操作器与从动工具匹配连接后及主操作器对从动工具遥操作之前(如操作者持握主操作器的手柄获得对应从动工具的控制权但还未开始主从遥操作时)，将手柄的姿态与从动工具的姿态进行匹配调整。当两者的姿态一致时，可以执行主操作器对从动工具的遥操作，可以改进后续遥操作的精度及体验。

在一些实施例中，方法700还包括：响应于预定条件得到满足，可以确定主操作器的手柄与从动工具之间的姿态匹配度。在一些实施例中，预定条件包括遥操作控制权的触发。在一些实施例中，可以通过触发装置实现遥操作控制权的触发。触发装置可以是设置于主操作器或显示器上方便操作者靠近、触碰、按动或划拨的开关。触发方式包括但不限于，保持靠近、触碰、划拨、点按或长按等。触发装置的触发方式可以是靠近传感器、拨动主操作器上的开关、触碰主操作器上的感应位置、长按或点按主操作器上的按键、踩主控台的脚踏板、操作主控台的显示屏等。在一些实施例中，匹配是指手柄的姿态与从动工具的姿态满足预设关系(例如，一致)，姿态匹配度是指手柄的当前姿态与从动工具的当前姿态之间匹配的程度。在一些实施例中，基于主操作器的手柄的当前姿态及从动工具的当前姿态，确定主操作器与从动工具之间的姿态匹配度。当姿态匹配度低于预设阈值时，响应于姿态匹配度低于预设阈值，生成对主操作器的手柄的当前姿态进行调整的控制信号以使得姿态匹配度高于或等于预设阈值。这样，当两者的姿态不匹配时可以自动进行姿态调整以实现两者姿态的一致性。当两者的当前姿态一致或者基本一致(姿态匹配度高于或者等于预设阈值)时，响应于姿态匹配度高于或等于预设阈值，建立主操作器与从动工具之间的主从映射，这样可以执行下一步的遥操作流程。

在一些实施例中，将主操作器的手柄的姿态与从动工具的姿态调整一致的方式包括：保持从动工具的当前姿态不改变，通过调整主操作器的手柄的姿态以使得主操作器的手柄的姿态与从动工具的姿态一致。

在一些实施例中，主操作器的手柄的目标姿态与从动工具的当前姿态一致，在主操作器与从动工具之间建立主从映射，可以执行主操作器对从动工具的遥操作，提高遥操作的操作精度以及遥操作的体验感。本领域技术人员可以理解，姿态一致是指姿态基本一致，主操作器的手柄的目标姿态与从动工具的当前姿态之间可以存在一定的误差，但是误差的范围在可接受的范围内。

在上述一些实施例中，在遥操作之前对手柄的姿态与从动工具的姿态进行匹配，当操作者开始操作(例如按下主操作器的手柄的夹钳按钮)，主从映射能够快速建立，主操作器和从动工具进入遥操作模式。此外，只对从动工具的当前姿态进行保持，操作者在非操作状态下仍可移动主操作器的手柄的位置，使其移动到合适的位置再进行遥操作匹配，极大增加了主操作器的手柄的运动空间。并且，上述提供的主从运动的控制方法，可以适用于多种不同原理和形式的从动端，且计算过程针对性强、计算量小，也降低将主操作器的手柄调整至目标姿态时的驱动量。

在上述一些实施例中，通过将主操作器与从动工具之间建立连接并实现控制权转移，在连接以及控制权转移的状态下确定主操作器的手柄与从动工具之间的姿态匹配度。若姿态匹配度满足预设阈值条件，则建立主操作器与从动工具之间的主从映射，执行遥操作步骤。若姿态匹配度不满足预设阈值条件，此时需要将主操作器的手柄的姿态调整至与从动工具的当前姿态一致，然后再建立主操作器与从动工具之间的主从映射，通过主操作器的手柄执行遥操作。在主操作器和从动工具建立遥操作关系之前及时调整主操作器的手柄的姿态与从动工具的姿态一致，实现了主操作器的手柄与从动工具之间的主从映射的准确性，提高操作者在遥操作时的操作体验，实现操作动作与实际动作的高精度匹配，同时避免了由于主操作器和从动工具彼此运动控制边界不一致导致的操作限制。

上述一些实施例中，当变换主操作器的控制对象(例如，从动工具)时，从动工具入腹的前端朝向与主操作器的手柄的当下的朝向很有可能是不同的。本公开提供的方法可以在主操作器和从动工具建立主从映射关系之前，操作者实际操作之前调整主操作器的手柄的姿态与从动工具的当前姿态一致，以实现操作者良好的操作体验以及动作预期与实际的高精度匹配，同时避免了主操作器和从动工具因为彼此运动控制边界不一致导致的操作限制。

本公开一些实施例中，本公开还提供了一种计算机设备，计算机设备包括存储器和处理器。存储器可以用于存储有至少一条指令，处理器与存储器耦合，用于执行至少一条指令以执行本公开的方法中的部分步骤或全部步骤，如图7、图8、图10、图11、图13和图15中公开的方法中的部分步骤或全部步骤。

图17示出根据本公开一些实施例的计算机设备1700的示意框图。参看图17，该计算机设备1700可以包括中央处理单元(CPU)1701、包括随机存取存储器(Random AccessMemory，RAM)1702和只读存储器(Read-Only Memory，ROM)1703的系统存储器1704，以及连接各部件的系统总线1705。计算机设备1700还可以包括输入/输出设备1706，和用于存储操作系统1713、应用程序1714和其他程序模块1715的大容量存储设备1707。输入/输出设备1706包括主要由显示器1708和输入设备1709组成的输入/输出控制器1710。

大容量存储设备1707通过连接到系统总线1705的大容量存储控制器(未示出)连接到中央处理单元1701。大容量存储设备1707或者计算机可读介质为计算机设备提供非易失性存储。大容量存储设备1707可以包括诸如硬盘或者光盘只读存储器(Compact DiscRead-Only Memory，CD-ROM)驱动器之类的计算机可读介质(未示出)。

不失一般性，计算机可读介质可以包括计算机存储介质和通信介质。计算机存储介质包括以用于存储诸如计算机可读指令、数据结构、程序模块或其他数据等信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动介质。计算机存储介质包括只读存储器、随机存取存储器、闪存或其他固态存储其技术，只读光盘、或其他光学存储、磁带盒、磁带、磁盘存储或其他磁性存储设备。当然，本领域技术人员可知计算机存储介质不局限于上述几种。上述的系统存储器和大容量存储设备可以统称为存储器。

计算机设备1700可以通过连接在系统总线1705上的网络接口单元1711连接到网络1712。

系统存储器1704或大容量存储设备1707还用于存储一个或者一个以上的指令。中央处理单元1701通过执行该一个或一个以上指令来实现本公开一些实施例中的方法的全部或者部分步骤。

本公开一些实施例中，本公开还提供一种计算机可读存储介质，存储介质中存储有至少一条指令，该至少一条指令由处理器执行以使计算机执行本公开一些实施例的方法中的部分步骤或全部步骤，如图7、图8、图10、图11、图13和图15中公开的方法中的部分步骤或全部步骤。计算机可读存储介质的示例包括计算机程序(指令)的存储器，例如，只读存储器、随机存取存储器、只读光盘、磁带、软盘和光数据存储设备等。

图18示出根据本公开一些实施例的手术机器人系统1800的示意图。本公开一些实施例中，参看图18，手术机器人系统1800可以包括手术工具1801、主控台车1802、和手术台车1803。手术台车1803上设置有驱动模组，用于驱动手术工具1801，手术工具1801安装在手术台车1803上并与驱动模组连接。主控台车1802与手术台车1803通信连接，用于控制手术工具1801执行手术操作。在一些实施例中，主控台车1802中的控制器或者手术台车1803中的控制器可以用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图7、图8、图10、图11、图13和图15中公开的方法中的部分步骤或全部步骤。在一些实施例中主控台车1802与手术台车1803之间通过有线传输或者无线传输方式连接。例如，主控台车1802与手术台车1803可以通过线缆连接。

在一些实施例中，手术工具1801包括操作臂和设置在操作臂末端的末端器械。在一些实施例中，手术机器人系统1800可以包括一台手术台车1803。在一些实施例中，手术机器人系统1800可以包括至少两台手术台车1803，每台手术台车1803安装一个手术工具1801。在一些实施例中，手术机器人系统1800还可以包括成像工具1804。成像工具1804可以包括操作臂和设置在操作臂末端的成像模组。成像工具1804可以设置在手术台车1803上并由对应的驱动模组驱动。成像模组获取的手术工具1801的操作臂及其末端器械的图像可以传输至主控台车1802。在一些实施例中，手术工具1801的部分或成像工具1804的部分可以作为从动工具。在一些实施例中，主控台车1802包括主操作器，主操作器用于遥操作手术工具1801或成像工具1804。在一些实施例中，手术工具1801例如是图19中示出的手术工具1900。在一些实施例中，主控台车1802例如是图20中示出的主控台车2000。在一些实施例中，手术台车1803例如是图21中示出的手术台车2100。

图19示出本公开一些实施例的手术工具1900的示意图。本公开一些实施例中，参看图19，手术工具1900包括驱动传动装置1990、操作臂1940和设置在操作臂末端的末端器械1960。在一些实施例中，驱动传动装置1990可以配合驱动模组驱动操作臂1940运动。驱动传动装置1990用于与驱动模组连接，驱动模组的驱动力经驱动传动装置1990传递至操作臂1940，进而驱动操作臂1940实现多自由度运动。驱动模组还可以控制末端器械1960执行手术操作。本公开一些实施例中，末端器械1960可以包括但不限于双极弯分离钳执行器、双极弯头抓钳执行器、单极弯剪执行器、单极电钩执行器、双极抓钳执行器、持针钳执行器与组织抓钳执行器。在一些实施例中，手术工具1900例如可以安装在图18中示出的手术台车1803或者图21中示出的手术台车2100。

图20示出本公开一些实施例的主控台车2000的示意图。本公开一些实施例中，参看图20，主控台车2000包括：控制器(控制器可以配置在计算机设备上，设置在主控台车2000内部)、主操作器2001、主控台车显示器(例如显示器2002-2004)和踏板(例如踏板2005-2007)。控制器分别与主操作器2001、主控台车显示器和踏板通信连接，用于与主操作器2001、主控台车显示器和踏板进行信号交互，并基于收集到的控制信息生成相应的控制指令。在一些实施例中，控制器还与手术台车通信连接，例如与图18中示出的手术台车1803通信连接，用于控制手术工具1801执行手术操作或者控制成像工具1804工作。在一些实施例中，主控台车2000的控制器还可以用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图7、图8、图10、图11、图13和图15中公开的方法中的部分步骤或全部步骤。

在一些实施例中，主操作器2001通常包括分别对应医疗工作人员左手操作的左主操作器(例如，用于控制第一操作臂)和右手操作的右主操作器(例如，用于控制第二操作臂)。在实际场景中，主操作器2001用于采集医疗工作人员的操作输入，医疗工作人员通过遥操作主操作器2001进而控制手术工具或成像工具在操作区域中的运动，以实现医疗操作。在一些实施例中，主操作器2001包括多自由度机械臂20011，多自由度机械臂20011上的每一个关节处设置主操作器传感器，通过每个关节的主操作器传感器生成关节信息(如关节角度数据)。在一些实施例中，多自由度机械臂20011具有六个自由度。在一些实施例中，主操作器2001的位姿可由主操作器关节的关节信息的集合(例如由这些关节信息组成的一维矩阵)来表示。在一些实施例中，主操作器2001还包括夹钳20012，夹钳20012可以用于控制末端器械的张合角度。在一些实施例中，主操作器2001具体可以是图16示出的主操作器1600.在一些实施例中，主控台车显示器包含立体显示器2002、主控外部显示器2003、主控触摸显示器2004。立体显示器2002显示术部图像及系统状态提示，主控外部显示器2003显示术部图像及系统状态提示，触摸显示器2004显示主控台车2000的软件用户界面。在一些实施例中，立体显示器2002或主控外部显示器2003显示的图像可以基于成像模组获取的图像确定，例如图21中示出的成像模组2160b。在一些实施例中，主控台车踏板用于采集医疗工作人员双脚的输入，包含电切踏板2005、电凝踏板2006、离合踏板2007等结构。

图21示出本公开一些实施例的手术台车2100的示意图。本公开一些实施例中，参看图21，手术台车2100包括：控制器(控制器可以配置在计算机设备上，设置在手术台车2100内部)、手术台车底盘2102、手术台车机箱2103、系统状态显示器2105、主立柱2106、主横梁2107、定位臂2108、驱动模组2109等部件。手术台车底盘2102用于实现手术台车2100的移动和固定功能。手术台车机箱2103用于在内部集成手术台车电气件。系统状态显示器2105用于显示手术台车系统用户界面和接收用户输入。主立柱2106可升降，其顶端与主横梁2107固定。主横梁2107的端部有横梁云台，横梁云台下端固定有多个定位臂2108。定位臂2108搭载驱动模组2109，驱动模组2109用于装载手术工具2101或成像工具2104(成像工具2104例如可以是3D电子内窥镜)。在一些实施例中，手术台车2100集成多支定位臂2108，每支定位臂2108具备多个运动关节。在一些实施例中，手术台车2100集成有多个手术工具2101和成像工具2104，多个手术工具2101的部分操作臂2140a及末端器械2160a和成像工具2104的部分操作臂2140b及成像模组2160b经鞘管2110进入工作空间。在一些实施例中，手术台车2100的控制器还可以用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图7、图8、图10、图11、图13和图15中公开的方法中的部分步骤或全部步骤。

注意，上述仅为本公开的示例性实施例及所运用技术原理。本领域技术人员会理解，本公开不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本公开的保护范围。因此，虽然通过以上实施例对本公开进行了较为详细的说明，但是本公开不仅仅限于以上实施例，在不脱离本公开构思的情况下，还可以包括更多其他等效实施例，而本公开的范围由所附的权利要求范围决定。

Claims (28)

1.一种主从运动的控制方法，包括：

获取定位图像；

基于所述定位图像，确定从动工具相对参考坐标系的当前姿态；

基于所述从动工具的当前姿态，确定主操作器的手柄的目标姿态；以及

基于所述主操作器的手柄的所述目标姿态，生成用于所述主操作器的控制信号。

2.根据权利要求1所述的控制方法，其特征在于，包括：

在所述定位图像中，识别位于所述从动工具上的多个位姿标识；以及

基于所述多个位姿标识，确定所述从动工具相对所述参考坐标系的当前姿态。

3.根据权利要求1所述的控制方法，其特征在于，包括：

在所述定位图像中，识别位于所述从动工具上的多个位姿标识；

基于所述多个位姿标识，识别位于所述从动工具上的角度标识，所述角度标识与所述多个位姿标识中的第一位姿标识具有位置关联关系；以及

基于所述角度标识和所述多个位姿标识，确定所述从动工具相对所述参考坐标系的当前姿态。

4.根据权利要求3所述的控制方法，包括：

基于所述角度标识和所述多个位姿标识，确定位姿标识坐标系相对从动工具坐标系的滚转角。

5.根据权利要求4所述的控制方法，包括：

基于所述多个位姿标识，确定所述位姿标识坐标系相对所述参考坐标系的姿态；以及

基于所述位姿标识坐标系相对所述从动工具坐标系的滚转角和所述位姿标识坐标系相对所述参考坐标系的姿态，确定所述从动工具相对所述参考坐标系的姿态。

6.根据权利要求5所述的控制方法，包括：

基于所述多个位姿标识在所述定位图像中的二维坐标和所述多个位姿标识在所述位姿标识坐标系中的三维坐标，确定所述位姿标识坐标系相对所述参考坐标系的姿态。

7.根据权利要求4所述的控制方法，包括：

基于所述位姿标识坐标系相对所述从动工具坐标系的滚转角和所述多个位姿标识在位姿标识坐标系中的三维坐标，确定所述多个位姿标识在所述从动工具坐标系中的三维坐标；以及

基于所述多个位姿标识在所述定位图像中的二维坐标和所述多个位姿标识在所述从动工具坐标系中的三维坐标，确定所述从动工具相对所述参考坐标系的姿态。

8.根据权利要求4-7中任一项所述的控制方法，包括：

确定所述角度标识在从动工具坐标系中标识的第一绕轴角度；

确定所述第一位姿标识在所述位姿标识坐标系中标识的第二绕轴角度；以及

基于所述第一绕轴角度和所述第二绕轴角度，确定所述位姿标识坐标系相对所述从动工具坐标系的滚转角。

9.根据权利要求3-7任一项所述的控制方法，所述位置关联关系包括：

所述角度标识与所述第一位姿标识在轴向的对应关系。

10.根据权利要求3所述的控制方法，包括：

基于所述多个位姿标识在所述定位图像中的二维坐标和所述多个位姿标识在位姿标识坐标系中的三维坐标，确定成像变换关系；

基于所述成像变换关系、所述多个位姿标识在位姿标识坐标系中的三维坐标和所述位置关联关系，在所述定位图像中确定多个角度标识候选区域；以及

从所述多个角度标识候选区域，识别所述角度标识。

11.根据权利要求10所述的控制方法，包括：

基于所述多个位姿标识在位姿标识坐标系中的三维坐标和所述位置关联关系，在所述位姿标识坐标系中确定多个角度标识候选三维坐标；以及

基于所述成像变换关系和所述多个角度标识候选三维坐标，在所述定位图像中确定所述多个角度标识候选区域。

12.根据权利要求10或11所述控制方法，包括：

确定每个所述角度标识候选区域中角点似然值最大的像素以形成像素集合；

确定所述像素集合中角点似然值最大的像素对应的角度标识候选区域作为待识别的角度标识候选区域；以及

使用多个角度图案匹配模板分别与所述待识别的角度标识候选区域进行匹配，以识别所述角度标识。

13.根据权利要求12所述的控制方法，包括：

响应于匹配失败，确定所述像素集合的剩余像素中角点似然值最大的像素对应的角度标识候选区域作为待识别的角度标识候选区域。

14.根据权利要求10或11所述的控制方法，包括：

基于所述角度标识所在的角度标识候选区域，确定与所述角度标识具有位置关联关系的所述第一位姿标识。

15.根据权利要求1-7、10、11中任一项所述的控制方法，其特征在于，还包括：

确定所述主操作器的手柄的当前姿态；以及

基于所述主操作器的手柄的目标姿态和当前姿态，生成用于所述主操作器的控制信号。

16.根据权利要求15所述的控制方法，其特征在于，所述主操作器包括用于控制所述手柄的姿态的至少一个姿态关节，所述控制方法包括：

获得所述至少一个姿态关节的关节信息；以及

基于所述至少一个姿态关节的关节信息，确定所述主操作器的所述当前姿态。

17.根据权利要求1-7、10、11中任一项所述的控制方法，其特征在于，所述从动工具包括操作臂和设置在所述操作臂末端的末端器械，确定所述从动工具相对参考坐标系的当前姿态包括：

确定所述末端器械相对于所述从动工具的基坐标系的当前姿态；或者

确定所述末端器械在显示器中的图像相对于世界坐标系的当前姿态。

18.根据权利要求1-7、10、11中任一项所述的控制方法，其特征在于，所述主操作器包括用于控制所述主操作器的手柄的姿态的至少一个姿态关节，并且所述控制信号包括用于控制所述至少一个姿态关节中的一个或多个姿态关节的控制信号。

19.根据权利要求18所述的控制方法，其特征在于，所述至少一个姿态关节中的所述一个或多个姿态关节包括非耦合姿态关节，所述的控制方法还包括：

获得所述至少一个姿态关节中除所述一个或多个姿态关节之外的其他姿态关节的关节信息；以及

基于所述其他姿态关节的关节信息，确定所述其他姿态关节的转换矩阵。

20.根据权利要求19所述的控制方法，其特征在于，还包括：

基于所述主操作器的手柄的所述目标姿态和所述其他姿态关节的转换矩阵，生成所述主操作器的所述控制信号。

21.根据权利要求1-7、10、11中任一项所述的控制方法，其特征在于，还包括：

响应于预定条件得到满足，确定所述主操作器的手柄与所述从动工具之间的姿态匹配度，所述预定条件包括遥操作控制权的触发。

22.根据权利要求21所述的控制方法，其特征在于，还包括：

基于所述主操作器的手柄的当前姿态及所述从动工具的当前姿态，确定所述主操作器的手柄与所述从动工具之间的姿态匹配度。

23.根据权利要求21所述的控制方法，其特征在于，还包括：

响应于所述姿态匹配度低于预设阈值，生成所述主操作器的手柄的所述控制信号以使得所述姿态匹配度高于或等于预设阈值。

24.根据权利要求21所述的控制方法，其特征在于，还包括：

响应于所述姿态匹配度高于或等于预设阈值，建立所述主操作器与所述从动工具之间的主从映射。

25.根据权利要求1-7、10、11中任一项所述的控制方法，其特征在于，所述主操作器的手柄的目标姿态与所述从动工具的当前姿态一致。

26.一种机器人系统，包括：

主操作器，包括多自由度机械臂、设置在所述多自由度机械臂上的手柄以及设置在所述多自由度机械臂上的至少一个关节处的至少一个电机和至少一个主操作器传感器，所述至少一个主操作器传感器用于获得所述至少一个关节的关节信息；

从动工具，包括操作臂和设置在所述操作臂末端的末端器械；

图像采集器，用于采集定位图像；以及

控制装置，与所述图像采集器和所述主操作器通信连接，所述控制装置被配置成用于执行如权利要求1-25中任一项所述的控制方法。

27.一种计算机设备，所述计算机设备包括：

存储器，用于存储至少一条指令；以及

处理器，与所述存储器耦合并且用于执行所述至少一条指令以执行如权利要求1-25中任一项所述的控制方法。

28.一种计算机可读存储介质，用于存储至少一条指令，所述至少一条指令由计算机执行时致使机器人系统实现如权利要求1-25中任一项所述的控制方法。