CN110695994A - 一种面向双臂机械手协同重复运动的有限时间规划方法 - Google Patents

Abstract

一种面向双臂机械手协同重复运动的有限时间规划方法，包括以下步骤：1)确定双臂机械手的左右末端执行器的期望目标轨迹，以及双臂机械手各关节的角期望回拢角度；2)设计二次优化指标，形成双臂机械手协同重复运动规划方案，其中可以任意指定双臂机械手实际运动时的初始关节角；3)引入初始修正因子，构建有限时间收敛的终态网络模型，并用该网络方程求解时变问题；4)将计算得到的结果传送给控制器来控制双臂机械手的协同重复运动。本发明精度较高、指定时间收敛，当左右臂的各关节角初始位置偏移期望位置时，双臂机械手各运动关节角在完成轨迹任务后，可以保证在指定的有限时间内回到初始期望位置，实现重复运动任务。

Description

一种面向双臂机械手协同重复运动的有限时间规划方法

技术领域

本发明属于双臂多连杆机械手的协同重复运动规划及控制技术，具体涉及一种在有限时间内收敛的逆运动学求解方法。当双臂机械手左右各运动关节角初始位置偏移期望轨迹时，通过引入初始修正因子，构建一个基于有限时间收敛的终态网络，最终在指定时间内完成轨迹任务。

背景技术

冗余机械手是一种自由度大于任务空间所需最少自由度的末端能动机械装置，它可以应用于组装、加工、焊接等工业当中。当机械手的末端执行器在笛卡尔操作空间做重复运动时，闭合的末端执行器的运动轨迹可能没有产生闭合的关节角轨迹，导致关节角出现偏差。这种非重复运动问题所带来的偏差可能会引起机械手在重复作业中出现不可预料的情况。为解决这类问题，曾经广泛应用伪逆方法，但是由于其公式计算过程较为复杂，求解过程中存在奇异点，且机械手物理关节角有极限，因而无法应用于求解周期性的重复运动，从而难以预测双臂机械手的运动轨迹。

假设具有n个自由度的双臂机械手在m维空间中工作，研究其末端轨迹与各运动关节位移之间的映射关系称为正运动学问题，具体描述为：

r_L(t)＝f_L(θ_L(t))

r_R(t)＝f_R(θ_R(t))

其中，r_L(t)和r_R(t)分别表示的是在笛卡尔坐标系下的左右机械手末端执行器的位移。θ_L(t)和θ_R(t)分别表示的是左右机械手的各关节角。末端笛卡尔空间与关节空间之间的微分运动关系为:

其中，

和

分别是r_L(t)和r_R(t)关于时间的导数，

和

分别是左右机械手的关节速度向量，

和

分别为机械手左右双臂对应的雅克比矩阵。

冗余双臂机械手实时运动控制的一个基本问题是冗余度解析问题，又被称作逆运动学或运动规划。应用最为广泛的伪逆控制法被广泛应用于机械手的运动控制中，但无法使得机械手完成重复运动任务。为了提高执行的效率，Y.Zhang,引入重复运动指标作为优化准则。形成重复运动规划(Repetitive motion planning, RMP)方案,使用二次规划(QP)和递归神经网络(RNN)来进行机械臂关节角冗余度解析。通常的神经网络求解器具有渐近收敛性能，在计算时间足够长以后，能够获得有效解，且能应用于实时在线求解各类时变问题。

发明内容

为了使得双臂机械手能够在有限的时间内左右两臂协同完成一项共同的任务，实现重复运动，本发明提供一种精度较高，易于实现的有限时间冗余双臂机械手轨迹规划方法，以具有有限时间收敛特性的终态神经网络求解，在初始位置偏移情形下，冗余机械臂各关节角仍然可以回到初始期望位置。

为解决上述技术问题，本发明通过以下技术方案予以实现：

一种面向双臂机械手协同重复运动规划的有限时间规划方法，包括以下步骤：

1)设双臂机械手的左右末端执行器的期望目标轨迹为

和

双臂机械手各关节的角期望回拢角度为和

2)通过设计终态吸引优化指标，形成双臂机械手协同重复运动规划方案，其中可以任意指定双臂机械手实际运动时的初始关节角，末端执行器可以不处于期望轨迹上；假设实际运动时左右两臂的关节角度为θ_L和θ_R,且分别以θ_L(0)和θ_R(0) 为左右机械手的运动起始点；

双臂机械手的协同重复运动解析方案转化为统一的二次规划问题：最小化受约束于其中， P(t)＝κ(Θ(t)-Θ(0))，κ＞0，Θ＝[θ_L；θ_R]，θ_L表示左边机械手的关节角度，θ_R表示右边机械手的关节角度，

