一种机器人双臂碰撞神经反射控制方法

技术领域

本发明涉及机器人控制技术，特别是涉及一种机器人双臂碰撞神经反射控制方法。

背景技术

随着我国经济建设的迅速发展，大量的双臂机器人得到应用。但是，目前机器人双臂控制方式都是对单臂分别控制各自运作，只是组装在一起，并不算真正意义上协同的双臂控制方式。

其中，双臂碰撞控制方法是机器人双臂控制方式的一个重要部分，现有的双臂碰撞控制方法主要有两种：第一种为基于力矩传感器检测；第二种为基于几何模型的碰撞检测，用基本几何体包围实际模型的碰撞检测，如专利号为202010123225.1的中国专利公开的一种双臂自碰撞检测方法就属于第二种。

上述双臂碰撞控制方法存在以下不足：

1、第一种基于力矩传感器的检测方法成本过高，存在安装问题，不一定适用于所有现有的机器人。其在检出碰撞时，碰撞实际已经发生，无法起到预防作用。

2、第二种基于几何模型的碰撞检测方法的碰撞精度需要取决于包络后的模型与实际模型的紧密程度，计算难度大，会出现人为的犯错或计算编程等错误，增加了此控制方法控制风险。

发明内容

本发明所要解决的技术问题，就是提供一种机器人双臂碰撞神经反射控制方法，其能在双臂协调工作的同时，提前预判双臂的碰撞，适应能力强、安全可靠。

解决上述技术问题，本发明采用的技术方案如下：

一种机器人双臂碰撞神经反射控制方法，机器人设有两条机械手臂，机械手臂分别设有通过关节连接在一起的臂杆，其特征在于，包括以下步骤：

步骤一：对两条机械手臂，分别获得每个关节所转动的角度和相邻关节的轴心之间的距离，从而分别计算出两条机械手臂所在的空间曲线；

步骤二：根据设定的采样长度，分别采集空间曲线上的多个坐标点，同一空间曲线上相邻的坐标点之间的距离等于采样长度，将两条机械手臂所在的空间曲线上采集的坐标点分别纳入第一点阵列和第二点阵列中，并且还将坐标点与其所在的臂杆对应；

步骤三：对第一点阵列中的坐标点与第二点阵列中的坐标点一一对应相减分别求得差值，最后获得其中的最小差值；

步骤四：实时计算获得两条机械手臂的臂杆之间的撞击距离，将最小差值与对应臂杆之间的撞击距离相比较；当最小差值小于对应臂杆的撞击距离时，输出神经反射信号。

可选的，步骤四中的撞击距离的计算方法为：获得每个关节的轴的惯量，实时获得每个关节的转动角速度，通过惯量和转动角速度计算出每段臂杆的减速距离，将最小差值对应的两段臂杆的减速距离、两机械手臂的最大半径与设定的阈值相加求得撞击距离。

可选的，对应关节的轴的惯量的计算方法为：将两条机械手臂分别水平伸直，且每个关节的轴心也处于水平状态，此时，获得对应关节在平衡时的电流，通过电流求出对应的重量值，根据重量值和相邻关节的重心点之间的距离计算得出对应的惯量。

可选的，惯量的计算在步骤一之前进行。

可选的，步骤三中获得最小差值的方法为：首先，将第一点阵列中的第一个坐标点与第二点阵列中的第一个坐标点相减获得差值，并存储；之后，每一次相减的差值均与存储的差值进行对比，将较小的差值进行存储；最后完成全部减法后，存储下来的差值将是最小差值。

可选的，控制方法在机器人的控制系统中独立运行，且控制方法输出的神经反射信号的优先级高于机器人的控制系统中的其他控制信号。

可选的，每个关节所转动的角度通过设置在关节中的编码器检测获得。

与现有技术相比，本发明具有以下有益效果：

1、本发明通过机械手臂的关节的转动角度、轴心之间的距离等数据的计算来实现对碰撞风险的预判，发出防撞预警，提前预防双臂的碰撞。其无需设置力矩传感器，成本低，且不会出现安装问题。

