CN110103225A - 一种数据驱动的机械臂重复运动控制方法与装置 - Google Patents

Abstract

本发明提供了一种数据驱动的机械臂重复运动控制方法与装置，整个系统均采用模型未知机械臂的估计雅可比矩阵来协助计算，包括如下步骤：1)获取冗余度机械臂的速度信息和关节角速度信息；2)根据所述速度信息和关节角速度信息得出估计雅可比矩阵；3)应用所述估计雅可比矩阵设计二次型的重复运动性能指标，生成二次型优化冗余度解析方案；4)将所述机械臂重复运动二次规划方案转化为标准二次规划的形式；5)运用二次规划求解器进行求解以确定冗余度机械臂的控制信号；6)将所述控制信号传递给下位机控制器驱动机械臂运动；本发明为模型未知机械臂提供了可靠的模型信息估算方法和重复运动控制方法，并在控制机械臂的同时保持较高的控制精度。

Description

一种数据驱动的机械臂重复运动控制方法与装置

技术领域

本发明涉及冗余度机械臂运动规划及控制领域，具体涉及一种冗余度 机械臂的逆运动学求解方法。

背景技术

冗余度机械臂具有自由度大于任务空间所需最少自由度的特性，广泛 应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机 械臂的逆运动学问题是指已知冗余度机械臂末端位置，确定冗余度机械臂 的关节角问题。当冗余度机械臂末端任务为一个封闭曲线时，即使末端执 行器回到了初始位置，但其各个关节可能与初始状态产生偏离，这种现象 叫做关节角偏差，或称非重复运动问题。如果冗余度机械臂不能实现重复运动，则在运行过程中可能会产生不可预料的情况，甚至会出现损害冗余 度机械臂甚至危害周围人员人身安全的问题；而重复运动规划方案就是要 设计适当的性能参数，使得冗余度机械臂末端执行完封闭曲线任务时，各 个关节角都能够回到其相对应的初始位置。

值得指出的是，现存的绝大部分的机械臂控制方法仅仅针对模型已知 的机械臂，并不适用模型未知的机械臂。在工业生产中，会出现机械臂负 载未知或者机械臂结构未知的情况，此时现有的控制方法无法使机械臂准 确完成所给定任务从而导致任务失败或机械臂损坏。而数据驱动技术可以 被应用于机械臂结构信息的获取，这给模型未知机械臂的控制提供了重要 的技术支撑。本发明提出一种数据驱动的机械臂重复运动控制方法，能够促使模型未知机械臂精确地完成重复运动任务，并有效地消除执行任务时 产生的关节角偏差和位置误差，这一优势对于工业生产是非常具有实际意 义的。综上所述，本发明专利具有新颖性与实用性。

发明内容

本发明所要解决的技术问题是提供一种数据驱动的机械臂重复运动控 制方法，旨在精确地实现模型未知的冗余度机械臂的重复运动。

本发明实施例第一方面提供了一种数据驱动的雅可比矩阵估计方法；

所述雅可比矩阵的估计方法包括：

获取所述模型未知的机械臂的速度信息和关节角速度信息；

通过雅可比矩阵迭代等式对雅可比矩阵进行估计；

所述雅可比矩阵迭代等式为：

其中，为机械臂的估计雅可比矩阵；为的时间导数；为冗余度机械臂 末端执行器的速度；为冗余度机械臂的关节角速度；∈＞0为控制算法收 敛速度的设计参数；上标T为向量或者矩阵的转置。

本发明实施例第二方面提供了一种数据驱动的机械臂重复运动控制方 法的设计步骤，具体包括：

设置最优化性能指标为冗余度机械臂的重复运动，通过估计雅可比矩 阵、关节角度极限和关节角速度极限确定二次型优化方案；

将所述二次型优化方案转换为标准二次规划；

对所述标准二次规划用二次规划求解器进行求解，得到求解结果；

根据所述求解结果确定所述冗余度机械臂的控制信号。

本发明实施例第三方面提供了一种数据驱动的机械臂重复运动控制方 法的二次规划优化方案；

所述二次型优化方案为：

