Trajectory optimization for collaborative recovery of parafoil systems using unmanned vessel

Abstract

Traditional trajectory optimization methods for the parafoil system set fixed-point landing as the objective. However, in recent payload fairing recovery missions, the recovery system comprising two parafoils is collaboratively recovered by a mobile vehicle, posing new challenges to the current trajectory optimization technique. In order to recover two parafoil systems autonomously with an unmanned surface vessel, this paper presents a trajectory optimization framework composed of three following component processes consecutively. Firstly, a feasibility judgment algorithm based on reachable boundary estimation is designed to determine the possibility of recovering two parafoil systems. Secondly, the decoupled-then-simultaneous strategy is proposed to enhance the convergence of solving the collaborative recovery problem. Thirdly, the finite-element collocation approach is utilized to convert the formulated trajectory optimization problems into nonlinear programming (NLP) problems, which are solved by a highly efficient NLP solver. Simulation results show that the proposed trajectory optimization framework can efficiently generate the optimal trajectory for recovering two parafoil systems with a vessel.

Introduction

Due to the advantages of low cost, high load capacity, and maneuverability, parafoil systems are widely used in space payload recovery, ¹ spacecraft landing, ² and supply delivery. ³ Recently, utilizing the mobile system to recover the parafoil system has become a growing trend. ⁴ The aerospace company SpaceX has achieved a breakthrough in recovering the payload fairings (PLFs) of the Falcon 9 rocket by catching the parafoil system with a vessel. Meanwhile, another company Rocket Lab has also proposed to recover its first-stage booster with the help of a parafoil system and a helicopter. These projects utilized a mobile vehicle to realize collaborative recovery, which is more flexible than the traditional fixed-point landing approach. ⁵ The resulting benefit is that the parafoil system can be more light-weighted and better respond to environmental uncertainties since its control burden is relieved by cooperation with the vessel. However, current collaborative recovery practices depend on manual control of the mobile platform to rendezvous with the parafoil system, resulting in extra mission complexity. As a result of underdeveloped technology, SpaceX has postponed its capture attempt and turned to land the parafoil system on the sea. Although the collaborative recovery mission is suspended, it still deserves to be studied in-depth because it is an enabling technology for PLF recovery and provides an alternative method for landing aerospace systems.

In order to achieve collaborative recovery, the mobile system needs to capture the parafoil system before it lands on the sea. Hence, it is necessary to judge the feasibility of recovery, and this relies on estimating the reachable boundary of each vehicle. In the literature on vehicle reentry, the reachable boundary is obtained by calculating a large number of boundary points, which are sampled by grids, ⁶ segments, ⁷ or virtual points. ⁸ Therefore, the problem of determining the reachable boundary is transformed into solving a series of trajectory optimization problems. The advantage of this method is that it takes complicated dynamic and path constraints into account, which enables a characterization of the complex reachable boundary. The downside is that solving considerable optimization problems can be computationally intensive. In studies on aircraft landing, the reachable boundary is estimated by regular closed curves, such as circle^9,10 and dual half-ellipse. ¹¹ The advantage of this method is that the reachable boundary is represented by a simple closed curve, which facilitates reachability judgment and reduces the computation burden. Previous literature on parafoil recovery utilize the glide ratio to estimate parafoil system’s reachable boundary. On this basis, the feasibility of reaching the fixed landing point is judged. ¹²

Trajectory optimization is one of the most important issues in the field of parafoil recovery. It generally falls into two categories: the traditional and the optimal approach. In the traditional approach, the landing trajectory is composed of circles and straight lines, among which the typical ones are the Dubins path ¹³ and the T path. ¹⁴ The optimization algorithm such as chance-constrained rapidly exploring random trees, ¹⁵ band-limited guidance method, ¹⁶ and heuristic method^17,18 is utilized to determine the parameters of landing trajectory. The traditional approach is widely applied in experiments because of its low computing power requirement. However, the dynamic constraints of the system are ignored in this approach. ¹⁹ The optimal approach is divided into the indirect method and the direct method. The indirect method derives the first-order necessary conditions according to Pontryagin maximum principle and solves a two-point boundary value problem (TBVP) to obtain the optimal landing trajectory.^13,20 The main advantage of the indirect method is that its solution satisfies the first-order optimal condition, and the shortcomings are the small convergence region, cumbersome formulation procedure, and the sensitivity to initial values for TBVP. It is mainly used to handle simplified parafoil models and is not applicable for planning the collaborative recovery trajectory. The direct method is preferred to generate the parafoil homing trajectory because of its ability to approximate dynamics at discrete points and to deal with complex constraints. Gauss pseudospectral method (GPM), which is a class of the direct method, has been used to obtain the numerical solution of parafoil trajectory optimization problems.^21,22 The state and control variables are parameterized using global polynomials at collocation nodes derived from a Gaussian quadrature, thus the trajectory optimization problem is transcribed into an NLP problem. For problems with smooth solutions, GPM provides accurate approximations and exponential convergence. ²³ However, for problems whose solutions are non-smooth or not well approximated by global polynomials, finite-element collocation approaches^24–26 are preferred, where the time interval is partitioned into subintervals and polynomials are used to approximate the state and control profiles over each subinterval. Apart from the work focusing on the fixed-point landing of parafoil system, Hewgley et al. discussed the procedure of recovering the parafoil system with the surface vessel. ²⁷ Sun et al. established the kinematic model of the parafoil system and the vessel, and utilized GPM method to obtain the collaborative recovery trajectory. ⁵

In this paper, a trajectory optimization framework is proposed for the collaborative recovery of two parafoil systems using an unmanned vessel, with the purpose to reduce the operation expense of PLF recovery and provide an alternative approach for recovering two PLF-parafoil systems. The recovery process is formulated as a trajectory optimization problem that consists of vehicle dynamics, boundary constraints, and path constraints. To determine the possibility of recovering two parafoil systems, reachable boundaries of vehicles are estimated and a feasibility judgment algorithm is designed. The direct method is often sensitive to the initial guess, and a rough initial guess may lead to solution failure of the corresponding NLP problem. ²⁸ Thus, the decoupled-then-simultaneous strategy is designed to improve the convergence of solving process and simultaneously optimize the overall trajectory. For the established trajectory optimization problem, the finite-element collocation approach is selected to transcribe the trajectory optimization problem to NLP problems addressed by the off-shelf solver. The main contributions of this paper lie in that (1) The collaborate recovery between two parafoil systems and an unmanned surface vessel is innovatively proposed; (2) The feasibility judgment of recovering the parafoil system with the unmanned surface vessel is discussed for the first time; and (3) The decoupled subproblems are formulated and solved to provide a high-quality initial trajectory for the collaborative recovery trajectory optimization process, so as to improve the convergence property.

Collaborative recovery problem

In this section, dynamic models for the collaborative fairing recovery process are modeled, the constraints acting on the vehicles are established, and the overall trajectory optimization problem is formulated in the end.

Dynamic models

The motions of the parafoil system and the vessel are described in the ground coordinate O _R X _R Y _R Z _R , as shown in Figure 1. Its origin O _R is located at the vessel’s initial position. The O _R Y _R axis points to the anti-wind direction, the O _R X _R axis points to right hand, and the O _R Z _R axis makes up a right-hand coordinate system. We define the body coordinate of each vehicle as O _B X _B Y _B Z _B . Its origin O _B locates at the center of the parafoil system.

Figure 1.

Coordinates of collaborative recovery.

Given that the parafoil system is unpowered and underactuated, it is sufficient to use the three degree-of-freedom (DOF) model to optimize its movement trajectory. The dynamic equations of the parafoil system are as follows^12,13:

$\begin{array}{l}{\dot{x}}^p=v^p\hspace{0.17em}\cos \hspace{0.17em}{\gamma }^p\hspace{0.17em}\sin \hspace{0.17em}{\psi }^p\\ {\dot{y}}^p=v^p\hspace{0.17em}\cos \hspace{0.17em}{\gamma }^p\hspace{0.17em}\cos \hspace{0.17em}{\psi }^p+w_{wind}\\ {\dot{z}}^p=v^p\hspace{0.17em}\sin \hspace{0.17em}{\gamma }^p\\ {\dot{v}}^p=-\frac{1}{m}\left(D^b+D^f\right)-g\hspace{0.17em}\sin \hspace{0.17em}{\gamma }^p\\ {\dot{\gamma }}^p=\frac{1}{m{v}^p}\left(L^b+L^f\right)\cos \hspace{0.17em}{\sigma }^p-\frac{g}{v^p}\cos \hspace{0.17em}{\gamma }^p\\ {\dot{\psi }}^p=\frac{1}{m{v}^p\hspace{0.17em}\cos \hspace{0.17em}{\gamma }^p}\left(L^b+L^f\right)\sin \hspace{0.17em}{\sigma }^p\\ {\dot{\sigma }}^p=\frac{1}{T_{\sigma }}\left(K_{\sigma }{\delta }_a-{\sigma }^p\right)\end{array}$

(1)

where m is the total system mass; v ^p is the velocity; γ ^p is the flight path angle; ψ ^p is the heading angle relative to the O _R Y _R axis; σ ^p is the bank angle; T _σ and K _σ are constants; w _wind is the estimated constant wind field on the ground coordinate; The gravity acceleration is $g=\mu /{\left(z^p+R_e\right)}^2$ , where μ is a constant and R _e is the radius of the earth; δ _a is the parafoil’s asymmetric flap deflection angle, which is controlled to realize turning maneuver. The lift and drag forces of the payload and the parafoil are defined as:

$\begin{array}{l}{D}^b=0.5\rho {\left(v^p\right)}^2{S}_{ref}^b{C}_D^b\\ L^b=0.5\rho {\left(v^p\right)}^2{S}_{ref}^b{C}_L^b\end{array}$

(2)

$\begin{array}{l}{D}^f=0.5\rho {\left(v^p\right)}^2{S}_{ref}^f\left(C_{D0}^f+C_{D{\delta }_s}{\delta }_s\right)\\ L^f=0.5\rho {\left(v^p\right)}^2{S}_{ref}^f\left(C_{L0}^f+C_{L{\delta }_s}{\delta }_s\right)\end{array}$

(3)

where the superscripts b and f correspond to the payload and the parafoil of the parafoil system; $S_{ref}^b$ and $S_{ref}^f$ are the reference areas; $\rho ={\rho }_0{e}^{-z^p/h_s}$ is the atmospheric density; ρ₀ and h _s are the reference density and height; $C_D^b$ , $C_L^b$ , $C_{D0}^f$ , $C_{D{\delta }_s}$ , $C_{L0}^f$ , and $C_{L{\delta }_s}$ are the coefficients; δ _s is the symmetric flap deflection angle, which is controlled to decelerate the parafoil system before touchdown.

Unlike the parafoil system, the unmanned vessel is powered by the engine to directly change its speed and angular velocity, making it necessary to incorporate the angular velocity term into the dynamic model. Hence, the dynamics of the vessel are expressed as follows:

$\begin{array}{l}{\dot{x}}^s=v^s\hspace{0.17em}\sin \hspace{0.17em}{\psi }^s\\ {\dot{y}}^s=v^s\hspace{0.17em}\cos \hspace{0.17em}{\psi }^s\\ {\dot{\psi }}^s={\omega }^s\\ {\dot{v}}^s=\frac{1}{m_s}\left(-T\hspace{0.17em}\sin {\delta }_T-D^s\right)\\ {\dot{\omega }}^s=\frac{1}{J_s}\left(r_{T}T\hspace{0.17em}\sin {\delta }_T-M_D^s\right)\end{array}$

(4)

where m _s is the mass of the vessel; J _s is the rotational inertia matrix; v ^s is the velocity; ω ^s is the angular velocity; T and δ _T are the margin and direction angle of thrust; and r _T denotes the distance from the center of mass to the direction of the engine. The drag force and damping term of the vessel are defined as:

$\begin{array}{l}{D}^s=0.5{\rho }_s{\left(v^s\right)}^2{S}_{ref}^s{C}_D^s\\ M_D^s=0.5{\rho }_s{\left({\omega }^s\right)}^2{S}_{ref}^s{C}_n^s\end{array}$

(5)

where $S_{ref}^s$ is the reference area of the vessel; ρ _s is the density of seawater; and $C_D^s$ and $C_n^s$ are the coefficients.

Constraints

The state and control variables of the parafoil system are denoted as ${\mathit{X}}_R^s={\left[x^s,y^s,{\psi }^s,v^s,{\omega }^s\right]}^{\mathrm{T}}$ and ${\mathit{U}}^{p\left(i\right)}={\left[{\delta }_a,{\delta }_s\right]}^{\mathrm{T}}$ , where the superscript i corresponds to each parafoil system and subscript R corresponds to the ground coordinate. The state and control variables of the vessel are defined as ${\mathit{X}}_R^s={\left[x^s,y^s,{\psi }^s,v^s,{\omega }^s\right]}^{\mathrm{T}}$ and ${\mathit{U}}^s={\left[T,{\delta }_T\right]}^{\mathrm{T}}$ . The dynamic models of the i-th parafoil system and the vessel can be written into the following compact form:

${\dot{\mathit{X}}}_R^{p\left(i\right)}={\mathit{f}}^{p\left(i\right)}\left({\mathit{X}}_R^{p\left(i\right)}\left(t\right),{\mathit{U}}^{p\left(i\right)}\left(t\right)\right)$

(6)

${\dot{\mathit{X}}}_R^{\hspace{0.17em}s}={\mathit{f}}^s\left({\mathit{X}}_R^s\left(t\right),{\mathit{U}}^s\left(t\right)\right)$

(7)

The initial constraints are expressed as follows:

${\mathit{X}}_R^{p\left(i\right)}\left(0\right)={\mathit{X}}_{init}^{p\left(i\right)}$

(8)

${\mathit{X}}_R^s\left(0\right)={\mathit{X}}_{init}^s$

(9)

Three terminal constraints are enforced when the vessel rendezvous the parafoil system. First, the distance between two vehicles should meet the specified threshold when the parafoil system reaches the terminal height. This ensures that the parafoil system is accurately captured by the net installed on the unmanned vessel. Second, the heading angles of the two vehicles should be kept close to guarantee that the parafoil approaches from aft of the vessel. Third, the parafoil system should land near upwind to perform a flared landing, thus reducing its ground velocity.

$\begin{array}{l}\Vert {\mathit{e}}_{27}{\mathit{X}}_R^{p\left(i\right)}\left(t_f^{\left(i\right)}\right)-{\mathit{e}}_{25}{\mathit{X}}_R^s\left(t_f^{\left(i\right)}\right)\Vert ⩽{△}_r\\ z^{p\left(i\right)}\left(t_f^{\left(i\right)}\right)=z_f\end{array}$

(10)

$\cos \left({△}_{\psi }\right)-\hspace{0.17em}\cos \left({\psi }^{p\left(i\right)}\left(t_f^{\left(i\right)}\right)-{\psi }^s\left(t_f^{\left(i\right)}\right)\right)⩽0$

(11)

$\cos \left({△}_{\alpha }\right)-\hspace{0.17em}\cos \left({\psi }^{p\left(i\right)}\left(t_f^{\left(i\right)}\right)-{\alpha }_w+\pi \right)⩽0$

(12)

where z _f is the capture altitude; $t_f^{\left(i\right)}$ is the terminal time of parafoil i; Δ _r , Δ _ψ , and Δ _α are constants; ${\mathit{e}}_{27}=\left[\begin{array}{ll}{\mathit{I}}_{2\times 2}& {\mathit{0}}_{2\times 5}\end{array}\right]$ and ${\mathit{e}}_{25}=\left[\begin{array}{ll}{\mathit{I}}_{2\times 2}& {\mathit{0}}_{2\times 3}\end{array}\right]$ are constant matrixes; and I _2×2 and 0_2×5 are the identity matrix and zero matrix; Here, the cosine function is utilized to include the condition where the vehicle makes several turns before rendezvous.

