CN114802829B - 一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统 - Google Patents

Abstract

本发明的目的是提供一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，该系统适用于在着陆阶段捕获和减速缓冲各类型可重复使用火箭，可保证非理想着陆状态火箭回收着陆过程的安全稳定；该系统将一根回收索编织成捕获框，并利用电机驱动捕获框主动地跟随并捕获着陆火箭，扩大了火箭着陆点偏差和着陆姿态角偏差范围，降低了对箭上导航制导控制系统的技术要求该系统可以通过调节捕获索框的大小和调整减速缓冲系统能力对不同的着陆火箭进行回收。该系统具有结构简单易实现、普适性良好和技术难度低的特点，有利于提高火箭回收的成功率，实现一种鲁棒性回收。

Description

一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统

技术领域

本发明涉及一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，属于航空航天领域。

背景技术

回收并重复使用火箭能够极大地降低火箭发射和探索太空的成本。自上世纪50年代以来，国内外航天人逐渐将火箭回收技术作为研究热点，探索了多种火箭回收方案。在美国SpaceX的猎鹰9成功地回收火箭的一级部段并重复使用后，着陆腿式垂直回收方案成为了多数航天机构的主要回收方式。然而，这种回收方式的成功需要火箭严格地以垂直姿态、零横向速度和零垂直速度到达着陆平台。为了达到这一要求，火箭的软硬件配套设施需要满足：火箭的发动机需要具有大幅推力调节和多次开关机的能力、导航制导系统具有高精准的软硬件支持和着陆支腿应具有大跨度支撑防倾倒能力和较强的缓冲能力。着陆状态的严格要求和软硬件高技术难度导致了猎鹰9一级回收失败90％以上发生在着陆段，也限制了我国关于着陆腿式垂直回收方案研究的开展，因此有必要自主创新地提出我国的火箭回收方案，推进可重复使用航天运载系统的发展。

发明内容

本发明的目的是提供一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，该系统适用于在着陆阶段捕获和减速缓冲各类型可重复使用火箭，可保证非理想着陆状态火箭回收着陆过程的安全稳定；该系统将一根回收索编织成捕获框，并利用电机驱动捕获框主动地跟随并捕获着陆火箭，扩大了火箭着陆点偏差和着陆姿态角偏差范围，降低了对箭上导航制导控制系统的技术要求。捕获到的火箭可以具有一定的着陆残余能量，这部分着陆残余能量将被索连接的液压缓冲减速装置被动地转化和消耗直至火箭静止稳定，从而降低对发动机的技术要求和对着陆腿缓冲能力的要求。在跟随捕获火箭阶段，设计了一种用于跟随捕获火箭的滤波器和控制器。针对具有横向和轴向移动的回收索的强非线性特性，设计了一个基于多体动力学模型和向后差分法的无迹卡尔曼滤波器，实现对回收索捕获框的非物质点观测数据的噪声滤波。针对PD控制器的初始时刻非连续特性，设计了含有饱和函数的PD控制器，降低初始回收索的振动，精确地控制地面回收索主动跟随捕获火箭。另外，在着陆火箭的重心上方布置可展开挂钩，用于悬挂捕获着陆火箭在地面回收装置上，使在整个回收过程中火箭呈现单摆构型，保证回收过程的稳定性。该系统可以通过调节捕获索框的大小和调整减速缓冲系统能力对不同的着陆火箭进行回收。该系统具有结构简单易实现、普适性良好和技术难度低的特点，有利于提高火箭回收的成功率，实现一种鲁棒性回收。

本发明的目的是通过下述技术方案实现的。

一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，包括：地面回收装置、跟随捕获控制系统、液压缓冲减速装置和火箭挂钩装置；

地面回收装置用于捕获火箭，包括支架、导轨、滑块、滑轮和一根回收索组成。导轨位于支架上方，连接成用于回收索运动的区域；一根回收索编制成网状结构，用于火箭回收的区域位于所述结构中；所述结构通过滑块、滑轮实现在导轨上的移动；

跟随捕获控制系统用于控制回收索的移动；

液压缓冲减速装置固定在地面上并与回收索连接，用于对待回收火箭进行减速；

火箭挂钩装置安装在待回收火箭外壳；

所述液压缓冲减速装置包括：主液缸、柱塞、动滑轮组、导向滑轮、过载控制阀系统、储能器、冷气瓶和传递钢索；通过传递钢索绕过柱塞端部的动滑轮组和导向滑轮与支架上的滑块连接。主液缸内充满油液，一端与过载控制阀连接，另一端的开口由柱塞封闭，柱塞可沿着主液缸的轴向滑动。储能器中间由浮动活塞分成两个腔，一端与过载控制阀连接，腔内充满了油液；另一端与冷气瓶相连，腔内充满了高压空气。

所述过载控制阀系统包括：重量选择器、凸轮、活塞、摇臂、节流阀和传动机构。重量选择器用于改变节流阀的初始开口，与摇臂连接。凸轮通过传动装置与柱塞相连，柱塞运动带动凸轮转动。凸轮压在摇臂上，带动活塞移动，改变节流阀开口面积，调节流经节流阀的油液流量，改变主液缸的压强，进而调节回收所索张力大小。凸轮的外形直接关系到火箭的减速缓冲过程的运动学特性，凸轮的外形通过一种用于火箭回收索系统的凸轮形状反问题设计方法来获得的。

每个所述滑块上布置磁流变阻尼器，来抑制两个回收索的横向振动和相对振动。

所述一根回收索编制成网状结构的方法为：一根回收索从一个支架上的滑轮开始，逆时针依次绕过相邻导轨上滑块的滑轮、相对导轨上滑块的滑轮和与之相邻的支架上的滑轮，按照此规律循环四次，直至绕到首个支架上的滑轮，一根回收索形成一个网状结构。

所述跟随捕获控制系统包括：视觉测量装置、滤波器和控制器；当火箭进入终端着陆阶段，即火箭距离地面回收索系统的回收平面一定高度时，视觉测量装置开始测量火箭顶端、末端和捕获框的位置信息。顶端和末端的位置信息经过滤波器过滤噪声后计算出回收索系统捕获框的目标位置；四个滑块的位置信息经过滤波器过滤噪声后计算出回收索系统捕获框中心的位置信息；然后将目标位置与回收索系统捕获框中心的位置信息进行比较生成误差信号作为控制器的输入信号，再经过控制器计算得到电机的转速控制信号，最后，电机带动回收索系统中的主动滑轮转动。针对具有横向和轴向移动的回收索的强非线性特性，提出了一种基于ALE索描述的移动索非物质点的无迹卡尔曼滤波器，实现对回收索捕获框的非物质点观测数据的噪声滤波。针对PD控制器的初始时刻非连续特性，设计了含有饱和函数的PD控制器，降低初始回收索的振动，精确地控制地面回收索主动跟随捕获火箭。

所述无迹卡尔曼滤波器通过下述方法得到：

步骤一：建立火箭单刚体和回收索-滑块刚柔耦合多体动力学代数微分方程组；利用消去法将代数微分方程组转换为微分方程组；采用向后差分法，确定系统的状态方程和观测方程；

步骤二：采用无迹卡尔曼滤波法对视觉测量信号进行滤波。

所述步骤一的具体实现方式为：

视觉测量装置测量火箭的顶端、末端和捕获框的位置信息时，受到测量噪声、环境噪声和索移动过程中不可避免的振动的影响。利用无迹卡尔曼滤波器设计状态观测器实时获取火箭和捕获框的准确信息，首先建立火箭系统和回收索-滑块系统的多体动力学方程，然后确定系统的状态方程和观测方程。

建立火箭单刚体多体动力学方程。定义回收索系统坐标系的原点在回收平面的中心o，x轴和y轴在回收平面上，分别与相邻的两个导轨平行，z轴过原点o向上，三轴满足右手准则，令全局坐标系为回收索坐标系，火箭的局部坐标系原点在火箭的质心O，Z轴过原点沿着火箭的轴线方向指向上，X轴和Y轴互相垂直并与与火箭的轴线垂直，满足右手准则。火箭刚体质心的广义坐标为其中/>和/>分别表示刚体质心的位置向量和转动向量。/>其中/>为转角，n为转动方向的单位向量。火箭上任意一点p在全局坐标系下的位置向量为

r_p＝r_g+A_gs_P (1)

其中s_p和r_p分别为点p在全局坐标系下和局部坐标系下的位置向量。正交旋转矩阵 为旋转向量/>的偏斜矩阵，I为3×3单位矩阵。

点p在全局坐标系下的速度表示为

其中为局部坐标系下的角速度向量，/>是矩阵/>的偏斜矩阵，并且局部角速度向量/>与广义角速度向量/>的传递矩阵为/> 为s_p的偏斜矩阵，/>为插值矩阵，/>为广义速度向量。

利用式(2)计算刚体的动能，并利用第一类拉格朗日方程，建立火箭单刚体多体动力学方程为

其中为广义加速度向量，M_g为火箭质量矩阵，/>为附加惯性力，/>为广义外力，具体表示为

