CN113681567A - 一种新型无传感器机器人碰撞检测方法及其系统 - Google Patents

Abstract

本发明公开的新型无传感器机器人碰撞检测方法，包括如下步骤：设置机器人的动作、姿态、路径规划；根据机器人的姿态、路径规划，计算各个动作中机械臂正常工作的估算电流、动态电流阈值并设定检测阈值；在机器人运动过程中，实时采集机械臂的实际电流，将实际电流与动态电流阈值相比较得到误差电流r；将误差电流r与设定的检测阈值相比较，若误差电流r大于检测阈值，则判定机器人的机械臂受到碰撞，此时执行碰撞反应程序；若误差电流r小于检测阈值，按照既定程序执行下一个动作；重复步骤S3、S4，直至完成所有动作。本发明通过电流的变化来检测外部碰撞力的大小，并控制机器人的启停以保证碰撞的迅速停止，不使用额外的传感器。

Description

一种新型无传感器机器人碰撞检测方法及其系统

技术领域

本发明涉及机器人碰撞检测防护技术领域，具体涉及一种新型无传感器机器人碰撞检测方法及其系统。

背景技术

随着涉及人类与各类机器人的协作任务越来越频繁，安全性成为需要首要考虑的问题。在发生机器人故障或误操作的情况下，人与机器人碰撞安全的重要性已受到工人和研究人员的广泛关注。

关于机器碰撞检测，已经有一些解决方法，安装用于吸收损伤的被动机构、通过使用视觉传感器来避免碰撞、以及基于碰撞分析和安全性评估机器人的设计和控制方案。还有其他一些关于碰撞检测的研究，例如，采用皮肤传感器来进行碰撞检测；也有一些使用机器人内嵌的关节力矩传感器进行力矩控制进行碰撞检测的研究；这些传统的碰撞检测方法是基于传感器的方法，并且传感器的成本通常很昂贵，因此可能会增加机器人的总成本。

安装用于吸收损伤的被动机构、通过使用视觉传感器来避免碰撞、以及基于碰撞分析和安全性评估的机器人的设计和控制方案。这些方法已显示出它们的有效性，但这些方法都显示一定的缺点。例如，安装附加的被动机构缺乏灵活性，并且还增加了机器人的尺寸和复杂性；视觉传感器的使用受到限制，因为在动态环境中计算负担可能很大，根据安全评估进行设计会导致任务性能与碰撞安全性之间进行权衡；皮肤传感器来进行碰撞检测，这些传统的碰撞检测方法是基于传感器的方法，并且传感器的成本通常很昂贵，因此可能会增加机器人的总成本。而且，将该方案应用于现有机器人需要在设计上进行其他更改，更不用说大多数工业机械手都未配备此类传感器。因此，最好采用碰撞检测算法而不使用额外的传感器。

近年来，通过检测机械臂的电流变化来判断是否发生碰撞开始出现诸多尝试。如公开号为CN 113021353 A公开的“机器人碰撞检测方法”，将检测伺服电机驱动电流，建立动力学模型或摩擦数学模型，对电流进行分解后进行碰撞判断，但改方案中需要建立系统动力学模型和摩擦数学模型，过程比较复杂，不利于推广应用。公开号为CN 112405523 A公开的机器人碰撞检测方法、装置及机器人，通过检测转速变化率来判断碰撞，计算复杂，效率低。此外还有通过检查电流变化差值进行机器人碰撞判断的方案，但该方案需要大量计算，直接影响最后判断效率和动作效率。

发明内容

发明目的：本发明目的在于针对现有技术的不足，提供一种新型无传感器机器人碰撞检测方法及其系统，通过电流的变化来判断是否发生碰撞，无需使用额外的传感器，响应速度快。

技术方案：本发明所述新型无传感器机器人碰撞检测方法，包括如下步骤：

S1：设置机器人的动作、姿态、路径规划；

S2：根据机器人的姿态、路径规划，计算各个动作中机械臂正常工作的估算电流，根据估算电流推导机械臂发生碰撞时的动态电流阈值，并设定检测阈值；

S3：在机器人运动过程中，实时采集机械臂的实测电流，将实测电流与动态电流阈值相比较得到误差电流r；

S4：将误差电流r与设定的检测阈值相比较，若误差电流r大于检测阈值，则判定机器人的机械臂受到碰撞，此时执行碰撞反应程序；若误差电流r小于检测阈值，按照既定程序执行下一个动作；

