CN115246120A - 检测碰撞的方法及相应的控制器、机械关节和机械臂 - Google Patents

Abstract

本发明属于机器人技术领域，具体公开了一种检测碰撞的方法及相应的控制器、机械关节和机械臂。该方法用于检测自动化设备在规划路径中运动时是否发生碰撞，其中，所述自动化设备中包括机械关节和位置信息采集装置，所述位置信息采集装置用于采集所述机械关节的位置信息，包括如下步骤：所述方法包括如下步骤：监测所述位置信息采集装置输出的实时位置信息；计算所述实时位置信息与所述规划路径中相应的规划位置信息之间的误差；若所述误差超过预设的阈值，则判定为发生碰撞；否则不认为发生碰撞，继续监测所述实时位置信息，并继续比较所述误差和所述阈值。本发明的优点在于简化了计算过程，提高了机器人的反应速度。

Description

检测碰撞的方法及相应的控制器、机械关节和机械臂

技术领域

本发明涉及机器人工程技术领域，尤其涉及了一种检测碰撞的方法及相应的控制器、机械关节和机械臂。

背景技术

自工业机器人投入实际使用后，机器人所能应用的环境也越来越多。相对传统的应用于大规模生产的传统机器人，较新发展出来的协作机器人(Collaborative Robot)具有更灵活，能够实现自我控制的特点。也可以说，协作机器人具有更“智能化”的特点。

目前，协作机器人一般被定义为可以在一定的协作区域内与人(真人操作者)直接进行交互的机器人。随之而产生的一个问题是，为了保证真人操作者的安全，协作机器人必须具备碰撞检测以及自动停止的功能。

业内通常采用检测马达电流是否突变，即电流环的方式，来判断是否发生碰撞。电流环的方式是通过控制各机械关节的驱动马达的电流来实现的，需要建立动力学模型。需要将机械关节中的编码器的位置和速度信息进行转化，换算为力矩信息；还需要将机械关节的驱动马达的电流信息也换算为力矩信息。多次的换算将导致精度的降低，并且大多数动力学模型会利用速度编码器和位置编码器两个编码器的信息，由于机械关节是非绝对刚性的，两个编码器的信息不完全同步，这又将带来一部分误差。

为了弥补上述的误差，有时，也会在上述方式以外再附加力矩传感器(包括关节力矩传感器、基座力矩传感器、末端力矩传感器等方式)来提高检测精度。但是这一方案的成本过高，而且要在机器人的机械关节狭小的空间内加上力矩传感器，使结构设计变得非常复杂，同时还会增加机械关节的体积和重量。

因此，亟需发展一种更优的用于协作机器人的碰撞检测方法。

发明内容

为了解决上述缺陷，本发明首先提供了一种碰撞检测的方法，改方法用于检测自动化设备在规划路径中运动时是否发生碰撞，其中，所述自动化设备中包括机械关节和位置信息采集装置，所述位置信息采集装置用于采集所述机械关节的位置信息。

所述方法包括如下步骤：

所述方法包括如下步骤：

监测所述位置信息采集装置输出的实时位置信息；

计算所述实时位置信息与所述规划路径中相应的规划位置信息之间的误差；

若所述误差超过预设的阈值，则判定为发生碰撞；否则不认为发生碰撞，继续监测所述实时位置信息，并继续比较所述误差和所述阈值。

上述的方法中，判定为发生碰撞后，所述自动化设备停止继续运动并发出警报。

上述的方法中，所述阈值与驱动所述机械关节的马达的转速线性相关，即：

U_单＝a_单θ′+b_单

其中，U_单表示所述机械关节的阈值，a_单和b_单是与所述机械关节相关的经验系数，θ’为驱动所述机械关节的马达的转速。

上述的方法中，在分别单独考虑每个所述机械关节的情况下，对每个所述机械关节分别比较所述误差和阈值U_单，其中，U_单表示单独考虑每个所述机械关节的情况下的阈值，若其中有一个所述机械关节的所述误差超过阈值U_单，则判定为发生碰撞。

上述的方法中，将一组驱动轴平行的所述机械关节视作关联关节，在考虑所述关联关节的情况下，阈值U_关与驱动所述关联关节的马达的转速线性相关。例如，阈值U_关可以是基于经验得出的综合性的值；或者，阈值U_关还可以是所述关联关节中每个所述机械关节在考虑所述关联关节的情况下的独立阈值U_关的累计值，其中，i表示所述关联关节中的第i个机械关节的序号，即：

