CN106985138A - 基于终态吸引优化指标的冗余机械臂轨迹规划方法 - Google Patents

Abstract

一种基于终态吸引优化指标的冗余机械臂轨迹规划方法，包括以下步骤：1)确定冗余机械臂末端执行器期望目标轨迹r^*(t)和期望回拢的关节角度θ^*(0)；2)设计终态吸引优化指标，形成机械臂重复运动规划方案，其中冗余机械臂实际运动时的初始关节角可以任意指定，不要求末端执行器处于期望轨迹上；给定冗余机械臂实际运动时的初始关节角度θ(0),以θ(0)为运动起始点，形成的重复运动规划方案描述为具终态吸引优化指标的二次规划；3)构建有限值激活函数的终态神经网络模型，有限值终态神经网络求解时变矩阵方程，将求解得到的结果用于控制各关节电机。本发明提供一种精度较高、有限时间收敛、易于实现的基于终态吸引优化指标的冗余机械臂轨迹规划方法。

Description

基于终态吸引优化指标的冗余机械臂轨迹规划方法

技术领域

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

背景技术

自由度(Degrees-of-Freedom,DOF)是指确定一个***在空间中的位置所需要的最小坐标数，六自由度的工业机械臂理论上可以以任意姿态到达可达范围中的任意一点。冗余机械臂是指末端执行器所拥有的DOF多于执行给定末端任务时所需的DOF。较之非冗余机械臂，冗余机械臂因其有多余的DOF，因而有更大的操作空间、操作更为灵活，以满足更多的功能与操作约束要求。例如，躲避环境中的障碍物，克服自身的关节物理限制，以及性能指标优化。

冗余机械臂实时运动控制的一个基本问题是冗余度解析问题，又被称作逆运动学或运动规划。目前，已有许多用于机器人的运动规划和控制的冗余度解析方案见诸文献。经典的做法是基于伪逆的冗余度解析方案。考虑在m维空间中作业的具有n个自由度的机械臂，末端轨迹与关节位移之间的关系(即正运动学问题)

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

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

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

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

这里，J⁺＝J^T(JJ^T)^-1是雅克比矩阵J的伪逆。

D.E.Whitney(Resolved motion rate control of manipulators and humanprostheses,IEEE Trans.Man-Machine Syst.,1969,10(2):47-53)于1969年提出如下具有等式约束的最小速度范数性能指标作为运动规划的目标函数：

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

其解为

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

末端执行器在笛卡尔操作空间做重复运动时，闭合的末端执行器运动轨迹可能产生非闭合的关节角轨迹，导致关节角偏差现象。这种非重复运动问题可能会引起机械臂在重复作业中出现不可预料的情况。一般来说,应用最为广泛的伪逆控制法不能获得重复性为了完成原有的重复运动。通常采用自运动的方法进行弥补,而自运动进行调整往往效率不高(详见Klein C A and Huang C,Review of Pseudo Inverse Control for use withKinematically Redundant Manipulators.IEEE Trans.Syst.Man.Cybern.1983,13(2):245-250；Tchon K,Janiak M.Repeatable approximation of the Jacobian pseudo-inverse.Systems and Control Letters,2009,58(12):849-856)。

基于二次优化(Quadratic Optimization,QP)的冗余解析方案受到关注，F.T.Cheng于1994年提出关节无偏差性能指标(F.-T.Cheng,T.-H.Chen,and Y.-Y.Sun,Resolving manipulator redundancy under inequality constraints,IEEETrans.Robotics Automat.,1994,10(1):65-71)：

为了高效地执行重复运动任务,Y.Zhang,引入重复运动指标作为优化准则。形成重复运动规划(Repetitive motion planning,RMP)方案,使用二次规划(QP)和递归神经网络(RNN)来进行冗余度解析(Zhang Y,Wang J,Xia Y.Adual neural network forredundancy resolution of kinematically redundant manipulators subject tojoint limits and joint velocity limits.IEEE Trans Neural Netw.,2003,14(3):658-667)。其中的重复运动指标为如下渐近收敛性能指标AOC(Asympototically-Convengent Optimality Criterion)：

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

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

发明内容

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

本发明解决其技术问题所采用的技术方案是：

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

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

