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

权利要求书

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。