U_关＝∑U_关i＝∑a_关iθ′_i+∑b_关i

其中，U_关表示所述关联关节的阈值，U_关i表示所述关联关节中的第i个机械关节的独立阈值，a_关i和b_关i是与第i个机械关节相关的经验系数，θ′_i为驱动第i个机械关节的第i个马达的转速；

若所述关联关节中的所有所述机械关节的所述误差的和超过阈值U_关，则判定为发生碰撞。

上述的方法中，在整体考虑所有所述机械关节的情况下，阈值U_all与驱动所有所述机械关节的马达的转速线性相关。例如，阈值U_all可以是基于经验得出的综合性的值；或者，阈值U_all还可以是每个所述机械关节在整体考虑所有所述机械关节的情况下的独立阈值U_alli的累计值，其中，i表示第i个机械关节的序号，即：

U_all＝∑U_alli＝∑a_alliθ′_i+∑b_alli

其中，U_all表示所有所述机械关节的阈值，U_alli表示第i个所述机械关节的阈值，a_alli和b_alli是与第i个机械关节相关的经验系数，θ′_i为驱动第i个机械关节的第i个马达的转速；

若所有所述机械关节的所述误差的和超过阈值U_all，则判定为发生碰撞。

上述的方法中，所述阈值进一步包括重阈值和轻阈值，所述重阈值大于所述轻阈值，当所述误差或所述误差的和超过所述重阈值时，相应的所述机械关节沿原路返回，当所述误差超过所述轻阈值而未超过所述重阈值时，相应的所述机械关节停机。

上述的方法中，连续N次比较所述误差和所述阈值，若其中有P次的比较结果是所述误差超过所述阈值，则判定发生碰撞，否则不认为发生碰撞，其中，N为自然数，P≤N。

上述的方法中，所述P次为连续的P次或者为不连续的P次。

上述的方法中，在连续N次比较的过程中，当出现P’次所述比较结果是所述误差超过所述阈值时，减小驱动部件输出的力矩，以减缓所述机械关节的运动速度，其中P’＜P。

相对于现有技术，上述的方法仅采集一种信息，即位置信息，通过将实际的位置信息与预定的位置信息进行对比来判断机械设备是否发生了碰撞，不仅减少了需采集的信息，计算过程也仅包括加减计算和逻辑判断计算，大大简化了计算过程，而得出的判断结果依旧非常可信。

为了进一步提高判断的可信度，过滤噪点信息，本方法采用了连续多次比较的方式来具体实施判决过程，需在连续N次的比较中有P次(P≤N)的比较结果是超过阈值才判定发生碰撞，从而避免误报警。

本发明还提出了一种用于检测碰撞的控制器，该控制器用于检测自动化设备在规划路径中运动时是否发生碰撞，其中，所述自动化设备中包括机械关节，所述控制器包括；

信号采集单元，用于采集所述机械关节的实时位置信息：

误差计算单元，用于计算所述实时位置信息与所述规划路径中相应的规划位置信息之间的误差；

累计判定单元，用于根据上述的方法来判定是否发生碰撞。

上述的控制器中，还包括警报单元，所述警报单元用于在所述累计判定单元判定发生碰撞后，通过声/光/电的方式来报警和/或锁定所述机械关节，以及在危险解除后，通过声/光/电的方式来提示。

相对于现有技术，上述的方法和控制器不只考虑一个孤立的机械关节，还对相互关联的若干个机械关节受到碰撞的情况综合判定。其中，所述相互关联的机械关节是指，驱动轴平行的一组机械关节。采用综合判定的方式，可以从多角度考核碰撞的强度，通过设置更具有针对性的阈值，分别对单个关节的受力情况和相互关联的关节的受力情况进行判定，然后可根据实际应用环境确定两种受力情况的权值，综合确定判决逻辑。

更进一步，还可以综合考虑单个关节、相互关联的关联关节以及全部关节整体的受力情况，即首先分别对单个关节、关联关节以及全部关节整体的受力情况进行判定，然后根据实际应用环境确定三种受力情况的权值，综合确定判决逻辑。将整个自动化设备的所有机械关节都纳入考虑范围，从三个层面(单关节、关联关节、整机)对碰撞进行了考虑，根据权重的不同，可以对不同环境下/不同需求情况下的碰撞判决进行灵活调整。