设计的最小化性能指标为冗余度机械臂的重复运动，受约束于雅可比 等式、关节角度极限、关节角速度极限，即为最小化 受约束于雅可比等式关节角度极 限θ^-≤θ≤θ⁺，关节角速度极限上标表示矩阵的伪逆，θ表 示冗余度机械臂的关节角度，θ₀表示冗余度机械臂的初始关节角度，表示 冗余度机械臂的关节角速度，γ＞0表示关节角度误差的反馈系数，表示 设定的末端执行器的期望速度，表示冗余度机械臂的估计雅可比矩阵，I 表示单位矩阵，θ^±表示关节角度的上下限，表示冗余度机械臂的关节角 速度的上下限，表示本发明中不同于现存重复运动方法的重要特 征，起到使末端执行器的位置误差与冗余度机械臂的关节角度误差解耦的 作用。

本发明实施例第四方面提供了一种数据驱动的机械臂重复运动控制方 法的二次规划优化方案；

所述二次规划为：

设计其性能指标为最小化受约束于其中表示关节角度误差的反馈系数，β^±为冗余度机械臂的关节角速度上下限，它们的第i个元 素分别定义为i表 示关节序号，i＝1，2，3...n，正常数k_α用来调节和保证关节角速度有足够 大的可行域。

本发明实施例第五方面提供了一种数据驱动的机械臂重复运动控制装 置；所述控制装置通过控制装置的各模块之间的信息传递完成从求解器到 冗余度机械臂重复运动的控制；

所述控制装置包括：

冗余度机械臂信息获取模块，用于获取所述模型未知的机械臂的位置 信息、速度信息、关节角信息和关节角速度信息；

估计雅可比矩阵构建模块，用于根据所述机械臂的速度信息和关节角 速度对雅可比矩阵进行估计；

轨迹信息获取模块，用于获取所述冗余度机械臂设定的轨迹信息，并 将轨迹信息传输给二次规划求解器；

等式构建模块，用于根据所述机械臂的速度信息和所述轨迹信息，按 照预设规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；

控制信号确定模块，用于在所述雅可比矩阵等式、关节角度极限和关 节角速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目 标冗余度机械臂的控制信号；

信息传递模块，用于获取所述目标冗余度机械臂的控制信号，并将控 制信号传递给下位机；

冗余度机械臂控制模块，用于根据所述目标冗余度机械臂的控制信号 控制目标冗余度机械臂，使目标冗余度机械臂实现重复运动。

附图说明

图1为本发明的流程图；

图2为实现本发明的冗余度机械臂三维模型图；

图3为实现本发明应用下的冗余度机械臂重复运动过程图；

图4为实现本发明应用下的冗余度机械臂关节角度和关节角速度变化 图；

图5为实现本发明应用下的冗余度机械臂末端执行器位置误差和速度 误差变化图；

图6为实现本发明应用下的冗余度机械臂雅可比矩阵估计误差；

具体实施方式

下面结合附图对本发明做进一步的说明。

图1为本发明的流程图；获取冗余度机械臂的速度信息和关节角速度 信息；然后根据所述速度信息和关节角速度信息得出估计雅可比矩阵；接 着设计二次型的重复运动性能指标，生成二次型优化解析方案；然后将二 次规划方案转化为二次规划的形式；接着运用二次规划求解器进行求解； 最后下位机控制器根据求解结果控制冗余度机械臂的运动。

图2为实现本发明的冗余度机械臂为一个空间七自由度的Sawyer机械 臂。该冗余度机械臂由七个连杆组成，通过关节一(1)、关节二(2)、关 节三(3)、关节四(4)、关节五(5)、关节六(6)、关节七(7)和末端执 行器(8)组成。

本发明的雅可比矩阵的估计方法包括：

获取所述模型未知的机械臂的速度信息和关节角速度信息；

通过雅可比矩阵迭代等式对雅可比矩阵进行估计；

所述雅可比矩阵迭代等式为：

其中，为机械臂的估计雅可比矩阵；为的时间导数；为冗余度机械臂 末端执行器的速度；为冗余度机械臂的关节角速度；∈＞0为控制算法收 敛速度的设计参数；上标T为向量或者矩阵的转置。

本发明设计的重复运动规划方案为

最小化：

约束条件：

θ^-≤θ≤θ⁺ (3)

其中，为重复运动的性能指标，上标T表示矩阵和向量的转置，上标表示矩阵的伪逆，θ表示冗余度机械臂的关节角度， θ₀表示冗余度机械臂的初始关节角度，表示冗余度机械臂的关节角速度， k＞0表示重复运动的反馈系数，表示设定的期望轨迹，J表示冗余度机 械臂的雅可比矩阵，I表示单位矩阵，表示冗余度机械臂的关节角速度的 上下限，θ^±表示冗余度机械臂的关节角度的上下限。

