Trajectory optimization of tail-sitter considering speed constraints

Abstract

Tail-sitters, with the advantages of both the fixed-wing unmanned aerial vehicles (UAVs) and vertical take-off and landing UAVs, have been widely designed and researched in recent years. With the change in modern UAV application scenarios, it is required that UAVs have fast maneuverable three-dimensional flight capabilities. Due to the highly nonlinear aerodynamics produced by the fuselage and wings of the tail-sitter, how to quickly generate a smooth and executable trajectory is a problem that needs to be solved urgently. We constrain the speed of the tail-sitter, eliminate the differential dynamics constraints in the trajectory generation process of the tail-sitter through differential flatness, and allocate the time variable of the trajectory through the state-of-the-art trajectory generation method named MINCO. By discretizing the trajectory in time, we convert the speed constraint on the vehicle into a soft constraint, thereby achieving the time-optimal trajectory for the tail-sitter to fly through any given waypoints.

1 Introduction

Compared with fixed-wing unmanned aerial vehicles (UAVs), vertical take-off and landing (VTOL) UAVs do not require a runway for takeoff and landing, and have the ability to hover in the air. They have been applied in various fields such as search and rescue [1], dam inspection [2], payload transportation [3], and agricultural assistance [4]. Common types of VTOL aircraft include quadcopters and helicopters.

Hybrid UAVs aim to retain the advantages of vertical take-off and landing UAVs while incorporating the strengths of fixed-wing UAVs—that is, they offer the ability to perform vertical take-offs and hover, with a long-duration flight capabilities and a larger payload capacity. Common types of hybrid UAVs include tilt-rotor [5, 6], tilt-wing [7], and dual-system [8, 9]. Among these, the tail-sitters have its rotors rigidly fixed to the wing, achieving the transition between vertical take-off/landing and cruising flight by tilting the entire body. This design offers simplicity in mechanics and ease of installation—qualities that are particularly important for small, low-cost, portable UAVs. Based on these advantages, an increasing number of institutions and researchers have taken an interest in tail-sitters, leading to the development of single-propeller [10, 11, 12], dual-propeller [13, 14, 15], quad-propeller [16, 17], even penta-propeller [18] tail-sitters.

Traditional applications of UAVs often operate in high-altitude areas. At a certain height, a UAV is considered as a uniformly moving two-dimensional point mass, so a combined navigation positioning system based on Global Positioning System (GPS) and Inertial Measurement Unit (IMU) is sufficient for the UAV to successfully complete preset tasks. However, with the changing scenarios of modern UAV applications, UAVs are increasingly required to fly in spatially constrained areas such as building clusters, forests, and even indoors, necessitating UAVs to possess swift maneuverability in three-dimensional flight. For tail-sitters, due to the aerodynamic forces generated by the fuselage and wings during flight, generating a smooth, efficient, and executable trajectory is an urgent problem.

Generating high-quality trajectories for tail-sitters presents four major algorithmic challenges. Firstly, under constraints of size, weight, and power, there are stringent requirements for real-time trajectory calculation using limited onboard resources. Secondly, to achieve full-envelope flight of the tail-sitter, trajectories need to be discretized in time, and the generation of these trajectories must consider the aerodynamics of the tail-sitter. Thirdly, reliable trajectories require acceptable states and inputs, so the trajectory cannot merely be smooth in terms of geometric shape (such as B-splines and Bezier curves), the trajectory was required to satisfy dynamic constraints. Fourthly, due to the high nonlinearity of the aerodynamics of the tail-sitter’s fuselage and wings, it’s a challenge to ensure that the generated trajectory does not cause the tail-sitter to experience unacceptable angle of attack during flight.

Our contributions are outlined as follows:

1.Based on differential flatness, we have designed a trajectory generation method for tail-sitters that precisely considers the three-dimensional dynamic model. This generates the optimal executable trajectory for the tail-sitter, considering flight time, actuator constraints, and speed constraints.

2.We propose an optimization-based multistage trajectory generation method. This allows the tail-sitter to start from a hovering state and generate optimal trajectories that pass through any given three-dimensional intermediate waypoints. To the best of our knowledge, this is the first time that this method has been proposed for tail-sitters.

2 Related work

There are currently two approaches to trajectory generation for tail-sitters.

The first approach treats the tail-sitter as a quadrotors during low-speed vertical flight, and as a fixed-wing aircraft during horizontal flight, separately applying mature trajectory generation methods for quadrotors and horizontal flight of fixed-wing aircraft. For example, Mellinger [19] was the first to verify that the parallel-axis quadrotors are differentially flat, enabling the quadrotors to generate trajectories passing through specified keyframes starting from the hovering state. Mueller [20] treated the trajectory planning between two points of the quadrotor as an Optimal Boundary Value Problem (OBVP), solving it using Pontryagin’s minimum principle. Both of these methods are directly applicable to tail-sitters during low-speed vertical flight. Similarly, for horizontal flight, where the tail-sitter performs two-dimensional planar motion, the well-known Dubins path [21] or Dubins-Polynomials trajectory [22] considering control constraints, are applicable.