本发明还提出了一种计算机可读的存储介质，所述存储介质中存储有指令，当所述指令被执行时，所述计算机实现如上所述的检测碰撞的方法。

相应的，本发明提出了一种机械关节，所述机械关节实施上述的碰撞检测方法。

相对于现有技术，上述的机械关节对碰撞的判决过程简单、迅速，还可以根据不同环境/不同需求灵活调整阈值和权值，大大增加了机械关节对环境的适应性。

相应的，本发明还提出了一种机械臂，所述机械臂实施如权利要求上述的碰撞检测方法。

相对于现有技术，上述的机械臂对碰撞的判决过程简单、迅速，还可以根据不同环境/不同需求灵活调整阈值和权值，大大增加了机械臂对环境的适应性。

附图说明

图1是本发明中机械关节的结构框图；

图2是本发明中机械关节的一个示意图；

图3是本发明中检测碰撞的方法的流程图；

图4是本发明中相互关联的机械关节的示意图；

图5是本发明中位置编码器输出的实际角度信息和理论角度信息的曲线对比图；

图6是本发明中一个示例性控制器的框图。

具体实施方式

以下由特定的具体实施例说明本发明的实施方式，本领域技术人员可由本说明书所揭示的内容轻易地了解本发明的其他优点及功效。虽然本发明的描述将结合较佳实施例一起介绍，但这并不代表此发明的特征仅限于该实施方式。恰恰相反，结合实施方式作发明介绍的目的是为了覆盖基于本发明的权利要求而有可能延伸出的其它选择或改造。为了提供对本发明的深度了解，以下描述中将包含许多具体的细节。本发明也可以不使用这些细节实施。此外，为了避免混乱或模糊本发明的重点，有些具体细节将在描述中被省略。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

应注意的是，在本说明书中，相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释，默认为同一定义。

还需声明的是，本发明中对方法、流程进行编号是为了便于引用，而不是限定先后顺序，各步骤之间如有先后顺序，当以文字说明为准。

本发明的发明构思的重点在于，摒弃现有的通过检测驱动马达的输出电流来判决自动化设备是否发生碰撞的思路，而是利用现有的自动化设备中普遍设置的位置信息采集装置来进行监测。具体的监测思路是，将监测到的实时位置信息与预先计算好的规划位置信息进行比较，当误差超过一定阈值时，判定自动化设备发生了碰撞。这样的方案不需要改动已有产品的已有结构，只需对控制软件进行升级即可。相对于布置一个自动化设备动辄就要半年以上的周期，软件升级可以大大缩短自动化设备投入使用的时间。

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步地详细描述。

协作机器人通常的工作场景是作为生产线上的一员，执行那些精度要求高、重复性大的劳动。协作机器人的活动范围内将会出现真人操作员，因此处于安全性的考虑，协作机器人需要有碰撞检测的功能。针对与人碰撞的情境，碰撞检测的精度必须更高，并且，检测的速度(即反应速度)越快越好。另一方面，协作机器人往往并不是仅参与单一产品的生产，当协作机器人更换岗位时，其碰撞检测的阈值通常也要做出调整，因此，能够适应经常调整参数也是预期之一。

图1是本发明中所论及的机械关节的结构框图，其示出了一个较为通用的机械关节的实施例框图。如图中所示，机械关节包活：马达1、减速器2、轴端编码器3、轴4和输出端5。

马达1(驱动部件)与轴4(传动部件)固定连接，用于带动轴4旋转，轴4与输出端5(力矩的输出端)固定连接，从而可驱动输出端5旋转。

减速器2用于降低马达1的转速。

轴端编码器3(信息采集装置)通常设置于接近输出端5的位置，以采集尽可能接近实际数值的数据。轴端编码器3可以包括速度编码器、位置编码器31(位置信息采集装置)等。位置编码器31优选设置于紧靠输出端5的位置，这样能够使采集到的位置信息最接近实际的位置偏移的数值。或者，受限于机械关节内部的空间限制，位置编码器31也可以以刚性连接的方式设置在离输出端5稍远的位置，例如图1中虚框所标注的位置编码器31的位置。图1中虚框标注的位置编码器31通过轴4与输出端5刚性连接，在这样的连接方式下，由于位置编码器31离实际受力位置有一定距离，会造成采集到的位置信息中包含固定偏差，但这一偏差可通过计算方程来纠正，本发明就这一点不展开讨论。

