CN116728394A - 基于定位图像的机器人系统的控制方法和机器人系统 - 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示出根据本公开一些实施例的手术工具的示意图；

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

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

具体实施方式

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

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

在本公开中，术语“位置”指对象或对象的一部分在三维空间中的定位(例如，可使用笛卡尔X、Y和Z坐标方向的变化描述三个平移自由度，例如分别沿笛卡尔X轴、Y轴和Z轴的三个平移自由度)。在本公开中，术语“姿态”指对象或对象的一部分的旋转设置(即三个旋转自由度之一或全部，例如可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指对象或对象的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。在本公开中，运动臂的一部分的位姿是指运动臂或其一部分定义的坐标系相对于运动臂所在的支架、基座定义的坐标系或世界坐标系的位姿。在本公开中，运动臂的位姿可以由当运动臂处于该位姿时，运动臂所包括的多个关节的关节值集合(例如由这些关节值组成的一维矩阵)来表示。在本公开中，关节的关节值指示相应关节相对于相应的关节轴所旋转的角度或者相对于初始位置移动的距离。在本公开中，运动臂或其一部分的运动路径是指运动臂或其一部分从一个位姿移动到另一个位姿所经过的路径。

在本公开中，参考坐标系可以理解为能够描述物体位姿的坐标系。根据实际的定位需求，参考坐标系可以选择以虚拟参照物的原点或实体参照物的原点为坐标系原点。在一些实施例中，参考坐标系可以为世界坐标系或者相机坐标系或者操作人员自身的感知坐标系等。

图1示出了根据本公开一些实施例的机器人系统100的结构框图。如图1所示，机器人系统100可以包括控制装置110、图像采集设备130以及与控制装置110连接的多个运动臂。在一些实施例中，如图1所示，多个运动臂可以包括第一运动臂120a和第二运动臂120b。控制装置110可以用于控制第一运动臂120a和第二运动臂120b。例如，控制装置110可以调整第一运动臂120a和第二运动臂120b的运动、位姿、相互协调等。在一些实施例中，控制装置110可以控制运动臂(例如，第一运动臂120a或第二运动臂120b)运动至目标位姿。在一些实施例中，第一运动臂120a和第二运动臂120b的末端可以分别包括第一末端(例如，末端臂1201a或者末端臂1201a的远端)和第二末端(例如，末端臂1201b或者末端臂1201b的远端)。控制装置110可以控制第一运动臂120a或第二运动臂120b运动至目标位姿，以使对应的末端运动至期望的位置和姿态。

在一些实施例中，图像采集设备130与控制装置110通信连接。在一些实施例中，图像采集设备130可以用于采集定位图像。在一些实施例中，辅助连接装置上设置有包括定位标识的定位装置(例如是辅助连接装置的鞘管上或者主体腔管上设置有定位装置)。定位图像中可以包括定位装置的部分或全部的图像(例如，定位图像中包括多个定位标识中的一部分)。在一些实施例中，定位标识包括位姿标识，基于位姿标识可以确定辅助连接装置的位置或姿态。在一些实施例中，定位标识可以包括位姿标识和角度标识(如下详述)，基于位姿标识和角度标识可以确定辅助连接装置的位置和姿态。

在一些实施例中，辅助连接装置的鞘管或者主体腔管的一部分处于图像采集设备130的观测视野之内，定位装置可以设置在鞘管或者主体腔管上。在一些实施例中，图像采集设备130可以包括但不限于双镜头图像采集设备或单镜头图像采集设备，例如双目或单目相机。根据不同的应用场景，图像采集设备130可以是工业相机、水下相机、微型电子相机、内窥镜相机等。在一些实施例中，图像采集设备130可以是位置固定的或者位置变化的，例如，固定在监控位置的工业相机或者位置或姿态可调节的内窥镜相机。在一些实施例中，图像采集设备130可以实现可见光波段成像、红外波段成像、CT(Computed Tomography，电子计算机断层扫描)成像和声波成像等中的至少一种。

在一些实施例中，控制装置110被配置成用于执行至少一条指令以执行本公开的方法中的部分步骤或全部步骤，如图9-10、图12-13、图15和图17-18中公开的方法中的部分步骤或全部步骤。在一些实施例中，控制装置110可以接收来自图像采集设备130的定位图像，并且对定位图像进行处理。例如，控制装置110可以在定位图像中，识别位于辅助连接装置上的定位标识。在一些实施例中，控制装置110可以基于定位图像，确定辅助连接装置的第一鞘管的位姿，例如，基于定位图像，确定第一鞘管相对参考坐标系的位姿。在一些实施例中，机器人系统100还可以包括三个、四个或更多的运动臂。机器人系统100可以包括手术机器人系统，例如腔镜手术机器人系统(例如，图2示出的机器人系统200，或图20示出的手术机器人系统2000)。应当理解，机器人系统100还可以包括用于其他领域(例如，制造、机械等等)的专用或通用机器人系统。

图2示出了根据本公开一些实施例的机器人系统200的立体结构示意图。如图2所示，机器人系统200可以包括手术台车230以及设置在手术台车230上的第一运动臂220a和第二运动臂220b。在一些实施例中，手术台车230可以包括基座2301和横梁2302。在一些实施例中，第一运动臂220a和第二运动臂220b可以活动设置在横梁2302上。应当理解，机器人系统200的多个运动臂也可以设置在多个手术台车上。例如，每个运动臂对应设置在一个手术台车上。或者，一个运动臂设置在一个手术台车上，其余多个运动臂设置在另一手术台车上。

在一些实施例中，机器人系统的每一运动臂(例如，图2示出的第一运动臂220a或第二运动臂220b)可以包括多根连杆以及多个关节串行连接而成。在一些实施例中，每个运动臂的每个关节可以包括电机，用于驱动相应的关节转动，进而带动相应的连杆转动。

图3示出了根据本公开一些实施例的机器人系统300的多个运动臂的结构示意图。如图3所示，以第二运动臂320b为例，第二运动臂320b可以包括关节32011b-32081b和连杆3201b-3208b。连杆3201b的近端与横梁3302连接，连杆3201b-3207b依次串连。关节32011b可以位于横梁3302与连杆3201b的近端连接处，关节32021b可以位于连杆3201b与连杆3202b的连接处，关节32031b可以位于连杆3202b与连杆3203b的连接处，关节32041b可以位于连杆3203b与连杆3204b的连接处，关节32051b可以位于连杆3204b与连杆3205b的连接处，关节32061b可以位于连杆3205b与连杆3206b的连接处，关节32071b可以位于连杆3206b与连杆3207b的连接处，关节32081b可以位于连杆3207b与连杆3208b的连接处。连杆3208b作为第二运动臂320b的最远端的连杆，形成第二运动臂320b的末端臂。末端臂的位置和姿态的确定和表示需要前述每个关节共同决定。

在一些实施例中，机器人系统还可以包括一个或多个手术器械(例如，图2中所示的第一手术器械260a和第二手术器械260b)。如图2所示，第一手术器械260a可以可拆卸地安装在第一运动臂220a的第一末端臂2208a上，并且第二手术器械260b可以可拆卸地安装在第二运动臂220b的第二末端臂2208b上。应当理解，第一手术器械260a和第二手术器械260b可以包括但不限于用于实施手术的夹钳、电刀或用于进行照明成像的图像捕获设备(例如内窥镜工具)等等。第一手术器械260a和第二手术器械260b的一部分可进入人类或动物的某个身体部位内以实施医疗操作，例如手术。在一些实施例中，手术器械例如可以包括图21示出的手术工具2100。

在一些实施例中，如图2所示，机器人系统200还可以包括辅助连接装置250，例如鞘套。辅助连接装置250的一部分可以被定位在人体或动物需要进行手术的身体部位，例如手术口(例如，切口或自然腔道)，另一部分用于与运动臂(例如与第一运动臂220a或第二运动臂220b的第一末端臂22081a或第二末端臂22081b)可拆卸地连接，以更好地为手术服务。

图4示出了根据本公开一些实施例的辅助连接装置400的局部剖面图。在一些实施例中，如图4所示，辅助连接装置400可以包括多个鞘管，例如第一鞘管451和第二鞘管452。在一些实施例中，辅助连接装置250还可以包括设置在多个鞘管上的多个连接部(例如第一连接部4511和第二连接部4521)。连接部可以包括但不限于夹钳、卡合结构、粘合结构、插拔结构、吸合结构。第一连接部4511和第二连接部4521可以分别固定设置在第一鞘管451和第二鞘管452上。在一些实施例中，辅助连接装置400还包括与至少一个鞘管连接的主体腔管453。在一些实施例中，主体腔管453可以包括多个工具通道4531，每个工具通道4531分别与对应的鞘管连通。在一些实施例中，辅助连接装置400还设置有定位装置，定位装置上包含定位标识。定位装置可以例如是设置在第一鞘管451上的定位装置4512或者设置在主体腔管453上的定位装置4532。在一些实施例中，定位装置4512或者定位装置4532例如可以是定位标签。定位标签例如是如图6示出的标签600或者图7示出的标签700。

在一些实施例中，手术器械安装到末端臂后，手术器械可以穿过辅助连接装置的鞘管进入手术操作空间(例如，病人的腹腔)。例如，手术工具或成像工具可以经鞘管以及工具通道进入手术操作空间。

应当理解，如图4所示的辅助连接装置400仅仅是示例性的。在一些实施例中，机器人系统可以包括其它数量的运动臂，例如一个、三个、四个或更多的运动臂，辅助连接装置可包括其他数量的鞘管，例如一个、三个、四个或更多的鞘管，每个鞘管上包括相应的连接部，以便用于每个鞘管与每个运动臂连接，并约束多个运动臂的末端之间的相对位姿关系。此外，手术器械与鞘管也不一定是一一对应。例如，在一些实施例中，多个手术器械可以通过同一根鞘管进入手术操作空间。

图5示出了根据本公开一些实施例的运动臂与辅助连接装置连接状态500的示意图。在一些实施例中，每个运动臂上可以包括与辅助连接装置的连接部配合的连接件(例如，图2所示的第一连接件22081a或第二连接件22081b或图5所示的连接件55082)。参看图5，辅助连接装置550包括四个鞘管(例如，鞘管551和鞘管552)以及与鞘管连接的主体腔管553。图5中示出运动臂的数量为四个，以运动臂520为例，运动臂520的连接件52082设置在末端臂5208的末端。连接件52082与鞘管551的连接部5511连接以实现运动臂520与辅助连接装置550的连接。在一些实施例中，连接件52082与鞘管551的连接部5511可拆卸地固定连接。在一些实施例中，辅助连接装置550例如可以与图4示出的辅助连接装置400具有相同的构造。

在一些实施例中，若手术器械为多个，多个手术器械可以同步或依序穿过对应的鞘管进入手术操作空间。例如，第一手术器械和第二手术器械可以分别穿过第一鞘管和第二鞘管同时进入手术操作空间，也可以是第一手术器械先穿过第一鞘管进入手术操作空间，之后第二手术器械穿过第二鞘管进入手术操作空间。在一些实施例中，辅助连接装置的鞘管可以是柔性的，并且手术器械延伸穿过辅助连接装置的部分也是柔性的。

应当理解，运动臂的末端例如可以是末端臂的远端、远端运动中心(RemoteCentre of Motion,RCM)或运动臂的连接件。运动臂的末端位姿可以是末端坐标系相对于运动臂所在的支架、基座的坐标系或世界坐标系的位姿。在一些实施例中，多个运动臂的末端的相对位姿关系可基于当前手术类型或辅助连接装置的构型确定。例如，基于当前手术类型可以确定辅助连接装置的构型或者选择适配当前手术的辅助连接装置。基于辅助连接装置的构型，确定辅助连接装置的多个鞘管之间的形状和相对位置关系，以确定多个运动臂的末端的相对位姿关系。例如，可基于第一鞘管和第二鞘管的形状和相对位置关系，确定第一运动臂的末端和第二运动臂的末端的相对位姿关系。在一些实施例中，第一运动臂与第二运动臂的末端的相对位姿关系可指示第一运动臂的末端和第二运动臂的末端的相对位置关系和相对姿态关系。例如，末端的相对位姿关系可以包括例如第一运动臂的第一末端臂或第一末端臂的一部分和第二运动臂的第二末端臂或第二末端臂的一部分之间的相对位姿关系。或者，末端的相对位姿关系还可以包括第一手术器械(安装在第一末端臂上)和第二手术器械(安装在第二末端臂上)之间的相对位姿关系。或者，末端的相对位姿关系还可以包括固定设置在第一末端臂和第二末端臂上的第一连接件和第二连接件之间的相对位姿关系。在一些实施例中，相对位姿关系可以存储在相关联的相对位姿模型中，以用于计算第二运动臂的末端的目标位姿。由于第一连接件和第二连接件分别固定在第一末端臂和第二末端臂上，因此在第一末端臂和第二末端臂符合末端的相对位姿关系时，第一连接件和第二连接件可以分别与第一连接部和第二连接部连接。

本公开的一些实施例中，每个运动臂的末端的目标位置或姿态可以通过相应运动臂所包括的多个关节中的一个或多个关节实现。在一些实施例中，运动臂用于实现目标姿态的多个关节相对于该运动臂的用于实现目标位置的多个关节更靠近运动臂的远端。应当理解，实现运动臂的末端的目标姿态和目标位置的多个关节还可以包括其他设置方式，可以根据具体需求来设置。

在术前准备阶段，辅助连接装置的位姿是相对固定的(例如，辅助连接装置与病人的腹壁连接，辅助连接装置处于相对固定的状态以避免牵拉病人腹壁)。在一些实施例中，可以基于辅助连接装置的位姿，对运动臂进行术前摆位，以使运动臂处在适宜与辅助连接装置的鞘管连接的位置。

在一些实施例中，辅助连接装置上设置有多个位姿标识。例如，辅助连接装置的第一鞘管上或者主体腔管上分布有多个位姿标识。例如，多个位姿标识沿周向分布在第一鞘管或者主体腔管上(例如，围绕周长的全部或部分)。基于多个位姿标识的图像，可以确定第一鞘管的位姿。在一些实施例中，多个位姿标识可以包括单排或多排的位姿标识。

在一些实施例中，辅助连接装置上设置有多个位姿标识和至少一个角度标识。例如，辅助连接装置的第一鞘管或者主体腔管上分布有多个位姿标识和至少一个角度标识。以主体腔管为例，多个位姿标识沿周向分布在主体腔管上，多个角度标识沿周向分布在主体腔管上。多个位姿标识与多个角度标识在主体腔管上沿轴向并列设置。例如，多个位姿标识与多个角度标识设置在主体腔管的柱状部分的外表面上。

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

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

在一些实施例中，定位装置例如可以是标签(例如，图6示出的标签600或图7示出的标签700)。在一些实施例中，多个位姿标识与多个角度标识可以设置在标签上，标签贴附在辅助连接装置上。在一些实施例中，标签贴附在第一鞘管或者主体腔管的周侧。

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

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

图6示出根据一些实施例的包括多个位姿标识与多个角度标识的标签600的示意图。图7示出设置在第一鞘管或者主体腔管周侧并形成圆筒形的标签700的示意图。可以理解，简单起见，标签600可以与标签700包括相同的位姿标识图案和角度标识图案。

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

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

在一些实施例中，角度标识或位姿标识的绕轴角度或者滚转角可以通过角度标识图案角点或位姿标识图案角点的绕轴角度表示。角度标识图案角点相对于辅助连接装置坐标系(例如建立在第一鞘管或者主体腔管上的坐标系，如图7所示的XY坐标系)的角度是已知的或预先确定的，例如图7中的角度标识图案角点R₇在XY坐标系下与X轴的角度为θ。基于位置关联关系，可以得到与其位置相关联的位姿标识图案角点P₇相对X轴的夹角为角度θ。应该理解，角度标识图案角点R₇和位姿标识图案角点P₇对应的角度θ可以称为角度标识或位姿标识的绕Z轴的绕轴角度或者滚转角。在本公开中，绕轴角度或者滚转角是指绕Z轴的角度。可以理解，清楚起见，图7中将角度标识图案角点R₇和位姿标识图案角点P₇示为分离，但它们是重合的。

图8示出根据本公开一些实施例的辅助连接装置800设置定位标识的示意图。如图8所示，辅助连接装置的主体腔管853的部分位于病人的体内(例如，主体腔管853的部分位于腹腔830内)。多个位姿标识和角度标识可以沿周向设置在辅助连接装置800的主体腔管853上。例如，如图6所示的标签600沿周向设置在主体腔管853上，形成圆筒形的角度标识图案带810和位姿标识图案带820。多个位姿标识图案角点分布在主体腔管853的位姿标识图案带820的截面圆821上，多个角度标识图案角点分布在主体腔管853的角度标识图案带810的截面圆811上。在一些实施例中，多个位姿标识和角度标识还可以沿周向设置在鞘管上。例如，多个位姿标识和角度标识可以采用与标识图案带810和位姿标识图案带820类似的方案设置在鞘管851上。

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

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

本公开一些实施例提供了一种用于机器人系统的控制方法。在一些实施例中，该机器人系统包括多个运动臂，例如如图2或图5所示。图9示出根据本公开一些实施例的用于机器人系统的控制方法900的流程图。方法900中的部分或全部步骤可以由机器人系统100的控制装置(例如控制装置110)或者图20示出的主控台车2040或手术台车2030的控制器来执行。控制装置110可以配置在计算设备上。方法600可以由软件、固件和/或硬件来实现。在一些实施例中，方法900可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图9，在步骤901，获取定位图像。在一些实施例中，定位图像中包含辅助连接装置的一部分和辅助连接装置上的多个位姿标识。在一些实施例中，定位图像还可以包括辅助连接装置的一部分和辅助连接装置上的至少一个角度标识。例如，定位图像中包含主体腔管的一部分和主体腔管上的多个位姿标识和至少一个角度标识。在一些实施例中，可以从如图1所示的图像采集设备130接收定位图像。例如，控制装置110可以接收图像采集设备130主动发送的定位图像。或者，控制装置110可以向图像采集设备130发送图像请求指令，图像采集设备130响应图像请求指令向控制装置110发送定位图像。

在步骤903，在定位图像中，识别位于辅助连接装置上的多个位姿标识。在一些实施例中，识别位于辅助连接装置上的多个位姿标识的示例性方法可以包括如图13和图15所示的方法。在一些实施例中，控制装置110可以通过图像处理算法识别定位图像中的部分或全部的位姿标识。在一些实施例中，图像处理算法可以包括特征识别算法，图像处理算法可以提取或识别位姿标识的特征。例如，图像处理算法可以包括角点检测算法，用于检测位姿标识图案角点。该角点检测算法可以是包括但不限于基于灰度图的角点检测、基于二值图像的角点检测、基于轮廓曲线的角点检测中的一种。例如，图像处理算法可以是颜色特征提取算法，用于检测位姿标识图案中的颜色特征。再例如，图像处理算法可以是轮廓检测算法，用于检测位姿标识图案的轮廓特征。在一些实施例中，控制装置可以通过识别模型识别定位图像中的部分或全部的位姿标识。

在一些实施例中，方法900还包括基于多个位姿标识，识别位于辅助连接装置上的角度标识。在一些实施例中，在识别出多个位姿标识之后，根据位置关联关系，识别位于辅助连接装置上的角度标识。在一些实施例中，角度标识与第一位姿标识的位置关联关系可以是如图6或图7中示出的位置关联关系。在一些实施例中，第一位姿标识(例如是第一位姿标识图案或第一位姿标识图案角点)是指多个位姿标识中与角度标识具有位置关联关系的位姿标识。在一些实施例中，识别角度标识的示例性方法包括如图17所示的方法。

在步骤905，基于多个位姿标识，确定辅助连接装置的第一鞘管相对参考坐标系的位姿。在另一些实施例中，方法900还包括在定位图像中，识别位于辅助连接装置上的多个位姿标识和角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系；以及基于角度标识和多个位姿标识，确定第一鞘管相对参考坐标系的位姿。

在一些实施例中，确定第一鞘管相对参考坐标系的位姿的示例性方法包括如图10或图12所示的方法。在一些实施例中，可以基于角度标识、第一位姿标识和多个位姿标识，确定第一鞘管相对参考坐标系的位姿。在一些实施例中，第一鞘管的位姿可以通过第一鞘管的坐标系的位姿表示。例如，第一鞘管的坐标系相对参考坐标系的位姿可以作为第一鞘管的位姿。参考坐标系例如可以是机器人系统或运动臂的基座标系或者世界坐标系等。在一些实施例中，第一鞘管的坐标系原点可以设置在第一鞘管的入口位置，还可以设置在第一鞘管的连接部上。在一些实施例中，辅助连接装置例如可以是图2所示的辅助连接装置250，或图4所示的辅助连接装置400，或图5所示的辅助连接装置550。

在一些实施例中，方法900还包括：基于角度标识和多个位姿标识，确定辅助连接装置坐标系和位姿标识坐标系的变换关系。在一些实施例中，根据辅助连接装置坐标系和位姿标识坐标系的变换关系，可以将位姿标识坐标系中的三维坐标转换为在辅助连接装置坐标系中对应的三维坐标。在一些实施例中，根据辅助连接装置坐标系和位姿标识坐标系的变换关系和位姿标识坐标系相对参考坐标系的位姿，得到辅助连接装置坐标系相对参考坐标系的位姿。

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

在一些实施例中，辅助连接装置坐标系可以是基于多个位姿标识或多个角度标识，在辅助连接装置的鞘管或主体腔管上设定的固定坐标系。在一些实施例中，辅助连接装置坐标系的Z轴平行于鞘管或主体腔管的轴向，辅助连接装置坐标系的XY平面与多个位姿标识图案角点处于同一平面，或者与多个角度标识图案角点处于同一平面。

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

示例性地，参看图8，辅助连接装置坐标系{wm}≡[X_wm Y_wm Z_wm]^T的原点为多个位姿标识图案角点所在截面圆821的圆心，X轴方向为原点指向其中一个位姿标识图案角点，Z轴的方向平行鞘管或主体腔管的轴向，Y轴垂直于XZ平面。辅助连接装置坐标系{wm}的X轴与截面圆811的二维平面坐标系{wm1}≡[X_wm1 Y_wm1]^T的X轴平行，辅助连接装置坐标系的Y轴与截面圆811的二维平面坐标系{wm1}的Y轴平行。角度标识图案角点在截面圆811的二维平面坐标系{wm1}中标识的绕轴角度可以与其在辅助连接装置坐标系{wm}中标识的绕轴角度相等。位姿标识坐标系{wm0}≡[X_wm0 Y_wm0 Z_wm0]^T的原点为多个位姿标识图案角点所在截面圆821的圆心，X轴方向为原点指向其中一个位姿标识图案角点，Z轴的方向平行鞘管或主体腔管的轴向，Y轴垂直于XZ平面。继续参看图8，辅助连接装置坐标系{wm}的Z轴与位姿标识坐标系{wm0}的Z轴重合。辅助连接装置坐标系{wm}相对位姿标识坐标系{wm0}的变换关系可以通过位姿标识坐标系{wm0}相对辅助连接装置坐标系{wm}的滚转角α₀确定。滚转角α₀可以是指位姿标识坐标系{wm0}相对辅助连接装置坐标系{wm}绕Z轴的转动角度。

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

α₀＝α₁-α₂ (1)

其中α₁为第一绕轴角度，α₂为第二绕轴角度。第一绕轴角度为角度标识图案角点(例如，角度标识图案角点R₈)在辅助连接装置坐标系中标识的绕轴角度。第二绕轴角度为第一位姿标识图案角点(例如，位姿标识图案角点P₈)在位姿标识坐标系中标识的绕轴角度。

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

参看图10，在步骤1001，基于角度标识和多个位姿标识，确定位姿标识坐标系相对辅助连接装置坐标系的滚转角。在一些实施例中，确定角度标识在辅助连接装置坐标系中标识的第一绕轴角度。确定第一位姿标识在位姿标识坐标系中标识的第二绕轴角度。基于第一绕轴角度和第二绕轴角度，确定位姿标识坐标系相对辅助连接装置坐标系的滚转角。在一些实施例中，位姿标识坐标系相对坐标系的滚转角可以基于公式(1)确定。

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

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

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

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

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

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

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

参看图10，在步骤1005，基于位姿标识坐标系相对辅助连接装置坐标系的滚转角和位姿标识坐标系相对参考坐标系的位姿，确定第一鞘管相对参考坐标系的位姿。

本领域技术人员可以理解，本公开一些实施例还可以基于位姿标识坐标系相对辅助连接装置坐标系的滚转角和位姿标识坐标系相对参考坐标系的位姿，确定辅助连接装置坐标系相对参考坐标系的位姿作为鞘管或主体腔管相对参考坐标系的位姿。例如，以参考坐标系为世界坐标系为例，辅助连接装置坐标系相对世界坐标系的位姿具体如下：

^wR_wm＝^wR_wm0·rot_z(α₀)

^wP_wm＝^wP_wm0 (3)

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

在一些实施例中，辅助连接装置坐标系相对世界坐标系的位姿具体计算公式(4)如下：

^wR_wm＝^wR_lens ^lensR_wm0 ^wm0R_wm

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

其中，^wR_lens为相机坐标系相对世界坐标系的姿态，^wP_lens为相机坐标系相对世界坐标系的位置，^lensR_wm0为位姿标识坐标系相对相机坐标系的姿态，^lensP_wm0为位姿标识坐标系相对相机坐标系的位置，^wm0R_wm为辅助连接装置坐标系相对位姿标识坐标系的姿态，^wm0P_wm为辅助连接装置坐标系相对位姿标识坐标系的位置。

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

参看图12，在步骤1201，基于位姿标识坐标系相对辅助连接装置坐标系的滚转角和多个位姿标识在位姿标识坐标系中的三维坐标，确定多个位姿标识在辅助连接装置坐标系中的三维坐标。可以理解，已知位姿标识坐标系相对辅助连接装置坐标系的滚转角，可以根据坐标变换将多个位姿标识图案角点在位姿标识坐标系中的三维坐标变换为在辅助连接装置坐标系中的三维坐标。

在步骤1203，基于多个位姿标识在定位图像中的二维坐标和多个位姿标识在辅助连接装置坐标系中的三维坐标，确定第一鞘管相对参考坐标系的位姿。在一些实施例中，步骤1203可以与方法1000中步骤1003和1005类似的实现。

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

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

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

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

CL＝max(c_xy,c₄₅)

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

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

其中，τ为设定的常数，例如设定为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值大于第一阈值的像素点可以作为候选位姿标识图案角点。

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

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

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

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

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

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

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

其中，一阶导数(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，高斯混合模型)方法中的任意一种。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

返回图9的方法900，在步骤907，基于第一鞘管的位姿，确定第一运动臂的第一目标位姿。在一些实施例中，第一运动臂的第一目标位姿可以通过第一运动臂所包括的多个关节的目标关节值集合(例如，由这些目标关节值组成的一维矩阵)来表示。

在一些实施例中，方法900还包括：确定第一运动臂的第一摆位点的目标位姿；以及基于第一摆位点的目标位姿和第一运动臂的运动学模型，确定第一运动臂的第一目标位姿。在一些实施例中，可以基于第一鞘管的位姿确定第一摆位点的目标位姿。本公开中，运动臂的摆位点可以为运动臂在摆位时与鞘管连接的位置的位姿参照点。

在摆位点处于预定的目标位姿时，运动臂上与鞘管的连接位置处于适宜与鞘管连接的位姿。本公开中，摆位点的位姿可以通过摆位点的坐标系的位姿表示。例如，摆位点的坐标系相对参考坐标系的位姿可以作为摆位点的位姿。在一些实施例中，运动臂的摆位点可以设置在运动臂的连接件上，或者运动臂的摆位点还可以设置在运动臂的末端，或者运动臂的摆位点为运动臂的远端运动中心。例如，运动臂的摆位点可以设置在运动臂的末端，并且摆位点的位姿可以与运动臂末端的位姿相同(例如，运动臂末端的坐标系与摆位点坐标系相同)或者与运动臂末端的位姿具有预定的变换关系。在一些实施例中，运动臂可以通过运动学模型描述，运动臂的运动学模型可以基于运动臂的结构确定。在一些实施例中，可以基于D-H参数法构建运动臂的运动学模型。例如，确定运动臂的关节对应的D-H矩阵，基于关节的D-H矩阵确定运动臂的运动学模型。基于第一摆位点的目标位姿和第一运动臂的运动学模型，可以通过逆运动学算法解算出第一运动臂的第一目标位姿。在一些实施例中，基于第一摆位点的目标位姿和第一运动臂的运动学模型，可以通过逆运动学算法解算出第一运动臂的部分或全部关节的关节值作为第一目标位姿。

在一些实施例中，方法900还包括：确定第一运动臂的多个关节中的特征关节的预定目标关节值；以及基于第一摆位点的目标位姿、预定目标关节值以及第一运动臂的运动学模型，确定第一运动臂的其他关节的目标关节值。在一些实施例中，运动臂的多个关节中的特征关节可以为多个关节中容易与其它运动臂或结构发生碰撞的关节，例如，图3所示的关节32051b或32061b。应当理解，在机器人系统包括多个运动臂(例如三个或四个运动臂)时，不同的运动臂的特征关节的预定目标关节值可以不同。在一些实施例中，其他关节的目标关节值可以包括第一运动臂的除特征关节外的所有其他关节的目标关节值。在一些实施例中，基于第一摆位点的目标位姿、预定目标关节值和第一运动臂的运动学模型，可以通过逆运动学算法解算出第一运动臂的其他关节的目标关节值。

在一些实施例中，方法900还包括：判断其他关节的目标关节值是否在相应关节的关节运动范围之内；以及响应于其他关节的目标关节值中的至少一个不在相应关节的关节运动范围之内，将预定目标关节值递增或递减预设的调整值，以调整预定目标关节值。应当理解，运动臂的每一关节具有一定的运动范围，各个关节的关节运动范围包括相应关节的最小极限关节值和最大极限关节值之间的范围。在一些实施例中，最小极限关节值以及最大极限关节值可以不在关节运动范围之内。例如而非作为限制，有的关节在18度到45度之间运动，有的关节在45度到90度之间运动，还有的关节在-90度到-45度之间运动等等。在一些实施例中，预设的调整值可以设置为例如0.2°或0.5°等等来调整预定目标关节值。应理解，0.2°或0.5°仅作为示例，调整值还可以设置为其他值。在一些实施例中，递增或递减预设的调整值，直到有解或者达到特征关节的关节运动范围(可以不包括极限值)。例如，有解可以表示预定目标关节值或者其他关节的目标关节值在对应关节的关节运动范围之内。在一些实施例中，方法900还包括判断调整后的预定目标关节值是否在特征关节的关节运动范围之内。

在一些实施例中，方法900还包括：响应于其他关节的目标关节值都在相应关节的关节运动范围之内，基于预定目标关节值和其他关节的目标关节值，确定第一运动臂的第一目标位姿。例如，响应于第一运动臂的其他关节的目标关节值都在相应关节的关节运动范围之内，选择预定目标关节值和其他关节的目标关节值的集合作为第一运动臂的第一目标位姿。

在步骤909，控制第一运动臂运动至第一目标位姿，以与辅助连接装置的第一鞘管连接。在一些实施例中，控制装置可以控制第一运动臂的多个关节运动，以使第一运动臂由第一运动臂的第一初始位姿运动至第一目标位姿，以与第一鞘管连接。例如，在第一目标位姿，第一末端臂与辅助连接装置的第一鞘管连接。

在一些实施例中，多个运动臂还包括第二运动臂。方法900还包括：基于第一鞘管的位姿或第一目标位姿，确定第二运动臂的第二目标位姿。在一些实施例中，方法900还包括：基于第一鞘管和第二鞘管的相对位姿关系和第一鞘管的位姿或者第一目标位姿和第一运动臂与第二运动臂的相对位姿关系，确定第二运动臂上的第二摆位点的目标位姿，其中，第一鞘管和第二鞘管的相对位姿关系或者第一运动臂与第二运动臂的相对位姿关系是基于辅助连接装置的构型确定的；以及基于第二运动臂上的第二摆位点的目标位姿，确定第二运动臂的第二目标位姿。在辅助连接装置的构型或结构确定之后，第一鞘管和第二鞘管的相对位姿关系或者第一运动臂与第二运动臂的相对位姿关系可基于辅助连接装置的构型或结构得到确定。根据辅助连接装置的构型可以基本确定第二鞘管的坐标系和第一鞘管的坐标系的位姿变换关系或者第一运动臂与第二运动臂的相对位姿关系。例如，可以预先确定第二鞘管的坐标系和第一鞘管的坐标系的位姿变换关系或者第一运动臂与第二运动臂的相对位姿关系。在实际操作中，可以方便地调用预先确定的第二鞘管的坐标系和第一鞘管的坐标系的位姿变换关系或者第一运动臂与第二运动臂的相对位姿关系。在一些实施例中，基于第一鞘管和第二鞘管的相对位姿关系和第一鞘管的位姿，可以确定第二鞘管的位姿。在一些实施例中，基于第二鞘管的位姿，可以确定第二摆位点的目标位姿。在一些实施例中，与第一运动臂类似，可以基于第二摆位点的目标位姿和第二运动臂的运动学模型确定第二运动臂的第二目标位姿。例如，基于第二摆位点的目标位姿和第二运动臂的运动学模型，可以通过逆运动学算法解算出第二运动臂的部分或全部关节的关节值作为第二目标位姿。在一些实施例中，方法900还包括：控制第二运动臂运动至第二目标位姿，以与第二鞘管连接。在一些实施例中，控制装置可以控制第二运动臂的多个关节运动，以使第二运动臂由第二运动臂的第二初始位姿运动至第二目标位姿，以与第二鞘管连接。例如，在第二目标位姿，第二末端臂与辅助连接装置的第二鞘管连接。

在一些实施例中，多个运动臂还包括第二运动臂，方法900还包括：基于定位图像，获得辅助连接装置上的第二鞘管的位姿，其中，定位图像中包括设置在辅助连接装置的第二鞘管上的定位装置；以及基于第二鞘管的位姿，确定第二运动臂的第二目标位姿。在一些实施例中，第二鞘管上的定位装置例如是包括鞘管定位标识的鞘管定位装置。在一些实施例中，鞘管定位装置的鞘管定位标识可以设置在第二鞘管的坐标系所在的位置，也可以是鞘管定位装置与第二鞘管的坐标系具有确定的位姿变换关系。在一些实施例中，第二鞘管的位姿可以通过第二鞘管的坐标系的位姿表示。例如，第二鞘管的坐标系相对参考坐标系的位姿可以作为第二鞘管的位姿。在一些实施例中，第二鞘管的坐标系原点可以设置在第二鞘管的入口位置，还可以设置在第二鞘管的连接部上。

在一些实施例中，鞘管定位标识包括多个位姿标识或角度标识。例如，鞘管定位标识可以包括与标签600或标签700中类似的位姿标识或角度标识。方法900还包括：在定位图像中，识别位于第二鞘管上的多个位姿标识；基于识别出的第二鞘管上的多个位姿标识，识别位于第二鞘管上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系；以及基于角度标识和多个位姿标识，确定第二鞘管相对参考坐标系的位姿。

在一些实施例中，与上述的确定第一鞘管的位姿类似，方法900还包括：基于辅助连接装置的主体腔管的位姿和辅助连接装置的构型，确定第二鞘管的位姿。其中，主体腔管的位姿可以基于包括主体定位标识的定位装置确定。例如，在一些实施例中，定位装置的主体定位标识可以设置在主体腔管的坐标系所在的位置，基于主体定位标识确定主体腔管的位姿。在一些实施例中，主体定位标识包括多个位姿标识或角度标识。例如，主体定位标识可以包括与标签600或标签700中类似的位姿标识或角度标识。

在一些实施例中，方法900还包括：控制第一运动臂与第一鞘管连接。在一些实施例中，控制装置控制第一运动臂与第一鞘管连接。在一些实施例中，控制第一运动臂的第一连接件与第一鞘管的第一连接部连接以使得第一运动臂与第一鞘管连接。在一些实施例中，方法900还包括：控制第二运动臂与第二鞘管连接。与第一运动臂类似，可以由控制装置控制第二运动臂与第二鞘管连接。在一些实施例中，控制第二运动臂的第二连接件与第二鞘管的第二连接部连接以使得第二运动臂与第二鞘管连接。在一些实施例中，同步或依序控制第一运动臂和第二运动臂分别与第一鞘管和第二鞘管连接。例如，控制装置控制第一运动臂和第二运动臂同时运动以分别连接第一鞘管和第二鞘管。例如，控制装置可以先控制第一运动臂与第一鞘管连接，响应于第一运动臂与第一鞘管连接，控制装置控制第二运动臂与第二鞘管连接。

在一些实施例中，可以基于第一目标位姿确定第一运动臂的第一运动路径。在一些实施例中，方法900还包括：基于第一运动路径，控制第一运动臂运动至第一目标位姿。在一些实施例中，可以基于第二目标位姿确定第二运动臂的第二运动路径。方法900还包括：基于第二运动路径，控制第二运动臂运动至第二目标位姿。在一些实施例中，方法900还包括基于第一运动路径和第二运动路径，同步或依序控制第一运动臂运动至第一目标位姿和第二运动臂运动至第二目标位姿。应当理解，运动路径可以包括运动臂的多个关节的路径。在一些实施例中，可以基于运动路径控制运动臂的多个关节运动，以使运动臂按设定的运动路径运动至目标位姿。例如，控制装置可以基于第一运动路径控制第一运动臂的多个关节运动，以使第一运动臂按设定的第一运动路径运动至第一目标位姿。

图18示出了根据本公开一些实施例的用于确定运动臂的运动路径的方法1800的流程图。方法1800中的部分或全部步骤可以由机器人系统100的控制装置(例如控制装置110)或者图20示出的主控台车2040或手术台车2030的控制器来执行。方法1800可以由软件、固件和/或硬件来实现。在一些实施例中，方法1800可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。在一些实施例中，方法1800可以用于确定第一运动臂的第一运动路径或第二运动臂的第二运动路径。

参看图18，在步骤1801，获取运动臂的初始位姿。在一些实施例中，运动臂的初始位姿可通过利用安装在运动臂的各个关节处的传感器获得运动臂的各个关节的初始关节值来获取。应当理解，初始位姿也可以包括运动臂的当前位姿。

在步骤1803，基于运动臂的初始位姿和目标位姿，确定运动臂的运动路径。在一些实施例中，例如，基于运动臂的初始位姿和目标位姿，可以利用插值法来规划运动臂从其初始位姿运动到其目标位姿的运动路径。

在一些实施例中，方法1800还可以包括：判断当前运动臂和多个运动臂的其他运动臂之间是否会形成干涉关系。例如，在规划第二运动臂的第二运动路径的过程中，判断第一运动臂和第二运动臂之间是否会形成干涉关系。干涉关系可以包括例如第二运动臂与第一运动臂会发生碰撞等。在一些实施例中，响应于第二运动臂的所有的其他关节的目标关节值都在相应关节的关节运动范围之内，判断第二运动臂与第一运动臂之间是否会形成干涉关系。关节运动范围可以基于第一运动臂的位姿确定，以避免与第一运动臂发生干涉。在一些实施例中，响应于第二运动臂与第一运动臂之间不会形成干涉关系，基于第二运动路径，控制第二运动臂运动至第二目标位姿。在一些实施例中，在运动臂的关节值有多组满足条件的解(例如有多组满足条件的第二运动臂的目标关节值)时，可以选择第二运动臂的各关节与第一运动臂发生干涉的概率最低的一组解，作为第二运动臂的目标关节值。应当理解，可以根据第二运动臂的预定目标关节值和其他关节的目标关节值与第一运动臂对应关节值之间的距离，来确定发生干涉的概率以及判断是否发生干涉。例如，距离越大，则表示发生干涉的概率越低。

在一些实施例中，方法900还包括：基于定位图像，确定辅助连接装置的主体腔管的位姿；以及基于辅助连接装置的主体腔管的位姿和辅助连接装置的构型，确定第一鞘管相对参考坐标系的位姿。在一些实施例中，辅助连接装置的主体腔管的位姿可以通过主体腔管的坐标系相对参考坐标系的位姿表示。在一些实施例中，主体腔管的位姿可以作为辅助连接装置的位姿。在一些实施例中，定位装置例如是包括主体定位标识的主体定位装置(主体定位装置可以例如是设置在主体腔管453上的定位装置4532)。主体定位装置的主体定位标识可以设置在主体腔管的坐标系所在的位置，主体定位装置也可以是与主体腔管的坐标系具有确定的位姿变换关系。应当理解，在辅助连接装置的构型或结构确定之后，第一鞘管和主体腔管的相对位姿关系可以得到确定。根据辅助连接装置的构型可以基本确定第一鞘管的坐标系和主体腔管的坐标系的位姿变换关系。在一些实施例中，主体定位标识包括多个位姿标识或角度标识。例如，主体定位标识可以包括与标签600或标签700中类似的位姿标识或角度标识。在一些实施例中，可以基于主体定位标识中的角度标识和多个位姿标识，确定主体腔管的位姿。在一些实施例中，确定主体腔管的位姿方法可以与图10中示出的方法1000或者图12示出的方法1200类似的实现。

在一些实施例中，机器人系统的控制装置被配置成执行本公开的方法。例如，如图9-10、图12-13、图15和图17-18中公开的方法中的部分步骤或全部步骤。在一些实施例中，机器人系统例如可以是图1示出的机器人系统100、图2示出的机器人系统200或图20示出的手术机器人系统2000。

在一些实施例中，机器人系统还包括：辅助连接装置，包括至少一个鞘管，用于与多个运动臂连接(例如，图5示出的鞘管551与对应的运动臂520连接)；以及位姿检测装置，包括：至少一个定位装置，定位装置包括多个位姿标识和至少一个角度标识；以及图像采集器，与控制装置连接，用于采集包括至少一个定位装置的定位图像。在一些实施例中，辅助连接装置例如可以是图2示出的辅助连接装置250，或图4示出的辅助连接装置400，或图5示出的辅助连接装置550。在一些实施例中，至少一个定位装置例如可以是图4中示出的定位装置4512或者定位装置4532。在一些实施例中，至少一个定位装置例如还可以是图5中示出的定位装置5512。在一些实施例中，图像采集器可以例如设置在手术台车的主体或横梁上。例如，图像采集器例如是图2示出的横梁2302上设置的图像采集设备23021或者图3示出的横梁3302上设置的图像采集设备33021。在一些实施例中，图像采集器可以例如设置在运动臂上，例如设置在图5所示的运动臂520上。在一些实施例中，辅助连接装置包括与至少一个鞘管连接的主体腔管，至少一个定位装置包括设置在主体腔管上的主体定位装置(例如，图4中示出的主体定位装置4532)，主体定位装置包括主体定位标识。在一些实施例中，至少一个定位装置包括设置在所述第一鞘管上的鞘管定位装置(例如，图4中示出的鞘管定位装置4512)，鞘管定位装置包括鞘管定位标识。

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

图19示出根据本公开一些实施例的计算机设备1900的示意框图。参看图19，该计算机设备1900可以包括中央处理单元(Central Processing Unit，CPU)1901、包括随机存取存储器(Random Access Memory，RAM)1902和只读存储器(Read-Only Memory，ROM)1903的系统存储器1904，以及连接各部件的系统总线1905。计算机设备1900还可以包括输入/输出设备1906，和用于存储操作系统1913、应用程序1914和其他程序模块1915的大容量存储设备1907。输入/输出设备1906包括主要由显示器1908和输入设备1909组成的输入/输出控制器1910。

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

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

计算机设备1900可以通过连接在系统总线1905上的网络接口单元1911连接到网络1912。

系统存储器1904或大容量存储设备1907还用于存储一个或者一个以上的指令。中央处理单元1901通过执行该一个或一个以上指令来实现本公开一些实施例中的方法的全部或者部分步骤，如图9-10、图12-13、图15和图17-18中公开的方法中的部分步骤或全部步骤。

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

图20示出根据本公开一些实施例的手术机器人系统2000的示意图。本公开一些实施例中，参看图20，手术机器人系统2000可以包括手术工具2060a、主控台车2040、和手术台车2030。手术台车2030上设置有驱动模组，用于驱动手术工具2060a，手术工具2060a安装在手术台车2030上并与驱动模组连接。主控台车2040与手术台车2030通信连接，用于控制手术工具2060a执行手术操作。在一些实施例中，主控台车2040或手术台车2030的控制装置可以用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图9-10、图12-13、图15和图17-18中公开的方法中的部分步骤或全部步骤。在一些实施例中主控台车2040与手术台车2030之间通过有线传输或者无线传输方式连接。例如，主控台车2040与手术台车2030可以通过线缆连接。在一些实施例中，手术机器人系统2000还可以包括成像工具2060b。成像工具2060b可以包括操作臂和设置在操作臂末端的成像模组。成像工具2060b可以设置在手术台车2030上并由对应的驱动模组驱动。成像模组获取的手术工具2060a的操作臂及其执行器的图像可以传输至主控台车2040。在一些实施例中，手术工具2060a例如是图21中示出的手术工具2100。在一些实施例中，主控台车2040例如是图22中示出的主控台车2200。在一些实施例中，手术台车2030例如是图23中示出的手术台车2300。

在一些实施例中，手术机器人或系统至少包括两个手术工具，手术工具包括操作臂和设置在操作臂末端的执行器。在一些实施例中，手术机器人系统可以包括一台手术台车，一台手术台车能够安装至少两个手术工具。在一些实施例中，手术机器人系统可以包括至少两台手术台车，每台手术台车安装一个手术工具。

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

图22示出本公开一些实施例的主控台车2200的示意图。本公开一些实施例中，参看图22，主控台车2200包括：控制器(控制器可以配置在计算机设备上，设置在主控台车2200内部)、主操作器2201、主控台车显示器(例如显示器2202-2204)和踏板(例如踏板2205-2207)。控制器分别与主操作器2201、主控台车显示器和踏板通信连接，用于与主操作器2201、主控台车显示器和踏板进行信号交互，并基于收集到的控制信息生成相应的控制指令。在一些实施例中，控制器还与手术台车通信连接，例如与图20中示出的手术台车2030通信连接，用于控制手术工具2060b执行手术操作或者控制成像工具2060a工作。在一些实施例中，主控台车2200的控制器还可以用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图9-10、图12-13、图15和图17-18中公开的方法中的部分步骤或全部步骤。

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

图23示出本公开一些实施例的手术台车2300的示意图。本公开一些实施例中，参看图23，手术台车2300包括：控制器(控制器可以配置在计算机设备上，设置在手术台车2300内部)、手术台车底盘2302、手术台车机箱2303、系统状态显示器2305、主立柱2306、主横梁2307、运动臂2320、驱动模组2309等部件。手术台车底盘2302用于实现手术台车2300的移动和固定功能。手术台车机箱2303用于在内部集成手术台车电气件。系统状态显示器2305用于显示手术台车系统用户界面和接收用户输入。主立柱2306可升降，其顶端与主横梁2307固定。主横梁2307的端部有横梁云台，横梁云台下端固定有多个运动臂2320。运动臂2320搭载驱动模组2309，驱动模组2309用于装载手术工具2360a或成像工具2360b(成像工具2360b例如可以是3D电子内窥镜)。在一些实施例中，手术台车2300集成多支运动臂2320，每支运动臂2320具备多个运动关节。在一些实施例中，手术台车2300集成有多个手术工具2360a和成像工具2360b，多个手术工具2360a的部分操作臂2361a及执行器2362a和成像工具2360b的部分操作臂2361b及成像模组2362b经辅助连接装置的鞘管和主体腔管2353进入工作空间。在一些实施例中，手术台车2300的控制器还可以用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图9-10、图12-13、图15和图17-18中公开的方法中的部分步骤或全部步骤。

虽然已经举例说明和描述了本发明的具体实施方案，但是对于本领域技术人员来说显而易见的是，在不脱离本发明实质和范围的情况下可以做出多种其他改变和变型。因此，在随附的权利要求书中包括属于本发明范围内的所有这些改变和变型。

Claims (26)

1.一种用于机器人系统的控制方法，所述机器人系统包括多个运动臂，所述多个运动臂包括第一运动臂，所述方法包括：

获取定位图像；

在所述定位图像中，识别位于辅助连接装置上的多个位姿标识；

基于所述多个位姿标识，确定所述辅助连接装置的第一鞘管相对参考坐标系的位姿；

基于所述第一鞘管的位姿，确定所述第一运动臂的第一目标位姿；以及

控制所述第一运动臂运动至所述第一目标位姿，以与所述辅助连接装置的所述第一鞘管连接。

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

基于所述多个位姿标识，识别位于所述辅助连接装置上的角度标识，所述角度标识与所述多个位姿标识中的第一位姿标识具有位置关联关系；以及

基于所述角度标识和所述多个位姿标识，确定所述第一鞘管相对所述参考坐标系的位姿。

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

基于所述角度标识和所述多个位姿标识，确定位姿标识坐标系相对辅助连接装置坐标系的滚转角；

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

基于所述位姿标识坐标系相对所述辅助连接装置坐标系的滚转角和所述位姿标识坐标系相对所述参考坐标系的位姿，确定所述第一鞘管相对所述参考坐标系的位姿。

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

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

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

基于所述位姿标识坐标系相对所述辅助连接装置坐标系的滚转角和所述多个位姿标识在位姿标识坐标系中的三维坐标，确定所述多个位姿标识在所述辅助连接装置坐标系中的三维坐标；以及

基于所述多个位姿标识在所述定位图像中的二维坐标和所述多个位姿标识在所述辅助连接装置坐标系中的三维坐标，确定所述第一鞘管相对所述参考坐标系的位姿。

6.根据权利要求3-5中任一项所述的控制方法，包括：

确定所述角度标识在辅助连接装置坐标系中标识的第一绕轴角度；

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

基于所述第一绕轴角度和所述第二绕轴角度，确定所述位姿标识坐标系相对所述辅助连接装置坐标系的滚转角。

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

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

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

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

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

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

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

9.根据权利要求8所述控制方法，包括：

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

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

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

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

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

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

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

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

基于所述定位图像，确定所述辅助连接装置的主体腔管的位姿；以及

基于所述辅助连接装置的主体腔管的位姿和所述辅助连接装置的构型，确定所述第一鞘管相对所述参考坐标系的位姿。

13.根据权利要求1-5、8-12中任一项所述的控制方法，还包括控制所述第一运动臂与所述第一鞘管连接。

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

获取所述第一运动臂的第一初始位姿；

基于所述第一初始位姿和所述第一目标位姿，确定所述第一运动臂的第一运动路径；以及

基于所述第一运动路径，控制所述第一运动臂运动至所述第一目标位姿。

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

确定所述第一运动臂的第一摆位点的目标位姿；以及

基于所述第一摆位点的目标位姿和所述第一运动臂的运动学模型，确定所述第一运动臂的第一目标位姿。

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

确定所述第一运动臂的多个关节中的特征关节的预定目标关节值；以及

基于所述第一摆位点的目标位姿、所述预定目标关节值以及所述第一运动臂的运动学模型，确定所述第一运动臂的其他关节的目标关节值。

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

判断所述其他关节的目标关节值是否在相应关节的关节运动范围之内；以及

响应于所述其他关节的目标关节值中的至少一个不在相应关节的关节运动范围之内，将所述预定目标关节值递增或递减预设的调整值，以调整所述预定目标关节值；或者

响应于所述其他关节的目标关节值都在相应关节的关节运动范围之内，基于所述预定目标关节值和所述其他关节的目标关节值，确定所述第一运动臂的第一目标位姿。

18.根据权利要求1所述的控制方法，所述多个运动臂还包括第二运动臂，所述方法包括：

基于所述第一鞘管的位姿或所述第一目标位姿，确定所述第二运动臂的第二目标位姿；以及

控制所述第二运动臂运动至所述第二目标位姿，以与所述第二鞘管连接。

19.根据权利要求18所述的控制方法，还包括：

基于所述第一鞘管和第二鞘管的相对位姿关系和所述第一鞘管的位姿或者所述第一目标位姿和所述第一运动臂与所述第二运动臂的相对位姿关系，确定所述第二运动臂上的第二摆位点的目标位姿，其中，所述第一鞘管和所述第二鞘管的相对位姿关系或者所述第一运动臂与所述第二运动臂的相对位姿关系是基于所述辅助连接装置的构型确定的；以及

基于所述第二运动臂上的第二摆位点的目标位姿，确定所述第二运动臂的第二目标位姿。

20.根据权利要求18或19所述的控制方法，包括：

基于所述第一目标位姿，确定所述第一运动臂的第一运动路径；

基于所述第二目标位姿，确定所述第二运动臂的第二运动路径；以及

基于所述第一运动路径和所述第二运动路径，同步或依序控制所述第一运动臂运动至所述第一目标位姿和所述第二运动臂运动至所述第二目标位姿。

21.一种计算机设备，包括：

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

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

22.一种计算机可读存储介质，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器执行以使计算机执行如权利要求1-20中任一项所述的控制方法。

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

多个运动臂，所述多个运动臂包括第一运动臂；以及

控制装置，被配置成执行如权利要求1-20中的任一项所述的控制方法。

24.根据权利要求23所述的机器人系统，还包括：

辅助连接装置，包括至少一个鞘管，用于与所述多个运动臂连接；以及

位姿检测装置，包括：

至少一个定位装置，所述定位装置包括多个位姿标识和至少一个角度标识；以及

图像采集器，与所述控制装置连接，用于采集包括所述至少一个定位装置的定位图像。

25.根据权利要求24所述的机器人系统，所述辅助连接装置包括与所述至少一个鞘管连接的主体腔管，所述至少一个定位装置包括设置在所述主体腔管上的主体定位装置。

26.根据权利要求24所述的机器人系统，所述至少一个定位装置包括设置在所述第一鞘管上的鞘管定位装置。