CN106695797A - 基于双臂机器人协同操作的柔顺控制方法及系统 - Google Patents

Abstract

本发明公开了一种基于双臂机器人协同操作的柔顺控制方法，包括双臂协同控制模块根据目标负载的期望运动轨迹和期望力建立动力学模型，得到各个机械臂的末端期望位姿和末端期望力；单臂控制模块完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度；驱动模块根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。本发明还提供一种基于双臂机器人协同操作的柔顺控制系统，本方案采用主从式和共享式策略进行负载的公共力分解，得到两个机械臂的期望操作力，进而提出了双臂协同操作的主从式力柔顺控制方法和共享式力柔顺控制方法。广泛应用于机器人技术的双臂协作控制领域。

Description

基于双臂机器人协同操作的柔顺控制方法及系统

技术领域

本发明涉及机器人技术的双臂协作控制领域，尤其涉及一种基于双臂机器人协同操作的柔顺控制方法及系统。

背景技术

单臂机器人的操作任务简单、灵巧性不足，与传统单个机械臂相比，双臂机器人更能适应工厂中为人类设计的工作空间，同时双臂可以完成装配、搬运等复杂的操作任务。双臂机器人在具有较高的灵活性和任务适应性的同时，也带来了更加复杂的建模、规划以及控制等问题。而双臂机器人的研究重点则为双臂协调完成复杂的载荷搬运以及装配等任务，由于双臂在进行协调操作时会形成闭链约束，同时双臂机器人系统在建模与控制上存在一定误差，如果不对末端操作力进行控制，很有可能会对负载甚至机械臂自身造成意想不到的破坏。

为了增强机械臂与环境的交互与感知，目前大多数机械臂均在关节或是末端安装有力传感器，通过阻抗控制方法或是力位混合控制方法即可实现机械臂的力柔顺操作。但是针对双臂协同搬运载荷等典型任务，尤其是双臂机器人与目标负载形成闭链约束后的双臂协同运动规划与柔顺操作问题，尚未提出安全可靠的控制方法。

发明内容

为了解决上述技术问题，本发明的目的是提供一种基于双臂机器人协同操作的柔顺控制方法及系统。

本发明所采用的技术方案是：

本发明提供一种基于双臂机器人协同操作的柔顺控制方法，包括以下步骤：

双臂协同控制模块根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

单臂控制模块完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

驱动模块根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。

作为该技术方案的改进，所述步骤还包括系统对双臂之间公共力的分解。

作为该技术方案的改进，所述对公共力的分解的方法包括采用主从分解模式。

作为该技术方案的改进，所述主从分解模式包括通过给定其中一机械臂的末端期望操作力/力矩F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T，进而可求解得到另一机械臂的末端期望力/力矩为其中，F_L表示负载所受环境的广义力；表示负载所受地球的广义重力，F_IL表示负载的广义惯性力，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵。

作为该技术方案的改进，所述对公共力的分解的方法包括采用共享分解模式。

进一步地，所述共享分解模式包括将两个机械臂同等看待，采用伪逆法求解负载受力平衡方程，可得到：

其中，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵，F_ea、F_eb分别为两机械臂的末端期望操作力/力矩，F_L表示负载所受环境的广义力；表示负载所受地球的广义重力；F_IL表示负载的广义惯性力。

进一步地，所述单臂控制模块采用基于位置的阻抗控制，通过机械臂的导纳特性将末端的力、力矩误差等效为输入的位置与姿态增量，实现机械臂的力柔顺操作。

另一方面，本发明还提供一种基于双臂机器人协同操作的柔顺控制系统，包括：双臂协同控制模块、驱动模块和单臂控制模块；

所述双臂协同控制模块用于执行步骤根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

所述单臂控制模块用于执行步骤完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

所述驱动模块用于执行步骤根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。

本发明的有益效果是：本发明提供的基于双臂机器人协同操作的柔顺控制方法及系统，针对双臂协同搬运载荷等典型任务，提出了目标负载操作的闭链双臂协调柔顺控制方法，可以确保双臂机器人在对负载进行闭链约束下的协同操作的同时，将机械臂的末端操作力控制在期望的范围。为了求解负载力平衡方程，采用主从式和共享式策略进行负载的公共力分解，得到两个机械臂的期望操作力，进而提出了双臂协同操作的主从式力柔顺控制方法和共享式力柔顺控制方法。单臂控制器采用基于位置的阻抗控制，仅需要进行机械臂末端操作力的检测以及关节空间的位置控制即可，降低了对硬件平台的要求。