带物理约束的冗余度机械臂的二次型优化方案(1)-(4)可描述为如下的 二次规划方案：

最小化：

约束条件：

其中，γ＞0表示重复运动的反馈系数，φ＝θ，φ₀＝θ₀，β^±为冗余度机械 臂的关节角速度上下限，它们的第i个元素分别定义为i表示关节序号，i＝1_，2，3...n，正常数k_α用来调节和保证关节角速度足够大的可行域。下 面给出一个可行的求解器的示例(本发明不限于该求解器示例)，通过使用 梯度下降法加上速度补偿的方法，上述问题又可以等价为分段线性投影方 程组：

其中，γ＞0表示末端执行器位置误差反馈系数，ρ表示递归辅助参数，表示速度补偿，ξ＞0表示控制收敛的系数，0≤||n||≤n₀，n表示一个有界 零均值独立同分布随机噪声，n₀为一个很小的值表示噪声的边界，表示被 噪声n污染的关节角速度，λ表示位置误差的反馈系数。另外，设定

本发明有两个特点，首先本发明面向模型未知的机械臂，通过数据驱 动技术存储机械臂的模型信息，并将学习到的模型信息用于控制机械臂， 将学习和控制结合，提供了更加有效、全面的控制方案。另外，本发明的 重复运动方案的理论位置误差等于零，并且与关节误差解耦，克服了以往 的所有控制方法例如《一种冗余度机械臂重复运动规划的方法》(申请号： 201010264141.6)存在理论位置误差的缺点，为重复运动控制方案提供了 更加精确的控制方法，对提升工业生产的质量和效率有极大的意义。

通过上位机的二次规划求解器得到该二次规划(8)的解后，再将结果 转化为电机驱动所需要的控制信号，并通过该冗余度机械臂的控制器来驱 动冗余度机械臂各关节电机运动，从而驱动各关节电机使冗余度机械臂重 复运动。

现结合一个具体的实例操作对本发明的工作流程进行如下说明。

利用MATLAB软件对本发明方法进行七自由度的Sawyer冗余度机械臂重 复运动轨迹追踪的实验仿真。具体参数设置如下：ξ＝λ＝2000，γ＝1， n₀＝0.005，k_α＝10，冗余度机械臂的关节角度极限为 θ⁺＝-θ^-＝[2.5，2.5，2.5，2.5，2.5，2.5]弧度，冗余度机械臂的关节角速 度极限为弧度/秒，执行任 务时间设为20秒，执行任务为跟踪三尖瓣路径，初始的角度设置为 φ₀＝[0，-0.5，-0.5，-1.5，2，-0.5，0]弧度。通过二次规划求解器求解，将计 算得到的结果再传送给冗余度机械臂控制器从而控制冗余度机械臂的运 动。本发明通过分析运行结果，可以得到在完成冗余度机械臂轨迹任务时 位置误差很小，非常顺利地完成了所给定的任务。

图3 为实现本发明应用下的冗余度机械臂获取估计雅可比矩阵并进行重复运动的过程图。图3 中，子图1，2，3，4 为冗余度机械臂雅可比矩阵估计过程，此过程初期机械臂存在抖动现象，处于获取估计雅可比矩阵状态，进而学习机械臂结构。子图5，6，7，8，9 为应用所述估计雅可比矩阵控制机械臂重复运动的过程，其中子图5 和子图9 体现了在一个任务循环中，结束时冗余度机械臂的状态和初始冗余度机械臂的状态完全相同，最终完成了重复运动任务。

图4为实现本发明应用下的冗余度机械臂重复运动过程中关节角度和 关节角速度变化图。从图4看出，关节角度和关节误差在一个任务循环结 束时回到了初始状态，并且也保证处于关节极限的范围内，这一点满足重 复运动的要求。

图5为实现本发明应用下的冗余度机械臂重复运动过程中末端执行器 位置误差和速度误差变化图。从图5看出，冗余度机械臂末端执行器位置 误差小于1.5×10^-4米，冗余度机械臂末端执行器速度误差小于4×10^-3米/ 秒，冗余机械臂精确地完成了所给任务。

