一种基于终态吸引优化指标的冗余机械臂重复运动规划方法与流程

本发明涉及冗余机械臂的重复运动规划及控制技术，具体地，涉及一种有限时间收敛性能指标、在初始偏移情形下的冗余机械臂的逆运动学求解方法。

背景技术：

机械臂是拟人手臂、手腕和手功能的机械电子装置，其末端任务包括搬运、焊接、油漆和组装等，目前已广泛应用于工业制造、医学治疗、娱乐服务、消防、军事和太空探索等领域中。一个机械手臂一般拥有3个或3个以上的自由度，在用机械手臂去完成某个特定的工作而又多余的自由度时，该机械手臂被定义为冗余机械臂。同非冗余机械臂相比，冗余机械臂有更大的操作空间，多余的自由度能够满足更多的功能约束，比如物理极限躲避和环境障碍物躲避。

控制冗余机械臂实时运动的一个基本问题是冗余度解析方案。给出末端运动的轨迹，如何实时地得到各关节的速度、角速度和力矩值，经典的做法是基于伪逆的冗余度解析方案。考虑在m维空间中作业的具有n个自由度的机械臂，末端轨迹与关节位移之间的关系(即正运动学问题)

r(t)＝f(θ(t))

其中，r(t)表示机械臂末端执行器在工作空间中笛卡尔坐标系下的位移，θ(t)表示关节位移。末端笛卡尔空间与关节空间之间的微分运动关系为

其中，是r的时间导数，是关节速度向量，是机械臂的雅克比矩阵。

对于冗余机械臂，传统方法是求解moore-penrose广义逆(伪逆)，可得关节变量速度的最小二乘解为

这里，j⁺＝j^t(jj^t)^-1是雅克比矩阵j的伪逆。

d.e.whitney(resolvedmotionratecontrolofmanipulatorsandhumanprostheses,ieeetrans.man-machinesyst.,1969,10(2):47-53；(即：操纵器和人工假肢的运动速率控制方法)于1969年提出如下具有等式约束的最小速度范数性能指标作为运动规划的目标函数：

式中，a为正定加权矩阵。求解上述规划问题，需求解以下方程组

其解为

式(1)是式(3)当a＝i时的特殊情形。也可看出，规划问题是通过求解方程组(2)得到解决的。

冗余度解析的研究重点是冗余机械臂的轨迹规划，其性能直接关系到机械臂能否成功完成给定的末端任务。当末端执行器的运动轨迹是闭合的，在机械臂完成末端工作任务后，各个关节角变量在运动空间中的轨迹不一定封闭。这种非重复性问题可能产生不期望的关节位形，使得冗余机械臂末端封闭轨迹的重复作业出现预料之外的情况，甚至会导致意外及危险情况的发生。应用最为广泛的伪逆控制法不能获得运动的重复性。为了完成原有的重复运动，通常采用自运动的方法进行弥补,而自运动进行调整往往效率不高。(详见kleincaandhuangc,reviewofpseudoinversecontrolforusewithkinematicallyredundantmanipulators.ieeetrans.syst.man.cybern.1983,13(2):245-250；即：基于伪逆控制方法的冗余机械臂运动规划)。

基于二次优化(quadraticoptimization,qp)的冗余解析方案受到关注，f.t.cheng于1994年提出关节无偏差性能指标(f.-t.cheng,t.-h.chen,andy.-y.sun,resolvingmanipulatorredundancyunderinequalityconstraints,ieeetrans.roboticsautomat.,1994,10(1):65-71；即：不等式约束条件下的冗余机械臂轨迹规划方法)：

为了高效地执行重复运动任务,y.zhang,引入重复运动指标作为优化准则。形成重复运动规划(repetitivemotionplanning,rmp)方案,使用二次规划(qp)和递归神经网络(rnn)来进行冗余度解析(zhangy,wangj,xiay.adualneuralnetworkforredundancyresolutionofkinematicallyredundantmanipulatorssubjecttojointlimitsandjointvelocitylimits.ieeetransneuralnetw.,2003,14(3):658-667；即：基于关节角度和角速度限制的冗余机械臂轨迹规划方法)。其中的重复运动指标为如下渐近收敛性能指标aoc(asympototically-convengentoptimalitycriterion)：

递归神经网络是求解基于二次型优化描述的冗余度解析问题的有效方法。通常的神经网络求解器具有渐近收敛性能，在计算时间足够长以后，能够获得有效解，且能应用于动态时变优化问题。

最近，具有有限时间收敛性能的递归神经网络被用求解时变问题。相比于具有渐近收敛动态特性的递归神经网络，终态收敛动态特性具有有限时间收敛性，不仅能够改进收敛速度，而且达到较高收敛精度。然而，诸文献中的有限时间收敛的神经网络都采用线性激励函数，或具有无限值激励函数，实际实现时，由于能量有限，无限值激励函数神经网络实现时存在本质困难。

技术实现要素：

为了克服现有冗余机械臂轨迹规划方式的精度较低、收敛较慢、不易实现的不足，本发明提供一种精度较高、有限时间收敛、易于实现的基于终态吸引优化指标的冗余机械臂轨迹规划方法；本发明的重复运动性能优化指标，

为了克服现有冗余机械臂轨迹规划方式的精度较低、收敛较慢、不易实现的不足，本发明提供一种精度较高、有限时间收敛、易于实现的基于终态吸引优化指标的冗余机械臂轨迹规划方法；本发明的重复运动性能优化指标，即终态吸引优化指标，形成冗余机械臂轨迹规划的二次优化方法。以具有有限值激活函数的终态神经网络作为求解器，在初始位置偏移情形下，实现冗余机械臂有限时间收敛的重复运动规划任务。

为了实现上述目的，本发明提供如下的技术方案：

一种基于终态吸引优化指标的冗余机械臂轨迹规划方法，包括以下步骤：

1)确定冗余机械臂末端执行器期望目标轨迹r^*(t)和期望回拢的关节角度θ^*(0)；