上式中m和J为火箭刚体的质量和惯量矩阵。F_f为火箭受到的气动力和推力等外力。

火箭多体动力学方程具有非线性，将系统的状态方程(3)改写为如下隐式形式

其中F₁为系统的隐式状态方程，q_g为系统的广义加速度向量、广义速度向量和广义坐标向量，t为时间。

火箭底端和顶端的测量方程为

其中s_t＝[0 0 l_t]^T和s_b＝[0 0 l_b]^T分别为火箭顶端和底端在火箭局部坐标系下的位置向量，l_t和l_b分别为火箭顶端和底端到质心的距离。h₁为测量方程。

针对回收索-滑块系统，滑块采用刚体建模，并利用移动副约束滑块只能沿着光滑导轨运动；回收索采用ALE(Arbitrary Lagrange Euler)索单元进行建模。充分利用ALE索单元具有物质坐标与网格的独立性的特点：(1)将绳索绕过滑轮通过建立一个ALE索节点，将网格坐标约束在滑轮位置，将主动滑轮的卷入卷出绳索通过对ALE索节点的物质坐标约束进行建模；(2)为了观测捕获框的中心位置，在捕获框各边中心设置一个ALE节点，并利用事件驱动动态网格技术驱动所述各边ALE节点随着对应的滑块移动；(3)为了获取回收索的振动，对各段绳索布置均匀网格，并由物质坐标比例约束确保系统的保拓扑性。

滑块的刚体动力学模型与火箭的刚体动力学模型形式一致，其动力学方程为

其中M_s为滑块的质量矩阵，为四个滑块的总广义加速度向量，/>为滑块的附加广义惯性力，/>为滑块的广义外力。滑块s₁,s₂,s₃和s₄只能沿着导轨滑动，需要增加滑块与导轨之间的移动副约束，不考虑导轨的变形，所述约束相当于滑块与地面之间的约束

其中为滑块在全局坐标系下的位置，w为回收区域的边长的一半，/>为滑块的局部坐标系的x方向的单位向量，/>为滑块的局部坐标系的y方向的单位向量，x_o,y_o,z_o为全局坐标系的单位方向向量。

回收索采用ALE索单元建模，两节点ALE索单元的广义坐标为

其中r_i＝[x_i y_i z_i]^T为两个节点(i＝1,2)的位置向量，p_i是两个节点(i＝1,2)的物质坐标。

ALE索单元内任意一点的位置由单元两节点的线性插值得到

r＝N_rq_r, (10)

其中N_r＝[N₁I N₂I]^T为单元的形函数矩阵，为索单元两个结点的位置向量，N₁＝1-s,N₂＝s，s＝(p-p₁)/(p₂-p₁)为归一化后的物质坐标s∈[0 1]，p为ALE索单元内任意一点的物质坐标。

考虑形函数是时间可变的，单元内任一点的速度和加速度通过位置对时间求一阶导和二阶导得到

其中

ALE索单元的控制方程根据虚功原理得出

其中F_f为外力，F_s为弹性力，为惯性力，绳索密度为ρ_c，截面积为A_c。弹性力的虚功表示为

其中ε为轴向应变，β_c为瑞利弹性阻尼系数，E为弹性模量，系数k_c表示绳只能承受拉力。

根据虚功原理，得到索单元的动力学方程为

其中，单元质量阵M_e，物质流动导致的广义惯性力弹性力和阻尼力对应的广义力/>广义外力/>分别表示为

一根回收索在主动滑轮、定滑轮和捕获框边中心位置各布置一个ALE索节点，并利用这些ALE索节点将一根回收索分为16段，在每段中均匀布置若干ALE索单元，则一根回收索全部用ALE单元建模。考虑到ALE索单元物质坐标和网格坐标的独立性，还需要建立三种约束：

(1)索过滑轮约束

将索过滑轮建模为滑轮对应位置的一个ALE节点，并对这个ALE节点的网格进行约束，只允许物质的流动，约束方程为

其中和r_ii分别为第ii个滑轮的位置向量和滑轮对应的ALE索节点的位置向量，ii＝1,2,...,13。

对于主动滑轮卷入和卷出回收索，采用ALE节点的物质坐标约束来建模，约束方程为

其中为主动滑轮(jj＝a,b,c,d,a)对应的ALE节点的物质速度，f_jj(t)为对应的主动滑轮线速度控制指令。一根回收索首尾连接，第一个ALE索节点和最后一个ALE索节点与同一个主动滑轮建立约束。

(2)捕获框边中点ALE节点事件驱动动态约束

为了从动力学方程中直接获取捕获框边的中心位置信息，在捕获框四个边的中心位置各布置一个ALE索节点c_i(i＝1,2,3,4)，并约束四个索节点始终与平行滑块s_i(i＝1,2,3,4)的中心位置对齐，由滑块的运动驱动ALE索节点的移动，则事件驱动动态约束表示为

其中为捕获框边中心位置的ALE节点c_i(i＝1,2,3,4)的位置，为对应滑块s_i(i＝1,2,3,4)的位置。

(3)段内节点物质坐标比例约束

为了获取回收索的振动特性和保持系统的拓扑性不变，每段回收索划分若干ALE索单元，每段的网格在移动过程中单元数不变并保持均匀分布。对于由ALE节点m₁和节点m₂为边界的一段回收索，对段内的ALE节点的物质坐标建立物质坐标比例约束

其中和/>为这段索的边界节点m₁和节点m₂的物质坐标，p_kk为段内的ALE索节点kk物质坐标，kk＝m₁+1,m₁+2,....,m₂-1。

整合滑块的动力学方程(7)、ALE索单元的动力学方程(13)、滑块的约束方程(8)和ALE索节点对应的约束方程式(14)—式(17)，得到回收索-滑块刚柔耦合多体动力学方程为

其中M为系统的总质量矩阵，为n_s维系统的广义加速度，Q_a、Q_e和Q_f为系统的总附加广义惯性力、总广义弹性力和总广义外力。/>为m_s维系统的总约束方程。λ为拉格朗日乘子，Φ_q为约束方程的Jacobian矩阵。

式(18)是代数微分方程组，由于含有未知的非状态变量拉格朗日乘子和质量矩阵不可逆，不能直接作为无迹卡尔曼滤波器的状态方程。利用消去法，将n_s维代数微分方程转化为f_s＝n_s-m_s维微分方程组，使微分方程组的个数等于系统的实际自由度。

为了消去非独立广义坐标，首先利用约束方程找到非独立广义坐标和独立广义坐标之间的关系。由于回收索-滑块系统的约束方程是非定常的，对约束方程进行一次和二次导数得

引入独立速度 是非独立速度/>在常数矩阵B上的投影/>则将速度约束方程增广为

其中矩阵B为对约束方程的Jacobian矩阵Φ_q主轴高斯三角化法构造的布尔矩阵。

则广义速度和广义加速度表示为

其中矩阵S是右端项逆矩阵的前m_s行，矩阵R是右端项逆矩阵的后f_s＝n_s-m_s行。b和c为式(19)中的中间变量。

去掉式(18)的第一行中的拉格朗日乘子项，左乘R^T，利用关系式(21)将系统的微分代数微分方程转换为独立坐标z的微分动力学方程组

将回收索-滑块多体系统动力学方程(22)，改写为隐式形式作为系统的状态传递函数

捕获框边的中心的测量方程为

综上所述，火箭单刚体系统和回收索-滑块刚柔耦合系统的非线性动力学微分方程组分别为式(5)和式(23)。火箭顶端与底端的测量方程是非线性方程组式(6)，捕获框边的中心的测量方程为线性方程组式(24)。

为了便于后续的计算分析，统一两个系统的隐式动力学方程组式(5)和式(23)为

利用向后差分法对系统动力学方程(25)进行时间积分离散。由于无迹卡尔曼滤波使用的是前一个时间步的统计信息，因此选择向后差分格式为一步定步长格式。当前t_k＝t_k-1+Δt时刻的z_k为待求解变量，Δt是积分步长，t_k-1时刻的z_k-1值已知，那么t_k时刻的z_k需要满足

其中预估t_k时刻和/>

则式(25)转化为了一个非线性代数方程，该方程仅是z_k的非线性方程，采用牛顿迭代法求解。

在第m次迭代，位置、速度和加速度矢量分别记为则牛顿迭代增量Δz由求解如下线性方程组得到

JΔz＝d(27)

其中Jacobian迭代矩阵为

更新后新的位置、速度和加速度矢量满足

完成更新后，若当前时间步隐式动力学方程d的范数大于设定的迭代误差tol，进入下一次牛顿迭代；否则说明迭代已经收敛，退出牛顿迭代循环并将收敛值记为然而，由于无迹卡尔曼滤波的每个时间步状态传递的为均值和协方差的统计值，采用向后差分法求解隐式微分方程时，求解t_k时刻的解，需要t_k-1时刻的位置z_k-1、速度/>和加速度向量的信息。因此选择无迹卡尔曼滤波器的状态变量为/>t_k-1时间步的状态变量传递到t_k时间步的状态变量的过程，式(26)-式(28)，简记为

