一种连续在轨运动的六自由度捕获轨迹试验方法与流程

本发明属于高超声速风洞试验技术领域，具体涉及一种连续在轨运动的六自由度捕获轨迹试验方法。

背景技术：

风洞捕获轨迹试验技术（简称cts）是运用在风洞试验中的一种机电一体化装置，用来模拟分离体从主机上分离后的运动轨迹，为分离体在其主机上的合适布局和投放参数控制提供可靠依据。它利用计算机、六自由度装置和风洞相互配合，通过测量分离体模型的气动载荷求解分离体模型空间六自由度运动方程组来获取分离体模型的运动位置或速度，进而实现投放轨迹模拟。

目前，cts的控制方式主要是基于闭环的位置控制方式，分离体模型处于定速间歇式运动模式，主要存在以下不足：

1）分离体模型在运动到目标位置后暂停，进行信号测量和数据计算，然后再进行下一步的运动，并需要对位置进行反复修正，故获得一条分离体轨迹的时间较长，其试验效率低，能耗高；

2）分离体模型运动为非连续性，轨迹点的下一点位置是预测产生，并不一定和分离体模型六自由度动力学方程规定位置始终重合，模型重心也不能连续保持在真实运动轨迹上。

除此之外，国内外还有cts的速度控制方法，求解分离体飞行的运动学六自由度方程，算出给定时间步长后分离体的质心运动速度与绕质心转动的角速度，再使用插值方法得到连续的速度与角速度，然后控制六自由度机构带动分离体模型按照运动轨迹与运动姿态移动，逐步推进完成一条分离轨迹的测量，但求解出来的速度之间使用二次插值多项式的方法，运动的位置可能会与六自由度运动微分方程计算出的位置差别较大。

由于cts系统六自由度机构运动的轨迹不能在试验前确定，所以传统工业六自由度机械手的成熟的控制策略难以直接运用到cts试验的控制中。

技术实现要素：

本发明所要解决的技术问题是提供一种连续在轨运动的六自由度捕获轨迹试验方法。

本发明的连续在轨运动的六自由度捕获轨迹试验方法，其特点是：所述的试验方法使用的试验设备包括上位计算机、下位计算机、电机驱动器、电机、六自由度机构、位置传感器、速度传感器和测力天平；所述的电机驱动器包括x1轴电机驱动器、x2轴电机驱动器、y轴电机驱动器、z1轴电机驱动器、z2轴电机驱动器、α轴电机驱动器、β轴电机驱动器和γ轴电机驱动器；所述的电机包括与电机驱动器对应的x1轴电机、x2轴电机、y轴电机、z1轴电机、z2轴电机、α轴电机、β轴电机和γ轴电机；所述的电机轴上安装有绝对式编码器，绝对式编码器将采集的数据处理成位置和速度信息，绝对式编码器具有位置传感器和速度传感器的双重功能；

其连接关系是，所述的上位计算机、下位计算机和电机驱动器通过网线连接，电机驱动器和电机通过信号线和动力线连接，位置传感器、速度传感器和下位计算机通过信号线连接、测力天平和下位计算机通过信号线连接；分离体模型内部安装有测力天平，测力天平的支杆与六自由度机构固定连接；所述的电机驱动器驱动电机控制六自由度机构通过测力天平的支杆带动分离体模型运动；

所述的上位计算机安装有人机互动程序，人机互动程序用于读取试验参数和保存数据、触发试验、显示当前速度和位置、显示天平信号；

所述的下位计算机安装有plc软件，用于实时控制电机运动，并在plc软件内编写捕获轨迹试验的程序；

所述的位置传感器检测分离体模型的运行位置信号，并将其传送至下位计算机；

所述的速度传感器检测分离体模型的运行速度信号，并将其传送至下位计算机；

所述的测力天平测量分离体模型的气动力，并将其传送至下位计算机；

所述的连续在轨运动的六自由度捕获轨迹试验方法包括以下步骤：

1a.通过上位计算机人机交互界面输入与试验相关的测力天平系数、气动解算参数、轨迹运行段数m、每段轨迹中的轨迹点数n，模型几何缩比尺，速度变换尺度，单段轨迹运动完成时间，提前解算时间，天平延时时间，读取天平数据时间；输入六自由度机构运动学逆解矩阵r1，输入六自由度机构空间的速度映射到关节空间的雅克比矩阵r2；

