一种运载火箭加泄连接器自动对接轨迹规划方法与流程

本发明涉及一种运载火箭加泄连接器自动对接轨迹规划方法，属于运载火箭地面设备加注技术领域。

背景技术：

运载火箭或导弹与地面设备之间的气、液接口设备一般称为连接器，液体运载火箭箭体推进剂加注口和地面加注设备的接口设备成为加泄连接器。在发射之前，运载火箭一直通过加泄连接器与加注管路连接在一起，完成推进剂的加注工作。

而加泄连接器和箭体推进剂加注口的连接，需要进行对接操作。现有技术条件下，加泄连接器和箭体推进剂加注口的连接均通过人员手动操作完成，而运载火箭的推进剂一般为肼类燃料或低温燃料，人员手动操作存在较大的安全风险，因此加泄连接器与箭体推进剂加注口的自动对接应运而生。

运载火箭在加注时，会受到风载荷的影响而导致未知的随机晃动，这给自动对接的控制增加了难度。另外，箭体活门与连接器相对速度、相对加速度均有严格的要求，因此在精对接阶段需要对连接器进行轨迹规划，使对接瞬间运载火箭加泄连接器和箭体推进剂加注口的相对速度和相对加速度在安全范围内，即使对接瞬间二者受力和冲击均处于安全范围内，以保证连接器与箭体推进剂加注口在对接瞬间不会损坏。

技术实现要素：

本发明的技术解决问题是：克服现有技术的不足，提供一种易于实现、安全性高的运载火箭加泄连接器自动对接轨迹规划方法，消除运载火箭加泄连接器和运载火箭箭体推进剂加注口对接时的撞击风险，实现安全可靠对接。

本发明的技术解决方案是：一种运载火箭加泄连接器自动对接轨迹规划方法，步骤如下：

(1)根据已知的运载火箭在地面与加泄连接器自动对接过程中的晃动和局部变形的最大包络范围，通过实时测量加泄连接器和运载火箭箭体推进剂加注口的距离，控制六自由度运动机构沿着轴向运动至最大包络范围的边缘，使加泄连接器位于最大包络范围之外一定距离，完成接近运动；

(2)除加泄连接器沿至运载火箭箭体推进剂加注口的方向运动的运动自由度以外，调整加泄连接器的其余五个自由度后，使加泄连接器与运载火箭箭体推进剂加注口在其余五个自由度的位置和姿态一致，完成粗对接；

(3)根据步骤(1)加泄连接器位于最大包络范围之外的距离以及最大包络范围，使用轨迹规划算法进行精对接运动的轨迹规划，即确定加泄连接器的轨迹方程；

(4)保持其余五个自由度的位置和姿态调整，使加泄连接器与运载火箭箭体推进剂加注口在其余五个自由度的位置和姿态始终实时保持一致，根据接近运动的轨迹规划驱动加泄连接器沿加泄连接器与运载火箭箭体推进剂加注口的直线连接线，向运载火箭箭体推进剂加注口运动，直至加泄连接器沿加泄连接器与运载火箭箭体推进剂加注口接触上后完成对接，使加泄连接器与运载火箭箭体推进剂加注口接触时刻的速度一致。

所述的步骤(3)中的轨迹规划算法为：

定义约束条件如下：

①位置约束：z(t0)＝z0，z(t0+δt)＝0

②速度约束：v(t0)＝0，v(t0+δt)＝0

③加速度约束：a(t0)＝0，a(t0+δt)＝0

其中，t0为算法起始时间即轨迹规划运动的开始时刻，δt为算法运行时间即从粗对接完成时刻至完成对接时刻的时间，z0为运载火箭箭体推进剂加注口与连接器的已知距离即完成接近运动后加泄连接器和运载火箭箭体推进剂加注口的距离，z(t0)表示t0时刻加泄连接器和运载火箭箭体推进剂加注口的距离；v(t0)是指t0时刻加泄连接器和运载火箭箭体推进剂加注口的相对速度；a(t0)是指t0时刻加泄连接器和运载火箭箭体推进剂加注口的相对加速度；

