CN114536402A - 基于关联标识的机器人系统故障检测处理方法及机器人系统 - Google Patents

Abstract

本公开涉及故障检测处理技术领域，公开一种机器人系统故障检测处理方法。机器人系统包括控制装置、至少一个驱动装置以及执行臂，至少一个驱动装置基于控制装置发出的驱动信号，驱动执行臂。该方法包括：确定执行臂的末端的目标位姿；获取定位图像；在定位图像中，识别位于执行臂的末端上的多个位姿标识；基于多个位姿标识，识别位于执行臂的末端上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系；基于角度标识和多个位姿标识，确定执行臂的末端的实际位姿；基于目标位姿与实际位姿满足误差条件，使机器人系统进入安全模式；以及确定机器人系统的故障状况。

Description

基于关联标识的机器人系统故障检测处理方法及机器人系统

技术领域

本公开涉及故障检测处理技术领域，尤其涉及一种基于关联标识的机器人系统故障检测处理方法及机器人系统。

背景技术

在机器人系统运行过程中，需要对机器人系统的工作状况进行检测，在发生故障时采取必要的应对措施，否则可能会操作失败，甚至造成不可挽回的损失。

执行臂是机器人系统的重要组成部分，执行臂的位姿准确程度可以作为衡量机器人系统是否发生故障的重要指标。现有技术中，可以使用激光跟踪仪等设备测量执行臂的位姿，然而，激光跟踪仪等设备至少存在价格昂贵、无法一次性测得位姿、需要定期校准等技术问题。

另一方面，现有的机器人系统在检测到发生故障时，会对整个机器人系统进行停机检测。这种处理方式在实际生产作业中，会使作业效率下降，影响用户体验。

因此，需要提供一种能简单有效地检测机器人系统故障的方法，能根据故障状况采取对应的处理措施，以兼顾机器人系统的安全性和用户体验。

发明内容

在一些实施例中，本公开提供了一种机器人系统故障检测处理方法。机器人系统包括控制装置、至少一个驱动装置以及执行臂，至少一个驱动装置基于控制装置发出的驱动信号，驱动执行臂。该方法可以包括：确定执行臂的末端的目标位姿；获取定位图像；在定位图像中，识别位于执行臂的末端上的多个位姿标识；基于多个位姿标识，识别位于执行臂的末端上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系；基于角度标识和多个位姿标识，确定执行臂的末端的实际位姿；基于目标位姿与实际位姿满足误差条件，使机器人系统进入安全模式；以及确定机器人系统的故障状况。

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

在一些实施例中，本公开提供了一种计算机可读存储介质，用于存储至少一条指令，至少一条指令由计算机执行时致使计算机执行本公开一些实施例中任一项的方法。

在一些实施例中，本公开提供了一种机器人系统，包括：执行臂，执行臂的末端设置有至少一个角度标识和多个位姿标识；至少一个驱动装置，用于驱动执行臂；至少一个驱动装置传感器，与至少一个驱动装置耦合并且用于获得至少一个驱动装置的状态信息；图像采集设备，用于采集执行臂的定位图像；以及控制装置，被配置为与至少一个驱动装置、至少一个驱动装置传感器、图像采集设备连接，执行本公开一些实施例中任一项的方法。

附图说明

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

图2示出根据本公开一些实施例的故障检测处理系统的示意图；

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

图4示出根据本公开一些实施例的设置在执行臂末端周侧并形成圆筒形的标签的示意图；

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

图6示出根据本公开一些实施例的故障检测处理方法的流程图；

图7示出根据本公开一些实施例的基于机器人系统的故障状况进行故障处理的方法的流程图；

图8示出根据本公开一些实施例的用于机器人系统的通信故障检测处理的方法的流程图；

图9示出根据本公开一些实施例的用于机器人系统的执行臂故障检测处理的方法的流程图；

图10示出根据本公开一些实施例的用于机器人系统的目标位姿故障检测处理的方法的流程图；

图11示出根据本公开一些实施例的确定执行臂的末端坐标系相对参考坐标系的位姿的方法的流程图；

图12示出根据本公开另一些实施例的确定执行臂的末端坐标系相对参考坐标系的位姿的方法的流程图；

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

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

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

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

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

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

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

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

具体实施方式

为使本公开解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图详细说明本公开实施例的技术方案作。本领域技术人员可以理解，所描述的实施例应视为在所有方面都是示例性的而非限制性的，仅仅是本公开示例性实施例，而不是全部的实施例。

在本公开中，术语“位置”指物体或物体的一部分在三维空间中的定位(例如，可使用笛卡尔X、Y和Z坐标方面的变化描述三个平移自由度，例如分别沿笛卡尔X轴、Y轴和Z轴的三个平移自由度)。在本公开中，术语“姿态”指物体或物体的一部分的旋转设置(例如，三个旋转自由度，可使用滚转、俯仰和偏转来描述这三个旋转自由度)。在本公开中，术语“位姿”指物体或物体的一部分的位置和姿态的组合，例如可使用以上提到的六个自由度中的六个参数来描述。

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

在本公开中，物体可以理解为需要被定位的对象或目标，例如执行臂或者执行臂的末端。执行臂或其一部分的位姿可以是指执行臂或其一部分定义的坐标系相对参考坐标系的位姿。

