CN108621163A - 一种面向汇榫工艺的冗余双臂机器人协作控制方法 - Google Patents

Abstract

本发明提供了一种面向汇榫工艺的冗余双臂机器人协作控制方法，包括以下步骤：双臂协调控制系统根据松协调任务的特点建立了完整的协调约束关系，通过分析了双臂协调系统的动力学特性，建立了双臂协调系统的动力学模型。单臂控制子系统针对机械臂冗余特性，引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，设计了松协调任务优先级控制方法，在单臂闭环控制的基础上对双臂之间的约束进行控制。该方法根据汇榫任务的特点建立完整的协调约束关系，确保机械臂按照期望的轨迹运动且实现对双臂间相对误差的控制，通过对零空间中的运动进行规划，实现对冗余关节阻抗的控制和避关节运动极限。

Description

一种面向汇榫工艺的冗余双臂机器人协作控制方法

技术领域

本发明涉及机器人技术的双臂协作控制领域，尤其涉及一种面向汇榫工艺的冗余双臂机器人协作控制方法。

背景技术

与单个单臂机器人以及两个单臂机器人所构成的控制系统相比，冗余双臂机器人在面对工业现场等复杂的场景中，具有适用范围广、协作能力强、可靠性高等独特的优势。作为冗余双臂机器人系统的关键技术，冗余双臂机器人协调控制方法已成为研究热点之一。

木工艺中的汇榫工艺即试装，把做好的榫汇入卯中，看两构件垂直度等来决定榫卯的修整程度，使对应的榫卯松紧度合适，对应构件结合紧密。

对于冗余双臂机器人的协调控制，目前大多数采用位置和力混合控制方法或阻抗控制方法，专利[CN106695797A]公开了一种基于双臂机器人协同操作的柔顺控制方法，采用主从式和共享式策略进行负载的公共力分解，得到两个机械臂的期望操作力，进而提出了双臂协同操作的主从式力柔顺控制方法和共享式力柔顺控制方法，其中单臂控制模块采用基于位置的阻抗控制，实现机械臂的力柔顺操作。

发明内容

目前，本领域一般的阻抗控制方法，对双臂机器人强耦合的特点考虑较少，对操作任务分解后转换为独立的单机械臂控制问题，没有对双臂间的相对误差进行控制，不利于精细化操作；并且存在位置和力混合控制方法存在不稳定及易受外界干扰等问题。

为解决上述方法不足，本发明提供了一种面向汇榫工艺的双臂机器人协作控制方法，包括以下步骤：

双臂协调控制系统根据松协调任务的特点建立了完整的协调约束关系，通过分析了双臂协调系统的动力学特性，建立了双臂协调系统的动力学模型。

单臂控制子系统针对机械臂冗余特性，引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，设计了松协调任务优先级控制方法，在单臂闭环控制的基础上对双臂之间的约束进行控制。

驱动模块根据关节空间轨迹控制机械臂完成汇榫任务。

作为该技术方案的改进，所述步骤还包括双臂协调系统的动力学分析。

作为该技术方案的改进，所述双臂协调系统的动力学分析包括采用位姿变换矩阵进行双臂的正运动求解。

作为该技术方案的改进，所述双臂协调系统的动力学分析包括进行逆运动学的求解。

进一步地，所述的逆运动学的求解包括采用改进的雅克比伪逆法进行冗余机械臂的运动学逆解，可得到冗余机器臂的运动学逆解为：

式中，为方程的齐次解，α为任意的n*1阶向量，I为n*n的单位矩阵，J⁺为冗余机械臂的雅克比逆矩阵

进一步地，所述的逆运动学求解包括将操作空间中的期望位置和速度作为闭环反馈量，在改进的雅克比伪逆法基础上，进行冗余机械臂的运动学逆解，可得到冗余机器臂的运动学逆解为：

式中，x_d,分别为机器臂期望的位置和速度，α为任意的n*1阶向量，N(N^TN)^-1N^T为α在零空间上的投影，K_x为位置增益，J⁺为冗余机械臂的雅克比逆矩阵

进一步地，所述的逆运动学求解包括采用梯度投影法进行优化，可得

式中，k为常量系数，H(q)为避关节运动极限性能指标函数，▽H(q)为H(q)的梯度向量

进一步地，所述的双臂协调系统的动力学模型包括基于拉格朗日方程建立的动力学方程，可得到

其中，为机械臂的离心科式力矩阵，G_i(q_i)为机械臂的重力矢量，F_i为机械臂与环境的作用力，u_i为机械臂关节驱动力矩，M_i(q_i)为机械臂惯性矩阵，分别为机械臂关节空间角度，速度和加速度向量。