The total flap deflection of the parafoil system is limited, leading to constraints on the symmetric and asymmetric flap deflection angle.

$\begin{array}{l}-{\delta }_{\max }^p⩽{\delta }_a\left(t\right)⩽{\delta }_{\max }^{\hspace{0.17em}p}\\ 0⩽{\delta }_s\left(t\right)⩽{\delta }_{\max }^{\hspace{0.17em}p}\\ {\delta }_s\left(t\right)+{\delta }_a\left(t\right)⩽{\delta }_{\max }^{\hspace{0.17em}p}\\ {\delta }_s\left(t\right)-{\delta }_a\left(t\right)⩽{\delta }_{\max }^{\hspace{0.17em}p}\end{array}$

(13)

The above constraints can be simplified as:

$\begin{array}{l}0⩽{\delta }_s\left(t\right)\\ {\delta }_s\left(t\right)+\left|{\delta }_a\left(t\right)\right|⩽{\delta }_{\max }^{\hspace{0.17em}p}\end{array}$

(14)

Similarly, the margin and direction of the vessel’s thrust are constrained due to power and mechanical limits.

$-T_{\max }/3⩽T\left(t\right)⩽T_{\max }$

(15)

$-{\delta }_{\max }^T⩽{\delta }_T\left(t\right)⩽{\delta }_{\max }^T$

(16)

where ${\delta }_{\max }^p$ corresponds to the maximum flap deflection angle; T_max is the maximum thrust; and ${\delta }_{\max }^T$ is the maximum direction angle of thrust. The negative value of $T\left(t\right)$ corresponds to deceleration maneuver. The maximum reverse thrust is assumed be one-third of the maximum forward thrust for the surface vessel.

What’s more, the maximum velocity of the vessel is restricted, and reverse movement is not allowed:

$0⩽v^s\left(t\right)⩽v_{\max }$

(17)

To provide a smooth control profile for trajectory tracking and control, the change rates of controls are constrained as:

$\begin{array}{l}-{\dot{\delta }}_{a\hspace{0.17em}\max }⩽{\dot{\delta }}_a\left(t\right)⩽{\dot{\delta }}_{a\hspace{0.17em}\max }\\ -{\dot{\delta }}_{s\hspace{0.17em}\max }⩽{\dot{\delta }}_s\left(t\right)⩽{\dot{\delta }}_{s\hspace{0.17em}\max }\end{array}$

(18)

$\begin{array}{l}-{\dot{T}}_{\max }⩽\dot{T}\left(t\right)⩽{\dot{T}}_{\max }\\ -{\dot{\delta }}_{T\hspace{0.17em}\max }⩽{\dot{\delta }}_T\left(t\right)⩽{\dot{\delta }}_{T\hspace{0.17em}\max }\end{array}$

(19)

where ${\dot{\delta }}_{a\hspace{0.17em}\max }$ , ${\dot{\delta }}_{s\hspace{0.17em}\max }$ , ${\dot{T}}_{\max }$ , and ${\dot{\delta }}_{T\hspace{0.17em}\max }$ are the upper bounds for control variables. The parafoil actuators are assumed zero-time delay to facilitate the trajectory optimization process. ²²

Overall formulation

The main objectives, including the landing precision and landing direction have already been considered in the terminal constraints. Therefore, the control energy is considered in the objective function. Firstly, the flap deflection control of parafoil systems are minimized to reduce the control effort of electrically driven motors, which commands the control line to deflect left and right flaps. Similarly, thrust control of the vessel is minimized to reduce fuel consumption. Thus, the objective function that minimizes the control energy is defined as follows

$J=\left({\int }_0^{t_f^{p\left(1\right)}}{\Vert {\overline{\mathit{U}}}^{p\left(1\right)}\Vert }^{2}dt+{\int }_0^{t_f^{p\left(2\right)}}{\Vert {\overline{\mathit{U}}}^{p\left(2\right)}\Vert }^{2}dt\right)+{\int }_0^{\max \left(t_f^{p\left(1\right)},t_f^{p\left(2\right)}\right)}{\Vert {\overline{\mathit{U}}}^s\Vert }^{2}dt$

(20)

where ${\overline{\mathit{U}}}^{p\left(1\right)}$ , ${\overline{\mathit{U}}}^{p\left(2\right)}$ , and ${\overline{\mathit{U}}}^s$ are the non-dimensioned control variables.

Then, the collaborative recovery trajectory optimization problem is constructed as Problem P1

$\begin{array}{l}\underset{{\mathit{U}}^{p\left(i\right)},{\mathit{U}}^s,t_f^{p\left(i\right)}}{\min }J\\ \begin{array}{l}\mathrm{s}.\mathrm{t}.\text{DynamicConstraints}:\left(6\right)\left(7\right)\\ \text{PathConstraints}:\left(14\right)-\left(19\right)\\ \text{InitialConstraints}:\left(8\right)\left(9\right)\\ \text{TerminalConstraints}:\left(10\right)-\left(12\right)\\ i=\mathrm{1,2}\end{array}\end{array}$

(21)

The formulated trajectory optimization problem contains the dynamics and related constraints of two parafoil systems and an unmanned vessel, leading to a larger problem size than the usual pinpoint landing. Besides, the capture moments of two parafoil systems are optimization variables, meaning that it is a complex two-phase free-terminal-time problem. Directly handling Problem P1 may get stuck at an infeasible point or fail to converge. In the following sections, we will design methods to judge the feasibility of collaborative recovery and obtain optimal recovery trajectories.

Recovery feasibility judgment

In this section, the reachable boundaries of the parafoil system and the vessel are estimated. Based on the estimation, a feasibility judgment algorithm is designed to determine the possibility of collaborative recovery and estimate the recovery point.

Reachable boundary estimation

According to Wu and Clothier ¹¹ the reachable boundary of the flight vehicle is closely related to its flight time. Since we estimate the parafoil’s reachable boundary at the start of the recovery mission, its flight time is long enough to perform a turning maneuver. Therefore, it is appropriate to use a circle to estimate the reachable boundary of the parafoil system. As in Figure 2, the estimated circle is determined by three points in the body coordinate. Point $A^{p\left(i\right)}$ is the farthest boundary point along the O _B Y _B axis. Point $B^{p\left(i\right)}$ and point $B^{p{\left(i\right)}^{\prime }}$ are the farthest boundary points along the O _B X _B axis in two directions. Since there is no difference between the left turn and right turn in the body coordinate, the reachable boundary is symmetric along the O _B Y _B axis. Then, the circle is uniquely determined by point $A^{p\left(i\right)}$ and point $B^{p\left(i\right)}$ , and its radius and center point are denoted as $r^{p\left(i\right)}$ and $O_B^{p\left(i\right)}$ .

Figure 2.

Illustration of estimated circle.

In consideration of the dynamic model and related constraints, we formulate the following trajectory optimization problem to obtain the boundary points, whereas the penalty term replaces the original cost function (20). The problem is referred to as Problem P2. The cost function $J_{bound}^{p\left(i\right)}$ is set to $-{\Vert y^{p\left(i\right)}\left(t_f^{p\left(i\right)}\right)\Vert }^2$ for calculating point $A^{p\left(i\right)}$ , and set to $-{\Vert x^{p\left(i\right)}\left(t_f^{p\left(i\right)}\right)\Vert }^2$ for calculating point $B^{p\left(i\right)}$ , respectively.

