本发明涉及机器人的控制，尤其涉及一种闭链式多臂机器人柔顺控制方法。
背景技术：
当多臂机器人力封闭情况下完成一个操作任务时，不仅需要保证机械臂的安全性，还需要保证操作物体的安全性，单臂柔顺控制可以保证机械臂的末端操作安全，但是当多个机械臂同时操作物体时，由于机械臂期望位姿和实际位姿之间存在误差，物体存在受力过大或者过小的可能性，导致物体的安全性将无法保证。近年来，随着力控制的需求越来越大，多臂协调问题的研究也逐渐从纯位置控制、主从控制、力/位混合控制发展到了阻抗控制阶段。在多臂紧协调操作任务中，为了保证系统的整体安全性，纯位置控制方法对多臂的运动学参数要求非常高，这需要对机械臂的运动学参数进行很好的标定，同时对于机械臂控制需要精确的模型，因此这种控制方式仅适用于系统的位置柔性较大的情况；在位置控制模式下，为了协调多臂机器人之间的运动，主从控制方式被提出，但是该方法的控制器为两个，两个控制器之间通过协调器进行数据之间的交换，因此多条臂之间的控制命令存在耦合，不利于操作稳定；力/位混合控制在多臂协调中存在力控制和位置控制之间转换的问题，尤其在操作状态发生变化的情况时，多臂系统力控制和位置控制存在阶跃跳变的情况；相反，阻抗控制通过不断调整刚度系数，有效地避免了在状态切换过程中力或者位置控制的跳变情况。然而，单纯的阻抗控制方法忽略了机器人多臂的相对误差，导致两者的叠加误差不断累积，进而严重影响多臂的操作性能。因此不适用于实现物体与多臂机器人之间相对位置和接触力的协调控制。
技术实现要素：
针对国内外相关技术存在的问题和缺陷，提出一种用于处理具有阻抗特性的闭链式多臂机器人的位姿同步性问题的控制方法。
本发明为解决上述技术问题采用以下技术方案：
一种闭链式多臂机器人柔顺控制方法，机器人有n条机械臂，序号为i＝1,…,i；所述控制方法具体步骤如下：
步骤s10，由机械臂操作任务计算机械臂i末端期望接触力；
步骤s11，通过安装在机械臂i末端六维力矩传感器测量机械臂i末端实际接触力；
步骤s12，由所述机械臂操作任务计算机械臂i末端期望位姿；
步骤s13，通过机械臂关节角度传感器测量机械臂i关节绝对角度；
步骤s14，向闭链式多机械臂同步阻抗控制器中引入所述机械臂i末端期望操作力、机械臂i末端实际接触力、机械臂i末端期望位姿和机械臂i关节绝对角度数据；
步骤s15，根据所述闭链式多机械臂同步阻抗控制器得到机械臂期望加速度；
步骤s16，根据所述闭链式多机械臂同步阻抗控制器得到机械臂期望速度；
步骤s17，根据所述闭链式多机械臂同步阻抗控制器得到械臂同步协调期望位姿。
步骤s18，实现机械臂闭环控制。
进一步的，所述步骤s14的具体过程如下：
步骤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的具体过程如下：
步骤s1801，通过步骤s1407测量得到的所述机械臂关节绝对角度和建立关节的速度观测器计算得到关节角速度；
步骤s1802，根据机械臂速度雅克比关系和步骤s1801计算得到的所述关节角速度求得机械臂末端实际速度；
步骤s1803，根据所述机械臂末端实际速度和机械臂末端位姿计算机械臂末端实际加速度；
步骤s1804，根据所述机械臂末端实际速度和机械臂期望速度之间的差值，求得机械臂速度补偿量；
步骤s1805，根据雅克比关系和步骤s1804得到的机械臂速度补偿量求得瞬间时刻的关节速度补偿量，在某一个很小的时间段内，该关节速度补偿量与关节速度补偿量等效；
步骤s1806，根据所述机械臂末端实际加速度和机械臂期望加速度之间的差值得到机械臂加速度补偿量；
步骤s1807，根据所述机械臂加速度补偿量得到关节加速度补偿量；
步骤s1808，根据所述机械臂同步协调期望位姿和机械臂末端实际位姿差得到机械臂位姿补偿量；
步骤s1809，根据所述机械臂位姿补偿量和逆运动学关系得到关节角度补偿量；
步骤s1810，根据步骤s1805得到的瞬间时刻的关节速度补偿量，步骤s1807得到的关节加速度补偿量和步骤s1809得到关节角度补偿量实现机械臂关节闭环控制。
进一步的，所述机械臂的阻抗控制系统采用笛卡尔阻抗控制系统；所述笛卡尔阻抗控制系统的内环采用笛卡尔位置控制，外环采用阻抗控制器。
进一步的，所述同步阻抗控制器输入有绝对位姿误差、同步位姿误差以及耦合位姿误差。
与现有技术相比，本发明有以下有益效果：
本发明提供的一种闭链式多臂机器人同步阻抗控制方法，可以应用于物体与多臂机器人之间相对位置和接触力的协调控制，实现系统控制精度的提高、以及确保系统安全性与稳定性，为多臂机器人的共融技术提供了便利；设计以绝对位姿误差、同步位姿误差以及耦合位姿误差作为输入的同步阻抗控制器，从而实现闭链式多臂机器人系统的协调柔顺特性，进而保证多臂末端位姿的同步柔顺性。
附图说明
图1为闭链式多臂机器人同步阻抗控制流程；
图2为闭链式同步阻抗控制器框图；
图3为机械臂控制框图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，下面结合附图及具体实施例对本发明的技术方案做进一步的详细说明：
如图1所示，一种闭链式多臂机器人柔顺控制方法，设机器人有n条机械臂，序号为i＝1,…,i；控制方法具体步骤如下：
步骤s10，由机械臂操作任务计算机械臂i末端期望接触力；
步骤s11，通过安装在机械臂i末端六维力矩传感器测量机械臂i末端实际接触力；
步骤s12，由机械臂操作任务计算机械臂i末端期望位姿；
步骤s13，通过机械臂关节角度传感器测量机械臂i关节绝对角度；
步骤s14，向闭链式多机械臂同步阻抗控制器中引入机械臂i末端期望操作力、机械臂i末端实际接触力、机械臂i末端期望位姿和机械臂i关节绝对角度数据。
该控制器的结构如图2所示：
步骤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，实现机械臂闭环控制，如图3所示：
步骤s1801，通过步骤s1407测量得到的机械臂关节绝对角度和建立关节的速度观测器计算得到关节角速度；
步骤s1802，根据机械臂速度雅克比关系和步骤s1801计算得到的关节角速度求得机械臂末端实际速度；
步骤s1803，根据机械臂末端实际速度和机械臂末端位姿计算机械臂末端实际加速度；
步骤s1804，根据机械臂末端实际速度和机械臂期望速度之间的差值，求得机械臂速度补偿量；
步骤s1805，根据雅克比关系和步骤s1804得到的机械臂速度补偿量求得瞬间时刻的关节速度补偿量，在某一个很小的时间段内，该关节速度补偿量与关节速度补偿量等效；
步骤s1806，根据机械臂末端实际加速度和机械臂期望加速度之间的差值得到机械臂加速度补偿量；
步骤s1807，根据机械臂加速度补偿量得到关节加速度补偿量；
步骤s1808，根据机械臂同步协调期望位姿和机械臂末端实际位姿差得到机械臂位姿补偿量；
步骤s1809，根据机械臂位姿补偿量和逆运动学关系得到关节角度补偿量；
步骤s1810，根据步骤s1805得到的瞬间时刻的关节速度补偿量，步骤s1807得到的关节加速度补偿量和步骤s1809得到关节角度补偿量实现机械臂关节闭环控制。
机械臂的阻抗控制系统采用笛卡尔阻抗控制系统；笛卡尔阻抗控制系统的内环采用笛卡尔位置控制，外环采用阻抗控制器。
同步阻抗控制器输入有绝对位姿误差、同步位姿误差以及耦合位姿误差。
以上所述实施方式仅是本发明的具体和详细描述，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。