一种移动机械臂重复运动规划方法与流程

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

背景技术：

移动机械臂是由一个或者多个连杆机械臂安装在一个可移动平台上的一种操作工具。相比于底座固定的冗余机械臂而言，移动机械臂具有更广的工作空间，并且为同一任务提供了多种自由度分配的方案。在未来机器人化的智能家居服务中，人们期望机器人像人一样大范围移动，移动机器人和机械臂的实时运动规划与控制已经成为人们关注的焦点之一。

随着冗余机械臂运动控制领域研究成果的大量涌现，二次优化逐渐取代了基于伪逆的运动规划方法。许多基于二次优化方法重复运动规划算法被广泛应用于底座固定的冗余机械臂运动控制中。随着研究成果的大量涌现，为了高效地执行重复运动任务，渐近收敛优化重复运动方案被开发，并且结合递归神经网络(rnn)用于求解底座非固定的移动机械臂冗余度解析问题，取得了很好的实验结果。但是，面对可移动机械臂各个初始关节角不在期望位置的情形下，如何建立有效可行的运动学方案，当冗余机械臂初始位置偏移期望位置时，实现冗余机械臂有限时间内重复运动，本发明提供一种精度较高、有限时间收敛、易于实现的基于终态吸引优化指标的冗余机械臂轨迹规划方法，以具有有限值激活函数的终态神经网络作为求解器，在初始位置偏移情形下，冗余机械臂各关节角仍然可以回到初始期望位置。

冗余机械臂在实时运动控制中存在的一个基础问题是冗余度解析问题，考虑在m维空间中作业的具有n个自由度的机械臂，末端轨迹与关节位移之间的关系(即正运动学问题)

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

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

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

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

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

末端执行器在笛卡尔操作空间做重复运动时，闭合的末端执行器运动轨迹可能产生非闭合的关节角轨迹，导致关节角偏差现象。这种非重复运动问题可能会引起机械臂在重复作业中出现不可预料的情况。一般来说,应用最为广泛的伪逆控制法不能获得重复性为了完成原有的重复运动。通常采用自运动的方法进行弥补,而自运动进行调整往往效率不高。

技术实现要素：

为了克服现有的移动机械臂运动规划方法的精度较低、无法实现有限时间收敛的不足，本发明提供一种精度高、有限时间收敛的移动机械臂重复运动规划方法，当机械臂初始各个关节角偏移期望位置时，保证移动机械臂可以有效完成工作任务，无需考虑初始各个关节角是否在期望的任务轨迹上。

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

一种移动机械臂重复运动规划方法，包括以下步骤：

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

2)设计有限时间收敛的优化指标，形成移动机械臂重复运动规划方案，其中移动机械臂实际运动时的初始关节角可以任意指定(只需要保证关节角的运动幅度在关节角物理限制范围之内)，不要求末端执行器处于期望轨迹上；给定冗余机械臂实际运动时的初始关节角度θ(0),以θ(0)为运动起始点，形成的重复运动规划方案描述为具有有限时间收敛优化指标的二次规划：

其中，g＝λ(qw-q(0))^q/p∈r^3+m，κ＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率，0＜q/p＜1，q、p分别为奇数，λ＝diag(λ1,λ2,...λ3+m)∈r^(3+m)×(3+m)是一斜对角线矩阵，且λ中的每一个元素λi＞0∈r,i＝1,2,...3+m，i是单位矩阵，||·||2表示二范数向量，

在式(1)中jw∈r^n×(s+m)表示整个移动机械臂系统的雅克比矩阵，rwd∈rⁿ为移动机械臂末端执行器运动的期望轨迹，rw为末端执行器实际运动的轨迹，表示末端执行器运动时的速度向量，为了避免机械臂的移动过程中受到损坏，在移动机械臂运动过程中需要考虑各个关节角转动范围的限制，

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

其中，关节角位移偏差e(t)＝θ(t)-θ(0)，此动态方程所表达的系统有限时间收敛于零，βe1＞0，βe2＞0，γ＞0，q、p是影响收敛速率的设计参数，满足q＜p，且均为正奇数，φ(·):r^n×n→r^n×n为严格单调递增的激活函数，满足φ(-·)＝-φ(·)，有限值激活函数φ(e^q/p(t))＝(1-exp(-ξe^q/p(t)))/(1+exp(-ξe^q/p(t)))，ξ＞2，即满足

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

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

