机器人的安全控制方法和系统

技术领域

本发明主要涉及机器人技术领域，尤其涉及一种机器人的安全控制方法和系统。

背景技术

共融机器人(Coexisting-Cooperative-Cognitive Robots,Tri-Co Robots)是指能与作业环境、人和其它机器人自然交互、自主适应复杂动态环境并协同作业的机器人。共融机器人中最重要的一环在于人机共处的安全性。据统计，目前机器人安全事故中，大部分事故为人被夹伤和撞击致伤。因此需要机器人具备碰撞检测的功能。

目前碰撞检测的方案中，常在机器人关节处安装关节力矩传感器和双编码器来估计外力矩。这种碰撞检测方式比较单一，缺乏安全冗余性，一旦关节力矩传感器或双编码器发生故障，则碰撞检测失败。并且这种方案需要的关节力矩传感器和双编码器成本较高，应用有限。

因此，亟需一种可靠的、适用性广的机器人安全控制方法。

发明内容

本发明要解决的技术问题是提供一种机器人的安全控制方法和系统，解决现有碰撞检测的方案缺乏安全冗余性的问题。

为解决上述技术问题，本发明提供了一种机器人的安全控制方法，包括：通过第一传感单元采集机器人各个关节的关节角；根据所述关节角计算机器人末端的第一末端位姿，判断所述第一末端位姿是否超过第一预设限制值，如果是，则触发安全机制；通过第二传感单元采集机器人基座的基座数据和机器人末端的末端数据，所述基座数据包括基座加速度和基座磁场位置，所述末端数据包括末端加速度、末端磁场位置、末端角速率；根据所述基座数据和所述末端数据计算机器人末端的第二末端位姿，判断所述第二末端位姿是否超过所述第一预设限制值，如果是，则触发安全机制。

可选地，所述第一传感单元包括多个编码器，每个编码器位于一个关节附近。

可选地，所述第二传感单元包括安装于机器人基座的第一惯性传感器和第一地磁传感器，及安装于机器人末端的第二惯性传感器、第二地磁传感器和MEMS陀螺仪。

可选地，通过第二传感单元采集机器人基座的基座数据和机器人末端的末端数据包括：通过所述第一惯性传感器采集所述基座加速度，通过所述第一地磁传感器采集所述基座磁场位置；通过所述第二惯性传感器采集所述末端加速度，通过所述第二地磁传感器采集所述末端磁场位置；通过所述MEMS陀螺仪采集所述末端角速率。

可选地，根据所述关节角计算机器人末端的第一末端位姿包括：根据所述关节角进行机器人正解运算得到所述第一末端位姿。

可选地，根据所述基座数据和所述末端数据计算机器人末端的第二末端位姿包括：根据所述基座数据和所述末端数据计算机器人末端坐标系到机器人基坐标系的变换矩阵；根据所述变换矩阵计算所述末端加速度在所述机器人基坐标下的第二末端加速度；对所述第二末端加速度进行积分得到第二末端速度，对所述第二末端速度进行积分得到所述第二末端位姿。

可选地，所述安全机制包括预警、减速、制动和停止。

可选地，方法还包括：计算所述第一末端位姿与所述第二末端位姿的差值，判断所述差值是否超过第一阈值，如果是，则触发安全机制。

可选地，判断所述差值是否超过第一阈值包括：判断所述第一末端位姿与所述第二末端位姿中任何一个分量的差值，是否超过第一阈值，如果是，则触发安全机制。

可选地，方法还包括：根据所述关节角计算机器人末端的第一末端速度，判断所述第一末端速度是否超过第二预设限制值，如果是，则触发安全机制；根据所述基座数据和所述末端数据计算机器人末端的第二末端速度，判断所述第二末端速度是否超过所述第二预设限制值，如果是，则触发安全机制。

可选地，根据所述关节角计算机器人末端的第一末端速度包括：对所述关节角进行差分处理对得到各个关节的关节速度；根据所述关节速度和雅可比矩阵得到所述第一末端速度。

可选地，根据所述基座数据和所述末端数据计算机器人末端的第二末端速度包括：根据所述基座数据和所述末端数据计算机器人末端坐标系到机器人基坐标系的变换矩阵；根据所述变换矩阵计算所述末端加速度在所述机器人基坐标下的第二末端加速度；对所述第二末端加速度进行积分得到所述第二末端速度。

