CN109093623B - 一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法 - Google Patents

Abstract

本发明公开一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法，扭矩计算包括：S1、闭环切割；S2、计算等价的开环结构扭矩，再转换为闭环结构中的驱动电机的实际扭矩。碰撞检测与拖动示教方法包括：S3、利用电机电流估算扭矩；S4、结合电机电流的估算扭矩值、扭矩传感器的测量扭矩值和计算出的实际扭矩判断是否产生碰撞信号；S5、根据碰撞信号的大小及符号进行刹车或电子助力。本发明可以将传统的递归牛顿欧拉求解开环机械臂关节扭矩的方法推广到任意闭环结构的机械臂，并且适用于基于普通无刷有刷电机，使这类机械臂实现碰撞检测与拖动示教的功能。本发明同时使碰撞信号的检测达到了更高的鲁棒性与抗躁能力。

Description

一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法

技术领域

本发明属于扭矩补偿领域，具体涉及一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法。此方法可以用于任何带有闭环结构，如平行四连杆与扭矩传感器的机械臂，为其每个关节计算主动补偿的扭矩，从而实现精准的轨迹跟随，以及碰撞检测及拖动示教。

背景技术

目前的市面上的协作机器人全部采用“扭矩电机”，或者叫“盘式电机”。此种电机无需额外的传动系统即可提供足够的扭矩。对这类电机组成的机械臂进行碰撞检测或拖动示教的电子助力时，由于没有传动系统，电机的电流对外力的响应较快，因此可以直接使用电流作为电机输出端所受扭矩的估计值。然而此类电机最大的缺点是成本高，且成本与扭力大小不成线性关系。因此基于扭矩电机的协作机械臂，可延展性较差。这就使得目前市面上的协作机器人均限制在一定的形态以及臂展、负重等参数范围内，很难有突破。为了使协作机械臂能有更大的臂展和负重，需要更加灵活的本体结构与传动系统的配合，例如基于主动驱动的平行四连杆的机械臂结构。然而这类机械臂结构，由于较高的传动比，使得电机输出端感受到的扭力变化相对实际的扭力施加处较小，也就导致实际发生碰撞时，电机的电流变化较小，在电机内信噪比较高的环境下很难单凭电流判断是否真的发生碰撞，且难以判断外力的大小。此外，基于闭环的机械臂结构无法直接使用递归牛顿欧拉方法对其各个关节的补偿扭矩进行计算，因此需要特定的方式求解闭环结构的各个关节的补偿扭矩。

发明内容

为了解决现有技术存在的上述问题，本发明目的在于提供一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法。

本发明所采用的技术方案为：

一种基于闭环结构的扭矩计算方法，包括如下步骤：

S1、闭环切割：将任何一个闭环结构的每一个环断开成两条关节路径，断开后，将原闭环结构等价成多个开环结构，等价的开环结构中，将原被动驱动关节当做带有虚拟驱动器的主动关节；

S2、计算等价的开环结构扭矩，再转换为闭环结构中的驱动电机的实际扭矩。

本发明可以将传统的递归牛顿欧拉求解开环机械臂关节扭矩的方法推广到任意闭环结构的机械臂，并且适用于基于普通无刷有刷电机，而不是扭矩电机的带有高传动比的机械臂上，使这类机械臂实现碰撞检测与拖动示教的功能。

具体地，所述步骤S2的实现方法为：计算等价的开环结构扭矩，求出开环结构与闭环结构的关节角度之间的对应关系，然后计算闭环结构中的驱动电机的实际扭矩。

本发明提出一种不需要解析，而是通过实验或仿真数据，列出线性方程组的方法来解出闭环与开环的关节映射。求出开环结构与闭环结构的关节角度之间的对应关系的步骤包括：

S21、使用仿真软件或者实验，测定若干组闭环结构在不同姿态下的关节角度，获得多组关节角度数据；