1b.通过下位机计算机plc软件控制电机运动，将分离体模型运动至初始位姿（），并设置解算的轨迹段数i=0；

1c.判断当前解算的轨迹段数i是否为0，如果i=0，下位机计算机采集测力天平测量的分离体模型所受的力和力矩；如果i≠0，下位机计算机采集第i段轨迹在（t-t2-t3）至（t-t2）时间范围内天平测量分离体模型所受的力和力矩，并计算力和力矩的平均值；

1d.下位机计算机计算全尺寸分离体所受的力和力矩；

1e.通过微分方程组数值算法解算全尺寸分离体动力学方程组，获得第（i+1）段轨迹上n个轨迹点的位姿与速度；

1f.通过天地转换方法，获得风洞坐标系下分离体模型相应的n个轨迹点的位姿与速度；

1g.通过速度缩比与运动学逆解方法，获得分离体模型对应这n个轨迹点位姿与速度的各电机转角与转速；

1h.利用各电机转角与转速，通过电机运动曲线规划方法，规划各电机的运动曲线；

1i.已解算的轨迹段数i=i+1；

1j.判断此时i是否为1，如果i=1，则按第i段轨迹运动，如果i≠1，则等待第（i-1）段轨迹运动完成后，再按第i段轨迹运动；

1k.当第i段轨迹运动时间，满足时，判断i是否小于轨迹运行段数m，如果i≤m则回到步骤1c,如果i＞m则等待第i段轨迹运动完成后输出轨迹，结束试验。

所述的微分方程组数值算法包括一阶或二阶微分方程组数值算法中的一种。

所述的天地转换方法如下：

全尺寸分离体的质心运动由全尺寸分离体n个轨迹点的位姿（）与速度（）进行描述；

模型几何缩尺比为，开始试验时分离体模型的初始位姿（），风洞坐标系下分离体模型对应轨迹点的位姿()与速度()，具体计算公式如下：

所述的速度缩比与运动学逆解方法包括以下步骤：

4a.将分离体模型运动时间增加速度变换尺度倍，分离体模型各自由度速度同时减少速度变换尺度倍，则分离体模型实际运动时的速度（）为

4b.根据分离体模型某个轨迹点的位姿（）和六自由度机构运动学逆解矩阵r1、计算出各电机对应的转角（），具体计算公式如下：

实际运动时的速度（）和六自由度机构空间的速度映射到关节空间的雅克比矩阵r2，转速（），具体计算公式如下：

所述的电机运动曲线规划方法如下：

已知单段轨迹中相邻两个轨迹点的电机对应的角度，和角速度，（），在单段轨迹中相邻两个轨迹点运动时间后，使用三次多项式插值，得到运动曲线的方程如下：

其中：

。

本发明的连续在轨运动的六自由度捕获轨迹试验方法中涉及的天地转换方法的原理是：全尺寸分离体的质心运动是天上实际的轨迹，当转换为风洞中的情况时，分离体模型的质心位移按几何缩尺比减小，同理，分离体模型的线速度按全尺寸分离体质心的线速度几何缩尺比减小。因为天上的全尺寸分离体和风洞中的分离体模型的相对姿态不变，所以分离体模型姿态角不按几何缩尺比减小，分离体模型的角速度与全尺寸分离体的角速度相同。

本发明的连续在轨运动的六自由度捕获轨迹试验方法中用户由上位计算机人机交互界面输入试验与模型相关参数，下位计算机求解全尺寸分离体飞行六自由度运动学方程，算出给定时间步长后全尺寸分离体的位姿和速度；根据求解出来的数据，进行天地转换，得到风洞坐标系下分离体模型的轨迹点，通过速度缩比和六自由度机构逆解得到各电机对应的转角与转速，利用各电机的转角和转速，规划出一条电机运动的连续曲线。规划完成后，通知下位计算机控制电机使cts六自由度机构带动分离体模型运动，在轨迹运动完之前，考虑天平信号延迟以及分离体飞行六自由度动力学方程求解时间，提前解算并规划下一条轨迹，保证分离体模型从一条轨迹平滑过渡到下一条轨迹。