2)给定冗余机械臂实际运动时的初始关节角度θ(0),以θ(0)为运动起始点，将冗余机械臂重复运动规划描述为二次规划问题，其性能指标为以下终态吸引的优化指标toc：

其中，0＜α＜1，sgn(·)为取符号函数，θ(t)-θ^*(0)表示各个关节角与初始期望关节角位移偏差，r^*表示机械臂末端执行器期望的运动轨迹，表示末端执行器期望的速度向量；由于机械臂的初始位置不在期望的轨迹上，通过减小末端执行器期望路径与实际运动轨迹位置间的误差(r^*-f(θ))，改变末端执行器的运动方向，βr＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率；βθ＞0是一设计参数，用来形成关节位移的动态性能；j(θ)是冗余机械臂雅可比矩阵，f(θ)是冗余机械臂实际运动轨迹；

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

其中，e为误差变量，βe＞0为一设计参数，φ(·):r^n×n→r^n×n为严格单调递增的激活函数，满足φ(-·)＝-φ(·)；

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

式中，0＜α＜1，sgn(·)为取符号函数，λ(t)为拉格朗日乘子向量，λ^t是λ(t)向量的转置；

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

wy＝v(6)

其中，

i为单位矩阵

记e＝wy-v，以式(5)所描述的有限值终态神经网络动态方程求解步骤3)中的矩阵方程(6)，其中，取

得到各关节角自运动轨迹。

本发明的技术构思为：将冗余机械臂轨迹规划的优化指标设计为一种终态吸引的优化指标toc(teminaloptimalitycriterion)，即

其中，θ、分别表示冗余机械臂的关节角度和角速度，θ^*(0)是各个关节角的期望初始值，0＜α＜1，βθ＞0是一设计参数，用来形成关节位移的动态性能，sgn(·)为取符号函数。

上述可重复运动优化方案的设计思想是期望获得下述动态方程

其中，关节角位移偏差ε(t)＝θ(t)-θ(0)。此动态方程所表达的系统有限时间收敛于零，需要的收敛时间t为

当优化的指标函数达到最小值时，冗余机械臂的各个关节角可以回拢到期望的目标轨迹上。

为了求解优化指标toc下的重复运动规划问题，采用一种带有限值激活函数的终态神经网络模型，该网络的动态方程如下：

其中，βe＞0为一设计参数，e为动态方程的收敛误差，φ(·):r^n×n→r^n×n为严格单调递增的激活函数，满足φ(-·)＝-φ(·)。在动态方程(5)中的激活函数φ(·)能量有界，即满足

与现有的技术相比，本发明有如下优点：

本发明提供一种终态吸引优化指标toc，在初始位置偏移情形下，实现冗余机械臂有限时间收敛的重复运动规划任务。相比于已有的重复运动规划方法，该方案具有更快的时间收敛特点，而且最终机械臂各个关节角的回拢精度更高。相比于具有渐近收敛递归神经网络，有限值终态神经网络具有有限时间收敛特性，它适于时变问题求解(式(6)为具有时变矩阵方程)，该神经网络求解器采用的激活函数是有限值激活函数，激活函数的能量有界，在工程应用中易于实现，且成本低，符合工程实际需要。

附图说明

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

图2为取不同α值时的终态神经网络激活函数φ(·)。

图3为采用本发明重复规划方案的冗余机械臂puma560。

图4为冗余机械臂puma560末端执行器的运动轨迹。