However, this brings about new challenges. Due to the large area of the tail-sitter’s fuselage and wings exposed to the air, the transition between low-speed vertical flight and horizontal flight is extremely challenging. Verling [23] accomplished the transition between hovering and horizontal flight by linearly increasing or decreasing the pitch angle, without considering the influence of the nonlinear aerodynamics generated by the tail-sitter during the transition on the pitch angle. Lyu [24] designed a controller that can achieve the transition between hovering and horizontal flight with minimal changes in altitude, again without considering the aerodynamic forces generated during the transition, thus requiring a large amount of trial and error in experiments. To incorporate the aerodynamics of the tail-sitter during the transition phase, the transition phase problem is usually treated as a nonlinear optimization problem. Since the transition generally occurs on the sagittal plane of the fuselage, Kita [25] modeled the aerodynamics and thrust on the sagittal plane of the tail-sitter, calculated the reference trajectory of the pitch angle offline, and achieved the transition in the shortest time while limiting altitude changes. Oosedo [26] did similar work for the same purpose. Naldi and Marconi [27] numerically solved the problem considering both minimum time and minimum energy during the transition phase. Li [28] optimized the energy of the tail-sitter during the forward flight transition, obtaining the trajectory of the pitch angle. Furthermore, McIntosh and Mishra [29] generated obstacle avoidance trajectories during the transition phase considering wake effects on the aerodynamic forces, but this trajectory is two-dimensional and simplistically considers obstacles as circles.

To sum up, from a computational perspective, existing transition methods have issues such as large errors, inaccurate models, and high computational costs, making online planning difficult to achieve. From a practical application perspective, the transition phase only occurs in one-dimensional or two-dimensional Euclidean space, which is unable to achieve rapid and agile flight.

In under-actuated systems, differential flatness is a very important property. This property, which applies to dynamical systems described by ordinary differential equations, was first proposed by Fliess [30]. Utilizing differential flatness transformations, the system’s reference states and reference inputs can be directly determined by the transient information of the flat output trajectory, without the need for integrating the differential equations of the dynamical system. The differential flatness property of quadrotors was first proposed by Mellinger [19] and has been repeatedly proven to be an effective method for trajectory planning in the development of rotorcraft over the past decade [31, 32, 33, 34, 35, 36].

Recently, Tal [37] and Lu [38] have proposed tail-sitter trajectory planning based on differential flatness.

Tal [37] proposed the differential flatness property of the tail-sitter based on the $\phi$-theory aerodynamic model [39], where the position of the vehicle and the Euler yaw angle were chosen as flat outputs. By solving the minimum snap trajectory in flat space, they achieved maneuverable flight of the tail-sitter [40]. However, Tal’s work still has shortcomings. First, the $\phi$-theory model approximates the differential motion equations model with polynomials, and its error will reduce the quality of the trajectory and control performance. Second, the $\phi$-theory model assumes no wind conditions, considering only the attitude and speed of the aircraft in the inertial frame, rather than the aerodynamic angles and airspeed. Third, Tal’s method assumes that the aircraft will not be subjected to lateral forces, so the fuselage is hollowed out, which is not applicable to the general tail-sitter model. Compared with the $\phi$-theory model, Lu [38] considered the real, complete 3D model of the tail-sitter without any simplifications, demonstrated the differential flatness property of the precise aerodynamic model and more general tail-sitter fuselage, and enabled the tail-sitter to perform maneuvers such as the Cuban eight at high altitudes. It’s worth mentioning that while Tal hollowed out the fuselage to make the aircraft not consider the lateral forces it is subjected to, Lu forced the tail-sitter to perform coordinated flight, i.e., no sideslip, leading to the same effect of avoiding lateral forces as Tal’s. Due to the highly nonlinear aerodynamics, lateral forces make the solution of the vehicle’s attitude and control very complex. Although Zhou [41] solved this highly nonlinear constraint using numerical methods, this method is still not suitable for real-time computation and is something we strive to avoid.

On the trajectory optimization level, neither Lu nor Tal have delved deeply. Trajectory optimization has a long history in control literature [42]. These types of problems come in various forms, but overall they optimize a series of inputs to dynamic systems, subject to constraints including but not limited to motion differential equations, obstacles, and control inputs. Many existing solvers can handle trajectory optimization problems with a general structure and obtain high-quality solutions, such as GPOPS-II [43] based on the pseudospectral method and ACADO [44] based on the shooting method. These solvers typically discretize trajectory optimization problems into nonlinear programming problems with a large number of optimization variables and equality constraints through direct or indirect methods, and then resort to some high-performance general nonlinear programming solvers, such as SNOPT [45] and IPOPT [46]. However, in practical applications, trajectory planning often has constraints that are difficult to describe explicitly, non-smooth constraints, integer variables, and so on [47]. Moreover, general solvers are usually affected by computational efficiency. The author has tried to use SNOPT in GPOPS-II to solve the two-dimensional trajectory of a tail-sitter with a 6-D state and 2-D input, from a stationary takeoff from the ground to a certain position to enter level flight, and it took more than 10 minutes.

