CN116141319A - 三绳索并联机器人的控制方法、装置、设备和存储介质 - Google Patents

Abstract

本发明实施例提供一种三绳索并联机器人的控制方法、装置、设备和存储介质，涉及并联机器人技术领域。其中，这种控制方法包含步骤S1至步骤S8。S1、获取上位机发送的期望轨迹。S2、根据期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩。S3、根据驱动电机的期望转矩，控制电机运行。S4、获取电机编码器输出的驱动电机的实际转矩和实际转速。S5、根据驱动电机的实际转矩和期望转矩，对驱动电机进行比例积分的闭环补偿控制。S6、根据驱动电机的实际转速进行积分，获取理论绳长。S7、根据理论绳长，以期望位置作为迭代初值，通过牛顿迭代法获取末端执行器的实际位置。S8、根据实际位置和期望位置，对期望轨迹进行比例微分的闭环补偿控制。

Description

三绳索并联机器人的控制方法、装置、设备和存储介质

技术领域

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

背景技术

随着工业技术的不断提高，重载搬运机器人发展迅速。在汽车行业、电子行业、橡胶及塑料行业、船舶行业、金属制品行业、食品行业均有应用。主要从事分拣、搬运、铸造和码垛等工种。传统工业机器人存在惯性大、不灵活、易用性低等缺点。未来重载搬运机器人将朝着智能一体化、轻量化和虚拟调试的趋势发展。

根据结构的不同，机器人主要分为串并联机器人；根据材料不同，机器人又分为柔性机器人与刚性机器人。与这些机器人相比，绳牵引并联机器人结合了并联机器人结构高刚度、高精度、负载能力强等优点与柔性机器人的优点绳索驱动质量轻、驱动源固定、惯性小的优点。

因为绳索机器人本身的柔性问题绳牵引并联机器人具有对自标定方法的特殊要求。标定是提高机器人精度的必要手段，传统的标定手段需要借助高精度的外部测量设备，并且需要完成复杂的标定流程才能实现。传统的控制方法费时、繁琐且高设备成本的标定方法极大地限制了绳牵引并联机器人实现快速拆解、装配及重构作业的能力。

有鉴于此，申请人在研究了现有的技术后特提出本申请。

发明内容

本发明提供了一种三绳索并联机器人的控制方法、装置、设备和存储介质，以改善上述技术问题中的至少一个。

第一方面、

本发明实施例提供了一种三绳索并联机器人的控制方法，其包含步骤S1至步骤S8。

S1、获取上位机发送的期望轨迹。

S2、根据期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩。

S3、根据驱动电机的期望转矩，控制电机运行。

S4、获取电机编码器输出的驱动电机的实际转矩和实际转速。

S5、根据驱动电机的实际转矩和期望转矩，对驱动电机进行比例积分的闭环补偿控制。

S6、根据驱动电机的实际转速进行积分，获取理论绳长。

S7、根据理论绳长，以期望位置作为迭代初值，通过牛顿迭代法获取末端执行器的实际位置。

S8、根据实际位置和期望位置，对期望轨迹进行比例微分的闭环补偿控制。

第二方面、

本发明实施例提供了一种三绳索并联机器人的控制装置，其包含：

期望轨迹获取模块，用于获取上位机发送的期望轨迹。

初始参数获取模块，用于根据期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩。

驱动模块，用于根据驱动电机的期望转矩，控制电机运行。

实际参数获取模块，用于获取电机编码器输出的驱动电机的实际转矩和实际转速。

转矩补偿模块，用于根据驱动电机的实际转矩和期望转矩，对驱动电机进行比例积分的闭环补偿控制。

理论绳长获取模块，用于根据驱动电机的实际转速进行积分，获取理论绳长。

实际位置获取模块，用于根据理论绳长，以期望位置作为迭代初值，通过牛顿迭代法获取末端执行器的实际位置。

位置补偿模块，用于根据实际位置和期望位置，对期望轨迹进行比例微分的闭环补偿控制。

第三方面、

本发明实施例提供了一种三绳索并联机器人的控制设备，其包括处理器、存储器，以及存储在存储器内的计算机程序。计算机程序能够被处理器执行，以实现如第一方面任意一段所说的三绳索并联机器人的控制方法。

第四方面、

本发明实施例提供了一种计算机可读存储介质。该计算机可读存储介质包括存储的计算机程序，其中，在计算机程序运行时控制计算机可读存储介质所在设备执行如第一方面任意一段所说的三绳索并联机器人的控制方法。