在图1所示结构的基础上，本发明仅监测位置编码器31输出的实时位置信息，将其与规划位置信息进行比较，进而实现碰撞检测的目的。

图2是本发明中一个实际的机械关节的示意图。图中所示的编码器31紧靠输出端5，处于比较有利于采集位置信息的位置，在后续的计算过程中，可以不需要加入修正固定误差的步骤。

图3以图2所示结构为基础，示出了本发明中检测碰撞的方法的一个实施例的流程图。图中的判决过程包括了以独立的一个机械关节为对象进行判决、以一组关联关节为对象进行判决以及以所有机械关节为总的对象从整体上进行判决三种判决逻辑。在实际应用中，可以根据需要选取其中的一种或两种逻辑来实际执行，也可以如图3所示，同时应用三种逻辑来实际执行，本文中不一一赘述。例如，若只选取第一种逻辑来实际执行碰撞检测的工作，则在实际应用中只孤立的考虑单个机械关节的受力情况，只要有一个(任一个)机械关节的误差超过阈值U_单，就需要停止/暂停协作机器人的工作。又如，若同时选取第一种和第二种逻辑来实际执行碰撞检测的工作，则在实际应用中就综合考虑了单个关节和关联关节的受力情况。当单个关节的误差超过阈值U_单或者/并且一组关联关节的误差和超过阈值U_关时，才需要停止协作机器人的工作。其中，关联关节是指驱动轴平行的一组机械关节。

本发明的检测碰撞的方法可以用于检测自动化设备(例如，协作机器人)在规划路径中运动时是否发生碰撞，其中，所述自动化设备中设有用于采集该自动化设备的位置改变信息的位置信息采集装置(例如，编码采集器31)。其中，所述的采集该自动化设备的位置改变信息也可以是采集该自动化设备的关键/敏感节点的位置改变信息。例如，本实施例中，对协作机器人的机械关节的位置改变进行监测。

图4所示为一个常见的协作机器人的示意图。图中的协作机器人包括6个机械关节42、43、45、47、48、49和连接在机械关节之间的连杆44、46。机械关节42、43、45、47、48、49可分别按图中虚线所示的旋转轴执行旋转的动作。该旋转的动作被位置信息采集装置(例如，编码采集器31)所采集到，从而可传输给控制器415，由控制器415对协作机器人的运动状态进行判决。所述协作机器人通过底座41固定在地面、桌面之类的固定物上。用于控制所述协作机器人的运动的控制器415通过电缆与所述协作机器人连接。在各所述机械关节开始运动前，协作机器人的控制器415对每一个所述机械关节的运动路径都制定好规划路径；在每一个采集信息的采集周期内，控制器415接收每一个所述机械关节中的位置编码器31采集到的实时位置信息；对应于位置编码器31采集数据的时间点，控制器415根据所述规划路径可以获取到规划位置信息；分别计算每一个实时位置信息和规划位置信息之间的误差，将该误差与一阈值相比较，若误差超出阈值范围，则可认为该机械关节的运动超出规划路径的范围。

回到图3，图3给出的实施例包括以下步骤：

步骤S1，采集每一个机械关节的实时位置信息。对于不同的位置信息采集装置，其采集到的信息是不同的，图2所示的机械关节中，采集到的是该机械关节的角度信息。且，本步骤，或者说本流程图所示的过程并不是一次性的，而是在协作机器人工作的过程中按周期T一直反复执行，直至出现碰撞报警，该采集—判定的过程才暂时停止。

步骤S2，计算步骤S1中采集到的实时位置信息与规划位置信息之间的误差。

步骤S3和S4，对于每个机械关节分别比较误差与阈值U_单，并记录本次循环中误差是否超出阈值U_单。当误差超出阈值时，可认为发生了碰撞。通过简单的减法计算和比较运算来判断是否发生碰撞大大简化了判决过程，提高了判决效率。根据背景技术中所提及的现有技术，需要根据力学原理、运动学原理以及马达的电流等参数建立计算模型，不仅运算比较复杂，其占用CPU的时间也较长，也就是说协作机器人的反应速度将会下降。而采用本实施例所述的判决方法，能提高协作机器人的反应速度。