S5：重复步骤S3、S4，直至完成所有动作。

进一步完善上述技术方案，所述步骤S3中机械臂的实测电流通过设置在机械臂电机单相电流通路与驱动器之间的电流采样模块进行采集。

进一步地，所述电流采样模块包括霍尔元件、铜箔，所述铜箔用于采集机械臂电机表面磁场，所述霍尔元件通过磁场变化得到电压信号，电压信号经换算为电流信号。

进一步地，所述电流采样模块还设有比较电路，比较电路对所述电压信号进行滤波处理。

进一步地，所述步骤S4中，若执行碰撞反应程序结果为碰撞故障清除，则返回原控制程序继续执行。

进一步的，所述步骤S2中的计算各个动作中机械臂正常工作的估算电流包括：在机械臂电机加速时，估算电流值叠加机械臂电机加速产生的电流值。

进一步地，所述步骤S2中的计算各个动作中机械臂正常工作的估算电流包括：机械臂电机单相电流i与机械臂电机转动速度v之间的关系为：i＝0.35+0.0003v；

所述根据估算电流推导出机械臂发生碰撞时的动态电流阈值为：

其中i_dyna为动态电流阈值，i_min为准静态相电流、即最低转速下的单相电流检测阈值，k为机械臂电机单相电流与速度的系数，vm为机械臂电机转动最大速度，|v|为机械臂电机转动速度的绝对值。

用于实现上述方法的新型无传感器机器人碰撞检测系统，包括主控芯片、通讯芯片、功率驱动模块、过流保护模块、电流采样模块、霍尔元件；所述主控芯片通过通讯芯片与上位机进行通讯，用于接收上位机传输的控制指令以及向上位机发送采样信息；所述主控芯片根据接收的控制指令，控制功率驱动模块驱动机械臂电机动作，所述过流保护模块用于功率驱动过程中的过流保护，所述电流采样模块通过霍尔元件获取机械臂电机与功率驱动模块之间的实测电流值并传输至主控芯片，主控芯片将实测电流值发送至上位机进行误差电流分析，在误差电流大于检测阈值时，上位机发送停止指令至主控芯片，主控芯片通过驱动模块控制电机停止动作。

进一步地，所述上位机包括正常电流估算模块、动态电流阈值计算模块、误差电流比较模块；所述正常电流估算模块用于根据机器人的姿态、路径规划，计算各个动作中机械臂正常工作的估算电流；所述动态电流阈值计算模块根据估算电流计算动态电流阈值并设定检测阈值；所述误差电流比较模块将主控芯片传输的实测电流值与动态电流阈值相比较生成误差电流，将误差电流与检测阈值相比较，若误差电流r大于检测阈值，判定机器人的机械臂受到碰撞。

所述正常电流估算模块用于计算机械臂电机单相电流i，机械臂电机单相电流i与机械臂电机转动速度v之间的关系为：i＝0.35+0.0003v；所述动态电流阈值计算模块用于计算动态电流阈值

其中i_min为准静态相电流、即最低转速下的单相电流检测阈值，k为机械臂电机单相电流与速度的系数，vm为机械臂电机转动最大速度，|v|为机械臂电机转动速度的绝对值。

有益效果：与现有技术相比，本发明的优点在于：本发明通过上位机的快速计算电机的动态电流阈值，再根据霍尔效应采集电机的实测电流，经过滤波之后计算其与动态电流阈值的误差，并将误差值与设定的检测阈值进行比较，从而判断机器人是否受到外界物体的碰撞，不需要建立复杂的系统动力学模型，也不需要测量加速度，在实际应用中，只需要检测电机的电流即可，计算上更加高效，可以容易地用于现有系统而不需要对机器人的结构做出改变。

本发明提供的方法直接对预定轨迹、姿态进行电流估算，计算快，进而通过实测电流与动态电流阈值之间的误差来与检测阈值进行比较，从而判断碰撞是否发生：若是，立即驱动机器人做出反应，响应快；若不是，直接对下一个动作或姿态估计电流更新。此方法简单有效地实现了机器人的碰撞检测，省去了繁琐的动力学参数辨识的过程，相对于现有的计算转速、加速度、电流变化率等参数，节约了大量的计算量和计算时间，使得最终机器人碰撞检测反应速度上得到很大地提升，在人机协作机器人等领域具有良好的应用价值，更具有普适性。

