CN109940615B - 一种面向双臂机械手同步重复运动规划的终态网络优化方法 - Google Patents

Abstract

一种面向双臂机械手同步重复运动规划的终态网络优化方法，包括以下步骤：1)确定双臂机械手左右两个末端执行器期望目标轨迹r^* _L(t)和r^* _R(t)，双臂机械手左右关节角期望回拢的角度θ^* _L(0)和θ^* _R(0)；2)设计终态吸引优化指标，形成双臂机械手同步重复运动规划方案；3)构建有限值激活函数的终态网络模型，以终态网络求解时变矩阵方程；4)将求解得到的结果用于控制各关节电机，驱动机械臂执行任务。本发明提供一种精度高、有限时间收敛的双臂机械手可重复运动方法，当机械手左右各关节角初始位置任意给定时，保证双臂机械手仍然可以有效完成工作任务，而无需考虑初始各个关节角是否在期望的任务轨迹上。

Description

一种面向双臂机械手同步重复运动规划的终态网络优化方法

技术领域

本发明涉及双臂多连杆机械手的同步重复运动规划及控制技术，具体地，涉及一种有限时间收敛性能指标、在机械手左右双臂各运动关节角初始位置偏移期望轨迹情形下的机械手逆运动学求解方法。

背景技术

机器人技术被广泛应用在各个领域，于生产、研究中发挥着巨大的作用。而机器人的操作手，即多连杆机械臂，可将末端执行器移动到空间的指定位置并完成相应的轨迹任务。根据自由度(Degrees-of-Freedom,DOF)的多少，机械臂可划分为冗余机械臂和非冗余机械臂。冗余机械臂拥有的自由度多于完成给定末端任务所需的DOF。较之非冗余机械臂，冗余机械臂更灵活且更具优势。双臂机械手由于其固有的优点成为了机器人领域中一个新的研究热点。相比于单臂的机械手，双臂机械手近似人的双臂，具有强大的协调能力，能够在复杂的工作任务和多变的工作环境中具有独特的优势。这也是其在实际工程应用中日益凸显出重要性的原因之一。

双臂机械手被广泛用于有限工作空间内的重复单调工作。通过已知末端执行器的位置和姿态，求解其对应的冗余机械臂各个关节角的可行路径，这是一个基本的逆运动学求解问题。但当冗余机器臂具有更多的自由度时，逆运动学问题往往存在多个解甚至无穷组解。为解决这类问题，通常应用伪逆方法来获得关节角位置的通解。考虑在m维空间中作业的具有n个自由度的左右双臂机械手，其末端轨迹与关节位移之间的映射关系(即正运动学问题)

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)的时间导数，

和

分别是关节速度向量，

和

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

末端执行器在笛卡尔操作空间做重复运动时，闭合的末端执行器运动轨迹可能没有产生闭合的关节角轨迹，导致关节角偏差现象。这种非重复运动问题所带来的偏差可能会引起机械臂在重复作业中出现不可预料的情况。应用最为广泛的伪逆公式由于计算过程的复杂性、求解过程中的奇异点和机械手物理关节角极限等问题，通常无法应用于求解周期性的重复运动，使得双臂机械手的运动轨迹难以预测。在大多数情况下，基于二次型优化的运动解析方案通常可以克服伪逆控制法的缺点，实现重复运动。

最近，具有有限时间收敛性能的递归网络被用求解时变问题。相比于具有渐近收敛动态特性的递归神经网络，具有有限时间收敛特性的终态网络模型，不仅能够改进收敛速度，而且达到较高收敛精度。

发明内容

为了使双臂机械手各关节角在初始位置偏移期望位置时，在有限的时间内实现机械臂的重复运动，本发明提供一种精度较高、有限时间收敛、易于实现的基于终态吸引优化指标的双臂机械手同步重复运动轨迹规划方法，以具有有限值激活函数的终态网络作为求解器，在左右臂的各关节角初始位置偏移情形时，双臂机械手各运动关节角完成轨迹任务后，最终仍然可以回到初始期望位置。

为了解决上述技术问题本发明提供如下的技术方案：

一种面向双臂机械手同步重复运动规划的终态网络优化方法，包括以下步骤：

1)确定双臂机械手左右两个末端执行器期望目标轨迹r^* _L(t)和r^* _R(t)，双臂机械手各关节角期望回拢的角度θ^* _L(0)和θ^* _R(0)；

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

其中，

Θ＝[θ_L；θ_R]。θ_L表示左边机械臂的关节角度，θ_R表示右边机械臂的关节角度，

表示双臂机械手的各运动关节角速度，Θ^*(0)为机械手左右两臂各关节角的期望初始值，β_Θ＞0，0＜δ＜1是一设计参数，用来形成关节位移的动态性能。Θ(t)-Θ^*(0)表示双臂机械手各关节角初始位置与初始期望位置之间的位移偏差，

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

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

3)构建有限值激活函数的终态网络模型，其动态特性由下述方程描述

其中，关节角位移偏差E(t)＝Θ(t)-Θ(0)，此动态方程所表达的系统有限时间收敛于零，0＜δ＜1,β_E＞0；

在运动规划方案(1)达到最小值时，双臂机械手的各个关节角可以回拢到期望的目标轨迹上；

为求解步骤2)中的二次规划，建立拉格朗日函数：

式中，λ(t)为拉格朗日乘子向量，λ^T是λ(t)向量的转置；通过拉格朗日函数对各个变量求导，并令其为零，得下述时变矩阵方程

WY＝v (3)

其中，

I为单位矩阵

记E＝WY-v，以式(2)所描述的有限值终态网络求解时变矩阵方程(3)，得到系统的求解方程如下

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