作为该技术方案的改进，所述的松协调任务优先级控制方法包括主任务是确保机械臂按照期望的轨迹运动且实现对双臂间相对误差的控制，子任务是对增广空间中零空间的运动进行规划，实现对冗余关节阻抗的控制和避关节运动极限。

作为该技术方案的改进，所述的松协调主任务控制方法包括引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，可得

式中，M_d为左(右)机械臂笛卡尔操作空间惯性矩，分别为双臂之间相对位置误差，相对速度误差和相对加速度误差，B_d ^*，K_d ^*为左(右)机械臂相对右(左)机械臂的阻尼刚度系数，F为机械臂与环境的作用力

作为该技术方案的改进，所述的松协调子任务控制方法包括将机械臂操作空间和零空间组合在一起，构成增广操作空间，并对冗余机械臂的驱动力矩进行解耦，可得零空间的关节驱动力矩

其中，J_N为零空间的雅克比矩阵，为增广空间的速度误差分量， 为关节空间速度。

本发明提供的面向大型木结构工艺的双臂机器人协作控制方法，针对大型木结构工艺中汇榫等典型任务，提出了冗余双臂机器人松协调系统任务优先级控制方法，可以确保机械臂按照期望的轨迹运动且实现对双臂间相对误差的控制，同时通过对零空间中的运动进行规划，实现对冗余关节阻抗的控制和避关节运动极限。本发明为双臂机器人的协作控制提供了一种有效的控制方案，确保机械臂按照期望的轨迹运动且实现对双臂间相对误差的控制，同时通过对零空间中的运动进行规划，实现对冗余关节阻抗的控制和避关节运动极限。本方案广泛适用于冗余双臂机器人产品，用以完成大型木结构工艺中汇榫等协作控制任务。由于在改进的雅克比伪逆法基础上，将操作空间中的期望位置和速度作为闭环反馈量，建立了基于闭环的逆运动学求解方法，可以消除机器人控制的累计误差和避免关节角超出运动极限的问题。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例的冗余双臂机器人基于闭环的运动学逆解求解方法示意图。

图2是本发明实施例的单臂控制子系统控制方法框图。

图3是本发明实施例的冗余双臂机器人控制方法流程图。

具体实施方式

本发明提供提供一种面向汇榫工艺的双臂机器人协作控制方法，包括以下步骤：

双臂协调控制系统根据松协调任务的特点建立了完整的协调约束关系，通过分析了双臂协调系统的动力学特性，建立了双臂协调系统的动力学模型。

单臂控制子系统针对机械臂冗余特性，引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，设计了松协调任务优先级控制方法，在单臂闭环控制的基础上对双臂之间的约束进行控制。

驱动模块根据关节空间轨迹控制机械臂完成汇榫任务。

作为该技术方案的改进，所述步骤还包括双臂协调系统的动力学分析。

作为该技术方案的改进，所述双臂协调系统的动力学分析包括采用位姿变换矩阵进行双臂的正运动求解。

作为该技术方案的改进，所述双臂协调系统的动力学分析包括进行逆运动学的求解。

进一步地，所述的逆运动学的求解包括采用改进的雅克比伪逆法进行冗余机械臂的运动学逆解，可得到冗余机器臂的运动学逆解为：

式中，为方程的齐次解，α为任意的n*1阶向量，I为n*n的单位矩阵，J⁺为冗余机械臂的雅克比逆矩阵

进一步地，所述的逆运动学求解包括将操作空间中的期望位置和速度作为闭环反馈量，在改进的雅克比伪逆法基础上，进行冗余机械臂的运动学逆解，可得到冗余机器臂的运动学逆解为：

式中，x_d,分别为机器臂期望的位置和速度，α为任意的n*1阶向量，N(N^TN)^-1N^T为α在零空间上的投影，K_x为位置增益，J⁺为冗余机械臂的雅克比逆矩阵

进一步地，所述的逆运动学求解包括采用梯度投影法进行优化，可得

式中，k为常量系数，H(q)为避关节运动极限性能指标函数，▽H(q)为H(q)的梯度向量

进一步地，所述的双臂协调系统的动力学模型包括基于拉格朗日方程建立的动力学方程，可得到

其中，为机械臂的离心科式力矩阵，G_i(q_i)为机械臂的重力矢量，F_i为机械臂与环境的作用力，u_i为机械臂关节驱动力矩，M_i(q_i)为机械臂惯性矩阵，分别为机械臂关节空间角度，速度和加速度向量。