图1示出根据本公开一些实施例的机器人系统100的结构示意图。在一些实施例中，如图1所示，机器人系统100可以包括主控台车110、从动台车130及控制装置120。控制装置120可以与主控台车110和从动台车130通信连接，例如可通过线缆连接、也可以通过无线连接，以实现与主控台车110和从动台车130之间的通信。主控台车110包括供操作者遥操作的主操作器，用于显示操作区域的图像的显示器，以及供操作者进行触控操作的触摸屏，从动台车130包括用于执行作业的执行臂。通过控制装置120实现主控台车110中的主操作器与从动台车130中的执行臂之间的主从映射，实现主操作器对执行臂的运动控制。在一些实施例中，执行臂被设置成能够通过管套、鞘套等进入操作区域，且在操作区域与待操作对象(例如，工件、人体组织等)接触并产生作用力。其中，管套、鞘套可以固定在墙体壁面、动物躯体等上形成开口(例如人工开口或自然开口)处，操作区域可以是进行作业的区域。执行臂可以是可形变臂，在执行臂的远端可以设置有末端器械(例如，执行器)，该末端器械可以包括但不限于挖掘器械、水下作业器械、手术器械等。本领域技术人员可以理解，主控台车110和从动台车130可以采用其他结构或者形式，例如基座、支架或建筑物等。

图2示出了根据本公开一些实施例的故障检测处理系统200的示意图(以下也称“系统200”)。如图2所示，系统200可以包括控制装置220、执行臂230、至少一个驱动装置260、以及图像采集设备250。执行臂230可以由可形变臂或刚性臂实现。在一些实施例中，执行臂230在末端或远端处可以包括执行臂末端231。在一些实施例中，执行臂末端231的远端处可以设置有末端器械240。

在一些实施例中，控制装置220可以与至少一个驱动装置260通信连接并向驱动装置260发送驱动信号，控制执行臂230运动，以使执行臂末端231运动至期望的位置和姿态。例如，驱动装置260可以是伺服电机，可以接受控制装置的指令以控制执行臂230运动，使执行臂末端231运动至相应的目标位姿。控制装置220可以例如通过通信接口与驱动装置260耦合的驱动装置传感器通信连接，以接收驱动装置260的运行数据，实现对驱动装置260的状态监控。在本公开的一个示例中，该通信接口可以为CAN(Controller Area Network)总线通信接口，其使得控制装置220能够通过CAN总线与驱动装置260以及驱动装置传感器连接通信。

在一些实施例中，如图2所示，系统200还可以包括用于停止执行臂230的动作的至少一个制动单元270，至少一个制动单元270可以与控制装置220通信连接。在一些实施例中，至少一个制动单元270可以是与至少一个驱动装置260对应地设置，基于来自控制装置220的制动信号停止驱动装置260的运动。例如，制动单元270可以基于制动信号，通过机械制动(例如，电磁抱闸制动)或电气制动方式停止驱动装置260的运动。在一些实施例中，至少一个制动单元270也可以用于断开至少一个驱动装置260与对应的传动机构之间的传动连接，进而停止执行臂230的动作。

控制装置220可以与图像采集设备250通信连接。在一些实施例中，图像采集设备250可以用于采集定位图像。定位图像中可以包括执行臂230的部分或全部的图像。在一些实施例中，图像采集设备250可以包括但不限于双镜头图像采集设备或单镜头图像采集设备，例如双目或单目相机。在一些实施例中，图像采集设备250可以用于采集执行臂末端231的图像，执行臂末端231上可以设置有定位标识(例如可以是图3所示的标签300或图4所示的标签400)。定位标识可以包括位姿标识和角度标识(如下详述)。

如图2所示，执行臂末端231处于图像采集设备250的观测视野251之内，则采集的定位图像中可以包括执行臂末端231的图像。根据不同的应用场景，图像采集设备250可以是工业相机、水下相机、微型电子相机、内窥镜相机等。在一些实施例中，图像采集设备250可以是位置固定的或者位置变化的，例如，固定在监控位置的工业相机或者位置或姿态可调节的内窥镜相机。在一些实施例中，图像采集设备250可以实现可见光波段成像、红外波段成像、CT(Computed Tomography，电子计算机断层扫描)成像和声波成像等中的至少一种。根据采集的图像的种类不同，本领域技术人员可以选择不同的图像采集设备作为图像采集设备250。

在一些实施例中，控制装置220可以接收来自图像采集设备250的定位图像，并且对定位图像进行处理。例如，控制装置220可以在定位图像中，识别位于执行臂末端231上的多个位姿标识和至少一个角度标识，并且确定执行臂末端231相对参考坐标系(例如，世界坐标系)的位姿，作为执行臂末端231的实际位姿。

在一些实施例中，系统200可以基于执行臂末端231的实际位姿和目标位姿，判断机器人系统是否发生故障。在一些实施例中，控制装置220可以在检测到发生故障时，使机器人系统进入安全模式，并确定机器人系统的故障状况。在一些实施例中，控制装置220还可以以预定周期确定执行臂末端231的实际位姿，以通过多个故障检测循环，实时确定机器人系统的故障状况。在一些实施例中，控制装置220可以基于机器人系统的故障状况，生成与故障状况对应的故障报警信号。在一些实施例中，故障报警信号可以由声音、灯光、屏幕(例如，触摸屏)上显示的提示信息或是它们的组合等表示。在一些实施例中，故障报警信号可以根据故障种类包括多个不同类型的报警信号，不同类型的报警信号可以通过不同的声音、不同颜色的灯光或不同的提示信息表示。在一些实施例中，故障报警信号可以包括询问用户是否使用手动模式的提示信息。例如，故障报警信号可以包括提示消息和手动模式选择菜单。本领域技术人员应理解，系统200可以应用于多个领域(例如，物流、工业制造、医疗等)的专用或通用的机器人系统，机器人系统可以是遥操作机器人系统也可以是非遥操作机器人系统。作为一个示例，系统200可以应用于例如手术机器人等机器人系统，执行臂末端231的远端处设置的末端器械240例如可以是手术执行器。

在一些实施例中，执行臂上(例如，执行臂末端231上)分布有多个位姿标识和至少一个角度标识。例如，多个位姿标识沿周向分布在执行臂末端231上，多个角度标识沿周向分布在执行臂末端231上。多个位姿标识与多个角度标识在执行臂末端231上沿轴向并列设置。例如，多个位姿标识与多个角度标识设置在执行臂末端231的柱状部分的外表面上。

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

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

