CN115958596B - 双冗余机械臂运动规划方法及装置、设备、存储介质 - Google Patents

Abstract

本发明属于机械臂控制技术领域，公开了一种双冗余机械臂运动规划方法及装置、设备、存储介质，通过将多个加速度层指标参数融合到一起，同时充分考虑到机械臂在运动过程中的关节角、关节角速度和关节角加速度的物理极限，可以有效保证机械臂在给定的物理极限范围内工作，建立加速度层多指标优化模型，求解该模型的过程中，将该模型转化为二次规划形式，从而将求解加速度层多指标路径优化转化为求解标准的二次规划问题，从而可以实现冗余机械臂在加速度层上完成给定运动任务，提高双冗余机械臂的运动控制精度，满足冗余机械臂在重力、动力或者力控制层上的运动控制需求。

Description

双冗余机械臂运动规划方法及装置、设备、存储介质

技术领域

本发明属于机械臂控制技术领域，具体涉及一种双冗余机械臂运动规划方法及装置、设备、存储介质。

背景技术

冗余机械臂指的是具有冗余性的机械臂，冗余性是指机械臂的自由度大于其完成给定的工作任务的自由度，该特性使得机械臂具有更高的灵活性去完成复杂的任务，但是也增加了机械臂的控制难度以及机械臂的求解运算量。采用传统的机械臂求解方法不能够实时解决冗余机械臂在执行任务过程中存在的关节角偏移、超越关节角极限等问题，因此冗余机械臂在执行任务过程中的运动精度较低。

近年来，很多学者开始研究如何提高冗余机械臂在执行任务过程中的运动精度问题，目前主要集中在误差补偿、实验分析和理论预测以及运动规划算法方法对关节角偏移问题进行改善。但是在实际的应用过程中，存在各个关节角无法精确定位到所需的轨迹曲线上的情况，关节角偏离目标位置。特别是双冗余机械臂，需要考虑到两个机械臂的协调操作，更是难上加难。

采用传统的方法求解双冗余机械臂问题，存在运算量大以及无解的情况，同时将应用方法应用到实际过程中，仍然存在关节角严重偏离目标位置的情况，导致机械臂运动精度降低。

现有技术中提出一些计算量较小、实时性较强的基于二次型优化的冗余度解析方法。然而目前的二次型优化方法通常是采用机械臂速度层上的逆运动学模型进行目标优化求解，而在一些机械臂需要重力、动力或者力控制层上的问题上，速度层上的运动规划优化算法并不能完全满足冗余机械臂运动控制的需求，导致双冗余机械臂的运动控制精度较低。

发明内容

本发明的目的在于提供一种双冗余机械臂运动规划方法及装置、设备、存储介质，可以实现冗余机械臂在加速度层上完成给定运动任务，从而提高双冗余机械臂的运动控制精度。

本发明第一方面公开一种双冗余机械臂运动规划方法，包括：

根据冗余机械臂关节角、关节角速度和关节角加速度的物理极限，确定多个物理约束条件；

将多个物理约束条件统一到加速度层，获得加速度层不等式；

根据加速度层逆运动学求解模型，对多个加速度层指标参数进行加权融合获得多指标优化模型；

将所述加速度层不等式添加至所述多指标优化模型中，获得左右两个机械臂各自的加速度层运动规划模型；

将左右两个机械臂的加速度层运动规划模型，分别转化为标准的子二次规划模型；

将两个所述子二次规划模型转化成一个标准的二次规划模型；

对所述二次规划模型进行求解，获得求解结果；

根据所述求解结果，对左右两个机械臂进行控制。

本发明第二方面公开一种双冗余机械臂运动规划装置，包括：

确定单元，用于根据冗余机械臂关节角、关节角速度和关节角加速度的物理极限，确定多个物理约束条件；

处理单元，用于将多个物理约束条件统一到加速度层，获得加速度层不等式；

融合单元，用于根据加速度层逆运动学求解模型，对多个加速度层指标参数进行加权融合获得多指标优化模型；