Mellinger [19] use fixed-duration splines to characterize the flat trajectories of quadrotors, integrating the square norm of the fourth-order time derivative as a cost function for quadratic programming to ensure the smoothness of the trajectory. This scheme fixes the total time of the trajectory and then allocates time to each stage of the trajectory, lacking effective optimization for the entire trajectory. Additionally, this scheme only supports simplified dynamic constraints. Bry [22] eliminates equality constraints by using boundary derivative transformations, thus solving an unconstrained quadratic programming problem and obtaining a closed-form solution. However, the efficiency of solving sparse linear equation systems is questionable when the problem scale is large. Furthermore, the vehicle’s flight speed and other dynamic constraints cannot be explicitly incorporated into the trajectory generation. Burke [48] solves the primal-dual variable of quadratic programming problems with linear complexity, but the advantage of their algorithm only becomes apparent when there are many trajectory stages. Wang [49] provide an analytical inverse of the boundary derivative transformation, along with the analytical gradient of parameters, but this analytical inverse is only applicable to situations with only one start and one end point and cannot solve multistage trajectory.

In summary, many state-of-the-art UAVs trajectory planning methods in academia are to solve weakened problems of basic problems, either sacrificing time optimization, sacrificing constraint fidelity, or using heuristic approximate solutions, to exchange for the computational efficiency of online planning. However, the quality of the trajectory solution is far from the truth value.

3 Flight Dynamics

This section introduces the dynamic model of the tail-sitter, which forms the basis of our trajectory generation algorithm. Section 3.1 introduces the tail-sitter we use, and in section 3.2 we define the inertial coordinate system and body coordinate system we use. Section 3.3 presents the translational and rotational dynamics of the tail-sitter, and the aerodynamics that the tail-sitter is subjected to are shown in section 3.4.

3.1 Tail-sitter Model

The vehicle we use is the SWAN K1 PRO, developed by HEQ UAV Technical Company, located in Shenzhen, China. The vehicle has a total mass of 1.3328kg and a wingspan of 1.085m, as shown in Figure 1. As can be seen, the vehicle has no control surfaces, and all torques and forces are generated by the four propellers on the fuselage. Specifically, propellers 1 and 3 constantly rotate clockwise, while propellers 2 and 4 constantly rotate counterclockwise, balancing torques with each other.

Figure 1: SWAN K1 PRO

3.2 Coordinate Frames

As shown in Figure 2, the inertial frame $\left\{\mathbf{O}\mathbf{x}\mathbf{y}\mathbf{z}\right\}$ is defined as North-East-Down (NED), and correspondingly, the body frame $\left\{\mathbf{O}_{b}\mathbf{x}_{b}\mathbf{y}_{b}\mathbf{z}_{b}\right\}$ is defined as Front-Right-Down (FRD). Here, $O$ is the origin of the world coordinate system, and $O_{b}$ is the center of gravity of the vehicle.

Figure 2: coordinate frames and aerodynamic forces

3.3 Vehicle Equations of Motion

We consider the state of the tail-sitter $\mathbf{x}=\left\{\mathbf{p},\mathbf{v},\mathbf{R},\boldsymbol{\omega}\right\}$, where $\mathbf{p}\in\mathbb{R}^{3}$ and $\mathbf{v}\in\mathbb{R}^{3}$ are, respectively, the vehicle position and velocity in the world-fixed reference frame. $\mathbf{R}\in SO(3)$ denotes the rotation from the inertial frame to the body frame, $\boldsymbol{\omega}\in\mathbb{R}^{3}$ is the angular velocity of the vehicle. The input for the tail-sitter at the aspects of force and torque is $\mathbf{u}=\left\{f,\boldsymbol{\tau}\right\}$, where $f$ and $\boldsymbol{\tau}\in\mathbb{R}^{3}$ denote the thrust and control moment vector produced by 4 propellers, respectively. Hence, The vehicle translational and rotational dynamics are given by:

$$\dot{\mathbf{p}}=\mathbf{v}$$ (1a)

$$\dot{\mathbf{v}}=\mathbf{g}+\frac{1}{m}(f\mathbf{R}\mathbf{e}_{1}+\mathbf{R}\mathbf{f}_{a})$$ (1b)

$$\dot{\mathbf{R}}=\mathbf{R}\left\lfloor\boldsymbol{\omega}\right\rfloor$$ (1c)

$$\mathbf{J}\dot{\boldsymbol{\omega}}=\boldsymbol{\tau}+\mathbf{M}_{a}-\boldsymbol{\omega}\times\mathbf{J}\boldsymbol{\omega}$$ (1d)