S22、设步骤S21取得了N组关节角度数据，每一组都由Q_close[1]到Q_close[n]组成，其中n为闭环结构中实际关节的数量，令Q_open[1]到Q_open[m]为此闭环结构等价的开环结构的所有关节的关节角度，其中m为开环结构中关节的数量，将所有数据汇总并形成矩阵形式，如式(1)：

Qc＝A*Qo (1)

其中Qc为n*N的矩阵，Qo为m*N的矩阵，A为n*m的矩阵；

S23、利用最小二乘法解出式(1)的A矩阵，即得到了开环结构与闭环结构的关节角度之间的对应关系。

进一步地，所述步骤S21中，关节角度数据量为关节数量的4倍以上。数据量越多，得到的解精准度越高，由于只是对闭环结构在不同姿态下的关节角度进行测量或者仿真，数据获取相对比较容易。

进一步地，计算闭环结构中的驱动电机的实际扭矩的步骤包括：

S24、将每一个闭环结构对应的开环结构，采用递归欧拉方法算出开环结构关节扭矩后，对每一个闭环结构分别解出其闭环扭矩，对于一个闭环结构，其驱动电机扭矩tao_c与开环结构关节扭矩tao_o的关系可由式(2)得出：

tao_c＝A*tao_o (2)

其中tao_c为n维列向量，tao_o为m维列向量；

S25、在计算完所有驱动电机的实际扭矩后，将所有驱动电机的计算扭矩命名为Torque_computed，Torque_computed为s维向量，其中s为机械臂总电机数量。

由一种基于闭环结构的扭矩计算方法计算出的实际扭矩进行碰撞检测与拖动示教方法，包括如下步骤：

S3、利用电机电流估算扭矩；

S4、结合电机电流的估算扭矩值、扭矩传感器的测量扭矩值和计算出的实际扭矩判断是否产生碰撞信号；

S5、根据碰撞信号的大小及符号进行刹车或电子助力。

本发明同时考虑扭矩传感器以及电机电流估算的扭矩值，使用高通低通滤波器，再结合两种值的时间差因素，使碰撞信号的检测达到了更高的鲁棒性与抗躁能力。

虽然从扭矩传感器已经能获得测量扭矩值，但是电机端的电流在受到外力时依然会产生变化，只是这种变化经过传动系统的高减速比而被削弱，但是它仍然存在，且具有相当高的信躁比。因此本发明从电机电流值中估算出扭矩值，以便与扭矩传感器的测量扭矩值结合使用，提高系统的鲁棒性与抗躁能力。

具体地，所述步骤S3利用电机电流估算扭矩的步骤包括：

S31、在有刷与无刷直流电机中，电机输出扭矩有如式(3)关系：

I*R＝V-Kv*w

Torque_motor＝I*Kv*gear (3)

其中V为驱动器提供给电机的电压，I为电机电流，R为电机电阻值，w为电机转速，单位为radian/sec，Kv为电机常数，I*Kv是电机端扭矩；

S32、再根据传动系统的减速比将电机端扭矩I*Kv放大，得到与扭矩传感器相同规模的数值，即从电机电流值中估算出扭矩值Torque_motor。

进一步地，所述步骤S4的具体实现步骤包括：

S41、假设任意时刻机械臂的扭矩传感器的测量扭矩值为Torque_sensor，电机电流的估算扭矩值为Torque_motor，将扭矩传感器的测量扭矩值和电机电流的估算扭矩值与理论计算扭矩Torque_computed相减得到两个余项，如式(4)：

Residual_sensor＝Torque_sensor–Torque_computed

Residual_motor＝Torque_motor–Torque_comuted (4)

S42、分别对两个余项计算高通滤波值和低通滤波值，如式(5)：

Residual_sensor_lowpass＝0.01*Residual_sensor_lowpass+(1-0.01)*Residual_sensor

Residual_sensor_highpass＝0.2*Residual_sensor_highpass+(1-0.2)*Residual_sensor