作为该技术方案的改进，所述的松协调任务优先级控制方法包括主任务是确保机械臂按照期望的轨迹运动且实现对双臂间相对误差的控制，子任务是对增广空间中零空间的运动进行规划，实现对冗余关节阻抗的控制和避关节运动极限。

作为该技术方案的改进，所述的松协调主任务控制方法包括引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，可得

式中，M_d为左(右)机械臂笛卡尔操作空间惯性矩，分别为双臂之间相对位置误差，相对速度误差和相对加速度误差，B_d ^*，K_d ^*为左(右)机械臂相对右(左)机械臂的阻尼刚度系数，F为机械臂与环境的作用力

作为该技术方案的改进，所述的松协调子任务控制方法包括将机械臂操作空间和零空间组合在一起，构成增广操作空间，并对冗余机械臂的驱动力矩进行解耦，可得零空间的关节驱动力矩

其中，J_N为零空间的雅克比矩阵，为增广空间的速度误差分量，为关节空间速度。

(1)冗余双臂机器人系统运动学及动力学分析

利用双臂机器人机械臂相邻关节坐标系i-1和i之间的位姿变换矩阵可得右机械臂末端坐标系相当于基坐标系的位姿变换矩阵，即右机械臂正运动关系可表示为

其中q^r为右机械臂关节角向量，n为机械臂自由度

同理，左机械臂正运动关系可以表示为

其中q^l为左机械臂关节角向量，n为机械臂自由度

设冗余机械臂在操作空间的运动轨迹为所对应的机械臂关节空间运动轨迹为则根据机械臂操作空间速度与关节空间速度关系及机械臂冗余特性，可得到基于通用的雅克比的冗余机械臂的运动学逆解为：

式中为方程的齐次解，α为任意的n*1阶向量，I为n*n的单位矩阵，J⁺为冗余机械臂的雅克比逆矩阵

上式冗余机械臂逆运动学逆解虽然考虑到机械臂自运动，但忽略了累计误差，所以在通用的雅克比伪逆法基础上，建立基于闭环的逆运动学求解方法，参照图1。将操作空间的期望位置和速度作为闭环反馈量消除累计误差，可得到基于闭环的冗余机器臂的运动学逆解为：

式中x_d,分别为机器臂期望的位置和速度，α为任意的7*1阶向量，N(N^TN)^-1N^T为α在零空间上的投影，K_x为位置增益，J⁺为冗余机械臂的雅克比逆矩阵。

双臂协调系统的动力学模型包括基于拉格朗日方程建立的动力学方程，可得到

其中，为机械臂的离心科式力矩阵，G_i(q_i)为机械臂的重力矢量，F_i为机械臂与环境的作用力，u_i为机械臂关节驱动力矩，M_i(q_i)为机械臂惯性矩阵，分别为机械臂关节空间角度，速度和加速度向量。

(2)汇榫工艺特点及约束关系分析

松协调任务指的是在共享的工作空间内，左右机械臂执行各自任务，但各机械臂的运动受到一定约束，即左右机械臂需在特定时间以特定姿态到达特定位置。汇榫工艺即试装，把做好的榫汇入卯中，看两构件垂直度等来决定榫卯的修整程度，是典型的松协调任务，即可看做右机械臂夹持操作物A(卯)运动，左机械臂夹持操作物B(榫)在操作物A(卯)上操作，由于该系统形成一个封闭运动链，可得双臂之间的完整位姿约束为

式中，为双臂末端坐标系在基坐标系的旋转矩阵，为左机械臂相当于右机械臂的旋转矩阵。

(3)基于空间弹簧阻尼并联模型的松协调主任务控制方法

给出以下定义：

机械臂轨迹跟踪误差：在基坐标系中机械臂期望轨迹和实际运动轨迹之间的误差。

双臂间的相对误差：由右(左)机械臂实际轨迹根据双臂约束关系得到的左(右)机械臂期望轨迹与左(右)机械臂实际运动轨迹间的误差。

为了同时控制机械臂跟踪误差和双臂间的相对误差，引入松协调操作空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，可得

式中，M_d为左(右)机械臂笛卡尔操作空间惯性矩，分别为双臂之间相对位置误差，相对速度误差和相对加速度误差，B_d ^*，K_d ^*为左(右)机械臂相对右(左)机械臂的阻尼刚度系数，F为机械臂与环境的作用力。

由于冗余双臂机器人的冗余特性，存在非空的零空间，机械臂在零空间中进行自运动。对自运动进行规划，可以克服奇异性，避关节角极限和躲避障碍物等。