进一步地，阈值U_单可以包括重阈值U_单U和轻阈值U_单L两种，重阈值是在比较严苛的条件下给出的阈值，一般当误差超过重阈值时，表示出现了比较严重的撞击。同理，轻阈值是在比较宽松的条件下给出的阈值，一般当误差超过轻阈值(但未超过重阈值)时，表示虽然出现了问题，但是影响不恶劣。因此，在本实施例的后续处理中，当误差大于轻阈值而小于重阈值时，协作机器人停机并报警，等待人工干预排除危险(步骤S11和S10)。而当误差超过重阈值时，协作机器人沿原路返回并报警。最终都在得到安全的通知的情况下继续执行下一次操作指令(步骤S9和S10)。

为了提高判决的可靠性，过滤噪声样本，本实施例采用多次误差超过阈值后才判决出现碰撞。若在连续N次的循环比较中有P次的记录是误差超过阈值的，则判定该机械关节/关联关节/所有关节整体发生了碰撞的情况，需要考虑是否报警。其中，根据具体使用环境的不同，所述的P次可以是连续的，也可以是间歇的。通常，两种情况下的P的设定也是不同的。

作为一个折中方案，在连续N次的循环比较过程中，当出现P’次(P’＜P)比较结果是误差超过阈值时，减小驱动部件输出的力矩，以减缓所述机械关节的运动速度，从而达到降低可能的碰撞所带来的损坏/损失的目的。

步骤S5和S6，对于一组关联关节，例如图4所示的机械关节43、45、47，则可累计该组关联关节中所有机械关节的误差和是否超过阈值U_关。本领域技术人员应知，根据前文对关联关节的定义，图4中还存在由机械关节43、45、47、49组成的一组关联关节，还可以存在由机械关节42、48组成的一组关联关节，以及其他符合定义的关联关节。实际应用本发明时，关注哪些关联关节可以由实际需要来确定。

继续讨论步骤S5和S6，与上文类同，阈值U_关可以包括重阈值U_关U和轻阈值U_关L两种，在后续处理中，当误差大于轻阈值而小于重阈值时，协作机器人停机，等待人工干预排出危险(步骤S11和S10)。而当误差超过重阈值时，协作机器人沿原路返回并报警，以等待安全的通知和/或下一次操作指令(步骤S9和S10)。

步骤S7和S8，对于整个协作机器人的所有机械关节进行整体考虑的情况，累计所有机械关节的误差和是否超过阈值U_all(重阈值U_allU和轻阈值U_allL)。其累计方式和判决逻辑与之前所述相类似，此处不再赘述，区别仅在于，连续的次数N3与记录误差超过第三阈值的次数P3的取值可能与步骤S4中的取值可以不同。

本实施例中的三种逻辑是顺序执行，即，若在步骤S4中就判定发生碰撞，那么就认为协作机器人已经发生了碰撞，需要进入下一步的暂停或人工干预的步骤。

在其他实施例中，还可以并行执行上述三种逻辑，即，同时分别判断单个机械关节、关联关节以及所有机械关节整体上是否发生碰撞，得到三个是否碰撞的结论。然后对这些结论进的逻辑运算，从而决定是否需要报警以及停止动作。此处的逻辑运算可以是逻辑与也可以是逻辑或，还可以是逻辑与和逻辑或的综合运算，具体可由使用环境来决定。一个安全程度较高的实施例中，可选择逻辑或的运算，即只要上述的三个是否碰撞的结论中有一个是发生碰撞的情况，就停止工作，等待人工介入。

出于保障生产安全的目的，步骤S3-S4中，若在连续N次的循环比较中已出现P’次(P’＜P)误差超过阈值U_单的情况，则可减小输出端5的输出力矩，以减缓相应机械关节的动作。同理，步骤S5和S6以及和步骤S7和S8中也可同样执行上述减缓动作的操作，其不同之处在于，N、P和P’的值可以设置为不同的值。

图5是本发明中位置编码器输出的实际角度信息和理论角度信息的曲线对比图。其中，实线为位置编码器31实际采集到的位置信息(即编码器角度值)，虚线为规划路径中的位置信息。由图可以看出，在时刻t₀，虚线和实线是重合的，即此时协作机器人(或者说协作机器人的某一机械关节)是按规划路径运动的，在时刻t₁和t₂，实线和虚线出现了明显的分歧，即θ_1实，θ_2实，和θ_1设，θ_2设之间出现了明显的误差，也就是说，此时协作机器人(或者说协作机器人的某一机械关节)出现了异常的运动，未按规划路径运动。计算|θ_1设-θ_1实|和|θ_2设-θ_2实|，得到t₁和t₂时刻的误差，将其与阈值相比，得到的结果用于判决所述协作机器人是否遇到碰撞的情况。具体的判决原则，则可参考有关图3的流程图的说明。

