[发明专利]一种闭链式多臂机器人柔顺控制方法

权利要求书

1.一种闭链式多臂机器人柔顺控制方法，其特征在于，机器人有n条机械臂，序号为i=1,…,I；所述控制方法具体步骤如下：

步骤S10，由机械臂操作任务计算机械臂i末端期望接触力；

步骤S11，通过安装在机械臂i末端六维力矩传感器测量机械臂i末端实际接触力；

步骤S12，由所述机械臂操作任务计算机械臂i末端期望位姿；

步骤S13，通过机械臂关节角度传感器测量机械臂i关节绝对角度；

步骤S14，向闭链式多机械臂同步阻抗控制器中引入所述机械臂i末端期望操作力、机械臂i末端实际接触力、机械臂i末端期望位姿和机械臂i关节绝对角度数据；其具体过程为：

步骤S1400，根据所述机械臂i末端实际接触力和机械臂i末端期望接触力得到机械臂i的末端力误差，并将所述机械臂i的末端力误差引入到机械臂i末端阻抗控制器中；

步骤S1401，通过所述机械臂i末端阻抗控制器得到阻抗期望加速度；

步骤S1402，通过所述机械臂i末端阻抗控制器得到阻抗期望速度；

步骤S1403，根据所述机械臂i末端阻抗控制器得到阻抗期望位姿；

步骤S1404，根据机械臂i的期望轨迹得到运动期望加速度；

步骤S1405，根据机械臂i的期望轨迹得到运动期望速度；

步骤S1406，根据机械臂i的期望轨迹得到运动期望位姿；

步骤S1407，通过机械臂角度传感器测量机械臂关节绝对角度；

步骤S1408，根据所述阻抗期望位姿和运动期望位姿得到机械臂期望位姿；

步骤S1409，根据所述机械臂关节绝对角度计算机械臂末端实际位姿；

步骤S1410，将所述机械臂期望位姿和所述机械臂末端实际位姿引入多机械臂同步控制器；

步骤S15，根据所述闭链式多机械臂同步阻抗控制器得到机械臂期望加速度；

步骤S16，根据所述闭链式多机械臂同步阻抗控制器得到机械臂期望速度；

步骤S17，根据所述闭链式多机械臂同步阻抗控制器得到械臂同步协调期望位姿；

步骤S18，实现机械臂闭环控制。

2.根据权利要求1所述的一种闭链式多臂机器人柔顺控制方法，其特征在于，

步骤S15，所述机械臂期望加速度由所述阻抗期望加速度和运动期望加速度得到；

步骤S16，所述机械臂期望速度由所述阻抗期望速度和运动期望速度得到；

步骤S17，所述械臂同步协调期望位姿由所述阻抗期望位姿和运动期望位姿得到。

3.根据权利要求2所述的一种闭链式多臂机器人柔顺控制方法，其特征在于，所述步骤S18的具体过程如下：

步骤S1801，通过步骤S1407测量得到的所述机械臂关节绝对角度和建立关节的速度观测器计算得到关节角速度；

步骤 S1802，根据机械臂速度雅克比关系和步骤S1801计算得到的所述关节角速度求得机械臂末端实际速度；

步骤 S1803，根据所述机械臂末端实际速度和机械臂末端位姿计算机械臂末端实际加速度；

步骤 S1804，根据所述机械臂末端实际速度和机械臂期望速度之间的差值，求得机械臂速度补偿量；

步骤 S1805，根据雅克比关系和步骤S1804得到的机械臂速度补偿量求得瞬间时刻的关节速度补偿量，在每个控制闭环周期内，该关节速度补偿量与关节速度补偿量等效；

步骤S1806，根据所述机械臂末端实际加速度和机械臂期望加速度之间的差值得到机械臂加速度补偿量；

步骤 S1807，根据所述机械臂加速度补偿量得到关节加速度补偿量；

步骤S1808，根据所述机械臂同步协调期望位姿和机械臂末端实际位姿差得到机械臂位姿补偿量；

步骤S1809，根据所述机械臂位姿补偿量和逆运动学关系得到关节角度补偿量；

步骤 S1810，根据步骤S1805得到的瞬间时刻的关节速度补偿量，步骤S1807得到的关节加速度补偿量和步骤S1809得到关节角度补偿量实现机械臂关节闭环控制。

4.根据权利要求3所述的一种闭链式多臂机器人柔顺控制方法，其特征在于，所述机械臂的阻抗控制系统采用笛卡尔阻抗控制系统；所述笛卡尔阻抗控制系统的内环采用笛卡尔位置控制，外环采用阻抗控制器。

5.根据权利要求4所述的一种闭链式多臂机器人柔顺控制方法，其特征在于，所述同步阻抗控制器输入有绝对位姿误差、同步位姿误差以及耦合位姿误差。