本发明的连续在轨运动的六自由度捕获轨迹试验方法是一种提高分离体模型运动轨迹精度的cts试验方法，该试验方法提出了连续在轨控制方式，保证了分离体模型按照分离体飞行六自由度动力学方程解算出来的轨迹进行运动。该试验方法能够同时控制所计算出来轨迹点的位姿和速度，在模型运动过程中提前测量天平数据，求解全尺寸分离体六自由度运动方程，并规划下一段运动轨迹，无需暂停运动进行信号测量和数据计算，使得整个分离体模型运动轨迹无间歇，确保分离体模型保持在真实轨迹上，提高了试验精度，增大了试验数据量，缩短了cts试验时间，降低了试验成本。

附图说明

图1为本发明中的分离体模型六自由度机构控制系统原理图；

图2为本发明中的捕获轨迹试验流程图。

具体实施方式

下面结合附图和实施例详细说明本发明。

如图1所示，本发明的连续在轨运动的六自由度捕获轨迹试验方法使用的试验设备包括上位计算机、下位计算机、电机驱动器、电机、六自由度机构、位置传感器、速度传感器和测力天平；所述的电机驱动器包括x1轴电机驱动器、x2轴电机驱动器、y轴电机驱动器、z1轴电机驱动器、z2轴电机驱动器、α轴电机驱动器、β轴电机驱动器和γ轴电机驱动器；所述的电机包括与电机驱动器对应的x1轴电机、x2轴电机、y轴电机、z1轴电机、z2轴电机、α轴电机、β轴电机和γ轴电机；所述的电机轴上安装有绝对式编码器，绝对式编码器将采集的数据处理成位置和速度信息，绝对式编码器具有位置传感器和速度传感器的双重功能；

其连接关系是，所述的上位计算机、下位计算机和电机驱动器通过网线连接，电机驱动器和电机通过信号线和动力线连接，位置传感器、速度传感器和下位计算机通过信号线连接、测力天平和下位计算机通过信号线连接；分离体模型内部安装有测力天平，测力天平的支杆与六自由度机构固定连接；所述的电机驱动器驱动电机控制六自由度机构通过测力天平的支杆带动分离体模型运动；

所述的上位计算机安装有人机互动程序，人机互动程序用于读取试验参数和保存数据、触发试验、显示当前速度和位置、显示天平信号；

所述的下位计算机安装有plc软件，用于实时控制电机运动，并在plc软件内编写捕获轨迹试验的程序；

所述的位置传感器检测分离体模型的运行位置信号，并将其传送至下位计算机；

所述的速度传感器检测分离体模型的运行速度信号，并将其传送至下位计算机；

所述的测力天平测量分离体模型的气动力，并将其传送至下位计算机；

如图2所示，所述的连续在轨运动的六自由度捕获轨迹试验方法包括以下步骤：

1a.通过上位计算机人机交互界面输入与试验相关的测力天平系数、气动解算参数、轨迹运行段数m、每段轨迹中的轨迹点数n，模型几何缩比尺，速度变换尺度，单段轨迹运动完成时间，提前解算时间，天平延时时间，读取天平数据时间；输入六自由度机构运动学逆解矩阵r1，输入六自由度机构空间的速度映射到关节空间的雅克比矩阵r2；

1b.通过下位机计算机plc软件控制电机运动，将分离体模型运动至初始位姿（），并设置解算的轨迹段数i=0；

1c.判断当前解算的轨迹段数i是否为0，如果i=0，下位机计算机采集测力天平测量的分离体模型所受的力和力矩；如果i≠0，下位机计算机采集第i段轨迹在（t-t2-t3）至（t-t2）时间范围内天平测量分离体模型所受的力和力矩，并计算力和力矩的平均值；

1d.下位机计算机计算全尺寸分离体所受的力和力矩；

1e.通过微分方程组数值算法解算全尺寸分离体动力学方程组，获得第（i+1）段轨迹上n个轨迹点的位姿与速度；

1f.通过天地转换方法，获得风洞坐标系下分离体模型相应的n个轨迹点的位姿与速度；

1g.通过速度缩比与运动学逆解方法，获得分离体模型对应这n个轨迹点位姿与速度的各电机转角与转速；

1h.利用各电机转角与转速，通过电机运动曲线规划方法，规划各电机的运动曲线；

1i.已解算的轨迹段数i=i+1；