where $\mathbf{g}=(0,0,9.8)^{T}$ is the gravitational acceleration, and $m$ is the vehicle mass. $\mathbf{e}_{1}=(1,0,0)^{T}$, $\mathbf{e}_{2}=(0,1,0)^{T}$, $\mathbf{e}_{3}=(0,0,1)^{T}$, are unit vectors. $\mathbf{f}_{a}\in\mathbb{R}^{3}$ and $\mathbf{M}_{a}\in\mathbb{R}^{3}$ are the aerodynamics force and moment in the body frame, which will be introduced in Section 3.4. $\mathbf{J}\in\mathbb{R}^{3\times 3}$ is the inertia tensor matrix of the vehicle about the body frame with the center of gravity as the origin. The notation $\left\lfloor\cdot\right\rfloor$ converts a 3-D vector into a skew-symmetric matrix.

It should be note that we assume that the thrust direction is aligned to the $\mathbf{x}_{b}$. For cases where propellers have a fixed installation angle, it just needs to be transformed by a constant matrix.

3.4 Aerodynamics

As Lu [38] has done, we require the tail-sitter to perform coordinated flight. The coordinated flight is flight without side-slip, that is, as shown in Figure 2, $\beta=0$. This type of flight always places the air speed experienced by the UAV in the $\mathbf{x}_{b}\mathbf{O}_{b}\mathbf{z}_{b}$ plane of the body frame, but does not limit the degrees of freedom of the tail-sitter in space. By applying centripetal force through roll to turn, the tail-sitter can still reach any position. Compared with uncoordinated flight, coordinated flight not only achieves the maximum aerodynamic efficiency, but also minimizes the potential adverse aerodynamic torques [50], thus greatly reducing the computational load of the trajectory generation algorithm.

Therefore, the aerodynamic force $\mathbf{f}_{a}$ is modeled in the body frame as follows:

$$\mathbf{f}_{a}=\left[\begin{array}[]{ccc}-\cos\alpha&0&\sin\alpha\\ 0&1&0\\ -\sin\alpha&0&-\cos\alpha\end{array}\right]\left[\begin{array}[]{l}\mathcal{D}\\ \mathcal{Y}\\ \mathcal{L}\end{array}\right]$$ (8)

where $\alpha$ is the angle of attack. $\mathcal{L}$, $\mathcal{D}$, $\mathcal{Y}$ are respectively the lift, drag, and side force produced by fuselage and wings. The aerodynamic moment vector $\mathbf{M}_{a}$ consists of rolling $L$, pitching $M$, and yawing $N$ moment along the body axis $\mathbf{x}_{b}$, $\mathbf{y}_{b}$, $\mathbf{z}_{b}$. :

$$\mathbf{M}_{a}=(L,M,N)^{T}$$ (9)

Referring to Etkin and Reid [51], the aerodynamic forces and moments are further parameterized as:

$$\begin{split}\mathcal{L}&=\frac{1}{2}\rho V^{2}SC_{L}\\ \mathcal{D}&=\frac{1}{2}\rho V^{2}SC_{D}\\ \mathcal{Y}&=\frac{1}{2}\rho V^{2}SC_{Y}\\ L&=\frac{1}{2}\rho V^{2}S\bar{c}C_{l}\\ M&=\frac{1}{2}\rho V^{2}S\bar{c}C_{m}\\ N&=\frac{1}{2}\rho V^{2}S\bar{c}C_{n}\end{split}$$ (10)

where $V=||\mathbf{v}_{a}||$ is the norm of the air speed, $\rho$ is the density of the air, $S$ is the reference area of the wing, $\bar{c}$ is the mean aerodynamic chord. Note that $\mathbf{v}_{a}=\mathbf{v}-\mathbf{w}$, where $\mathbf{w}$ is wind velocity. $C_{L}$, $C_{D}$, and $C_{Y}$ are the lift coefficient, drag coefficient, and side force coefficients of the vehicle, respectively, while $C_{l}$, $C_{m}$, and $C_{n}$ are the rolling, pitching, and yawing moment coefficients, respectively. They are each a set of dimensionless numbers, the values of which are solely related to the aircraft’s shape and attitude.

The real vehicle mentioned in Section 3.1 was scanned and reconstructed in three dimensions, and the resulting geometric model was input into Ansys Fluent to calculate the aerodynamic coefficients of the vehicle at different angles of attack. The air speed was set to $8m/s$, the y-plus value was set to $1$, the boundary layer was set to $5$ layers, the boundary layer growth rate was set to $1.15$, the turbulence model we selected was the commonly used Spalart-Allmaras model for aircraft, and the pressure-velocity coupling algorithm we selected was SIMPLEC. The experiment found that when the number of grids reached $4.57$ million, there was no significant change in the results. Figure 3 shows the air speed over the surface of the aircraft when the angle of attack is $10$.

Figure 3: The air speed over the surface of the tail-sitter when the angle of attack is 10

