基于关联标识的误差检测方法及机器人系统与流程

1.本公开涉及误差检测技术领域，尤其涉及一种基于关联标识的误差检测方法及机器人系统。


背景技术：

2.通常，用于遥操作的机器人系统包括用于执行操作的执行臂及用于控制执行臂运动的主操作器。在实际场景中，执行臂被设置成能够进入操作区域，操作者通过遥操作主操作器而控制执行臂在操作区域中的运动，进而通过设置在执行臂末端(执行器)进行操作。这种机器人通过主操作器与执行臂之间的运动转换实现主操作器对执行臂的运动控制。
3.机器人对操作精度和人机交互体验要求很高。遥操作过程中，需要实时检测执行臂的位姿误差，以确定执行臂有否正确地按照操作者的意愿移动到与主操作器的操作对应的位置和姿态，进而实时把握机器人系统的工作状况。


技术实现要素：

4.在一些实施例中，本公开提供了一种误差检测方法。该方法可以包括：获得执行臂的末端的目标位姿；获取定位图像；在定位图像中，识别位于执行臂的末端上的多个位姿标识；基于多个位姿标识，识别位于执行臂的末端上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系；基于角度标识和多个位姿标识，确定执行臂的末端的实际位姿；以及响应于目标位姿与实际位姿满足误差检测条件，生成与故障相关的控制信号。
5.在一些实施例中，本公开提供了一种计算机设备，包括：存储器，用于存储至少一条指令；以及处理器，与存储器耦合并且用于执行至少一条指令以执行本公开一些实施例中任一项的方法。
6.在一些实施例中，本公开提供了一种计算机可读存储介质，用于存储至少一条指令，至少一条指令由计算机执行时致使计算机执行本公开一些实施例中任一项的方法。
7.在一些实施例中，本公开提供了一种机器人系统，包括：主操作器，包括机械臂、设置在机械臂上的手柄以及设置在机械臂上的至少一个关节处的至少一个主操作器传感器，至少一个主操作器传感器用于获得至少一个关节的关节信息；执行臂，执行臂的末端设置有至少一个角度标识和多个位姿标识；至少一个驱动装置，用于驱动执行臂；至少一个驱动装置传感器，与至少一个驱动装置耦合并且用于获得至少一个驱动装置的状态信息；图像采集设备，用于采集执行臂的定位图像；以及控制装置，被配置为与主操作器、至少一个驱动装置、至少一个驱动装置传感器、图像采集设备连接，执行本公开一些实施例中任一项的方法。
附图说明
8.图1示出根据本公开一些实施例的机器人系统的结构示意图；
9.图2示出根据本公开一些实施例的误差检测系统的示意图；
10.图3示出根据本公开一些实施例的误差检测方法的流程图；
11.图4示出根据本公开一些实施例的确定执行臂的末端的目标位姿的方法的流程图；
12.图5示出根据本公开一些实施例的主从运动映射中的坐标系示意图；
13.图6示出根据本公开一些实施例的包括多个位姿标识与多个角度标识的标签示意图；
14.图7示出根据本公开一些实施例的标签设置在执行臂末端周侧形成的圆筒形标签的示意图；
15.图8示出根据本公开一些实施例的实施场景示意图；
16.图9示出根据本公开一些实施例的确定执行臂的末端的实际位姿的方法的流程图；
17.图10示出根据本公开另一些实施例的确定执行臂的末端的实际位姿的方法的流程图；
18.图11示出根据本公开一些实施例的多个位姿标识在截面圆的示意图；
19.图12示出根据本公开一些实施例的用于识别位姿标识的方法的流程图；
20.图13示出根据本公开一些实施例的位姿标识图案的示意图；
21.图14示出根据本公开一些实施例的用于搜索位姿标识的方法的流程图；
22.图15示出根据本公开一些实施例的搜索位姿标识的示意图；
23.图16示出根据本公开一些实施例的识别角度标识的方法的流程图；
24.图17示出根据本公开一些实施例的计算机设备的示意框图；
25.图18示出根据本公开一些实施例的机器人系统的示意图。
具体实施方式
26.为使本公开解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图详细说明本公开实施例的技术方案作。本领域技术人员可以理解，所描述的实施例应视为在所有方面都是示例性的而非限制性的，仅仅是本公开示例性实施例，而不是全部的实施例。
27.在本公开的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本公开和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本公开的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。
28.在本发明公开的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“耦合”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连；可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明公开中的具体含义。在本发明公开中，定义靠近操作者的一端为近端、近部或后端、后部，靠近作业对象的一端为远端、末端、远部或前端、前部。本领域技术人员可以理解，
本公开的实施例可以适用于包括但不限于在地表、地下、水下、太空和生物体内等多种环境中作业的机械装置上设置的执行臂。
29.在本公开中，术语“位置”指物体或物体的一部分在三维空间中的定位(例如，可使用笛卡尔x、y和z坐标方面的变化描述三个平移自由度，例如分别沿笛卡尔x轴、y轴和z轴的三个平移自由度)。在本公开中，术语“姿态”指物体或物体的一部分的旋转设置(例如，三个旋转自由度，可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指物体或物体的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。在本公开中，主操作器的手柄的位姿可由主操作器关节的关节信息的集合(例如由这些关节信息组成的一维矩阵)来表示。执行臂的位姿可由执行臂的驱动信息来确定。在本公开中，关节的关节信息可以包括相应关节相对于相应的关节轴所旋转的角度或者相对初始位置移动的距离。
30.在本公开中，参考坐标系可以理解为能够描述物体位姿的坐标系。根据实际的定位需求，参考坐标系可以选择以虚拟参照物的原点或实体参照物的原点为坐标系原点。在一些实施例中，参考坐标系可以为世界坐标系或者主操作器、执行臂、相机所在空间的坐标系或者操作人员自身的感知坐标系等等。
31.在本公开中，物体可以理解为需要被定位的对象或目标，例如执行臂或者执行臂的末端。执行臂或其一部分(例如末端)的位姿可以是指执行臂或其一部分定义的坐标系相对参考坐标系的位姿。
32.图1示出根据本公开一些实施例的机器人系统100的结构示意图。在一些实施例中，如图1所示，机器人系统100可以包括主控台车110、从动台车130及控制装置120。控制装置120可以与主控台车110和从动台车130通信连接，例如可通过线缆连接、也可以通过无线连接，以实现与主控台车110和从动台车130之间的通信。主控台车110包括供操作者遥操作的主操作器，从动台车130包括用于执行作业的至少一个执行臂。通过控制装置120实现主控台车110中的主操作器与从动台车130中的执行臂之间的主从映射，实现主操作器对执行臂的运动控制。在一些实施例中，执行臂被设置成能够通过管套、鞘套等进入操作区域，其中，管套、鞘套可以固定在墙体壁面、动物躯体等上形成开口(例如人工开口或自然开口)处，操作区域可以是进行作业的区域。执行臂可以是连续体可形变臂，在执行臂的远端可以设置有末端器械(例如，执行器)，该末端可以包括但不限于挖掘器械、水下作业器械、分拣器械、手术器械等。本领域技术人员可以理解，主控台车110和从动台车130可以采用其他结构或者形式，例如基座、支架或建筑物等。
33.在一些实施例中，除作业工具以外，执行臂也可以作为视觉工具使用，视觉工具的末端器械可以包括但不限于图像采集设备或照明设备等。在一些实施例中，主控台车可以包括主操作器和用于显示操作区域的图像的显示器。图像采集设备可以用于采集操作区域的图像，并将采集到的图像传送至从动台车。图像经从动台车中的视频处理模块处理后，显示在从动台车的显示器上。操作者通过显示器中的图像实时获得执行臂的末端相对参考坐标系的位姿。主操作器相对参考坐标系的位姿是操作者真实感知到的姿态。操作者通过遥操作主操作器所感受到的位姿变化与操作者在显示器中感知到的执行臂的末端的姿态变化之间符合预设的位姿关系，这样，通过远程遥操作主操作器，基于预设的位姿关系将主操作器的位姿变换转换为执行臂的末端的位姿变化，进而实现对执行臂的末端的位姿控制。
这样，当操作者握住主操作器的手柄运动以操作执行臂时，基于直观性操作的原则，操作者感受到的执行臂的末端器械的姿态变化量与操作者感受到的主操作器的姿态变化量保持一致，有助于改善操作者的遥操作感受以及遥操作精度。
34.在遥操作过程中，有时执行臂会无法按照操作者的期望准确地移动到与主操作器的操作对应的位置和姿态。本公开中，在遥操作的过程中检测执行臂的位姿误差，确定执行臂有否正确地按照操作者的意愿进行移动，进而实时把握机器人系统的工作状况。本领域技术人员可以理解，根据本公开一些实施例的位姿误差检测方法也可以在非遥操作过程中执行。
35.图2示出根据本公开一些实施例的误差检测系统200的示意图。如图2所示，系统200可以包括主操作器210、控制装置220、执行臂230、以及图像采集设备250。执行臂230可以由可形变臂或刚性臂实现。在一些实施例中，执行臂230在末端或远端处可以包括执行臂末端231，执行臂末端231上可以设置有末端器械240。
36.控制装置220可以与至少一个驱动装置通信连接，向驱动装置发送驱动信息，控制执行臂230运动，以使执行臂末端231运动至期望的位置和姿态。例如，控制执行臂230运动的至少一个驱动装置可以为伺服电机，可以接受控制装置的指令以控制执行臂230运动。在一些实施例中，控制装置220可以基于主操作器210的位姿和主操作器210与执行臂230之间的映射关系，确定执行臂末端231的目标位姿。
37.图像采集设备250与控制装置220通信连接。在一些实施例中，图像采集设备250可以用于采集定位图像，图像采集设备250可以包括但不限于双镜头图像采集设备或单镜头图像采集设备，例如双目或单目相机。定位图像中可以包括位于操作区域内的执行臂230的部分或全部的图像。在一些实施例中，图像采集设备250可以用于采集执行臂末端231的图像，执行臂末端231上可以设置有定位标识。定位标识可以包括位姿标识和角度标识(如下详述)。
38.如图2所示，执行臂末端231处于图像采集设备250的观测视野251之内，则采集的定位图像中可以包括执行臂末端231的图像。根据不同的应用场景，图像采集设备250可以是工业相机、水下相机、微型电子相机、内窥镜相机等。在一些实施例中，图像采集设备250可以是位置固定的或者位置变化的，例如，固定在监控位置的工业相机或者位置或姿态可调节的内窥镜相机。在一些实施例中，图像采集设备250可以实现可见光波段成像、红外波段成像、ct(computed tomography，电子计算机断层扫描)成像和声波成像等中的至少一种。根据采集的图像的种类不同，本领域技术人员可以选择不同的图像采集设备作为图像采集设备250。
39.在一些实施例中，控制装置220可以接收来自图像采集设备250的定位图像，并且对定位图像进行处理。例如，控制装置220可以在定位图像中，识别位于执行臂末端231上的多个位姿标识和至少一个角度标识，并且确定执行臂末端231相对参考坐标系(例如世界坐标系)的位姿，作为执行臂末端231的实际位姿。
40.在本公开中，控制装置220可以基于执行臂末端231的目标位姿和实际位姿对执行臂230进行误差检测，确定执行臂末端231是否准确移动到了与主操作器210的操作对应的位置和姿态，进而判断执行臂230是否发生故障并生成相应的控制信号。在一些实施例中，控制装置220还可以以预定周期确定执行臂末端231的目标位姿和实际位姿，以通过多个检
测循环实时对执行臂230进行误差检测。本领域技术人员应理解，系统200可以应用于多个领域(例如，医疗、工业制造等等)的专用或通用机器人系统，例如图1所示的机器人系统100，或图18所示的机器人系统1800。作为一个示例，系统200可以应用于例如手术机器人等机器人系统，执行臂末端231的远端处设置的末端器械240例如可以是手术执行器。
41.本公开的一些实施例提供了一种用于机器人系统的执行臂的误差检测方法。图3示出根据本公开一些实施例的误差检测方法300(以下也简称“方法300”)的流程图。方法300可以由硬件、软件或者固件实现或执行。在一些实施例中，方法300可以由机器人系统(例如，图1所示的机器人系统100，或图18所示的机器人系统1800)执行。在一些实施例中，方法300可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图18所示的控制装置1820)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法300。在一些实施例中，这些指令可以存储在计算机可读介质上。
42.参看图3，在步骤301，获得执行臂的末端的目标位姿。在一些实施例中，可以基于主操作器的位姿与执行臂的末端的位姿之间的主从运动映射关系，根据主操作器的位姿确定执行臂的末端的目标位姿。获得执行臂的末端的目标位姿的示例性方法包括如图4所示的方法。
43.图4示出根据本公开一些实施例的确定执行臂的末端的目标位姿的方法400的流程图。如图4所示，该方法400中的部分或全部步骤可以由控制装置(例如，图1所示的控制装置120、图2所示的控制装置220或图18所示的控制装置1820)来执行。方法400中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法400可以由机器人系统(例如，图1所示的机器人系统100或图18所示的机器人系统1800)执行。在一些实施例中，方法400可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的控制装置1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
44.图5示出根据本公开一些实施例的主从运动映射中的坐标系示意图。在图5中各坐标系的定义如下：执行臂基坐标系{tb}，原点位于执行臂的基座或者入鞘套出口，与基座延长线或者入鞘套轴向方向一致，方向如图5所示。相机坐标系{lens}，原点位于相机中心，相机轴线方向为方向，视野摆正后上方为方向。执行臂的末端坐标系{wm}，原点位于执行臂的末端，与末端的轴线方向一致，方向如图5所示。参考坐标系{w}，可以是主操作器或执行臂或相机所在空间的坐标系，例如执行臂基坐标系{tb}，或者世界坐标系，如图5所示。在一些实施例中，可以以操作者的体感为基准，当操作者端坐于主控台之前时，体感向上为方向，体感向前方向为方向。显示器坐标系{screen}，原点位于显示器中心，垂直屏幕画面向内方向为正方向，屏幕画面上方为方向。主操作器基坐标系{combx}，坐标轴方向如图5所示。主操作器的手柄坐标系{h}，坐标轴方向如图5所示。
45.下面以图5所示的坐标系为例，对确定执行臂的末端的目标位姿的方法400进行描述。但是，本领域技术人员可以理解，可以采用其他坐标系定义实现确定执行臂的末端的目
标位姿的方法400。
46.参看图4，在步骤401，可以确定主操作器的当前位姿，当前位姿包括当前位置和当前姿态。在一些实施例中，主操作器的当前位姿是相对主操作器基坐标系{combx}的位姿。例如，主操作器的位姿是主操作器的手柄或其一部分定义的坐标系相对主操作器基坐标系{combx}(例如，主操作器所在的支架或基座定义的坐标系、或世界坐标系)的位姿。在一些实施例中，确定主操作器的当前位置包括确定主操作器的手柄相对主操作器基坐标系{combx}的当前位置，确定主操作器的当前姿态包括确定主操作器的手柄相对主操作器基坐标系{combx}的当前姿态。
47.在一些实施例中，可以基于坐标变换确定主操作器的当前位姿。例如，可以基于主操作器的手柄的坐标系{h}与主操作器基坐标系{combx}之间的变换关系确定手柄的当前位姿。通常情况下，主操作器基坐标系{combx}可以设置在主操作器所在的支架或基座上，且在遥操作过程中，主操作器基坐标系{combx}保持不变。
48.在一些实施例中，可以基于主操作器传感器确定主操作器的当前位姿。在一些实施例中，接收主操作器的至少一个关节的当前关节信息，并且基于至少一个关节的当前关节信息，确定主操作器的当前位姿。例如，主操作器的当前位姿是基于主操作器传感器获得的至少一个关节的当前关节信息确定的。主操作器传感器设置在主操作器的至少一个关节位置处。例如，主操作器包括至少一个的关节，在至少一个关节处设置至少一个主操作器传感器。基于主操作器传感器获取对应关节的关节信息(位置或者角度)，计算主操作器的当前位姿。例如，基于正向运动学算法计算主操作器的当前位置和当前姿态。
49.在一些实施例中，主操作器包括用于控制手柄的姿态的至少一个姿态关节。确定主操作器的手柄的当前姿态包括：获得至少一个姿态关节的关节信息，并且基于至少一个姿态关节的关节信息，确定主操作器的当前姿态。主操作器包括机械臂，机械臂包括位置关节以及姿态关节。姿态关节调整主操作器的姿态，通过一个或者多个姿态关节控制主操作器达到目标姿态。位置关节调整主操作器的位置，通过一个或者多个的位置关节控制主操作器达到目标位置。主操作器传感器设置在度机械臂的姿态关节以及位置关节处，用于获取姿态关节以及位置关节对应的关节信息(位置或者角度)。根据获取到的关节信息，可以确定主操作器的手柄相对主操作器基坐标系{combx}的当前位姿。例如，主操作器可以包括7个关节，其中，关节5、关节6和关节7为姿态关节，用于调整主操作器的手柄的姿态。基于姿态关节的主操作器传感器获取的关节信息(如角度)以及正向运动学算法，计算主操作器的当前姿态。关节1、关节2、关节3为位置关节，用于调整主操作器的手柄的位置。基于位置关节的主操作器传感器获取的关节信息(如位置)以及正向运动学算法，计算主操作器的当前位置。
50.在步骤403，可以基于主操作器的当前位姿及主操作器与执行臂的末端的位姿关系，确定执行臂的末端的目标位姿。例如，建立主操作器与执行臂的末端的主从映射关系，通过遥操作主操作器以控制执行臂的末端的位姿。位姿关系包括执行臂的末端相对参考坐标系{w}的位姿与主操作器相对参考坐标系{w}的位姿之间的关系。参考坐标系{w}包括主操作器或执行臂或相机所在空间的坐标系或者世界坐标系。
51.在一些实施例中，主操作器与执行臂的末端之间的位姿关系可以包括主操作器的位姿变化量与执行臂的末端的位姿变化量之间的关系，例如相等或者成比例。确定执行臂
的末端的目标位姿包括：确定主操作器的先前位姿，确定执行臂的末端的起始位姿，并且基于主操作器的先前位姿和当前位姿以及执行臂的末端的起始位姿，确定执行臂的末端的目标位姿。主操作器的先前位姿以及当前位姿可以是主操作器的手柄相对主操作器基坐标系{combx}的位姿。执行臂的末端的起始位姿以及目标位姿可以是执行臂的末端相对执行臂基坐标系{tb}的位姿。
52.执行臂的末端的位姿可以包括执行臂的末端坐标系{wm}相对执行臂基坐标系{tb}的位姿。执行臂的基坐标系{tb}可以是执行臂所安装的基座的坐标系、执行臂的末端所穿过的鞘套的坐标系(例如，鞘套出口的坐标系)、执行臂的近端运动中心点(remote center of motion，rcm)的坐标系等。例如，执行臂的基坐标系{tb}可以设置在鞘套出口位置处，且在遥操作过程中，执行臂的基坐标系{tb}保持不变。可以对执行臂的末端的起始位姿进行坐标系变换，得到相对其他坐标系(例如，参考坐标系)的姿态。
53.在一些实施例中，可以接收主操作器的至少一个关节的先前关节信息，并且基于至少一个关节的先前关节信息，确定主操作器的先前位姿。例如，基于主操作器传感器读取主操作器在先前时间以及当前时间的关节信息，确定主操作器的手柄的先前位姿和当前位姿。基于手柄相对主操作器基坐标系{combx}的先前位置以及当前位置，确定主操作器的手柄的位置变化量。基于手柄相对主操作器基坐标系{combx}的先前姿态以及当前姿态，确定主操作器的手柄的姿态变化量。
54.在一些实施例中，可以接收上一轮检测循环获得的执行臂的末端的实际位姿，作为本轮检测循环中执行臂的末端的起始位姿。例如，在每一轮检测循环中，相机可以对执行臂的末端拍摄定位图像，通过定位图像可以识别位于执行臂的末端上的多个位姿标识和至少一个角度标识，进而确定执行臂的末端的实际位姿(在后详述)，该实际位姿可以作为下一轮检测循环中执行臂的末端的起始位姿。例如，对于第一轮检测循环，则可以采用执行臂的末端的初始位姿(例如，执行臂的零位)作为第一轮检测循环的起始位姿。
55.在一些实施例中，可以基于主操作器的先前位姿以及当前位姿，确定主操作器的位姿变化量。可以基于主操作器的位姿变化量及主操作器与执行臂的末端的位姿关系，确定执行臂的末端的位姿变化量。可以基于执行臂的末端的起始位姿以及执行臂的末端的位姿变化量，确定执行臂的末端的目标位姿。
56.位姿关系可以包括位置关系以及姿态关系。主操作器与执行臂的末端之间的位置关系可以包括主操作器的位置变化量与执行臂的末端的位置变化量之间的关系，例如相等或成比例。主操作器与执行臂的末端之间的姿态关系可以包括主操作器的姿态变化量与执行臂的末端的姿态变化量之间的关系，例如相等或成比例。
57.在一些实施例中，方法400还包括：确定主操作器的手柄相对主操作器基坐标系的当前位置，确定手柄相对主操作器基坐标系的先前位置，确定执行臂的末端相对执行臂基坐标系的起始位置，以及基于手柄相对主操作器基坐标系的先前位置和当前位置、执行臂基坐标系与主操作器基坐标系的变换关系、执行臂的末端相对执行臂基坐标系的起始位置，确定执行臂的末端相对执行臂基坐标系的目标位置。例如，基于主操作器传感器读取的主操作器在先前时间对应的关节信息确定主操作器的先前位置，基于主操作器传感器读取的主操作器在当前时间对应的关节信息确定主操作器的当前位置。基于手柄相对主操作器基坐标系{combx}的先前位置以及当前位置确定主操作器的位置变化量。基于上一轮检测
循环获得的执行臂的末端的实际位姿确定执行臂的末端的起始位置。基于主操作器的位置变化量及主操作器与执行臂的末端的位姿关系确定执行臂的末端的位置变化量。基于执行臂的末端的起始位置以及执行臂的末端的位置变化量确定执行臂的末端的目标位置。
58.在一些实施例中，方法400还包括：确定主操作器的手柄相对主操作器基坐标系的当前姿态，确定手柄相对主操作器基坐标系的先前姿态，确定执行臂的末端相对执行臂基坐标系的起始姿态，以及基于手柄相对主操作器基坐标系的先前姿态和当前姿态、执行臂基坐标系与主操作器基坐标系的变换关系、执行臂的末端相对执行臂基坐标系的起始姿态，确定执行臂的末端相对执行臂基坐标系的目标姿态。例如，基于主操作器传感器读取的主操作器在先前时间对应的关节信息确定主操作器的先前姿态，基于主操作器传感器读取的主操作器在当前时间对应的关节信息确定主操作器的当前姿态。基于手柄相对主操作器基坐标系{combx}的先前姿态以及当前姿态确定主操作器的姿态变化量。基于上一轮检测循环获得的执行臂的末端的实际位姿确定执行臂的末端的起始姿态。基于主操作器的姿态变化量及主操作器与执行臂的末端的位姿关系确定执行臂的末端的姿态变化量。基于执行臂的末端的起始姿态以及执行臂的末端的姿态变化量确定执行臂的末端的目标姿态。
59.在一些实施例中，位姿关系包括：执行臂的末端在参考坐标系{w}中的位置变化量与主操作器在参考坐标系{w}中的位置变化量成比例，可以表示为：
60.w
δp
wm
＝k
·wδphꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
61.在公式(1)中，左侧的wδp
wm
表示执行臂的末端相对参考坐标系{w}的位置变化量，右侧的wδph表示主操作器相对参考坐标系{w}的位置变化量。并且，wδp
wm
与wδph呈比例关系，比例系数为k。
62.在一些实施例中，可以基于主操作器相对参考坐标系{w}的先前位置wp
h(t0)
及当前位置wph确定主操作器的位置变化量wδph。例如，在遥操作中的t0时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对参考坐标系{w}的先前位置wp
h(t0)
。在遥操作中的t1时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对参考坐标系{w}的当前位置wph。基于主操作器在t0时刻的先前位置wp
h(t0)
以及主操作器在t1时刻的当前位置wph，得到主操作器的位置变化量wδph。在一些实施例中，t0时刻至t1时刻之间可以包括执行臂的多个控制循环，t0时刻可以是遥操作指令被触发的时刻或者多个控制循环开始的时刻，t1时刻可以是遥操作指令结束的时刻或者多个控制循环完成的时刻。
63.在一些实施例中，可以基于执行臂的末端相对参考坐标系{w}的起始位置wp
wms
及目标位置wp
wmt
，确定执行臂的末端的位置变化量wδp
wm
。在一些实施例中，执行臂的检测循环(例如，t0至t1)可以涵盖执行臂的多个控制循环。例如，执行臂的上一轮检测循环可以在t0时刻结束，执行臂的本轮检测循环可以在t0时刻开始，在t1时刻结束。在一些实施例中，可以将上一轮检测循环(例如，t0时刻)获得的执行臂的末端的实际位姿中的实际位置wp
wmr(t0)
，确定为当前检测循环中执行臂的末端相对参考坐标系{w}的起始位置wp
wms
。可以基于手柄的位置变化量wδph以及执行臂的末端相对参考坐标系{w}的起始位置wp
wms
，确定执行臂的末端相对参考坐标系{w}的目标位置wp
wmt
。
64.在公式(1)中，执行臂的末端相对参考坐标系{w}的位置变化量wδp
wm
可以由执行臂的末端相对参考坐标系{w}的目标位置wp
wmt
以及执行臂的末端(例如，在t0时刻)相对参
考坐标系{w}的起始位置wp
wms
的差值表示，如公式(2)所示，
65.w
δp
wm
＝wp
wmt-w
p
wms
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
66.在公式(1)中，主操作器相对参考坐标系{w}的位置变化量wδph可以由主操作器(例如，在t1时刻)相对参考坐标系{w}的当前位置wph以及主操作器(例如，在t0时刻)相对参考坐标系{w}的先前位置wp
h(t0)
的差值表示，如公式(3)所示，
67.w
δph＝wp
h-w
p
h(t0)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
68.在一些实施例中，在公式(1)的左右两侧分别乘以相同的矩阵
tbrw
，基于公式(1)至公式(3)得到公式(4)，
69.tbrw
(wp
wmt-w
p
wms
)＝k
·
tbrw
(wp
h-w
p
h(t0)
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
70.基于公式(4)的左侧得到公式(5)，
71.tbrw
(wp
wmt-w
p
wms
)＝
tb
p
wmt-tb
p
wms
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
72.基于公式(4)的右侧得到公式(6)，
73.k
·
tbrw
(wp
h-w
p
h(t0)
)＝k
·
tbrcombx
(
combx
p
h-combx
p
h(t0)
)
ꢀꢀꢀꢀ
(6)
74.基于公式(5)及公式(6)得到公式(7)，
75.tb
p
wmt
＝k
·
tbrcombx
(
combx
p
h-combx
p
h(t0)
)+
tb
p
wms
ꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
76.基于公式(7)，在一些实施例中，可以基于手柄相对主操作器基坐标系{combx}的先前位置
combx
p
h(t0)
和当前位置
combx
ph、执行臂的末端相对执行臂基坐标系{tb}的当前位置
tb
p
wms
、主操作器基坐标系{combx}与执行臂基坐标系{tb}的变换关系
tbrcombx
，确定执行臂的末端相对执行臂基坐标系{tb}的目标位置
tb
p
wmt
。
77.在一些实施例中，执行臂的末端在参考坐标系{w}中的姿态与主操作器在参考坐标系{w}中的姿态一致。在一些实施例中，执行臂的末端相对参考坐标系{w}的姿态变化量与主操作器相对参考坐标系{w}的姿态变化量一致，可以表示为：
78.wrwms-wmt
＝
wrh(t0)-h
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
79.在公式(8)中，左侧的
wrwms-wmt
表示执行臂的末端的姿态相对参考坐标系{w}的姿态变化量，右侧的
wrh(t0)-h
表示主操作器相对参考坐标系{w}的姿态变化量。
80.在一些实施例中，可以基于主操作器相对参考坐标系{w}的先前姿态
wrh(t0)
及当前姿态wrh确定主操作器的姿态变化量
wrh(t0)-h
。例如，在遥操作中的t0时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对参考坐标系{w}的先前姿态
wrh(t0)
。在遥操作中的t1时刻，可以基于主操作器传感器获取到的主操作器的关节信息，确定主操作器的手柄相对参考坐标系{w}的当前姿态wrh。可以基于主操作器在t0时刻的先前姿态
wrh(t0
)以及主操作器在t1时刻的当前姿态wrh，得到主操作器的姿态变化量
wrh(t0)-h
。类似地，在一些实施例中，t0时刻至t1时刻可以对应于单个检测循环，可以包括执行臂的多个控制循环，t0时刻可以是遥操作指令被触发的时刻或者检测循环开始的时刻，t1时刻可以是遥操作指令结束的时刻或者检测循环完成的时刻。
81.在一些实施例中，可以基于执行臂的末端相对参考坐标系{w}的起始姿态
wrwms
及目标姿态
wrwmt
，确定执行臂的末端的姿态变化量
wrwms-wmt
。同样地，在一些实施例中，执行臂的检测循环(例如，t0至t1)可以涵盖执行臂的多个控制循环。例如，执行臂的上一轮检测循环可以在t0时刻结束，执行臂的本轮检测循环可以在t0时刻开始，在t1时刻结束。在一些实施例中，可以将上一轮检测循环(例如，t0时刻)获得的执行臂的末端的实际位姿中的实际
姿态
wrwmr(t0)
，确定为当前检测循环中执行臂的末端相对参考坐标系{w}的起始姿态
wrwms
。可以基于手柄的姿态变化量
wrh(t0)-h
以及执行臂的末端相对参考坐标系{w}的起始姿态
wrwms
，确定执行臂的末端相对参考坐标系{w}的目标姿态
wrwmt
。
82.在公式(8)中，执行臂的末端的姿态相对参考坐标系{w}的姿态变化量
wrwms-wmt
可以基于执行臂的末端相对参考坐标系{w}的起始姿态wp
wms
以及执行臂的末端相对参考坐标系{w}的目标姿态
wrwmt
确定。主操作器相对参考坐标系{w}的姿态变化量
wrh(t0)-h
可以基于手柄(例如，在t0时刻)相对参考坐标系{w}的先前姿态
wrh(t0)
以及手柄在(例如，在t1时刻)相对参考坐标系{w}的当前姿态wrh确定。
83.具体参见公式(9)，
84.wrwmt
(
wrwms
)
t
＝wrh(
wrh(t0)
)
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
85.在一些实施例中，在公式(9)的左右两侧分别乘以相同的矩阵
tbrw
(
tbrw
)
t
，基于公式(9)得到公式(10)，
86.tbrwwrwmt
(
wrwms
)
t
(
tbrw
)
t
＝
tbrww
rh(
wrh(t0)
)
t
(
tbrw
)
t
ꢀꢀꢀꢀꢀ
(10)
87.基于公式(10)的左侧得到公式(11)，
88.tbrwwrwmt
(
wrwms
)
t
(
tbrw
)
t
＝(
tbrwwrwmt
)(
tbrwwrwms
)
t
＝
tbrwmt
(
tbrwms
)
t
ꢀꢀ
(11)
89.基于公式(10)的右侧得到公式(12)，
90.tbrww
rh(
wrh(t0)
)
t
(
tbrw
)
t
＝
tb
rh(
tbrh(t0)
)
t
＝(
tbrcombxcombx
rh)(
tbrcombxcombxrh(t0)
)
t
ꢀꢀ
(12)
91.综合公式(8)至公式(12)，可得出执行臂的末端在遥操作过程中的目标姿态
tbrwmt
表达式如公式(13)，
92.tbrwmt
＝
tbrcombx
(
combx
rh(
combxrh(t0)
)
t
)
combxrtbtbrwms
ꢀꢀꢀꢀꢀꢀ
(13)
93.基于公式(13)，在一些实施例中，可以基于手柄相对主操作器基坐标系{combx}的先前姿态
combxrh(t0)
和当前姿态
combx
rh、执行臂的末端相对执行臂基坐标系{tb}的起始姿态
wrwms
及执行臂基坐标系{tb}与主操作器基坐标系{combx}的变换关系
combxrtb
，确定执行臂的末端相对执行臂基坐标系{tb}的目标姿态
tbrwmt
。
94.在一些实施例中，执行臂基坐标系{tb}与主操作器基坐标系{combx}的变换关系
combxrtb
可以是基于执行臂基坐标系{tb}与相机坐标系{lens}的变换关系
lensrtb
、相机坐标系{lens}与显示器坐标系{screen}的变换关系
screenrlens
、显示器坐标系{screen}与主操作器基坐标系{combx}的变换关系
combxrscreen
确定。
95.在一些实施例中，主操作器与显示器的变换关系可以预先确定，例如主操作器与显示器可以分别固定设置在主控台车上，显示器坐标系{screen}与主操作器基坐标系{combx}具有预定的变换关系。一些实施例中，执行臂基坐标系{tb}与相机坐标系{lens}具有预定的变换关系。在一些实施例中，相机可以设置在视觉工具的末端，在操作者进行作业之前，视觉工具已经运动结束，执行臂基坐标系{tb}与相机坐标系{lens}的变换关系
lensrtb
不再变化。
96.在一些实施例中，显示器坐标系{screen}与相机坐标系{lens}对于视野方向的定义一致。因此执行臂的末端在显示器中的图像相对于显示器坐标系{screen}的位置变化量与执行臂的末端相对于相机坐标系{lens}的位置变化量一致。这样，当操作者握住主操作器的手柄操作时，操作者感受到的执行臂末端的执行器的图像的位姿变化与操作者感受到的主操作器的手柄的位姿变化保持预设的变换关系。
97.在一些实施例中，执行臂的末端相对参考坐标系{w}的目标位姿可以基于执行臂的末端相对执行臂基坐标系{tb}的目标位姿以及执行臂基坐标系{tb}与参考坐标系{w}的变换关系
wrtb
确定。在一些实施例中，执行臂基坐标系{tb}与参考坐标系{w}具有预定的变换关系。具体如公式(14)，
[0098][0099]
本领域技术人员可以理解，执行臂基坐标系{tb}可以作为参考坐标系{w}。
[0100]
在一些实施例中，执行臂上(例如，执行臂末端231上)分布有多个位姿标识和至少一个角度标识。例如，多个位姿标识沿周向分布在执行臂末端231上，多个角度标识沿周向分布在执行臂末端231上。多个位姿标识与多个角度标识在执行臂末端231上沿轴向并列设置。例如，多个位姿标识与多个角度标识设置在执行臂末端231的柱状部分的外表面上。
[0101]
在一些实施例中，每个角度标识与位姿标识之一具有位置关联关系。基于该位置关联关系，通过位姿标识的位置，可以确定角度标识可能分布的区域。或者，通过角度标识的位置，可以确定位姿标识可能分布的区域。位置关联关系可以根据位姿标识和角度标识的具体布置方式确定，并且可以是预先设计的。
[0102]
在一些实施例中，位置关联关系可以包括角度标识与位姿标识在轴向的对应关系。例如，位置关联关系可以包括沿轴向的偏移。基于在轴向的对应关系，在已知一个或多个位姿标识在执行臂末端上的位置的情况下，沿轴向偏移一定的距离可以确定角度标识可能存在的区域。例如，位置关联关系还可以包括沿轴向的斜对齐等等。
[0103]
在一些实施例中，多个位姿标识与多个角度标识可以设置在贴附在执行臂末端周侧的标签上。
[0104]
在一些实施例中，位姿标识可以包括位姿标识图案和位姿标识图案角点，角度标识可以包括角度标识图案和角度标识图案角点。在一些实施例中，位姿标识图案和角度标识图案可以设置在贴附在执行臂末端上的标签上，或者可以印刷在执行臂末端上，或者可以是由执行臂末端自身的物理构造形成的图案，例如，可以包括凹陷或凸起及其组合。在一些实施例中，位姿标识图案或角度标识图案可以包括以亮度、灰度、色彩等形成的图案。在一些实施例中，位姿标识图案和角度标识图案可以包括主动(例如，自发光)或被动(例如，反射光线)提供被图像采集设备探测的信息的图案。本领域技术人员可以理解，在一些实施例中，位姿标识的位姿可以由位姿标识图案角点坐标系的位姿来表示，角度标识的位姿可以由角度标识图案角点坐标系的位姿来表示。
[0105]
在一些实施例中，位姿标识图案或角度标识图案设置在执行臂末端上适于被图像采集设备采集图像的区域，例如，在工作过程中可以被图像采集设备的视场覆盖的区域或者在工作过程中不容易被干扰或遮挡的区域。
[0106]
图6示出根据一些实施例的包括多个位姿标识与多个角度标识的标签600的示意图。图7示出设置在执行臂末端周侧并形成圆筒形的标签700的示意图。可以理解，简单起见，标签600可以与标签700包括相同的位姿标识图案和角度标识图案。
[0107]
参看图6，多个位姿标识(本公开中以“〇”符号表示位姿标识图案角点)和多个角度标识(本公开中以
“△”
符号表示角度标识图案角点)并排设置。多个位姿标识图案611可
以相同或类似，并且多个位姿标识图案角点位于多个位姿标识图案611中。多个角度标识图案621-626可以不同，并且多个角度标识图案角点位于多个角度标识图案621-626中。
[0108]
每个角度标识与位姿标识之一可以具有位置关联关系。例如，如图6所示，在箭头所示的方向上，部分位姿标识(例如位姿标识图案611)和对应的角度标识(例如，角度标识图案621)沿箭头方向排列并且具有间隔距离d1。参看图7，在沿周向设置状态下，标签600变为空间构造为圆筒形的标签700，每个角度标识与位姿标识之一的位置关联关系可以包括角度标识与位姿标识在轴向(如图7中z轴正方向)的对应关系。基于在轴向的对应关系，在已知一个或多个位姿标识在执行臂末端上的位置的情况下，沿轴向偏移一定的距离(如距离d1)可以确定角度标识可能存在的区域。在一些实施例中，角度标识与位姿标识在轴向的对应关系可以通过角度标识图案角点与位姿标识图案角点在轴向的对应关系表示。在一些实施例中，基于角度标识与位姿标识在轴向的对应关系，角度标识图案角点与位姿标识图案角点之一沿z轴方向的投影重合。
[0109]
在一些实施例中，角度标识或位姿标识的绕轴角度或者滚转角可以通过角度标识图案角点或位姿标识图案角点的绕轴角度表示。角度标识图案角点相对于执行臂坐标系(例如建立在执行臂末端的末端坐标系，如图7所示的xy坐标系)的角度是已知的或预先确定的，例如图7中的角度标识图案角点r7在xy坐标系下与x轴的角度为θ。基于位置关联关系，可以得到与其位置相关联的位姿标识图案角点p7相对x轴的夹角为角度θ。应该理解，角度标识图案角点r7和位姿标识图案角点p7对应的角度θ可以称为角度标识或位姿标识的绕z轴的绕轴角度或者滚转角。在本公开中，绕轴角度或者滚转角是指绕z轴的角度。可以理解，清楚起见，图7中将角度标识图案角点r7和位姿标识图案角点p7示为分离，但它们是重合的。
[0110]
图8示出根据本公开一些实施例的实施场景800的示意图。如图8所示，执行臂840包括执行臂末端830以及远端的执行器860，多个位姿标识和角度标识可以沿周向设置在执行臂末端830上。例如，如图6所示的标签600沿周向设置在执行臂末端830上，形成圆筒形的角度标识图案带810和位姿标识图案带820。多个位姿标识图案角点分布在执行臂末端830的位姿标识图案带820的截面圆821上，多个角度标识图案角点分布在执行臂末端830的角度标识图案带810的截面圆811上。
[0111]
在一些实施例中，多个角度标识图案为不同的图案。每个角度标识图案用于指示或标识不同的绕轴角度。在一些实施例中，每个角度标识的图案与所标识的绕轴角度具有一一对应关系，基于角度标识的图案可以确定所标识的绕轴角度。
[0112]
例如，如图8所示，多个不同的角度标识图案(如图6中示出的多个角度标识图案621-626)沿圆柱形结构周向均匀分布，形成角度标识图案角点a-f。设定角度标识图案角点a对应的角度标识图案为参照图案(例如，设定角度标识图案角点a对应的角度标识图案用于标识0
°
绕轴角度)，设立平面坐标系{wm1}，则可以根据其余角度标识图案与角度标识图案角点a对应的角度标识图案的位置关系确定其余角度标识图案包含的角度标识图案角点标识的绕轴角度。例如，参看图8，当识别出角度标识图案角点b对应的角度标识图案，则根据角度标识图案角点b对应的角度标识图案与角度标识图案角点a对应的角度标识图案的位置关系，可以确定在截面圆811的二维平面坐标系内角度标识图案角点b标识的绕轴角度为60
°
。截面圆811的二维平面坐标系的原点为截面圆811的圆心，x轴方向为原点指向角度
标识图案角点a，y轴垂直于x轴。
[0113]
在一些实施例中，执行器860的位姿可以通过将执行臂的末端坐标系{wm}平移预定距离来确定。或者，执行器860的位姿可以近似地等于执行臂的末端坐标系{wm}的位姿。
[0114]
在一些实施例中，基于执行臂的末端坐标系相对参考坐标系的位姿，确定执行器860相对参考坐标系{w}的位姿。具体计算公式如下：
[0115][0116]
其中，
wrtip
为执行器相对参考坐标系的姿态，wp
tip
为执行器相对参考坐标系的位置，
wmrtip
为执行器相对执行臂的末端坐标系的姿态，
wm
p
tip
为执行器相对执行臂的末端坐标系的位置，
wrwm
为执行臂的末端坐标系相对参考坐标系的姿态，wp
wm
为执行臂的末端坐标系相对参考坐标系的位置。
[0117]
继续参看图3，在步骤303，获取定位图像。在一些实施例中，定位图像中包含执行臂的末端上的多个位姿标识和至少一个角度标识。在一些实施例中，可以从如图2所示的图像采集设备250接收定位图像。例如，控制装置220可以接收图像采集设备250主动发送的定位图像。或者，控制装置220可以向图像采集设备250发送图像请求指令，图像采集设备250响应图像请求指令向控制装置220发送定位图像。
[0118]
继续参看图3，在步骤305，在定位图像中，识别位于执行臂的末端上的多个位姿标识。例如，识别位于执行臂的末端上的多个位姿标识的示例性方法可以包括如图12和图14所示的方法。在一些实施例中，控制装置220可以通过图像处理算法识别定位图像中的部分或全部的位姿标识。在一些实施例中，图像处理算法可以包括特征识别算法，图像处理算法可以提取或识别位姿标识的特征。例如，图像处理算法可以包括角点检测算法，用于检测位姿标识图案角点。该角点检测算法可以是包括但不限于基于灰度图的角点检测、基于二值图像的角点检测、基于轮廓曲线的角点检测中的一种。例如，图像处理算法可以是颜色特征提取算法，用于检测位姿标识图案中的颜色特征。再例如，图像处理算法可以是轮廓检测算法，用于检测位姿标识图案的轮廓特征。
[0119]
在一些实施例中，控制装置可以通过识别模型识别定位图像中的部分或全部的位姿标识。
[0120]
继续参看图3，在步骤307，基于多个位姿标识，识别位于执行臂的末端上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系。在一些实施例中，在识别出多个位姿标识之后，根据位置关联关系，识别位于执行臂的末端上的角度标识。在一些实施例中，角度标识与第一位姿标识的位置关联关系可以是如图6或图7中示出的位置关联关系。在一些实施例中，第一位姿标识(例如是第一位姿标识图案或第一位姿标识图案角点)是指多个位姿标识中与角度标识具有位置关联关系的位姿标识。识别角度标识的示例性方法包括如图16所示的方法。
[0121]
继续参看图3，在步骤309，基于角度标识和多个位姿标识，确定执行臂的末端的实际位姿。获得执行臂的末端的实际位姿的示例性方法包括如图9或图10所示的方法。在一些实施例中，可以基于角度标识、第一位姿标识和多个位姿标识，获得执行臂的末端相对参考坐标系的位姿，作为执行臂的末端的实际位姿。
[0122]
在一些实施例中，方法300还包括：基于角度标识和多个位姿标识，确定执行臂的末端坐标系和位姿标识坐标系的变换关系。在一些实施例中，根据执行臂的末端坐标系和位姿标识坐标系的变换关系，可以将位姿标识坐标系中的三维坐标转换为在执行臂的末端坐标系中对应的三维坐标。在一些实施例中，根据执行臂的末端坐标系和位姿标识坐标系的变换关系和位姿标识坐标系相对参考坐标系的位姿，得到执行臂的末端坐标系相对参考坐标系的位姿。
[0123]
在一些实施例中，执行臂的末端坐标系和位姿标识坐标系的变换关系可以包括位姿标识坐标系相对执行臂的末端坐标系的滚转角。在一些实施例中，可以基于角度标识和第一位姿标识，确定位姿标识坐标系相对执行臂的末端坐标系的滚转角。应该理解，位姿标识坐标系相对执行臂的末端坐标系的滚转角可以是位姿标识坐标系绕执行臂的末端坐标系的z轴的转动的角度。
[0124]
在一些实施例中，执行臂的末端坐标系可以是基于多个位姿标识或多个角度标识，在物体上设定的固定坐标系。在一些实施例中，执行臂的末端坐标系的z轴平行于执行臂末端的轴向，执行臂的末端坐标系的xy平面与多个位姿标识图案角点处于同一平面，或者与多个角度标识图案角点处于同一平面。
[0125]
在一些实施例中，可以确定位姿标识坐标系，以方便确定多个位姿标识的位置。在一些实施例中，位姿标识的位置可以通过位姿标识图案角点的位置表示。在一些实施例中，位姿标识坐标系的z轴平行于执行臂末端的轴向或与轴向重合，位姿标识坐标系的xy平面与多个位姿标识图案角点处于同一平面。
[0126]
示例性地，参看图8，执行臂的末端坐标系{wm}≡[x
wm y
wm z
wm
]
t
的原点为多个位姿标识图案角点所在截面圆821的圆心，x轴方向为原点指向其中一个位姿标识图案角点，z轴的方向平行执行臂末端830的轴向，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轴的方向平行物体执行臂末端830的轴向，y轴垂直于xz平面。继续参看图8，执行臂的末端坐标系{wm}的z轴与位姿标识坐标系{wm0}的z轴重合。执行臂的末端坐标系{wm}相对位姿标识坐标系{wm0}的变换关系可以通过位姿标识坐标系{wm0}相对执行臂的末端坐标系{wm}的滚转角α0确定。滚转角α0可以是指位姿标识坐标系{wm0}相对执行臂的末端坐标系{wm}绕z轴的转动角度。
[0127]
在一些实施例中，参看图8，滚转角α0通过如下公式计算：
[0128]
α0＝α
1-α2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0129]
其中α1为第一绕轴角度，α2为第二绕轴角度。第一绕轴角度为角度标识图案角点(例如，角度标识图案角点r8)在执行臂的末端坐标系中标识的绕轴角度。第二绕轴角度为第一位姿标识图案角点(例如，位姿标识图案角点p8)在位姿标识坐标系中标识的绕轴角度。
[0130]
图9示出根据本公开一些实施例的确定执行臂的末端的实际位姿的方法900的流程图。如图9所示，该方法900中的部分或全部步骤可以由控制装置(例如，图1所示的控制装
置120，图2所示的控制装置220，或图18所示的控制装置1820)来执行。方法900中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法900可以由机器人系统(例如，图1所示的机器人系统100或图18所示的机器人系统1800)执行。在一些实施例中，方法900可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的控制装置1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0131]
参看图9，在步骤901，基于角度标识和多个位姿标识，确定位姿标识坐标系相对执行臂的末端坐标系的滚转角。在一些实施例中，确定角度标识在执行臂的末端坐标系中标识的第一绕轴角度。确定第一位姿标识在位姿标识坐标系中标识的第二绕轴角度。基于第一绕轴角度和第二绕轴角度，确定位姿标识坐标系相对执行臂的末端坐标系的滚转角。在一些实施例中，位姿标识坐标系相对执行臂的末端坐标系的滚转角可以基于上述公式(16)确定。
[0132]
在步骤903，基于多个位姿标识，确定位姿标识坐标系相对参考坐标系的位姿。位姿标识在相应坐标系中的坐标可以通过位姿标识图案角点在相应坐标系中的坐标表示。例如，位姿标识在定位图像中的二维坐标和在位姿标识坐标系中的三维坐标可以通过位姿标识图案角点的坐标表示。在一些实施例中，基于多个位姿标识图案角点在定位图像中的二维坐标和多个位姿标识图案角点在位姿标识坐标系中的三维坐标，确定位姿标识坐标系相对参考坐标系的位姿。在一些实施例中，基于多个位姿标识图案角点在定位图像中的二维坐标、多个位姿标识图案角点在位姿标识坐标系中的三维坐标和相机坐标系相对参考坐标系的变换关系，确定位姿标识坐标系相对参考坐标系的位姿。
[0133]
在一些实施例中，基于多个位姿标识的分布，确定多个位姿标识图案角点在位姿标识坐标系中的三维坐标。例如，参看图11，每个位姿标识图案角点均位于在截面圆1122的圆周上，截面圆1122的圆心与半径r均是已知的。设定截面圆1122的圆心为位姿标识坐标系的原点，xy平面位于截面圆1122上，x轴可以指定为由原点出发指向任一已确定的位姿标识图案角点(例如，位姿标识图案角点p11)，进而可基于多个位姿标识的分布，确定每个位姿标识图案角点在位姿标识坐标系中的三维坐标。例如，如图11所示，位姿标识图案角点p11在位姿标识坐标系的三维坐标为(r,0,0)，则其余位姿标识图案角点在位姿标识坐标系中的三维坐标可以根据以下公式计算：
[0134]cm
＝[r
·
cos((m-1)
·
χ) r
·
sin((m-1)
·
χ) 0]
t
ꢀꢀꢀꢀꢀꢀꢀ
(17)
[0135]
其中，cm为以位姿标识图案角点p11作为起点，第m个位姿标识图案角点在位姿标识坐标系中的三维坐标；χ为相邻的位姿标识图案角点之间的绕轴夹角。
[0136]
在一些实施例中，相机坐标系相对参考坐标系的变换关系可以是已知的。例如，参考坐标系为世界坐标系，相机坐标系相对世界坐标系的变换关系可以根据相机摆放的位姿确定。另一些实施例中，根据实际的需求，参考坐标系也可以是相机坐标系本身。
[0137]
在一些实施例中，基于相机成像原理与投影模型，基于多个位姿标识图案角点在定位图像中的二维坐标和多个位姿标识图案角点在位姿标识坐标系中的三维坐标，确定位姿标识坐标系相对相机坐标系的位姿。基于位姿标识坐标系相对相机坐标系的位姿和相机坐标系相对参考坐标系的变换关系，可以得到位姿标识坐标系相对参考坐标系的位姿。在一些实施例中，还可以考虑相机的内参。例如，相机的内参可以是如图2所示的图像采集设
备250的相机内参。相机的内参可以是已知的或者经过标定而得到的。
[0138]
在一些实施例中，相机坐标系可以理解为以相机原点建立的坐标系。例如，以相机的光心为原点建立的坐标系或者以相机的镜头中心为原点建立的坐标系。当相机为双目相机时，相机坐标系的原点可以是相机左镜头的中心，或者右镜头的中心，或者左右镜头中心连线上的任意一点(例如该连线的中点)。
[0139]
参看图9，在步骤905，基于位姿标识坐标系相对执行臂的末端坐标系的滚转角和位姿标识坐标系相对参考坐标系的位姿，确定执行臂的末端坐标系相对参考坐标系的位姿，作为执行臂的末端的实际位姿。
[0140]
例如，执行臂的末端坐标系相对参考坐标系的位姿具体如下：
[0141]
wrwm
＝
wrwm0
·
rotz(α0)
[0142]wp
wm
＝wp
wm0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0143]
其中，
wrwm
为执行臂的末端坐标系相对参考坐标系的姿态，wp
wm
为执行臂的末端坐标系相对参考坐标系的位置，
wrwm0
为位姿坐标系相对参考坐标系的姿态，wp
wm0
为位姿坐标系相对参考坐标系的位置，rotz(α0)表示绕执行臂的末端坐标系的z轴转动滚转角α0。
[0144]
在一些实施例中，执行臂的末端坐标系相对参考坐标系的位姿具体计算公式如下：
[0145]
wrwm
＝
wrlenslensrwm0wm0rwm
[0146]wp
wm
＝
wrlens
(
lensrwm0wm0
p
wm
+
lens
p
wm0
)+wp
lens
ꢀꢀꢀ
(19)
[0147]
其中，
wrlens
为相机坐标系相对参考坐标系的姿态，wp
lens
为相机坐标系相对参考坐标系的位置，
lensrwm0
为位姿标识坐标系相对相机坐标系的姿态，
lens
p
wm0
为位姿标识坐标系相对相机坐标系的位置，
wm0rwm
为执行臂的末端坐标系相对位姿标识坐标系的姿态，
wm0
p
wm
为执行臂的末端坐标系相对位姿标识坐标系的位置。
[0148]
图10示出根据本公开另一些实施例的确定执行臂的末端的实际位姿的方法1000的流程图。方法1000可以是图9的方法900的替换实施例。如图10所示，该方法1000中的部分或全部步骤可以由数据处理装置(例如，图1所示的控制装置120，图2所示的控制装置220，或图18所示的控制装置1820)来执行。方法1000中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1000可以由机器人系统(例如，图1所示的机器人系统100，或图18所示的机器人系统1800)执行。在一些实施例中，方法1000可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的控制装置1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0149]
参看图10，在步骤1001，基于位姿标识坐标系相对执行臂的末端坐标系的滚转角和多个位姿标识在位姿标识坐标系中的三维坐标，确定多个位姿标识在执行臂的末端坐标系中的三维坐标。可以理解，已知位姿标识坐标系相对执行臂的末端坐标系的滚转角，可以根据坐标变换将多个位姿标识图案角点在位姿标识坐标系中的三维坐标变换为在执行臂的末端坐标系中的三维坐标。
[0150]
在步骤1003，基于多个位姿标识在定位图像中的二维坐标和多个位姿标识在执行臂的末端坐标系中的三维坐标，确定执行臂的末端坐标系相对参考坐标系的位姿，作为执行臂的末端的实际位姿。在一些实施例中，步骤1003可以与方法900中步骤903和905类似地实现。
[0151]
图12示出根据本公开一些实施例的用于识别位姿标识的方法1200的流程图。如图12所示，该方法1200中的部分或全部步骤可以由数据处理装置(例如，图1所示的控制装置120，图2所示的控制装置220，或图18所示的控制装置1820)来执行。方法1200中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1200可以由机器人系统(例如，图1所示的机器人系统100，或图18所示的机器人系统1800)执行。在一些实施例中，方法1200可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的控制装置1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0152]
参看图12，在步骤1201，从定位图像中确定多个候选位姿标识。在一些实施例中，候选位姿标识可以通过候选位姿标识图案角点表示。在一些实施例中，候选位姿标识图案角点可以是指经过对定位图像进行初步处理或者初步识别得到的可能的位姿标识图案角点。在一些实施例中，可以先从定位图像中截取roi(region of interest，感兴趣区域)，从roi中确定多个候选位姿标识。其中，roi可以是定位图像的全图像，也可以是部分区域。例如，可以基于上一帧图像(例如，上一图像处理周期的定位图像)确定的多个位姿标识图案角点一定范围内的区域截取当前帧的roi。对非第一帧的定位图像，roi可以为上一图像处理周期的多个位姿标识图案角点的坐标构成的虚点为中心的一定距离范围内的区域。一定距离范围可以是位姿标识图案角点平均间隔距离的固定倍数，例如两倍。应理解，预定倍数还可以是上一图像处理周期中多个候选位姿标识图案角点平均间隔距离的可变倍数。
[0153]
在一些实施例中，方法1200可以包括：确定定位图像中各像素点的角点似然值(corner likelihood,cl)。在一些实施例中，像素点的角点似然值可以是表征像素点作为特征点(例如，角点)的可能性的数值。在一些实施例中，在计算各像素点的角点似然值之前可以对定位图像进行预处理，之后确定预处理之后的图像中各像素点的角点似然值。图像的预处理例如可以包括：图像灰度化、图像去噪、图像增强中的至少一种。
[0154]
例如，图像预处理可以包括：从定位图像中截取roi，将roi转为相应的灰度图像。
[0155]
在一些实施例中，确定roi中的每个像素点的角点似然值的方式例如可以包括对roi范围内每一个像素点进行卷积操作，得到每个像素点的一阶和/或二阶导数。利用roi范围内每个像素点的一阶和/或二阶导数求出每个像素点的角点似然值。示例性地，各像素点的角点似然值可以根据如下公式计算：
[0156]
cl＝max(c
xy
,c
45
)
[0157]cxy
＝τ2·
|i
xy
|-1.5
·
τ
·
(|i
45
|+|i
n45
|)
[0158]c45
＝τ2·
|i
45_45
|-1.5
·
τ
·
(|i
x
|+|iy|)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0159]
其中，τ为设定的常数，例如设定为2；i
x
、i
45
、iy、i
n45
分别是像素点在0、π/4、π/2、-π/4四个方向的一阶导数；i
xy
和i
45_45
分别是像素点在0,π/2和π/4,-π/4方向的二阶导数。
[0160]
在一些实施例中，将roi划分成多个子图像。例如，可以采用非极大抑制法在一个roi范围中平均分割出多个子图像。在一些实施例中，可以将roi平均分割成5
×
5像素的多个子图像。上述实施例为示例性的，并非限制性的，应当理解，还可以将定位图像或roi分割成其他尺寸大小的多个子图像，例如，分割成9
×
9像素的多个子图像。可以确定每个子图像中的cl值最大的像素点，将每个子图像中的cl值最大的像素点与第一阈值进行比较，确定cl值大于第一阈值的像素点集合。在一些实施例中，第一阈值可以设定为0.06。应当理解，
第一阈值还可以设定为其他值。在一些实施例中，cl值大于第一阈值的像素点可以作为候选位姿标识图案角点。
[0161]
参看图12，在步骤1203，基于位姿图案匹配模板，从多个候选位姿标识中识别初始位姿标识。在一些实施例中，通过位姿图案匹配模板与候选位姿标识图案角点之一处的图像进行匹配，确定达到预设位姿图案匹配度标准的候选位姿标识图案角点为初始位姿标识图案角点。
[0162]
在一些实施例中，位姿图案匹配模板与位姿标识图案角点附近区域的图像具有相同或相似的特征。若位姿图案匹配模板与候选位姿标识图案角点附近区域的图像的匹配度达到预设位姿图案匹配度标准(例如，匹配度高于阈值)，则可以认为候选位姿标识图案角点附近区域的图案与位姿图案匹配模板具有相同或相似的特征，进而可认为当前的候选位姿标识图案角点为位姿标识图案角点。
[0163]
在一些实施例中，确定像素点集合中cl值最大的像素点，作为待匹配候选位姿标识图案角点。例如，可以将该像素点集合中的所有像素点让cl值从大到小的顺序排序，并将cl值最大的像素点作为待匹配候选位姿标识图案角点。在确定了待匹配候选位姿标识图案角点之后，使用位姿图案匹配模板与待匹配候选位姿标识图案角点处的图案进行匹配，如果达到了预设位姿图案匹配度标准，则确定待匹配候选位姿标识图案角点为识别出的初始位姿标识图案角点。如果待匹配候选位姿标识图案角点未达到预设的匹配度标准，则选择次级cl值的像素点(cl值第二大的像素点)作为待匹配候选位姿标识图案角点，使用位姿图案匹配模板与该候选位姿标识图案角点处的图像进行匹配，依次类推，直至识别出初始位姿标识图案角点。
[0164]
在一些实施例中，位姿标识图案可以为黑白相间的棋盘格图形，因此位姿图案匹配模板可以为相同的棋盘格图形，利用位姿图案匹配模板的灰度分布gm与候选位姿标识图案角点对应的像素点的像素邻域灰度分布g
image
间的相关性系数(correlation coefficient,cc)来进行匹配。像素点的像素邻域灰度分布g
image
为以该像素点为中心一定范围内(例如，10
×
10像素)像的像素的灰度分布。具体公式如下：
[0165][0166]
其中，var为方差函数，cov为协方差函数。在一些实施例中，当cc值小于0.8时，像素领域内的灰度分布与位姿图案匹配模板相关性较低，则判定该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点，否则认为该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点。
[0167]
在一些实施例中，方法1200包括：确定候选位姿标识图案角点的边缘方向。例如，如图13所示，图13中包括一个位姿标识图案1311，候选位姿标识图案角点为图13中的角点p13，那么该角点p13的边缘方向可以是指形成角点p13的边缘的方向，如图13中虚线箭头示意的方向。
[0168]
在一些实施例中，边缘方向可以通过对以候选位姿标识图案角点为中心的一定范围邻域(例如10
×
10像素)的每个像素在平面坐标系的x方向和y方向的一阶导数值(i
x
和iy)确定。例如，边缘方向可以通过以下公式进行计算：
[0169][0170]
其中，一阶导数(i
x
和iy)可以通过对一定范围邻域范围内每一个像素点进行卷积操作得到。一些实施例中，通过对每个范围邻域内的像素点的边缘方向i
angle
和对应的权重i
weight
进行聚类计算获得该像素点的边缘方向，选择权重i
weight
占比最大的类对应的i
angle
作为边缘方向。需要说明的是，如果存在多个边缘方向，则选择权重i
weight
占比最大的多个类对应的i
angle
作为边缘方向。
[0171]
在一些实施例中，聚类计算所用的方法可以是k均值方法、birch(balanced iterative reducing and clustering using hierarchies，基于层次结构的平衡迭代聚类方法)方法、dbscan(density-based spatial clustering of applications with noise，具有噪声的基于密度的聚类方法)方法、gmm(gaussian mixed model，高斯混合模型)方法中的任意一种。
[0172]
在一些实施例中，方法1200包括：根据边缘方向旋转位姿图案匹配模板。根据边缘方向旋转位姿图案匹配模板，可以将位姿图案匹配模板与候选位姿标识图案角点处的图像对齐。
[0173]
候选位姿标识图案角点的边缘方向可以用于确定该候选标识图案角点处的图像在定位图像中的设置方向。在一些实施例中，根据边缘方向旋转位姿图案匹配模板，可以将位姿图案匹配模板调整至与候选位姿标识图案角点处的图像方向相同或接近相同以便于进行图像匹配。
[0174]
参看图12，在步骤1205，以初始位姿标识作为起点，搜索位姿标识。
[0175]
例如，图14示出根据本公开一些实施例的用于搜索位姿标识的方法1400的流程图。如图14所示，该方法1400中的部分或全部步骤可以由数据处理装置(例如，图1所示的控制装置120，图2所示的控制装置220，或图18所示的控制装置1820)来执行。方法1400中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1400可以由机器人系统(例如，图1所示的机器人系统100，或图18所示的机器人系统1800)执行。在一些实施例中，方法1400可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的控制装置1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0176]
参看图14，在步骤1401，以初始位姿标识作为起点，确定第二个位姿标识。在一些实施例中，以初始位姿标识图案角点作为起点，在设定的搜索方向上搜索第二个位姿标识图案角点。在一些实施例中，设定的搜索方向可以包括：初始位姿标识图案角点的正前方(对应0
°
角度方向)、正后方(对应180
°
角度方向)、正上方(90
°
角度方向)、正下方(-90
°
角度方向)和斜向(例如
±
45
°
角度方向)中的至少一个方向。
[0177]
在一些实施例中，设定的搜索方向为n个，例如在8个方向进行搜索，每个搜索方向vsn可以根据如下公式计算：
[0178]vsn
＝[cos(n
·
π/4)sin(n
·
π/4)],(n＝1,2,
…
,8)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(23)
[0179]
在一些实施例中，当前步骤中设定的搜索方向可以根据上一帧确定的多个位姿标识图案角点中相邻位姿标识图案角点之间的偏差角度确定。示例性地，预定的搜索方向可以根据如下公式计算：
[0180][0181][0182]
其中，(xj,yj)为上一帧(或者上个图像处理周期)确定的多个位姿标识图案角点的二维坐标；n
last
为上一帧确定的多个位姿标识图案角点的个数；v
s1
为第一个设定的搜索方向；v
s2
为第二个设定的搜索方向。
[0183]
在一些实施例中，如图15所示，以初始位姿标识图案角点p151的坐标位置作为搜索起点，在设定的搜索方向上搜索第二个位姿标识图案角点p152的坐标位置具体可以包括：以初始位姿标识图案角点p151的坐标位置作为搜索起点，通过搜索框(例如，图15中的虚线框)以一定的搜索步长在设定的搜索方向v151上搜索位姿标识图案角点。若搜索框内存在至少一个候选位姿标识图案角点，则优先选择搜索框内角点似然值最大的候选位姿标识图案角点为第二个位姿标识图案角点p152。在搜索框限制在合适的大小情况下，以初始位姿标识图案角点p151的坐标位置作为搜索起点进行第二位姿标识图案角点p152搜索时，搜索框内出现的候选位姿标识图案角点中角点似然值最大的候选位姿标识图案角点为位姿标识图案角点的可能性较大。因此，可以认为搜索框内角点似然值最大的候选位姿标识图案角点为第二个位姿标识图案角点p152，以便提高数据处理速度。其他实施方式中，为了提高位姿标识图案角点识别的准确度，选择搜索框内出现的候选位姿标识图案角点中角点似然值最大的候选位姿标识图案角点进行角点的识别，以确定该角点似然值最大的候选位姿标识图案角点是否为位姿标识图案角点。例如，对位姿图案匹配模板与该角点似然值最大的候选位姿标识图案角点处一定范围内的图像进行匹配，满足预设位姿图案匹配度标准的候选位姿标识图案角点可以认为是搜索到的第二个位姿标识图案角点p152。
[0184]
在一些实施例中，继续参看图15，搜索框的大小可以逐步增大，从而搜索范围逐步增大。搜索步长可与搜索框的边长同步变化。其他实施方式中，搜索框的大小也可以为固定的大小。
[0185]
在一些实施例中，位姿标识图案可以为黑白相间的棋盘格图形，可以采用公式(21)中的相关性系数cc来进行图案匹配。如果cc大于阈值，则认为该角点似然值最大的候选位姿标识图案角点是位姿标识图案角点，记为第二个位姿标识图案角点。
[0186]
参看图14，在步骤1403，基于初始位姿标识、第二个位姿标识，确定搜索方向。在一些实施例中，搜索方向包括：第一搜索方向和第二搜索方向。第一搜索方向可以是以初始位姿标识图案角点的坐标位置为起点，且远离第二位姿标识图案角点的方向。第二搜索方向可以是以第二位姿标识图案角点的坐标位置为起点，且远离第一位姿标识图案角点的方向。例如，图15中所示的搜索方向v152。
[0187]
在步骤1405，以初始位姿标识或第二个位姿标识作为起点，在搜索方向上搜索位姿标识。在一些实施例中，若以第一位姿标识图案角点为新的起点，则可以以上述实施例中的第一搜索方向作为搜索方向进行位姿标识图案角点的搜索。若以第二位姿标识图案角点为新的搜索起点，则可以以上述实施例中的第二搜索方向作为搜索方向进行位姿标识图案角点的搜索。在一些实施例中，搜索新的位姿标识图案角点(例如，图15中的第三位姿标识
图案角点p153)可以与步骤1401类似地执行。在一些实施例中，搜索步长可以是初始位姿标识图案角点和第二个位姿标识图案角点之间的距离l1。
[0188]
在一些实施例中，响应于搜索到的位姿标识图案角点数量大于或等于位姿标识图案角点数量阈值，停止对位姿标识图案角点的搜索。例如，当搜索到(识别到)四个位姿标识图案角点，停止对位姿标识图案角点的搜索。
[0189]
在一些实施例中，响应于搜索的距离大于第n-1个位姿标识图案角点和第n-2个位姿标识图案角点的距离的设定倍数，停止对第n个位姿标识图案角点的搜索，其中n≥3。例如，搜索的结束条件可以是搜索的距离大于两倍的前两个位姿标识图案角点的距离。这样，搜索第三个位姿标识图案角点的最大搜索距离是初始位姿标识图案角点和第二个位姿标识图案角点的距离的两倍。若达到该搜索距离还未搜索到位姿标识图案角点，则认为未找到第三个位姿标识图案角点并且搜索结束。
[0190]
在一些实施例中，若搜索到的位姿标识图案角点的总数大于或等于设定的阈值(例如，设定的阈值为4)，则认为成功识别到了足够的位姿标识图案角点。若找到的位姿标识图案角点总数小于设定的数值，则认为上述步骤中基于初始位姿标识图案角点的搜索不成功。搜索不成功的情况下，从候选位姿标识图案角点中重新确定新的初始位姿标识图案角点，之后基于该重新确定的初始位姿标识图案角点为搜索起点，搜索其余的位姿标识图案角点。与方法1200类似，可以重新确定新的初始位姿标识图案角点，并且与方法1400类似，可以以新的位姿标识图案角点为搜索起点，搜索其余的位姿标识图案角点。
[0191]
在一些实施例中，在搜索到或者识别到位姿标识图案角点之后，还可以对已确定的位姿标识图案角点进行亚像素定位，以提高位姿标识图案角点的位置精度。
[0192]
在一些实施例中，可以对像素点的cl值基于模型进行拟合，以确定经亚像素定位后的位姿标识图案角点的坐标。例如，roi中每个像素点的cl值的拟合函数可以为二次曲面函数，该函数的极值点为亚像素点。拟合函数可以如下：
[0193]
s(x,y)＝ax2+by2+cx+dy+exy+f
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0194][0195]
其中，s(x，y)为每个roi中的所有像素点的cl值拟合函数，a、b、c、d、e、f为系数；xc为位姿标识的x坐标，yc为位姿标识的y坐标。
[0196]
图16示出根据本公开一些实施例的识别角度标识的方法1600的流程图。如图16所示，该方法1600中的部分或全部步骤可以由数据处理装置(例如，图1所示的控制装置120，图2所示的控制装置220，或图18所示的控制装置1820)来执行。方法1600中的部分或全部步骤可以由软件、固件和/或硬件来实现。在一些实施例中，方法1600可以由机器人系统(例如，图1所示的机器人系统100，或图18所示的机器人系统1800)执行。在一些实施例中，方法1600可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图18所示的控制装置1820)读取并执行。在一些实施例中，这些指令可以存储在计算机可读介质上。
[0197]
参看图16，在步骤1601，基于多个位姿标识在定位图像中的二维坐标和多个位姿标识在位姿标识坐标系中的三维坐标，确定成像变换关系。在一些实施例中，位姿标识坐标系可以是在方法300所示实施例中详述的位姿标识坐标系。例如，位姿标识坐标系为如图8
所示。在一些实施例中，成像变换关系可以是指位姿标识坐标系中的三维坐标与定位图像中的二维坐标的变换关系。应该理解，基于成像变换关系，也可以将定位图像中的二维坐标变换为位姿标识坐标系中的三维坐标。在一些实施例中，多个位姿标识在位姿标识坐标系中的三维坐标可以基于公式(17)确定。在一些实施例中，多个位姿标识的数量可以大于或等于4个，例如，可以基于4个位姿标识在定位图像中的二维坐标和在位姿标识坐标系中对应的4个三维坐标，得到成像变换关系。
[0198]
参看图16，在步骤1603，基于成像变换关系、多个位姿标识在位姿标识坐标系中的三维坐标和位置关联关系，在定位图像中确定多个角度标识候选区域。在一些实施例中，角度标识候选区域可以表示角度标识图案的候选区域。在一些实施例中，基于多个位姿标识图案角点在位姿标识坐标系中的三维坐标和位置关联关系，在位姿标识坐标系中确定多个角度标识图案角点候选三维坐标。例如，根据多个位姿标识图案角点在位姿标识坐标系中的三维坐标，沿轴向偏移一定的距离可以确定多个在位姿标识坐标系中的三维坐标。这些三维坐标由多个角度标识图案角点候选三维坐标表示。例如，参看图6，位置关联关系为角度标识和对应的位姿标识沿位姿标识坐标系的z轴间隔一定距离。在确定位姿标识图案角点的位置的前提下，沿z轴的正方向或反方向移动一定距离得到的位置可以认为是角度标识图案角点在位姿标识坐标系下的候选位置。
[0199]
在一些实施例中，基于成像变换关系和多个角度标识图案角点候选三维坐标，在定位图像中确定多个角度标识候选区域。例如，基于成像变换关系和多个角度标识图案角点候选三维坐标，在定位图像中得到多个角度标识图案角点候选二维坐标。在一些实施例中，基于多个角度标识图案角点候选二维坐标，确定多个角度标识图案候选区域。例如，以每个角度标识图案角点候选二维坐标为中心，在定位图像中确定一定范围大小(例如，5
×
5像素、10
×
10像素等)的区域作为角度标识候选区域。在一些实施例中，一定范围大小的区域大于或等于角度标识图案成像后的大小。角度标识图案成像后的大小可以基于角度标识图案的实际大小和成像变换关系得到。
[0200]
参看图16，在步骤1605，从多个角度标识候选区域，识别角度标识。在一些实施例中，角度标识包括角度标识图案和角度标识图案角点。在一些实施例中，方法1600可以包括确定每个角度标识候选区域中角点似然值最大的像素点以形成像素集合。在一些实施例中，像素点的角点似然值可以是在执行方法1200时计算的，也可以是基于公式(20)重新计算得到。方法1600还包括确定像素集合中角点似然值最大的像素点对应的角度标识候选区域作为待识别的角度标识候选区域。方法1600还包括使用多个角度图案匹配模板分别与待识别的角度标识候选区域进行匹配，以识别角度标识。在一些实施例中，角度标识图案为图形特征不同的图案。多个角度图案匹配模板可以是指与多个角度标识图案分别对应的具有相同或相似图形特征的标准角度图案模板。在一些实施例中，通过确定多个角度标识候选区域，可以在多个角度标识候选区域进行角度标识的识别，避免了在整个图像范围内进行角度标识的识别，提高了数据处理的速度。
[0201]
在一些实施例中，可以使用平方差匹配法、归一化平方差匹配法、相关匹配法、归一化相关匹配法、相关系数匹配法和归一化相关系数匹配法中的任意一种模板匹配算法对角度图案匹配模板与角度标识候选区域进行匹配运算。
[0202]
在一些实施例中，由于角度图案匹配模板与角度标识图案具有相同或相似的图形
特征，因此角度标识的图案信息可以包括对应的角度图案匹配模板的图案信息。例如，角度图案匹配模板的形状、图像中可被识别的特征等。在一些实施例中，每个角度图案匹配模板与对应的角度标识图案所标识的绕轴角度具有一一对应关系。基于特定的角度图案匹配模板或者识别出的角度标识对应的角度标识图案的图案信息，确定第一绕轴角度。
[0203]
在一些实施例中，方法1600可以包括响应于匹配失败，确定像素集合的剩余像素点中角点似然值最大的像素点对应的角度标识候选区域作为待识别的角度标识候选区域。在一些实施例中，在确定新的待识别的角度标识候选区域之后，使用多个角度图案匹配模板分别与待识别的角度标识候选区域进行匹配，以识别角度标识。
[0204]
在一些实施例中，基于识别出的角度标识所在的角度标识候选区域，确定与角度标识具有位置关联关系的第一位姿标识。在一些实施例中，多个角度标识候选区域分别对应识别出的多个位姿标识图案角点中的至少一个，在确定识别出的角度标识所在的角度标识候选区域之后，可以基于多个角度标识候选区域与多个位姿标识图案角点的对应关系，确定第一位姿标识图案角点。
[0205]
继续参看图3，在步骤311，响应于目标位姿与实际位姿满足误差检测条件，生成与故障相关的控制信号。在获得执行臂的末端的目标位姿与实际位姿之后，控制装置确定执行臂的末端的位姿误差，以判断执行臂是否正确地进行到了操作者期望的位置与姿态，进而判断机器人系统是否出现故障。在一些实施例中，当执行臂的目标位姿与实际位姿满足误差检测条件(例如大于等于误差阈值)时，控制装置判断执行臂没有正确移动到与主操作器对应的位置与姿态，发出与故障相关的控制信号。例如，控制装置可以发出第一报警信号，该第一报警信号指示执行臂的控制发生故障。
[0206]
在一些实施例中，控制装置可以在遥操作中实时获取执行臂的多组目标位姿和实际位姿，基于该多组目标位姿和实际位姿综合判断执行臂的运行状况。在一些实施例中，控制装置可以以预定周期确定执行臂末端的目标位姿和实际位姿，通过多个检测循环对执行臂进行误差检测，并应用数理统计方法对多组误差进行分析，在满足误差检测条件时发出与故障相关的控制信号。
[0207]
例如，在第k次误差检测循环中，位姿差可以表示如下：
[0208][0209]
其中，为第k次误差检测循环时执行臂的位置差，为第k次误差检测循环时执行臂的角度差，p
tk
为第k次误差检测循环时执行臂的目标位置，r
tk
为第k次误差检测循环时执行臂的目标姿态，为第k次误差检测循环时执行臂的实际位置，r
rk
为第k次误差检测循环时执行臂的实际姿态，表示与之间的转角。
[0210]
在一些实施例中，控制装置可以将多个检测循环中获得的误差储存至存储器，并对这些误差进行累计，当误差的累计值满足误差检测条件(例如，超过阈值)时，发出与故障相关的控制信号。
[0211]
在一些实施例中，方法300还包括响应于目标位姿与实际位姿满足误差检测条件，接收用于驱动执行臂的至少一个驱动装置的状态信息，以及响应于状态信息与至少一个驱动装置的驱动信息满足故障检测条件，发出第二报警信号，第二报警信号指示执行臂的驱动装置发生故障。
[0212]
在一些实施例中，驱动装置设置有驱动装置传感器，驱动装置传感器与驱动装置耦合并且用于获得驱动装置的状态信息。例如，驱动装置可以包括至少一个驱动电机，驱动装置传感器可以包括电位计或者编码器，驱动装置传感器与驱动电机耦合以记录和输出电机的状态信息。控制装置基于执行臂的末端的目标位姿向至少一个驱动装置发送驱动信息，并通过驱动装置传感器接收用于驱动执行臂的至少一个驱动装置的状态信息，在状态信息与驱动信息满足故障检测条件(例如大于等于误差阈值)时，发出用于指示驱动执行臂的至少一个驱动装置发生故障的第二报警信号。
[0213]
本公开一些实施例中，本公开还提供了一种计算机设备，计算机设备包括存储器和处理器。存储器可以用于存储有至少一条指令，处理器与存储器耦合，用于执行至少一条指令以执行本公开的方法中的部分步骤或全部步骤，如图3、图4、图9、图10、图12、图14和图16中公开的方法中的部分步骤或全部步骤。
[0214]
图17示出根据本公开一些实施例的计算机设备1700的示意框图。参看图17，该计算机设备1700可以包括中央处理单元(cpu)1701、包括随机存取存储器(ram)1702和只读存储器(rom)1703的系统存储器1704，以及连接各部件的系统总线1705。计算机设备1700还可以包括输入/输出系统，和用于存储操作系统1713、应用程序1714和其他程序模块1715的大容量存储设备1707。输入/输出设备包括主要由显示器1708和输入设备1709组成的输入/输出控制器1710。
[0215]
大容量存储设备1707通过连接到系统总线1705的大容量存储控制器(未示出)连接到中央处理单元1701。大容量存储设备1707或者计算机可读介质为计算机设备提供非易失性存储。大容量存储设备1707可以包括诸如硬盘或者光盘只读存储器(compact disc read-only memory，cd-rom)驱动器之类的计算机可读介质(未示出)。
[0216]
不失一般性，计算机可读介质可以包括计算机存储介质和通信介质。计算机存储介质包括以用于存储诸如计算机可读指令、数据结构、程序模块或其他数据等信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动介质。计算机存储介质包括ram、rom、闪存或其他固态存储其技术，cd-rom、或其他光学存储、磁带盒、磁带、磁盘存储或其他磁性存储设备。当然，本领域技术人员可知计算机存储介质不局限于上述几种。上述的系统存储器和大容量存储设备可以统称为存储器。
[0217]
计算机设备1700可以通过连接在系统总线1705上的网络接口单元1711连接到网络1712。
[0218]
系统存储器1704或大容量存储设备1707还用于存储一个或者一个以上的指令。中央处理器1701通过执行该一个或一个以上指令来实现本公开一些实施例中的方法的全部或者部分步骤。
[0219]
本公开一些实施例中，本公开还提供一种计算机可读存储介质，存储介质中存储有至少一条指令，该至少一条指令由处理器执行以使计算机执行本公开一些实施例的方法中的部分步骤或全部步骤，如图3、图4、图9、图10、图12、图14和图16中公开的方法中的部分
步骤或全部步骤。计算机可读存储介质的示例包括计算机程序(指令)的存储器，例如，只读存储器(read-only memory，rom)、随机存取存储器(random access memory，ram)、只读光盘(compact disc read-only memory，cd-rom)、磁带、软盘和光数据存储设备等。
[0220]
图18示出根据本公开一些实施例的机器人系统的示意图1800。如图18所示，机器人系统1800，包括：主操作器1810、控制装置1820、驱动装置1860、从动工具1850、和图像采集设备1870。主操作器1810包括机械臂、设置在机械臂上的手柄以及设置在机械臂上的至少一个关节处的至少一个主操作器传感器，至少一个主操作器传感器用于获得至少一个关节的关节信息。在一些实施例中，主操作器1810包括六自由度机械臂，六自由度机械臂上的每一个关节处设置一个主操作器传感器，通过每个关节的主操作器传感器生成关节信息(如关节角度数据)。在一些实施例中，主操作器传感器采用电位计和/或编码器。在从动工具1850上设置有执行臂1840，在一些实施例中，执行臂1840包括多构节连续体可形变臂。在执行臂1840的末端1830上可以形成有或设置有至少一个角度标识和多个位姿标识，在末端1830的远端可以设置有执行器。图像采集设备1870可以用于采集执行臂1840的定位图像。驱动装置1860用于驱动执行臂1840，至少一个驱动装置传感器与至少一个驱动装置耦合并且用于获得驱动信息。控制装置1820与主操作器1810、至少一个驱动装置1860以及图像采集设备1870通信连接，被配置为执行本公开一些实施例的方法中的部分步骤或全部步骤，如图3、图4、图9、图10、图12、图14和图16中公开的方法中的部分步骤或全部步骤。
[0221]
机器人对操作精度和人机交互体验要求很高。在机器人系统操作过程中，执行臂如果无法准确、快递地移动目标位置和姿态，会降低操作者的操作体验，甚至会导致操作失败，产生不必要的风险。在本公开的实施例中，通过对执行臂的实际位姿进行检测，并与操作者所期望的执行臂的目标位姿进行实时比较，可以发现所存在的故障风险。本公开的实施例可以改善机器人系统的操作性和安全性，降低机器人系统操作过程中执行臂的位姿误差而带来的操作风险。
[0222]
注意，上述仅为本公开的示例性实施例及所运用技术原理。本领域技术人员会理解，本公开不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本公开的保护范围。因此，虽然通过以上实施例对本公开进行了较为详细的说明，但是本公开不仅仅限于以上实施例，在不脱离本公开构思的情况下，还可以包括更多其他等效实施例，而本公开的范围由所附的权利要求范围决定。