通过采用上述技术方案，本发明可以取得以下技术效果：

本发明实施例通过轨迹跟踪控制和力跟踪控制同时对机器人进行运动控制，通过力位混合控制实现对机器人较为精准的运动控制。有效地提高机器人在面对来自风力、滑轮摩擦等来自外界环节影响因素的适应能力，并且可以兼顾运动的加速度以及连续性，进而满足了对绳牵引机器人的末端负载进行轨迹规划控制的快速响应、末端轨迹的连续性以及外界交互柔顺性的要求。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是控制方法的流程示意图。

图2是三绳索并联机器人的轴测图。

图3是三绳索并联机器人的电路连接示意图。

图4是控制方法的原理图。

图5是控制装置的结构示意图。

图中标记：1-固定底座、2-立柱、3-转台轴承、4-滑轮、5-卷扬机、6-横梁、7-末端执行器。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一、

请参阅图1至图4，本发明第一实施例提供一种三绳索并联机器人的控制方法，其可由三绳索并联机器人的控制设备来执行(以下简称：控制设备)。特别地，由控制设备中的一个或多个处理器来执行，以实现步骤S1至步骤S8。

S1、获取上位机发送的期望轨迹。

如图2和图3所示，三绳索并联机器人包括三组结构相同的绳索结构，以及连接在三组绳索结构的末端执行器7。绳索结构包括固定底座1、竖直安装在固定底座上的立柱2、安装在立柱2顶部的转台轴承3、安装在转台轴承上的滑轮4、接合于两组绳索结构之间的横梁6、安装在固定底座上的卷扬机5、传动连接于卷扬机5的驱动电机，以及通过滑轮和且两端分别接合在末端执行器7和卷扬机5上的绳索。

驱动电机驱动卷扬机5转动，以使绳索伸长或缩短，从而改变末端执行器7的位置。末端执行器7用于吊装负载物。关于三绳索并联机器人的具体结构可以查看申请人申请的申请号为“202210570198.1”，发明名称为“能量回收的绳索机器人及其控制方法、装置、存储介质”的专利文件，本发明在此不再赘述。

可以理解的是，在本实施例中，控制设备为运动控制器。其能够接收上位机发送的控制轨迹。在其它实施例中，控制设备可以是便携笔记本计算机、台式机计算机、服务器、智能手机或者平板电脑等具有计算性能的电子设备。

具体的，在上位机中直接设置末端执行器期望轨迹P。其中，P＝f(t)。

首先，给一个时间信号t。然后，设置末端位置带时间的期望轨迹

S2、根据期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩。

具体的，期望位置能够从期望轨迹直接得到。期望转矩需要对期望轨迹进行运动学逆解得到。

已知末端执行器每时刻的位姿以及固定的滑轮中心点坐标，可以通过几何关系获取机器人的实时的期望绳长。运用根据绳索机器人的末端负载的坐标位置进行控制的期望轨迹，根据运动学逆解，解算出所述机器人的绳索长度；

在上述实施例的基础上，本发明的一个可选地实施例中，步骤S2具体包括步骤S21至步骤S23。

S21、根据期望轨迹，获取末端执行器的期望位置。

具体的，根据期望轨迹能够获取末端执行器各个时刻的坐标。即当前时间的实时位置坐标。

S22、获取滑轮中心坐标，并根据滑轮中心坐标和期望轨迹，获取三绳索的期望长度。具体的，期望绳长为通过当前实时的期望轨迹跟踪控制得到的绳长。三绳索的期望长度计算模型为：

式中，l_i＝[l₁，l₂，l₃]为三绳索绳长组成的列向量、A_i为第i个滑轮中心坐标、P为末端执行器坐标、T表示转置。

S23、根据三绳索的期望长度，通过绳索机器人的动力学模型，获取驱动电机的期望转矩。其中，动力学模型为：

式中，τ＝[τ₁τ₂τ₃]为电机驱动器的力矩、J_m为驱动器的等效惯量、r为绞盘半径、J为三根绳长分别对末端执行器规划的X、Y、Z三个方向轨迹分别求导而得来的雅可比矩阵、l＝[l₁，l₂，l₃]为三绳索绳长组成的列向量、I为单位矩阵、B_m为驱动器的等效阻尼系数矩阵、M为动平台的质量矩阵、G为动平台的动力向量。