x_k＝BDF(x_k-1,t_k-1) (29)

综上所述，式(29)为火箭单刚体系统和回收索-滑块系统的状态传递函数，统一观测方程并改写为离散形式；考虑过程噪声和测量噪声，用于无迹卡尔曼滤波的状态函数和传递函数为

其中x为系统状态变量，动力学方程的广义坐标个数为f，则状态变量个数为nn＝3f_s。BDF(x)和h(x)分别状态传递函数和观测函数，W和V是过程噪声和测量噪声，分别具有协方差矩阵Q和R。

所述步骤二的具体实现方式为：

火箭单刚体和回收索-滑块系统多体动力学方程具有非线性特性。无迹卡尔曼滤波使用无迹变换来处理均值和协方差的非线性传递问题，能够获得准确稳定的滤波结果。无迹卡尔曼滤波分为预测和更新两个部分。状态变量x是nn维随机变量，经历无迹变换后得到2nn+1个Sigma点X。无迹卡尔曼滤波过程采用矩阵形式来表示。

(1)预测

计算预测的状态均值和预测协方差/>

计算预测的测量均值μ_k、测量协方差S_k和互协方差C_k

式(31)和式(32)中的矩阵形式的无迹变换为

其中nn为状态变量的维度，下标a为均值，下标v为协方差，上标为第几个采样点。参数λ＝α²(nn+κ)-nn是一个缩放比例参数，用来降低总的预测误差，α的取值控制了采用点的分布状态，κ取值确保(nn+P)为半正定矩阵，β≥0是一个非负权系数，可以合并方程中高阶项的动差。

(2)更新

计算滤波器增益K_k，状态变量均值m_k和协方差矩阵P_k

综上所述，通过建立火箭单刚体和回收索-滑块系统的多体动力学模型，即式(3)和式(18)，然后利用向后差分法推导系统状态方程和观测方程(式(30))，最后利用无迹卡尔曼滤波(式(31)和式(32))，实现对视觉测量装置测量的火箭顶端与末端位置信息和捕获框的位置信息的滤波。

所述含有包含函数的PD控制器通过下述方法得到：

步骤一：由经过滤波器滤波的火箭顶端和末端位置信息计算控制器的输入信号；

视觉测量装置的测量坐标系为回收索系统坐标系，经过滤波的火箭顶端和末端的位置信息在全局坐标系下分别为r_b＝[x_b y_b z_b]^T和r_t＝[x_t y_t z_t]^T，捕获框的目标位置为

捕获框的移动是通过电机驱动主动滑轮转动，进而带动回收索拖动滑块在光滑导轨上滑动来实现的。四个主动滑轮a,b,c和d对应的目标卷入卷出索长度为

u_a＝-x_tgt+y_tgt,u_b＝-x_tgt-y_tgt,u_c＝x_tgt-y_tgt,u_d＝x_tgt+y_tgt. (36)

其中各主动滑轮卷入卷出索的方向以绕z轴逆时针为正。

则控制器的输入信号为

u＝[u_a u_b u_c u_d]^T (37)

步骤二：利用滤波器滤波后的绳索位置信息计算控制器的反馈信号；

滤波器获得的捕获框四个边的中心点c₁,c₂,c₃和c₄的位置信息为和/>则捕获框中心位置为

控制器的反馈信号为

c＝[c_a c_b c_c c_d]^T (39)

其中c_a＝-x_c+y_c,c_b＝-x_c-y_c,c_c＝x_c-y_c,c_d＝x_c+y_c。

步骤三：设计含有饱和函数的PD控制器

由步骤一和步骤二获得的控制器输入信号和反馈信号得到控制器的控制信号为

e＝u-c (40)

四个主动滑轮控制器均采用速度PD控制律。考虑到在主动跟随捕获阶段的初始时刻，捕获框位于着陆平面的中心，而目标位置如果远离回收坐标系中心，PD控制律将产生一个较大的电机转速指令，出现不连续性，这里在PD控制器的输出指令增加了一个饱和函数κ(t)过渡初始时刻的不连续，

控制器的控制律为

其中v_p∈R^4×1为电机的转动控制速度，a₀和a₁分别为比例系数和微分增益系数。饱和函数κ(t)是时间的函数，具体表示为当t≤t_c/2时，κ(t)＝2t/t_c，当t＞t_c/2时，κ(t)＝1。t_c为整个跟随捕获阶段的总时间。

所述凸轮形状反问题设计方法通过下述三个步骤实现：

步骤一：确定减速缓冲段的目标过载、速度和位移曲线；

在有限的缓冲位移限制下，过载采用恒定的值时，最大过载的值为最小。然而，为了避免初始碰撞力过大和减速为零后反弹，过载在缓冲初始和结束时的值应为零。综合考虑上述两种情况，火箭的缓冲过载分为三个阶段，在第一阶段火箭被捕获瞬间过载为零，经历t₁时间后匀速增大至最大值第二阶段的过载保持恒定值/>经历的时间为t₂；第三阶段过载从/>开始匀速减小，经过时间t₃后减速为零，所述/>为最大的过载值，所述过载值向下为正，则减速缓冲过程的目标过载曲线为

对式(42)进行一次积分得到减速缓冲过程中火箭的速度为：

其中为捕获时刻火箭的竖直方向速度，向下为正。

对式(43)进一步积分得到减速缓冲过程中火箭位移z为：

在火箭的理想过载的上升段和下降段所用的时间相等，即t₁＝t₃的情况下，火箭的最大过载、速度、距离有如下关系式：

其中z_max为火箭减速缓冲过程中的最大位移。

在t＝t₁+t₂+t₃时刻的火箭速度减速至零，则将关系式t₁＝t₃代入式(43)的第四个式子，得

则前两个阶段的时间t₁和t₂满足如下关系式：

在t＝t₁+t₂+t₃时刻的减速缓冲距离不超过z_max，即将t＝t₁+t₂+t₃代入式(44)的第三个时间段的表达式应小于等于z_max，并考虑t₁＝t₃，得到

进一步，利用关系式式(46)消去式(48)中的t₁，则得到t₂的范围为

当时，由式(46)和关系式t₁＝t₃得：

减速缓冲段的目标过载、速度和位移曲线由式(42)-式(44)确定，其中各阶段时间由式(49)和式(50)确定。

步骤二：建立回收索系统动力学方程，根据目标运动曲线求解目标液压力；

为了建立回收索系统的动力学模型，忽略回收索和传递索的弹性变形，考虑火箭在回收区域中心被捕获，缓冲距离z与柱塞位移x_hy之间的关系为

其中w是常数，为回收区域边长的一半。

对式(51)求导，得到柱塞的速度和加速度分别为

则当火箭以式(42)-式(44)的目标过载、速度和位移进行减速缓冲时，柱塞的位移、速度和加速度由式(51)-式(53)确定。

对于火箭，当在回收区域中心被捕获时，不考虑回收索与滑轮之间的摩擦，各段回收索张力相同，均为T，则回收索系统的动力学方程表示为

其中z为火箭的缓冲位移，向下为正，m_r为火箭的质量，且和g＝9.81m/s²。

对于液压缓冲装置中的柱塞，若忽略连接回收索与传递索的滑轮组的质量，回收索与传递索的张力相等，均为T，则作用在柱塞的力大小为4T，柱塞的动力学方程为

/>

其中m_hy为柱塞和带滑轮的十字头的总质量，为液压阻尼力，μ为油液的阻尼系数，A₁为柱塞的横截面积，P₁为主液缸压强。

将式(54)和式(55)中的回收索和传递索张力消去，得系统的动力学方程为

则为了使减速缓冲段实现目标火箭缓冲过载、速度和位移，液压缓冲装置需要提供的目标液压力为

其中火箭和柱塞的质量为已知常量，火箭和柱塞的加速度由式(42)和式(53)决定。

步骤三：根据目标液压力，设计凸轮目标形状；

液压缓冲装置的柱塞受到的液压力表示为

其中ΔP＝P₁-P₂为主液缸与储能器的压强差，P₁为主液缸的压强，P₂为储能器的压强。

当主液缸的油液在柱塞推动下，从主液缸流入储能器的过程中，主液缸的油液压强与流过节流阀的油液流量有关，主液缸与储能器的压强差表示为

其中ρ_u为油液密度，A为节流阀横截面积，当油液流经节流阀时，油液的流量系数c_d随着节流阀小孔横截面积A的变化而发生改变，c_d＝1.1A^0.1。

储能器的压强P₂与空气压缩瓶中的气体压强P_f相等，由于气体被压缩过程的时间较短，视为绝热过程，则

其中P_f0为空气压缩瓶的初始压强，V_f0为空气压缩瓶的初始体积，λ₁为压缩气体的绝热系数，A_f为储能器活塞的有效面积，储能器活塞的位移u_f等于主液缸活塞的位移x_hy。

将式(59)和式(60)代入式(58)，得到作用在柱塞上的液压力的具体表达式为

其中

