[发明专利]一种多关节等速训练控制方法及系统

权利要求书

1.一种多关节等速训练控制方法，其特征是，包括以下步骤：

S1：获取转动臂的目标转速；

S2：在时刻，获取转动臂的第一转动角度，获取驱动组件对转动臂的第一输出扭矩，获取转动臂的第一实时转速；

S3：根据所述第一实时转速和所述第一转动角度，计算在时刻时，所述转动臂的第二转动角度；

S4：根据所述第一实时转速、所述第一转动角度和所述第一输出扭矩，计算在时刻时，驱动组件对转动臂的第二输出扭矩和所述转动臂的第二实时转速；

S5：在时刻时，驱动组件以第二输出扭矩驱动所述转动臂以所述第二实时转速转动；

S6：令=，并重复步骤S2-S5；

在步骤S2中，通过第一输出扭矩可以计算外力矩，所述外力矩的计算公式如下：

其中：为扭矩传感器获取的系统整体扭矩，为转动臂自身重力，为转动臂质心距离旋转中心的等效杆长，为转动臂的角度；

在步骤S3中，所述第二转动角度的计算公式为：

；

其中，为第二转动角度，为第一转动角度，为第一实时转速；

在步骤S4中，所述第二输出扭矩和所述第二实时转速均采用PID算法计算，所述PID算法的计算公式为：

其中：为比例系数，为用户设定值与当前状态值得差；为积分系数，为误差的累加；为微分系数，为当前误差与上次误差的差；

在所述转动臂的所述第二实时转速与所述目标转速不同时，所述驱动组件输出的所述第二输出扭矩和所述转动臂的所述第二实时转速均通过所述PID算法计算。

2.根据权利要求1所述的一种多关节等速训练控制方法，其特征是，在步骤S2中，通过角度传感器获取所述转动臂的所述第一转动角度；通过扭矩传感器获取所述驱动组件对所述转动臂施加的所述第一输出扭矩；通过转速传感器获取所述转动臂的所述第一实时转速。

3.根据权利要求1所述的一种多关节等速训练控制方法，其特征是，在步骤S1中，还需要获取转动臂的限位角度。

4.一种多关节等速训练控制系统，用于实现权利要求1-3任一所述的一种多关节等速训练控制方法，其特征是，包括：

角度获取模块，用于获取转动臂的摆动角度；

输出扭矩获取模块，用于获取驱动组件的输出扭矩；

转速获取模块，用于获取转动臂的转速；

控制器，用于根据第一实时转速和第一转动角度计算第二转动角度、用于根据第一实时转速、第一转动角度和第一输出扭矩计算第二输出扭矩和第二实时转速、用于控制驱动组件输出第二输出扭矩。

5.根据权利要求4所述的一种多关节等速训练控制系统，其特征是，所述控制器还包括计时模块。

6.根据权利要求4或5所述的一种多关节等速训练控制系统，其特征是，所述角度获取模块设为角度传感器；所述输出扭矩获取模块设为扭矩传感器；所述转速获取模块设为转速传感器。