这种直接利用机器人关节电机的电流数据来进行碰撞检测的方法，避免了给机械臂安装额外的传感器(例如力/扭矩传感器或加速度计)或柔性装置，降低了机械臂的生产成本。对实测电流数据进行滤波处理，使得设置的碰撞检测阈值更加精确。

附图说明

图1是本发明的结构示意图；

图2是碰撞检测系统的电路框图；

图3是碰撞时滤波电路示意图；

图4是碰撞时电流变化速度示意图；

图5是碰撞检测方法流程图；

图6是碰撞动态阈值示意图；

图7是电流采集滤波处理示意图。

具体实施方式

下面通过附图对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。

如图1所示的新型无传感器机器人碰撞检测系统，主要包括：上位机4、机器人，上位机存储有PC端操作程序，机器人包括地面运动部件3、碰撞检测部件2、被测运动部件1，机器人设有碰撞检测硬件系统。碰撞检测部件2为碰撞发生碰撞重点检测区域，该区域发生碰撞会比较灵敏地被检测到，被测运动部件1是该机器人主要提供功能服务的位置，如提供头模测试功能服务的机器人，该头模测试功能服务主要由被测运动部件完成。

如图2所示，碰撞检测系统的硬件包括：主控芯片、过流保护模块、功率驱动模块、电流采样模块、电机、霍尔元件，主控芯片型号采用STM32。在机器人电机运转过程中，碰撞检测硬件系统会收集电机的运行电流信息、电机转速信息，并通过通讯芯片将这些信息发送至PC端用于运行碰撞检测算法，对采集信息在线分析之后判断是否发生碰撞，进而对主控芯片发出指令，使电机停止转动。具体过程如下：

1、机器人接通电源后完成自检，此时处于就绪状态、等待指令；

2、PC将规划好的运动轨迹、姿态组合等控制指令发给通讯芯片；

3、通讯模块将指令传输至主控芯片；

4、主控芯片根据控制指令，调度功率驱动模块，驱动各个电机开始按照指定的方向、速度、角度转动，机械臂开始运行，同时电流采样、霍尔元件、过流保护模块开始工作，完成电流采集、控制功能，同时将采集的数据反馈给主控芯片，主控芯片通过通讯芯片传输给PC；

5、机械臂受到外力碰撞后，电流采样、霍尔元件将采集到实测电流数据传输至主控芯片，主控芯片通过通讯芯片传输至PC，PC比较实测电流是否超出正常阈值范围，若超出，PC通过通讯芯片发送停止信号给主控芯片，主控芯片控制各个电机停止；

6、机械臂停止运动。

机器人的运动伺服系统包括电流环、位置环、速度环组成的三环控制结构。电流环位于三环最底层，即驱动器内部，驱动器通过霍尔元件检测电机各相的输出电流，将电流反馈差值与给定检测电流值进行比较，使输出电流尽量靠近设定值，由于电流环控制电机转矩，电流环对于力矩变化的动态响应最快，电流闭环结构能够充分发挥电机的过载能力，并且在电机过流时还能给予保护。

在驱动器和三相无刷直流电机其中一相电流通路中间加入电流采样模块，使其中一相的电流先经电流采样模块再进入驱动器，实现通过添加的电流采样模块采集电机相电流数据。

电流采样模块基于霍尔感应原理设计，包括霍尔元件以及铜箔，霍尔元件具有偏差较小、线性度高的特点，铜箔在靠近集成电路表面的位置，有电流流过时会产生磁场，霍尔感应元件根据该磁场的变化，再经过内部电路的硬件处理输出一个电压信号。这个电压信号经过换算就得到流经铜箔的电流大小。该电压信号经过电阻分压后直接由STM32的模数转换模块进行高速采样，检测得到电机的实时电流。由于电路自带硬件滤波，能够滤除高频噪声的干扰。

检测电流与模拟信号输出V_out的关系如下：

式中ia——检测脚实际流过的电流(A)；

K——线性度，K＝66mV/A。

电流从正方向流进时，输出模拟电压为：

而电流从反方向流进时，输出模拟电压为：

为了让电路可靠稳定工作，设置一个带有迟滞功能的比较电路，能滤除的噪声，通过设定比较电阻的值，改变电路的迟滞灵敏度，输出精度达到185mV/A。电流传感器输出电压为模拟量，需要经过模数转换将其转换为数字量才能被芯片ADC模块采集。STM32的最大转换速率为1MHz，转换时间为1us。