具体的，通过对末端执行器位置的位置坐标进行的位置规划所获得的绳长即l_i对时间微分得到绳速：

式中，/>

为绳索速度。再将绳索速度对时间求微分得到绳索加速度：/>

式中，/>

为绳索加速度

根据绳速和绳索加速度使用拉格朗日法进行动平台建模，再通过滑轮与绳长的关系，使用微分平坦性通过绳长、绳索速度与加速度信息表达出电机驱动器的力矩，即动力学模型。该过程将非线性系统转化为线性系统，从而建立起整个绳索机器人的动力学模型。直接由绳长的一阶导二阶导得到电机转矩，从而减少计算量，缩短计算时间。

S3、根据驱动电机的期望转矩，控制电机运行。

在上述实施例的基础上，本发明的一个可选地实施例中，步骤S3具体包括步骤S31至步骤S32。

S31、根据电机转矩和驱动信号之间的关系，获取电机控制信号。

S32、根据电机控制信号，控制绳索的驱动电机运行。

具体的，根据电机的说明书能够获取电机的输出量和控制信号之间的关系：1V＝aN·m。根据前面计算得到的期望转矩，转化得到电机的控制信号控制电机运行。

S4、获取电机编码器输出的驱动电机的实际转矩和实际转速。

具体的，从编码器中能够直接获取电机的输出信息，包括实际转矩和实际转速。

S5、根据驱动电机的实际转矩和期望转矩，对驱动电机进行比例积分的闭环补偿控制。

具体的，对实际电机转矩与期望电机转矩形成的误差使用比例积分控制，从而形成力的闭环控制。通过比例积分控制进行参数调整，调整之后给反馈回的实际电机转矩一个补偿，从而形成力的闭环控制。将实际转矩与期望转矩的误差通过比例积分控制返回给需要执行的电机转矩。其中：

电机转矩误差定义为：e_D＝τ-τ_实，式中，τ为期望电机转矩、τ_实实际电机转矩。

定义由比例积分控制得出的补偿末端位置为e_1D。驱动电机的转矩的比例积分的闭环补偿控制模型为：

式中，e_1D为扭矩补偿、k_p1I为比例参数、e_D为实际转矩和期望转矩之间的误差、

为积分参数。

S6、根据驱动电机的实际转速进行积分，获取理论绳长。

在上述实施例的基础上，本发明的一个可选地实施例中，步骤S6具体包括步骤S61至步骤S62。

S61、根据驱动电机的实际转速，获取绳索速度。其中，绳索速度的计算模型为：

式中，/>

为第i根绳的实际绳速、v_i卷为第i根绳对应的卷筒转速、r为卷筒半径、r_绳为绳索的半径。

S62、根据绳索速度进行积分，获取理论绳长。其中，理论绳长的计算模型为：l_i＝∫v_idt，式中，v_i为绳索速度。

具体的，通过编码器获得电机的实际转速。再通过已知的卷扬机的卷筒直径与绳索缠绕圈数以及绳索直径得到绳索速度

最后，由绳索速度对时间的积分得到实际绳长l_i。

S7、根据理论绳长，以期望位置作为迭代初值，通过牛顿迭代法获取末端执行器的实际位置。

具体的，将转化而来的实际绳长，通过运动学正解转化为末端轨迹位置，转化过程如下。其中，运动学正解就是已知绳长求解末端执行器的位姿。

在上述实施例的基础上，本发明的一个可选地实施例中，使用牛顿迭代法得到末端位姿。通过使用牛顿迭代法，将期望轨迹作为迭代初值，求出末端实际位置的实际位姿。牛顿迭代法的计算模型为：

f_i(P)＝(A_i-P)^T(A_i-P)-l_i ²

式中，f_i(P)为期望绳长与实际绳长的平方差、A_i为第i根绳索对应的滑轮中心坐标点、P为末端执行器的期望位置、T表示转置、l_i＝[l₁，l₂，l₃]为三绳索的理论绳长组成的列向量。

具体的，将f_i(P)进行泰勒一阶展开，得到如下表达式：

/>

f_i(P₀+ΔP)-f_i(P₀)＝JΔP

f_i(P₀+ΔP)-f_i(P₀)＝ΔP_k→f(P_k)＝JΔP_k→ΔP_k＝-J⁺f(P_k)

式中，P为步骤S01中的期望轨迹的坐标位置，P₀+ΔP沿f_i(P)趋于P₀所以ΔP趋于0，A_i为第i根绳索对应的滑轮中心坐标点，f_i(P)为期望绳长与实际绳长的平方差，f_i(P₀)为f_i(P)在P₀+ΔP的一阶导数，