Due to the absence of side-slip and the fact that the aircraft is symmetrical about plane $\mathbf{x}_{b}\mathbf{O}_{b}\mathbf{z}_{b}$, the actual aerodynamic forces acting on the vehicle are only lift, drag, and pitching moment. We measure the angle of attack every 10 degrees and fit the results obtained with a carefully selected polynomial function. The final results are as shown in the Figure 4.

((a))

((b))

((c))

Figure 4: The results obtained from Fluent

It should be noted that we assume the aerodynamic forces are not affected by the propeller slipstream, which is generally true for quad-rotor tail-sitters, as the propellers are mounted on rack on either side of the wing, far away from the fuselage.

4 Multistage trajectory optimization

4.1 Problem Formulation

We translate the trajectory planning problem as an multi-objective optimization problem. We require the trajectory to minimize snap, i.e., the fourth derivative of position, and total time, while satisfying the maximum speed constraint of the tail-sitter and passing through intermediate waypoints. If we assume that there are $M-1$ intermediate waypoints, so it can be known that the total trajectory is divided into $M$ segments. We use $T_{i}$ to represent the duration of the $i$th piece of the total trajectory.

The multi-objective optimization problem is formulated as follows:

$$\min\int_{0}^{\sum_{1}^{M}T_{i}}\mathbf{p}^{(4)}(t)^{T}\mathbf{p}^{(4)}(t)dt$$ (11a)

$$\min\sum_{1}^{M}T_{i}$$ (11b)

$$\mathrm{s.t.}\ t\in[0,\sum_{1}^{M}T_{i}]$$ (11c)

$$\left\|\mathbf{v}(t)\right\|<V_{max}$$ (11d)

$$\mathbf{p}^{[3]}(0)=\mathbf{s}_{0},\ \mathbf{p}^{[3]}(\sum_{1}^{M}T_{i})=\mathbf{s}_{f}$$ (11e)

$$\mathbf{p}(T_{i})=\mathbf{s}_{i},\ 1\leq i<M$$ (11f)

$$T_{i}>0,\ 1\leq i<M$$ (11g)

$$\dot{\mathbf{p}}=\mathbf{v}$$ (11h)

$$\dot{\mathbf{v}}=\mathbf{g}+a_{T}\mathbf{R}\mathbf{e}_{1}+\frac{1}{m}\mathbf{R}\mathbf{f}_{a}$$ (11i)

$$\dot{\mathbf{R}}=\mathbf{R}\left\lfloor\boldsymbol{\omega}\right\rfloor$$ (11j)

$$\mathbf{J}\dot{\boldsymbol{\omega}}=\boldsymbol{\tau}+\mathbf{M}_{a}-\boldsymbol{\omega}\times\mathbf{J}\boldsymbol{\omega}$$ (11k)

where $\mathbf{p}^{(x)}(t)$ means the $x$-th derivative of position, $\mathbf{s}_{0}$, $\mathbf{s}_{f}$, and$\mathbf{s}_{i}$ are all user-defined, $\mathbf{p}^{[x]}\in\mathbb{R}^{(x+1)\times 3}$ defined by

$$\mathbf{p}^{[x]}=(\mathbf{p},\dot{\mathbf{p}},\ldots,\mathbf{p}^{(x)})^{T}$$ (12)

Let’s briefly explain equation (11). First, it is clear that equations (11h)-(11k) are equivalent to equations (1a)-(1d), which means that our trajectory must satisfy the dynamics constraints of the tail-sitter. Second, in practice, minimizing snap roughly corresponds to reducing the required control moment and thus increasing the likelihood that the control input limits are satisfied and the trajectory is feasible. This is why we choose to minimize the integral of snap. Finally, from equation (10), it can be known that the aerodynamic forces produced by the tail-sitter’s fuselage and wings are highly related to speed. If there is no constraint on the tail-sitter’s speed, the generated trajectory could cause the tail-sitter to have an unacceptable angle of attack, leading to the tail-sitter’s inputs being unexecutable or even causing the tail-sitter to crash directly. This is why we must keep the tail-sitter’s speed below a certain value.

4.2 Differential Flatness

We eliminate the dynamics constraints (11h)-(11k) by differential flatness.

Consider the following type of the dynamical system:

$$\dot{\mathbf{x}}=f(\mathbf{x})+g(\mathbf{x})\mathbf{u}$$ (13)

with state $\mathbf{x}\in\mathbb{R}^{n}$, and input $\mathbf{u}\in\mathbb{R}^{m}$. The map $g$ is assumed to have rank $m$. If there exists a flat output $\mathbf{y}$ such that $\mathbf{x}$ and $\mathbf{u}$ can be represented by finite order derivatives of $\mathbf{y}\in\mathbb{R}^{m}$:

$$\dot{\mathbf{x}}=\mathfrak{A}(\mathbf{y},\dot{\mathbf{y}},\ldots,\mathbf{y}^{(k)})$$ (14)