式中，m＝b^tb∈r^(s+m)×(s+m)，h＝b^tg，λ(t)为拉格朗日乘子向量，λ^t(t)是λ(t)向量的转置，通过拉格朗日函数对各个变量求导，并令其为零，得下述矩阵方程

e＝wy-v(3)

i为单位矩阵

最后，记时变矩阵方程误差e＝wy-v，为求解矩阵方程(3)，依据终态神经网络动态方程(2)构建神经网络模型(4)，得到各关节角自运动轨迹：

将求解得到的结果用于控制各关节电机。本发明的有益效果主要表现在：本发明提供一种有限时间收敛的优化指标，在初始位置偏移情形下，实现移动机械臂各个关节角仍然可以回到期望初始位置，实现重复运动规划任务。相对于已有运动规划方法，该神经网络求解优化指标使得所提方案具有有限时间收敛的特点，有利于提高计算精度和速度。

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

附图说明

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

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

图3为采用本发明重复规划方案的移动机械臂结构图，其中，1为末端执行器，2,3,4分别为不同的关节角。

图4为移动机械臂底座的几何运动模型。

图5为移动机械臂末端执行器的运动轨迹。

图6为移动机械臂末端执行器的各位置误差轨迹。

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

具体实施方式:

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

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

第一步.确定期望轨迹

设定移动机械臂期望回拢的关节角度

将目标路径设定为一个cardioid轨迹任务，给定机械臂的末端执行器的期望路径为

其中，α＝1m，t＝[0,t]，t＝10为移动机械臂的运动周期，s＝3πα²/2＝4.71m²为闭合cardioid轨迹面积，可以求得该闭合曲线的周长l＝8α＝8m。

设定移动机械臂期望回拢的关节角度

θ^*(0)＝[0,0,3π/10,π/36,π/36,π/12]^t

考虑到移动机械臂的初始位置可能不在期望的运动轨迹上，将移动机械臂的六个关节角度初值设为θ(0)＝[0,0,3π/10,π/36,π/36,π/12+0.005]^t。

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

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

其中，g＝λ(qw-q(0))^q/p∈r^3+m，κ＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率。0＜q/p＜1，q、p分别为奇数，λ＝diag(λ1,λ2,...λ3+m)∈r^(3+m)×(3+m)是一斜对角线矩阵，且λ中的每一个元素λi＞0∈r,i＝1,2,...3+m，i是单位矩阵，||·||2表示二范数向量，在式(9-5)中jw∈r^n×(s+m)表示整个移动机械臂系统的雅克比矩阵，rwd∈rⁿ为移动机械臂末端执行器运动的期望轨迹，rw为末端执行器实际运动的轨迹。表示末端执行器运动时的速度向量。

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

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

e＝wy-v(3)

i为单位矩阵

最后，记时变矩阵方程误差e＝wy-v。为求解矩阵方程(3)，依据终态神经网络动态方程(2)构建神经网络模型(4)，得到各关节角自运动轨迹：

将求解得到的结果用于控制各关节电机。

图2为φ(e^q/p(t))＝(1-exp(-δe^q/p(t)))/(1+exp(-δe^q/p(t)))取不同δ值的终态神经网络激活函数。从图中可以看出，δ的取值在0和1之间，在图中分别取δ＝0.1,0.2,0.4,0.6,0.8，随着δ的增大，曲线的在零点附近的收敛速度越慢。

用于实现本发明重复规划方案的移动机械臂如图3所示。该机械臂由1个移动基座，3个可移动轮子(其中一个在移动基座后面)，通过关节2、关节3、关节4和末端执行器1组成。该移动机械臂的连杆长度分别为l1＝0.3米，l2＝0.25米，l3＝0.1米。

可移动底座的几何构成和运动学模型如图4所示。图中，xc和yc分别表示圆心c投影在xw,yw的角度位置。由于移动移动机械臂只在平面上运动，因此zw＝0。r为底座轮子的半径，l表示底座圆心到轮子的垂直距离，表示轮子旋转的角速度。由于初始状态时，底座轮子沿着x0方向运动，则θ0＝π/3。分别表示xc，yc，θ0表示底座的起始运动方向。分别表示底座三个轮子的转动速度。

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

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