为该系统的运动学雅可比矩阵，

为f_i(P₀+ΔP)在P₀+ΔP的一阶泰勒余项，P_k为第k次迭代而得到的末端轨迹坐标，ΔP_k＝JΔP即P_k+1经第k+1次迭代而得的末端轨迹坐标的一阶泰勒余项，J⁺为J的Moore-Penrose的广义逆，ε为迭代精度。

令P_k+1＝P_k+ΔP_k，maxΔP≤ε即得出最后正解位置ΔP_k。

本专利在现有力位混合控制技术的基础上，使用牛顿迭代法根据绳长计算末端执行器位置，但是由于牛顿迭代法在进行迭代时，难以确定迭代初值，本算法使用期望轨迹作为迭代初值进行末端位置的求解，使用该方法相比于传统视觉控制器确定末端执行器的位姿反馈更快。

S8、根据实际位置和期望位置，对期望轨迹进行比例微分的闭环补偿控制。

具体的，由于位置的控制对响应的速度要求较高，因而将步骤S7算出的末端执行器的实际位置坐标反馈到控制回路中。与该时刻期望轨迹的坐标形成两者之间的误差使用比例微分控制方法对误差进行参数调整。调整之后给反馈回来的实际位置一个补偿形成新的执行轨迹，从而进行位置的闭环控制。

定义末端位姿误差为：e_I＝P-P_实，式中，P为期望位置、P_实为实际位置。

定义由比例微分控制方法得出的补偿末端位姿为e_1I：期望轨迹的比例微分的闭环补偿控制模型为：

式中，e_1I为位置补偿、k_p2I为比例参数、e_I为实际位置和期望位置的误差、T_d为积分参数。

相对于电机转速的半闭环位置控制，对期望轨迹进行比例微分的闭环补偿控制能把控制系统输出量的一部分或全部，通过一定方法和装置反送回系统的输入端，然后将反馈信息与原输入信息进行比较，再将比较的结果施加于系统进行控制，避免系统偏离预定目标；使用全闭环控制相比于半闭环控制，由于末端轨迹在转化为模拟量输入电机时存在一定机械误差，因此将电机得来的转速再逆向转变为末端执行器的位置作为反馈，相对于直接使用电机转速作为反馈能一定程度上减小机械误差。

本发明实施例的三绳索并联机器人的控制方法通过同时使用位置跟踪控制和力跟踪控制这两种控制方式对机器人进行运动控制的技术，通过力位混合控制，实现对机器人较为精准的运动控制。

本发明实施例通过轨迹跟踪控制和力跟踪控制同时对机器人进行运动控制，通过力位混合控制实现对机器人较为精准的运动控制。有效地提高机器人在面对来自风力、滑轮摩擦等来自外界环节影响因素的适应能力，并且可以兼顾运动的加速度以及连续性，进而满足了对绳牵引机器人的末端负载进行轨迹规划控制的快速响应、末端轨迹的连续性以及外界交互柔顺性的要求。

实施例二、

如图5所示，本发明实施例提供了一种三绳索并联机器人的控制装置，其包含：

期望轨迹获取模块100，用于获取上位机发送的期望轨迹。

初始参数获取模块200，用于根据期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩。

驱动模块300，用于根据驱动电机的期望转矩，控制电机运行。

实际参数获取模块400，用于获取电机编码器输出的驱动电机的实际转矩和实际转速。

转矩补偿模块500，用于根据驱动电机的实际转矩和期望转矩，对驱动电机进行比例积分的闭环补偿控制。

理论绳长获取模块600，用于根据驱动电机的实际转速进行积分，获取理论绳长。

实际位置获取模块700，用于根据理论绳长，以期望位置作为迭代初值，通过牛顿迭代法获取末端执行器的实际位置。

位置补偿模块800，用于根据实际位置和期望位置，对期望轨迹进行比例微分的闭环补偿控制。

在上述实施例的基础上，本发明的一个可选地实施例中，初始参数获取模块200具体包括：

期望位置获取单元，用于根据期望轨迹，获取末端执行器的期望位置。

期望长度获取单元，用于获取滑轮中心坐标，并根据滑轮中心坐标和期望轨迹，获取三绳索的期望长度。其中，三绳索的期望长度计算模型为：