$\begin{array}{l}\underset{{\mathit{U}}^{p\left(i\right)},t_f^{p\left(i\right)}}{\min }{J}_{bound}^{p\left(i\right)}\\ \mathrm{s}.\mathrm{t}.\hspace{0.17em}{\dot{\mathit{X}}}_B^{p\left(i\right)}={\mathit{f}}^{p\left(i\right)}\left({\mathit{X}}_B^{p\left(i\right)}\left(t\right),{\mathit{U}}^{p\left(i\right)}\left(t\right)\right)\\ {\mathit{X}}_B^{p\left(i\right)}\left(0\right)={\left[\mathrm{0,0},z_{init}^{p\left(i\right)},v_{init}^{p\left(i\right)},{\gamma }_{init}^{p\left(i\right)},0,{\sigma }_{init}^{p\left(i\right)}\right]}^{\mathrm{T}}\\ z^{p\left(i\right)}\left(t_f^{p\left(i\right)}\right)=z_f\\ \begin{array}{l}0⩽{\delta }_s\left(t\right)\\ {\delta }_s\left(t\right)+\left|{\delta }_a\left(t\right)\right|⩽{\delta }_{\max }^p\end{array}\\ -{\dot{\delta }}_{a\hspace{0.17em}\max }⩽{\dot{\delta }}_a\left(t\right)⩽{\dot{\delta }}_{a\hspace{0.17em}\max }\\ -{\dot{\delta }}_{s\hspace{0.17em}\max }⩽{\dot{\delta }}_s\left(t\right)⩽{\dot{\delta }}_{s\hspace{0.17em}\max }\end{array}$

(22)

After estimating the reachable boundary of the parafoil system in the body coordinate, the rotation and translation transformations are conducted to acquire the reachable boundary in the ground coordinate.

$\left[\begin{array}{l}{x}_R^{p\left(i\right)}\\ y_R^{p\left(i\right)}\end{array}\right]=\left[\begin{array}{ll}\cos \left({\psi }_{init}^{p\left(i\right)}\right)& \sin \left({\psi }_{init}^{p\left(i\right)}\right)\\ -\hspace{0.17em}\sin \left({\psi }_{init}^{p\left(i\right)}\right)& \cos \left({\psi }_{init}^{p\left(i\right)}\right)\end{array}\right]\left[\begin{array}{l}{x}_B^{p\left(i\right)}\\ y_B^{p\left(i\right)}\end{array}\right]+\left[\begin{array}{l}{x}_{init}^{p\left(i\right)}\\ y_{init}^{p\left(i\right)}\end{array}\right]+\left[\begin{array}{l}0\\ {\int }_0^{t_f^{p\left(i\right)}}{w}_{wind}dt\end{array}\right]$

(23)

where $\left(x_{init}^{p\left(i\right)},y_{init}^{p\left(i\right)}\right)$ is the initial position of the parafoil system; ${\psi }_{init}^{p\left(i\right)}$ is the initial heading angle; $\left(x_B^{p\left(i\right)},y_B^{p\left(i\right)}\right)$ is the position of radius center in body coordinate, while $\left(x_R^{p\left(i\right)},y_R^{p\left(i\right)}\right)$ is that of radius center in ground coordinate, which is denoted as $O^{p\left(i\right)}$ . $t_f^{p\left(i\right)}$ is the average flight time obtained by solving point $A^{p\left(i\right)}$ and point $B^{p\left(i\right)}$ since the influence of turning control on the vertical velocity is insignificant. ²⁹ The wind profile w _wind is generally measured and estimated before the recovery of parafoil system. Here, we follow the previous literature to use the estimated constant wind velocity profile, ²¹ which corresponds to the mean value of wind during the measurement time.

The reachable boundary of the vessel is determined by the parafoil system it aims to catch. To be specific, the flight time $t_f^{p\left(i\right)}$ of i-th parafoil determines the corresponding reachable boundary of the vessel. Taking advantage of the symmetric property, the approximation circle of the vessel to catch i-th parafoil is also uniquely determined by point $A^{s-\left(i\right)}$ and point $B^{s-\left(i\right)}$ , and its radius and center point are denoted as $r^{s-\left(i\right)}$ and $O_B^{s-\left(i\right)}$ . These two points are obtained by solving the following Problem P3 with the cost function $J_{bound}^s$ defined as $-{\Vert y^s\left(t_f^s\right)\Vert }^2$ and $-{\Vert x^s\left(t_f^s\right)\Vert }^2$ .

$\begin{array}{l}\underset{{\mathit{U}}^s,t_f^s}{\min }{J}_{bound}^s\\ \mathrm{s}.\mathrm{t}.\hspace{0.17em}{\dot{\mathit{X}}}_B^s={\mathit{f}}^s\left({\mathit{X}}_B^s\left(t\right),{\mathit{U}}^s\left(t\right)\right)\\ {\mathit{X}}_B^s\left(0\right)={\left[\mathrm{0,0,0},v_{init}^s,{\omega }_{init}^s\right]}^{\mathrm{T}}\\ t_f^s⩽t_f^{p\left(i\right)}\\ -T_{\max }/3⩽T\left(t\right)⩽T_{\max }\\ -{\delta }_{\max }^T⩽{\delta }_T\left(t\right)⩽{\delta }_{\max }^T\\ 0⩽v^s\left(t\right)⩽v_{\max }\\ -{\dot{T}}_{\max }⩽\dot{T}\left(t\right)⩽{\dot{T}}_{\max }\\ -{\dot{\delta }}_{T\hspace{0.17em}\max }⩽{\dot{\delta }}_T\left(t\right)⩽{\dot{\delta }}_{T\hspace{0.17em}\max }\end{array}$

(24)

The reachable boundary in the ground coordinate is acquired through rotation and translation transformations similar to (23). The only difference is that the influence of the wind field is ignored when considering the vessel’s reachable boundary.

Feasibility judgment algorithm

Due to the unpowered nature of parafoil system, it has limited control to its vertical velocity. Hence, the flight time of each parafoil system is determined by the initial altitude. In the collaborative recovery mission, the initial altitudes of parafoil systems are set to different values, thus enabling the vessel to perform sequential recovery. To facilitate discussion, we denote the parafoil system with lower altitude as Parafoil 1 and the other parafoil as Parafoil 2. Based on the analysis above, the reachable boundaries of Parafoil 1 and Parafoil 2 are estimated as circle $R^{p\left(1\right)}$ and circle $R^{p\left(2\right)}$ . The corresponding boundaries of the vessel are circle $R^{s-\left(1\right)}$ and circle $R^{s-\left(2\right)}$ . Through boundary estimation, the flight time of two parafoil systems is derived, denoted as $t_f^{p\left(1\right)}$ and $t_f^{p\left(2\right)}$ . We define d _m to evaluate the maximum distance the vessel can travel in the time interval between Parafoil 1 landing and Parafoil 2 landing:

$d_m=\left(t_f^{p\left(2\right)}-t_f^{p\left(1\right)}\right)v_{\max }$

(25)

If circle $R^{p\left(1\right)}$ intersects circle $R^{s-\left(1\right)}$ (denoted as $R^{p\left(1\right)}\cap R^{s-\left(1\right)}$ ) and circle $R^{p\left(2\right)}$ intersects circle $R^{s-\left(2\right)}$ (denoted as $R^{p\left(2\right)}\cap R^{s-\left(2\right)}$ ), it means the vessel can recover either Parafoil 1 or Parafoil 2. However, the vessel might fail to recover Parafoil 2 after catching Parafoil 1. We define C as the closest point to $R^{p\left(2\right)}$ from the intersection area of $R^{p\left(1\right)}$ and $R^{s-\left(1\right)}$ . If the distance from C to $R^{p\left(2\right)}$ is shorter than d _m , we assume it is possible to recover Parafoil 2 after catching Parafoil 1. As illustrated in Figure 3, there may be three cases of C due to the different relative positions of reachable boundaries. We define three points C₁, C₂, and C₃ to denote each case, which are checked to determine the possibility of collaborative recovery. For example, suppose the vessel catches Parafoil 1 at point C₁ $\left(C_1{O}^s<r^{s-\left(1\right)}\right)$ and manages to reach Parafoil 2 $\left(C_1{O}^{p\left(2\right)}<r^{p\left(2\right)}+d_m\right)$ in the remaining time. It is appropriate to decide that the vessel can catch both parafoil systems. Suppose one of the three points satisfies the condition, it is judged feasible to perform collaborative recovery, and the corresponding point C is considered as the landing point of Parafoil 1. Otherwise, the vessel can’t catch both parafoil systems.