Residual_motor_lowpass＝0.01*Residual_motor_lowpass+(1-0.01)*Residual_motor

Residual_motor_highpass＝0.2*Residual_motor_highpass+(1-0.2)*Residual_motor (5)

S43、判断高通滤波值与低通滤波值的差值是否大于设定的阈值，若是，则判断有碰撞发生，如式(6)：

Contact_sensor＝|Residual_sensor_highpass-Residual_sensor_lowpass|>threshold

Contact_motor＝|Residual_motor_highpass-Residual_motor_lowpass|>threshold (6)

进一步地，所述步骤S43中，判断有碰撞发生还包括条件：Contact_sensor时间早于Contact_motor发生。这样就实现了同时利用扭矩传感器与电机读数来判断碰撞信号，且不会被扭矩传感器或电机这两方单方面的噪声所干扰。

具体地，所述步骤S5的具体步骤包括：

S51、根据高通滤波值与低通滤波值的差值的大小，得出碰撞外力的大小，其符号得出碰撞外力的方向；

S52、根据碰撞信号的大小及符号进行如下操作：

A、进行碰撞检测时，检测到碰撞信号即令全部电机刹车；

B、进行拖动示教时，通过电机补偿进行电子助力，为每一个电机各自的信号值大小，在位置速度环的内部额外补偿一个电压信号，其大小为：

V_gain＝(Residual_sensor_highpass-Residual_sensor_lowpass)*R/Kt。

本发明的有益效果为：

本发明可以将传统的递归牛顿欧拉求解开环机械臂关节扭矩的方法推广到任意闭环结构的机械臂，并且适用于基于普通无刷有刷电机，而不是扭矩电机的带有高传动比的机械臂上，使这类机械臂实现碰撞检测与拖动示教的功能。

本发明同时考虑扭矩传感器以及电机电流估算的扭矩值，使用高通低通滤波器，再结合两种值的时间差因素，使碰撞信号的检测达到了更高的鲁棒性与抗躁能力。

附图说明

图1是本发明-实施例的闭环切割示意图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步阐述。

为了解决此问题，如图1所示，本实施例的一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法，包括如下步骤：

第一步、闭环切割

对于传统的开环序列关节机械臂结构，要计算其任意姿态下的补偿扭矩，通常使用的方法是拉格朗日方法以及递归牛顿欧拉方法。其中拉格朗日方法复杂度在O(N^4)，不适合在20Hz以上的控制环中运行，因此真正普遍使用的是后者，即递归牛顿欧拉方法。该方法的特点是将机械臂看成一颗树形结构，从根节点开始，逐级递归，向叶节点计算每一个关节质心的力以及转动动能，最后再从叶节点逆向递归，计算每一个关节的扭矩。然而，如果机械臂带有闭环结构，如图1左侧，则无法直接使用递归牛顿欧拉方法。

然后本发明采用如下方法实现闭环切割：

S11、将任何一个闭环结构的每一个环断开成两条关节路径，闭环结构为带闭环的序列关节机械臂结构；

S12、断开后，将原闭环结构等价成多个开环结构，开环结构为开环序列结构；

S13、等价的开环结构中，将原被动驱动关节当做带有虚拟驱动器的主动关节，原闭环结构由于闭环施加的力约束被开环结构的纯几何约束替代。

第二步、计算等价的开环结构扭矩，再转换为闭环结构中的驱动电机的实际扭矩。

要由等价的开环结构的扭矩计算出闭环结构的扭矩，需要求出开环结构与闭环结构的关节角度之间的对应关系，这种关系在简单结构中可以通过几何分析直接推导，例如平行四连杆，然而能应用于任意闭环结构的通用控制设计方法，本发明提出一种不需要解析，而是通过实验或仿真数据，列出线性方程组的方法来解出闭环与开环的关节映射。称为开环闭环雅科宾矩阵计算，详细过程如下：