式中，l_i＝[l₁，l₂，l₃]为三绳索绳长组成的列向量、A_i为第i个滑轮中心坐标、P为末端执行器坐标、T为转置。

期望转矩获取单元，用于根据三绳索的期望长度，通过绳索机器人的动力学模型，获取驱动电机的期望转矩。其中，动力学模型为：

式中，τ＝[τ₁τ₂τ₃]为电机驱动器的力矩、J_m为驱动器的等效惯量、r为绞盘半径、J为三根绳长分别对末端执行器规划的X、Y、Z三个方向轨迹分别求导而得来的雅可比矩阵、l＝[l₁，l₂，l₃]为三绳索绳长组成的列向量、I为单位矩阵、B_m为驱动器的等效阻尼系数矩阵、M为动平台的质量矩阵、G为动平台的动力向量。

在上述实施例的基础上，本发明的一个可选地实施例中，驱动模块300具体包括：

电机控制信号获取单元，用于根据电机转矩和驱动信号之间的关系，获取电机控制信号。

电机驱动单元，用于根据电机控制信号，控制绳索的驱动电机运行。

在上述实施例的基础上，本发明的一个可选地实施例中，驱动电机的转矩的比例积分的闭环补偿控制模型为：

式中，e_1D为扭矩补偿、k_p1I为比例参数、e_D为实际转矩和期望转矩之间的误差、/>

为积分参数。

在上述实施例的基础上，本发明的一个可选地实施例中，理论绳长获取模块600具体包括：

绳索速度获取单元，用于根据驱动电机的实际转速，获取绳索速度。其中，绳索速度的计算模型为：

式中，/>

为第i根绳的实际绳速、v_i卷为第i根绳对应的卷筒转速、r为卷筒半径、r_绳为绳索的半径。

理论绳长获取单元，用于根据绳索速度进行积分，获取理论绳长。其中，理论绳长的计算模型为：l_i＝∫v_idt，式中，v_i为绳索速度。

在上述实施例的基础上，本发明的一个可选地实施例中，牛顿迭代法的计算模型为：f_i(P)＝(A_i-P)^T(A_i-P)-l_i ²，式中，f_i(P)为期望绳长与实际绳长的平方差、A_i为第i根绳索对应的滑轮中心坐标点、P为末端执行器的期望位置、T表示转置、l_i＝[l₁，l₂，l₃]为三绳索的理论绳长组成的列向量。

在上述实施例的基础上，本发明的一个可选地实施例中，期望轨迹的比例微分的闭环补偿控制模型为：

式中，e_1I为位置补偿、k_p2I为比例参数、e_I为实际位置和期望位置的误差、T_d为积分参数。

实施例三、

本发明实施例提供了一种三绳索并联机器人的控制设备，其包括处理器、存储器，以及存储在存储器内的计算机程序。计算机程序能够被处理器执行，以实现如实施例一任意一段所说的三绳索并联机器人的控制方法。

实施例四、

本发明实施例提供了一种计算机可读存储介质。该计算机可读存储介质包括存储的计算机程序，其中，在计算机程序运行时控制计算机可读存储介质所在设备执行如实施例一任意一段所说的三绳索并联机器人的控制方法。

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

另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，电子设备，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。

应当理解，本文中使用的术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”或“响应于检测”。类似地，取决于语境，短语“如果确定”或“如果检测(陈述的条件或事件)”可以被解释成为“当确定时”或“响应于确定”或“当检测(陈述的条件或事件)时”或“响应于检测(陈述的条件或事件)”。

实施例中提及的“第一\第二”仅仅是是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二”在允许的情况下可以互换特定的顺序或先后次序。应该理解“第一\第二”区分的对象在适当情况下可以互换，以使这里描述的实施例能够以除了在这里图示或描述的那些内容以外的顺序实施。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种三绳索并联机器人的控制方法，其特征在于，包含：

获取上位机发送的期望轨迹；

根据所述期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩；

根据所述驱动电机的期望转矩，控制电机运行；

获取电机编码器输出的驱动电机的实际转矩和实际转速；

根据所述驱动电机的实际转矩和期望转矩，对所述驱动电机进行比例积分的闭环补偿控制；

根据所述驱动电机的实际转速进行积分，获取理论绳长；

根据所述理论绳长，以所述期望位置作为迭代初值，通过牛顿迭代法获取末端执行器的实际位置；

根据所述实际位置和所述期望位置，对所述期望轨迹进行比例微分的闭环补偿控制。