$$\dot{\mathbf{u}}=\mathfrak{B}(\mathbf{y},\dot{\mathbf{y}},\ldots,\mathbf{y}^{(j)})$$ (15)

then the system is said to be differentially flat [30]. $\mathfrak{A}$ and $\mathfrak{B}$ each represent a set of differential flat transformations, determined by $f$ and $g$. $k$ and $j$ are both natural numbers.

Fortunately, for dynamical systems like those described in equation (1), and states and inputs defined as in equation (2), their differential flat transformations have been thoroughly proven by Lu [38]. In the case of coordinated flight of tail-sitters, the flat output $\mathbf{y}$ is the position of the vehicle. Therefore, trajectory optimization can be carried out in the flat output space $\mathbf{y}$, and the continuity order in the corresponding flat output space can ensure that the dynamical differential constraints are precisely satisfied.

4.3 Minimum Snap Trajectory Generation

We define time vector $\mathbf{T}$ as

$$\mathbf{T}=(T_{1},T_{2},\ldots,T_{M})^{T}$$ (16)

Equation (11a) is a linear quadratic optimization problem, which ensures the generation of the smoothest possible trajectory from determined temporal parameters $\mathbf{T}$ and spacial parameters $\mathbf{p}_{i}$. Although linear quadratic optimization problems have been widely studied and applied, previous works typically confines the problem to a single stage, considering only one set of boundary conditions and trajectory generation problems under various cost functions [20, 52]. Wang [47] proved that for a multistage snap minimization problem, the position of the vehicle can be represented by a set of seventh-degree polynomials, without the need for computation of the cost function (11a) itself or its gradient.Specifically, for the trajectory optimization problem in the following form:

$$\min\int_{0}^{\sum_{1}^{M}T_{i}}\mathbf{p}^{(4)}(t)^{T}\mathbf{p}^{(4)}(t)dt$$ (17a)

$$\mathrm{s.t.}\ t\in[0,\sum_{1}^{M}T_{i}]$$ (17b)

$$\mathbf{p}^{[3]}(0)=\mathbf{s}_{0},\ \mathbf{p}^{[3]}(\sum_{1}^{M}T_{i})=\mathbf{s}_{f}$$ (17c)

$$\mathbf{p}(T_{i})=\mathbf{s}_{i},\ 1\leq i<M$$ (17d)

$$T_{i}>0,\ 1\leq i<M$$ (17e)

and if the initial and final conditions, i.e., $\mathbf{s}_{0}$ and $\mathbf{s}_{f}$, and the intermediate waypoints, i.e., $\mathbf{s}_{i}$, are given, and the time parameter $\mathbf{T}$ of the trajectory is also specified, then the entire trajectory exists and can be uniquely determined. Moreover, the trajectory is composed of $M$ seventh-degree polynomials.

Therefore, the expression for the $i$th segment of the trajectory is denoted by:

$$\mathbf{p}_{i}(t)=\mathbf{c}_{i}^{T}\rho(t),t\in[0,T_{i}]$$ (18)

where $\mathbf{c}_{i}\in\mathbb{R}^{8\times 3}$ are the coefficients of seventh-degree polynomials, $\rho(x)=(1,x,x^{2},\ldots,x^{7})^{T}$. We use relative time for each segment of the trajectory, i.e., the starting time for each segment is 0. The state of the vehicle at time $T_{i}$ is identical to that at the beginning of the next segment. Hence, the trajectory could be described by a coefficient matrix $\mathbf{c}\in\mathbb{R}^{8M\times 3}$ defined by:

$$\mathbf{c}=(\mathbf{c}_{1},\mathbf{c}_{2},\ldots,\mathbf{c}_{M})^{T}$$ (19)

Define $\mathbf{H}_{0}$, $\mathbf{G}_{M}\in\mathbb{R}^{4\times 8}$ as:

$$\mathbf{H}_{0}=(\rho(0),\dot{\rho}(0),\ldots,\rho^{(3)}(0))^{T}$$ (20)

$$\mathbf{G}_{M}=(\rho(T_{M}),\dot{\rho}(T_{M}),\ldots,\rho^{(3)}(T_{M}))^{T}$$ (21)

Define $\mathbf{H}_{i}$, $\mathbf{G}_{i}\in\mathbb{R}^{8\times 8}$ as:

$$\mathbf{H}_{i}=(\mathbf{0}^{8\times 1},-\rho(0),-\dot{\rho}(0),\ldots,-\rho^{(6)}(0))^{T}$$ (22)

$$\mathbf{G}_{i}=(\rho(T_{i}),\rho(T_{i}),\dot{\rho}(T_{M}),\ldots,\rho^{(6)}(T_{M}))^{T}$$ (23)

In order to obtain the matrix $\mathbf{c}$, we establish a linear system:

$$\mathbf{A}\mathbf{c}=\mathbf{b}$$ (24)

