CN114460899B - 一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法 - Google Patents

Abstract

本发明公开了一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法，包括S1、根据并联执行器的轨迹坐标点以及四根绳索牵引处的坐标，获取四根绳索运动到轨迹坐标点的理论绳长；S2、获取绳索的理论绳长集合；S3、获取理论绳长集合对应的实际收紧状态下的绳长集合；S4、获取并联执行器的实际轨迹坐标点；S5、基于并联执行器的实际轨迹坐标点，利用实际收紧状态下的绳长集合，预测下一轨迹坐标点对应的各绳索的实际长度。优点是：通过预测绳索长度的方式，将绳索的误差反馈补偿到预测中，修正绳索的实际长度，从而使其控制姿态更加精准；解决了由于悬垂引起的控制精度不高的问题，提升了目标的控制精度。

Description

一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控 制方法

技术领域

本发明涉及运动姿态控制技术领域，尤其涉及一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法。

背景技术

目前，四柔索牵引并联执行器运动控制技术在舞台表演中已得到了较为广泛的应用。但由于使用环境特殊，导致其在工作过程中，常因绳索悬垂导致运动位置控制位置不精准，同时绳索的悬垂也影响了观赏体验。

发明内容

本发明的目的在于提供一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法，从而解决现有技术中存在的前述问题。

为了实现上述目的，本发明采用的技术方案如下：

一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法，包括如下步骤，

S1、根据并联执行器的轨迹坐标点以及四根绳索牵引处的坐标，获取四根绳索运动到轨迹坐标点的理论绳长；

S2、依据四根绳索运动到轨迹坐标点的理论绳长获取绳索的理论绳长集合；

S3、依据绳索的理论绳长集合，获取理论绳长集合对应的实际收紧状态下的绳长集合；

S4、根据实际收紧状态下的绳长集合，获取并联执行器的实际轨迹坐标点；

S5、基于并联执行器的实际轨迹坐标点，利用实际收紧状态下的绳长集合，预测下一轨迹坐标点对应的各绳索的实际长度。

优选的，步骤S1具体为，

假设并联执行器的任意轨迹坐标点为E(x,y,z)，四根绳索牵引处的坐标点分别表示为A、B、C、D，这四个坐标点的坐标值分别为A(x₁,y₁,z₁)、B(x₂,y₂,z₂)、C(x₃,y₃,z₃)、D(x₄,y₄,z₄)；则四根绳索运动到任意轨迹坐标点E的理论绳长分别为L1、L2、L3、L4，计算公式如下，

优选的，步骤S2中获取的绳索的理论绳长集合为L，其表达式为，

其中，n为并联执行器的运动轨迹点所对应的绳索记录数据；第一行的n个数据从左至右依次为第一根绳索运行到n个轨迹坐标点所对应的理论绳长；第二行的n个数据从左至右依次为第二根绳索运行到n个轨迹坐标点所对应的理论绳长；第三行的n个数据从左至右依次为第三根绳索运行到n个轨迹坐标点所对应的理论绳长；第四行的n个数据从左至右依次为第四根绳索运行到n个轨迹坐标点所对应的理论绳长。

优选的，步骤S3中获取的实际收紧状态下的绳长集合为Ls，其表达式为，

其中，第一行的n个数据从左至右依次为第一根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第二行的n个数据从左至右依次为第二根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第三行的n个数据从左至右依次为第三根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第四行的n个数据从左至右依次为第四根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长。

优选的，步骤S4具体为，根据四根绳索实际收紧状态下的绳长，获取并联执行器的实际轨迹坐标点E_f(x_f，y_f，z_f)，计算过程如下，

其中，dLs为对Ls求导；J(E_f)为并联执行器在位置E_f处的雅克比矩阵；dE_f为对E_f求导；且

其中，m＝1，2，3，4；

利用牛顿迭代法求解并联执行器的实际轨迹坐标点E_f(x_f，y_f，z_f)：设置迭代初始位置为E_fc(x_fc，y_fc，z_fc)，定位精度要求为eps，

E_fc(x_fc，y_fc，z_fc)＝E_fc[0，0，0]

eps＝0.000001；

利用下式获取并联执行器的实际轨迹坐标点E_f(x_f，y_f，z_f)，

E_f ^r+1＝E_f ^r+(J^-1(E_f ^r))(L^*-L^r)