本发明的有益效果主要表现在：本发明提供一种具有有限时间收敛特性的终态网络模型，基于该终态网络，形成了一种有限时间内收敛的同步重复运动规划方案，使双臂机械手不用考虑初始左右各关节角的位置，在完成给定的轨迹后各个关节角仍然可以回到期望的初始位置，实现重复运动规划任务。该方案相对于已有的运动规划方法来说，具有有限时间收敛的特点，其所对应的终态网络求解方法不仅加快了动态网络的收敛速度，而且提高了网络的收敛精度。这种网络的求解借助的激活函数为有限值激活函数，这为求解相关的时变问题提供了计算工具，而且在实际应用时更易于实现。

附图说明

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

图2为采用本发明重复规划方案的双臂机械手。

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

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

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

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

具体实施方式

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

参照图1～图6，一种面向双臂机械手同步重复运动规划的终态网络优化方法，由以下4个步骤组成：1、分别确定双臂机械手左右末端执行器期望目标轨迹和期望回拢各关节角度2、建立具有有限时间收敛特性的双臂机械手同步重复运动二次规划方案3、以有限值终态网络求解二次规划问题，获得左右各运动关节角轨迹。4、将求解得到的结果驱动电机运行，使机械臂完成轨迹任务。

第一步、确定期望轨迹

设定双臂机械手期望回拢的各关节角度。将目标路径设定左边为半径0.1m的圆形轨迹，右边为半径0.05m的圆形轨迹，确定左右圆的期望轨迹方程分别为

其中，机械臂末端完成圆形轨迹的时间T＝10s。设定双臂机械手期望回拢的左右关节角度

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

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

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

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

第二步、建立双臂机械手同步重复运动的二次规划方案

为实现双臂机械手有限时间收敛的同步重复运动规划，将重复运动轨迹规划描述为以下二次规划问题，其优化指标为

其中，

Θ＝[θ_L；θ_R]。θ_L表示左边机械臂的关节角度，θ_R表示右边机械臂的关节角度，

表示双臂机械手的各运动关节角速度，Θ^*(0)为机械手左右两臂各关节角的期望初始值，β_Θ＞0，0＜δ＜1是一设计参数，用来形成关节位移的动态性能。Θ(t)-Θ^*(0)表示双臂机械手各关节角初始位置与初始期望位置之间的位移偏差，

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

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

第三步、以有限值终态网络求解上述二次规划问题

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

WY＝v

其中，

I为单位矩阵

记时变矩阵方程误差E＝WY-v。依据有限值终态网络动态方程(3)构建神经网络模型，得到系统的求解方程如下

用于实现本发明同步重复规划方案的双臂机械手如图2所示。该机械臂系统由2个固定基座组成，两个基座之间的距离为l＝0.5m。左右两个机械臂分别由4个连接杆构成，通过关节a1、关节a2、关节a3、关节a4、关节a5组成。该双臂机械手如图连杆长度分别为L₁＝0.2035m，L₂＝0.19m，L₃＝0.139m，L₄＝0.315m。

该机械臂系统的末端执行器在空间中的运动轨迹如图3所示。图中给出目标圆轨迹及机械臂末端执行器运动轨迹。可以看出，左右末端执行器的初始位置不在期望的轨迹上。随着时间的增加，实际轨迹和期望轨迹吻合。当以递归网络求解同步重复运动规划方案时，末端执行器的回拢速度慢于终态网络的求解速度。

定义计算误差J_E(t)＝||W(t)y(t)-v(t)||₂。图4给出分别用有限值终态网络和传统递归神经网络求解二次规划问题的误差收敛轨迹。从图中可以看出，以有限值终态网络求解时，当时间t接近0.1s时，误差收敛至零，当时间t接近1s时，误差精度达到0.1*10^-3。

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

冗余机械臂在完成轨迹任务时，各个关节角的运动轨迹如图6所示。θ_R1...θ_R5.表示右边机械臂的五个运动角，.θ_L1...θ_L5.表示左边机械臂的五个运动角，从图中可以看到所有关节角的运动过程，所有关节角最终回到了运动的初始期望位置。

Claims (1)

1.一种面向双臂机械手同步重复运动规划的终态网络优化方法，其特征在于，所述方法包括以下步骤：

1)确定双臂机械手左右两个末端执行器期望目标轨迹r^* _L(t)和r^* _R(t)，双臂机械手各关节角期望回拢的角度θ^* _L(0)和θ^* _R(0)；

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

其中，

Θ＝[θ_L；θ_R]，θ_L表示左边机械臂的关节角度，θ_R表示右边机械臂的关节角度，

表示双臂机械手的各运动关节角速度，Θ^*(0)为机械手左右两臂各关节角的期望初始值，β_Θ＞0，0＜δ＜1是一设计参数，用来形成关节位移的动态性能，Θ(t)-Θ^*(0)表示双臂机械手各关节角初始位置与初始期望位置之间的位移偏差，

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

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

3)构建有限值激活函数的终态网络模型，其动态特性由下述方程描述

其中，关节角位移偏差E(t)＝Θ(t)-Θ(0)，此动态方程所表达的系统有限时间收敛于零，0＜δ＜1,β_E＞0；

在运动规划方案(1)达到最小值时，双臂机械手的各个关节角可以回拢到期望的目标轨迹上；

为求解步骤2)中的二次规划，建立拉格朗日函数：

式中，λ(t)为拉格朗日乘子向量，λ(t)^T是λ(t)向量的转置；通过拉格朗日函数对各个变量求导，并令其为零，得下述时变矩阵方程

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

其中，

I为单位矩阵

记E＝W(t)Y(t)-v(t)，以式(2)所描述的有限值终态网络求解时变矩阵方程(3)，得到系统的求解方程如下

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

Images