基于斯图尔特平台的六自由度摇摆装置的加减速方法

摘要

本发明公开了一种基于斯图尔特平台的六自由度摇摆装置的加减速方法，提供六自由度摇摆装置，包括如下步骤：步骤S10、位置反解，获取所述六自由度摇摆装置的初始位姿坐标，通过计算获取六个所述电缸支腿的伸缩量信息；步骤S20、位置正解，根据六个所述电缸支腿的所述伸缩量信息，采用牛顿‑拉夫逊迭代法进行位置正解计算，通过多次采样、多次计算得到最终的初始迭代值；步骤S30、轨迹规划，通过定时插补法与正弦加减速控制算法相结合的方式对所述六自由度摇摆装置进行控制。与相关技术相比，本发明提供的基于斯图尔特平台的六自由度摇摆装置的加减速方法，能够解决传统控制发明中存在的运动产生轻微冲击问题，并提高平台使用寿命。

说明书

技术领域

本发明涉及运动平台技术领域，尤其涉及一种基于斯图尔特平台的六自由度摇摆装置的加减速方法。

背景技术

斯图尔特(Stewart)平台是一个具有较强非线性耦合特征的多输入、多输出控制系统，其运动学正反解算法的精度和实时性以及动力学控制的鲁棒性的决定其控制系统性能的关键，传动的六轴斯图尔特平台控制系统采用计算机+运动控制卡或运动控制器和嵌入式控制器的发明。

目前的斯图尔特平台的六自由度摇摆装置一般作为实验装置，模拟船只在水中的摇晃姿态，提供六个自由度的运动，最多模拟出三个轴的空间运动。然而，传统的斯图尔特平台在运动时有些容易被忽视的缺点：首先，为了提高机械结构的响应速度和精度，提高控制的增益，导致机械间的磨损加快，并且短期内无法察觉；其次，高增益带来的瞬时高加速度对整个斯图尔特平台装置产生冲击，对试验对象产生一定的影响，不适合提供高精度的试验条件。因此，由于传统的线性控制发明在控制周期伊始的偏差过大，导致控制量偏大，造成类似于阶跃信号对结构造成冲击，也正是由于这样的方式能够使得相应速度得到提升。

在目前的设备中，在一些要求不高的场合，一般使用梯形加减速或指数型加减速。这两种发明的优点是运算量小、实现简单，缺点是在加减速启停阶段，会出现速度突变和加速度突变，对机械系统会产生冲击。梯形加减速控制算法是指系统在加速时，速度沿固定斜率直线上升，加速度保持不变，速度均匀增加；而在减速时，速度则沿一定斜率直线下降，加速度保持不变，速度均匀下降。传统的提醒加减速分为三个阶段：1)加速过程阶段，速度沿固定斜率直线上升：vi+1＝vi+aT(1)式中：vi+1为第i+1个插补周期的速度；vi为第i个插补周期的速度；a为用户设定的加速度；T为插补周期。2)匀速过程阶段，速度保持不变，即vi+1＝vi(2)。3)减速过程阶段，速度沿直线下降：vi+1＝vi-aT(3)这种梯形的加减速算法的优点是实现起来简单，运算量小，适合用于低速、低成本的系统解决方案。但是，因为它采用恒定的加速度，加加速度和加速度的导数0，因而在加减速的起点和终点位置加速度有突变，这也导致在实际的系统中，出现伺服电机的各种震动和噪声。系统运行不够平稳等现象。

因此，有必要提供一种能够解决传统控制发明中存在的运动产生轻微冲击问题，并提高平台使用寿命的基于斯图尔特平台的六自由度摇摆装置的加减速方法来解决上述问题。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种能够解决传统控制发明中存在的运动产生轻微冲击问题，并提高平台使用寿命的基于斯图尔特平台的六自由度摇摆装置的加减速方法。

为实现上述目的，本发明提供了如下技术方案:

一种基于斯图尔特平台的六自由度摇摆装置的加减速方法，提供六自由度摇摆装置，所述六自由度摇摆装置包括运动平台、固定平台、连接所述运动平台与所述固定平台的六个结构完全相同的电缸支腿及与所述电缸支腿连接的伺服电机，所述运动平台通过第一连接铰与六个所述电缸支腿的一端连接，所述固定平台通过第二连接铰与六个所述电缸支腿的另一端连接，包括如下步骤：

步骤S10、位置反解，获取所述六自由度摇摆装置的初始位姿坐标，通过计算获取六个所述电缸支腿的伸缩量信息；

步骤S20、位置正解，根据六个所述电缸支腿的所述伸缩量信息，采用牛顿-拉夫逊迭代法进行位置正解计算，通过多次采样、多次计算得到最终的初始迭代值；

步骤S30、轨迹规划，通过定时插补法与正弦加减速控制算法相结合的方式对所述六自由度摇摆装置进行控制。

优选的，所述伺服电机设置于所述电缸支腿靠近所述固定平台的一端。