本发明为实时根据机械手臂所在的空间曲线等数据来预判碰撞风险，且计算更简单，能更好的避免出现背景技术中的基于几何模型的碰撞检测方法的人为错误，控制更加可靠安全。

2、由于惯量等参数的精度极高，发出警报实际停止时不会有碰撞，停止距离通过阈值决定，停止精度带毫米级，停止效果和接触式碰撞控制系统无异，比传统的碰撞式碰撞控制系统有以下优点：本发明得到碰撞警报后实际是没任何接触性损坏的，传统的碰撞式碰撞控制系统则是实际碰撞，会出现掉漆、刮擦、抱闸松动等等不可避免的接触性损伤。

3、本发明的控制方法可以做成硬件电路，独立运作或者嵌入到其他控制方式中，解决了烦琐的软件编程，和人为的设置错误。做成硬件电路时，响应速度快，原则上做的了零延迟，解决了软件运算的延时问题。

4、本发明优于传统的单臂单独运作方式，协调性更好，解决了传统控制不适用于双臂机器人的特点。

附图说明

图1为本发明的控制方法的流程框图；

图2为关节转动速度对应的扭矩图；

图3为本发明的控制方法的第一种实现方式的原理示意图；

图4为本发明的控制方法的第二种实现方式的原理示意图。

具体实施方式

在本发明的描述中，若干的含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。

本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。

下面结合实施例对本发明进一步描述。

实施例：

本发明的控制方法可以设计成硬件电路，也可以设计成软件嵌入到其他控制方式中。由于是双臂汇总到一个大脑(控制器)中，这种类人型双臂设计神经发射控制方式，优于传统的单臂单独运作方式，协调性更好，适用于双臂机器人系统。

本实施例所涉及的一种机器人1，其设有两条机械手臂2，两条机械手臂2分别设在左右两边，机械手臂2分别设有通过关节4连接在一起的臂杆3，关节4上设有轴，每个关节4均可转动，关节4和臂杆3的数量均为多个，根据实际机器人1的设计而定。

本实施例的机器人双臂碰撞神经反射控制方法包括以下步骤：

步骤一：对两条机械手臂，分别获得每个关节所转动的角度和相邻关节的轴心之间的距离，从而分别计算出两条机械手臂所在的空间曲线；

其中，每个关节所转动的角度通过设置在关节中的编码器检测获得。

步骤一的一种具体方法如下：

编码器为绝对值编码器，绝对值编码器的1圈为360度，而绝对值编码器一圈对应读数是1～N，假设编码器的数值为T，则可以求出，关节所转动的角度A＝360/N*T。

其中，相邻关节的轴心之间的距离是固定的，为直线距离，可以从图纸中测出，或者实际中测出。上述求出的机器人手臂所在的空间曲线，有多少个轴则有多少段，命名为Curve-N，表达式为(x-x0)/a＝(y-y0)/b＝(z-z0)/c，其中(a,b,c)为方向向量。

步骤二：根据设定的采样长度，分别采集空间曲线上的多个坐标点，同一空间曲线上相邻的坐标点之间的距离等于采样长度，将两条机械手臂所在的空间曲线上采集的坐标点分别纳入第一点阵列和第二点阵列中，即第一点阵列对应第一机械手臂，第二点阵列对应第二机械手臂，并且还将坐标点与其所在的臂杆对应；其中，采样长度可以在控制系统中设置，用电路传函写导入到电路中，控制函数，采样长度的大小将影响采样精度，采样长度越小，采集的坐标点将越多，精度也就越高；

步骤二的一种具体方法如下：

其中的坐标点可以表达为(X1,Y1,Z1)～(XN,YN,ZN)。点阵列可以表达为Array[0..N1,0..N2,0..N3]，表达式中的N1代表机械手臂，N2代表的关节，N3代表的是坐标点的序号，每一个表达式Array[0..N1,0..N2,0..N3]对应一个坐标点；由于机器人有两个机械手臂，那么N1的取值即为0和1，那么第一点阵列中的坐标点的N1为0，第二点阵列中的坐标点的N1为1；