2.根据权利要求1所述的三绳索并联机器人的控制方法，其特征在于，根据所述期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩，具体包括：

根据所述期望轨迹，获取所述末端执行器的期望位置；

获取滑轮中心坐标，并根据所述滑轮中心坐标和所述期望轨迹，获取三绳索的期望长度；其中，三绳索的期望长度计算模型为：

式中，l_i＝[l₁，l₂，l₃]为三绳索绳长组成的列向量、A_i为第i个滑轮中心坐标、P为末端执行器坐标、T为转置；

根据所述三绳索的期望长度，通过绳索机器人的动力学模型，获取所述驱动电机的期望转矩；其中，动力学模型为：

式中，τ＝[τ₁τ₂τ₃]为电机驱动器的力矩、J_m为驱动器的等效惯量、r为绞盘半径、J为三根绳长分别对末端执行器规划的X、Y、Z三个方向轨迹分别求导而得来的雅可比矩阵、l＝[l₁，l₂，l₃]为三绳索绳长组成的列向量、I为单位矩阵、B_m为驱动器的等效阻尼系数矩阵、M为动平台的质量矩阵、G为动平台的动力向量。

3.根据权利要求1所述的三绳索并联机器人的控制方法，其特征在于，所述驱动电机的转矩的比例积分的闭环补偿控制模型为：

式中，e_1D为扭矩补偿、k_p1I为比例参数、e_D为实际转矩和期望转矩之间的误差、/>

为积分参数。

4.根据权利要求1所述的三绳索并联机器人的控制方法，其特征在于，根据所述三绳索的期望长度和所述驱动电机的实际转速进行积分，获取理论绳长，具体包括：

根据所述驱动电机的实际转速，获取绳索速度；其中，绳索速度的计算模型为：

式中，/>

为第i根绳的实际绳速、v_i卷为第i根绳对应的卷筒转速、r为卷筒半径、r绳为绳索的半径；

根据所述绳索速度进行积分，获取理论绳长；其中，理论绳长的计算模型为：l_i＝∫v_idt，式中，v_i为绳索速度。

5.根据权利要求1所述的三绳索并联机器人的控制方法，其特征在于，所述牛顿迭代法的计算模型为：f_i(P)＝(A_i-P)^T(A_i-P)-l_i ²，式中，f_i(P)为期望绳长与实际绳长的平方差、A_i为第i根绳索对应的滑轮中心坐标点、P为末端执行器的期望位置、T表示转置、l_i＝[l₁，l₂，l₃]为三绳索的理论绳长组成的列向量。

6.根据权利要求1所述的三绳索并联机器人的控制方法，其特征在于，所述期望轨迹的比例微分的闭环补偿控制模型为：

式中，e_1I为位置补偿、k_p2I为比例参数、e_I为实际位置和所述期望位置的误差、T_d为积分参数。

7.根据权利要求1至6任意一项所述的三绳索并联机器人的控制方法，其特征在于，根据所述驱动电机的期望转矩，控制电机运行，具体包括：

根据电机转矩和驱动信号之间的关系，获取电机控制信号；

根据所述电机控制信号，控制绳索的驱动电机运行。

8.一种三绳索并联机器人的控制装置，其特征在于，包含：

期望轨迹获取模块，用于获取上位机发送的期望轨迹；

初始参数获取模块，用于根据所述期望轨迹，获取末端执行器的期望位置，以及驱动电机的期望转矩；

驱动模块，用于根据所述驱动电机的期望转矩，控制电机运行；

实际参数获取模块，用于获取电机编码器输出的驱动电机的实际转矩和实际转速；

转矩补偿模块，用于根据所述驱动电机的实际转矩和期望转矩，对所述驱动电机进行比例积分的闭环补偿控制；

理论绳长获取模块，用于根据所述驱动电机的实际转速进行积分，获取理论绳长；

实际位置获取模块，用于根据所述理论绳长，以所述期望位置作为迭代初值，通过牛顿迭代法获取末端执行器的实际位置；

位置补偿模块，用于根据所述实际位置和所述期望位置，对所述期望轨迹进行比例微分的闭环补偿控制。

9.一种三绳索并联机器人的控制设备，其特征在于，包括处理器、存储器，以及存储在所述存储器内的计算机程序；所述计算机程序能够被所述处理器执行，以实现如权利要求1至7任意一项所述的三绳索并联机器人的控制方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至7任意一项所述的三绳索并联机器人的控制方法。

Images