本发明为双臂协同操作提供了一种安全可靠的控制方案，确保双臂机器人在对负载进行闭链约束下的协同操作时，将机械臂的末端操作力控制在期望的范围。本方案可以广泛适用于成熟的工业机械臂产品以及自主研发的机械臂实验平台，用以完成目标负载搬运以及装配等双臂协作任务。

附图说明

下面结合附图对本发明的具体实施方式作进一步说明：

图1是本发明第一实施例的双臂机器人操作目标载荷所形成的闭链约束示意图；

图2是本发明第二实施例的双臂操作时的负载受力分析示意图；

图3是本发明第三实施例的基于位置的阻抗控制示意图；

图4是本发明第四实施例的目标负载操作的闭链双臂协调柔顺控制示意图；

图5是本发明第五实施例的控制方法流程示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明提供一种基于双臂机器人协同操作的柔顺控制方法，包括以下步骤：

双臂协同控制模块根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

单臂控制模块完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

驱动模块根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。

作为该技术方案的改进，所述步骤还包括系统对双臂之间公共力的分解。

作为该技术方案的改进，所述对公共力的分解的方法包括采用主从分解模式。

作为该技术方案的改进，所述主从分解模式包括通过给定其中一机械臂的末端期望操作力/力矩F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T，进而可求解得到另一机械臂的末端期望力/力矩为其中，F_L表示负载所受环境的广义力；表示负载所受地球的广义重力，F_IL表示负载的广义惯性力，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵。

作为该技术方案的改进，所述对公共力的分解的方法包括采用共享分解模式。

进一步地，所述共享分解模式包括将两个机械臂同等看待，采用伪逆法求解负载受力平衡方程，可得到：

其中，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵，F_ea、F_eb分别为两机械臂的末端期望操作力/力矩，F_L表示负载所受环境的广义力；表示负载所受地球的广义重力；F_IL表示负载的广义惯性力。

进一步地，所述单臂控制模块采用基于位置的阻抗控制，通过机械臂的导纳特性将末端的力、力矩误差等效为输入的位置与姿态增量，实现机械臂的力柔顺操作。

另一方面，本发明还提供一种基于双臂机器人协同操作的柔顺控制系统，包括：双臂协同控制模块、驱动模块和单臂控制模块；

所述双臂协同控制模块用于执行步骤根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

所述单臂控制模块用于执行步骤完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

所述驱动模块用于执行步骤根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。

本发明公开了一种目标负载操作的闭链双臂协调柔顺控制方法，主要包括：建立双臂机器人系统的动力学模型，考虑闭链约束关系，得到双臂机器人的协调运动学以及动力学方程；以目标负载为研究对象，建立负载的内力平衡方程；采用主从式和共享式策略进行负载的公共力分解，得到两个机械臂的期望操作力，进而提出了双臂协同操作的主从式力柔顺控制方法和共享式力柔顺控制方法。双臂机器人的实时操作力由安装在机械臂末端的六维力/力矩传感器得到，各个机械臂的力柔顺操作通过基于位置的阻抗控制实现。本发明适用于存在闭链强耦合约束的双臂紧协调操作，在保证双臂协调运动的同时，将末端操作力控制在期望的范围内，为双臂机器人的闭链操作问题提供了一种安全有效的控制方法。

目标负载操作的闭链双臂协调柔顺控制方法，包括双臂协同控制模块、单臂控制模块、驱动模块，其还可包括传感器信息融合与时延后处理等模块。其中：双臂协同控制模块(双臂协同控制器)首先由任务规划得到负载的期望运动轨迹和期望力作为输入，进一步通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿输入和末端期望力输入；

单臂控制模块(单臂控制器)完成对任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

驱动模块(底层硬件)根据规划的期望关节角度，驱动机械臂完成任务轨迹；

传感器信息融合与时延后处理模块采集双臂机器人系统各个传感器的信息，对信号间的时差和时延进行处理，最后同时反馈给上位机控制软件。

其中，双臂协同控制模块完成双臂之间的协调位置分解与公共力分解考虑双臂对目标负载操作时所形成的闭链约束，可以得到如下双臂协调运动学方程以及各个机械臂的动力学方程如下：

其中，表示坐标系Oy相对于坐标系Ox的位姿齐次变换矩阵；