位置环和速度环主要用于电机控制，直接决定了电机的控制精度，电流垂直于外磁场通过导体时载流介质会发生偏转，在与电流和磁场均垂直的方向会产生额外的电场，从而在导体的两端形成电势差，即霍尔效应。霍尔元件输出矩形数字信号信号，能够直接用于数字控制，稳定性比较好。霍尔元件是将霍尔效应结合到集成电路技术上形成的一种基于磁场的传感器，能感知与磁信息有关的物理量，通过永磁无刷直流电机，可以通过霍尔效应对电机进行位置和速度的测量。

基于元件脉冲信号计数的测速方法主要有：M法、T法以及M/T法。M法又称为测频率法。在设定的检测时间长度内，收集霍尔元件输出信号的数量计算转动速度。由于测速的时间区间具有不确定性，测频率法在某些情况下可能产生1～2个脉冲信号的误差。因此M法适用于电机高速转动的情形，脉冲信号数量较大的时候测量精度较高。T法又称为测周期法。根据相邻两脉冲信号之间的时间差计算转速，同样的在速度较高的情况下T法测速可能出现1-2个脉冲信号的误差。因此T法适用于电机低速转动的情形。M/T法又称频率/周期法。通过同时测量时间与时间区间内的转速脉冲数量计算转速，因此在时间轴配置合理的情况下，M/T法在低速、高速下均能够实现较高的精度。由于需要对机器人在各种速度、加速度下运行时的电流波动进行测量，所以选用在低速和高速下都能精确测量的M/T法。

在没有发生碰撞时，机器人电机的电流是保持恒定的，电机加速本身使得电流增大，碰撞的发生使得电流在此基础上进一步增大，电机的电流会迅速产生变化，碰撞对电流的影响是叠加的，而不会被电机正常加减速的电流波动所覆盖。随着电机转速不断增加，电机相电流值也随之增加，稳态相电流与速度成近似线性关系。若发生碰撞，机械臂会产生加速度，滤波电流和电流变化速度对比图如图3所示，共有5个周期，在前面的2个周期没有受到碰撞力，后3个有不同程度的碰撞力。从图4可以看出，当机器人受到碰撞时，电流的变化速度在一段区域比设置的阈值大，因此这种方法可以成功检测碰撞。

碰撞检测部分的电机电流通常无法直接测出，本发明采用霍尔元件测得，并使用滤波器对电流进行滤波处理。

如图5所示的检测方法，包括如下步骤：

S1：上位机控制系统设置机器人的动作、姿态、路径规划；

S2：上位机控制系统根据机器人运动轨迹、姿态，计算出各个动作的正常电流范围，推导出动态电流阈值，如图6所示；电机加速本身使得电流增大，碰撞的发生使得电流在此基础上进一步增大，碰撞对电流的影响是叠加的，而不会被电机正常加减速的电流波动所覆盖；相电流与速度呈线性关系为：i＝0.35+0.0003v，随着电机转速不断增加，电机相电流值也随之增加，稳态相电流与速度成近似线性关系，不同速度下，维持该转速的力矩不同，速度越大所需的输出力矩越大，因此相电流也越大，针对不同速度匀速转动的碰撞检测，得到动态阈值的通式：

其中i_min为准静态相电流、即最低转速下的单相电流检测阈值，k为机械臂电机单相电流与速度的系数，vm为机械臂电机转动最大速度，|v|为机械臂电机转动速度的绝对值；

S3：在机器人运动过程中，电流采样模块实时采集当前机器人实际电流值，通过滤波获得滤波后的实测电流值，如图7所示，将实际电流值和动态电流阈值比较得到误差电流r；

S4：将误差电流r与之前设定的检测阈值进行比较，若误差电流r大于检测阈值则判定机器人此时受到碰撞，上位机控制系统将会由原有的控制程序转换为相应的碰撞反应程序；若误差电流r小于检测阈值，此时机器人按照原有控制程序运行；

S5：在机器人运动过程中，当碰撞故障清除之后，机器人会回到控制程序重复步骤3、4，直到结束。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (10)

1.一种新型无传感器机器人碰撞检测方法，其特征在于，包括如下步骤：

S1：设置机器人的动作、姿态、路径规划；