回到图3，根据对多年实践的总结，本发明中的阈值与驱动所述机械关节的马达的转速线性相关，即阈值可通过以下公式计算：

U_单＝a_单θ′+b_单 (1)

其中，U_单表示所述机械关节的阈值，a_单和b_单是与所述机械关节相关的经验系数，θ’为驱动所述机械关节的马达的转速。

由于马达的转速θ’并非一成不变，因此，根据公式(1)所确定的阈值是一个动态的值，根据该动态的阈值进行的判决则具有更高可信度，其与协作机器人当前的运动速度的相关性更深。

在分别单独考虑每个所述机械关节的情况下，对每个所述机械关节分别比较所述误差和所述阈值，若其中有一个所述机械关节的所述误差超过阈值U_单，则判定为发生碰撞。

在考虑关联关节的情况下，阈值U_关与驱动所述关联关节的马达的转速线性相关。例如，阈值U_关可以是基于经验得出的综合性的值。也就是说，公式(1)中的a、b、θ’均为综合性的取值，而非实测得出的数据。例如，a、b、θ’的值取各机械关节的平均值，或其他本领域技术人员可以采用的统计值。

或者，阈值U_关还可以是所述关联关节中每个所述机械关节在考虑所述关联关节的情况下的独立阈值U_关；的累计值，其中，i表示所述关联关节中的第i个机械关节的序号，即：

U_关＝∑U_关i＝∑a_关iθ′_i+∑b_关i (2)

其中，U_关表示所述关联关节的阈值，U_关i表示所述关联关节中的第i个机械关节的独立阈值，a_关i和b_关i是与第i个机械关节相关的经验系数，θ′_i为驱动第i个机械关节的第i个马达的转速。

若所述关联关节中的所有所述机械关节的所述误差的和超过阈值U_关，则判定为发生碰撞。

在整体考虑所有所述机械关节的情况下，阈值U_all与驱动所有所述机械关节的马达的转速线性相关。例如，类似对阈值U_关的陈述，阈值U_all可以是基于经验得出的综合性的值。

或者，所述阈值U_all还可以是每个所述机械关节在整体考虑所有所述机械关节的情况下的独立阈值U_alli的累计值，其中，i表示第i个机械关节的序号，即：

U_all＝∑U_alli＝∑a_alliθ′_i+∑b_alli (3)

其中，U_all表示所有所述机械关节的阈值，U_alli表示第i个所述机械关节的阈值，a_alli和b_alli是与第i个机械关节相关的经验系数，θ′_i为驱动第i个机械关节的第i个马达的转速。

若所有所述机械关节的所述误差的和超过阈值U_all，则判定为发生碰撞。

对于公式(1)、(2)和(3)中的经验系数a_单和b_单、a_关i和b_关i、，a_alli和b_alli通常是不同的。以下提供一组实际使用的阈值计算公式。该实例中的经验系数与协作机器人的重量、臂长等条件相关，不同机器人应在多次实践的基础上修改不同的经验系数。

①各机械关节单独判定：

②机械关节42、43、45组成的关联关节联合判定，其中，θ′₂为机械关节43的马达的转速，θ′₃为机械关节45的马达的转速，θ′₄为机械关节47的马达的转速：

轻阈值	U<sub>关L</sub>＝0.0009θ′<sub>2</sub>+0.001θ′<sub>3</sub>+0.0008θ′<sub>4</sub>+0.055
		重阈值	U<sub>关U</sub>＝0.0016θ′<sub>2</sub>+0.002θ′<sub>3</sub>+0.0016θ′<sub>4</sub>+0.011

③所有机械关节整体判定，其中：

图6是本发明中一个示例性控制器415的框图。控制器415包括信号采集单元71、误差计算单元72、累计判定单元73、警报单元74。

信号采集单元71，用于采集所述机械关节的实时位置信息：

误差计算单元72，用于计算所述实时位置信息与所述规划路径中相应的规划位置信息之间的误差；

累计判定单元73，用于实现碰撞判决过程。例如，可按如图3所示的方法来进行判决。

警报单元74，所述警报单元用于在所述累计判定单元判定发生碰撞后，通过声/光/电的方式来报警和/或锁定所述机械关节，以提醒工作人员及时介入，排除危险。还可以在危险解除(安全)后，通过声/光/电的方式来提示可重新开始预定的工作。