为Arm-k的广义质量矩阵；

为Arm-k的非线性哥氏力和离心力矢量；

分别为Arm-k的重力项；

分别为Arm-k的雅克比；

分别为Arm-k的关节驱动力矩。

进行双臂协调操作时负载的受力平衡分析，得到操作力约束方程如下：

上式可简写为：其中F_L表示负载所受环境的广义力；表示负载所受地球的广义重力；F_IL表示负载的广义惯性力，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵。

根据负载受力平衡方程进行负载操作的公共力分解，然而6个负载受力平衡方程包含12个未知数，无法独立求解，故采用主从模式或共享模式两种模式对其进行分解。

1)主从分解模式

定义其中一个机械臂为主臂(记为Arm-a)，产生主动运动；另一个机械臂为从臂(记为Arm-b)，跟随主臂做被动运动。

给定主臂的期望末端操作力/力矩：F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T；

可以求解得到从臂的末端期望力/力矩：

其进一步可给出如下定义。

主从式力柔顺控制策略：根据操作任务需求或机械臂负载能力，定义双臂机器人系统中的主臂和从臂；给定主臂的期望运动轨迹和末端操作力，通过双臂的闭链约束方程和负载力平衡方程计算得到从臂的期望运动轨迹和末端操作力；最后通过独立的单臂柔顺控制器完成双臂协同操作任务。上述控制流程即定义为双臂协同主从式力柔顺控制。

2)共享分解模式

共享模式将两个机械臂同等看待，采用伪逆法求解负载受力平衡方程，可以得到：

其中，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵，F_ea、F_eb分别为两机械臂的末端期望操作力/力矩，F_L表示负载所受环境的广义力；G_L表示负载所受地球的广义重力；F_IL表示负载的广义惯性力。

进一步给出如下定义：

共享式力柔顺控制策略：直接对双臂的闭链约束方程和负载力平衡方程进行分解，得到两个机械臂的末端期望运动轨迹和期望操作力，达到双臂综合最优的效果；通过独立的单臂柔顺控制器完成双臂协同操作任务。上述控制流程即定义为双臂协同共享式力柔顺控制。

进一步，单臂控制模块采用基于位置的阻抗控制，硬件上只需要机械臂末端操作力的检测以及关节空间的位置控制；通过机械臂的导纳特性将末端的力、力矩误差等效为输入的位置与姿态增量，即可实现机械臂的力柔顺操作。

机械臂的导纳特性的数学模型可以表示为：

其中：M为机械臂的惯性系数；B为机械臂的阻尼系数；K为机械臂的刚度系数；x、x_e分别表示机械臂末端的实际位姿向量和期望位姿向量；F为机械臂末端操作力偏差；

对上式进行拉普拉斯变换，当已知操作力误差时，可以求得与末端操作力偏差对应的位姿偏差如下：

其中双臂协同控制器由双臂之间的协调位置分解与公共力分解组成。考虑双臂对目标负载操作时所形成的闭链约束，可以得到如下双臂协调运动学方程以及各个机械臂的动力学方程如下：

其中，表示坐标系Oy相对于坐标系Ox的位姿齐次变换矩阵；

为Arm-k的广义质量矩阵；

为Arm-k的非线性哥氏力和离心力矢量；

分别为Arm-k的重力项；

分别为Arm-k的雅克比；

分别为Arm-k的关节驱动力矩。

进行双臂协调操作时负载的受力平衡分析，得到操作力约束方程如下：

上式简写为：其中F_L：负载所受环境的广义力；负载所受地球的广义重力；F_IL：负载的广义惯性力，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵。

根据负载受力平衡方程进行负载操作的公共力分解，然而6个负载受力平衡方程包含12个未知数，无法独立求解，故采用主从模式与共享模式两种模式对其进行分解。

1)主从分解模式

定义其中一个机械臂为主臂(记为Arm-a)，产生主动运动；另一个机械臂为从臂(记为Arm-b)，跟随主臂做被动运动。

给定主臂的期望末端操作力/力矩：F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T；

可以求解得到从臂的末端期望力/力矩：

进一步给出如下定义：

主从式力柔顺控制策略：根据操作任务需求或机械臂负载能力，定义双臂机器人系统中的主臂和从臂；给定主臂的期望运动轨迹和末端操作力，通过双臂的闭链约束方程和负载力平衡方程计算得到从臂的期望运动轨迹和末端操作力；最后通过独立的单臂柔顺控制器完成双臂协同操作任务。上述控制流程即定义为双臂协同主从式力柔顺控制。

