CN108908347A - 一种面向冗余移动机械臂容错型重复运动规划方法 - Google Patents

Abstract

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

Description

一种面向冗余移动机械臂容错型重复运动规划方法

技术领域

本发明涉及移动机械臂的重复运动规划及控制技术，具体地，涉及一种基于终态网络技术的优化性能指标、在移动机械臂初始姿态偏移目标轨迹情形下的移动机械臂逆运动学求解方法。

背景技术

移动机械臂通常由移动平台(移动机器人中的一种)和装在移动平台上的机械臂组成。移动机械臂不但具有机器臂的操作灵活性，而且具有移动机器人工作空间的广阔性。随着移动机械臂性能的逐渐提高，人们对生活品质的美好向往以及探索未知环境的需要，移动机械臂的应用拓展至军事、医疗、娱乐、太空探索等诸多领域。

移动机械臂的研究通常将移动平台和机械臂作为一个整体，即两者在执行任务时同时运动，整个系统可视为一个机械臂系统。根据自由度(Degrees-of-Freedom,DOF)的多少，该机械臂系统可划分为冗余移动机械臂和非冗余移动机械臂。冗余移动机械臂是指所拥有的DOF多于完成给定末端任务所需的DOF的机械臂。对应地，非冗余移动机械臂是指执行给定的末端任务时没有多余DOF的机械臂。显然，较之非冗余移动机械臂，冗余移动机械臂因其有多余的DOF而更为灵活且更具优势，这也是其在实际工程应用中日益凸显出重要性的原因之一。

移动机械臂通常配置6个以上自由度，加上移动平台的自由度，构成冗余运动链，为此，冗余逆解规划问题成为这类机器人应用研究中的重要任务之一。冗余移动机械臂可在满足给定任务约束的同时，实现各运动关节角重构。通过已知末端执行器的位置和姿态，求解其对应的移动平台各转动角和机械臂各个关节角的值。

考虑在m维空间中作业的具有n个自由度的机械臂，末端轨迹与各运动关节位移之间的关系(即正运动学问题)

r_w(t)＝f(θ(t))

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

其中，是r_w的时间导数，是各运动关节速度向量，是移动机械臂系统的雅克比矩阵。

绝大多数移动机械臂的重复运动规划工作只关注各运动关节角能否回到初始运动的位置，较少涉及初始各关节角异位的重复运动。在一个真实的移动机械臂运动过程中，各运动关节角的初始位置往往是不准确的，包括移动的底座和多杆的机械臂初始位置和期望初始位置之间的各种关节角误差，即运动关节角异位现象。一般来说,应用最为广泛的伪逆控制法不能完成封闭的轨迹任务。

发明内容

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

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

一种面向冗余移动机械臂容错型重复运动规划方法，包括以下步骤：

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

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

其中K∈R^m×(n+2)表示整个移动机械臂系统的雅克比矩阵，

r_wd∈Rⁿ为移动机械臂末端执行器运动的期望轨迹；表示末端执行器运动时的速度向量，r_w为末端执行器实际运动的轨迹；κ＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率；I是单位矩阵，C＝[B；Acosφ]∈R^m×2，φ为移动平台的航向角，分别为左、右轮子的转动角度，r为轮子半径，b为轮子到点P₀的距离，d为点P₀到点P_c的距离；p_c＝[x_c,y_c]^T，x_c、y_c分别为点p_c在水平面上的坐标，θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]^T，θ₁,θ₂,θ₃,θ₄,θ₅,θ₆分别为各关节旋转角度，β_E1＞0，β_E2＞0，β_E3＞0，β_E1、β_E2、β_E3都为可调节常量，p_c(0)、φ(0)、θ(0)分别为p_c、φ、θ的初始状态；为了避免机械臂的移动过程中受到损坏，在移动机械臂运动过程中需要考虑各个关节角转动范围的限制；

q⁺＝[∞,∞,0.5236,0.1745,6.2832,6.2832,6.2832,6.2832]^T,q^-＝-q⁺，

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

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

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

为求解步骤2)中的重复运动规划方案，建立拉格朗日函数

式中，W＝D^TD∈R^(n+2)×(n+2)，h＝D^Tz∈Rⁿ⁺²，λ(t)为拉格朗日乘子向量，λ^T(t)是λ(t)向量的转置，通过拉格朗日函数对各个变量求导，并令其为零，得下述矩阵方程

E＝Mu-p (3)

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

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

本发明的有益效果主要表现在：本发明提供一种基于有限值终态网络的终态吸引优化指标，在各运动关节角初始位置任意给定的情形下，实现移动机械臂各个关节角仍然可以回到期望初始位置，实现重复运动规划任务。相对于已有运动规划方法，该网络求解优化指标使得所提方案具有有限时间收敛的特点，有利于提高计算精度和速度。同时，相比于具有渐近收敛递归神经网络，有限值终态网络具有有限时间收敛特性，该终态网络求解方法采用的激活函数是有限值激活函数，为相关时变问题求解提供了实时计算工具，并且在实际应用时易于实现。

附图说明

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

图2为采用本发明重复规划方案的移动机械臂CAD模型。

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

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

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

图6为六自由度机械臂各关节角变化轨迹。