1j.判断此时i是否为1，如果i=1，则按第i段轨迹运动，如果i≠1，则等待第（i-1）段轨迹运动完成后，再按第i段轨迹运动；

1k.当第i段轨迹运动时间，满足时，判断i是否小于轨迹运行段数m，如果i≤m则回到步骤1c,如果i＞m则等待第i段轨迹运动完成后输出轨迹，结束试验。

所述的微分方程组数值算法包括一阶或二阶微分方程组数值算法中的一种。

所述的天地转换方法如下：

全尺寸分离体的质心运动由全尺寸分离体n个轨迹点的位姿（）与速度（）进行描述；

模型几何缩尺比为，开始试验时分离体模型的初始位姿（），风洞坐标系下分离体模型对应轨迹点的位姿()与速度(),具体计算公式如下：

。

所述的速度缩比与运动学逆解方法包括以下步骤：

4a.将分离体模型运动时间增加速度变换尺度倍，分离体模型各自由度速度同时减少速度变换尺度倍，则分离体模型实际运动时的速度（）为

；

4b.根据分离体模型某个轨迹点的位姿（）和六自由度机构运动学逆解矩阵r1、计算出各电机对应的转角（），具体计算公式如下：

实际运动时的速度（）和六自由度机构空间的速度映射到关节空间的雅克比矩阵r2，转速（），具体计算公式如下：

。

所述的电机运动曲线规划方法如下：

已知单段轨迹中相邻两个轨迹点的电机对应的角度，和角速度，（），在单段轨迹中相邻两个轨迹点运动时间后，使用三次多项式插值，得到运动曲线的方程如下：

其中：

。

实施例1

本实施例按照上述实施方式实施，需要注意的是微分方程组数值算法采用一阶微分方程组数值算法。微分方程组公式如下：

其中，依次为全尺寸分离体受到的沿x向,y向和z向的合力；

依次为全尺寸分离体受到的绕x轴,y轴和z轴的合力矩；

为依次为全尺寸分离体相对主体的速度沿其体轴x向、y向和z向的分量，单位；

为依次为全尺寸分离体相对主体的速度沿其体轴x向、y向和z向的分量的变化率，单位；

为依次为全尺寸分离相对主体转动的角速度沿其体轴x轴、y轴和z轴的投影，单位；

为依次为全尺寸分离相对主体转动的角加速度沿其体轴x轴、y轴和z垂轴的投影，单位；

为依次为全尺寸分离体绕其体轴x轴、y轴和z轴转动的惯性矩，单位；

为全尺寸分离体关于在其纵向对称面内的转动惯性积，单位

为全尺寸分离体的质量，单位。

以上方程组为全尺寸分离体动力学方程组，求解以上方程组使用四阶经典rungekutta法，将上一时刻的全尺寸分离体速度和角速度以及全尺寸分离体受到的合力和合力矩，代入四阶经典rungekutta公式中，求解出下一时刻的分离体速度（）和角速度（）的变化率。将变化率积分后的增量带入上一时刻的速度和角速度可得到当前时刻的速度和角速度，再积分可以得到位置（）和角度（），得到每段轨迹上每个轨迹点的位姿（）和速度（）。

实施例2

本实施例与实施例1的实施方式基本相同，主要区别在于：微分方程组数值算法采用二阶微分方程组数值算法。微分方程组公式如下：

其中，为全尺寸分离体的质量；

为全尺寸分离体在固定坐标系下位移向量，，为其二阶导数向量；

滚转角，偏航角为俯仰角，为对应姿态角一阶导数向量，为对应姿态角二阶导数向量；

为分离体模型相对固定坐标系的方向余弦矩阵；其计算公式如下：

为体轴系角速度，为体轴系角速度变化率；

为和的转化矩阵，其公式为：

为全尺寸分离体所受的合力向量；

为全尺寸分离体所受到合力矩向量；

为分离体关于分离体体轴系的转动惯量矩阵；

为体轴系角速度坐标方阵，其中方阵表达方式如下：

得到以上二阶微分方程组公式后，用纽马克β法等数值解法，算出、及其一阶导数。

本发明不局限于上述具体实施方式，所属技术领域的技术人员从上述构思出发，不经过创造性的劳动，所作出的种种变换，均落在本发明的保护范围之内。