表示双臂机械手的各运动关节角速度，

表示机械手左右末端执行器期望的运动轨迹，

表示左右末端执行器期望的速度向量；由于机械手的左右初始位置可能不在期望的轨迹上，分别通过减小左右臂末端执行器期望位置与实际运动轨迹位置间的误差 (Υ-f(Θ))，改变末端执行器的运动方向，β_Υ＞0表示位置的参数增益，用来调节左右双臂的末端执行器运动时的速率，J(Θ)为双臂机械手雅克比矩阵，f(Θ)是双臂机械手的实际运动轨迹；

3)构建有限时间收敛终态网络模型，其动态特性由下述方程描述

其中，关节角位移偏差E(t)＝Θ(t)-Θ(0)。η_E是一个非正定的设计参数,σ_E的值与η_E的取值有关，为一初始修正向量，使系统误差能在指定的有限时间内收敛到零；

ξ(t)满足以下条件：

a.当t∈(-∞,0]∪[Δ,+∞)时，ξ(t)＝0；

b.当t∈(0,Δ)时，

其中，t₀为起始时间，Δ为时间间隔，当二次规划问题获得最优解时，双臂机械手的各个关节角可以回拢到期望的目标轨迹上；

通过对拉格朗日函数二次优化问题中的各个变量求导，并令其为零，得如下时变矩阵方程，

W(t)Y(t)＝v(t)

其中，

I为单位矩阵

记时变矩阵方程误差E(t)＝W(t)Y(t)-v(t)。根据终态网络动态特性方程

构建神经网络模型，得到终态网络求解方程如下：

与现有技术相比，本发明有如下有益效果：

本发明实现了一种新型的有限时间收敛的协同重复运动规划方案求解方法，使双臂机械手不用考虑初始左右各关节角的位置，左右各关节角通过互相协同工作的方式，完成轨迹任务，最终可以回到期望的初始位置，实现重复运动规划任务。

附图说明

图1为本发明提供的协同重复规划方案求解的流程图。

图2为本发明中用于重复运动规划的双臂机械手。

图3为双臂机械手左右两个末端执行器的运动轨迹。

图4为以终态网络和递归神经网络求解时的误差轨迹。

图5为以终态网络求解时x,y,z轴上的误差轨迹。

图6为以终态网络求解时双臂机械手左右各关节角的运动轨迹。

具体实施方式

下面结合附图对本发明做进一步的描述。

参照图1～图6，一种面向基于二次规划的双臂机械手协同重复运动有限时间规划方法，包括以下步骤:1).确定双臂机械手左右末端执行器期望目标轨迹和期望回拢各关节角度；2).建立冗余双臂机械手重复运动二次规划方案；3). 构建具有有限时间收敛特性的终态网络，求解二次规划问题，从而获得左右各运动关节角轨迹；4).将计算得到的结果传送给下位机控制器驱动机械手运动。

所述步骤1)中，首先设定双臂机械手期望回拢的各关节角度，

将目标路径设定为一个六角形轨迹，确定六角形的期望轨迹方程

为：

其中，

其中，

分别表示右臂末端执行器在x,y,z轴方向上的期望轨迹，

分别表示左臂末端执行器在x,y,z轴方向上的期望轨迹。此外，机械手末端完成六角形轨迹的时间设定为T＝12s。

由于双臂机械手各关节角的初始位置可能不在期望的运动轨迹上，将该双臂机械手的七个关节角度初值分别设为

θ_R＝[0；π/4；π/4；π/4+0.1；π/4；0；0]

θ_L＝[0；π/4；π/4；π/4+0.1；π/4；0；0]

其中第4个关节角偏离期望位置0.1rad。

所述步骤2)中，双臂机械手的协同重复运动解析方案可以转化为以下二次规划问题：

其中，P(t)＝κ(Θ(t)-Θ(0))，且κ＞0，Θ＝[θ_L；θ_R]。

表示双臂机械手的各运动关节角速度，Θ^*(0)为机械手左右两臂各关节角的期望初始值。Θ(t)-Θ^*(0)表示双臂机械手各关节角初始位置与初始期望位置之间的位移偏差，

表示机械手左右末端执行器期望的运动轨迹，表示左右末端执行器期望的速度向量。考虑到机械手的左右初始位置可能不在期望的轨迹上，所以分别通过减小左右臂末端执行器期望位置与实际运动轨迹位置间的误差 (Υ-f(Θ))来改变末端执行器的运动方向。β_Υ＞0表示位置的参数增益，用来调节左右双臂的末端执行器运动时的速率。J(Θ)是双臂机械手的雅克比矩阵。