在一些实施例中，多个位姿标识与多个角度标识可以设置在贴附在执行臂末端周侧的标签上。

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

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

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

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

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

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

图5示出根据本公开一些实施例的实施场景500的示意图。如图5所示，执行臂540包括执行臂末端530以及远端的执行器560，多个位姿标识和角度标识可以沿周向设置在执行臂末端530上。例如，如图3所示的标签300沿周向设置在执行臂末端530上，形成圆筒形的角度标识图案带510和位姿标识图案带520。多个位姿标识图案角点分布在执行臂末端530的位姿标识图案带520的截面圆521上，多个角度标识图案角点分布在执行臂末端530的角度标识图案带510的截面圆511上。

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

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

在一些实施例中，执行器560的位姿可以通过将执行臂的末端坐标系{wm}平移预定距离来确定。或者，执行器560的位姿可以近似地等于执行臂的末端坐标系{wm}的位姿。

在一些实施例中，基于执行臂的末端坐标系相对参考坐标系的位姿，确定执行器560相对参考坐标系{w}的位姿。具体计算公式如下：

其中，^wR_tip为执行器相对参考坐标系的姿态，^wP_tip为执行器相对参考坐标系的位置，^wmR_tip为执行器相对执行臂的末端坐标系的姿态，^wmP_tip为执行器相对执行臂的末端坐标系的位置，^wR_wm为执行臂的末端坐标系相对参考坐标系的姿态，^wP_wm为执行臂的末端坐标系相对参考坐标系的位置。

本公开的一些实施例提供了一种用于机器人系统的故障检测处理方法。图6示出根据本公开一些实施例的故障检测处理方法600(以下也简称“方法600”)的流程图。方法600可以由硬件、软件或者固件实现或执行。在一些实施例中，方法600可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法600可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法600。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图6，在步骤601，确定执行臂的末端的目标位姿。在一些实施例中，可以预先设置执行臂的末端的目标位姿。例如，执行臂的末端的目标位姿可以由用户通过输入装置预先输入并存储于机器人系统的存储器中。或者，执行臂的末端的目标位姿也可以由位姿生成单元通过随机生成获得。在一些实施例中，可以基于主操作器的位姿，确定执行臂的末端的目标位姿。例如，在遥操作过程中，可以基于主操作器的位姿与执行臂的末端的位姿之间的主从运动映射关系，根据主操作器的位姿确定执行臂的末端的目标位姿。

在一些实施例中，控制装置220可以基于执行臂的末端的目标位姿，向至少一个驱动装置260发送驱动信号，以控制执行臂的末端231的运动。在一些实施例中，控制装置220可以基于执行臂的末端的目标位姿，通过执行臂运动学模型的逆运动学数值迭代算法，确定控制执行臂运动的至少一个驱动装置的驱动信号。应当理解，运动学模型可以表示执行臂的关节空间和任务空间的运动关系的数学模型。例如，运动学模型可以通过DH(Denavit-Hartenberg)参数法和指数积表示法等方法建立。在一些实施例中，执行臂的末端的目标位姿为执行臂的末端在参考坐标系中的目标位姿。

继续参看图6，在步骤603，获取定位图像。在一些实施例中，定位图像中包含执行臂的末端上的多个位姿标识和至少一个角度标识。在一些实施例中，可以从如图2所示的图像采集设备250接收定位图像。例如，控制装置220可以接收图像采集设备250主动发送的定位图像。或者，控制装置220可以向图像采集设备250发送图像请求指令，图像采集设备250响应图像请求指令向控制装置220发送定位图像。

继续参看图6，在步骤605，在定位图像中，识别位于执行臂的末端上的多个位姿标识。例如，识别位于执行臂的末端上的多个位姿标识的示例性方法可以包括如图14和图16示的方法。在一些实施例中，控制装置220可以通过图像处理算法识别定位图像中的部分或全部的位姿标识。在一些实施例中，图像处理算法可以包括特征识别算法，图像处理算法可以提取或识别位姿标识的特征。例如，图像处理算法可以包括角点检测算法，用于检测位姿标识图案角点。该角点检测算法可以是包括但不限于基于灰度图的角点检测、基于二值图像的角点检测、基于轮廓曲线的角点检测中的一种。例如，图像处理算法可以是颜色特征提取算法，用于检测位姿标识图案中的颜色特征。再例如，图像处理算法可以是轮廓检测算法，用于检测位姿标识图案的轮廓特征。

在一些实施例中，控制装置可以通过识别模型识别定位图像中的部分或全部的位姿标识。

继续参看图6，在步骤607，基于多个位姿标识，识别位于执行臂的末端上的角度标识，角度标识与多个位姿标识中的第一位姿标识具有位置关联关系。在一些实施例中，在识别出多个位姿标识之后，根据位置关联关系，识别位于执行臂的末端上的角度标识。在一些实施例中，角度标识与第一位姿标识的位置关联关系可以是如图3或图4中示出的位置关联关系。在一些实施例中，第一位姿标识(例如是第一位姿标识图案或第一位姿标识图案角点)是指多个位姿标识中与角度标识具有位置关联关系的位姿标识。识别角度标识的示例性方法包括如图18所示的方法。

继续参看图6，在步骤609，基于角度标识和多个位姿标识，确定执行臂的末端的实际位姿。获得执行臂的末端的实际位姿的示例性方法包括如图11或图12所示的方法。在一些实施例中，可以基于角度标识、第一位姿标识和多个位姿标识，获得执行臂的末端相对参考坐标系的位姿，作为执行臂的末端的实际位姿。