采用五次多项式插值方法来得到满足约束要求的加泄连接器的轨迹方程为：

z(t)表示t时刻加泄连接器和运载火箭箭体推进剂加注口的实时距离；

根据除加泄连接器沿至运载火箭箭体推进剂加注口的方向运动的运动自由度以外加泄连接器的其余五个自由度与加泄连接器的轨迹方程z(t)表示的自由度线性叠加，得到加泄连接器的六自由度变化量。

空间六自由度运动的机械结构包括驱动机构，根据空间六自由度运动的机械结构的几何关系，通过逆运动学计算得出机械结构中的驱动机构即作动器的线位移或者角位移，并驱动作动器动作。

δt的求取方法为：根据精对接运动中空间六自由度运动的机械结构的极限速度及z0进行确定。

加泄连接器位于最大包络范围之外一定距离中的一定距离为5mm～10mm。

所述的加泄连接器指运载火箭与推进剂加注设备之间的连接装置，加泄连接器为包括加注管路和外围设备，呈圆柱型，加泄连接器自动对接过程为将加泄连接器安装于一个具备空间六自由度运动的机械结构上，即三个平动自由度、三个转动自由度运动，机械结构中能够主动进行直线运动或转动的机构为作动器，通过控制算法驱动此机械结构完成加泄连接器与运载火箭箭体的连接；

本发明与现有技术相比具有如下优点：

(1)本发明将自动对接过程分为三个阶段，并在精对接阶段进行轨迹规划，工程上可实现，填补了国内运载火箭加泄连接器自动对接控制领域的空白。

(2)本发明的轨迹规划方法不同于现有轨迹规划算法，现有轨迹规划算法主要是针对静止固定目标与运动规律已知的移动目标设计，本发明的轨迹规划算法专门为随机可测运动目标进行轨迹规划设计，算法对随机可测运动目标的跟踪而言，稳定性好。

(3)本发明兼顾自动对接的快速性、安全性要求，仅在精对接阶段引入轨迹规划方法以提升对接运动的安全性，其余阶段不考虑轨迹规划问题，保证对接运动的快速性。

(4)本发明的轨迹规划方法将精对接运动阶段分离为向箭体推进剂加注口的接近运动和以一已知距离的跟踪运动，在理论上实现对接瞬间箭体推进剂加注口和加泄连接器的相对速度、相对加速度为零，理论上杜绝了撞击风险。

(5)本发明的轨迹规划方法不同于现有轨迹规划算法预先设定轨迹，而是按照约束条件实时地调整规划轨迹，以适应箭体推进剂加注口的随机晃动。

附图说明

图1为本发明轨迹规划方法的流程图；

图2为本发明自动对接各阶段的流程图；

图3为本发明的坐标定义和位姿空间示意图；

具体实施方式

本发明一种运载火箭加泄连接器自动对接轨迹规划方法，将自动对接过程分为三个阶段，分别为接近运动阶段、粗对接阶段、精对接阶段，然后将精对接阶段自动对接运动描述为随机可测目标的伺服跟踪运动，并采用线性叠加的方法，将自动对接运动分为两部分：一是加泄连接器和箭体推进剂加注口以一已知距离进行伺服跟踪运动，二是加泄连接器和箭体推进剂加注口基于轨迹规划算法的接近运动，分别对两部分运动进行控制算法设计，对两部分运动量线性叠加后得出自动对接运动的位移量，最后实现自动对接运动。精对接阶段过程中，需要加泄连接器与箭体推进剂加注口接触，由于运载火箭在风载荷的影响下会出现随机晃动，因此对接过程中存在加泄连接器与箭体推进剂加注口对接时刻发生撞击的风险，本发明提出的轨迹规划方法可有效消除撞击风险，提升了自动对接安全性。

如图2所示，，本发明的基本思路为：一种运载火箭加泄连接器自动对接轨迹规划方法，其特征在于步骤如下：

