CN109760063B - 并联机器人的控制方法、装置、设备及存储介质 - Google Patents

Abstract

本申请公开了一种并联机器人的控制方法、装置、设备及存储介质。其中，方法包括对给定的末端执行器运动路径进行逆运动学运算，获取并联机器人各关节的关节角度曲线；根据并联机器人的动力学模型，获取并联机器人各驱动电机的负载转矩曲线；至少分别对各关节角度曲线及各负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对并联机器人进行运动控制。分别对各关节角度曲线及各负载转矩曲线的特征点进行样条插值处理，将负载的非线性模型转化为了简单的、利于处理的模式，降低了计算量，随着计算量的降低，其计算速度得以提升，进而提高了机器人的动态响应能力，以保证机器人动作的高速性。

Description

并联机器人的控制方法、装置、设备及存储介质

技术领域

本发明一般涉及机器人技术领域，具体涉及一种并联机器人的控制方法、装置、设备及存储介质。

背景技术

现阶段对DELTA(一种并联机器人)机器人控制器的主要研究一般是以运动学和动力学模型为基础，并结合常用的控制方法，例如PID(Proportion、Integral、Derivative；比例、积分、微分)控制方法等，再将设计好的控制器直接与软件生成的机器人机械结构进行仿真，最后对仿真的结果进行对比，确定控制器是否可用。但实际上机器人是由位于静平台(或称为定平台)上的电机驱动的，其并非简单的机械结构，因此如何规避或降低复杂的机器人非线性模型求解，并提高系统的稳态性能及动态性能，是本领域技术人员研究的一个重点方向。

发明内容

本申请期望提供一种并联机器人的控制方法、装置、设备及存储介质，用以降低机器人非线性模型求解计算量，提高机器人的动态响应能力。

第一方面，本发明提供一种并联机器人的控制方法，包括以下步骤：

对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线；

根据所述并联机器人的动力学模型，获取所述并联机器人各驱动电机的负载转矩曲线；

至少分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对所述并联机器人进行运动控制；所述特征点至少为所述关节角度曲线及所述负载转矩曲线对应于所述末端执行器运动路径上，加速度改变的点和/或速度方向改变的点。

进一步地，所述并联机器人为DELTA机器人。

进一步地，根据以下关系式获取所述关节角度曲线：

A_i＝(l_a-Δr)²+2η_i(l_a-Δr)+ξ；

B_i＝4l_az；

C_i＝(l_a+Δr)²-2η_i(l_a+Δr)²+ξ；

Δr＝R-r；

其中，θ为关节角度，l_a为主动臂长度，l_b为从动臂长度，(x,y,z)为末端执行器运动路径的坐标，R为静平台外接圆半径，r为动平台外接圆半径。

进一步地，根据以下关系式获取所述负载转矩曲线：

其中，F＝[τ₁ τ₂ τ₃]，τ_i为负载转矩，I_rf为主动臂转动惯量，M为从动臂及负载的等效质量，

为执行器末端加速度,

为执行器末端速度，

为末端执行器重力，

为主动臂重力，J为雅可比矩阵。

第二方面，本发明提供一种并联机器人的控制装置，包括：

关节角度曲线获取单元，用于对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线；

负载转矩曲线获取单元，用于根据所述并联机器人的动力学模型，获取所述并联机器人各驱动电机的负载转矩曲线；

控制单元，用于至少分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对所述并联机器人进行运动控制；所述特征点至少为所述关节角度曲线及所述负载转矩曲线对应于所述末端执行器运动路径上，加速度改变的点和/或速度方向改变的点。

进一步地，所述并联机器人为DELTA机器人。

进一步地，所述关节角度曲线获取单元，用于根据以下关系式获取所述关节角度曲线：

A_i＝(l_a-Δr)²+2η_i(l_a-Δr)+ξ；

B_i＝4l_az；

C_i＝(l_a+Δr)²-2η_i(l_a+Δr)²+ξ；

Δr＝R-r；

其中，θ为关节角度，l_a为主动臂长度，l_b为从动臂长度，(x,y,z)为末端执行器运动路径的坐标，R为静平台外接圆半径，r为动平台外接圆半径。

进一步地，所述负载转矩曲线获取单元，用于根据以下关系式获取所述负载转矩曲线：

其中，F＝[τ₁ τ₂ τ₃]，τ_i为负载转矩，I_rf为主动臂转动惯量，M为从动臂及负载的等效质量，

为执行器末端加速度,

为执行器末端速度，

为末端执行器重力，