针对某一确定的液压缓冲装置，且柱塞的位移和速度已知，式(61)中的第一项和第三项是确定的，即是确定的，另外液体密度和柱塞的横截面积为常量，而且柱塞的速度由火箭的理想运动特性决定，则柱塞的液压力主要通过节流阀的横截面积来控制，由式(61)和式(57)确定满足设计要求的目标节流阀横截面积为/>

在减速缓冲过程中，柱塞向主液缸内滑移，通过传动机构带动过载控制阀系统中的凸轮转动，凸轮推动活塞使节流阀的阀芯运动，调节节流阀的开口面积，从而控制油液流量、改变主液缸压强。节流阀小孔的截面积A与节流阀的阀芯位置y的关系为

其中φ是顶角半角，d为主液缸和节流阀的连接处的阀口直径，简化参数分别为m_A＝πd sinφ和n_A＝sin 2φ/(2d)。

由上式(63)得知，节流阀的开口面积只与节流阀阀芯的位置y有关，根据式(62)目标节流阀开口面积A^tgt得目标节流阀阀芯位置曲线为

同时，为了简化分析，整合凸轮转角与摇臂长度，阀芯的位置y只受到凸轮转角控制

y＝y₀(1-k₁θ-k₂θ²-k₃θ³-k₄θ⁴) (65)

其中y₀＝d/k_m为阀芯的初始位置，k_m为重量调节系数，改变阀芯初始开口用于对不同质量的火箭进行回收减速缓冲，k₁，k₂，k₃和k₄是凸轮曲面位形的函数系数。

柱塞与节流阀凸轮通过传动装置连接，当柱塞的位移x_hy与节流阀凸轮转角θ间的关系为θ＝x_hy，将此关系式代入式(65)，并考虑式(64)阀芯的目标位置y^tgt，目标凸轮曲面位形系数满足如下关系式

采用最小二乘法确定凸轮曲面位形系数，令f＝1-k_my^tgt/d和x＝x_hy，拟合的相对误差为

其中n_k为数据点数。将式(67)对k_j(j＝1,2,3,4)求导，得

整理为未知量k_j(j＝1,2,3,4)的线性方程组为

A_kk＝F (69)

求解上式线性代数微分方程，求得凸轮的形状系数为

/>

其中

为实现火箭在减速缓冲段按照目标的运动曲线运动，凸轮的目标形状由式(70)确定。

所述储能器的初始体积和初始压强通过下述方法得到：

为了保证火箭向下的速度减速为零后静止稳定不发生反弹，需要对冷气瓶的初始气体体积进行设计。由液压缓冲力的表达式(61)和柱塞的速度与火箭的向下速度的关系式(52)可知，当火箭的速度减速为零时，柱塞的速度也为零，则主液缸与储能器没有压强差，液压缓冲力只与浮动活塞的位置有关，即

此时，若要求火箭静止稳定，即则式(56)被改写为火箭的静力学平衡方程

2F_hy sinα-m_rg＝0 (72)

将式(71)代入式(72)，并考虑到和则储能器的初始体积和初始压强满足关系式

所述火箭挂钩系统为在火箭重心上方周向安装的四个弹性挂钩。火箭在着陆阶段燃料消耗殆尽，且发动机在火箭底部向下喷气减速，火箭的重心偏离几何中心向下。本发明将挂钩安装在火箭的几何中心或偏上，使得着陆缓冲力受力点远离发动机，减弱对高成本发动机的破坏。挂钩由长短两个杆组成，其中长杆的一端通过柱铰与箭体外壳连接，中部通过弹簧阻尼器连接在箭体外壳上，末端通过弹簧和限位装置与短杆构成一定角度的钩。挂钩在发射阶段通过带有爆炸螺栓的刚性捆绑带收拢在箭体上，在着陆阶段爆炸螺栓解锁，挂钩在压缩的弹簧阻尼器作用下展开。不需要配置展开动力。挂钩的安装位置选定为两个贮箱的箱间段，这段壳臂结构可以通过加厚或者布置加强筋提高局部的应力极限强度。

有益效果

1.本发明提供了一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，利用地面主动跟随捕获控制系统，扩大了火箭的着陆偏差裕度，降低了对火箭上发动机和控制系统的技术要求，采用地面液压减速缓冲装置，降低了可回收火箭箭上着陆装置的质量，实现了一种鲁棒性回收。

2.本发明提供了一种跟随捕获控制系统，具有主动跟随捕获着陆火箭的能力，能够捕获具有偏差着陆状态的火箭，扩大了火箭的允许着陆状态偏差范围，降低了箭上导航制导控制系统的要求。

3.本发明提供了一种地面液压减速缓冲装置，具有较强的吸能能力，将火箭着陆减速缓冲装置置于地面，无需在箭上布置缓冲装置，减小了箭上回收装置的质量和技术难度。减速缓冲过程的运动学特性可设计，针对特定的回收着陆火箭，利用整个系统的简化动力学方程提出了一种用于火箭回收索系统的凸轮形状反问题设计方法，降低减速缓冲过程的最大过载。

4.本发明采用单摆稳定构型，火箭的挂钩高于重心位置，允许着陆过程中火箭发生摆动。因着陆受力位置远离发动机，有利于对回收关键部件发动机的安全回收重复使用。

5.本发明能够回收各类型火箭，只需在回收任务前简单调整结构参数，具有可设计性和普适性，地面回收索系统实现了一种鲁棒性火箭回收着陆。

6.针对效果2所述的“跟随捕获控制系统”，视觉测量装置测量的信息包含噪声，本发明提出了一种基于ALE索描述的移动索非物质点的无迹卡尔曼滤波器，利用约束Jacobian矩阵将系统的多体动力学微分代数方程转换为代数微分方程，利用向后差分法构建无迹卡尔曼滤波器的一次预测，实现对非线性系统的高精度稳定滤波。

7.针对效果6所述的“无迹卡尔曼滤波器”，对应于无迹卡尔曼滤波的一步预测，向后差分法采用一步积分。向后差分法预估广义坐标、广义速度和广义加速度，利用牛顿迭代更新这些量，由于使用无迹变换来处理均值和协方差的非线性传递问题，卡尔曼滤波的状态变量由原广义坐标变为广义坐标、广义速度和广义加速度的组合向量，实现了全状态的非线性传递。

8.针对效果6所述的“无迹卡尔曼滤波器”，捕获框边中心是视觉测量装置测量的信息，这是一个空间坐标和物质坐标均时变的回收索上的点。为了实现对这些点的观测，本发明利用物质坐标和网格相对独立的ALE索单元对回收索建模，在捕获框边中心建立一个ALE索单元，并建立这个ALE索节点与对应的滑块的事件驱动动态网格约束，保证此ALE节点始终位移捕获框边的中心位置附近，观测函数可由系统的广义坐标直接表达。实现了对空间坐标和物质坐标时变的索上一点的直接观测。

9.针对效果7所述的“无迹卡尔曼滤波器”，本发明所提供的方法可用于其他非线性多体动力学系统的滤波，普适性高。

10.针对效果2所述的“跟随捕获控制系统”的控制器，本发明设计了一种含饱和函数的PD控制器，在保证跟随捕获的准确性的前提下，过渡初始时刻的不连续。

11.针对效果3所述的“凸轮形状反问题设计方法”，现有液压缓冲装置多为正问题动力学特性分析，并没有一套完备的理论设计方法。本发明基于系统动力学方程对液压缓冲装置进行设计，形成一套完备的反问题设计理论体系，得到了一种直接的理论设计公式。

12.针对效果3所述的“凸轮形状反问题设计方法”，本发明所得的液压缓冲装置理论设计方法及验证方法简洁直观，易于编程实现，缩短了研发周期，降低了研发成本。

13.针对效果3所述的“凸轮形状反问题设计方法”，本发明基于系统动力学特性进行反问题设计，所设计装置经过验证能够精准的满足动力学设计要求，可以满足工程应用的要求。

14.针对效果2所述的“凸轮形状反问题设计方法”，本发明可应用于各种参数下的液压缓冲装置及其他类型减速缓冲装置的动力学反问题设计及验证，普适性高。

附图说明

图1本发明实施例基于无迹卡尔曼滤波的精确控制火箭回收索系统示意图；

图2本发明实施例回收火箭流程简图；

图3本发明实施例地面回收索装置结构图；

图4本发明实施例的着陆火箭结构图；

图5本发明实施例的跟随捕获控制系统工作原理图；

图6本发明实施例的跟随捕获控制系统电机控制卷出回收索长度与捕获框位移的关系图；

图7本发明实施例的跟随捕获控制系统的滤波器工作流程图；

图8本发明实施例的跟随捕获控制系统的回收索-滑块系统多体动力学模型；

图9本发明实施例的跟随捕获控制系统的滤波器对捕获框边中心移动位移滤波结果；

图10本发明实施例的跟随捕获控制系统的滤波器对捕获框边中心移动位移滤波偏差；

图11本发明实施例液压缓冲装置结构图；

图12本发明实施例的过载控制阀系统结构图；

图13本发明实施例的火箭的目标运动特性曲线；

图14本发明实施例的火箭与柱塞受力分析简图；

图15本发明实施例的柱塞的目标运动特性曲线；