where $\mathbf{A}\in\mathbb{R}^{8M\times 8M}$ and $\mathbf{b}\in\mathbb{R}^{8M\times 3}$ are:

$$\mathbf{A}=\begin{pmatrix}&\mathbf{H}_{0}&\mathbf{0}&\mathbf{0}&\ldots&\mathbf{0}\\ &\mathbf{G}_{1}&\mathbf{H}_{1}&\mathbf{0}&\ldots&\mathbf{0}\\ &\mathbf{0}&\mathbf{G}_{2}&\mathbf{H}_{2}&\ldots&\mathbf{0}\\ &\vdots&\vdots&\vdots&\ddots&\vdots\\ &\mathbf{0}&\mathbf{0}&\mathbf{0}&\ldots&\mathbf{H}_{M-1}\\ &\mathbf{0}&\mathbf{0}&\mathbf{0}&\ldots&\mathbf{G}_{M}\end{pmatrix}$$ (25)

$$\mathbf{b}=({\mathbf{p}_{0}^{[3]}}^{T},\mathbf{p}_{1},\mathbf{0}^{3\times 7},\ldots,\mathbf{p}_{M-1},\mathbf{0}^{3\times 7},{\mathbf{p}_{f}^{[3]}}^{T})^{T}$$ (26)

The matrix $\mathbf{A}$ which must be nonsingularity can be considered as a function of the time vector $\mathbf{T}$, and the matrix $\mathbf{b}$ can be viewed as a function of the spatial parameter $\mathbf{p}$. By solving equation (24), a minimum snap trajectory can be obtained.

Figure 5 illustrates the minimum snap trajectories under different temporal and spatial parameters. Compared to the trajectory shown in Figure 5(a), Figure 5(b) shows the result of changing the duration $T_{4}$ between waypoints $\mathbf{p}_{3}$ and $\mathbf{p}_{4}$ while kee** the waypoint positions constant. Figure 5(c) shows the result of altering the position of $\mathbf{p}_{3}$ while kee** the time used between waypoints constant. It can be observed that the speed of the vehicle is highly correlated with the spatial parameters $\mathbf{p}$ and the time parameters $\mathbf{T}$. Even a slight change in any element of these parameters not only affects the speed of the vehicle in the global trajectory but also causes a change in the shape of the entire trajectory.

((a))

((b))

((c))

Figure 5: minimum snap trajectory with 4 intermediate waypoints

4.4 Trajectory Optimization

4.4.1 Penalty Function of Continuous-time Constraints

The maximum speed constraint, i.e., equation (11d), is a soft constraint, as we allow minor exceedance over the maximum speed constraint we set on the trajectory. Setting the maximum speed constraint slightly lower than the actual maximum allowable speed is necessary in some cases. This point will be elaborated in detail in Section 4.4.3.

When the constraint have clear physical meanings and the requirements for precision are not high, the penalty functional method is a simple and effective method. Furthermore, penalty methods have no requirement on a feasible initial guess, which is nontrivial to construct.

Equation (11d) requires that the inequality is satisfied at any moment on the trajectory. However, since the trajectory position is composed of $M$ seventh-degree polynomials, this means that in order to satisfy the maximum speed constraint, we need to find the maximum value of $M$ different sixth-degree polynomials on different convex sets $M$ times, which is extremely challenging. We ensure the satisfaction of the continuous-time constraints at a certain resolution by adopting temporal discretization in the time domain. We define:

$$P(||\mathbf{v}||)=\sum_{i=1}^{M}\sum_{j=1}^{N}\max[V_{ij}-V_{max},0]^{3}$$ (27)

where $N$ controls the resolution of trajectory, $V_{ij}$ represents the speed at a sampled point on the trajectory. There are two aspects worth noting. First, the larger the value of $N$, the less likely it is for the trajectory to have a speed exceeding the constraint at any given moment. Second, the cubic form of the function forms a differentiable strictly convex penalty.

4.4.2 Temporal Constraint Elimination

It is evident that $\mathbf{T}\in\mathbb{R}_{>0}^{M}$, and equation (11g) is a hard constraint. This restrict the domain of $\mathbf{T}$ to simple manifolds, and optimization on the manifold frequently requires retractions. We satisfy this hard constraint by defining:

$$\mathbf{T}=e^{\mathbf{x}}$$ (28)

where $\mathbf{x}=(x_{1},x_{2},\ldots,x_{M})$. As a result, we can directly optimize the unconstrained surrogate variables $\mathbf{x}$ in Euclidean space.

It is worth noting that equation (11b) is the sum of linear functions with respect to each segment of trajectory time $T_{i}$, and is thus a convex function. The function $f(x)=e^{x}$ is also a convex function over its domain $x\in R$. Since the sum of convex functions is convex, setting $\mathbf{T}=e^{\mathbf{x}}$ does not alter the convexity of equation (11b).

4.4.3 Results

In summary, we propose to solve a lightweight relaxed optimization via unconstrained LP. The relaxation to (11) is defined as