本发明还提供一种计算机可读的存储介质，所述存储介质中存储有指令，当所述指令被执行时，所述计算机实现如上所述的检测碰撞的方法。

在本文所提供的说明中，提供了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

类似地，应当理解，为了精简本发明并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如权利要求书所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。

本领域那些技术人员可以理解，可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件，以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外，可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述，本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的替代特征来代替。

此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。

Claims (15)

1.一种检测碰撞的方法，其特征在于，用于检测自动化设备在规划路径中运动时是否发生碰撞，其中，所述自动化设备中包括机械关节和位置信息采集装置，所述位置信息采集装置用于采集所述机械关节的位置信息；

所述方法包括如下步骤：

监测所述位置信息采集装置输出的实时位置信息；

计算所述实时位置信息与所述规划路径中相应的规划位置信息之间的误差；

若所述误差超过预设的阈值，则判定为发生碰撞；否则不认为发生碰撞，继续监测所述实时位置信息，并继续比较所述误差和所述阈值。

2.如权利要求1所述的方法，其特征在于，判定为发生碰撞后，所述自动化设备停止继续运动并发出警报。

3.如权利要求1所述的方法，其特征在于，所述阈值与驱动所述机械关节的马达的转速线性相关。

4.如权利要求3所述的方法，其特征在于，在分别单独考虑每个所述机械关节的情况下，对每个所述机械关节分别比较所述误差和阈值U_单，其中，U_单表示单独考虑每个所述机械关节的情况下的阈值，若其中有一个所述机械关节的所述误差超过所述阈值U_单，则判定为发生碰撞。

5.如权利要求3所述的方法，其特征在于，将一组驱动轴平行的所述机械关节视作关联关节，在考虑所述关联关节的情况下，阈值U_关与驱动所述关联关节的马达的转速线性相关；若所述关联关节中的所有所述机械关节的所述误差的和超过所述阈值U_关，则判定为发生碰撞。

6.如权利要求3所述的方法，其特征在于，在整体考虑所有所述机械关节的情况下，阈值U_all与驱动所有所述机械关节的马达的转速线性相关；若所有所述机械关节的所述误差的和超过阈值U_all，则判定为发生碰撞。

7.如权利要求3-6任一项所述的方法，其特征在于，所述阈值进一步包括重阈值和轻阈值，所述重阈值大于所述轻阈值，当所述误差或所述误差的和超过所述重阈值时，相应的所述机械关节沿原路返回，当所述误差超过所述轻阈值而未超过所述重阈值时，相应的所述机械关节停机。

8.如权利要求1-6任一项所述的方法，其特征在于，连续N次比较所述误差和所述阈值，若其中有P次的比较结果是所述误差超过所述阈值，则判定发生碰撞，否则不认为发生碰撞，其中，N为自然数，P≤N。

9.如权利要求8所述的方法，其特征在于，所述P次为连续的P次或者为不连续的P次。

10.如权利要求8所述的方法，其特征在于，在连续N次比较的过程中，当出现P’次所述比较结果是所述误差超过所述阈值时，以减缓所述机械关节的运动速度，其中P’＜P。

11.一种用于检测碰撞的控制器，其特征在于，用于检测自动化设备在规划路径中运动时是否发生碰撞，其中，所述自动化设备中包括机械关节，所述控制器包括；

信号采集单元，用于采集所述机械关节的实时位置信息：

误差计算单元，用于计算所述实时位置信息与所述规划路径中相应的规划位置信息之间的误差；

累计判定单元，用于根据权利要求3-10中的任一项所述的方法来判定是否发生碰撞。

12.如权利要求11所述的控制器，其特征在于，还包括警报单元，所述警报单元用于在所述累计判定单元判定发生碰撞后，通过声/光/电的方式来报警和/或锁定所述机械关节，以及在危险解除后，通过声/光/电的方式来提示。

13.一种计算机可读的存储介质，其特征在于，所述存储介质中存储有指令，当所述指令被执行时，所述计算机实现如权利要求1-10任一项所述的检测碰撞的方法。

14.一种机械关节，其特征在于，所述机械关节实施如权利要求1-10任一项所述的碰撞检测方法。

15.一种机械臂，其特征在于，所述机械臂实施如权利要求1-10任一项所述的碰撞检测方法。

Images