添加单元，用于将所述加速度层不等式添加至所述多指标优化模型中，获得左右两个机械臂各自的加速度层运动规划模型；

转化单元，用于将左右两个机械臂的加速度层运动规划模型，分别转化为标准的子二次规划模型；以及，将两个所述子二次规划模型转化成一个标准的二次规划模型；

求解单元，用于对所述二次规划模型进行求解，获得求解结果；

控制单元，用于根据所述求解结果，对左右两个机械臂进行控制。

本发明第三方面公开一种电子设备，包括存储有可执行程序代码的存储器以及与所述存储器耦合的处理器；所述处理器调用所述存储器中存储的所述可执行程序代码，用于执行第一方面公开的双冗余机械臂运动规划方法。

本发明第四方面公开一种计算机可读存储介质，所述计算机可读存储介质存储计算机程序，其中，所述计算机程序使得计算机执行第一方面公开的双冗余机械臂运动规划方法。

本发明的有益效果在于，所提供的双冗余机械臂运动规划方法及装置、设备、存储介质，通过将多个加速度层指标参数融合到一起，同时充分考虑到机械臂在运动过程中的关节角、关节角速度和关节角加速度的物理极限，可以有效保证机械臂在给定的物理极限范围内工作，建立加速度层多指标优化模型，求解该模型的过程中，将该模型转化为二次规划形式，从而将求解加速度层多指标路径优化转化为求解标准的二次规划问题，从而可以将影响冗余机械臂在加速度层执行任务过程中的重要参数通过不同的权重系数进行融合，可以有效改善冗余机械臂在执行任务过程中存在的关节角偏移以及运动精度不高的问题，实现冗余机械臂在加速度层上完成给定运动任务，提高双冗余机械臂的运动控制精度，满足冗余机械臂在重力、动力或者力控制层上的运动控制需求。

附图说明

此处的附图，示出了本发明所述技术方案的具体实例，并与具体实施方式构成说明书的一部分，用于解释本发明的技术方案、原理及效果。

除非特别说明或另有定义，不同附图中，相同的附图标记代表相同或相似的技术特征，对于相同或相似的技术特征，也可能会采用不同的附图标记进行表示。

图1是一种双冗余机械臂运动规划方法的流程图；

图2是一种加速度层指标参数的权重系数预测方法；

图3是一种双冗余机械臂运动规划装置的结构示意图；

图4是一种电子设备的结构示意图。

附图标记说明：

301、确定单元；302、处理单元；303、融合单元；304、添加单元；305、转化单元；306、求解单元；307、控制单元；401、存储器；402、处理器。

具体实施方式

除非特别说明或另有定义，本文所使用的所有技术和科学术语与所属技术领域的技术人员通常理解的含义相同。在结合本发明的技术方案以现实的场景的情况下，本文所使用的所有技术和科学术语也可以具有与实现本发明的技术方案的目的相对应的含义。本文所使用的“第一、第二…”仅仅是用于对名称的区分，不代表具体的数量或顺序。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

除非特别说明或另有定义，本文所使用的“所述”、“该”为相应位置之前所提及或描述的技术特征或技术内容，该技术特征或技术内容与其所提及的技术特征或技术内容可以是相同的，也可以是相似的。

毫无疑义，与本发明的目的相违背，或者明显矛盾的技术内容或技术特征，应被排除在外。为了便于理解本发明，下面将参照说明书附图对本发明的具体实施例进行更详细的描述。

如图1所示，本发明实施例公开一种双冗余机械臂运动规划方法，该方法的执行主体可为如计算机电脑、笔记本电脑、平板电脑等电子设备，或内嵌于电子设备中的双冗余机械臂运动规划装置，本发明对此不作限定。在本实施例中，以电子设备为例进行阐述。该包括以下步骤S10～S80：

S10、电子设备根据冗余机械臂关节角、关节角速度和关节角加速度的物理极限，确定多个物理约束条件。

在本发明实施例中，对于左右两个机械臂的关节角θ_L/R、关节角速度和关节角加速度/>的物理极限，分别给出三个物理约束条件括θ^- _L/R≤θ_L/R≤θ⁺ _L/R、其中，L/R表示左/右机械臂。

