[发明专利]协作机械臂及其运动控制方法、碰撞检测方法、控制系统

权利要求书

1.一种协作机械臂的运动控制方法，其特征在于，包括：

根据运动控制任务，计算关节的关节理论运动状态；

根据所述关节理论运动状态，计算关节伺服电机的理论力矩；

根据所述理论力矩，计算所述关节伺服电机的电流限制范围；

控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围。

2.根据权利要求1所述的运动控制方法，其特征在于，

所述根据所述理论力矩，计算所述关节伺服电机的电流限制范围，包括：

根据所述理论力矩以及所述关节伺服电机的调节力矩阈值，计算所述电流限制范围。

3.根据权利要求2所述的运动控制方法，其特征在于，

所述电流限制范围包括电流输出约束上限和电流输出约束下限；

所述控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围，包括：

当所述给所述关节伺服电机的驱动电流超过所述电流输出约束上限时，按照所述电流输出约束上限对所述驱动电流进行调整，得到调整后的驱动电流；

当所述给所述关节伺服电机的驱动电流低于所述电流输出约束下限时，按照所述电流输出约束下限对所述驱动电流进行调整，得到调整后的驱动电流。

4.根据权利要求3所述的运动控制方法，其特征在于，所述方法还包括：

根据所述理论力矩与所述调节力矩阈值确定力矩约束上限和力矩约束下限；

基于所述力矩约束上限与预设的转矩常数计算所述电流输出约束上限；

基于所述力矩约束下限与所述预设的转矩常数计算所述电流输出约束下限。

5.根据权利要求1所述的运动控制方法，其特征在于，

所述根据运动控制任务，计算关节的关节理论运动状态之后，所述运动控制方法还包括：

根据所述关节理论运动状态计算所述关节伺服电机的电机理论运动状态；

将所述电机理论运动状态输入对应的PID控制模块，计算所述关节伺服电机的驱动电流。

6.根据权利要求5所述的运动控制方法，其特征在于，

所述电机理论运动状态包括理论转角状态，所述PID控制模块为位置式闭环PID控制模块。

7.根据权利要求6所述的运动控制方法，其特征在于，

所述控制输出给所述关节伺服电机的驱动电流不超出所述电流限制范围，包括：

获取所述关节伺服电机的实际转角；

根据所述电机理论运动状态，获取当前时刻对应的所述关节伺服电机的理论转角；

将所述实际转角和所述理论转角输入到所述PID控制模块或将所述实际转角与所述理论转角的差值输入到所述PID控制模块，通过所述PID控制模块对输出给所述关节伺服电机的驱动电流进行调整，控制调整后的驱动电流不超出所述电流限制范围。

8.根据权利要求1所述的运动控制方法，其特征在于，所述根据所述关节理论运动状态，计算关节伺服电机的理论力矩，包括；

将所述关节理论运动状态输入预设的逆动力学模型；

利用所述逆动力学模型对所述关节理论运动状态进行运算，得到关节理论力矩；

获取所述关节的减速比；

根据所述关节理论力矩和所述减速比，计算所述关节伺服电机的理论力矩；

其中，所述关节理论运动状态包括关节转角、关节速度以及关节加速度。

9.根据权利要求8所述的运动控制方法，其特征在于，

所述利用所述逆动力学模型对所述关节理论运动状态进行运算，得到关节理论力矩，包括：

利用所述逆动力学模型中的机器人惯量和加速度，获取惯量力矩；

利用所述逆动力学模型中的科氏力与离心力模型和速度，获取科氏力与离心力力矩；

利用所述逆动力学模型中的重力模型和关节转角，获取重力力矩；

利用所述逆动力学模型中的摩擦力模型和关节速度，获取摩擦力力矩；

将所述惯量力矩、所述科氏力与离心力力矩、所述重力力矩以及所述摩擦力力矩相加，得到所述关节理论力矩。