2)共享分解模式

共享模式将两个机械臂同等看待，采用伪逆法求解负载受力平衡方程，可以得到：

进一步给出如下定义：

共享式力柔顺控制策略：直接对双臂的闭链约束方程和负载力平衡方程进行分解，得到两个机械臂的末端期望运动轨迹和期望操作力，达到双臂综合最优的效果；通过独立的单臂柔顺控制器完成双臂协同操作任务。上述控制流程即定义为双臂协同共享式力柔顺控制。

单臂控制器采用基于位置的阻抗控制，硬件上只需要机械臂末端操作力的检测以及关节空间的位置控制；通过机械臂的导纳特性将末端的力、力矩误差等效为输入的位置与姿态增量，即可实现机械臂的力柔顺操作。

(1)双臂动力学建模以及负载力平衡分析

参照图1，双臂空间机器人系统的一般运动学模型由基座、n_a自由度的串联机械臂(记为Arm-a)和n_b自由度的串联机械臂(记为Arm-b)组成。其中，各个变量定义如下：

B₀：为作为空间机器人基座的航天器平台；

B_L：为双臂协同操作的目标负载；

分别表示Arm-a和Arm-b的第i个连杆；

为Arm-a和Arm-b的第i个关节；

为Arm-a和Arm-b的第i个关节的关节角度；

当进行双臂协调操作时，双臂机器人与目标操作物形成闭链约束，因此已知目标操作物的期望位姿轨迹^BT_L，可以根据闭链约束方程分别得到两个机械臂末端在各自参考坐标系下的表示：

其中，表示机械臂k的末端工具坐标系相对于基座坐标系的齐次变换矩阵；表示机械臂k的基座坐标系相对于双臂机器人本体坐标系下的齐次变换矩阵；表示目标负载的本体坐标系相对于机械臂k末端工具坐标系的齐次变换矩阵；fkine表示机械臂的正运动学方程；

对任意给定任务，根据式(1)-(4)，通过逆运动学方程便可以得到双臂机器人系统协调操作时各个关节的控制输入。

假设双臂机器人基座平台与双臂不产生同时运动，双臂协调操作时可以将双臂机器人系统中的各个机械臂近似为基座固定的机械臂时，得到各个机械臂的动力学方程如下：

其中：M_k(k＝a,b)为对称正定的惯量矩阵，C_k为科氏力/离心力项，G_k为动力学方程中的重力项；τ_k为关节的控制力矩，为机械臂操作时所受到的外力，本发明中特指双臂与负载形成闭链时机械臂末端受到的接触力，分别为两机械臂的雅克比矩阵。

参照图2，为双臂同时抓持目标负载时的受力情况。对双臂协调操作时负载的受力平衡进行分析，得到操作力/力矩约束方程。

其中：

f_L,τ_L为环境施加给目标载荷的外力、外力矩；

-f_ea,-τ_ea为Arm-a末端施加给载荷的力、力矩(与载荷施加给Arm-a末端的力f_ea、力矩τ_ea互为作用力及反作用力)；

-f_eb,-τ_eb为Arm-b末端施加给载荷的力、力矩(与载荷施加给Arm-b末端的力f_eb、力矩τ_eb互为作用力及反作用力)；

r_L、r_a、r_b分别为力f_L、f_ea、f_eb作用点到载荷质心的矢量

v_L,ω_L为负载质心坐标系的线速度和角速度；

M_L为负载的质量；

I_L为负载质心转动惯量；

G_L＝M_Lg为负载所受的重力；

最后可得到：

双臂协调力柔顺控制的关键是如何将common点的广义力、力矩等效到两个臂的末端。将式(9)简写为：

式(10)中：

F_L：负载所受环境的广义力；

负载所受地球的广义重力；

F_IL：负载的广义惯性力。

(2)负载操作的公共力分解方法

对于给定的任务，F_L、F_IL可求(作为已知项)。然而，式(10)包含了12个未知数(F_ea、F_eb分别为6个)，且仅有6个方程，无法独立求解。为了求解欠定的负载力平衡方程(10)，分别采用主从式和共享式策略进行负载的公共力分解，得到两个机械臂的期望操作力，进而提出了双臂协同操作的主从式力柔顺控制方法和共享式力柔顺控制方法。

