[发明专利]机器人的人机协作控制方法、装置和机器人

权利要求书

1.一种机器人的人机协作控制方法，其特征在于，应用于包括多个机械臂的机器人，每个机械臂末端在不同运动方向上设有各自的控制模式，所述不同运动方向包括负载接触面的法线方向，所述法线方向设有导纳力控模式，所述多个机械臂能用于当接触负载时在所述法线方向上受到方向相反的作用力，所述方法包括：

在依据所述导纳力控模式控制每个机械臂末端在沿所述负载接触面的法线方向上的实际接触力达到期望作用力的情况下，检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力；

若存在外力，则根据每个机械臂末端在各个运动方向上受到的外力分量依据对应运动方向上的控制模式计算对应方向上的位置偏移量，根据对应机械臂在每个运动方向上的位置偏移量和受到所述外力时的初始位置控制所述对应机械臂进行运动以响应负载跟随操作。

2.根据权利要求1所述的方法，其特征在于，所述检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力之前，所述方法还包括：

在控制对应机械臂末端保持与所述期望作用力相等的实际接触力的情况下，依据预设规划路线控制所有机器臂运动到目标位置。

3.根据权利要求1所述的方法，其特征在于，所述不同运动方向包括与所述负载接触面的法线方向垂直的重力方向、与所述重力方向和所述法线方向分别垂直的另一方向，所述重力方向和所述另一方向上各自设有导纳阻抗模式或导纳拖动模式，每个机械臂末端在所述重力方向上和所述另一方向上设有对应的预设阈值；

计算对应机械臂末端在所述重力方向或所述另一方向上的位置偏移量，包括：

根据对应机械臂末端在重力方向或另一方向上受到的外力分量与对应的预设阈值之间的差值依据对应方向上的控制模式计算所述对应机械臂末端在重力方向或另一方向上的位置偏移量。

4.根据权利要求3所述的方法，其特征在于，在所述导纳阻抗模式下，当检测到外力分量消失预定时间时，控制所述机械臂末端在对应运动方向上返回至受到外力分量时的初始位置；

在所述导纳拖动模式下，当检测到外力分量消失时，控制所述机械臂末端在对应运动方向上停留在当前位置。

5.根据权利要求3所述的方法，其特征在于，所述导纳阻抗模式的控制方程为：

；

其中，M_d、B_d和K_d分别为惯性矩阵、阻尼矩阵和刚度矩阵，为机械臂末端在所述重力方向或所述另一方向上受到的外力分量；X_r2、Ẋ_r2和Ẍ_r2依次为机械臂末端在所述重力方向或所述另一方向上的初始位置及所述初始位置的一阶导数和二阶导数；X_c2、Ẋ_c2和Ẍ_c2依次为机械臂末端在所述重力方向或所述另一方向上的期望位置、期望速度和期望加速度。

6.根据权利要求3所述的方法，其特征在于，所述导纳拖动模式的控制方程为：

；

其中，为机械臂末端在所述重力方向或所述另一方向上受到的外力分量；B_d为阻尼矩阵；Ẋ_r3为机械臂末端在所述重力方向或所述另一方向上的初始位置的一阶导数；Ẋ_c3为机械臂末端在所述重力方向或所述另一方向上的期望速度。