继续参看图6，在步骤611，基于目标位姿与实际位姿满足误差条件，使机器人系统进入安全模式。例如，控制装置可以确定执行臂的末端在参考坐标系中的目标位姿，并基于执行臂的定位图像确定执行臂的末端的实际位姿，在目标位姿与实际位姿满足误差条件(例如大于等于误差阈值)时，判断机器人系统发生故障，进而使机器人系统进入安全模式。在本公开中，安全模式可以是在检测到机器人系统发生故障时，停止与故障相关的执行臂的动作以确保机器人作业安全性的模式。

在一些实施例中，使机器人系统进入安全模式可以包括停用驱动执行臂的至少一个驱动装置。例如，在系统200确定机器人系统发生故障而使其进入安全模式时，控制装置220可以向与至少一个驱动装置260对应的至少一个制动单元270发送制动信号，至少一个制动单元270基于制动信号对至少一个驱动装置260进行制动，以停止执行臂230的动作，使执行臂末端231保持位姿。在一些实施例中，使机器人系统进入安全模式还可以包括允许机器人系统的用户使用手动模式，在手动模式下，用户可以参与决定如何处理机器人系统发生的故障，具体内容后述。

在一些实施例中，方法600还可以包括基于执行臂的末端在参考坐标系中的目标位姿与执行臂的末端在参考坐标系中的实际位姿，确定位姿差，以及基于位姿差与误差条件，确定机器人是否发生故障。位姿差可以包括位置差和姿态差。在一些实施例中，可以通过一次执行臂检测确定执行臂的末端的目标位姿与实际位姿的位姿差，在该位姿差满足误差条件时，确定机器人系统发生故障，进而使机器人系统进入安全模式。在一些实施例中，可以通过多次执行臂检测，确定执行臂的末端的目标位姿与实际位姿的多个位姿差，在这些位姿差满足误差条件(例如，多个位姿差的平均值或累计值大于等于误差阈值)时，确定机器人系统发生故障，进而使机器人系统进入安全模式。在一些实施例中，方法600还可以包括，以预定检测周期确定执行臂的末端的实际位姿(例如，以预定检测周期执行步骤603-609)，以通过多个检测循环，实时确定或累计确定机器人系统是否发生故障。

在第k次执行臂检测循环中，位姿差可以表示如下：

其中，

为第k次执行臂检测循环时执行臂的位置差，

为第k次执行臂检测循环时执行臂的角度差，P_t ^k为第k次执行臂检测循环时执行臂的目标位置，R_t ^k为第k次执行臂检测循环时执行臂的目标姿态，

为第k次执行臂检测循环时执行臂的实际位置，R_r ^k为第k次执行臂检测循环时执行臂的实际姿态，

表示

与

之间的转角。

继续参看图6，在步骤613，确定机器人系统的故障状况。在安全模式下，系统200可以对机器人系统进行故障诊断，确定机器人系统的故障状况。在一些实施例中，对机器人系统进行故障诊断可以包括在机器人系统进入安全模式时，控制装置对执行臂及其相关组件(例如，驱动装置、通信模块等)进行检测，以确定发生故障的位置以及状况等。在本公开中，机器人的故障状况可以包括可恢复故障和不可恢复故障。在一些实施例中，可恢复故障例如可以尝试从故障中恢复，在故障恢复成功后可以使机器人系统再次上线继续作业。不可恢复故障例如代表机器人系统无法继续作业，需要中止系统操作并采取相应的处理措施。

在一些实施例中，方法600还可以包括基于机器人系统的故障状况，进行与故障状况对应的故障处理。图7示出根据本公开一些实施例的基于机器人系统的故障状况进行故障处理的方法700的流程图。方法700可以由硬件、软件或者固件实现或执行。在一些实施例中，方法700可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法700可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法700。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图7，在步骤701，确定故障状况。在一些实施例中，在机器人系统进入安全模式时，控制装置220可以对与故障相关的执行臂上的多个组件同时进行故障诊断，以确定故障的具体位置和原因。或者，在机器人系统进入安全模式时，控制装置220也可以对与故障相关的执行臂上的多个组件依次进行故障诊断，逐个排除未发生故障的组件，以确定机器人系统的故障状况。步骤701可以与方法600中的步骤613类似地实现。故障状况可以包括不同故障类型的故障，例如可恢复故障和不可恢复故障。

继续参看图7，在步骤703，确定故障状况是否为可恢复故障。在一些实施例中，可以将机器人系统的故障预先分类为可恢复故障或不可恢复故障，并将分类情况作为故障检测表存储至机器人系统的存储器中，在检测到故障时，与故障检测表进行对比以确定故障状况是否为可恢复故障。

继续参看图7，响应于确定机器人系统的故障状况为可恢复故障，进至步骤705，执行故障恢复操作。故障恢复操作例如可以包括基于故障状况进行系统重启、模块重启、驱动程序初始化等。在一些实施例中，控制装置220可以在确定故障状况为可恢复故障时，发出故障报警信号以指示发生故障，和/或自动执行故障恢复操作以尝试恢复故障。在另一些实施例中，控制装置220也可以发出故障报警信号(例如，在触摸屏上显示提示信息和菜单)以询问用户是否进行故障恢复操作。在一些实施例中，控制装置220可以在故障报警信号经过规定时间但用户仍未进行操作时，自动执行故障恢复操作。

另一方面，响应于确定机器人系统的故障状况为不可恢复故障，进至步骤711，使机器人系统进入退出模式。在一些实施例中，控制装置220可以发出故障报警信号(例如，在触摸屏上显示提示信息和菜单)以询问用户是否使机器人系统进入退出模式。另外，在机器人系统的故障状况为可恢复故障但用户不允许执行故障恢复操作时，也可以使机器人系统进入退出模式。在退出模式下，用户例如可以通过触摸屏激活机器人系统的自动退出功能，以使执行臂230及其末端231安全撤出操作区域。或者，用户也可以通过触摸屏激活手动退出功能，按照规定的操作流程手动退出执行臂230。