2)设计终态吸引优化指标，形成机械臂重复运动规划方案，其中冗余机械臂实际运动时的初始关节角可以任意指定，不要求末端执行器处于期望轨迹上；给定冗余机械臂实际运动时的初始关节角度θ(0),以θ(0)为运动起始点，形成的重复运动规划方案描述为具终态吸引优化指标的二次规划：

其中，θ、分别表示冗余机械臂的关节角度和角速度，θ^*(0)是各个关节角的期望初始值，q、p是影响收敛速率的设计参数，二者为满足q＜p的正奇数；θ(t)-θ^*(0)表示各个关节角与初始期望关节角位移偏差，r^*表示机械臂末端执行器期望的运动轨迹，表示末端执行器期望的速度向量；

由于机械臂的初始位置不在期望的轨迹上，通过减小末端执行器期望路径与实际运动轨迹位置间的误差(r^*-f(θ))，改变末端执行器的运动方向，β_r＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率；β_θ＞0是一设计参数，用来形成关节位移的动态性能；J(θ)是冗余机械臂雅可比矩阵，f(θ)是冗余机械臂实际运动轨迹；

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

其中，E为误差变量，β_E＞0为一设计参数，q、p是影响收敛速率的设计参数，满足q＜p，且均为正奇数，φ(·):R^n×n→R^n×n为严格单调递增的激活函数，满足φ(-·)＝-φ(·)；

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

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

WY＝v (3)

其中，

I为单位矩阵

记E＝WY-v，以式(2)所描述的有限值终态神经网络求解时变矩阵方程(3)，其中，取

φ(E^q/p(t))＝(1-exp(-ξE^q/p(t)))/(1+exp(-ξE^q/p(t)))

且ξ＞2，将求解得到的结果用于控制各关节电机。

将冗余机械臂轨迹规划的优化指标设计为一种终态吸引的优化指标TOC(TeminalOptimality Criterion)，即

其中，θ、分别表示冗余机械臂的关节角度和角速度，θ^*(0)是各个关节角的期望初始值，q、p是影响收敛时间的设计参数，二者为满足q＜p的正奇数，β_θ＞0是一设计参数，用来形成关节位移的动态性能。

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

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

在指标函数达到最小值时，冗余机械臂的运动末端执行器在速度层上实现重复运动。

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

其中，β_E＞0为一设计参数，q、p是影响该网络收敛时间的设计参数，满足q＜p，且为正奇数，E为动态方程的收敛误差，φ(·):R^n×n→R^n×n为严格单调递增的激活函数，满足φ(-·)＝-φ(·)。当q＝p时，式(2)退化为渐近收敛的递归神经网络模型。式(5)中激活函数的形式是多样的，不同的激活函数导致具有不同收敛速度的终态神经网络求解器。为了方便实际实现，本发明建议有限值激活函数，即满足

例如，φ(E^q/p(t))＝(1-exp(-μE^q/p(t)))/(1+exp(-μE^q/p(t)))，μ＞2。

本发明的有益效果主要表现在：本发明提供一种终态吸引优化指标TOC，在初始位置偏移情形下，实现冗余机械臂有限时间收敛的重复运动规划任务。相对于已有运动规划方法，终态吸引优化指标TOC使得所提方案具有有限时间收敛的特点，有利于提高计算精度。

相比于具有渐近收敛递归神经网络，有限值终态神经网络具有有限时间收敛特性，该终态神经网络求解方法采用的激活函数是有限值激活函数，为相关时变问题求解提供了实时计算工具，并且在实际应用时易于实现。

附图说明

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

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

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

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

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

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

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

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

具体实施方式

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

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

第一步.确定期望轨迹

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

θ^*(0)＝[0,-π/4,0,π/2,0,-π/4,0]^T

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

第二步.建立冗余机械臂重复运动的二次规划方案

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

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

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

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

WY＝v (10)

I为单位矩阵

记时变矩阵方程误差E＝WY-v。为求解矩阵方程(10)，依据有限值终态神经网络动态方程(5)构建神经网络模型

图2为φ(E^q/p)＝(1-exp(-ξE^q/p))/(1+exp(-ξE^q/p))取不同ξ值的终态神经网络激活函数，且ξ＞2。从图中可以看到随着ξ的增大,有限值终态神经网络动态方程误差收敛的速度会变快。