(1)根据已知的火箭在地面与加泄连接器自动对接过程中的晃动和局部变形的最大包络范围，通过实时测量加泄连接器和运载火箭箭体推进剂加注口的距离，控制六自由度运动机构沿着轴向运动至最大包络范围的边缘，使加泄连接器位于最大包络范围之外一定距离，完成接近运动；

(2)除加泄连接器沿至运载火箭箭体推进剂加注口的方向运动的运动自由度以外，调整加泄连接器的其余五个自由度后，使加泄连接器与运载火箭箭体推进剂加注口在其余五个自由度的位置和姿态一致，完成粗对接；

(3)根据步骤(1)加泄连接器位于最大包络范围之外的距离以及最大包络范围，使用轨迹规划算法进行精对接运动的轨迹规划，即确定加泄连接器的轨迹方程；

(4)保持其余五个自由度的位置和姿态调整，使加泄连接器与运载火箭箭体推进剂加注口在其余五个自由度的位置和姿态始终实时保持一致，根据接近运动的轨迹规划驱动加泄连接器沿加泄连接器与运载火箭箭体推进剂加注口的直线连接线，向运载火箭箭体推进剂加注口运动，直至加泄连接器沿加泄连接器与运载火箭箭体推进剂加注口接触上后完成对接，使加泄连接器与运载火箭箭体推进剂加注口接触时刻的速度一致。

下面结合附图和具体实施例对本发明进行详细说明。

如图2所示，在运载火箭加注准备过程中，需要将加泄连接器和箭体推进剂加注口进行对接，而运载火箭在风载荷的影响下，箭体推进剂加注口会出现随机晃动，出于安全的考虑，在加泄连接器与箭体推进剂加注口对接瞬间，二者的相对速度和相对加速度均有严格的要求，以防止撞击导致的安全事故的发生。

由于箭体刚度和结构的限制，箭体推进剂加注口随机晃动在一定的空间范围内，空间范围除空间位置外还包括空间姿态，即加注口的随机晃动为空间六自由度随机晃动，这个范围为最大包络范围，通过一定的手段可以监测到最大包络范围和各自由度上的晃动最大速度的角速度。

加泄连接器安装于一个具备空间六自由度运动能力的机构上，即六自由度机构上，六自由度的驱动机构为作动器。将加泄连接器前端轴心作为机构末端位置，则建立坐标系如下：由于加泄连接器呈圆柱体形状，因此选择在连接器的前端轴心处建立惯性坐标系oi-xiyizi与连接器前端轴心的连体坐标系oc-xcyczc，两个坐标系的初始位姿完全重合，其中连体坐标系oc-xcyczc在自动对接系统工作过程中随连接器一起运动，并且连体坐标系与加泄连接器的相对位置保持不变，即加泄连接器各部分在连体坐标系下的坐标保持不变。坐标示意图如图3所示。

如图3所示，以连接器前端轴心为坐标原点oi(oc)；xi(xc)轴正方向指向铅垂向上方向，zi(zc)轴与水平线平行，方向指向连接器外部，yi(yc)轴由右手定则确定。

根据坐标定义，连体坐标系oc-xcyczc相对于惯性坐标系oi-xiyizi的位姿可以使用广义坐标向量qc来表示，其中tc为连体坐标系原点在惯性坐标系下的坐标向量tc＝[xc,yc,zc]^t，pc为连体坐标系在惯性坐标系系下的姿态角(欧拉角)，其中其中，为连体坐标系绕xi轴旋转角度，也称为偏航角或偏转角；θc为连体坐标绕yi轴旋转角度，也称为俯仰角；φc为连体坐标系绕zi轴旋转角度，也称为滚转角。

在未晃动条件下，箭体推进剂加注口在惯性坐标系下的初始位姿的广义坐标为(0,0,1,0,0,0)，箭体推进剂加注口的晃动包络范围为(±0.1，±0.1，1±0.1，±1，±1，±1)，其中姿态坐标的单位为弧度，位置坐标的单位为米。

自动对接的三个阶段为，接近运动阶段、粗对接阶段、精对接阶段。如图3所示。