继续参看图7，在步骤707，确定故障恢复是否成功。在一些实施例中，控制装置220可以在执行故障恢复操作后，对发生可恢复故障的组件再次确定故障状况，以确定故障是否成功恢复。步骤707可以与方法600中的步骤613或方法700中的步骤701类似地实现。

继续参看图7，响应于在故障恢复操作成功，进至步骤709，使机器人系统退出安全模式。或者，响应于故障恢复操作失败，进至步骤711，使机器人系统进入退出模式。

在一些实施例中，机器人系统故障可以包括通信故障。

在一些实施例中，机器人系统可以包括用于中继控制装置发送的驱动信号的通信节点以及用于与通信节点通信的通信模块。通信节点可以通过通信模块与例如图2中的控制装置220通信连接，向至少一个驱动装置260发送来自控制装置220的驱动信号。在一些实施例中，通信节点可以包括与至少一个驱动装置260对应的控制芯片，用于执行通信功能和适当的处理功能。例如，控制芯片可以用于基于驱动装置260的状态信息，计算执行臂230的末端231的位姿，或者基于来自控制装置220的控制信号，计算驱动装置260的驱动信号。

图8是示出根据本公开一些实施例的用于机器人系统的通信故障检测处理的方法800的流程图。方法800可以由硬件、软件或者固件实现或执行。在一些实施例中，方法800可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法800可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法800。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图8，在步骤801，向通信节点发送问询信号。在一些实施例中，在机器人系统进入安全模式时，控制装置220可以通过通信模块向通信节点发送问询信号，以确定机器人系统的通信是否正常。例如，控制装置220可以通过通信模块向通信节点发送问询信号，通信节点在收到问询信号时会通过通信模块向控制装置220发送回应信号，控制装置220基于是否接收到回应信号来判断机器人系统是否存在通信故障。

在步骤803，响应于无法接收到通信节点的回应信号，生成指示发生通信故障的通信故障报警信号和/或重启通信模块。例如，在无法接收到通信节点的回应信号时，控制装置220可以生成通信故障报警信号以指示发生通信故障，和/或自动重启通信模块以尝试恢复通信故障。在一些实施例中，控制装置220也可以生成通信故障报警信号(例如，在触摸屏上显示提示信息和菜单)以询问用户是否重启通信模块。或者，控制装置220可以在生成通信故障报警信号后用户长时间(例如，超过规定时间)未进行操作时，自动重启通信模块。

在一些实施例中，通信节点可以定时向控制装置220发送信号。控制装置220可以响应于预定时间内没有接收到来自通信节点的信号，生成指示发生通信故障的通信故障报警信号和/或重启通信模块。

在另一些实施例中，机器人系统也可以不包括通信节点，而是包括用于使控制装置与至少一个驱动装置的通信连接的通信模块。控制装置220通过通信模块接收至少一个驱动装置260的状态信息(例如，可以由与至少一个驱动装置260耦合的驱动装置传感器发送)以确定机器人系统是否存在通信故障。在一些实施例中，响应于无法收到至少一个驱动装置的状态信息，生成指示发生通信故障的通信故障报警信号和/或重启用于与至少一个驱动装置通信的通信模块。例如，在无法接收到至少一个驱动装置260的状态信息时，控制装置220可以生成通信故障报警信号以指示发生通信故障，和/或自动重启通信模块以尝试恢复通信故障。在一些实施例中，控制装置220也可以生成通信故障报警信号以询问用户是否重启通信模块，或是在用户长时间未进行操作时，自动重启通信模块。

在一些实施例中，响应于重启之后通信故障恢复，使机器人系统退出安全模式。例如，在自动或用户手动重新通信模块后，控制装置220在可以接收到来自通信节点的回应信号或至少一个驱动装置260的状态信息，且没有检测到其他障碍的情况下，确定机器人系统从故障中恢复成功，使机器人系统退出安全模式以继续执行操作。在一些实施例中，响应于重启之后通信故障未恢复，使机器人系统进入退出模式。例如，在自动或手动重新通信模块后，控制装置220仍无法接收到来自通信节点的回应信号或至少一个驱动装置260的状态信息时，确定机器人系统无法继续工作，使机器人系统进入退出模式。

在一些实施例中，机器人系统故障可以包括用于驱动装置故障或是执行臂故障。

在一些实施例中，控制装置基于执行臂的末端的目标位姿向至少一个驱动装置发送驱动信号，并通过驱动装置传感器接收用于驱动执行臂的至少一个驱动装置的状态信息。在一些实施例中，在机器人系统进入安全模式时，控制装置基于至少一个驱动装置的状态信息和至少一个驱动装置的驱动信号，确定机器人系统的故障状况。

在一些实施例中，响应于状态信息与控制装置发出的至少一个驱动装置的驱动信号满足驱动装置故障条件，生成驱动装置故障报警信号，驱动装置故障信号指示执行臂的驱动装置发生故障。例如，在安全模式下，控制装置220可以在至少一个驱动装置260的状态信息与向其发送的驱动信号满足驱动装置故障条件(例如大于等于误差阈值)时，确定至少一个驱动装置260发生故障，生成驱动装置故障报警信号和/或使机器人系统进入退出模式，或者也可以询问用户(例如，在触摸屏上显示提示信息和菜单)是否使机器人系统进入退出模式。

在一些实施例中，响应于状态信息与控制装置发出的至少一个驱动装置的驱动信号不满足驱动装置故障条件，生成执行臂故障报警信号和/或使机器人系统进入退出模式，执行臂故障报警信号指示发生执行臂故障。例如，在安全模式下，控制装置220可以在至少一个驱动装置260的状态信息与向其发送的驱动信号不满足驱动装置故障条件(例如小于误差阈值)时，确定至少一个驱动装置260正常工作而执行臂230发生故障，生成执行臂故障报警信号和/或使机器人系统进入退出模式，或者也可以询问用户(例如，在触摸屏上显示提示信息和菜单)是否使机器人系统进入退出模式。