用于实现本发明重复规划方案的冗余机械臂PA10如图3所示。该机械臂由1个基座，2个连接杆构成，通过关节5、关节6、关节7、关节8、关节9、关节10和关节11组成。该冗余机械臂PA10连杆长度l＝[0.45,0.5,0.08]^T米。

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

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

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

表1

为比较渐近收敛网络与终态神经网络的收敛性能，定义计算误差J_E(t)＝||W(t)y(t)-v(t)||₂。图8给出分别用有限值终态神经网络和递归神经网络求解二次规划问题的误差收敛轨迹。从图中可以看出，以有限值终态神经网络求解时，当时间t接近2秒时，误差收敛精度达到1.8*10^-3,以递归神经网络求解时,误差收敛精度只达到0.02。

Claims (1)

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

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

2)设计终态吸引优化指标，形成机械臂重复运动规划方案，其中冗余机械臂实际运动时的初始关节角可以任意指定，不要求末端执行器处于期望轨迹上；给定冗余机械臂实际运动时的初始关节角度θ(0),以θ(0)为运动起始点，形成的重复运动规划方案描述为具终态吸引优化指标的二次规划：

$\begin{array}{cc}\underset{\stackrel{\·}{\mathrm{\θ}}\left(t\right)}{\min imize}& \frac{1}{2}\left(\stackrel{\·}{\mathrm{\θ}}\right(t)+{\mathrm{\β}}_{\mathrm{\θ}}{\left({\left(\mathrm{\θ}\left(t\right)-{\mathrm{\θ}}^{*}\left(0\right)\right)}^{q/p}\right)}^T(\stackrel{\·}{\mathrm{\θ}}\left(t\right)+{\mathrm{\β}}_{\mathrm{\θ}}{\left(\mathrm{\θ}\left(t\right)-{\mathrm{\θ}}^{*}\left(0\right)\right)}^{q/p})\\ subjectto& J\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}={\stackrel{\·}{r}}^{*}+{\mathrm{\β}}_r(r^{*}-f(\mathrm{\θ}\left)\right)\end{array}---\left(1\right)$

其中，θ、分别表示冗余机械臂的关节角度和角速度，θ^*(0)是各个关节角的期望初始值，q、p是影响收敛速率的设计参数，二者为满足q＜p的正奇数；

θ(t)-θ^*(0)表示各个关节角与初始期望关节角位移偏差，r^*表示机械臂末端执行器期望的运动轨迹，表示末端执行器期望的速度向量；

由于机械臂的初始位置不在期望的轨迹上，通过减小末端执行器期望路径与实际运动轨迹位置间的误差(r^*-f(θ))，改变末端执行器的运动方向，β_r＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率；β_θ＞0是一设计参数，用来形成关节位移的动态性能；J(θ)是冗余机械臂雅可比矩阵，f(θ)是冗余机械臂实际运动轨迹；

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

$\stackrel{\·}{E}\left(t\right)=-{\mathrm{\β}}_E\mathrm{\φ}\left(E^{q/p}\right(t\left)\right)---\left(2\right)$

其中，E为误差变量，β_E＞0为一设计参数，q、p是影响收敛速率的设计参数，满足q＜p，且均为正奇数，φ(·):R^n×n→R^n×n为严格单调递增的激活函数，满足φ(-·)＝-φ(·)；

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

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

WY＝v (3)

其中，

I为单位矩阵

$Y=\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \mathrm{\λ}\end{array}\right],$

$v=\left[\begin{array}{c}-{\mathrm{\β}}_{\mathrm{\θ}}{\left(\mathrm{\θ}-{\mathrm{\θ}}^{*}\left(0\right)\right)}^{q/p}\\ {\stackrel{\·}{r}}^{*}+{\mathrm{\β}}_r(r^{*}-f(\mathrm{\θ}\left)\right)\end{array}\right].$

记E＝WY-v，以式(2)所描述的有限值终态神经网络求解时变矩阵方程(3)，其中，取

φ(E^q/p(t))＝(1-exp(-ξE^q/p(t)))/(1+exp(-ξE^q/p(t)))

且ξ＞2，将求解得到的结果用于控制各关节电机。