S20、电子设备将多个物理约束条件统一到加速度层获得加速度层不等式。

由于本发明是在加速度层进行模型求解，需要将以上三个物理约束条件用一个不等式统一到加速度层，可以转化为加速度层不等式：其中，/>分别表示机械臂设定的加速度层物理极限，/>

用于定义临界区域和/>关节角进入临界区域时将发生减速，k_α表示关节角在临界区域内减速的快慢，k_α＞0，k_β表示/>的可行域。

S30、电子设备根据加速度层逆运动学求解模型，对多个加速度层指标参数进行加权融合获得多指标优化模型。

在本发明实施例中，根据冗余机械臂的运动规划模型r_L/R＝f(θ_L/R)求二次导数，可得到加速度层逆运动学求解模型其中为雅克比矩阵，/>分别表示机械臂末端执行器的速度矢量和关节角速度矢量，/>表示的是关节角加速度。其中，m和n分别表示任务空间的维数和关节空间的维数。

其中，加速度层指标参数指的是影响冗余机械臂加速度层的运行精度的参数。优选的，多个加速度层指标参数包括最小加速度二范数无穷范数/>以及重复运动优化范数/>

其中，最小加速度二范数指的是关节角加速度的瞬时动能最小值的二范数，无穷范数/>指的是为了均衡能量的分配，保证各个关节角速度和关节角加速度均在一定的合理范围内无穷范数，重复运动优化范数/>用于保证机械臂在执行完给定任务后机械臂的末时关节角位置θ(t)和初始位置θ(0)之间的误差最小化。

具体的，步骤S30可以包括以下步骤S301～S303：

S301、电子设备根据加速度层逆运动学求解模型，对多个加速度层指标参数进行加权融合获得目标优化表达式。

在本发明实施例中，根据加速度层逆运动学求解模型，即加速度层的运动模型，通过权重系数对影响冗余机械臂加速度层的运行精度的三个指标参数进行加权融合处理，获得目标优化表达式，如下式(1)所示：

其中，γ∈(0,1),δ∈(0,1),ν∈(0,1)为最小加速度二范数、无穷范数和重复运动优化范数的权重系数，γ,δ,ν用来权衡各个加速度层指标参数的重要程度，同时为了能够满足无穷范数的特性，需要保证满足ν＝(1-γ-δ)∈(0,1)，其中，C_L/R为重复运动的设定参数，α、β分别表示设计参数。

S302、电子设备采用模糊推理控制理论，根据目标优化表达式，预测各个加速度层指标参数的权重系数。

由于三个加速度层指标参数：最小加速度二范数无穷范数/>和重复运动优化范数/>分别表示影响冗余机械臂不同性能指标的函数，因此在融合的过程中，每个指标参数的权重系数也需要根据实际需要进行调整。在本发明实施例中，权重系数可以通过模糊推理的运动控制预测方法进行预测。具体过程为：

采用模糊控制理论，分别将其中两个加速度层指标参数(如γ,ν)定义为输入，然后根据目标优化表达式模糊化其隶属函数，再根据控制规则进行模糊逻辑推理，以及进行隶属函数清晰化处理，获得对应的权重系数输出。在确定其中两个加速度层指标参数的权重系数γ,ν之后，可以计算剩余一个加速度层指标参数的权重系数δ＝(1-γ-ν)。

其中，当模糊逻辑推理出加速度层指标参数满足程度较好时，其权重系数取较低的值，当加速度层指标参数满足程度不好时，其权重系数取较大的值，其模糊推理流程如图2所示。

S303、电子设备将各个加速度层指标参数的权重系数代入目标优化表达式，获得多指标优化模型。

S40、电子设备将加速度层不等式添加至多指标优化模型中，获得左右两个机械臂各自的加速度层运动规划模型。

其中，左右两个机械臂各自的加速度层运动规划模型通过以下公式(1)-(3)表示：

S50、电子设备将左右两个机械臂的加速度层运动规划模型，分别转化为标准的子二次规划模型。