图16本发明实施例的主液缸与储能器压强差曲线；

图17本发明实施例的阀芯位置和节流阀截面积曲线；

图18本发明实施例的凸轮外形曲线；

图19本发明实施例的火箭运动特性验证曲线。

其中，1—地面回收索装置，2—着陆火箭，3—跟随捕获控制系统，4—液压缓冲装置，1.1—支架，1.2—光滑导轨，1.3—滑块，1.4—磁流变阻尼器，1.5—回收钢索，1.6—捕获框，1.7—电机驱动的主动滑轮，1.8—定滑轮，2.1—长杆，2.2—短杆，2.3—柱铰，2.4—弹簧阻尼器，2.5—弹簧，2.6—限位销，2.7—刚性套环，2.8—爆炸螺栓，2.9—火箭级段，4.1—十字头，4.2—柱塞，4.3—主液缸，4.4—传递钢索，4.5—过载控制阀系统,4.6—储能器，4.7—浮动活塞，4.8—冷气瓶，4.9—传动装置，4.10—动滑轮组，4.5.1—凸轮，4.5.2-4.5.5—摇臂，4.5.6—节流阀，4.5.7—活塞，4.5.8—重量选择器。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

如图1，基于无迹卡尔曼滤波的精确控制火箭回收索系统包括地面回收索装置1、着陆火箭2、跟随捕获控制系统3和液压缓冲装置4四个部分组成。如图2所示，整个回收过程分为主动跟随捕获阶段和被动减速缓冲阶段。

如图3所示，地面回收索装置搭建在四个竖立的刚性支架1.1上，四个光滑直滑轨1.2分别连接相邻两个支架1.1，在距离地面一定高度上构建一个正方形回收区域在oxy面内。在每个光滑导轨1.2上套上一个滑块1.3，滑块只能沿着导轨滑动，且滑动范围为整个导轨的长度范围，因此捕获范围为整个回收区域。在每一个滑块1.3上安装两个定滑轮1.8，每一个支架1.1顶端安装一个电机驱动的主动滑轮1.7，并选择一个支架上布置三个定滑轮1.8。一个回收索1.5首尾相连，绕过滑块上的定滑轮1.8、支架上的主动滑轮1.7及此支架的定滑轮1.8，在回收区域内形成一个正方形捕获框1.6，捕获框的宽度由滑块上的两个定滑轮距离决定。在每一个滑块上布置三个磁流变阻尼器1.4，对回收索横向振动进行半主动控制。主动滑轮1.7带摩擦凹槽的缠绕多圈回收索来防止打滑，在回收索移动过程中忽略其振动和伸长量，因此捕获框1.6在回收区域的移动位移可近似为滑块1.3在导轨1.2上的移动位移。在主动跟随捕获段，回收钢索1.5和传递钢索4.4均在液压缓冲装置4的作用下呈张紧状态，且在此过程中作用在液压缓冲装置柱塞4.2上的力不足以推动柱塞运动，因此连接回收索与传递索的动滑轮4.10静止，回收索1.5只在回收平面内移动。主动滑轮1.7在电机的驱动下卷入卷出回收索，带动滑块1.3在光滑导轨1.2上移动，来实现捕获框1.6的移动。每一个主动滑轮1.7均需要控制捕获框在回收区域内两个方向的移动，并且控制相对的两个滑块1.3的移动位移相同，同一方向运动的两个回收索保持平行。回收索1.5既能在回收平面内移动，也可以在火箭2的带动下向下运动。在被动减速缓冲阶段，电机1.7关机，滑块1.3可在光滑导轨1.2上自由滑动，回收索1.5在着陆火箭2的带动下向下运动，动滑轮4.10竖直向上运动，支架上的竖直回收索1.5进入回收区域，在此过程中，液压缓冲装置4提供的阻力对火箭2进行减速缓冲。

如图4所示，与地面火箭回收索系统1的捕获框1.6对应，在火箭中心平面周向安装四个弹性挂钩，挂钩由长短两个杆2.1,2.2组成，长杆的一端通过柱铰2.3与火箭的外壳连接，可在火箭轴线与柱铰点组成的平面内OXZ转动，长杆的中点与火箭外壳通过弹簧阻尼器2.4连接，末端与短杆通过弹簧阻尼器2.5和限位销2.6连接，长杆与短杆形成一定角度的钩。挂钩在发射阶段利用带有爆炸螺栓2.8的刚性套环2.7收拢在火箭外壳上。在着陆阶段爆炸螺栓2.8爆炸解锁，刚性套环2.7与火箭分离，长杆2.1在弹簧阻尼器2.4作用下弹开，短杆在弹簧2.5作用下弹开至限位角度形成钩状。

如图5所示，捕获框1.6跟随和捕获火箭2是由跟随捕获控制系统3控制主动滑轮1.7的转动来实现。跟随捕获控制系统由视觉测量装置、滤波器、计算单元和控制器组成。当火箭进入视觉测量装置的测量区域，视觉测量装置测量火箭2的顶端和底端的中心在回收坐标系下的位置信号，利用滤波器将这些信号中的噪声信号过滤，再由计算单元计算出地面回收索系统捕获框的中心的目标位置。当火箭底端高于回收平面oxy时，目标位置为火箭底端中心在回收平面oxy的投影；当火箭底端进入捕获框时，目标位置为火箭轴线与回收平面的交点。目标位置是控制系统的期望值和输入量。控制器采用含饱和函数的PD控制律，四个电机为执行器，没有考虑索的柔性的影响，以视觉测量装置测量的捕获框边中心位置来计算捕获框的中心位置，捕捕获框中心位置初始时刻位于坐标系oxy的原点o.四个主动滑轮1.7是被控对象。当不考虑索的振动和回收索与滑轮之间的相对滑移，将捕获框的中心位置通过一个微分环节反馈到控制器，跟随捕获控制系统形成一个闭环伺服控制系统。

如图6所示，跟随捕获控制系统3控制主动滑轮1.7转动，通过滑块1.3带动回收索1.5从初始位置移动到目标位置。每个主动滑轮要控制捕获框的x和y两个方向的移动，回收索绕过主动滑轮的索长度与捕获框的目标位移的关系为式(36)。捕获框的中心位置为视觉测量装置测量捕获框边的中心的滤波值，作为控制器的反馈信号如式(39)所示。控制系统从控制信号e到反馈信号c的开环传递函数可表示为

其中K(s)是饱和函数κ(s)的拉氏变换。

主动滑轮采用速度控制器，为了保证稳态误差为零，选择微分增益a₁为1。当t≤t_c/2时，闭环系统存在理论解，主动滑轮的卷入卷出位移为

可以得到响应的相对误差为

只要a₀＞0，相对误差趋近于0，且a₀越大，相对误差收敛速度越快。若要实现在t＝t_c/2时刻，捕获框的中心与目标位置的相对偏差为||e/u||＜e^-5，比例增益需要满足a₀＞20/t_c。

如图7所示，采用无迹卡尔曼滤波器和向后差分法对视觉测量系统测量的火箭顶端和底端位置信号和捕获框边中心的位置信号进行滤波。无迹卡尔曼滤波的一步预测方程采用向后差分法进行计算。将状态向量由原广义坐标的位移、速度和加速度向量组成，对当前时间步的位移、速度和加速度由前一时间步的相应值进行预估。采用牛顿迭代方法将系统多体动力学方程组成的非线性方程组转换为状态迭代增量的线性方程组，求解迭代增量，更新当前时刻的状态向量，直到迭代收敛，完成一步向后差分积分，得到当前时刻的位置、速度和加速度值作为一步无迹卡尔曼预测传递的值。无迹卡尔曼滤波使用无迹变换来处理均值和协方差的非线性传递，对于每一个时间步，首先生成状态向量采样向量，然后利用向后差分法预测下一个时间步的先验预估值，然后计算先验状态均值和协方差，生成先验状态向量采样，计算观测向量的先验预估值、观测均值和协方差。更新系统的卡尔曼增益、状态变量的均值和协方差，至此完成一步无迹卡尔曼滤波，重复直至完成N个时间步的无迹卡尔曼滤波。

无迹卡尔曼滤波器的状态方程由火箭单刚体和回收索-滑块刚柔耦合多体动力学方程建立的。回收索-滑块刚柔耦合动力学模型如图8所示，滑块采用刚体建模，一根回收索由ALE索单元进行建模，支架上的主动滑轮和滑块上的定滑轮均由一个ALE节点和对网格坐标的约束(式(14))建模。为了能够通过多体动力学模型直接观测到捕获框边中心的位移，在捕获框边中心c_i(i＝1,2,3,4)建立一个ALE索节点，并利用事件驱动动网格约束(式(15))约束边中心始终随着对应的滑块滑动s_i(i＝1,2,3,4)。将滑轮位置的ALE节点和捕获框边中心的ALE节点作为边界节点，将一根回收索分为16段，在每段内布置m-1个ALE索节点，利用物质坐标比例约束(式(17))约束每段回收索的单元数保持不变，单元长度和每段的索长度成比例，以此在保证真实模拟绳索的横向振动和振动波传递的基础上保持系统的拓扑不变性。综上所述，建立回收索-滑块的刚柔耦合多体动力学方程为一个非线性代数微分方程。利用约束方程的Jacobian矩阵将代数微分方程改写为微分方程，用于无迹卡尔曼滤波的一步预测。由于捕获框边中心由滑块驱动的ALE节点建模，观测方程可直接由回收索-滑块系统的状态向量提取，如式(24)所示。