S21、使用仿真软件或者实验，测定若干组闭环结构在不同姿态下的关节角度，获得多组关节角度数据，关节角度数据量优选关节数量的4倍以上，数据量越多，得到的解精准度越高，由于只是对闭环结构在不同姿态下的关节角度进行测量或者仿真，数据获取相对比较容易。

S22、设步骤S21取得了N组关节角度数据，每一组都由Q_close[1]到Q_close[n]组成，其中n为闭环结构中实际关节的数量，令Q_open[1]到Q_open[m]为此闭环结构等价的开环结构的所有关节的关节角度，其中m为开环结构中关节的数量，将所有数据汇总并形成矩阵形式，如式(1)：

Qc＝A*Qo (1)

其中Qc为n*N的矩阵，Qo为m*N的矩阵，A为n*m的矩阵。

S23、利用最小二乘法解出式(1)的A矩阵，即得到了开环结构与闭环结构的关节角度之间的对应关系。

然后再利用雅科宾矩阵计算闭环结构中的驱动电机的实际扭矩。包括如下步骤：

S24、将每一个闭环结构对应的开环结构，采用递归欧拉方法算出开环结构关节扭矩后，对每一个闭环结构分别解出其闭环扭矩，对于一个闭环结构，其驱动电机扭矩tao_c与开环结构关节扭矩tao_o的关系可由式(2)得出：

tao_c＝A*tao_o (2)

其中tao_c为n维列向量，tao_o为m维列向量。

S25、在计算完所有驱动电机的扭矩后，将所有驱动电机的计算扭矩命名为Torque_computed，Torque_computed为s维向量，其中s为机械臂总电机数量。

第三步、利用电机电流估算扭矩。

本发明的带闭环的序列关节机械臂结构为带有传动系统和扭矩传感器的机械臂结构，虽然从扭矩传感器已经能获得测量扭矩值，但是电机端的电流在受到外力时依然会产生变化，只是这种变化经过传动系统的高减速比而被削弱，但是它仍然存在，且具有相当高的信躁比，因此本发明从电机电流值中估算出扭矩值，以便与扭矩传感器的测量扭矩值结合使用，提高系统的鲁棒性与抗躁能力，具体步骤如下：

S31、在有刷与无刷直流电机中，电机输出扭矩有如式(3)关系：

I*R＝V-Kv*w

Torque_motor＝I*Kv*gear (3)

其中V为驱动器提供给电机的电压，I为电机电流，R为电机电阻值，w为电机转速，单位为radian/sec，Kv为电机常数。除了I之外其他值均可从对电机的标定或者厂商数据获得，I*Kv是电机端扭矩。

S32、再根据传动系统的减速比将电机端扭矩I*Kv放大，得到与扭矩传感器相同规模的数值，即从电机电流值中估算出扭矩值Torque_motor。

第四步、结合电机电流的估算扭矩值、扭矩传感器的测量扭矩值和第二步理论计算出的实际扭矩判断是否产生碰撞信号。

具体步骤如下：

S41、假设任意时刻机械臂的扭矩传感器的测量扭矩值为Torque_sensor，电机电流的估算扭矩值为Torque_motor，将扭矩传感器的测量扭矩值和电机电流的估算扭矩值与理论计算扭矩Torque_computed相减得到两个余项，如式(4)：

Residual_sensor＝Torque_sensor–Torque_computed

Residual_motor＝Torque_motor–Torque_comuted (4)

S42、分别对两个余项计算高通滤波值和低通滤波值，如式(5)：

Residual_sensor_lowpass＝0.01*Residual_sensor_lowpass+(1-0.01)*Residual_sensor

Residual_sensor_highpass＝0.2*Residual_sensor_highpass+(1-0.2)*Residual_sensor

Residual_motor_lowpass＝0.01*Residual_motor_lowpass+(1-0.01)*Residual_motor