令其中/>R表示姿态矩阵，可将上述公式(1)-(3)运动模型转化为两个标准的子二次规划模型为：

s.t.B_L/Rx_L/R＝b_L/R (5)

E_L/Rx_L/R≤d_L/R (6)

其中，

B_L/R＝[J_L/R 0]∈R^m×(n+1),d＝0∈R²ⁿ，/>其中/>是一个无穷大常量，用来替换+∞。

S60、电子设备将两个子二次规划模型转化成一个标准的二次规划模型。

将上述公式(4)-(7)子二次规划模型转化为一个标准的二次规划模型，如下式(8)-(11)所示：

s.t.Qx＝h (9)

Ax≤e (10)

x^-≤x≤x⁺ (11)

其中

e＝0∈R⁴ⁿ，

进一步优选的，在机械臂的实际应用过程中，由于存在扰动和计算机计算误差等因素会影响到模型的精度，为了进一步提高模型的运算精度，可以对步骤S30中的加速度层逆运动学求解模型引入速度和位置误差的反馈系统，即为：其中K_p和K_v分别表示速度误差和位置误差反馈的增广矩阵，维数均为m×m。

基于此，上述公式(2)则更新为以下公式(12)：

以及，步骤S50中将左右两个机械臂的加速度层运动规划模型分别转化的两个标准的子二次规划模型时，其中则更新为：

以及，步骤S60中将两个子二次规划模型转化成一个标准的二次规划模型时，其中则更新为：

S70、电子设备对二次规划模型进行求解，获得求解结果。

步骤S70中优选的，可以将二次规划模型转化成线性变分不等式问题，然后采用基于线性变分不等式的原对偶神经网络，对线性变分不等式问题进行求解，获得求解结果。

具体的，求一个原对偶决策变量y^*∈Ω使得该变量满足线性变分不等式问题(y-y^*)^T(My^*+g)≥0，其中原对偶决策变量y和其上下极限为：

其中u∈R^2m和v∈R⁴ⁿ分别表示代表的等式(9)和不等式(10)。同时，扩展矩阵M和向量g定义为：

最后采用基于线性变分不等式的原对偶神经网络的描述，即动力学系统，来求解二次规划(Quadratic Programming，QP)问题：

基于此，可以在求解的基础上采用神经网络求解器对求解的二次规划模型的结果进行优化，进一步提高准确性。

S80、电子设备根据求解结果，对左右两个机械臂进行控制。

可见实施本发明实施例，通过将多个加速度层指标参数融合到一起，同时充分考虑到机械臂在运动过程中的关节角、关节角速度和关节角加速度的物理极限，可以有效保证机械臂在给定的物理极限范围内工作，建立加速度层多指标优化模型，求解该模型的过程中，将该模型转化为二次规划形式，从而将求解加速度层多指标路径优化转化为求解标准的二次规划问题，从而可以将影响冗余机械臂在加速度层执行任务过程中的重要参数通过不同的权重系数进行融合，可以有效改善冗余机械臂在执行任务过程中存在的关节角偏移以及运动精度不高的问题，实现冗余机械臂在加速度层上完成给定运动任务，提高双冗余机械臂的运动控制精度，满足冗余机械臂在重力、动力或者力控制层上的运动控制需求。

如图3所示，本发明实施例公开一种双冗余机械臂运动规划装置，包括确定单元301、处理单元302、融合单元303、添加单元304、转化单元305、求解单元306、控制单元307，其中，

确定单元301，用于根据冗余机械臂关节角、关节角速度和关节角加速度的物理极限，确定多个物理约束条件；

处理单元302，用于将多个物理约束条件统一到加速度层，获得加速度层不等式；

融合单元303，用于根据加速度层逆运动学求解模型，对多个加速度层指标参数进行加权融合获得多指标优化模型；

添加单元304，用于将加速度层不等式添加至多指标优化模型中，获得左右两个机械臂各自的加速度层运动规划模型；

转化单元305，用于将左右两个机械臂的加速度层运动规划模型，分别转化为标准的子二次规划模型；以及，将两个子二次规划模型转化成一个标准的二次规划模型；