将机械臂操作空间和零空间组合在一起，构成增广操作空间，并对冗余机械臂的驱动力矩进行解耦，可得零空间的关节驱动力矩

其中，J_N为零空间的雅克比矩阵，为增广空间的速度误差分量，为关节空间速度。

进一步给出如下定义：

任务优先级控制方法：主任务确保机械臂按照期望的轨迹运动且实现对双臂间相对误差的控制，子任务是对零空间中的运动进行规划，实现对冗余关节阻抗的控制和避关节角极限。

参照图2，是单臂控制子系统控制方法框图，如对于右机械臂而言，其期望运动轨迹和实际轨迹之差为轨迹跟踪误差，根据双臂约束关系由左机械臂得到右机械臂的期望轨迹，其与右机械臂实际轨迹之差为相对误差，控制器根据这两类误差值产生力矩驱动右机械臂，同理可得左机械臂的工作原理。

参照图3，是冗余双臂机器人控制方法流程图，一种面向汇榫工艺的双臂机器人协作控制方法，包括以下步骤：

双臂协调控制系统根据松协调任务的特点建立了完整的协调约束关系，通过分析了双臂协调系统的动力学特性，建立了双臂协调系统的动力学模型。

单臂控制子系统针对机械臂冗余特性，引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，设计了松协调任务优先级控制方法，在单臂闭环控制的基础上对双臂之间的约束进行控制。

驱动模块根据关节空间轨迹控制机械臂完成汇榫任务。

以上显示和描述了本发明的基本原理、主要特征及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (9)

1.一种面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，包括以下步骤：

双臂协调控制系统根据汇榫工艺的特点建立了完整的协调约束关系，通过分析双臂协调系统的动力学特性，建立双臂协调系统的动力学模型；

单臂控制子系统针对机械臂冗余特性，引入空间弹簧阻尼并联模型，建立关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，设计松协调任务优先级控制方法，在单臂闭环控制的基础上对双臂之间的约束进行控制。

2.根据权利要求1所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述步骤还包括双臂协调系统的动力学分析。

3.根据权利要求2所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述双臂协调系统的动力学分析包括采用位姿变换矩阵进行双臂的正运动求解。

4.根据权利要求2所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述双臂协调系统的动力学分析包括进行逆运动学的求解。

5.根据权利要求4所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述的逆运动学的求解包括采用改进的雅克比伪逆法进行冗余机械臂的运动学逆解，可得到冗余机器臂的运动学逆解为：

式中，为方程的齐次解，α为任意的n*1阶向量，I为n*n的单位矩阵，J⁺为冗余机械臂的雅克比逆矩阵。

6.根据权利要求4所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述的逆运动学的求解包括将操作空间中的期望位置和速度作为闭环反馈量，在改进的雅克比伪逆法基础上，进行冗余机械臂的运动学逆解，可得到冗余机器臂的运动学逆解为：

式中，x_d,分别为机器臂期望的位置和速度，α为任意的n*1阶向量，N(N^TN)^-1N^T为α在零空间上的投影，K_x为位置增益，J⁺为冗余机械臂的雅克比逆矩阵。

对上述的逆运动学求解包括采用梯度投影法进行优化，可得

式中，k为常量系数，H(q)为避关节运动极限性能指标函数，▽H(q)为H(q)的梯度向量。

7.根据权利要求1-6任一项所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述的单臂控制子系统针对机械臂冗余特性，引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，设计了松协调任务优先级控制方法，在单臂闭环控制的基础上对双臂之间的约束进行控制。

8.根据权利要求7所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述的松协调主任务控制方法包括引入了空间弹簧阻尼并联模型，建立了关于机械臂轨迹跟踪误差和双臂相对误差的阻抗关系式，可得

式中，M_d为左(右)机械臂笛卡尔操作空间惯性矩，分别为双臂之间相对位置误差，相对速度误差和相对加速度误差，B_d ^*，K_d ^*为左(右)机械臂相对右(左)机械臂的阻尼刚度系数，F为机械臂与环境的作用力。

9.根据权利要求8所述的面向汇榫工艺的冗余双臂机器人协作控制方法，其特征在于，所述的松协调子任务控制方法包括将机械臂操作空间和零空间组合在一起，构成增广操作空间，并对冗余机械臂的驱动力矩进行解耦，可得零空间的关节驱动力矩

其中，J_N为零空间的雅克比矩阵，为增广空间的速度误差分量， 为关节空间速度。