Figure 3.

Three cases of C. (a) C₁, (b) C₂, and (c) C₃.

Based on the above discussion, the feasibility judgment algorithm is demonstrated in Algorithm 1. It effectively excludes infeasible conditions and provides a reference for the mission design. Note that this judgment algorithm provides an optimistic estimate and the ultimate recovery feasibility remains to be determined in the trajectory optimization procedure. One reason is that the estimated boundary is larger than the actual reachable area, as is shown in Section 5. Another reason is that the boundary and path constraints are not fully considered in the feasibility judgment. Hence, the feasibility judgment algorithm is utilized to make a preliminary judgment and provide X _C as the estimated landing point of Parafoil 1.

Trajectory optimization framework

In this section, the decoupled-then-simultaneous strategy is designed to handle the collaborative recovery trajectory optimization problem. First, the origin problem is decoupled into three simple subproblems, through which the reference trajectory is obtained. Then, the simultaneous problem is tackled to obtain the overall trajectory. After that, the finite-element collocation approach discretizes trajectory optimization problems into NLP problems solved by an efficient solver. Finally, the overall trajectory optimization framework is presented.

Decoupled-then-simultaneous strategy

Since the collaborative recovery problem formulated in Section 2 is a complex multi-vehicle free-terminal-time problem, the direct method may get stuck at an infeasible point or fail to converge. It is necessary to provide a high-quality initial guess, which satisfies dynamic constraints and reduces path and terminal constraints violation, so as to start the iteration and improve the solution convergence. Hence, the decoupled stage is introduced to provide feasible trajectories as an initial guess for the solving process.

To acquire the trajectory of two parafoil systems, the single parafoil recovery problem is established, where Parafoil 1 and Parafoil 2 are considered separately. The terminal position constraint in (10) is reformulated as a cost term in the cost function, which reduces the solution difficulty. Another penalty term that minimizes the distance from the parafoil landing point to point X _C is added, making the landing positions of two parafoil systems as close as possible. Therefore, the cost function $J^{p\left(i\right)}$ for i-th parafoil is defined as:

$\begin{array}{l}{J}^{p\left(i\right)}={\int }_0^{t_f^{p\left(i\right)}}\left({\Vert {\overline{\mathit{U}}}^{p\left(i\right)}\Vert }^2+{\Vert {\overline{\mathit{U}}}^s\Vert }^2\right)dt\\ +{\omega }_1{\Vert {\mathit{e}}_{27}{\mathit{X}}_R^{p\left(i\right)}\left(t_f^{p\left(i\right)}\right)-{\mathit{e}}_{25}{\mathit{X}}_R^s\left(t_f^{p\left(i\right)}\right)\Vert }^2+{\omega }_2{\Vert {\mathit{e}}_{27}{\mathit{X}}_R^{p\left(i\right)}\left(t_f^{p\left(i\right)}\right)-{\mathit{X}}_C\Vert }^2\end{array}$

(26)

where ω₁ and ω₂ are the constant weights; X _C is the predicted Parafoil 1 landing point, provided by the feasibility judgment algorithm in the previous section. Then, Problem P4 is formulated as

$\begin{array}{l}\underset{{\mathit{U}}^{p\left(i\right)},{\mathit{U}}^s,t_f^{p\left(i\right)}}{\min }{J}^{p\left(i\right)}\\ \begin{array}{l}\mathrm{s}.\mathrm{t}.\hspace{0.17em}\text{DynamicConstraints}:\left(6\right)\left(7\right)\\ \text{PathConstraints}:\left(14\right)-\left(19\right)\\ \text{InitialConstraints}:\left(8\right)\left(9\right)\\ \text{TerminalConstraints}:\left(11\right)-\left(12\right)\\ i=1ori=2\end{array}\end{array}$

(27)

Solving the Problem P4 for i = 1 and i = 2 provides the trajectory of Parafoil 1 and Parafoil 2, along with the trajectory of vessel to recover Parafoil 1. Hence, the remaining work is to obtain the trajectory for the vessel to travel from the landing point of Parafoil 1 to that of Parafoil 2. The corresponding boundary constraints of the vessel are:

$\{\begin{array}{l}{\mathit{X}}_R^s\left(0\right)={\mathit{X}}_{p\left(1\right)}^s\\ {\mathit{U}}^s\left(0\right)={\mathit{U}}_{p\left(1\right)}^s\end{array}$

(28)

where ${\mathit{X}}_{p\left(1\right)}^s$ and ${\mathit{U}}_{p\left(1\right)}^s$ are the state and control variables of the vessel when it catches Parafoil 1. The cost function J ^s is written as:

$J^s={\int }_0^{t_f^s}{\Vert {\overline{\mathit{U}}}^s\Vert }^{2}dt+{\omega }_1{\Vert {\mathit{e}}_{25}{\mathit{X}}_R^s\left(t_f^s\right)-{\mathit{e}}_{27}{\mathit{X}}_f^{p\left(2\right)}\Vert }^2$

(29)

where ${\mathit{X}}_f^{p\left(2\right)}$ is the terminal state variables obtained by solving Problem P4 for i = 2; The trajectory optimization problem for the vessel (Problem P5) is defined as:

$\begin{array}{l}\underset{{\mathit{U}}^s,t_f^s}{\min }{J}^s\\ \mathrm{s}.\mathrm{t}.\hspace{0.17em}{\dot{\mathit{X}}}_R^s={\mathit{f}}^s\left({\mathit{X}}_R^s\left(t\right),{\mathit{U}}^s\left(t\right)\right)\\ {\mathit{X}}_R^s\left(0\right)={\mathit{X}}_{p\left(1\right)}^s\\ {\mathit{U}}^s\left(0\right)={\mathit{U}}_{p\left(1\right)}^s\\ t_f^s⩽t_f^{p\left(2\right)}-t_f^{p\left(1\right)}\\ -T_{\max }/3⩽T\left(t\right)⩽T_{\max }\\ -{\delta }_{\max }^T⩽{\delta }_T\left(t\right)⩽{\delta }_{\max }^T\\ 0⩽v^s\left(t\right)⩽v_{\max }\\ -{\dot{T}}_{\max }⩽\dot{T}\left(t\right)⩽{\dot{T}}_{\max }\\ -{\dot{\delta }}_{T\hspace{0.17em}\max }⩽{\dot{\delta }}_T\left(t\right)⩽{\dot{\delta }}_{T\hspace{0.17em}\max },\end{array}$

(30)

The advantage of the decoupled stage is that it decouples the complex original problem into three trivial subproblems. Besides, the terminal position constraints are imposed as penalty terms in the cost function, which further reduces the solution difficulty. However, the decoupled framework and penalized terminal distance condition may lead to suboptimal trajectories, and the maneuverability of vessel is not fully exploited. Therefore, it is necessary to solve the collaborative recovery problem in a unified framework that comprehensively considers the control margins of all vehicles while meeting the strict capture constraints. To overcome the computation difficulty, outputs of the decoupled stage are taken as the initial guess here. The time-domain of the simultaneous problem is divided into two phases: $\left(0,t_{mid}\right)$ and $\left(t_{mid},t_f\right)$ , which corresponds to recover Parafoil 1 and Parafoil 2. The cost function is rewritten as:

$J^a={\int }_0^{t_{mid}}{\Vert {\overline{\mathit{U}}}^{p\left(1\right)}\Vert }^{2}dt+{\int }_0^{t_f}\left({\Vert {\overline{\mathit{U}}}^{p\left(2\right)}\Vert }^2+{\Vert {\overline{\mathit{U}}}^s\Vert }^2\right)dt$

(31)

To ensure the continuity of state variables, linking constraints at the intermediate point are imposed.

$\{\begin{array}{l}{\mathit{X}}_{phase1}^{p\left(2\right)}\left(t_{mid}\right)={\mathit{X}}_{phase2}^{p\left(2\right)}\left(t_{mid}\right)\\ {\mathit{X}}_{phase1}^s\left(t_{mid}\right)={\mathit{X}}_{phase2}^s\left(t_{mid}\right)\end{array}$

(32)

Therefore, the complete simultaneous trajectory optimization problem (Problem P6) is formulated as:

$\begin{array}{l}\underset{{\mathit{U}}^{p\left(i\right)},{\mathit{U}}^s,t_{mid},t_f}{\min }{J}^a\\ \mathrm{s}.\mathrm{t}.\hspace{0.17em}0⩽t⩽t_{mid}\left(phase1\right)\\ \begin{array}{l}\begin{array}{l}\text{DynamicConstraints}:\left(6\right)\left(7\right)\\ \text{PathConstraints}:\left(14\right)-\left(19\right)\\ \text{InitialConstraints}:\left(8\right)\left(9\right)\end{array}\},i=\mathrm{1,2}\\ \text{TerminalConstraints}:\left(10\right)-\left(12\right),i=1\\ t_{mid}⩽t⩽t_f\left(phase2\right)\end{array}\\ \begin{array}{l}\text{DynamicConstraints}:\left(6\right)\left(7\right)\\ \text{PathConstraints}:\left(14\right)-\left(19\right)\\ \text{InitialConstraints}:\left(32\right)\\ \text{TerminalConstraints}:\left(10\right)-\left(12\right)\end{array}\},i=2\end{array}$