其中，E_f ^r+1表示在r位置处的终止迭代位置；E_f ^r表示绳索在r位置处的迭代位置；L^*为驱动绳索的长度向量，L^r为第r次迭代得到的绳索长度值；J^-1(E_f ^r)为第r次迭代得到的雅克比矩阵的逆矩阵；r为迭代次数，迭代终止的条件是||E_f ^r+1-E_f ^r||≤eps。

优选的，步骤S5具体为，

通过实际收紧状态下的绳长集合Ls中的第m根绳索来预测其在下一轨迹坐标点对应的实际长度，绳索m在实际收紧状态下的绳长集合为Ls^m，

其中，括号中的n个数据从左至右依次为绳索m运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；

经过一次累加Ls^m后生成的新数据集为,

其中，Ls^mm(i)为绳索m在实际收紧状态下的绳长集合Ls^m经过累加后的新数据集；j为原始数据集中的元素，i为累加数据集中的元素；

新数据集的组合表达式为，

设Ls^mm满足一阶常微分方程，则，

其中，t为时序；a，u为常数；则根据上式推导出Ls^mm的时序响应方程为，

其中，与/>通过最小二乘估计来获得；通过上式即可获取后时序的绳索收紧长度；后时序的绳索收紧长度集合为Ls^mm；k为数据变量值；k＝1,2,3,…,n时，Ls^mm(k+1)为拟合值；当k≥n时，Ls^mm(k+1)为预测值；

利用绳索的理论绳长集合L与后时序的绳索收紧长度集合为Ls^mm进行加权计算，即可获取绳索m在下一轨迹坐标点对应的实际长度Ls^m′，

其中，δ为权值系数，计算公式为，

本发明的有益效果是：通过预测绳索长度的方式，将绳索的误差反馈补偿到预测中，从预测开始，修正绳索的实际长度，从而使其控制姿态更加精准；解决了由于悬垂引起的控制精度不高的问题，提升了目标的控制精度，并改善了观感体验，为舞台盛宴的视觉效果提供了新的技术支持。

附图说明

图1是本发明实施例中控制方法的流程示意图；

图2是本发明实施例中四柔索并联执行器的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不用于限定本发明。

如图1所示，本实施例中，提供了一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法，包括如下步骤，

S1、根据并联执行器的轨迹坐标点以及四根绳索牵引处的坐标，获取四根绳索运动到轨迹坐标点的理论绳长；

S2、依据四根绳索运动到轨迹坐标点的理论绳长获取绳索的理论绳长集合；

S3、依据绳索的理论绳长集合，获取理论绳长集合对应的实际收紧状态下的绳长集合；

S4、根据实际收紧状态下的绳长集合，获取并联执行器的实际轨迹坐标点；

S5、基于并联执行器的实际轨迹坐标点，利用实际收紧状态下的绳长集合，预测下一轨迹坐标点对应的各绳索的实际长度。

本实施例中，参见图2，可知四柔索并联执行器的结构为四根绳索的一端均与并联执行器相连，四根绳索的另一端分别连接在四个收放机构上，四个收放机构位于分别位于四个方位。四个收放机构均由一个控制器进行整体控制，实现收放。

步骤S1具体为，假设并联执行器的任意轨迹坐标点为E(x,y,z)，四根绳索牵引处的坐标点分别表示为A、B、C、D，这四个坐标点的坐标值分别为A(x₁,y₁,z₁)、B(x₂,y₂,z₂)、C(x₃,y₃,z₃)、D(x₄,y₄,z₄)；则四根绳索运动到任意轨迹坐标点E的理论绳长分别为L1、L2、L3、L4，计算公式如下，

本实施例中，步骤S2中获取的绳索的理论绳长集合为L，其表达式为，

其中，n为并联执行器的运动轨迹点所对应的绳索记录数据；第一行的n个数据从左至右依次为第一根绳索运行到n个轨迹坐标点所对应的理论绳长；第二行的n个数据从左至右依次为第二根绳索运行到n个轨迹坐标点所对应的理论绳长；第三行的n个数据从左至右依次为第三根绳索运行到n个轨迹坐标点所对应的理论绳长；第四行的n个数据从左至右依次为第四根绳索运行到n个轨迹坐标点所对应的理论绳长。。

本实施例中，由于安装误差、环境误差以及绳索本身等各方面误差叠加的原因，导致理论绳索长度数值下发给收放机构，使其进行收放绳索操作后，绳索常常呈现悬垂松弛状态。为了解决绳索悬垂问题，对其进行多次收紧绳索测量实验，然后再将理论绳长对应的实际收紧状态下的绳长进行测量并记录，获取实际收紧状态下的绳长集合Ls。