例如：Array[0,0,0]代表的是第一机械手臂的第一个关节所连接的臂杆上的第一个坐标点；Array[1,0,0]代表的是第二机械手臂的第一个关节所连接的臂杆上的第一个坐标点。关节前方连接的臂杆与此关节关联，那么根据N2，即可知道坐标点位于哪个臂杆上。

步骤三：对第一点阵列中的坐标点与第二点阵列中的坐标点一一对应相减分别求得差值，最后获得其中的最小差值；

其中，获得最小差值的方法为：首先，将第一点阵列中的第一个坐标点与第二点阵列中的第一个坐标点相减获得差值，并存储；之后，每一次相减的差值均与存储的差值进行对比，将较小的差值进行存储；最后完成全部减法后，存储下来的差值将是最小差值。

步骤三的一种具体方法如下：

第一点阵列中的第一个坐标点Array[0,0,0]与第二点阵列中的第一个坐标点Array[1,0,0]相减，根据距离公式：

d＝√[(x1-x2)^2+(y1-y2)^2+(z1-z2)^2]，

求出差值，即距离d1，将d1的值写入dmix；

之后，根据同样的方法，将第一点阵列中的第一个点Array[0,0,0]和第二点阵列的第二个坐标点Array[1,0,1]相减，求出差值，即距离d2；

将d2的值与dmix中的值对比，如果d2的值小于dmix中的值，那么将原先dmix中的值清除，将d2的值写入dmix中；

如此类推，直到将第一点阵列中每个坐标点与第二点阵列中的坐标点全部相减完成后，那么dmix的值即为最小差值，也就是最小距离。

步骤四：实时计算获得两条机械手臂的臂杆之间的撞击距离，将最小差值与对应臂杆之间的撞击距离相比较；当最小差值小于对应臂杆的撞击距离时，输出神经反射信号，神经反射信号为碰撞警报信号。机器人的控制系统接受到神经反射信号时将发出警报并停止动作。而当最小差值大于对应臂杆的撞击距离时，即没有撞击风险，那么将返回上面步骤重复运行，不断进行循环检验。

其中，撞击距离的计算方法为：获得每个关节的轴的惯量，实时获得每个关节的转动角速度，通过惯量和转动角速度计算出每段臂杆的减速距离，将最小差值对应的两段臂杆的减速距离、两机械手臂的最大半径与设定的阈值相加求得撞击距离。阈值的具体数值由经验值求得。

最小差值对应的两段臂杆为：最小差值由两个坐标点相减获得，这两个坐标点各自对应的臂杆即为最小差值对应的两段臂杆。

其中，对应关节的轴的惯量的计算方法为：将两条机械手臂分别水平伸直，且每个关节的轴心也处于水平状态，此时，获得对应关节在平衡时的电流，通过电流求出对应的重量值，此重量值为对应关节之后的机械手臂的重量，根据重量值和相邻关节的重心点之间的距离计算得出对应的惯量。

当机械手臂的前端安装有工具时，通过上述惯量的计算方法可以把工具的惯量也计算出来，精度极高。工具即为机器人在工作时使用的工具，例如：涂胶机器人的机械手臂的前端会安装各种胶枪等。

上述的惯量的计算在步骤一之前进行，即在没有惯量的值时，先执行上述惯量的计算方法，计算出惯量。

步骤四的一种具体方法如下：

其中，惯量的一种具体计算方法为：将两条机械手臂分别水平伸直，且每个关节的轴心也处于水平状态，此时，获得对应关节在平衡时的电流I，则有I/IRC＝W/WRC，其中额定电流IRC和额定功率WRC是固定参数，通过关节出厂数据可查，由此通过平衡时的电流I，求出重量Wn，其中n为轴号；

以每个关节的轴的中心为重心点Gn，其中n为轴号，Gn的空间三维坐标可从空间曲线中获得(XGn，YGn，ZGn)。

那么相邻关节之间的距离为：