$$\min\sum_{1}^{M}e^{x_{i}}+wP(||\mathbf{v}||)$$ (29)

where $w\in\mathbb{R}_{>0}$ is a weight number which should be a large constant. As can be seen, the original problem (11) has been transformed from a multi-objective optimization problem into an unconstrained optimal problem (29). According to Figure 5, given the waypoints, the speed of the vehicle is affected by the duration between two waypoints. Therefore, we choose $\mathbf{x}$ to become decision variable of expression (29).

As shown in Figure 5, changes to any element within the time parameter $\mathbf{T}$ will lead to alterations in the characteristics of the entire trajectory. This is where the difficulty lies in multistage trajectory optimization, that is, we require the gradient of equation (27) with respect to the time vector $\mathbf{T}$. With available gradient, the relaxation (29) could be solved by the limited-memory Broyden-Fletcher-Goldfarb-Shanno algorithm [53].

((a))

((b))

Figure 6: Two examples of trajectory optimization

Figure 6(a) shows the time-optimal trajectory of the tail-sitter ascending within a two-dimensional plane through given waypoints under certain speed constraints. Figure 6(b) shows the time-optimal trajectory of the tail-sitter through given waypoints in Euclidean space under certain speed constraints. The vehicle states and control quantities of the two trajectories are shown in Figures 7 and 8.

((a))

((b))

((c))

((d))

((e))

((f))

Figure 7: Details of the trajectory as shown in Figure 6(a)

((a))

((b))

((c))

((d))

((e))

((f))

Figure 8: Details of the trajectory as shown in Figure 6(b)

It should be noted that our algorithm allows the tail-sitter to pass through any waypoints, and Figures 6 are just a brief demonstration of the algorithm. The attitude of the tail-sitter is determined by its angle of attack, which is calculated from the vehicle’s speed and acceleration using Newton-Raphson method. This can sometimes lead to the solution converging to unreasonable values under certain circumstances. Since the control inputs of the tail-sitter are all calculated based on its attitude, an unreasonable angle of attack can lead to disastrous consequences. In practice, such situations often occur in the middle of the vehicle’s trajectory and when the tail-sitter is approaching its initial or final condition.

Figure 9(a) illustrates the unreasonable angle of attack at the initial and final condition on the trajectory as shown in Figure 6(a). As depicted in Figure 6(a), the tail-sitter moves in a straight line at the beginning and end stages, and it should not have a positive acceleration when the angle of attack is greater than 90 degrees. Figure 9(b) shows the unreasonable angle of attack calculated for the tail-sitter in the middle stages.

((a))

((b))

Figure 9: abnormal angle of attack

Given that Newton-Raphson method requires certain initial values, one solution to the situation shown in Figure 9(a) is to constantly manually adjust the initial values. However, this is troublesome and does not meet our need for real-time trajectory generation. Here, two solutions are proposed. Firstly, for the situation shown in Figure 9(a), it is usually due to the tail-sitter’s speed being too small, causing numerical instability in the solution. Therefore, a particularly small value can be set for the speed at the initial and final states, which usually solves the problem. Secondly, for the situation shown in Figure 9(b), it is usually because the acceleration required by the tail-sitter in a certain time interval is too large, causing the force generated by the tail-sitter’s fuselage and wings to be unable to meet the acceleration requirement. Therefore, this problem can be solved by appropriately reducing the value of the maximum speed constraint

Combining all elements above, the complete trajectory generation can be obtained as shown in Algorithm 1.

1 Given: Initial conditions $\mathbf{x}_{0}$, final conditions $\mathbf{x}_{f}$, waypoints $\mathbf{p}_{i}$.

2 L-BFGS:

3 Initialize $\mathbf{T}^{0}$, $\mathbf{g}^{0}\leftarrow\nabla h(\mathbf{T}^{0})$, $\mathbf{B}^{0}\leftarrow\mathbf{I}$, $k\leftarrow 0$

4 while $||\mathbf{g}^{k}||>\delta$ do

5      Calculating the trajectory generated by the temporal parameter $\mathbf{T}^{k}$ using MINCO;

6       By discretizing the velocity of the obtained trajectory, we derive the penalty function (27);

7       $\mathbf{d}\leftarrow-\mathbf{B}^{k}\mathbf{g}^{k}$;

8       $\mathbf{t}\leftarrow$ Lewis Overton line search;

9       $\mathbf{T}^{k+1}\leftarrow\mathbf{T}^{k}+\mathbf{t}\mathbf{d}$;

10       $\mathbf{B}^{k+1}\leftarrow$ Cautious-Limited-Memory-BFGS$(\mathbf{g}^{k+1}-\mathbf{g}^{k},\mathbf{T}^{k+1}-\mathbf{T}^{k})$;

11       $k\leftarrow k+1$;

12 end while

Solve $f$ and $\boldsymbol{\tau}$ by differential flatness transformation.

Algorithm 1 Multistage trajectory generation