图5为冗余机械臂puma560的各个关节角轨迹。

图6为冗余机械臂puma560各个关节角度和关节角速度。

图7为冗余机械臂puma560末端执行器的各位置误差轨迹。

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

具体实施方式

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

参照图1～图8，一种基于终态吸引优化指标的冗余机械臂重复运动规划方法，图1为冗余机械臂重复运动规划方案的流程，由以下3个步骤组成：1、确定冗余机械臂末端执行器期望目标轨迹和期望回拢各关节角度2、建立具有终态吸引优化指标的冗余机械臂重复运动二次规划方案3、以有限值终态神经网络求解二次规划问题，获得各关节角轨迹。

1)确定期望轨迹

设定冗余机械臂puma560期望回拢的关节角度

θ^*(0)＝[0,0,0,0,0,0]^t

确定圆轨迹的圆心坐标将圆的半径设定为0.2m,其圆面与x轴的夹角为π/6rad，末端执行器完成圆轨迹时间t＝10s。考虑到冗余机械臂puma560的初始位置可能不在期望的运动轨迹上，将机械臂的六个关节角度初值设为θ(0)＝[0,0,0,0,0+1,0]^t。

2)建立冗余机械臂重复运动的二次规划方案

为实现冗余机械臂有限时间收敛的重复运动规划，将冗余机械臂重复运动轨迹规划描述为以下二次规划问题，其终态吸引的优化指标为

其中，0＜α＜1，sgn(·)为取符号函数，θ、分别表示冗余机械臂的关节角度和关节角速度，θ^*(0)是各个关节角的期望初始值，βθ＞0是一设计参数，用来形成关节位移的动态性能。θ(t)-θ^*(0)表示各个关节角与初始期望关节角位移偏差，r^*表示机械臂末端执行器期望的运动轨迹，表示末端执行器期望的速度向量。由于机械臂的初始位置可能不在期望的轨迹上，通过减小末端执行器期望路径与实际运动轨迹位置间的误差(r^*-f(θ))，改变末端执行器的运动方向，βr＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率。j(θ)是冗余机械臂雅可比矩阵，f(θ)是冗余机械臂实际运动轨迹。

3)以有限值终态神经网络求解上述二次规划问题

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

其中，e为误差变量，βe＞0为一设计参数，φ(·):r^n×n→r^n×n为严格单调递增的激活函数，满足φ(-·)＝-φ(·)

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

式中，λ(t)为拉格朗日乘子向量，λ^t是λ(t)向量的转置；

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

wy＝v(6)

i为单位矩阵

其中，0＜α＜1，sgn(·)为取符号函数。

记e＝wy-v，以式(5)所描述的有限值终态神经网络动态方程求解步骤3)中的矩阵方程(6)，得到各关节角自运动轨迹。

图2为取不同α值的终态神经网络激活函数，且0＜α＜1。从图中可以看到随着α的减小,有限值终态神经网络动态方程误差收敛的速度会变快。

用于实现本发明重复规划方案的冗余机械臂puma560如图3所示。该机械臂由1个基座，3个连接杆构成，通过关节1、关节2、关节3、关节4、关节5、和关节6组成。该冗余机械臂puma560连杆长度l＝[0.4318,0.4318,0.25625]^t米。

机械臂的末端执行器在空间中的运动轨迹如图4所示。图中给出目标圆轨迹及机械臂末端执行器运动轨迹。可以看出，末端执行器的初始位置不在期望的轨迹上。随着时间的增加，实际轨迹和期望轨迹吻合，末端执行器的终值位置误差精度在三个方向xyz轴上达到10^-4，如图7所示。

当冗余机械臂puma560各关节角经过10s后，各关节角的终值误差达到10^-4，所有的关节轨迹基本闭合，其轨迹如图5所示。

为了验证终态吸引优化指标toc在重复运动规划中的有效性，机械臂puma560末端执行器完成圆轨迹过程中得到的关节角瞬态轨迹和角速度瞬态轨迹如图6所示。从图中可以看出，冗余机械臂的各关节角最终收敛于期望的关节角位置。当t＝10s时，机械臂运动前后各关节角与其期望关节角位置之间的最大偏差为5.299*10^-5,用递归神经网络求解得到的各个关节角的终值误差最大偏差为6.061*10^-4，如表1所示。

表1

为比较渐近收敛网络与终态神经网络的收敛性能，定义计算误差je(t)＝||w(t)y(t)-v(t)||2。图8给出分别用有限值终态神经网络和递归神经网络求解二次规划问题的误差收敛轨迹。从图中可以看出，以有限值终态神经网络求解时，当时间t接近2.2秒时，误差收敛精度已经到达零附近,以递归神经网络求解时,误差在5秒时还未收敛。