为了验证所述无迹卡尔曼滤波器对视觉测量装置测量的捕获框边中心的位置信息的滤波效果，设置回收索-滑块系统物理参数和多体动力学模型参数如下：

1.回收索半径0.02m，弹性模量210GPa，密度7800kg/m³，阻尼比0.005；

2.回收索捕获区域边长50m，捕获框边长7m，滑块长度7m，滑块质量30kg；

3.各段回收索内部ALE节点数m-1＝3；

4.四个滑块s_i(i＝1,2,3,4)的移动速度为

5.主动滑轮的转速为

其中

考虑回收索-滑块系统的过程噪声和测量噪声，仿真总时间为10s。利用无迹卡尔曼滤波和向后差分法对捕获框边中心的位移进行滤波，测量值、实际值和无迹卡尔曼滤波的值的对比图如图9所示。对比捕获框边中心的位移/>的测量噪声和滤波偏差，如图10所示，可以看出，利用无迹卡尔曼滤波对非线性回收索-滑块系统中的捕获框边中心测量的滤波测量具有明显效果。

如图11-图12所示，液压缓冲装置4由十字头4.1、柱塞4.2、主液缸4.3、传递索4.4、滑轮组4.10、过载控制阀4.5、储能器4.6和冷气瓶4.8组成。过载控制阀4.5由凸轮4.5.1、活塞4.5.2、4.5.7、摇臂4.5.3-4.5.5、节流阀4.5.6、重量选择器4.5.8和传动机构4.9等部件组成。在主动跟随捕获阶段，液压缓冲装置4中主液缸4.3中的液压只提供回收索1.5和传递索4.4的张紧力，滑轮组4.10、柱塞4.2和十字头4.1静止不动。在被动减速缓冲段，火箭2的挂钩挂在捕获框1.6上，带动竖直方向的回收索1.5进入回收区域，传递索4.4和滑轮组4.10向上运动，柱塞4.2向主液缸4.3内滑移，进而压迫油液流经过载控制阀4.5进入储能器4.6，在主液缸4.3与储能器4.6之间产生液压差。储能器4.6的浮动活塞4.7发生移动，储能器4.6中的液压和冷气瓶4.8中的气压相应的发生改变。

受限于支架高度1.1和火箭2长度，火箭2减速缓冲阶段的位移不宜过大。另外，为了实现火箭2的安全无损着陆，火箭2的减速缓冲过载不宜过大。考虑上述两个限制条件，设计减速缓冲段的理想过载曲线并根据理想过载曲线设计过载控制阀凸轮4.5.1的形状。利用滑轮组4.10、十字头4.1和传动机构4.9分别将火箭2的缓冲位移与传递索1.5的位移、传递索4.10位移与柱塞4.2位移以及柱塞4.2位移与凸轮4.5.1转角关联起来。火箭2向下减速运动带动回收索1.5向下运动，传递索4.10向上运动，柱塞4.2向主液缸4.3内滑移，凸轮4.5.1转动，与凸轮连接的活塞4.5.2带动节流阀4.5.6移动，改变节流阀4.5.6的截面面积，进而改变主液缸4.3与储能器4.6的液压差，最终调整了火箭2减速的过载达到理想设计曲线，实现火箭的安全减速缓冲。在这一过程中，火箭2的残余机械能转换为液压缓冲装置4的内能。液压缓冲装置4通过过载控制阀4.5的凸轮4.5.1形状和重量选择器4.5.8来调节不同着陆火箭减速过程的过载。

液压缓冲段完成时，火箭2的最终状态为静止悬挂在回收索系统1上，作用在火箭2上的回收索1.5张力的竖直方向分量应等于火箭2的重力。这一功能可以通过调整储能器4.6和冷气瓶4.8的初始气体体积来实现。这时，主液缸4.3的柱塞4.2和储能器浮动活塞4.7均静止，无液体流过节流阀4.5，主液缸4.3与储能器4.6没有液压差。当柱塞4.2横截面积等于浮动活塞4.7的横截面积时，储能器4.6液压等于冷气瓶4.8的气压。柱塞4.2的移动位移已知，液压缓冲装置4中的液体和气体的总体积亦是确定的，此时的主液缸4.3液压完全由冷气瓶4.8的初始气体决定。因此针对不同重量的着陆火箭2相应的调整冷气瓶4.8的初始气体体积来实现回收火箭2至最终的静止稳定。

为了安全无损地回收火箭2，在液压缓冲减速阶段火箭的运动学特性，如缓冲位移、速度和过载需要满足设计要求，需要设计液压缓冲装置4的参数并通过动力学计算来验证设计的准确性。当火箭回收索系统及火箭的相关参数如下：

1.火箭的质量m_r＝25ton；

2.火箭着陆速度v₀＝20m/s；

3.主液缸柱塞的质量m_hy＝27.2ton；

4.主液缸柱塞截面积和浮动活塞截面积：A₁＝A_f＝0.196m²

5.液体密度ρ_u＝1250kg/m³；

6.节流阀阀口直径d＝0.16m；

7.重量调节系数k_m＝10；

8.气体初始压强p_f0＝8.16标准大气压；

9.冷气瓶气体初始体积v_f0＝3.14m³；

10.液体粘性系数μ＝1000Ns/m；

根据所述过载控制阀系统凸轮结构的设计方法的步骤一，设定火箭2的运动学特性设计目标：和z_max＝10m，则根据式(42)-式(44)可得火箭的目标运动轨迹曲线如图13所示，且利用式(49)和式(50)可得三个阶段的时间分别为：t₁＝t₃＝0.175s和t₂＝0.45s。另外，根据如图15所示火箭与柱塞受力分析，并由式(51)-式(53)可知，柱塞4.2的运动学特性如图14所示。根据设计方法的步骤二，建立回收索系统动力学方程式(57)，根据目标运动曲线求解液压缓冲装置4的主液缸4.3和储能器4.6的压强差的设计目标曲线如图16所示。进一步，利用式(62)和式(64)，得到过载控制阀系统4.5的目标节流阀4.5.6截面积和目标阀芯位置如图17所示。通过前文所述，液压缓冲装置4的设计最终是对过载控制阀的凸轮4.5.1外形进行设计，利用式(69)最小二乘法对凸轮外形曲线进行拟合，得到凸轮4.5.1的外形曲线系数为

液压缓冲装置4的凸轮4.5.1外形曲线如图18所示。利用所设计的液压缓冲装置参数建立整体系统多体动力学模型进行仿真计算，得到火箭2的运动学特性如图19所示，当t＝1s时，火箭2的缓冲最大位移为10.05m，向下的速度减速为零。火箭2的最大过载发生在t＝0.6s，最大值为42.44m/s²，与设计值的相对误差6％，这是因为设计过程中没有考虑绳索的弹性引起的。经过多体动力学仿真验证，所设计的火箭2的过载最大值小于火箭的发射过载，因此，满足安全无损回收火箭2的要求。

整个回收的具体流程为：当着陆火箭2到达终端着陆阶段，地面回收索装置1的捕获框1.6在跟随捕获控制系统3控制下主动地跟随火箭2，直到火箭2进入捕获框1.6并将挂钩挂在回收索1.5上，火箭被捕获，跟随捕获控制系统3停止工作，主动滑轮的电机1.7关机。回收过程进入被动减速缓冲段，针对火箭2竖直方向的着陆速度偏差，当具有残余机械能的火箭2挂在回收索1.5后，带动回收索1.5向下运动，同时连接回收索1.5与传递索4.4的滑轮组4.10向上运动，与支架1.1平行的回收索1.5进入回收区域，作用在十字头4.1上的传递索4.4带动液压缓冲装置4中的柱塞4.2向主液缸4.3内滑移，油液流过节流阀4.5，主液缸4.3的液压、传递索4.4和回收索1.5的张力发生改变并达到相应的设计值，对火箭2进行减速缓冲，直到火箭2向下速度为零，相比于箭上着陆腿对质量和气动外形的限制，地面减速缓冲装置的减速缓冲能力更强。最终，在液压缓冲装置4的储能器4.6和冷气瓶4.8的压力作用下与火箭2的重力平衡稳定。针对火箭2的横向着陆位置和速度偏差，主动滑轮1.7的电机在被动减速缓冲段关机，滑块1.3可在光滑导轨1.2上自由滑动，而从滑块1.3的受力和火箭2向下运动的单摆构型可以看出，火箭2和滑块1.3总是在回收索1.5作用下向回收区域的中心靠拢，不考虑滑块1.3与导轨1.2的摩擦，火箭2最终会在回收区域的中心呈单摆构型静止稳定，完成回收，因此基于无迹卡尔曼滤波的精确控制火箭回收索系统还具有自动纠偏的能力。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，其特征在于：包括：地面回收装置、跟随捕获控制系统、液压缓冲减速装置和火箭挂钩装置；