1)接近运动阶段：控制六自由度机构使加泄连接器沿zi轴方向移动至惯性坐标系下广义坐标(0，0，0.85，0，0，0)处，即先运动到距离箭体推进剂加注口晃动最大包络范围0.05m处，处于最大包络范围之外，接近运动阶段采用恒速度规划，使连接器运动到0.85m处箭体推进剂加注口最大包络范围外时能够准确定位，不会因为超调导致进入到箭体推进剂加注口晃动最大包络范围内，以降低撞击的潜在风险。

2)粗对接阶段：惯性坐标系下，zi＝0.85并垂直于z轴的铅垂面内，控制六自由度机构使加泄连接器在惯性坐标系的广义坐标，即连体坐标系在惯性坐标系下的广义坐标，除zi方向的坐标外与箭体推进剂加注口在惯性坐标系的广义坐标保持一致，即控制加泄连接器对箭体推进剂加注口进行xi、yi、偏航、俯仰、滚转方向的随动运动，使以上五个自由度能够顺利进入随动跟踪的误差允许范围之内。完成粗对接阶段。

3)精对接阶段：控制六自由度机构使加泄连接器在保持对箭体推进剂加注口在惯性坐标系中xi、yi、偏航、俯仰、滚转方向随动运动的基础上，沿zi轴正方向运动，即加泄连接器向箭体推进剂加注口方向运动以完成对接。在精对接阶段，引入轨迹规划算法如下：

如图1所示，为本发明的轨迹规划方法的流程图。首先，进行自动对接运动分解为两部分，其一是将沿zi轴的对接运动分解为以0.85m的距离的跟踪运动，即加泄连接器与箭体推进剂加注口始终保持0.85m的距离的运动，其二是以0.85m距离进行轨迹规划，轨迹规划算法如下：

定义约束条件：

①位置约束：z(t0)＝z0，z(t0+δt)＝0

②速度约束：v(t0)＝0，v(t0+δt)＝0

③加速度约束：a(t0)＝0，a(t0+δt)＝0

其中，t0为算法起始时间，δt为算法运行时间，z0为箭体活门与连接器的已知距离，在本发明的具体实施方式中为0.85m。

采用五次多项式插值方法来得到满足约束要求的轨迹方程为：

由上述约束条件可知，此轨迹可实现在t0+δt时刻，加泄连接器和箭体推进剂加注口距离为0，即对接完成，同时此时加泄连接器前端和箭体推进剂加注口的相对速度和相对加速度均为0，即加泄连接器和箭体推进剂加注口对接时刻的受力为0，没有撞击风险。

通过计算得出δt最小范围以确定算法具体的运行时间，通过最优化的方法求取δtmin，即算法运行时间的最小值。最优化方法的求取步骤为：

首先，由六自由度机构作动器最大速度及角速度进行运动学计算得到六自由度机构在六个自由度方向上的最大速度；

其次，将箭体推进剂加注口各方向的晃动速度极限和六自由度机构在六个自由度方向上的最大速度比对，得到在满足箭体推进剂加注口晃动条件下，可用于轨迹规划的速度余量，即轨迹规划算法中zi轴运动的速度最大值。

再次，将轨迹规划算法中zi轴运动的速度最大值与z0进行计算，即可求取算法运行的最小时间δtmin，实际过程中，取δt＞δtmin即可。

得到δt后，将轨迹z(t)与以z0距离的跟踪运动进行线性叠加，即为zi轴正方向运动。使加泄连接器在保持对箭体推进剂加注口在惯性坐标系中xi、yi、偏航、俯仰、滚转方向随动运动的基础上，使加泄连接器沿上述叠加后的轨迹规划运动，即可实现加泄连接器精对接阶段的轨迹规划，沿此规划后的轨迹进行加泄连接器实时位姿调整，最终使加泄连接器与箭体推进剂加注口的距离为0，此时二者相对速度和相对加速度均为0，使加泄连接器和箭体推进剂加注口安全对接，避免撞击的风险。