1)主从分解模式

主从分解模式定义其中一个机械臂为主臂(记为Arm-a)，产生主动运动；另一个机械臂为从臂(记为Arm-b)，跟随主臂做被动运动。

给定主臂的期望末端操作力/力矩：F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T；

可以求解得到从臂的末端期望力/力矩：

给出如下定义：

主从式力柔顺控制策略：根据操作任务需求或机械臂负载能力，定义双臂机器人系统中的主臂和从臂；给定主臂的期望运动轨迹和末端操作力，通过双臂的闭链约束方程和负载力平衡方程计算得到从臂的期望运动轨迹和末端操作力；最后通过独立的单臂柔顺控制器完成双臂协同操作任务。上述控制流程即定义为双臂协调主从式力柔顺控制。

2)共享分解模式

共享模式将两个机械臂同等看待，采用伪逆法求解负载受力平衡方程，可以得到：

进一步给出如下定义：

共享式力柔顺控制策略：直接对双臂的闭链约束方程和负载力平衡方程进行分解，得到两个机械臂的末端期望运动轨迹和期望操作力，达到双臂综合最优的效果；通过独立的单臂柔顺控制器完成双臂协同操作任务。上述控制流程即定义为双臂协同共享式力柔顺控制。

主从式双臂协同力柔顺控制策略优先确定出主臂的末端操作力，因此主臂具有较高的优先级；而从臂则可以看作为满足双臂协同操作任务的辅助机械臂，其末端操作力由负载的受力平衡方程计算得出。另外，对于双臂异构或双臂负载能力不一致的协调操作情况，主从式力柔顺控制可以体现出更强的优越性，通过分别设置双臂不同的操作力控制目标，实现对载荷的协同操作。

共享式双臂协同力柔顺控制策略直接对负载受力平衡方程进行分解，同时考虑两个机械臂末端的操作力大小，实时控制闭链状态下负载内力在期望的范围内；控制中以实现负载的期望运动为最高优先级，双臂在满足闭链约束方程的条件下进行协同控制，尤其适用于对负载运动轨迹精度要求较高的场合。

(3)单臂基于位置的阻抗控制方法

当双臂末端的期望位置与期望操作力确定后，可对各个机械臂进行力柔顺控制操作。

参照图3，单臂控制器采用基于位置的阻抗控制，基于位置的阻抗控制通过机械臂的导纳特性将末端的力、力矩误差等效为输入的位置与姿态增量，对机械臂进行闭环控制。硬件上只需要机械臂末端操作力的检测以及关节空间的位置控制；通过机械臂的导纳特性将末端的力、力矩误差等效为输入的位置与姿态增量，即可实现机械臂的力柔顺操作。

通过给定与实际系统相对应的质量M、阻尼B与刚度K，便可以在自由或约束空间进行柔顺控制。其数学模型可以用下面的微分方程来描述：

如果将其应用到双臂机器人的从臂上，当检测到末端力误差F时，等效到输入端位置信号为Δx，进行拉普拉斯变换可以得到：

通过上式得到机械臂末端的位姿增量，与双臂协调位置分解得到的机械臂期望位姿进行叠加，从而实现单个机械臂的力柔顺控制操作。

(4)双臂主从柔顺控制方法

参照图4，基于上述双臂协调位置分解以及公共力分解策略，本发明提出一种目标负载操作的闭链双臂协调柔顺控制方法及系统。主要由双臂协同控制器、单臂控制器、底层硬件实现以及传感器信息融合与时延后处理等模块组成，其中：

双臂协同控制器首先由任务规划得到负载的期望运动轨迹和期望力作为输入，进一步通过满足双臂闭链约束的协调位置分解方程和公共力分解方程得到两个机械臂的末端期望位姿输入和末端期望力输入；其中公共力分解可以采用主从分解和共享分解两种模式。

单臂控制器完成对任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；首先双臂协同控制器得到的各个机械臂的期望位姿和期望力作为单臂控制的输入；计算期望操作力与传感器反馈的末端操作力的偏差，通过阻抗控制将操作力偏差等效为机械臂末端位姿的修正量；机械臂末端期望位姿与修正量叠加，通过逆运动学方程求解得到对应的各个关节期望角度，控制各个关节运动，进而实现机械臂的柔顺操作。