图9是示出根据本公开一些实施例的用于机器人系统的执行臂故障检测处理的方法900的流程图。方法900可以由硬件、软件或者固件实现或执行。在一些实施例中，方法900可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法900可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法900。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图9，在步骤901，接收执行臂的至少一个关节的实际关节信息。在一些实施例中，执行臂包括至少一个关节，在至少一个关节处设置有至少一个执行臂传感器，至少一个执行臂传感器用于获得至少一个关节的实际关节信息。实际关节信息可以包括关节的实际位置或实际角度。

在步骤903，基于执行臂的末端的目标位姿，确定执行臂的至少一个关节的目标关节信息。应理解，在机器人系统中，执行臂的位姿可由执行臂的关节的关节信息的集合(例如由这些关节信息组成的一维矩阵)来表示。在预先设置或随机生成执行臂的末端的目标位姿的情况下，例如可以逆向运动学算法确定执行臂的至少一个关节的目标关节信息。

在步骤905，响应于实际关节信息与目标关节信息满足执行臂故障条件，生成执行臂故障报警信号和/或使机器人系统进入退出模式，执行臂故障报警信号指示发生执行臂故障。例如，在安全模式下，控制装置220可以在至少一个关节的实际关节信息与目标关节信息满足执行臂故障条件(例如大于等于误差阈值)时，确定执行臂230发生故障，生成执行臂故障报警信号和/或使机器人系统进入退出模式，或者也可以询问用户(例如，在触摸屏上显示提示信息和菜单)是否使机器人系统进入退出模式。

在一些实施例中，机器人系统故障可以包括执行臂的末端的目标位姿超出执行臂的运动极限。图10是示出根据本公开一些实施例的用于机器人系统的目标位姿故障检测处理的方法1000的流程图。方法1000可以由硬件、软件或者固件实现或执行。在一些实施例中，方法1000可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法1000可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法1000。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图10，在步骤1001，判断目标位姿是否超出执行臂的运动极限。在一些实施例中，执行臂的工作空间可以预先存储在机器人系统的存储器中，执行臂的工作空间包括执行臂的运动极限。在一些实施例中，有可能会出现随机生成的目标位姿超过执行臂的运动极限的情况，从而导致执行臂的末端无法实现随机生成的目标位置和/或姿态。在一些实施例中，例如在遥操作过程中，有可能会发生与主操作器的位姿所对应的执行臂的末端的目标位姿超出执行臂的运动极限的情况。

在步骤1003，响应于目标位姿超出执行臂的运动极限，生成指示超出运动极限的报警信号。

在步骤1005，退出安全模式。例如，在安全模式下，控制装置可以在确定执行臂的末端的目标位姿位于执行臂的工作空间以外时，将执行臂的运动极限更新为执行臂的末端的目标位姿，然后使机器人系统退出安全模式以继续执行操作。或者，在遥操作过程中，控制装置可以自动断开或是提示用户断开主操作器与执行臂之间的主从映射，待用户将主操作器调整至合适位置后，重新完成主从匹配并建立主从映射，然后退出安全模式。

在一些实施例中，方法600还包括：基于角度标识和多个位姿标识，确定执行臂的末端坐标系和位姿标识坐标系的变换关系。在一些实施例中，根据执行臂的末端坐标系和位姿标识坐标系的变换关系，可以将位姿标识坐标系中的三维坐标转换为在执行臂的末端坐标系中对应的三维坐标。在一些实施例中，根据执行臂的末端坐标系和位姿标识坐标系的变换关系和位姿标识坐标系相对参考坐标系的位姿，得到执行臂的末端坐标系相对参考坐标系的位姿，作为执行臂的末端的实际位姿。

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

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

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

示例性地，参看图5，执行臂的末端坐标系{wm}≡[X_wm Y_wm Z_wm]^T的原点为多个位姿标识图案角点所在截面圆521的圆心，X轴方向为原点指向其中一个位姿标识图案角点，Z轴的方向平行执行臂末端530的轴向，Y轴垂直于XZ平面。执行臂的末端坐标系{wm}的X轴与截面圆511的二维平面坐标系{wm1}≡[X_wm1 Y_wm1]^T的X轴平行，执行臂的末端坐标系的Y轴与截面圆511的二维平面坐标系{wm1}的Y轴平行。角度标识图案角点在截面圆511的二维平面坐标系{wm1}中标识的绕轴角度可以与其在执行臂的末端坐标系{wm}中标识的绕轴角度相等。位姿标识坐标系{wm0}≡[X_wm0 Y_wm0 Z_wm0]^T的原点为多个位姿标识图案角点所在截面圆521的圆心，X轴方向为原点指向其中一个位姿标识图案角点，Z轴的方向平行物体执行臂末端530的轴向，Y轴垂直于XZ平面。继续参看图5，执行臂的末端坐标系{wm}的Z轴与位姿标识坐标系{wm0}的Z轴重合。执行臂的末端坐标系{wm}相对位姿标识坐标系{wm0}的变换关系可以通过位姿标识坐标系{wm0}相对执行臂的末端坐标系{wm}的滚转角α₀确定。滚转角α₀可以是指位姿标识坐标系{wm0}相对执行臂的末端坐标系{wm}绕Z轴的转动角度。

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

α₀＝α₁-α₂ (3)

其中α₁为第一绕轴角度，α₂为第二绕轴角度。第一绕轴角度为角度标识图案角点(例如，角度标识图案角点R5)在执行臂的末端坐标系中标识的绕轴角度。第二绕轴角度为第一位姿标识图案角点(例如，位姿标识图案角点P5)在位姿标识坐标系中标识的绕轴角度。