(33)

As discussed above, the decoupled-then-simultaneous strategy is generally divided into two stages. In the first stage, two parafoil recovery problems (Problem P4) are solved separately. After that, we get ${\mathit{\chi }}_d^{p\left(1\right)}$ and ${\mathit{\chi }}_d^{p\left(2\right)}$ , which are the trajectories for Parafoil 1 and Parafoil 2. Also, the trajectory for the vessel to catch Parafoil 1 is obtained as ${\mathit{\chi }}_d^{s-\left(1\right)}$ . Then, we solve Problem P5 to obtain the trajectory for the vessel to catch Parafoil 2, which is denoted as ${\mathit{\chi }}_d^{s-\left(2\right)}$ . In the second stage, ${\mathit{\chi }}_d^{p\left(1\right)}$ , ${\mathit{\chi }}_d^{p\left(2\right)}$ , ${\mathit{\chi }}_d^{s-\left(1\right)}$ , and ${\mathit{\chi }}_d^{s-\left(2\right)}$ , are provided as an initial guess to solve Problem P6. The obtained solution is the collaborative recovery trajectory to capture two parafoil systems. The complete decoupled-then-simultaneous strategy is summarized in Algorithm 2.

Finite-element collocation approach

As a class of the direct method, the finite-element collocation approach fully discretizes the state and control variable, converting the trajectory optimization problems into a large-scale NLP problem solved by the numerical solver. Without loss of generality, the following trajectory optimization problem is considered.^24,25

$\begin{array}{l}\underset{\mathit{u},t_f}{\min }J={\int }_0^{t_f}I\left(\mathit{x}\left(t\right),\mathit{u}\left(t\right)\right)dt+\phi \left(\mathit{x}\left(t_f\right)\right)\\ s.t.\hspace{0.17em}\dot{\mathit{x}}=\mathit{f}\left(\mathit{x}\left(t\right),\mathit{u}\left(t\right)\right)\\ \mathit{g}\left(\mathit{x}\left(t\right),\mathit{u}\left(t\right),\dot{\mathit{u}}\left(t\right)\right)⩽0\\ \mathit{x}\left(t_0\right)={\mathit{x}}_{init}\\ \mathit{\psi }\left(\mathit{x}\left(t_f\right)\right)⩽0\end{array}$

(34)

where $\mathit{x}\left(t\right)$ and $\mathit{u}\left(t\right)$ are the state and control variables; t _f is the terminal time; J is the cost function; I is the integrated function; and Φ is the terminal cost term.

The integration part in the cost function is converted to differential and boundary constraints, and the newly defined differential variable $x_I\left(t\right)$ is added to $\mathit{x}\left(t\right)$ .