所述步骤3)中，建立拉格朗日函数，各式(1)中各个变量求导，并令其为零，可得如下时变矩阵方程，

W(t)Y(t)＝v(t)

其中，

I为单位矩阵

记时变矩阵方程误差E(t)＝W(t)Y(t)-v(t)。依据有限时间收敛终态网络动态方程(1)，构建网络模型，得到网络求解方程如下：

用于实现本发明协同重复规划方案的双臂机械手如图2所示。该机械手系统由2个固定基座组成，两个基座之间的距离为l＝1.5754m。左右两个机械手分别由3个连接杆L₁,L₂,L₃构成，且连杆长度分别为L₁＝0.45m，L₂＝0.5m， L₃＝0.08m，如图2(a)所示。图2(b)分别表示左边机械手的七个关节角

和右边机械手的七个关节角

图3展示了该机械手的末端执行器在空间中的运动轨迹，包括目标六角形轨迹和机械手末端执行器的运动轨迹。可以看出，即使左右末端执行器的初始位置不在期望的轨迹上，但随着时间的增加，实际轨迹会和期望轨迹吻合。

图4所示的是分别用有限值终态网络和Zhang递归神经网络求解二次规划问题的误差收敛轨迹，定义计算误差为J_E(t)＝||W(t)Y(t)-v(t)||₂。从图中可以看出，当用递归网络求解协同重复运动规划方案时，末端执行器的回拢速度慢于终态网络。以终态网络求解时，当时间接近0.1s时，误差收敛至零，当时间接近2s时，误差精度达到1.4×10^-3。

图5为双臂机械手左边末端执行器在执行轨迹任务时在XYZ三个方向上的误差，从图中可以看出，当t＝6s时，三个方向上的误差精度达到10^-2。

冗余机械手在完成轨迹任务时，各个关节角的运动轨迹如图6所示。从图中可以看到所有关节角的运动曲线光滑且连续，最终回到了运动的初始期望位置。

Claims (1)

1.一种面向双臂机械手协同重复运动的有限时间规划方法，其特征在于，所述方法包括以下步骤：

1)确定双臂机械手的左右末端执行器的期望目标轨迹

和

以及双臂机械手各关节的角期望回拢角度

和

2)通过设计二次优化指标，形成双臂机械手协同重复运动规划方案，其中可以任意指定双臂机械手实际运动时的初始关节角，末端执行器可以不处于期望轨迹上；记实际运动时左右两臂的关节角度为θ_L和θ_R,且分别以θ_L(0)和θ_R(0)为左右机械臂的运动起始点，双臂机械手的协同重复运动解析方案转化为统一的二次规划问题：

其中，P(t)＝κ(Θ(t)-Θ(0))，κ＞0，Θ＝[θ_L；θ_R]，θ_L表示左边机械臂的关节角度，θ_R表示右边机械臂的关节角度，

表示双臂机械手的各运动关节角速度，表示机械手左右末端执行器期望的运动轨迹，

表示左右末端执行器期望的速度向量；由于机械手的左右初始位置可能不在期望的轨迹上，分别通过减小左右臂末端执行器期望位置与实际运动轨迹位置间的误差(Υ-f(Θ))，改变末端执行器的运动方向，β_Υ＞0表示位置的参数增益，用来调节左右双臂的末端执行器运动时的速率，J(Θ)为双臂机械手雅可比矩阵，f(Θ)是双臂机械手的实际运动轨迹；

3)引入初始修正因子，构建有限时间收敛的终态网络模型，其动态特性由下述方程描述

其中，关节角位移偏差E(t)＝Θ(t)-Θ(0)，η_E是一个非正定的设计参数，σ_E的值与η_E的取值有关，为一初始修正向量，使误差能在有限时间内收敛到零；

ξ(t)满足以下条件：

a.当t∈(-∞,0]∪[Δ,+∞)时，ξ(t)＝0；

b.当t∈(0,Δ)时，

其中，t₀为起始时间，Δ为时间间隔，当二次规划问题获得最优解时，双臂机械手的各个关节角可以回拢到期望的目标轨迹上；

通过对拉格朗日函数各个变量求导，并令其为零，得如下时变矩阵方程

W(t)Y(t)＝v(t) (3)

其中，

I为单位矩阵

记时变矩阵方程误差E(t)＝W(t)Y(t)-ν(t)，根据终态网络动态特性方程(2)，构建网络模型，得到终态网络模型的求解方程如下：

4)将步骤3)中求解得到的结果用于控制各关节电机，驱动机械臂执行任务。

Images