地面回收装置用于捕获火箭，包括支架、导轨、滑块、滑轮和一根回收索组成；导轨位于支架上方，连接成用于回收索运动的区域；一根回收索编制成网状结构，用于火箭回收的区域位于所述结构中；所述结构通过滑块、滑轮实现在导轨上的移动；

跟随捕获控制系统用于控制回收索的移动；包括：视觉测量装置、滤波器和控制器；当火箭进入终端着陆阶段，即火箭距离地面回收索系统的回收平面一定高度时，视觉测量装置开始测量火箭顶端、末端和捕获框的位置信息；顶端和末端的位置信息经过滤波器过滤噪声后计算出回收索系统捕获框的目标位置；四个滑块的位置信息经过滤波器过滤噪声后计算出回收索系统捕获框中心的位置信息；然后将目标位置与回收索系统捕获框中心的位置信息进行比较生成误差信号作为控制器的输入信号，再经过控制器计算得到电机的转速控制信号，最后，电机带动回收索系统中的主动滑轮转动；针对具有横向和轴向移动的回收索的强非线性特性，提出了一种基于ALE索描述的移动索非物质点的无迹卡尔曼滤波器，实现对回收索捕获框的非物质点观测数据的噪声滤波；针对PD控制器的初始时刻非连续特性，设计了含有饱和函数的PD控制器，降低初始回收索的振动，精确地控制地面回收索主动跟随捕获火箭；

液压缓冲减速装置固定在地面上并与回收索连接，用于对待回收火箭进行减速；

火箭挂钩装置安装在待回收火箭外壳；

所述无迹卡尔曼滤波器通过下述方法得到：

步骤一：建立火箭单刚体和回收索-滑块刚柔耦合多体动力学代数微分方程组；利用消去法将代数微分方程组转换为微分方程组；采用向后差分法，确定系统的状态方程和观测方程；

视觉测量装置测量火箭的顶端、末端和捕获框的位置信息时，受到测量噪声、环境噪声和索移动过程中不可避免的振动的影响；利用无迹卡尔曼滤波器设计状态观测器实时获取火箭和捕获框的准确信息，首先建立火箭系统和回收索-滑块系统的多体动力学方程，然后确定系统的状态方程和观测方程；

建立火箭单刚体多体动力学方程；定义回收索系统坐标系的原点在回收平面的中心o，x轴和y轴在回收平面上，分别与相邻的两个导轨平行，z轴过原点o向上，三轴满足右手准则，令全局坐标系为回收索坐标系，火箭的局部坐标系原点在火箭的质心O，Z轴过原点沿着火箭的轴线方向指向上，X轴和Y轴互相垂直并与火箭的轴线垂直，满足右手准则；火箭刚体质心的广义坐标为其中/>和/>分别表示刚体质心的位置向量和转动向量；/>其中/>为转角，n为转动方向的单位向量；火箭上任意一点p在全局坐标系下的位置向量为

r_p＝r_g+A_gs_P (1)

其中s_p和r_p分别为点p在全局坐标系下和局部坐标系下的位置向量；正交旋转矩阵 为旋转向量/>的偏斜矩阵，I为3×3单位矩阵；

点p在全局坐标系下的速度表示为

其中为局部坐标系下的角速度向量，/>是矩阵/>的偏斜矩阵，并且局部角速度向量/>与广义角速度向量/>的传递矩阵为/> 为s_p的偏斜矩阵，/>为插值矩阵，/>为广义速度向量；

利用式(2)计算刚体的动能，并利用第一类拉格朗日方程，建立火箭单刚体多体动力学方程为

其中为广义加速度向量，M_g为火箭质量矩阵，/>为附加惯性力，/>为广义外力，具体表示为

上式中m和J为火箭刚体的质量和惯量矩阵；F_f为火箭受到的外力；

火箭多体动力学方程具有非线性，将系统的状态方程式(3)改写为如下隐式形式

其中F₁为系统的隐式状态方程，q_g为系统的广义加速度向量、广义速度向量和广义坐标向量，t为时间；

火箭底端和顶端的测量方程为

其中s_t＝[0 0 l_t]^T和s_b＝[0 0 l_b]^T分别为火箭顶端和底端在火箭局部坐标系下的位置向量，l_t和l_b分别为火箭顶端和底端到质心的距离；h₁为测量方程；

针对回收索-滑块系统，滑块采用刚体建模，并利用移动副约束滑块只能沿着光滑导轨运动；回收索采用ALE(Arbitrary Lagrange Euler)索单元进行建模；充分利用ALE索单元具有物质坐标与网格的独立性的特点：(1)将绳索绕过滑轮通过建立一个ALE索节点，将网格坐标约束在滑轮位置，将主动滑轮的卷入卷出绳索通过对ALE索节点的物质坐标约束进行建模；(2)为了观测捕获框的中心位置，在捕获框各边中心设置一个ALE索节点，并利用事件驱动动态网格技术驱动所述各边ALE索节点随着对应的滑块移动；(3)为了获取回收索的振动，对各段绳索布置均匀网格，并由物质坐标比例约束确保系统的保拓扑性；

滑块的刚体动力学模型与火箭的刚体动力学模型形式一致，其动力学方程为

其中M_s为滑块的质量矩阵，为四个滑块的总广义加速度向量，/>为滑块的附加广义惯性力，/>为滑块的广义外力；滑块s₁,s₂,s₃和s₄只能沿着导轨滑动，需要增加滑块与导轨之间的移动副约束，不考虑导轨的变形，所述约束相当于滑块与地面之间的约束

其中为滑块在全局坐标系下的位置，w为回收区域的边长的一半，/>为滑块的局部坐标系的x方向的单位向量，/>为滑块的局部坐标系的y方向的单位向量，x_o,y_o,z_o为全局坐标系的单位方向向量；

回收索采用ALE索单元建模，两节点ALE索单元的广义坐标为

其中r_i＝[x_i y_i z_i]^T为两个节点(i＝1,2)的位置向量，p_i是两个节点(i＝1,2)的物质坐标；

ALE索单元内任意一点的位置由单元两节点的线性插值得到

r＝N_rq_r, (10)

其中N_r＝[N₁I N₂I]^T为单元的形函数矩阵，为索单元两个结点的位置向量，N₁＝1-s,N₂＝s，s＝(p-p₁)/(p₂-p₁)为归一化后的物质坐标s∈[0,1]，p为ALE索单元内任意一点的物质坐标；

考虑形函数是时间可变的，单元内任一点的速度和加速度通过位置对时间求一阶导和二阶导得到

其中

ALE索单元的控制方程根据虚功原理得出

其中F_f为外力，F_s为弹性力，为惯性力，绳索密度为ρ_c，截面积为A_c；弹性力的虚功表示为

其中ε为轴向应变，β_c为瑞利弹性阻尼系数，E为弹性模量，系数k_c表示绳只能承受拉力；

根据虚功原理，得到索单元的动力学方程为

其中，单元质量阵M_e，物质流动导致的广义惯性力弹性力和阻尼力对应的广义力广义外力/>分别表示为

一根回收索在主动滑轮、定滑轮和捕获框边中心位置各布置一个ALE索节点，并利用这些ALE索节点将一根回收索分为16段，在每段中均匀布置若干ALE索单元，则一根回收索全部用ALE单元建模；考虑到ALE索单元物质坐标和网格坐标的独立性，还需要建立三种约束：

(1)索过滑轮约束

将索过滑轮建模为滑轮对应位置的一个ALE索节点，并对这个ALE索节点的网格进行约束，只允许物质的流动，约束方程为

其中和r_ii分别为第ii个滑轮的位置向量和滑轮对应的ALE索节点的位置向量，ii＝1,2,...,13；

对于主动滑轮卷入和卷出回收索，采用ALE索节点的物质坐标约束来建模，约束方程为

其中为主动滑轮(jj＝a,b,c,d,a)对应的ALE索节点的物质速度，f_jj(t)为对应的主动滑轮线速度控制指令；一根回收索首尾连接，第一个ALE索节点和最后一个ALE索节点与同一个主动滑轮建立约束；

(2)捕获框边中点ALE索节点事件驱动动态约束

为了从动力学方程中直接获取捕获框边的中心位置信息，在捕获框四个边的中心位置各布置一个ALE索节点c_i(i＝1,2,3,4)，并约束四个索节点始终与平行滑块s_i(i＝1,2,3,4)的中心位置对齐，由滑块的运动驱动ALE索节点的移动，则事件驱动动态约束表示为

其中为捕获框边中心位置的ALE索节点c_i(i＝1,2,3,4)的位置，/>为对应滑块s_i(i＝1,2,3,4)的位置；

(3)段内节点物质坐标比例约束

为了获取回收索的振动特性和保持系统的拓扑性不变，每段回收索划分若干ALE索单元，每段的网格在移动过程中单元数不变并保持均匀分布；对于由ALE索节点m₁和索节点m₂为边界的一段回收索，对段内的ALE索节点的物质坐标建立物质坐标比例约束