可选地，方法还包括：计算所述第一末端速度与所述第二末端速度的速度差值，判断所述速度差值是否超过第二阈值，如果是，则触发安全机制。

为解决上述技术问题，本发明提供了一种机器人的安全控制系统，包括：第一检测单元，包括：第一传感单元，包括多个编码器，每个编码器位于一个关节附近，所述第一传感系配置为采集机器人各个关节的关节角；第一控制单元，配置为根据所述关节角计算机器人末端的第一末端位姿，判断所述第一末端位姿是否超过第一预设限制值，如果是，则触发安全机制；第二检测单元，包括：第二传感单元，包括安装于机器人基座的第一惯性传感器和第一地磁传感器，及安装于机器人末端的第二惯性传感器、第二地磁传感器和MEMS陀螺仪；所述第二传感单元配置为采集机器人基座的基座数据和机器人末端的末端数据，所述基座数据包括基座加速度和基座磁场位置，所述末端数据包括末端加速度、末端磁场位置、末端角速率；第二控制单元，配置为根据所述基座数据和所述末端数据计算机器人末端的第二末端位姿，判断所述第二末端位姿是否超过所述第一预设限制值，如果是，则触发安全机制。

可选地，所述第二控制单元还配置为：根据所述基座数据和所述末端数据计算机器人末端坐标系到机器人基坐标系的变换矩阵；根据所述变换矩阵计算所述末端加速度在所述机器人基坐标下的第二末端加速度；对所述第二末端加速度进行积分得到第二末端速度，对所述第二末端速度进行积分得到所述第二末端位姿。

可选地，所述第一控制单元还配置为根据所述关节角计算机器人末端的第一末端速度，判断所述第一末端速度是否超过第二预设限制值，如果是，则触发安全机制；所述第二控制单元还配置为根据所述基座数据和所述末端数据计算机器人末端的第二末端速度，判断所述第二末端速度是否超过所述第二预设限制值，如果是，则触发安全机制。

可选地，所述第一控制单元还配置为对所述关节角进行差分处理对得到各个关节的关节速度；根据所述关节速度和雅可比矩阵得到所述第一末端速度。

可选地，系统还包括：第三控制单元，配置为从所述第一控制单元获取所述第一末端位姿，从所述第二控制单元获取所述第二末端位姿，计算所述第一末端位姿与所述第二末端位姿的差值，判断所述差值是否超过第一阈值，如果是，则触发安全机制。

与现有技术相比，本发明具有以下优点：

本发明的机器人的安全控制方法和系统，提供两套相互独立的检测方法来观测机器人末端运动参数信息，具备较好的安全冗余性，增加机器人系统可靠性和稳定性；本发明采用惯性传感器、地磁传感器和MEMS陀螺仪来计算第二末端位姿，成本较低，适用性更广。

附图说明

包括附图是为提供对本申请进一步的理解，它们被收录并构成本申请的一部分，附图示出了本申请的实施例，并与本说明书一起起到解释本发明原理的作用。附图中：

图1是根据本发明一实施例的机器人系统的示意图；

图2是根据本发明一实施例的机器人关节的结构示意图；

图3是图1中机器人的安全控制系统的系统框图；

图4是根据本发明一实施例的机器人的安全控制方法的流程图；

图5是图4中步骤S44的流程图。

具体实施方式

为了更清楚地说明本申请的实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。除非从语言环境中显而易见或另做说明，图中相同标号代表相同结构或操作。

如本申请和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本申请保护范围的限制。此外，尽管本申请中所使用的术语是从公知公用的术语中选择的，但是本申请说明书中所提及的一些术语可能是申请人按他或她的判断来选择的，其详细含义在本文的描述的相关部分中说明。此外，要求不仅仅通过所使用的实际术语，而是还要通过每个术语所蕴含的意义来理解本申请。

本申请中使用了流程图用来说明根据本申请的实施例的系统所执行的操作。应当理解的是，前面或下面操作不一定按照顺序来精确地执行。相反，可以按照倒序或同时处理各种步骤。同时，或将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。