图11示出根据本公开一些实施例的确定执行臂的末端坐标系相对参考坐标系的位姿的方法1100的流程图。如图11所示，方法1100可以由硬件、软件或者固件实现或执行。在一些实施例中，方法1100可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法1100可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法1100。在一些实施例中，这些指令可以存储在计算机可读介质上。

参看图11，在步骤1101，基于角度标识和多个位姿标识，确定位姿标识坐标系相对执行臂的末端坐标系的滚转角。在一些实施例中，确定角度标识在执行臂的末端坐标系中标识的第一绕轴角度。确定第一位姿标识在位姿标识坐标系中标识的第二绕轴角度。基于第一绕轴角度和第二绕轴角度，确定位姿标识坐标系相对执行臂的末端坐标系的滚转角。在一些实施例中，位姿标识坐标系相对执行臂的末端坐标系的滚转角可以基于上述公式(3)确定。

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

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

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

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

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

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

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

参看图11，在步骤1105，基于位姿标识坐标系相对执行臂的末端坐标系的滚转角和位姿标识坐标系相对参考坐标系的位姿，确定执行臂的末端坐标系相对参考坐标系的位姿，作为实际位姿。

例如，以参考坐标系为世界坐标系为例，执行臂的末端坐标系相对世界坐标系的位姿具体如下：

^wR_wm＝^wR_wm0·rot_z(α₀)

^wP_wm＝^wP_wm0 (5)

其中，^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 (6)

其中，^wR_lens为相机坐标系相对世界坐标系的姿态，^wP_lens为相机坐标系相对世界坐标系的位置，^lensR_wm0为位姿标识坐标系相对相机坐标系的姿态，^lensP_wm0为位姿标识坐标系相对相机坐标系的位置，^wm0R_wm为执行臂的末端坐标系相对位姿标识坐标系的姿态，^wm0P_wm为执行臂的末端坐标系相对位姿标识坐标系的位置。

图12示出根据本公开另一些实施例的确定执行臂的末端坐标系相对参考坐标系的位姿的方法1200的流程图。方法1200可以是图11的方法1100的替换实施例。如图12所示，方法1200可以由硬件、软件或者固件实现或执行。在一些实施例中，方法1200可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法1200可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法1200。在一些实施例中，这些指令可以存储在计算机可读介质上。

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

在步骤1203，基于多个位姿标识在定位图像中的二维坐标和多个位姿标识在执行臂的末端坐标系中的三维坐标，确定执行臂的末端坐标系相对参考坐标系的位姿，作为实际位姿。在一些实施例中，步骤1203可以与方法1100中步骤1103和1105类似地实现。

图14示出根据本公开一些实施例的用于识别位姿标识的方法1400的流程图。如图14所示，方法1400可以由硬件、软件或者固件实现或执行。在一些实施例中，方法1400可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法1400可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法1400。在一些实施例中，这些指令可以存储在计算机可读介质上。

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

在一些实施例中，方法1400可以包括：确定定位图像中各像素点的角点似然值(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|) (7)

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

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

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

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

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

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

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

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

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

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

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

参看图14，在步骤1405，以初始位姿标识作为起点，搜索位姿标识。

例如，图16示出根据本公开一些实施例的用于搜索位姿标识的方法1600的流程图。如图16所示，方法1600可以由硬件、软件或者固件实现或执行。在一些实施例中，方法1600可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法1600可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法1600。在一些实施例中，这些指令可以存储在计算机可读介质上。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

图18示出根据本公开一些实施例的识别角度标识的方法1800的流程图。如图18所示，方法1800可以由硬件、软件或者固件实现或执行。在一些实施例中，方法1800可以由机器人系统(例如，图1所示的机器人系统100，或图20所示的机器人系统2000)执行。在一些实施例中，方法1800可以实现为计算机可读的指令。这些指令可以由通用处理器或专用处理器(例如，图1所示的控制装置120、图2所示的控制装置220或图20所示的控制装置2020)读取并执行。例如，用于机器人系统的控制装置可以包括处理器，被配置为执行方法1800。在一些实施例中，这些指令可以存储在计算机可读介质上。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

图20示出根据本公开一些实施例的机器人系统2000的示意图。本公开一些实施例中，参看图20，机器人系统2000可以包括：工具2050、驱动装置2060、图像采集设备2010以及控制装置(例如，处理器2020)，工具2050可以包括执行臂2040以及设置在执行臂2040远端的末端2030。在末端2030上可以形成有或设置有至少一个角度标识和多个位姿标识，在末端2030的远端可以设置有执行器。驱动装置2060可以用于控制执行臂2040及其末端2030的位姿，驱动装置传感器与驱动装置2060耦合并且用于获得驱动信息。图像采集设备2010可以用于采集执行臂2040的定位图像。处理器2020与驱动装置2060、驱动装置传感器以及图像采集设备2010连接，用于执行本公开一些实施例的方法中的部分步骤或全部步骤，如图6-图12、图14、图16和图18中公开的方法中的部分步骤或全部步骤。

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

Claims (26)

1.一种机器人系统故障检测处理方法，其特征在于，所述机器人系统包括控制装置、至少一个驱动装置以及执行臂，所述至少一个驱动装置基于控制装置发出的驱动信号，驱动所述执行臂，所述方法包括：

确定所述执行臂的末端的目标位姿；

获取定位图像；

在所述定位图像中，识别位于所述执行臂的末端上的多个位姿标识；

基于所述多个位姿标识，识别位于所述执行臂的末端上的角度标识，所述角度标识与所述多个位姿标识中的第一位姿标识具有位置关联关系；

基于所述角度标识和所述多个位姿标识，确定所述执行臂的末端的实际位姿；