底层硬件根据规划的期望关节角度，驱动机械臂完成任务轨迹；传感器信息融合与时延后处理模块采集双臂机器人系统各个传感器的信息(包括机械臂各个关节的位置和速度信息以及机械臂末端六维力传感器的操作力信息)，对信号进行滤波和时延处理，最后同时反馈给上位机控制软件。

一种基于双臂机器人协同操作的柔顺控制系统，包括：双臂协同控制模块、驱动模块和单臂控制模块；

所述双臂协同控制模块用于执行步骤根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

所述单臂控制模块用于执行步骤完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

所述驱动模块用于执行步骤根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。

参照图5，是本发明第五实施例的控制方法流程示意图。一种基于双臂机器人协同操作的柔顺控制方法，包括以下步骤：

双臂协同控制模块根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

单臂控制模块完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

驱动模块根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。

本发明中公开的目标负载操作的闭链双臂协调柔顺控制方法，可以确保双臂机器人在对负载进行闭链约束下的协同操作的同时，将机械臂的末端操作力控制在期望的范围。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (8)

1.一种基于双臂机器人协同操作的柔顺控制方法，其特征在于，包括以下步骤：

双臂协同控制模块根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

单臂控制模块完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

驱动模块根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。

2.根据权利要求1所述的基于双臂机器人协同操作的柔顺控制方法，其特征在于：所述步骤还包括系统对双臂之间公共力的分解。

3.根据权利要求2所述的基于双臂机器人协同操作的柔顺控制方法，其特征在于：所述对公共力的分解的方法包括采用主从分解模式。

4.根据权利要求3所述的基于双臂机器人协同操作的柔顺控制方法，其特征在于：所述主从分解模式包括通过给定其中一机械臂的末端期望操作力/力矩F_ea＝[f_ex,f_ey,f_ez,τ_ex,τ_ey,τ_ez]^T，进而可求解得到另一机械臂的末端期望力/力矩为其中，F_L表示负载所受环境的广义力；表示负载所受地球的广义重力，F_IL表示负载的广义惯性力，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵。

5.根据权利要求2所述的基于双臂机器人协同操作的柔顺控制方法，其特征在于：所述对公共力的分解的方法包括采用共享分解模式。

6.根据权利要求5所述的基于双臂机器人协同操作的柔顺控制方法，其特征在于，所述共享分解模式包括将两个机械臂同等看待，采用伪逆法求解负载受力平衡方程，可得到：

$\[\begin{array}{cc}-{\mathrm{\Γ}}_a& -{\mathrm{\Γ}}_b\end{array}\]\left[\begin{array}{c}{F}_{ea}\\ F_{eb}\end{array}\right]=-{\mathrm{\Γ}}_L{F}_L-{\stackrel{\‾}{G}}_L+F_{IL};$

$\left[\begin{array}{c}{F}_{ea}\\ F_{eb}\end{array}\right]={\left[\begin{array}{cc}-{\mathrm{\Γ}}_a& -{\mathrm{\Γ}}_b\end{array}\right]}^{\#}(-{\mathrm{\Γ}}_L{F}_L-{\stackrel{\‾}{G}}_L+F_{IL});$

其中，Γ_a、Γ_b分别代表两个机械臂的抓取矩阵，F_ea、F_eb分别为两机械臂的末端期望操作力/力矩，F_L表示负载所受环境的广义力；表示负载所受地球的广义重力；F_IL表示负载的广义惯性力。

7.根据权利要求1-6任一项所述的基于双臂机器人协同操作的柔顺控制方法，其特征在于：所述单臂控制模块采用基于位置的阻抗控制，通过机械臂的导纳特性将末端的力、力矩误差等效为输入的位置与姿态增量，实现机械臂的力柔顺操作。

8.一种基于双臂机器人协同操作的柔顺控制系统，其特征在于，包括：双臂协同控制模块、驱动模块和单臂控制模块；

所述双臂协同控制模块用于执行步骤根据目标负载的期望运动轨迹和期望力建立动力学模型，通过满足双臂闭链约束方程的协调位置分解和公共力分解得到各个机械臂的末端期望位姿和末端期望力；

所述单臂控制模块用于执行步骤完成对所述期望任务指令的解析与执行，得到机械臂各个关节的期望角度，通过基于位置的阻抗控制实现机械臂的力柔顺操作；

所述驱动模块用于执行步骤根据规划的期望关节角度，驱动所述机械臂完成任务轨迹。