图7为可移动底座轮子的角度变化轨迹。

图8为移动机械臂整体运动方向角变化轨迹。

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

具体实施方式:

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

参照图1～图9，一种面向冗余移动机械臂容错型重复运动规划方法，包括以下步骤：

1).确定期望轨迹

设定移动机械臂期望回拢的关节角度，将目标路径设定为一个闭合的圆轨迹任务，给定移动机械臂的末端执行器的期望路径为

其中，u＝t,t＝[0,T]，T＝10s为移动机械臂的运动周期，圆的半径为0.1m。

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

θ^*(0)＝[0,0,π/12,π/3,π/3,π/3,π/3,π/3]^T

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

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

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

q^-≤q≤q⁺

其中K∈R^m×(n+2)表示整个移动机械臂系统的雅克比矩阵，

r_wd∈Rⁿ为移动机械臂末端执行器运动的期望轨迹；表示末端执行器运动时的速度向量，r_w为末端执行器实际运动的轨迹；κ＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率。I是单位矩阵，C＝[B；Acosφ]∈R^m×2，φ为移动平台的航向角，分别为左、右轮子的转动角度，r为轮子半径，b为轮子到点P₀的距离，d为点P₀到点P_c的距离；p_c＝[x_c,y_c]^T，x_c、y_c分别为点p_c在水平面上的坐标，θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]^T，θ₁,θ₂,θ₃,θ₄,θ₅,θ₆分别为各关节旋转角度，β_E1＞0，β_E2＞0，β_E3＞0，β_E1、β_E2、β_E3都为常量，p_c(0)、φ(0)、θ(0)分别为p_c、φ、θ的初始状态；

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

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

E＝Mu-p (3)

I为单位矩阵

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

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

用于实现本发明重复规划方案的移动机械臂如图2所示。该机械臂由1个移动基座21，2个可移动轮子，通过六个可活动关节和末端执行器22组成。根据图2展示的移动机械臂模型，该移动机械臂的连杆长度分别为L1＝0.065m，L2＝0.555m，L3＝0.19m，L4＝0.13m，L5＝0.082m，L6＝0.133m。

可移动底座的几何构成和运动学模型如图3所示。图中，p_c表示机械臂与移动平台的连接点，它的坐标为(x_c,y_c,z_c)。由于移动平台只在平面上运动，因此z_c＝0。分别为左、右轮子的转动角度，r为轮子半径，b为轮子到点P₀的距离，d为点P₀到点P_c的距离，φ为移动平台的航向角。

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

当冗余移动机械臂各关节角和移动的底座经过10s后，可移动底座上部的机械臂各关节角和底座移动轮子的关节角均回到了期望的起始位置。所有的关节轨迹基本闭合，其轨迹如图6和图7所示。

图8给出了整个冗余移动机械臂系统运动方向角的变化情况。当轨迹任务完成后，运动方向角回到了期望的初始位置。

为比较传统递归神经网络与有限值终态网络的收敛性能，定义计算误差J_E＝||M(t)u(t)-p(t)||₂。图9给出分别用有限值终态网络和递归神经网络求解重复运动规划问题的误差收敛轨迹。从图中可以看出，以有限值终态网络求解时，当时间t接近0.03秒时，误差收敛精度达到6.664*10^-5,以递归神经网络求解时，系统的动态误差收敛速度较慢。

Claims (1)

1.一种面向冗余移动机械臂容错型重复运动规划方法，其特征在于，包括以下步骤：

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

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

其中K∈R^m×(n+2)表示整个移动机械臂系统的雅克比矩阵，

r_wd∈Rⁿ为移动机械臂末端执行器运动的期望轨迹；表示末端执行器运动时的速度向量，r_w为末端执行器实际运动的轨迹，κ＞0表示位置的参数增益，用来调节末端执行器运动到期望路径的速率；I是单位矩阵，C＝[B；Acosφ]∈R^m×2，φ为移动平台的航向角， 分别为左、右轮子的转动角度，r为轮子半径，b为轮子到点P₀的距离，d为点P₀到点P_c的距离；

p_c＝[x_c,y_c]^T，x_c、y_c分别为点p_c在水平面上的坐标，

θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]^T，θ₁,θ₂,θ₃,θ₄,θ₅,θ₆分别为各关节旋转角度，β_E1＞0，β_E2＞0，β_E3＞0，β_E1、β_E2、β_E3都为可调节常量，p_c(0)、φ(0)、θ(0)分别为p_c、φ、θ的初始状态；为了避免机械臂的移动过程中受到损坏，在移动机械臂运动过程中需要考虑各个关节角转动范围的限制；

q⁺＝[∞,∞,0.5236,0.1745,6.2832,6.2832,6.2832,6.2832]^T,q^-＝-q⁺，

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

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

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

为求解步骤2)中的重复运动规划方案，建立拉格朗日函数

式中，W＝D^TD∈R^(n+2)×(n+2)，h＝D^Tz∈Rⁿ⁺²，λ(t)为拉格朗日乘子向量，λ^T(t)是λ(t)向量的转置，通过拉格朗日函数对各个变量求导，并令其为零，得下述矩阵方程

E＝Mu-p (3)

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

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