Residual_motor_highpass＝0.2*Residual_motor_highpass+(1-0.2)*Residual_motor (5)

S43、判断高通滤波值与低通滤波值的差值是否大于设定的阈值，若是，则判断有碰撞发生，如式(6)：

Contact_sensor＝|Residual_sensor_highpass-Residual_sensor_lowpass|>threshold

Contact_motor＝|Residual_motor_highpass-Residual_motor_lowpass|>threshold (6)

本发明考虑到扭矩传感器是安装在关节处，会比电机先感应到碰撞，因此，加入一个额外条件，即Contact_sensor时间必须早于Contact_motor发生，这样就实现了同时利用扭矩传感器与电机读数来判断碰撞信号，且不会被扭矩传感器或电机这两方单方面的噪声所干扰。

第五步、根据碰撞信号的大小及符号进行刹车或电子助力。

具体步骤如下：

S51、根据高通滤波值与低通滤波值的差值的大小，得出碰撞外力的大小，其符号可以得出碰撞外力的方向。

S52、根据碰撞信号的大小及符号进行如下操作：

A、进行碰撞检测时，只要检测到碰撞信号即令全部电机刹车。

B、进行拖动示教时，通过电机补偿进行电子助力，为每一个电机各自的信号值大小，在位置速度环的内部额外补偿一个电压信号，其大小为：

V_gain＝(Residual_sensor_highpass-Residual_sensor_lowpass)*R/Kt

理论上力需要先乘以电阻，再除以电机常数才能转换成电压值。

本发明不局限于上述可选实施方式，任何人在本发明的启示下都可得出其他各种形式的产品，但不论在其形状或结构上作任何变化，凡是落入本发明权利要求界定范围内的技术方案，均落在本发明的保护范围之内。

Claims (10)

1.一种基于闭环结构的扭矩计算方法，其特征在于：包括如下步骤：

S1、闭环切割：将任何一个闭环结构的每一个环断开成两条关节路径，断开后，将原闭环结构等价成多个开环结构，等价的开环结构中，将原被动驱动关节当做带有虚拟驱动器的主动关节；

S2、计算等价的开环结构扭矩，再转换为闭环结构中的驱动电机的实际扭矩。

2.根据权利要求1所述的一种基于闭环结构的扭矩计算方法，其特征在于：所述步骤S2的实现方法为：计算等价的开环结构扭矩，求出开环结构与闭环结构的关节角度之间的对应关系，然后计算闭环结构中的驱动电机的实际扭矩。

3.根据权利要求2所述的一种基于闭环结构的扭矩计算方法，其特征在于：求出开环结构与闭环结构的关节角度之间的对应关系的步骤包括：

S21、使用仿真软件或者实验，测定若干组闭环结构在不同姿态下的关节角度，获得多组关节角度数据；

S22、设步骤S21取得了N组关节角度数据，每一组都由Q_close[1]到Q_close[n]组成，其中n为闭环结构中实际关节的数量，令Q_open[1]到Q_open[m]为此闭环结构等价的开环结构的所有关节的关节角度，其中m为开环结构中关节的数量，将所有数据汇总并形成矩阵形式，如式(1)：

Qc＝A*Qo (1)

其中Qc为n*N的矩阵，Qo为m*N的矩阵，A为n*m的矩阵；

S23、利用最小二乘法解出式(1)的A矩阵，即得到了开环结构与闭环结构的关节角度之间的对应关系。

4.根据权利要求3所述的一种基于闭环结构的扭矩计算方法，其特征在于：所述步骤S21中，关节角度数据量为关节数量的4倍以上。

5.根据权利要求4所述的一种基于闭环结构的扭矩计算方法，其特征在于：计算闭环结构中的驱动电机的实际扭矩的步骤包括：