求解单元306，用于对二次规划模型进行求解，获得求解结果；

控制单元307，用于根据求解结果，对左右两个机械臂进行控制。

作为一种可选的实施方式，求解单元306可以包括以下未图示的子单元：

转化子单元，用于将二次规划模型转化成线性变分不等式问题；

求解子单元，用于采用基于线性变分不等式的原对偶神经网络，对线性变分不等式问题进行求解，获得求解结果。

作为一种可选的实施方式，融合单元303可以包括以下未图示的子单元：

融合子单元，用于根据加速度层逆运动学求解模型，对多个加速度层指标参数进行加权融合获得目标优化表达式；

预测子单元，用于采用模糊推理控制理论，预测各个加速度层指标参数的权重系数；

代入子单元，用于将各个加速度层指标参数的权重系数代入目标优化表达式，获得多指标优化模型。

如图4所示，本发明实施例公开一种电子设备，包括存储有可执行程序代码的存储器401以及与存储器401耦合的处理器402；

其中，处理器402调用存储器401中存储的可执行程序代码，执行上述各实施例中描述的双冗余机械臂运动规划方法。

本发明实施例还公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行上述各实施例中描述的双冗余机械臂运动规划方法。

以上实施例的目的，是对本发明的技术方案进行示例性的再现与推导，并以此完整的描述本发明的技术方案、目的及效果，其目的是使公众对本发明的公开内容的理解更加透彻、全面，并不以此限定本发明的保护范围。

以上实施例也并非是基于本发明的穷尽性列举，在此之外，还可以存在多个未列出的其他实施方式。在不违反本发明构思的基础上所作的任何替换与改进，均属本发明的保护范围。

Claims (4)

1.双冗余机械臂运动规划方法，其特征在于，包括：

根据冗余机械臂关节角、关节角速度和关节角加速度的物理极限，确定多个物理约束条件；

将多个物理约束条件统一到加速度层，获得加速度层不等式；

根据加速度层逆运动学求解模型，对三个加速度层指标参数进行加权融合获得目标优化表达式；

采用模糊控制理论，分别将其中两个加速度层指标参数定义为输入，根据目标优化表达式模糊化其隶属函数，再根据控制规则进行模糊逻辑推理，以及进行隶属函数清晰化处理，获得对应的权重系数输出；

确定其中两个加速度层指标参数的权重系数之后，计算剩余一个加速度层指标参数的权重系数；其中，三个加速度层指标参数的权重系数总和为1；

将各个加速度层指标参数的权重系数代入所述目标优化表达式，获得多指标优化模型；

将所述加速度层不等式添加至所述多指标优化模型中，获得左右两个机械臂各自的加速度层运动规划模型；

将左右两个机械臂的加速度层运动规划模型，分别转化为标准的子二次规划模型；

将两个所述子二次规划模型转化成一个标准的二次规划模型；

将所述二次规划模型转化成线性变分不等式问题；

采用基于线性变分不等式的原对偶神经网络，对所述线性变分不等式问题进行求解，获得求解结果；

根据所述求解结果，对左右两个机械臂进行控制；

其中，三个加速度层指标参数包括最小加速度二范数无穷范数/>以及重复运动优化范数/>所述多指标优化模型通过以下公示表示：

其中，θ_L/R、分别代表左/右机械臂关节角、关节角速度和关节角加速度，γ∈(0，1)、δ∈(0，1)、ν∈(0，1)分别为最小加速度二范数、重复运动优化范数和无穷范数的权重系数，ν＝(1-γ-δ)∈(0,1)，/>C_L/R为重复运动的设定参数，α、β分别表示设计参数；

其中，多个物理约束条件包括θ^- _L/R≤θ_L/R≤θ⁺ _L/R、 其中，θ^- _L/R、θ⁺ _L/R分别是关节角θ_L/R的物理极限，/>分别是关节角速度/>的物理极限，/>分别是关节角加速度/>的物理极限；所述加速度层不等式为/>其中/>分别表示机械臂设定的加速度层物理极限， 其中，λ∈[0,1]用于定义临界区域和/>关节角进入临界区域时将发生减速，k_α表示关节角在临界区域内减速的快慢，k_α＞0，k_β表示/>的可行域；