Ln＝√[(XGn-XGn

其中n为轴号。

根据公式惯量SIn＝Wn*Ln^2，计算出每个关节的轴的惯量，其中n为轴号。

实时获得每个关节的转动角速度，由角度值一秒的变换量求出转动角速度As＝(At1-A t2)/T；那么，力矩Fn＝Sln*As。

通过力矩Fn得出的值，对照图2所示的坐标图，坐标图中纵轴表示力矩Fn、横轴表示转动角速度，通过力矩Fn的值对应获得横轴的值后，既可以计算得出减速距离dmin__Array[N1,N2]，Array[N1,N2]即为对应臂杆。

阈值T由经验值求得。如果T＝0，机器人通过本控制方法发出神经反射信号，使得机械手臂停止时，机械手臂则会实际发生碰撞，因此T需要取大于0的值，不同的系统T的单位可以在传函中自由获取，本实施例T对应单位mm，当T＝1时，机器人通过本控制方法发出神经反射信号，使得机械手臂停止时，两条机械手臂将在相距1mm处停下，实际不会发生碰撞，且停止精度带毫米级。

其中，机械手臂的最大半径D可以从图纸中测得，或者实际测得。

那么，撞击距离dmin为：

dmin＝dmin__Array[0,N2]+dmin__Array[1,N2]+2D+T；

其中，Array[0,N2]表示最小差值对应第一个机械手臂的臂杆，Array[0,N2]表示最小差值对应第二个机械手臂的臂杆，Array[0,N2]和Array[0,N2]中的N2的值不一定相同，根据最小差值所涉及到的臂杆获得。

实时监控撞击距离dmin与最小差值dmix之间的差值：

Dv＝dmin-dmix；

当Dv大于0时，则表示最小差值小于撞击距离，表示预计会发生碰撞，那么输出神经反射信号。

本控制方法在机器人的控制系统中为放在单独空间中或者后台独立运行，不会受其他程序的干涉，且控制方法输出的神经反射信号的优先级高于机器人的控制系统中的其他控制信号。机器人的控制系统中的执行器通过判断神经反射信号result是true或false，当为true时，切断电源，打开抱闸或者反方向控制关节的伺服电机。

机器人的控制系统中的执行器可以是继电器接触器抱闸等任何输出对象，不是本发明需要展开讨论的范围。等待神经反射信号result＝true时，通过触发中断或后台执行等方式，把停止机器人的任务高于其他任务，或者并联接入伺服系统。

如图1所示为本实施例的控制方法的流程框图，其大概流程如下：

启动机器人后，首先读取各个关节的轴的惯量，如果缺少惯量的数据，则执行惯量的计算方法，计算得到惯量的数据；

读取得到惯量的数据后，控制方法则从步骤一开始执行，读取各个编码器的数据，获取每个关节所转动的角度，如果此时不能读取到编码器的数据，则直接输出神经反射信号；之后，再根据相邻关节的轴心之间的距离，分别计算出两条机械手臂所在的空间曲线；

之后，根据设定的采样长度，采集坐标点，分别对应纳入第一点阵列和第二点阵列中，并且坐标点与其所在的臂杆对应；将第一点阵列与第二点阵列的坐标点一一对应相减，获得最小差值后，与撞击距离相比较，其中，撞击距离则通过对应的两段臂杆的减速距离、两机械手臂的最大半径与设定的阈值相加得到，减速距离则通过惯量和转动角速度计算得到；

最后，如果最小差值小于对应臂杆的撞击距离，即预计会撞击，则输出神经反射信号；如果最小差值大于对应臂杆的撞击距离，即没有撞击风险，那么将返回上面步骤重复运行，不断进行循环检验。

实现本机器人双臂碰撞神经反射控制方法，可以通过多种方式实现，以下为列举的两种：

第一种实现方式：如图3所示，本控制方法可以被处理器存储做后台程序执行，分开切割成为Main主程序执行内存区和后台中断程序神经反射控制内存区，分立运行输出到机器人的控制系统的执行器。

第二种实现方式：如图4所示，本控制方法做成控制电路，电路的方式并联输出到机器人的控制系统的执行器。

本发明的上述实施例并不是对本发明保护范围的限定，本发明的实施方式不限于此，凡此种种根据本发明的上述内容，按照本领域的普通技术知识和惯用手段，在不脱离本发明上述基本技术思想前提下，对本发明上述结构做出的其它多种形式的修改、替换或变更，均应落在本发明的保护范围之内。