图1是根据本发明一实施例的机器人系统的示意图。如图1所示，机器人系统100包括机器人本体10和机器人的安全控制系统。机器人本体10包括机器人基座111、多个关节112、多个机械臂113和机器人末端114。多个关节112包括基座与机械臂之间的关节、机械臂与机械臂之间的关节。关节主要起连接、传动及驱动机械臂以期望的轨迹运动的作用。图2是根据本发明一实施例的机器人关节的结构示意图。如图2所示，关节200包括驱动电机211和减速器212。其中驱动电机211为机器人动力源。减速器212用于将驱动电机211输出的扭矩和转速转换为机械臂B需要的扭矩和转速。

图3是图1中机器人的安全控制系统的系统框图。如图3所示，机器人的安全控制系统300包括第一检测单元31和第二检测单元32。第一检测单元31包括第一传感单元311和第一控制单元312。第二检测单元32包括第二传感单元321和第二控制单元322。第一传感单元311包括多个编码器，每个编码器位于一个关节附近，第一传感单元配置为采集机器人各个关节的关节角。如图2所示，在本实施例中，将编码器213放置在减速器212的输出侧，用于检测关节的关节角。在一些实施例中，编码器213也可以放置在减速器212的输入侧，本申请对此不作限制。

如图1所示，第二传感单元321可以包括安装于机器人基座的第一惯性传感器121和第一地磁传感器122，及安装于机器人末端的第二惯性传感器123、第二地磁传感器124和MEMS陀螺仪125。第一惯性传感器121用于采集机器人基座111的基座加速度。第一地磁传感器122用于采集机器人基座111的基座磁场位置。第二惯性传感器123用于采集机器人末端114的末端加速度。第二地磁传感器124用于采集机器人末端114的末端磁场位置。MEMS陀螺仪125用于采集机器人末端114的末端角速率。第一惯性传感器121和第二惯性传感器123可以为三轴加速度传感器，可以分别检测机器人基座和机器人末端在笛卡尔空间三个方向上的加速度分量。机器人的安全控制系统用于控制机器人本体在预设限制范围内运动，避免出现机器人本体与人之间非正常的接触或碰撞。