其中，所述加速度层逆运动学求解模型的表达式为：

其中K_p和K_v分别表示速度误差和位置误差反馈的增广矩阵，维数均为m×m，J是J(θ(t))的缩写，代表雅克比矩阵。

2.双冗余机械臂运动规划装置，其特征在于，包括：

确定单元，用于根据冗余机械臂关节角、关节角速度和关节角加速度的物理极限，确定多个物理约束条件；

处理单元，用于将多个物理约束条件统一到加速度层，获得加速度层不等式；

融合单元，用于根据加速度层逆运动学求解模型，对三个加速度层指标参数进行加权融合获得多指标优化模型；

添加单元，用于将所述加速度层不等式添加至所述多指标优化模型中，获得左右两个机械臂各自的加速度层运动规划模型；

转化单元，用于将左右两个机械臂的加速度层运动规划模型，分别转化为标准的子二次规划模型；以及，将两个所述子二次规划模型转化成一个标准的二次规划模型；

求解单元，用于对所述二次规划模型进行求解，获得求解结果；

控制单元，用于根据所述求解结果，对左右两个机械臂进行控制；

其中，所述求解单元包括：

转化子单元，用于将所述二次规划模型转化成线性变分不等式问题；

求解子单元，用于采用基于线性变分不等式的原对偶神经网络，对所述线性变分不等式问题进行求解，获得求解结果；

其中，所述融合单元包括：

融合子单元，用于根据加速度层逆运动学求解模型，对三个加速度层指标参数进行加权融合获得目标优化表达式；

预测子单元，用于采用模糊控制理论，分别将其中两个加速度层指标参数定义为输入，根据目标优化表达式模糊化其隶属函数，再根据控制规则进行模糊逻辑推理，以及进行隶属函数清晰化处理，获得对应的权重系数输出；以及，确定其中两个加速度层指标参数的权重系数之后，计算剩余一个加速度层指标参数的权重系数；其中，三个加速度层指标参数的权重系数总和为1；

代入子单元，用于将各个加速度层指标参数的权重系数代入所述目标优化表达式，获得多指标优化模型；

其中，三个加速度层指标参数包括最小加速度二范数无穷范数/>以及重复运动优化范数/>所述多指标优化模型通过以下公示表示：

其中，θ_L/R、分别代表左/右机械臂关节角、关节角速度和关节角加速度，γ∈(0，1)、δ∈(0，1)、ν∈(0，1)分别为最小加速度二范数、重复运动优化范数和无穷范数的权重系数，ν＝(1-γ-δ)∈(0,1)，/>C_L/R为重复运动的设定参数，α、β分别表示设计参数；

其中，多个物理约束条件包括θ^- _L/R≤θ_L/R≤θ⁺ _L/R、 其中，θ^- _L/R、θ⁺ _L/R分别是关节角θ_L/R的物理极限，/>分别是关节角速度/>的物理极限，/>分别是关节角加速度/>的物理极限；所述加速度层不等式为/>其中/>分别表示机械臂设定的加速度层物理极限， 其中，λ∈[0,1]用于定义临界区域和/>关节角进入临界区域时将发生减速，k_α表示关节角在临界区域内减速的快慢，k_α＞0，k_β表示/>的可行域；

其中，所述加速度层逆运动学求解模型的表达式为：

其中K_p和K_v分别表示速度误差和位置误差反馈的增广矩阵，维数均为m×m，J是J(θ(t))的缩写，代表雅克比矩阵。

3.电子设备，其特征在于，包括存储有可执行程序代码的存储器以及与所述存储器耦合的处理器；所述处理器调用所述存储器中存储的所述可执行程序代码，用于执行权利要求1所述的双冗余机械臂运动规划方法。

4.计算机可读存储介质，其特征在于，所述计算机可读存储介质存储计算机程序，其中，所述计算机程序使得计算机执行权利要求1所述的双冗余机械臂运动规划方法。