为主动臂重力，J为雅可比矩阵。

第三方面，本发明提供一种计算机设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述的并联机器人的控制方法。

第四方面，本发明提供一种可读存储介质，存储有计算机程序，所述计算机程序被执行时实现上述的并联机器人的控制方法。

上述方案，分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，将负载的非线性模型转化为了简单的、利于处理的模式，降低了计算量，随着计算量的降低，其计算速度得以提升，进而提高了机器人的动态响应能力，以保证机器人动作的高速性。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：

图1为本发明实施例提供的并联机器人的控制方法的流程图；

图2为本发明实施例提供的DELTA机器人的原理图；

图3为本发明实施例提供的末端执行器运动路径图；

图4为本发明实施例提供的关节角度曲线图；

图5为本发明实施例提供的关节2负载转矩曲线样条插值前后对比图；

图6为本发明实施例提供的关节2的关节角度曲线理论值与样条插值前后的实际值的对比图；

图7为图6的放大图；

图8为本发明实施例提供的并联机器人的控制装置的原理图；

图9为本发明实施例提供的计算机设备的原理图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

如图1所示，本发明实施例提供的并联机器人的控制方法，包括以下步骤：

S1：对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线；

对于并联机器人，由于闭链约束的存在，通过机器人运动学反解，也即逆运动学运算，即当已知机构输入件的位置时可以求解出机构主动件的位置，在给定动平台中心位置的坐标时，通过计算得出定平台与主动臂之间的角度，在末端执行器沿末端执行器运动路径进行运动时，该角度是在动态变化的，其可以用关节角度曲线来表示。

S2：根据所述并联机器人的动力学模型，获取所述并联机器人各驱动电机的负载转矩曲线；

不同的并联机器人结构具有不同的动力学模型，可以根据并联机器人具体结构选择对应的动力学模型，对选定的动力学模型进行求解即可以得到各驱动电机的负载转矩，其可以用负载转矩曲线来表示。

S3：至少分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对所述并联机器人进行运动控制；所述特征点至少为所述关节角度曲线及所述负载转矩曲线对应于所述末端执行器运动路径上，加速度改变的点和/或速度方向改变的点。

上述方案，分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，将负载的非线性模型转化为了简单的、利于处理的模式，降低了计算量，随着计算量的降低，其计算速度得以提升，进而提高了机器人的动态响应能力，以保证机器人动作的高速性。

此外，机器人的末端执行器在加速度、运动方向(也即速度方向)等物理量发生变化时，可能出现刚体抖动、冲击等现象，这些加速度和速度改变的点所对应的关节角度曲线及各负载转矩曲线上的点为特征点，通过样条插值的方式用一个或几个分段光滑的曲线去查过这些特征点，然后在进行求解，一方面可以降低计算量，另一方面还能消除刚体抖动、冲击等问题。

作为其中一种实现方式，如图2所示，该处的并联机器人以DELTA机器人为例进行说明。

DELTA机器人包括静平台和动平台，静平台上安装三个驱动电机，驱动电机例如是永磁同步电机，各驱动电机上连接一条支链，每条支链包括驱动臂和从动臂，驱动臂与驱动电机连接，驱动臂与从动臂通过球铰连接，从动臂的末端通过球铰与静平台连接，三条支链在驱动电机的带动下独立的运动，控制静平台在运动空间内做平动。其中，主动臂与竖直方向的夹角θ为关节空间，动平台中心的运动范围为工作空间，各支链自静平台到动平台的三个关节依次标记为F_i、J_i、E_i，i＝1，2，3。

DELTA机器人在设计之初，需要对其物理结构进行定型，即对其进行结构设计，确定各部件如驱动电机、主动臂、从动臂等的参数。设计时在满足工程整体要求(如工作路径、负载等)条件下，建立优化目标函数，根据优化的目标不同，建立如同的优化目标函数，如为了优化抓取的速度，则建立速度优化目标函数，如为了优化能耗，则建立能耗优化目标函数，通过优化目标函数对DELTA机器人进行运动学模型分析，剔除奇异位，搜索末端执行器在可达空间内的工作空间范围，结合动力学模型与实际工程限制要求，在工作空间内优化机器人的结构参数。需要说明但是，DELTA机器人的物理结构不是该发明的研究内容，这里不对其进行详述。