优选的，步骤S20中，所述牛顿-拉夫逊迭代法采用小周期采样。

优选的，步骤S20包括如下子步骤：

步骤S21、读取通过计算得到的六个所述电缸支腿的伸缩量信息Ld_i；

步骤S22、设置迭代位姿初值P与收敛半径ε；

步骤S23、通过位姿初值P反解计算得到L0_i；

步骤S24、计算所述电缸支腿的变化量fi＝L0_i-Ld_i；

判断fi绝对值的平方是否小于ε，若结果为是，则得到的P值为正解所求位姿，若结果为否，则对P值进行修正并带入步骤S23中，直至结果为是。

优选的，在步骤S24中，对P值的修正包括如下子步骤：

步骤S241、求迭代雅克比矩阵J；

步骤S242、通过所述迭代雅克比矩阵J重新获取位姿初值P。

优选的，步骤S30包括如下子步骤：

步骤S31、设置系统参数，定义最大加速度为Amax，插拔周期为T；

步骤S32、设置加减速参数，定义最大速度为Vmax，初始速度为Va，结束速度为Vb，行驶路程为S；

步骤S33、计算判断关键参数，计算Vmax条件下加速段正向积分值：Sup；

计算Vmax条件下减速段方向积分值：sdn；

判断S>Sup+Sdn是否成立，若不成立，则进行步骤S34与步骤S35，若不成立，则进行步骤S36与S37；

步骤S34、计算Vlim＝(4/pi*Amax*S+Vb+Vb+Va*Va)；

计算Vlim条件下加速度段正向积分值：Svup；

计算Vlim条件下减速段反向积分值：Svdn；

步骤S35、按照Vlim进行插补计算，按Svup，Svdn做插补速度函数切换，判断插补结束条件是否满足，若满足则进行步骤S37，不满足则重复此步骤；

Nvup＝pi*(Vlim-Va)/(2*Amax*T),Nvdn＝pi(Vlim-Vb)/(2*Amax*T)；

Vvup＝Va+(Vlim-Va)/2*(1-cos(piXcup/Nvup))；

Xvup＝Tick*T,Tick为插补街拍，加速段从零计算；

Xvdn＝Tick*T,Tick为插补街拍，减速段从零开始；

步骤S36、按Vmax进行插补计算，按Sup，Sdn，Seq做插补速度函数切换，判断插补结束条件是否满足，满足则进行步骤S37，不满足则重复此步骤；

Nvup＝pi*(Vlim-Va)/(2*Amax*T),Nvdn＝pi(Vlim-Vb)/(2*Amax*T)；

加速段：Vup＝Va+(Vmax-Va)/2*(1-cos(pi*Xup)/Nup)；

Xup＝Tick*T,Tick为插补节拍，加速段从零计算；

匀速段：Veq＝Vmax,匀速段距离Seq＝S-Sup–Sdn；

减速段：Vdn＝Vb+(Vmax-Vb)/2*(1+cos(pi*Xdn/Ndn))；

步骤S37、结束速度规划，进入下一条准备，数据化分析；

循环往复得到整个运行周期的每个时间T的运动位移。

综上所述，与现有技术相比，本发明提供的基于斯图尔特平台的六自由度摇摆装置的加减速方法，因为正弦速度的轨迹的导数是余弦，该导数便为运动过程中的加速度，因此，这样的计算方法对加速度的规划是非常平稳且有规律可循的，不会出现不可控的振动，同时，首先通过反解算法与正解算法计算出每个小周期的位姿，再通过对运动轨迹进行轨迹规划，通过正余弦的方式对每个周期的位移进行计算和分配，是斯图尔特平台的位姿轨迹的速度与加速度更趋于平滑，使得平台在启停和连续运动阶段不会出现抖动现象。

附图说明

图1为本发明提供的基于斯图尔特平台的六自由度摇摆装置的加减速方法的流程框图；

图2为本发明提供的基于斯图尔特平台的六自由度摇摆装置的加减速方法中步骤S20的算法流程图。

具体实施方式

下面结合附图和实施例，对本发明进行详细描述。下述实验例和实施例用于进一步说明但不限于本发明。

请参阅结合图1与图2，本发明提供了一种基于斯图尔特平台的六自由度摇摆装置的加减速方法，应用于六自由度摇摆装置，所述六自由度摇摆装置包括运动平台、固定平台、连接所述运动平台与所述固定平台的六个结构完全相同的电缸支腿及与所述电缸支腿连接的伺服电机。其中，所述运动平台通过第一连接铰与六个所述电缸支腿的一端连接，所述固定平台通过第二连接铰与六个所述电缸支腿的另一端连接。优选的，在本实施方式中，所述伺服电机设置于所述电缸支腿靠近所述固定平台的一端。

具体包括如下步骤：