如图3所示，第一控制单元312配置为根据第一传感单元311获取的关节角计算机器人末端的第一末端位姿，判断第一末端位姿是否超过第一预设限制值，如果是，则触发安全机制。假设第一传感单元311采集的机器人各个关节的关节角为(q

第二控制单元322配置为根据第二传感单元321获取的基座数据和末端数据计算机器人末端的第二末端位姿，基座数据包括基座加速度和基座磁场位置，末端数据包括末端加速度、末端磁场位置、末端角速率。判断第二末端位姿是否超过第一预设限制值，如果是，则触发安全机制。当机器人静止时，即当第一传感单元311采集得到的各个关节的关节角不变时，假设第一惯性传感器采集的基座加速度为(g

1)根据基座数据和末端数据计算机器人末端坐标系到机器人基坐标系的变换矩阵。

本发明涉及的机器人坐标系包括机器人基坐标系、机器人末端坐标系和世界坐标系。其中机器人基坐标系是以机器人基座为基准、用来描述机器人本体运动的直角坐标系，其xy平面与机器人基座平行，z轴则指向上方，如果机器人是倒置安装的，那z轴指向下方。世界坐标系是以大地作为参考的直角坐标系。机器人末端坐标系是以工具中心点为原点建立的坐标系，其默认的工具中心点是机器人末端的中心位置。

首先，根据第一惯性传感器采集的基座加速度(g

其中，ψ1、θ1、γ1为由机器人基座变换到世界坐标系的姿态变换角。在一些实施例中，θ1、γ1、ψ1可以被如下公式导出：

其中，g为重力加速度。

然后，根据第二惯性传感器采集的末端加速度(g

其中，ψ2、θ2、γ2为由机器人末端变换到世界坐标系的姿态变换角。在一些实施例中，θ2、γ2、ψ2可以被如下公式导出：

其中，g为重力加速度。

根据机器人基座相对于世界坐标系的旋转矩阵R

R

其中R

由上可知，当机器人静止时，机器人末端相对于机器人基坐标系的初始姿态矩阵R

当机器人运动时，可以采集安装于机器人末端的MEMS陀螺仪输出的角速率(ω

下面以欧拉角方法为例，给出通过欧拉角方法推导姿态角的步骤：

B

其中，k表示第k次运算，T

2)根据变换矩阵计算末端加速度在机器人基坐标下的第二末端加速度。

可以通过如下公式计算第二末端加速度：

/>

其中

3)对第二末端加速度进行积分得到第二末端速度，对第二末端速度进行积分得到第二末端位姿。

具体地，对第二末端加速度

x

y

z

而机器人姿态可以由旋转变换阵R

判断第二末端位姿(x

在一些实施例中，第一控制单元312还配置为根据关节角计算机器人末端的第一末端速度，判断第一末端速度是否超过第二预设限制值，如果是，则触发安全机制。优选地，第一控制单元还配置为对关节角进行差分处理对得到各个关节的关节速度；根据关节速度和雅可比矩阵得到第一末端速度。具体的，第一控制单元312对第一传感单元311采集的关节角(q

其中，v是末端速度矢量，

判断第一末端速度(v

优选地，可以通过如下公式计算第一末端和速度：

其中v

在一些实施例中，第二控制单元322还配置为根据第二传感单元321获取的基座数据和末端数据计算机器人末端的第二末端速度，判断第二末端速度是否超过第二预设限制值，如果是，则触发安全机制。如上，计算第二末端位姿时，根据变换矩阵可以计算末端加速度(g

优选地，可以通过如下公式计算第二末端和速度：

其中v

可以设置第二预设限制值为(T

在一些实施例中，机器人的安全控制系统还包括第三控制单元(未示出)。第三控制单元配置为从第一控制单元获取第一末端位姿(x

在一些实施例中，第三控制单元还配置为从第一控制单元获取第一末端速度(v

本发明的机器人的安全控制系统提供两套相互独立的检测单元来观测机器人末端运动参数信息，具备较好的安全冗余性，增加机器人系统可靠性和稳定性，适用性更广。

图4是根据本发明一实施例的机器人的安全控制方法的流程图。如图4所示，机器人的安全控制方法400包括如下步骤：

步骤S41：通过第一传感单元采集机器人各个关节的关节角。

优选地，第一传感单元包括多个编码器，每个编码器位于一个关节附近。编码器可以放置在关节的减速器的输出侧，也可以放置在减速器的输入侧。通过编码器采集各个机器人各个关节的关节角。采集的机器人各个关节的关节角可以为(q

步骤S42：根据关节角计算机器人末端的第一末端位姿，判断第一末端位姿是否超过第一预设限制值，如果是，则触发安全机制。

优选地，根据关节角(q

步骤S43：通过第二传感单元采集机器人基座的基座数据和机器人末端的末端数据，基座数据包括基座加速度和基座磁场位置，末端数据包括末端加速度、末端磁场位置、末端角速率。

优选地，第二传感单元包括安装于机器人基座的第一惯性传感器和第一地磁传感器，及安装于机器人末端的第二惯性传感器、第二地磁传感器和MEMS陀螺仪。通过第二传感单元采集机器人基座的基座数据和机器人末端的末端数据包括：通过第一惯性传感器采集基座加速度，通过第一地磁传感器采集基座磁场位置；通过第二惯性传感器采集末端加速度，通过第二地磁传感器采集末端磁场位置；通过MEMS陀螺仪采集末端角速率。

步骤S44：根据基座数据和末端数据计算机器人末端的第二末端位姿，判断第二末端位姿是否超过第一预设限制值，如果是，则触发安全机制。

图5是图4中步骤S44的流程图。如图5所示，根据基座数据和末端数据计算机器人末端的第二末端位姿包括：

步骤S441：根据基座数据和末端数据计算机器人末端坐标系到机器人基坐标系的变换矩阵。

当机器人静止时，即当采集得到的各个关节的关节角不变时，假设采集的基座加速度为(g

R

其中R

当机器人运动时，采集角速率(ω

步骤S442：根据变换矩阵计算末端加速度在机器人基坐标下的第二末端加速度。

可以通过如下公式计算第二末端加速度：

其中

步骤S443：对第二末端加速度进行积分得到第二末端速度，对第二末端速度进行积分得到第二末端位姿。

具体地，对第二末端加速度

x

y

z

而机器人姿态可以由旋转变换阵R

关于步骤S441～S443的更详细说明可以参考上述图3中第二控制单元322的配置说明，在此不再赘述。

判断第二末端位姿(x

在一些实施例中，方法还包括步骤：根据关节角计算机器人末端的第一末端速度，判断第一末端速度是否超过第二预设限制值，如果是，则触发安全机制。其中根据关节角计算机器人末端的第一末端速度包括对关节角(q

其中，v是末端速度矢量，

优选地，可以通过如下公式计算第一末端和速度：

其中v

在一些实施例中，方法还包括步骤：根据基座数据和末端数据计算机器人末端的第二末端速度，判断第二末端速度是否超过第二预设限制值，如果是，则触发安全机制。其中根据基座数据和末端数据计算机器人末端的第二末端速度包括：根据基座数据和末端数据计算机器人末端坐标系到机器人基坐标系的变换矩阵；根据变换矩阵计算末端加速度在机器人基坐标下的第二末端加速度；对第二末端加速度进行积分得到第二末端速度。计算第二末端位姿时，根据变换矩阵可以计算末端加速度(g

优选地，可以通过如下公式计算第二末端和速度：

其中v

在一些实施例中，方法还包括步骤：计算第一末端位姿与第二末端位姿的差值，判断差值是否超过第一阈值，如果是，则触发安全机制。其中判断差值是否超过第一阈值包括：判断第一末端位姿与第二末端位姿中任何一个分量的差值，是否超过第一阈值，如果是，则触发安全机制。计算第一末端位姿(x

在一些实施例中，方法还包括步骤：计算第一末端速度与第二末端速度的差值，判断差值是否超过第二阈值，如果是，则触发安全机制。优选地，计算第一末端速度(v

上文已对基本概念做了描述，显然，对于本领域技术人员来说，上述发明披露仅仅作为示例，而并不构成对本申请的限定。虽然此处并没有明确说明，本领域技术人员可能会对本申请进行各种修改、改进和修正。该类修改、改进和修正在本申请中被建议，所以该类修改、改进、修正仍属于本申请示范实施例的精神和范围。

同时，本申请使用了特定词语来描述本申请的实施例。如“一个实施例”、“一实施例”、和/或“一些实施例”意指与本申请至少一个实施例相关的某一特征、结构或特点。因此，应强调并注意的是，本说明书中在不同位置两次或多次提及的“一实施例”或“一个实施例”或“一替代性实施例”并不一定是指同一实施例。此外，本申请的一个或多个实施例中的某些特征、结构或特点可以进行适当的组合。

本申请的一些方面可以完全由硬件执行、可以完全由软件(包括固件、常驻软件、微码等)执行、也可以由硬件和软件组合执行。以上硬件或软件均可被称为“数据块”、“模块”、“引擎”、“单元”、“组件”或“系统”。处理器可以是一个或多个专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理器件(DAPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器或者其组合。此外，本申请的各方面可能表现为位于一个或多个计算机可读介质中的计算机产品，该产品包括计算机可读程序编码。例如，计算机可读介质可包括，但不限于，磁性存储设备(例如，硬盘、软盘、磁带……)、光盘(例如，压缩盘CD、数字多功能盘DVD……)、智能卡以及闪存设备(例如，卡、棒、键驱动器……)。

计算机可读介质可能包含一个内含有计算机程序编码的传播数据信号，例如在基带上或作为载波的一部分。该传播信号可能有多种表现形式，包括电磁形式、光形式等等、或合适的组合形式。计算机可读介质可以是除计算机可读存储介质之外的任何计算机可读介质，该介质可以通过连接至一个指令执行系统、装置或设备以实现通讯、传播或传输供使用的程序。位于计算机可读介质上的程序编码可以通过任何合适的介质进行传播，包括无线电、电缆、光纤电缆、射频信号、或类似介质、或任何上述介质的组合。

同理，应当注意的是，为了简化本申请披露的表述，从而帮助对一个或多个发明实施例的理解，前文对本申请实施例的描述中，有时会将多种特征归并至一个实施例、附图或对其的描述中。但是，这种披露方法并不意味着本申请对象所需要的特征比权利要求中提及的特征多。实际上，实施例的特征要少于上述披露的单个实施例的全部特征。

虽然本申请已参照当前的具体实施例来描述，但是本技术领域中的普通技术人员应当认识到，以上的实施例仅是用来说明本申请，在没有脱离本申请精神的情况下还可作出各种等效的变化或替换，因此，只要在本申请的实质精神范围内对上述实施例的变化、变型都将落在本申请的权利要求书的范围内。