S24、将每一个闭环结构对应的开环结构，采用递归欧拉方法算出开环结构关节扭矩后，对每一个闭环结构分别解出其闭环扭矩，对于一个闭环结构，其驱动电机扭矩tao_c与开环结构关节扭矩tao_o的关系可由式(2)得出：

tao_c＝A*tao_o (2)

其中tao_c为n维列向量，tao_o为m维列向量；

S25、在计算完所有驱动电机的实际扭矩后，将所有驱动电机的计算扭矩命名为Torque_computed，Torque_computed为s维向量，其中s为机械臂总电机数量。

6.由权利要求5所述的一种基于闭环结构的扭矩计算方法计算出的实际扭矩进行碰撞检测与拖动示教方法，其特征在于：包括如下步骤：

S3、利用电机电流估算扭矩；

S4、结合电机电流的估算扭矩值、扭矩传感器的测量扭矩值和计算出的实际扭矩判断是否产生碰撞信号；

S5、根据碰撞信号的大小及符号进行刹车或电子助力。

7.根据权利要求6所述的碰撞检测与拖动示教方法，其特征在于：所述步骤S3利用电机电流估算扭矩的步骤包括：

S31、在有刷与无刷直流电机中，电机输出扭矩有如式(3)关系：

I*R＝V-Kv*w

Torque_motor＝I*Kv*gear (3)

其中V为驱动器提供给电机的电压，I为电机电流，R为电机电阻值，w为电机转速，单位为radian/sec，Kv为电机常数，I*Kv是电机端扭矩；

S32、再根据传动系统的减速比将电机端扭矩I*Kv放大，得到与扭矩传感器相同规模的数值，即从电机电流值中估算出扭矩值Torque_motor。

8.根据权利要求7所述的碰撞检测与拖动示教方法，其特征在于：所述步骤S4的具体实现步骤包括：

S41、假设任意时刻机械臂的扭矩传感器的测量扭矩值为Torque_sensor，电机电流的估算扭矩值为Torque_motor，将扭矩传感器的测量扭矩值和电机电流的估算扭矩值与理论计算扭矩Torque_computed相减得到两个余项，如式(4)：

Residual_sensor＝Torque_sensor–Torque_computed

Residual_motor＝Torque_motor–Torque_computed (4)

S42、分别对两个余项计算高通滤波值和低通滤波值，如式(5)：

Residual_sensor_lowpass＝0.01*Residual_sensor_lowpass+(1-0.01)*Residual_sensor

Residual_sensor_highpass＝0.2*Residual_sensor_highpass+(1-0.2)*Residual_sensor

Residual_motor_lowpass＝0.01*Residual_motor_lowpass+(1-0.01)*Residual_motor

Residual_motor_highpass＝0.2*Residual_motor_highpass+(1-0.2)*Residual_motor (5)

S43、判断高通滤波值与低通滤波值的差值是否大于设定的阈值，若是，则判断有碰撞发生，如式(6)：

Contact_sensor＝|Residual_sensor_highpass-Residual_sensor_lowpass|>threshold

Contact_motor＝|Residual_motor_highpass-Residual_motor_lowpass|>threshold(6)。

9.根据权利要求8所述的碰撞检测与拖动示教方法，其特征在于：所述步骤S43中，判断有碰撞发生还包括条件：Contact_sensor时间早于Contact_motor发生。

10.根据权利要求8所述的碰撞检测与拖动示教方法，其特征在于：所述步骤S5的具体步骤包括：

S51、根据高通滤波值与低通滤波值的差值的大小，得出碰撞外力的大小，其符号得出碰撞外力的方向；

S52、根据碰撞信号的大小及符号进行如下操作：

A、进行碰撞检测时，检测到碰撞信号即令全部电机刹车；

B、进行拖动示教时，通过电机补偿进行电子助力，为每一个电机各自的信号值大小，在位置速度环的内部额外补偿一个电压信号，其大小为：

V_gain＝(Residual_sensor_highpass-Residual_sensor_lowpass)*R/Kv。

Images