基于所述目标位姿与所述实际位姿满足误差条件，使所述机器人系统进入安全模式；以及

确定所述机器人系统的故障状况。

2.根据权利要求1所述的方法，其特征在于，使所述机器人系统进入安全模式包括：

停用驱动所述执行臂的所述至少一个驱动装置；以及

允许所述机器人系统的用户使用手动模式。

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

响应于确定所述机器人系统的故障状况为可恢复故障，执行故障恢复操作；和/或

响应于确定所述机器人系统的故障状况为不可恢复故障，使所述机器人系统进入退出模式。

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

响应于所述故障恢复操作失败，使所述机器人系统进入所述退出模式；和/或

响应于所述故障恢复操作成功，使所述机器人系统退出所述安全模式。

5.根据权利要求3所述的方法，其特征在于，所述机器人系统包括用于中继所述控制装置发送的驱动信号的通信节点以及用于与所述通信节点通信的通信模块，

所述方法还包括：

向所述通信节点发送问询信号；以及

响应于无法接收到所述通信节点的回应信号，生成指示发生所述通信故障的通信故障报警信号和/或重启所述通信模块。

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

响应于无法从所述至少一个驱动装置接收状态信息，生成指示发生通信故障的通信故障报警信号和/或重启用于与所述至少一个驱动装置通信的通信模块。

7.根据权利要求5或6所述的方法，其特征在于，还包括：

响应于重启之后通信故障未恢复，使所述机器人系统进入退出模式。

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

接收所述至少一个驱动装置的状态信息；以及

响应于所述状态信息与所述控制装置发出的所述至少一个驱动装置的驱动信号满足驱动装置故障条件，生成驱动装置故障报警信号，所述驱动装置故障信号指示所述执行臂的驱动装置发生故障。

9.根据权利要求8所述的方法，其特征在于，还包括：

响应于所述状态信息与所述控制装置发出的所述至少一个驱动装置的驱动信号不满足驱动装置故障条件，生成执行臂故障报警信号和/或使所述机器人系统进入退出模式，所述执行臂故障报警信号指示发生执行臂故障。

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

判断所述目标位姿是否超出所述执行臂的运动极限；

响应于所述目标位姿超出所述执行臂的运动极限，生成指示超出运动极限的报警信号；以及

退出安全模式。

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

接收所述执行臂的至少一个关节的实际关节信息；

基于所述目标位姿，确定所述执行臂的至少一个关节的目标关节信息；以及

响应于所述实际关节信息与所述目标关节信息满足执行臂故障条件，生成执行臂故障报警信号和/或使所述机器人系统进入退出模式，所述执行臂故障报警信号指示发生执行臂故障。

12.根据权利要求1所述的方法，其特征在于，确定所述执行臂的末端的目标位姿包括：

预先确定或随机生成所述目标位姿；或者

基于主操作器的位姿，确定所述目标位姿。

13.根据权利要求1所述的方法，其特征在于，

基于所述角度标识和所述多个位姿标识，确定位姿标识坐标系相对执行臂的末端坐标系的滚转角；

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

基于所述位姿标识坐标系相对所述执行臂的末端坐标系的滚转角和所述位姿标识坐标系相对所述参考坐标系的位姿，确定所述实际位姿。

14.根据权利要求13所述的方法，其特征在于，包括：

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

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

基于所述角度标识和所述多个位姿标识，确定位姿标识坐标系相对执行臂的末端坐标系的滚转角；

基于所述位姿标识坐标系相对所述执行臂的末端坐标系的滚转角和所述多个位姿标识在位姿标识坐标系中的三维坐标，确定所述多个位姿标识在执行臂的末端坐标系中的三维坐标；以及

基于所述多个位姿标识在所述定位图像中的二维坐标和所述多个位姿标识在执行臂的末端坐标系中的三维坐标，确定所述实际位姿。

16.根据权利要求13-15中任一项所述的方法，其特征在于，包括：

确定所述角度标识在执行臂的末端坐标系中标识的第一绕轴角度；

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

基于所述第一绕轴角度和所述第二绕轴角度，确定所述位姿标识坐标系相对所述执行臂的末端坐标系的滚转角。

17.根据权利要求1、13-15中任一项所述的方法，其特征在于，所述位置关联关系包括：

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

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

基于所述多个位姿标识的分布，确定所述多个位姿标识在位姿标识坐标系中的三维坐标；

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

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

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

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

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

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

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

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

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

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

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

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

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

从所述定位图像中确定多个候选位姿标识；

基于位姿图案匹配模板，从所述多个候选位姿标识中识别初始位姿标识；以及

以所述初始位姿标识作为起点，搜索位姿标识。

23.根据权利要求1-6、8-15、18-22中任一项所述的方法，其特征在于，还包括：以预定周期确定所述执行臂的末端的实际位姿，以通过多个故障检测循环，实时确定所述机器人系统的故障状况。

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

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

处理器，与所述存储器耦合并且用于执行所述至少一条指令以执行根据权利要求1-23中任一项所述的机器人系统故障检测处理方法。

25.一种计算机可读存储介质，用于存储至少一条指令，所述至少一条指令由计算机执行时致使所述计算机执行根据权利要求1-23中任一项所述的机器人系统故障检测处理方法。

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

执行臂，所述执行臂的末端设置有至少一个角度标识和多个位姿标识；

至少一个驱动装置，用于驱动所述执行臂；

至少一个驱动装置传感器，与所述至少一个驱动装置耦合并且用于获得所述至少一个驱动装置的状态信息；

图像采集设备，用于采集所述执行臂的定位图像；以及

控制装置，被配置为与所述至少一个驱动装置、所述至少一个驱动装置传感器、所述图像采集设备连接，执行根据权利要求1-23中任一项所述的机器人系统故障检测处理方法。

Images