$\{\begin{array}{l}{\dot{x}}_I\left(t\right)=I\left(\mathit{x}\left(t\right),\mathit{u}\left(t\right)\right)\\ x_I\left(0\right)=0\end{array}$

(35)

The objective function is transformed into:

$J=x_I\left(t_f\right)+\phi \left(\mathit{x}\left(t_f\right)\right)=\Phi \left(\mathit{x}\left(t_f\right)\right)$

(36)

The inequality constraints on the derivative of control variables are transformed into differential and inequality constraints with an added variable u _dot .

$\{\begin{array}{l}\dot{\mathit{u}}={\mathit{u}}_{dot}\\ \mathit{g}\left(\mathit{x}\left(t\right),\mathit{u}\left(t\right),{\mathit{u}}_{dot}\left(t\right)\right)⩽0\end{array}$

(37)

First, the free-terminal-time problem is transformed into a fixed-terminal-time problem by introducing a new variable $\sigma \in \left[\mathrm{0,1}\right]$ , such that:

$t=t_f\sigma ,t\in \left[0,t_f\right]$

(38)

The normalized time horizon σ is evenly divided into N finite elements, which satisfies:

$0={\sigma }_0<{\sigma }_1<\cdots <{\sigma }_{N-1}<{\sigma }_N=1$

(39)

Then K + 1 interpolation points are selected in each finite element i. The state and control variables are approximated by Lagrange polynomial as follows:

$\begin{array}{l}{\mathit{x}}_i\left(\sigma \right)={\displaystyle \sum }_{j=0}^K{L}_j\left(\sigma \right){\mathit{x}}_{ij},L_j\left(\sigma \right)={\prod }_{k=0,\ne j}^K\frac{\left(\sigma -{\sigma }_{ik}\right)}{\left({\sigma }_{ij}-{\sigma }_{ik}\right)}\\ {\mathit{u}}_i\left(\sigma \right)={\displaystyle \sum }_{j=1}^K{\overline{L}}_j\left(\sigma \right){\mathit{u}}_{ij},{\overline{L}}_j\left(\sigma \right)={\prod }_{k=1,\ne j}^K\frac{\left(\sigma -{\sigma }_{ik}\right)}{\left({\sigma }_{ij}-{\sigma }_{ik}\right)}\\ where{\sigma }_{ik}={\sigma }_{\left(i-1\right)K}+h{\tau }_k,k=0,\dots ,K\end{array}$

(40)

where h = 1/K refers to the length of the finite element; τ₀ = 0 corresponds to the left endpoint, and 0 < τ _k ⩽1, k = 1, …, K are the shifted-Radau collocation points. ²⁶

The boundary constraints are fixed at the first and last finite-elements:

${\mathit{x}}_1\left({\sigma }_{10}\right)={\mathit{x}}_{init}$

(41)

$\mathit{\psi }\left({\mathit{x}}_N\left({\sigma }_{NK}\right)\right)⩽0$

(42)

The continuity of the state variables at the finite-element boundaries is enforced:

${\mathit{x}}_{i+1}\left({\sigma }_{\left(i+1\right)0}\right)={\mathit{x}}_i\left({\sigma }_{iK}\right),i=1,\dots ,N-1$

(43)

Utilizing (40), the collocation equations at the interpolation points are derived as:

$\begin{array}{l}{\displaystyle \sum }_{j=0}^K\frac{d{L}_j\left({\sigma }_{ik}\right)}{d\sigma }{\mathit{x}}_{ij}-t_f\mathit{f}\left({\mathit{x}}_{ik},{\mathit{u}}_{ik}\right)=0\\ i=1,\dots ,N,k=1,\dots ,K\end{array}$

(44)

where ${\mathit{x}}_i\left({\sigma }_{ik}\right)$ and ${\mathit{u}}_i\left({\sigma }_{ik}\right)$ are abbreviated as x _ik and u _ik .

Eventually, the trajectory optimization problem is discretized into the following NLP formulation solved by the highly efficient NLP solver. The solution is returned as the maneuvers of parafoil systems and the vessel.

$\begin{array}{l}\underset{{\mathit{u}}_{ik},t_f}{\min } \Phi \left({\mathit{x}}_{NK}\right)\\ s.t.{\displaystyle \sum }_{j=0}^K\frac{d{L}_j\left({\sigma }_{ik}\right)}{d\sigma }{\mathit{x}}_{ij}-t_f\mathit{f}\left({\mathit{x}}_{ik},{\mathit{u}}_{ik}\right)=0\\ \mathit{g}\left({\mathit{x}}_{ik},{\mathit{u}}_{ik}\right)⩽0\\ {\mathit{x}}_1\left({\sigma }_{10}\right)={\mathit{x}}_{init}\\ \mathit{\psi }\left({\mathit{x}}_N\left({\sigma }_{NK}\right)\right)⩽0\\ {\mathit{x}}_{i+1}\left({\sigma }_{\left(i+1\right)0}\right)={\mathit{x}}_i\left({\sigma }_{iK}\right)\end{array}$

(45)

Overall framework

To better illustrate how the trajectory optimization framework works, the whole procedure discussed above is presented in Figure 4 and concluded as follows:

Figure 4.

Trajectory optimization framework for collaborative recovery.

Step 1:

The boundary estimation problems (Problem P2 and Problem P3) are solved by finite-element collocation approach to estimate the reachable boundaries of Parafoil 1, Parafoil 2, and the vessel;

Step 2:

The feasibility of collaboratively recovering two parafoil systems with the vessel is judged via Algorithm 1;

Step 3:

Once the collaborative recovery is judged feasible, the guess of recovery point is provided to formulate decoupled trajectory optimization problems (Problem P4 and Problem P5);

Step 4:

The decoupled trajectory optimization problems obtained in Step 3 are solved to provide the initial guess of collaborative recovery trajectory;

Step 5:

The simultaneous trajectory optimization problem (Problem P6) is solved by the finite-element collocation approach based on the initial guess in Step 3;

Step 6:

The optimal trajectories for collaborative recovery are output.

Results and discussions

In this section, numerical results are presented to validate the proposed trajectory optimization algorithm. First, the effectiveness of the reachable boundary estimation is verified. Then, four mission cases are discussed to demonstrate the proposed trajectory optimization framework. Finally, a Monte-Carlo simulation that considers initial state uncertainties is performed. Without loss of generality, two identical parafoil systems are considered, and simulation parameters are given in Table 1. The simulation uses a fixed wind direction blowing in the negative direction of y-axis with a constant magnitude of 3 m/s. Numerical simulations are performed in the Python environment and executed on a laptop with an i7-10750H CPU and 16 GB RAM. Optimization problems were modeled using Pyomo ³⁰ and solved by the open-source NLP solver IPOPT. ³¹

Table 1.

Problem parameters for simulation.

Parameter	Value
μ	3.986e14
R e	6.378e6 m
ρ 0	1.226 kg/m3
ρ s	1.025 kg/m3
h s	7.254e3 m
m p	1500 kg
T σ	0.994
K σ	0.504
$S_{ref}^b$	50 m2
$S_{ref}^f$	165 m2
$C_D^b$	0.0636
$C_L^b$	0.122
$C_{D0}^f$	0.173
$C_{D{\delta }_s}$	1.086
$C_{L0}^f$	0.502
$C_{L{\delta }_s}$	0.892
m s	2e4 kg
J s	6.4e6 kg·m2
$S_{ref}^s$	50 m2
r T	20 m
$C_D^s$	1
$C_n^s$	0.4
${\delta }_{\max }^p$	10°
σ max	10°
T max	4e4 N
${\delta }_{\max }^s$	30°
v max	25 m/s
${\dot{\delta }}_{a\hspace{0.17em}\max }$	10°/s
${\dot{\delta }}_{s\hspace{0.17em}\max }$	10°/s
${\dot{T}}_{\max }$	3e4 N/s
${\dot{\delta }}_{T\hspace{0.17em}\max }$	25°/s
z f	20 m
Δ r	3 m
Δ ψ	15°
Δ α	30°

Reachable boundary estimation results

In this section, the effectiveness of reachable boundary estimation is validated, and the approximation error is analyzed. As is depicted in Figure 5, reachable boundaries of the parafoil system and the vessel are approximated by circles, each of which is determined by three boundary points computed in Section 3. A series of optimization problems similar to Problem P2 and Problem P3 are solved to sample the reachable boundary. Figure 5 shows that the obtained boundary points are contained in the estimated circle. The turning radius in Figure 5(b) is smaller than that in Figure 5(a) because the vessel has more turning maneuverability than the parafoil system.

Figure 5.

Comparison of estimated circle and sampled boundary points (altitude 3000 m). (a) Parafoil and (b) vessel.

To demonstrate the accuracy of the estimated reachable boundary, the distance error between the approximated circle and the sampled boundary points is summarized in Table 2. Four initial altitudes of the parafoil system are considered, corresponding to different flight time. The mean error ɛ _mean is obtained by averaging the distance error at all boundary points, and the relative error ɛ _relative is the ratio of ɛ _mean to the radius of estimated circle r _estimated (i.e., ɛ _relative = ɛ _mean /r _estimated × 100%). As shown in Table 2, the mean error and relative error of the parafoil are below 200 m and 4%, and that of the vessel are below 50 m and 0.5%. With a higher initial altitude, the relative error can be further reduced. Note that the error is caused by an over-optimistic estimate, which means the approximated circle contains some unreachable area. Since the feasibility judgment algorithm is utilized to perform a preliminary judgment, it is acceptable to approximate the reachable boundary by a circle.

Table 2.

Estimation error at different altitudes.

Altitude (m)	Parafoil error		Vessel error
	ɛ mean (m)	ɛ relative (%)	ɛ mean (m)	ɛ relative (%)
1500	144.06	3.65	25.94	0.41
2000	152.99	2.86	30.59	0.36
2500	162.92	2.42	30.61	0.29
3000	173.82	2.14	25.56	0.21

Trajectory optimization results

This section demonstrates the trajectory optimization framework via four mission cases. The initial heading angle, velocity, and angular rate of the vessel are set 0°, 10 m/s, and 0°/s in all cases. The initial conditions of two parafoil systems in different cases are given in Table 3.

Table 3.

Parameters of four collaborative recovery cases.

Case	Initial state
1	$\begin{array}{l}{X}_{init}^{p\left(1\right)}={\left[-10000\mathrm{m},2000\mathrm{m},2500\mathrm{m},18.93\mathrm{m}/\mathrm{s},-19.59°,60°,0°\right]}^{\mathrm{T}}\\ X_{init}^{p\left(2\right)}={\left[9000\mathrm{m},-2000\mathrm{m},3000\mathrm{m},19.59\mathrm{m}/\mathrm{s},-19.58°,-30°,0°\right]}^{\mathrm{T}}\end{array}$
2	$\begin{array}{l}{X}_{init}^{p\left(1\right)}={\left[-1000\mathrm{m},10000\mathrm{m},1800\mathrm{m},18.04\mathrm{m}/\mathrm{s},-19.59°,15°,0°\right]}^{\mathrm{T}}\\ X_{init}^{p\left(2\right)}={\left[5000\mathrm{m},300\mathrm{m},2100\mathrm{m},18.41\mathrm{m}/\mathrm{s},-19.58°,-45°,0°\right]}^{\mathrm{T}}\end{array}$
3	$\begin{array}{l}{X}_{init}^{p\left(1\right)}={\left[-6000\mathrm{m},4000\mathrm{m},1500\mathrm{m},17.67\mathrm{m}/\mathrm{s},-19.59°,30°,0°\right]}^{\mathrm{T}}\\ X_{init}^{p\left(2\right)}={\left[1500\mathrm{m},-1500\mathrm{m},2000\mathrm{m},18.29\mathrm{m}/\mathrm{s},-19.59°,0°,0°\right]}^{\mathrm{T}}\end{array}$
4	$\begin{array}{l}{X}_{init}^{p\left(1\right)}={\left[-2000\mathrm{m},2000\mathrm{m},1500\mathrm{m},17.67\mathrm{m}/\mathrm{s},-19.59°,30°,0°\right]}^{\mathrm{T}}\\ X_{init}^{p\left(2\right)}={\left[3000\mathrm{m},3000\mathrm{m},3000\mathrm{m},19.59\mathrm{m}/\mathrm{s},-19.58°,-30°,0°\right]}^{\mathrm{T}}\end{array}$

The reachable boundary estimations of four cases are depicted in Figure 6. The circles represent the estimated reachable boundary for each vehicle. The start and end points are marked by the solid square and the solid circle, and the initial heading angle is represented by the arrow. According to the feasibility judgment algorithm, the vessel cannot recover both parafoil systems in Case 1 due to the significant initial distance. As for the other three cases, the feasibility judgment algorithm determines that it is possible to conduct collaborative recovery and provides the guess of landing point. Then, the trajectory optimization is performed, and the obtained flight trajectories are presented in Figure 6. Specifically, in Case 2, the conditions ( $C_1{O}^s⩽r^{s-\left(1\right)}$ and $C_1{O}^{p\left(2\right)}⩽r^{p\left(2\right)}+d_m$ ) in the feasibility judgment algorithm are satisfied, which indicates the possibility of collaborative recovery. However, as shown in Figure 6(b), the vessel fails to recover both parafoil systems. This is because the feasibility judgment algorithm provides an optimistic estimate and the ultimate recovery feasibility remains to be determined in the trajectory optimization procedure. From Figure 6(c) and (d) it can be observed that the collaborative recovery is successfully performed in Cases 3–4.

Figure 6.

Collaborative recovery results of four cases. (a) Case 1, (b) case 2, (c) case 3, and (d) case 4.

Figures 7–9 depict the results of Case 4, where Parafoil 1 and Parafoil 2 are captured at 261.85 s and 500.07 s, respectively. The objective of recovering two parafoil systems is achieved successfully. Parafoil 1 makes a left turn maneuver and Parafoil 2 makes a right turn to approach from aft of the vessel and performs an upwind landing. The vessel accelerates to catch Parafoil 1 and then maneuvers an S-turn to recover Parafoil 2. Compared with Case 3, the initial distance between two parafoil systems decreases from 9.3 km to 5.1 km, and the capture time interval increases from 83.58 s to 240.26 s. Because of these favorable conditions, the vehicles accomplish the collaborative recovery with a smaller movement. This indicates that it is necessary to keep the two parafoil systems close in the horizontal plane and maximize the time interval between captures in the future recovery mission.

Figure 7.

Trajectory results of case 4. (a) 3-D and (b) X-Y plane.

Figure 8.

State and control profiles of parafoil (case 4).

Figure 9.

State and control profiles of vessel (case 4).

Monte-Carlo simulation

This section illustrates the convergence property and robustness of the proposed trajectory optimization method. The constant wind magnitude is randomly selected within $\left[\mathrm{0,5}\right]\mathrm{m}/\mathrm{s}$ . The starting conditions of two parafoil systems are perturbed, and their uniform distribution ranges are summarized in Table 4. We denote separate planning for two parafoil systems as Method 1 (which is also the decoupled stage proposed in Section 4.1), the direct application of finite-element collocation method as Method 2, and the overall framework proposed in this work as Method 3 (as in Figure 4). A 500-trial Monte-Carlo simulation is performed, and the results of three algorithms are summarized in Table 5.

Table 4.

Dispersion of initial states.

State	Range
$x_{init}^{p\left(1\right)}$	$\left[-3000,-1000\right]\mathrm{m}$
$z_{init}^{p\left(1\right)}$	$\left[\mathrm{1200,1800}\right]\mathrm{m}$
${\gamma }_{init}^{p\left(1\right)}$	$\left[-22,-16\right]°$
$x_{init}^{p\left(2\right)}$	$\left[\mathrm{2000,4000}\right]\mathrm{m}$
$z_{init}^{p\left(2\right)}$	$\left[\mathrm{2600,3400}\right]\mathrm{m}$
${\gamma }_{init}^{p\left(2\right)}$	$\left[-24,-20\right]°$
$y_{init}^{p\left(1\right)}$	$\left[\mathrm{1000,3000}\right]\mathrm{m}$
$v_{init}^{p\left(1\right)}$	$\left[\mathrm{14,18}\right]\mathrm{m}/\mathrm{s}$
${\psi }_{init}^{p\left(1\right)}$	$\left[\mathrm{30,90}\right]°$
$y_{init}^{p\left(2\right)}$	$\left[\mathrm{2000,4000}\right]\mathrm{m}$
$v_{init}^{p\left(2\right)}$	$\left[\mathrm{18,22}\right]\mathrm{m}/\mathrm{s}$
${\psi }_{init}^{p\left(2\right)}$	$\left[-\mathrm{60,0}\right]°$

Table 5.

Algorithm comparison results.

	Method 1	Method 2	Method 3
Cost function	70.01	379.30	25.27
Success rate	69.80%	3.80%	84.20%

From Table 5, Method 1 successfully solved 69.8% of trials, while Method 2 only tackles 3.8% and obtains a larger cost function than Method 1. This means that Method 2 either fails to converge or gets stuck in the local optimum. In contrast, Method 3 excludes 73 infeasible trials via the feasibility judgment algorithm and successfully solves 421 trials of the remaining 427 trials. Consequently, Method 3 achieves a success rate of 84.2% (421/500) and obtains the smallest cost function. The developed algorithm (Method 3), Method 1, and Method 2 all witnessed failure cases. For Method 3, this is because infeasible cases are recognized by the proposed framework; For Method 1 and Method 2, this is because the IPOPT solver is stuck at local infeasible or suboptimal points despite the fact that some of the cases are feasible. Compared with Method 1 and Method 2, the proposed trajectory optimization framework has a higher success rate in solving the collaborative recovery problem. It validates the improved robustness of the proposed framework in improving solution convergence and solution optimality under different constant wind conditions and initial state conditions.

The trajectory optimization results of Method 3 are presented in Figure 10. For the purpose of illustration, only 100 successful trials are plotted. In most of the trials, the vehicles perform a similar maneuver to realize collaborative recovery. Only in a small number of trials, the vessel or parafoil system makes a huge turn to realize landing in the same direction. The trajectory results demonstrate that the optimization framework proposed in this paper is robust to the dispersion of initial states.

Figure 10.

Trajectory results of Monte-Carlo simulation.

Conclusion

This paper proposes a trajectory optimization framework for collaboratively recovering two parafoil systems using an unmanned vessel. A trajectory optimization problem that characterizes vehicle dynamics and related constraints is established. The reachable boundaries of vehicles are estimated as circles, based on which the feasibility algorithm is designed. For solving the collaborative recovery problem, the decoupled-then-simultaneous strategy is designed, where the decoupled stage generates an initial trajectory guess and the simultaneous stage obtains the optimal trajectory. All problems are discretized by the finite-element collocation method and solved as an NLP problem. Simulation results show that estimating the reachable boundary as a circle is appropriate. The proposed trajectory optimization framework effectively solves the collaborative recovery problem and generates the trajectory. Through Monte-Carlo simulation, the robustness of the proposed framework to initial state deviation is validated. Comparing with the direct application of the finite-element collocation method, the proposed framework has better convergence property and solution optimality. In our future work, the method proposed in this paper will be extended to multi-vessels and multi-parafoils, thus expanding the scenario from PLF recovery to drone swarm recycling and other applications.