如图3所示，本实施例中，以在搜索得到的工作空间内给出工程中常用的“门型”曲线作为末端执行器运动路径为例。“门型”曲线包括竖直上升段，上升弧形过渡段，水平段，下降弧形过渡段及竖直下降段。在上述各段中执行器的运动是非匀速的，其需要经历静止-加速-减速-转向-加速-减速-转向-减速-静止等几个过程，这几个过程伴随着速度方向、加速度的改变，加速度改变的点、速度方向改变的点所对应的关节角度曲线及所述负载转矩曲线上的点为特这点。在选定末端执行器运动路径时，一定要保证末端执行器运动路径跃度光滑可导，也即四阶光滑可导。

根据以下关系式获取所述关节角度曲线：

A_i＝(l_a-Δr)²+2η_i(l_a-Δr)+ξ；

B_i＝4l_az；

C_i＝(l_a+Δr)²-2η_i(l_a+Δr)²+ξ；

Δr＝R-r；

其中，θ为关节角度，l_a为主动臂长度，l_b为从动臂长度，(x,y,z)为末端执行器运动路径的坐标，R为静平台外接圆半径，r为动平台外接圆半径。如图4所示，为求解得出的三个关节的关节角度曲线，关节角度曲线是角度关于时间的曲线。

进一步地，根据以下关系式获取所述负载转矩曲线：

其中，F＝[τ₁ τ₂ τ₃]，τ_i为负载转矩，I_rf为主动臂转动惯量，M为从动臂及负载的等效质量，

为执行器末端加速度,

为执行器末端速度，

为末端执行器重力，

为主动臂重力，J为雅可比矩阵。

上式2-1为DELTA机器人的动力学模型，式2-2为驱动电机矢量控制时直轴电流为零的电机模型，其相当于一个线性系统。驱动电机带动各支链运动，因此电机模型中

为与DELTA机器人的动力学模型中的τ_i相同，也即

用电机模型中的

去代替动力学模型中的τ_i，也即对动力学模型进行了线性化处理，降低了数据处理难度，绕开了复杂的机器人动力学模型与电机模型一体化线性处理过程。

在关节角度曲线的特征点处进行五次样条插值，得到多个分段光滑的五次多项式，作为驱动电机的给定信号，对负载转矩曲线进行五次样条插值，作为负载信号，基于给定信号与负载信号来对DELTA机器人进行运动控制。如图5所示，其中一关节的负载转矩曲线在进行样条插值前后的对比图，模型力矩曲线为样条插值前的负载转矩曲线，插值曲线为样条插值后的负载转矩曲线，如图6、7所示，其中一关节(关节2)的关节角度曲线的理论值(也即规划角度)与进行样条插值前后的实际值(也即输出角度)的对比图，从图中可以看出整个系统在达到稳定状态后仅在0.055～0.123s内出现较小偏差，误差控制在±5‰的范围内，也即通过样条插值话处理是可以接受的处理方式。

在对上述各式进行求解获得关节角度曲线及负载转矩曲线，及进行样条插值处理时，均可以利用Matlab软件进行求解计算。

第二方面，如图8所示，本发明实施例提供一种并联机器人的控制装置，包括：关节角度曲线获取单元1，用于对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线；负载转矩曲线获取单元2，用于根据所述并联机器人的动力学模型，获取所述并联机器人各驱动电机的负载转矩曲线；控制单元3，用于至少分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对所述并联机器人进行运动控制；所述特征点至少为所述关节角度曲线及所述负载转矩曲线对应于所述末端执行器运动路径上，加速度改变的点和/或速度方向改变的点。

该控制装置用于实现上述实施例的方法，其效果及工作原理参见上述实施例，这里不再赘述。

进一步地，所述并联机器人为DELTA机器人。

进一步地，所述关节角度曲线获取单元，用于根据以下关系式获取所述关节角度曲线：

A_i＝(l_a-Δr)²+2η_i(l_a-Δr)+ξ；

B_i＝4l_az；

C_i＝(l_a+Δr)²-2η_i(l_a+Δr)²+ξ；

Δr＝R-r；

其中，θ为关节角度，l_a为主动臂长度，l_b为从动臂长度，(x,y,z)为末端执行器运动路径的坐标，R为静平台外接圆半径，r为动平台外接圆半径。

进一步地，所述负载转矩曲线获取单元，用于根据以下关系式获取所述负载转矩曲线：

其中，F＝[τ₁ τ₂ τ₃]，τ_i为负载转矩，I_rf为主动臂转动惯量，M为从动臂及负载的等效质量，

为执行器末端加速度,

为执行器末端速度，

为末端执行器重力，

为主动臂重力，J为雅可比矩阵。

第三方面，如图9所示，本发明还提供一种计算机设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述的方法。