因此，步骤S3中获取的实际收紧状态下的绳长集合为Ls，其表达式为，

其中，第一行的n个数据从左至右依次为第一根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第二行的n个数据从左至右依次为第二根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第三行的n个数据从左至右依次为第三根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第四行的n个数据从左至右依次为第四根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长。

本实施例中，步骤S4具体为，根据四根绳索实际收紧状态下的绳长，获取并联执行器的实际轨迹坐标点E_f(x_f,y_f,z_f)，计算过程如下，

其中，dLs为对Ls求导；J(E_f)为并联执行器在位置E_f处的雅克比矩阵；dE_f为对E_f求导；且

其中，m＝1，2，3，4；通过改变m的值依次获取

获得雅克比矩阵后，再利用牛顿迭代法求解并联执行器的实际轨迹坐标点E_f(x_f，y_f，z_f)：首先设置迭代初始位置为E_fc(x_fc，y_fc，z_fc)，定位精度要求为eps，

E_fc(x_fc，y_fc，z_fc)＝E_fc[0，0，0]

eps＝0.000001；

利用下式获取并联执行器的实际轨迹坐标点E_f(x_f，y_f，z_f)，

E_f ⁱ⁺¹＝E_f ⁱ+(J^-1(E_f ⁱ))(L^*-Lⁱ)

其中，E_f ^r+1表示在r位置处的终止迭代位置；E_f ^r表示绳索在r位置处的迭代位置；L^*为驱动绳索的长度向量，L^r为第r次迭代得到的绳索长度值；J^-1(E_f ^r)为第r次迭代得到的雅克比矩阵的逆矩阵；r为迭代次数，迭代终止的条件是||E_f ^r+1-E_f ^r||≤eps。

至此，任意空间点的位置E(x，y，z)与其理论绳索长度L我们用样本数据集E与L表示，实际收紧状态下绳长以及实际收紧状态下的并联执行器空间位置点用样本数据集Ls与E_f表示。

本实施例中，绳索收紧通过测量不能得到其全部的值，样本数据集数量比较少，而且绳索收紧的数据也并不是一个有规律的数据集，因此，我们通过样本Ls中的绳索m进行预测。以第一根绳索为例，预测第一根绳索在下一轨迹坐标点对应的实际长度的具体过程为：

绳索1在实际收紧状态下的绳长集合为Ls¹，

其中，括号中的n个数据从左至右依次为绳索m运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；

经过一次累加Ls¹后生成的新数据集为,

Ls¹¹(i)为绳索1在实际收紧状态下的绳长集合Ls¹经过累加后的新数据集；j为原始数据集中的元素，i为累加数据集中的元素；

新数据集的组合表达式为，

设Ls¹¹满足一阶常微分方程，则，

其中，t为时序；a，u为常数；则根据上式推导出Ls¹¹的时序响应方程为，

其中，与/>通过最小二乘估计来获得；通过上式即可获取后时序的绳索收紧长度；后时序的绳索收紧长度集合为Ls¹¹；k为数据变量值；k＝1,2,3,…,n时，Ls^mm(k+1)为拟合值；当k≥n时，Ls^mm(k+1)为预测值。

利用绳索的理论绳长集合L与后时序的绳索收紧长度集合为Ls¹¹进行加权计算，即可获取绳索1在下一轨迹坐标点对应的实际长度Ls^1′，

其中，δ为权值系数，计算公式为，

同理，采用上述计算过程，可以获取其余三根绳索的实际长度值。

本实施例中，由于人为实验测量绳索长度的客观性因素，不可能测试每一个目标轨迹点所对应的实际松弛长度，因此，我们通过预测方法获得所需要目标轨迹点对应的实际绳索长度值，因此，只需要将Ls^1′的值即可将目标轨迹点对应的实际绳索长度值下发给控制器，由控制器控制相应的收紧机构对相应的绳索进行收放。

通过采用本发明公开的上述技术方案，得到了如下有益的效果：

本发明提供了一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法,本方法通过预测绳索长度的方式，将绳索的误差反馈补偿到预测中，从预测开始，修正绳索的实际长度，从而使其控制姿态更加精准；解决了由于悬垂引起的控制精度不高的问题，提升了目标的控制精度，并改善了观感体验，为舞台盛宴的视觉效果提供了新的技术支持。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。

Claims (1)

1.一种基于预测绳索长度的四柔索牵引并联执行器运动姿态控制方法，其特征在于：包括如下步骤，

S1、根据并联执行器的轨迹坐标点以及四根绳索牵引处的坐标，获取四根绳索运动到轨迹坐标点的理论绳长；步骤S1具体为，