S2：根据机器人的姿态、路径规划，计算各个动作中机械臂正常工作的估算电流，根据估算电流推导机械臂发生碰撞时的动态电流阈值，并设定检测阈值；

S3：在机器人运动过程中，实时采集机械臂的实测电流，将实测电流与动态电流阈值相比较得到误差电流r；

S4：将误差电流r与设定的检测阈值相比较，若误差电流r大于检测阈值，则判定机器人的机械臂受到碰撞，此时执行碰撞反应程序；若误差电流r小于检测阈值，按照既定程序执行下一个动作；

S5：重复步骤S3、S4，直至完成所有动作。

2.根据权利要求1所述的新型无传感器机器人碰撞检测方法，其特征在于：所述步骤S3中机械臂的实测电流通过设置在机械臂电机单相电流通路与驱动器之间的电流采样模块进行采集。

3.根据权利要求2所述的新型无传感器机器人碰撞检测方法，其特征在于：所述电流采样模块包括霍尔元件、铜箔，所述铜箔用于采集机械臂电机表面磁场，所述霍尔元件通过磁场变化得到电压信号，电压信号经换算为电流信号。

4.根据权利要求3所述的新型无传感器机器人碰撞检测方法，其特征在于：所述电流采样模块还设有比较电路，比较电路对所述电压信号进行滤波处理。

5.根据权利要求3所述的新型无传感器机器人碰撞检测方法，其特征在于：所述步骤S4中，若执行碰撞反应程序结果为碰撞故障清除，则返回原控制程序继续执行。

6.根据权利要求2所述的新型无传感器机器人碰撞检测方法，其特征在于：所述步骤S2中的计算各个动作中机械臂正常工作的估算电流包括：在机械臂电机加速时，估算电流值叠加机械臂电机加速产生的电流值。

7.根据权利要求6所述的新型无传感器机器人碰撞检测方法，其特征在于：所述步骤S2中的计算各个动作中机械臂正常工作的估算电流包括：机械臂电机单相电流i与机械臂电机转动速度v之间的关系为：i＝0.35+0.0003v；

所述根据估算电流推导出机械臂发生碰撞时的动态电流阈值为：

其中i_dyna为动态电流阈值，i_min为准静态相电流、即最低转速下的单相电流检测阈值，k为机械臂电机单相电流与速度的系数，vm为机械臂电机转动最大速度，|v|为机械臂电机转动速度的绝对值。

8.一种新型无传感器机器人碰撞检测系统，其特征在于：包括主控芯片、通讯芯片、功率驱动模块、过流保护模块、电流采样模块、霍尔元件；所述主控芯片通过通讯芯片与上位机进行通讯，用于接收上位机传输的控制指令以及向上位机发送采样信息；所述主控芯片根据接收的控制指令，控制功率驱动模块驱动机械臂电机动作，所述过流保护模块用于功率驱动过程中的过流保护，所述电流采样模块通过霍尔元件获取机械臂电机与功率驱动模块之间的实测电流值并传输至主控芯片，主控芯片将实测电流值发送至上位机进行误差电流分析，在误差电流大于检测阈值时，上位机发送停止指令至主控芯片，主控芯片通过驱动模块控制电机停止动作。

9.根据权利要求8所述的新型无传感器机器人碰撞检测系统，其特征在于：所述上位机包括正常电流估算模块、动态电流阈值计算模块、误差电流比较模块；所述正常电流估算模块用于根据机器人的姿态、路径规划，计算各个动作中机械臂正常工作的估算电流；所述动态电流阈值计算模块根据估算电流计算动态电流阈值并设定检测阈值；所述误差电流比较模块将主控芯片传输的实测电流值与动态电流阈值相比较生成误差电流，将误差电流与检测阈值相比较，若误差电流r大于检测阈值，判定机器人的机械臂受到碰撞。

10.根据权利要求9所述的新型无传感器机器人碰撞检测系统，其特征在于：所述正常电流估算模块用于计算机械臂电机单相电流i，机械臂电机单相电流i与机械臂电机转动速度v之间的关系为：i＝0.35+0.0003v；

所述动态电流阈值计算模块用于计算动态电流阈值i_dyna，

其中i_min为准静态相电流、即最低转速下的单相电流检测阈值，k为机械臂电机单相电流与速度的系数，vm为机械臂电机转动最大速度，|v|为机械臂电机转动速度的绝对值。

Images