计算机设备包括中央处理单元(CPU)501，其可以根据存储在只读存储器(ROM)502中的程序或者从存储部分加载到随机访问存储器(RAM)503中的程序而执行各种适当的动作和处理。在RAM503中，还存储有系统操作所需的各种程序和数据。CPU501、ROM502以及RAM503通过总线504彼此相连。输入/输出(I/O)接口505也连接至总线504。

以下部件连接至I/O接口505：包括键盘、鼠标等的输入部分506；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分；包括硬盘等的存储部分508；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分509。通信部分509经由诸如因特网的网络执行通信处理。驱动器也根据需要连接至I/O接口505。可拆卸介质511，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器510上，以便于从其上读出的计算机程序根据需要被安装入存储部分508。

特别地，根据本发明的实施例，上文参考流程图1描述的过程可以被实现为计算机软件程序。例如，上述实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分从网络上被下载和安装，和/或从可拆卸介质被安装。在该计算机程序被中央处理单元(CPU)501执行时，执行本申请的系统中限定的上述功能。

需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。

附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

描述于本发明实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现，所描述的单元也可以设置在处理器中。其中，这些单元的名称在某种情况下并不构成对该单元本身的限定。所描述的单元或模块也可以设置在处理器中，例如，可以描述为：一种处理器包括判断模块、解密单元。其中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定，例如，关节角度曲线获取单元还可以被描述为“用于对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线”。

第四方面，本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述的方法。

该计算机可读介质可以是上述实施例中描述的电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该电子设备执行时，使得该电子设备实现如上述实施例中所述的药物寄送方法。

例如，所述电子设备可以实现如图1中所示的：S1：对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线；S2：根据所述并联机器人的动力学模型，获取所述并联机器人各驱动电机的负载转矩曲线；S3：至少分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对所述并联机器人进行运动控制；所述特征点至少为所述关节角度曲线及所述负载转矩曲线对应于所述末端执行器运动路径上，加速度改变的点和/或速度方向改变的点。

应当注意，尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元，但是这种划分并非强制性的。实际上，根据本公开的实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。

此外，尽管在附图中以特定顺序描述了本公开中方法的各个步骤，但是，这并非要求或者暗示必须按照该特定顺序来执行这些步骤，或是必须执行全部所示的步骤才能实现期望的结果。附加的或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，以及/或者将一个步骤分解为多个步骤执行等。

通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的示例实施方式可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。

以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (4)

1.一种并联机器人的控制方法，其特征在于，包括以下步骤：

对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线；

根据所述并联机器人的动力学模型，获取所述并联机器人各驱动电机的负载转矩曲线；

至少分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对所述并联机器人进行运动控制；所述特征点至少为所述关节角度曲线及所述负载转矩曲线对应于所述末端执行器运动路径上，加速度改变的点和/或速度方向改变的点；

所述并联机器人为DELTA机器人；

根据以下关系式获取所述负载转矩曲线：

其中，F＝[τ₁ τ₂ τ₃]，τ_i为负载转矩，I_rf为主动臂转动惯量，M为从动臂及负载的等效质量，

为执行器末端加速度,

为执行器末端速度，

为末端执行器重力，

为主动臂重力，J为雅可比矩阵。

2.一种并联机器人的控制装置，其特征在于，包括：

关节角度曲线获取单元，用于对给定的末端执行器运动路径进行逆运动学运算，获取所述并联机器人各关节的关节角度曲线；

负载转矩曲线获取单元，用于根据所述并联机器人的动力学模型，获取所述并联机器人各驱动电机的负载转矩曲线；

控制单元，用于至少分别对各所述关节角度曲线及各所述负载转矩曲线的特征点进行样条插值处理，并根据样条插值处理后的关节角度曲线及负载转矩曲线对所述并联机器人进行运动控制；所述特征点至少为所述关节角度曲线及所述负载转矩曲线对应于所述末端执行器运动路径上，加速度改变的点和/或速度方向改变的点；

所述并联机器人为DELTA机器人；

所述负载转矩曲线获取单元，用于根据以下关系式获取所述负载转矩曲线：

其中，F＝[τ₁ τ₂ τ₃]，τ_i为负载转矩，I_rf为主动臂转动惯量，M为从动臂及负载的等效质量，

为执行器末端加速度,

为执行器末端速度，

为末端执行器重力，

为主动臂重力，J为雅可比矩阵。

3.一种计算机设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1所述的并联机器人的控制方法。

4.一种可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被执行时实现权利要求1所述的并联机器人的控制方法。

Images