图6为实现本发明应用下的冗余度机械臂雅可比矩阵估计误差。从图 6看出冗余度机械臂雅可比矩阵估计误差收敛速度非常快，并在0.5秒后 达到稳定。稳定时，冗余度机械臂雅可比矩阵估计误差处于10^-3的数量级， 说明了估计雅可比矩阵的精确度。

Claims (6)

1.一种数据驱动的机械臂重复运动控制方法与装置，其特征在于，根据模型未知的机械臂的速度信息和关节角速度信息对雅可比矩阵进行估计；

所述雅可比矩阵的估计方法包括：

获取所述模型未知的机械臂的速度信息和关节角速度信息；

通过雅可比矩阵迭代等式对雅可比矩阵进行估计；

所述雅可比矩阵迭代等式为：

其中，为机械臂的估计雅可比矩阵；为的时间导数；为冗余度机械臂末端执行器的速度；为冗余度机械臂的关节角速度；∈＞0为控制算法收敛速度的设计参数；上标^T为向量或者矩阵的转置。

2.根据权利要求1中所述的一种数据驱动的机械臂重复运动控制方法与装置，其特征在于，根据所述估计雅可比矩阵，利用二次型优化和标准二次规划方法确定冗余度机械臂的控制信号具体包括：

设置最优化性能指标为冗余度机械臂的重复运动，通过估计雅可比矩阵、关节角度极限和关节角速度极限确定二次型优化方案；

将所述二次型优化方案转换为标准二次规划；

对所述标准二次规划用二次规划求解器进行求解，得到求解结果；

根据所述求解结果确定所述冗余度机械臂的控制信号。

3.根据权利要求2所述的一种数据驱动的机械臂重复运动控制方法与装置，其特征在于，所述二次型优化方案为：

设计的最小化性能指标为冗余度机械臂的重复运动，受约束于雅可比等式、关节角度极限、关节角速度极限，即为最小化受约束于雅可比等式关节角度极限θ^-≤θ≤θ⁺，关节角速度极限上标表示矩阵的伪逆，θ表示冗余度机械臂的关节角度，θ₀表示冗余度机械臂的初始关节角度，表示冗余度机械臂的关节角速度，γ＞0表示关节角度误差的反馈系数，表示设定的末端执行器的期望速度，表示冗余度机械臂的估计雅可比矩阵，I表示单位矩阵，θ^±表示关节角度的上下限，表示冗余度机械臂的关节角速度的上下限，表示本发明中不同于现存重复运动方法的重要特征，起到使末端执行器的位置误差与冗余度机械臂的关节角度误差解耦的作用。

4.根据权利要求3所述的一种数据驱动的机械臂重复运动控制方法与装置，其特征在于，可将所述的二次型优化方案转化为二次规划，设计其性能指标为最小化受约束于其中φ＝θ，φ₀＝θ₀，γ＞0表示关节角度误差的反馈系数，β^±为冗余度机械臂的关节角速度上下限，它们的第i个元素分别定义为i表示关节序号，i＝1，2，3...n，正常数k_α用来调节和保证关节角速度有足够大的可行域。

5.根据权利要求2所述的一种数据驱动的机械臂重复运动控制方法与装置，其特征在于，可将所述二次规划求解器求解结果转化为电机驱动所需要的控制信号，驱动各关节电机使冗余度机械臂重复运动。

6.根据权利要求5所述的一种数据驱动的机械臂重复运动控制方法与装置，其特征在于，通过控制装置的各模块之间的信息传递完成从求解器到冗余度机械臂重复运动的控制；

所述控制装置包括：

冗余度机械臂信息获取模块，用于获取所述模型未知的机械臂的位置信息、速度信息、关节角信息和关节角速度信息；

估计雅可比矩阵构建模块，用于根据所述机械臂的速度信息和关节角速度对雅可比矩阵进行估计；

轨迹信息获取模块，用于获取所述冗余度机械臂设定的轨迹信息，并将轨迹信息传输给二次规划求解器；

等式构建模块，用于根据所述机械臂的速度信息和所述轨迹信息，按照预设规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；

控制信号确定模块，用于在所述雅可比矩阵等式、关节角度极限和关节角速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；

信息传递模块，用于获取所述目标冗余度机械臂的控制信号，并将控制信号传递给下位机；

冗余度机械臂控制模块，用于根据所述目标冗余度机械臂的控制信号控制目标冗余度机械臂，使目标冗余度机械臂实现重复运动。