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

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

【技术实现步骤摘要】
一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法
本专利技术属于扭矩补偿领域，具体涉及一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法。此方法可以用于任何带有闭环结构，如平行四连杆与扭矩传感器的机械臂，为其每个关节计算主动补偿的扭矩，从而实现精准的轨迹跟随，以及碰撞检测及拖动示教。
技术介绍
目前的市面上的协作机器人全部采用“扭矩电机”，或者叫“盘式电机”。此种电机无需额外的传动系统即可提供足够的扭矩。对这类电机组成的机械臂进行碰撞检测或拖动示教的电子助力时，由于没有传动系统，电机的电流对外力的响应较快，因此可以直接使用电流作为电机输出端所受扭矩的估计值。然而此类电机最大的缺点是成本高，且成本与扭力大小不成线性关系。因此基于扭矩电机的协作机械臂，可延展性较差。这就使得目前市面上的协作机器人均限制在一定的形态以及臂展、负重等参数范围内，很难有突破。为了使协作机械臂能有更大的臂展和负重，需要更加灵活的本体结构与传动系统的配合，例如基于主动驱动的平行四连杆的机械臂结构。然而这类机械臂结构，由于较高的传动比，使得电机输出端感受到的扭力变化相对实际的扭力施加处较小，也就导致实际发生碰撞时，电机的电流变化较小，在电机内信噪比较高的环境下很难单凭电流判断是否真的发生碰撞，且难以判断外力的大小。此外，基于闭环的机械臂结构无法直接使用递归牛顿欧拉方法对其各个关节的补偿扭矩进行计算，因此需要特定的方式求解闭环结构的各个关节的补偿扭矩。
技术实现思路
为了解决现有技术存在的上述问题，本专利技术目的在于提供一种基于闭环结构的扭矩计算、碰撞检测与拖动示教方法。本专利技术所采用的技术方案为：一种基于闭环结构的扭矩计算方法，包括如下步骤：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*wTorque_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_computedResidual_motor＝Torque_motor–Torque_comuted(4)S42、分别对两个余项计算高通滤波值和低通滤波值，如式(5)：Residual_sensor_lowpass＝0.01*Residual_sensor_lowpass+(1-0.01)*Residual_sensorResidual_sensor_highpass＝0.2*Residual_sensor_highpass+(1-0.2)*Residual_sensorResidual_motor_lowpass＝0.01*Residual_motor_lowpass+(1-0.01)*Residual_motorResidual_motor_highpass＝0.2*Residual_motor_highpass+(1-0.2)*Residual_motor(5)S43、判断高通滤波值与低通滤波值的差值是否大于设定的阈值，若是，则判断有碰撞发生，如式(6)：Contact_sensor＝|Residual_sensor_highpass-Residual_sensor_lowpass|>thresholdContact_motor＝|R本文档来自技高网...

【技术保护点】
1.一种基于闭环结构的扭矩计算方法，其特征在于：包括如下步骤：S1、闭环切割：将任何一个闭环结构的每一个环断开成两条关节路径，断开后，将原闭环结构等价成多个开环结构，等价的开环结构中，将原被动驱动关节当做带有虚拟驱动器的主动关节；S2、计算等价的开环结构扭矩，再转换为闭环结构中的驱动电机的实际扭矩。

【技术特征摘要】
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*wTorque_motor＝I*Kv*gear(3)其中V为驱动器提供给电机的电压，I为电机电流，R为电机电阻值，w为电机转速，单位为rad...

【专利技术属性】
技术研发人员：唐悦，卢展宏，
申请(专利权)人：北京秘塔网络科技有限公司，
类型：发明
国别省市：北京,11