步骤S10、位置反解，获取所述六自由度摇摆装置的初始位姿坐标，通过计算获取六个所述电缸支腿的伸缩量信息；在此步骤中，主要是通过所述六自由度摇摆装置的初始位姿坐标(X，Y，Z，α，β，γ)求解出六个所述电缸支腿的伸缩量(h1，h2，h3，h4，h5，h6)。

具体的，在惯性坐标中，已知上铰点与下铰点的坐标矩阵，可分别得出六个所述电缸支腿的长度表示为：

hi＝|AiBi|，i＝1，2，3，4，5，6；

Δhi＝hi-h，i＝1，2，3，4，5，6。其中，h为六个所述电缸支腿的初始长度。

步骤S20、位置正解，根据六个所述电缸支腿的所述伸缩量信息，采用牛顿-拉夫逊迭代法进行位置正解计算，通过多次采样、多次计算得到最终的初始迭代值；

优选的，为了避免出现算法不收敛的现象，在本步骤中，所述牛顿-拉夫逊迭代法采用小周期采样。如此设置，保证了一个采样周期的平台起点位姿和终点位姿变化小，将上一采样周期的正解计算结果作为本次计算的初始迭代值，具有计算收敛速度快，计算结果精度高的效果。

具体的，步骤S20包括如下子步骤：

步骤S21、读取通过计算得到的六个所述电缸支腿的伸缩量信息Ld_i；

步骤S22、设置迭代位姿初值P与收敛半径ε；

步骤S23、通过位姿初值P反解计算得到L0_i；

步骤S24、计算所述电缸支腿的变化量fi＝L0_i-Ld_i；

判断fi绝对值的平方是否小于ε，若结果为是，则得到的P值为正解所求位姿，若结果为否，则对P值进行修正并带入步骤S23中，直至结果为是；

更为具体的，在步骤S24中，对P值的修正包括如下子步骤：

步骤S241、求迭代雅克比矩阵J；

步骤S242、通过所述迭代雅克比矩阵J重新获取位姿初值P。

步骤S30、轨迹规划，通过定时插补法与正弦加减速控制算法相结合的方式对所述六自由度摇摆装置进行控制。

具体的，步骤S30包括如下子步骤：

步骤S31、设置系统参数，定义最大加速度为Amax，插拔周期为T；

步骤S32、设置加减速参数，定义最大速度为Vmax，初始速度为Va，结束速度为Vb，行驶路程为S；

步骤S33、计算判断关键参数，计算Vmax条件下加速段正向积分值：Sup；

计算Vmax条件下减速段方向积分值：sdn；

判断S>Sup+Sdn是否成立，若不成立，则进行步骤S34与步骤S35，若不成立，则进行步骤S36与S37；

步骤S34、计算Vlim＝(4/pi*Amax*S+Vb+Vb+Va*Va)；

计算Vlim条件下加速度段正向积分值：Svup；

计算Vlim条件下减速段反向积分值：Svdn；

步骤S35、按照Vlim进行插补计算，按Svup，Svdn做插补速度函数切换，判断插补结束条件是否满足，若满足则进行步骤S37，不满足则重复此步骤；

Nvup＝pi*(Vlim-Va)/(2*Amax*T),Nvdn＝pi(Vlim-Vb)/(2*Amax*T)；

Vvup＝Va+(Vlim-Va)/2*(1-cos(piXcup/Nvup))；

Xvup＝Tick*T,Tick为插补街拍，加速段从零计算；

Xvdn＝Tick*T,Tick为插补街拍，减速段从零开始；

步骤S36、按Vmax进行插补计算，按Sup，Sdn，Seq做插补速度函数切换，判断插补结束条件是否满足，满足则进行步骤S37，不满足则重复此步骤；

Nvup＝pi*(Vlim-Va)/(2*Amax*T),Nvdn＝pi(Vlim-Vb)/(2*Amax*T)；

加速段：Vup＝Va+(Vmax-Va)/2*(1-cos(pi*Xup)/Nup)；

Xup＝Tick*T,Tick为插补节拍，加速段从零计算；

匀速段：Veq＝Vmax,匀速段距离Seq＝S-Sup–Sdn；

减速段：Vdn＝Vb+(Vmax-Vb)/2*(1+cos(pi*Xdn/Ndn))；

步骤S37、结束速度规划，进入下一条准备，数据化分析；

循环往复得到整个运行周期的每个时间T的运动位移。

与现有技术相比，本发明提供的基于斯图尔特平台的六自由度摇摆装置的加减速方法，因为正弦速度的轨迹的导数是余弦，该导数便为运动过程中的加速度，因此，这样的计算方法对加速度的规划是非常平稳且有规律可循的，不会出现不可控的振动，同时，首先通过反解算法与正解算法计算出每个小周期的位姿，再通过对运动轨迹进行轨迹规划，通过正余弦的方式对每个周期的位移进行计算和分配，是斯图尔特平台的位姿轨迹的速度与加速度更趋于平滑，使得平台在启停和连续运动阶段不会出现抖动现象。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和修饰，这些改进和修饰也应视为本发明的保护范围。