假设并联执行器的任意轨迹坐标点为E(x,y,z)，四根绳索牵引处的坐标点分别表示为A、B、C、D，这四个坐标点的坐标值分别为A(x₁,y₁,z₁)、B(x₂,y₂,z₂)、C(x₃,y₃,z₃)、D(x₄,y₄,z₄)；则四根绳索运动到任意轨迹坐标点E的理论绳长分别为L1、L2、L3、L4，计算公式如下，

S2、依据四根绳索运动到轨迹坐标点的理论绳长获取绳索的理论绳长集合；步骤S2中获取的绳索的理论绳长集合为L，其表达式为，

其中，n为并联执行器的运动轨迹点所对应的绳索记录数据；第一行的n个数据从左至右依次为第一根绳索运行到n个轨迹坐标点所对应的理论绳长；第二行的n个数据从左至右依次为第二根绳索运行到n个轨迹坐标点所对应的理论绳长；第三行的n个数据从左至右依次为第三根绳索运行到n个轨迹坐标点所对应的理论绳长；第四行的n个数据从左至右依次为第四根绳索运行到n个轨迹坐标点所对应的理论绳长；

S3、依据绳索的理论绳长集合，获取理论绳长集合对应的实际收紧状态下的绳长集合；步骤S3中获取的实际收紧状态下的绳长集合为Ls，其表达式为，

其中，第一行的n个数据从左至右依次为第一根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第二行的n个数据从左至右依次为第二根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第三行的n个数据从左至右依次为第三根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；第四行的n个数据从左至右依次为第四根绳索运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；

S4、根据实际收紧状态下的绳长集合，获取并联执行器的实际轨迹坐标点；步骤S4具体为，根据四根绳索实际收紧状态下的绳长，获取并联执行器的实际轨迹坐标点E_f(x_f,y_f,z_f)，计算过程如下，

其中，dLs为对Ls求导；J(E_f)为并联执行器在位置E_f处的雅克比矩阵；dE_f为对E_f求导；且

其中，m＝1,2,3,4；

利用牛顿迭代法求解并联执行器的实际轨迹坐标点E_f(x_f,y_f,z_f)：设置迭代初始位置为E_fc(x_fc,y_fc,z_fc)，定位精度要求为eps，

E_fc(x_fc,y_fc,z_fc)＝E_fc[0,0,0]

eps＝0.000001；

利用下式获取并联执行器的实际轨迹坐标点E_f(x_f,y_f,z_f)，

E_f ^r+1＝E_f ^r+.J^-1(E_f ^r)/(L^*-L^r)

其中，E_f ^r+1表示在r位置处的终止迭代位置；E_f ^r表示绳索在r位置处的迭代位置；L^*为驱动绳索的长度向量，L^r为第r次迭代得到的绳索长度值；J^-1(E_f ^r)为第r次迭代得到的雅克比矩阵的逆矩阵；r为迭代次数，迭代终止的条件是‖E_f ^r+1-E_f ^r‖≤eps；

S5、基于并联执行器的实际轨迹坐标点，利用实际收紧状态下的绳长集合，预测下一轨迹坐标点对应的各绳索的实际长度；步骤S5具体为，

通过实际收紧状态下的绳长集合Ls中的第m根绳索来预测其在下一轨迹坐标点对应的实际长度，绳索m在实际收紧状态下的绳长集合为Ls^m，

其中，括号中的n个数据从左至右依次为绳索m运行到n个轨迹坐标点所对应的实际收紧状态下的绳长；

经过一次累加Ls^m后生成的新数据集为,

其中，Ls^mm(i)为绳索m在实际收紧状态下的绳长集合Ls^m经过累加后的新数据集；j为原始数据集中的元素，i为累加数据集中的元素；

新数据集的组合表达式为，

设Ls^mm满足一阶常微分方程，则，

其中，t为时序；a，u为常数；则根据上式推导出Ls^mm的时序响应方程为，

其中，与/>通过最小二乘估计来获得；通过上式即可获取后时序的绳索收紧长度；后时序的绳索收紧长度集合为Ls^mm；k为数据变量值；k＝1,2,3,…,n时，Ls^mm(k+1)为拟合值；当k≥n时，Ls^mm(k+1)为预测值；

利用绳索的理论绳长集合L与后时序的绳索收紧长度集合为Ls^mm进行加权计算，即可获取绳索m在下一轨迹坐标点对应的实际长度Ls^m′，

其中，δ为权值系数，计算公式为，