其中和/>为这段索的边界索节点m₁和索节点m₂的物质坐标，p_kk为段内的ALE索节点kk物质坐标，kk＝m₁+1,m₁+2,....,m₂-1；

整合滑块的动力学方程式(7)、ALE索单元的动力学方程式(13)、滑块的约束方程和ALE索节点对应的约束方程式(14)—式(17)，得到回收索-滑块刚柔耦合多体动力学方程为

其中M为系统的总质量矩阵，为n_s维系统的广义加速度，Q_a、Q_e和Q_f为系统的总附加广义惯性力、总广义弹性力和总广义外力；/>为m_s维系统的总约束方程；λ为拉格朗日乘子，Φ_q为约束方程的Jacobian矩阵；

式(18)是代数微分方程组，由于含有未知的非状态变量拉格朗日乘子和质量矩阵不可逆，不能直接作为无迹卡尔曼滤波器的状态方程；利用消去法，将n_s维代数微分方程转化为f_s＝n_s-m_s维微分方程组，使微分方程组的个数等于系统的实际自由度；

为了消去非独立广义坐标，首先利用约束方程找到非独立广义坐标和独立广义坐标之间的关系；由于回收索-滑块系统的约束方程是非定常的，对约束方程进行一次和二次导数得

引入独立速度 是非独立速度/>在常数矩阵B上的投影/>则将速度约束方程增广为

其中矩阵B为对约束方程的Jacobian矩阵Φ_q主轴高斯三角化法构造的布尔矩阵；

则广义速度和广义加速度表示为

其中矩阵S是右端项逆矩阵的前m_s行，矩阵R是右端项逆矩阵的后f_s＝n_s-m_s行；b和c为式(19)中的中间变量；

去掉式(18)的第一行中的拉格朗日乘子项，左乘R^T，利用关系式(21)将系统的微分代数微分方程转换为独立坐标z的微分动力学方程组

将回收索-滑块多体系统动力学方程式(22)，改写为隐式形式作为系统的状态传递函数

捕获框边的中心的测量方程为

综上所述，火箭单刚体系统和回收索-滑块刚柔耦合系统的非线性动力学微分方程组分别为式(5)和式(23)；火箭顶端与底端的测量方程是非线性方程组式(6)，捕获框边的中心的测量方程为线性方程组式(24)；

为了便于后续的计算分析，统一两个系统的隐式动力学方程组式(5)和式(23)为

利用向后差分法对系统动力学方程进行时间积分离散；由于无迹卡尔曼滤波使用的是前一个时间步的统计信息，因此选择向后差分格式为一步定步长格式；当前t_k＝t_k-1+Δt时刻的z_k为待求解变量，Δt是积分步长，t_k-1时刻的z_k-1值已知，那么t_k时刻的z_k需要满足

其中预估t_k时刻和/>

则式(25)转化为了一个非线性代数方程，该方程仅是z_k的非线性方程，采用牛顿迭代法求解；

在第m次迭代，位置、速度和加速度矢量分别记为则牛顿迭代增量Δz由求解如下线性方程组得到

JΔz＝d (27)

其中Jacobian迭代矩阵为

更新后新的位置、速度和加速度矢量满足

完成更新后，若当前时间步隐式动力学方程d的范数大于设定的迭代误差tol，进入下一次牛顿迭代；否则说明迭代已经收敛，退出牛顿迭代循环并将收敛值记为然而，由于无迹卡尔曼滤波的每个时间步状态传递的为均值和协方差的统计值，采用向后差分法求解隐式微分方程时，求解t_k时刻的解，需要t_k-1时刻的位置z_k-1、速度/>和加速度/>向量的信息；因此选择无迹卡尔曼滤波器的状态变量为/>t_k-1时间步的状态变量传递到t_k时间步的状态变量的过程，式(26)-式(28)，简记为

x_k＝BDF(x_k-1,t_k-1) (29)

综上所述，式(29)为火箭单刚体系统和回收索-滑块系统的状态传递函数，统一观测方程并改写为离散形式；考虑过程噪声和测量噪声，用于无迹卡尔曼滤波的状态函数和传递函数为

其中x为系统状态变量，动力学方程的广义坐标个数为f，则状态变量个数为nn＝3f_s；BDF(x)和h(x)分别状态传递函数和观测函数，W和V是过程噪声和测量噪声，分别具有协方差矩阵Q和R；

步骤二：采用无迹卡尔曼滤波法对视觉测量信号进行滤波；

火箭单刚体和回收索-滑块系统多体动力学方程具有非线性特性；无迹卡尔曼滤波使用无迹变换来处理均值和协方差的非线性传递问题，能够获得准确稳定的滤波结果；无迹卡尔曼滤波分为预测和更新两个部分；状态变量x是nn维随机变量，经历无迹变换后得到2nn+1个Sigma点X；无迹卡尔曼滤波过程采用矩阵形式来表示；

(1)预测

计算预测的状态均值和预测协方差/>

计算预测的测量均值μ_k、测量协方差S_k和互协方差C_k

式(31)和式(32)中的矩阵形式的无迹变换为

其中nn为状态变量的维度，下标a为均值，下标v为协方差，上标为第几个采样点；参数λ＝α²(nn+κ)-nn是一个缩放比例参数，用来降低总的预测误差，α的取值控制了采用点的分布状态，κ取值确保(nn+P)为半正定矩阵，β≥0是一个非负权系数，合并方程中高阶项的动差；

(2)更新

计算滤波器增益K_k，状态变量均值m_k和协方差矩阵P_k

综上所述，通过建立火箭单刚体和回收索-滑块系统的多体动力学模型，即式(3)和式(18)，然后利用向后差分法推导系统状态方程和观测方程式(30)，最后利用无迹卡尔曼滤波式(31)和式(32)，实现对视觉测量装置测量的火箭顶端与末端位置信息和捕获框的位置信息的滤波。

2.如权利要求1所述一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，其特征在于：

所述一根回收索编制成网状结构的方法为：一根回收索从一个支架上的滑轮开始，逆时针依次绕过相邻导轨上滑块的滑轮、相对导轨上滑块的滑轮和与之相邻的支架上的滑轮，按照此规律循环四次，直至绕到首个支架上的滑轮，一根回收索形成一个网状结构。

3.如权利要求1所述一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，其特征在于：

所述控制器通过下述方法得到：

步骤一：由经过滤波器滤波的火箭顶端和末端位置信息计算控制器的输入信号；

视觉测量装置的测量坐标系为回收索系统坐标系，经过滤波的火箭顶端和末端的位置信息在全局坐标系下分别为r_b＝[x_b y_b z_b]^T和r_t＝[x_t y_t z_t]^T，捕获框的目标位置为

捕获框的移动是通过电机驱动主动滑轮转动，进而带动回收索拖动滑块在光滑导轨上滑动来实现的；四个主动滑轮a,b,c和d对应的目标卷入卷出索长度为

u_a＝-x_tgt+y_tgt,u_b＝-x_tgt-y_tgt,u_c＝x_tgt-y_tgt,u_d＝x_tgt+y_tgt. (36)

其中各主动滑轮卷入卷出索的方向以绕z轴逆时针为正；

则控制器的输入信号为

u＝[u_a u_b u_c u_d]^T (37)

步骤二：利用滤波器滤波后的绳索位置信息计算控制器的反馈信号；

滤波器获得的捕获框四个边的中心点c₁,c₂,c₃和c₄的位置信息为和/>则捕获框中心位置为

控制器的反馈信号为

c＝[c_a c_b c_c c_d]^T (39)

其中c_a＝-x_c+y_c,c_b＝-x_c-y_c,c_c＝x_c-y_c,c_d＝x_c+y_c；

步骤三：设计含有饱和函数的PD控制器

由步骤一和步骤二获得的控制器输入信号和反馈信号得到控制器的控制信号为

e＝u-c (40)

控制器的控制律为

其中v_p∈R^4×1为电机的转动控制速度，a₀和a₁分别为比例系数和微分增益系数；饱和函数κ(t)是时间的函数，具体表示为当t≤t_c/2时，κ(t)＝2t/t_c，当t＞t_c/2时，κ(t)＝1；t_c为整个跟随捕获阶段的总时间。

4.如权利要求1所述一种基于无迹卡尔曼滤波器的精确控制火箭回收索系统，其特征在于：将挂钩安装在火箭的几何中心及偏上位置；挂钩由长短两个杆组成，其中长杆的一端通过柱铰与箭体外壳连接，中部通过弹簧阻尼器连接在箭体外壳上，末端通过弹簧和限位装置与短杆构成一定角度的钩；挂钩在发射阶段通过带有爆炸螺栓的刚性捆绑带收拢在箭体上，在着陆阶段爆炸螺栓解锁，挂钩在压缩的弹簧阻尼器作用下展开；不需要配置展开动力；挂钩的安装位置选定为两个贮箱的箱间段，这段壳臂结构通过加厚或者布置加强筋提高局部的应力极限强度。