Onboard Path Planning for Reusable Launch Vehicles Application to the Shuttle Orbiter Reentry Mission Vincent Morio∗,a , Franck Cazauranga , Ali Zolghadria , Philippe Vernisb a IMS lab/University of Bordeaux, 351 cours de la lib´ eration, 33405 Talence, France. b Astrium Space Transportation, 66 route de Verneuil, 78133 Les Mureaux, France.
Abstract This communication proposes a method for designing a model-based onboard path planning unit for reusable launch vehicles. Flatness approach is used to map the system dynamics into a lower dimension space. As a consequence, the number of optimization variables involved in the optimal control problem is reduced. In addition, nonconvex nonlinear trajectory constraints in the flatoutput space are inner approximated by means of superquadric shapes. Genetic algorithms are used to find a global solution both for the superquadric shapes and the associated geometric transformations tuning parameters. Finally, simulations results are presented to illustrate the proposed approach. Simulations are based on the terminal area energy management phase of the Shuttle orbiter STS-1 reentry mission. Key words: Trajectory planning, differential flatness, convexification, genetic algorithm, superquadrics.
Nomenclature A&L GA HAC N EP OCP RLV ST S T AEM T EP
Approach and Landing Genetic Algorithm Heading Alignment Cylinder Nominal Exit Point Optimal Control Problem Reusable Launch Vehicle Space Transportation System Terminal Area Energy Management TAEM Entry Point
∗ Corresponding
author. Email addresses:
[email protected] (Vincent Morio),
[email protected] (Franck Cazaurang),
[email protected] (Ali Zolghadri),
[email protected] (Philippe Vernis)
Preprint submitted to International Review of Aerospace Engineering
October 18, 2008
1. Introduction oday, trajectory generation for reusable launch vehicles (RLV) requires a significant amount of ground-level analysis. Depending on the type of T mission, ground intervention could be too complex, too long or temporarily impossible (i.e. in case of automated operation during a critical phase), and/or too costly. Reliable onboard path planning appears to be a promising alternative, as it could provide a greater flexibility to account for off-nominal conditions or even to recover timely the vehicle from faulty situations. The motivation behind this work is to provide a general and efficient method for onboard path planning of RLV. The main advantage over the existing strategies is that the initial optimal control problem is transformed into a convex geometric setup by using flatness approach and annexation techniques, leading to an integration-free programming problem, which is computationally tractable and can be solved quickly. The concept of differential flatness has been widely used in the framework of trajectory generation [14]. The main objective is to reduce the number of optimization variables involved in the optimal control problem (OCP) by mapping the nonlinear system dynamics to a lower dimensional space. However, flatness parameterization does not preserve the convexity property of the optimal control problem. In [15], the authors conclude that if the number of optimization variables is decreased while maintaining convexity, computational burden could be significantly reduced. It would be of great interest to keep the flat formulation to obtain a minimum number of optimization variables, while at the same time ensuring the convexity property of the OPC in the flat-output space. In [3], a technique called polyhedral annexation has been introduced, which consists of enlarging a polytope by annexing more and more portions of the nonconvex feasible set associated to some nonlinear trajectory constraints. However, although polytopes provide exibility by a large variety of reachable shapes, they become very difficult to handle in high order flat-output spaces. Recently, some results about convexification by n-D superquadric shapes have been obtained in [10], which constitute the starting point of this work. The contribution of this paper can be summarized as follows. First, flatness property of the guidance model is proved, and the original OCP problem is transformed into a geometric and integration-free framework. Next, a general and efficient methodology is proposed for the inner approximation of a set of nonconvex nonlinear constraints, by a smooth, convex one. Finally, a genetic algorithm (GA) is used to find a global solution both for the superquadric shapes and the associated geometric transformations tuning parameters. The overall method is then applied to the terminal area energy management phase of an atmospheric reentry mission. The remainder of the paper is organized as follows: section 2 briefly presents the interest of differential flatness for trajectory generation. Section 3 describes the proposed constraints convexification method. In section 4, the developed path 2
planning method is applied, step-by-step, to the TAEM phase of an atmospheric reentry mission. Some concluding remarks are given in a final section. 2. Differential flatness and trajectory generation 2.1. Brief review of differential flatness Differential flatness has been introduced in early 90’s [6] and has been applied to a number of problems since then: robust control [8], trajectory generation [14] and, more recently, fault diagnosis [4] and parameter estimation [5]. The main interest behind this concept lies in the ability to transform the nonlinear model of a process into a trivial one, i.e. a chain of pure integrators. Approximately, a nonlinear system is flat if there exists a set of variables such that all states and inputs can be expressed with respect to those outputs and a finite number of their time derivatives. More precisely, we consider a general dynamic system given by: x(t) ˙ = f (x(t), u(t)) (1) y(t) = h (x(t), u(t)) where t is the time variable, x is the n-dimensional state vector, u is the mdimensional input vector, y is the standard m-dimensional outputs vector, f (.) and h(.) are nonlinear smooth functions. We assume that m ≤ n. Note that, by definition, the number of flat output components is equal to the number of system inputs [6]. Definition 1 (Differential flatness). The nonlinear system (1) is (differentially) flat if and only if there exists a vector of smooth scalar functions z(t) = (z1 (t), . . . , zm (t)) such that: ˙ z(t),. ¨ . . , are differentially indepen• z(t) and its successive derivatives z(t), dent, • z(t) = Φ x(t), u(t), u(t), ˙ . . . , u(α) (t) (linearizing output), • Conversely, x and u can be expressed as: x(t) = Ψx z(t), z(t), ˙ . . . , z (β−1) (t) u(t) = Ψu z(t), z(t), ˙ . . . , z (β) (t)
(2)
where the multi-index β = (β1 , . . . , βm ) contains the characteristic numbers associated to the flat outputs such that, for i = 1, . . . , m : k d zi (t) ∗ βi = min k ∈ N : ∂ /∂uj 6= 0, j ∈ bme (3) dtk The elements of z are called flat outputs. Thus, nonlinear system trajectories are equivalent to those of the trivial system z (β) = v.
3
2.2. Optimal control problem in flat-output space It is assumed that all the path planning requirements, defined either at mission or vehicle levels, may be integrated into a general Optimal Control Problem (OCP) defined by: Z
tf
min J = C0 (x(t0 ), u(t0 )) +
x(t),u(t)
Ct (x(t), u(t)) dt t0
+Cf (x(tf ), u(tf )) subject to: x(t) ˙ = F (x(t), u(t)) , l0 ≤ A0 x(t0 ) + B0 u(t0 ) ≤ u0 lt ≤ At x(t) + Bt u(t) ≤ ut , lf ≤ Af x(tf ) + Bf u(tf ) ≤ uf L0 ≤ c0 (x(t0 ), u(t0 )) ≤ U0 Lt ≤ ct (x(t), u(t)) ≤ Ut , Lf ≤ cf (x(tf ), u(tf )) ≤ Uf
t ∈ [t0 , tf ]
(4)
t ∈ [t0 , tf ]
t ∈ [t0 , tf ]
where Ci , i = 0, t, f are appropriate cost functions. The transcription of the optimal control problem (4) into an nonlinear programming problem (NLP) in flat-output space is accomplished by parameterizing the flat outputs in terms of piecewise polynomial functions followed by a balanced discretization in the time parameter. In particular, the flat outputs {zi }m i=1 and their derivatives may be parameterized in terms of piecewise polynomial functions using a linear combination of B-spline basis functions [2], such that: (r)
zi (t, p0 , . . . , pNc −1 ) =
NX c −1
(0)
Bj,d (t)pj , t ∈ [t0 , tf ]
(5)
j=0 (0)
where Bj,d (t) is the 0th time derivative of the jth B-spline basis function of degree d, built on a given knot sequence, and pj is the corresponding jth control point [2]. This formulation is obtained using flatness properties which make it possible to get an integration-free formulation by removing the dynamic, trajectory and actuator constraints. An equivalent optimal control problem can be rewritten
4
in the flat-output space as: Z
tf
min J = G0 (z(t0 )) + z(t)
Gt (z(t)) dt + Gf (z(tf )) t0
subject to: l0 ≤ A˜0 z(t0 ) ≤ u0 lt ≤ A˜t z(t) ≤ ut , lf ≤ A˜f z(tf ) ≤ uf L0 ≤ c˜0 (z(t0 )) ≤ U0 Lt ≤ c˜t (z(t)) ≤ Ut , Lf ≤ c˜f (z(tf )) ≤ Uf
(6) t ∈ [t0 , tf ]
t ∈ [t0 , tf ]
In the next section, a procedure is presented to get a convex optimal control problem. The technique is based on the inner approximation, by superquadric shapes, of the trajectory constraints and the cost functional occurring in the OCP (6). 3. Optimal convexification The optimal control problem (6) obtained in the previous section may be directly used to compute an optimal TAEM trajectory. However, most of trajectory constraints, once transformed in the flat-output space, become highly nonlinear and nonconvex. In this section, a general and efficient methodology, based on deformable superquadric shapes and genetic algorithms, is proposed to compute approximating convex trajectory constraints and cost functional. This process results in a convex OCP which can be solved efficiently with improved computational performances. In order to perform a trade-off between complexity (i.e. number of attainable shapes), and numerical tractability in high order flat-output spaces, superquadrics primitives have been retained to inner-approximate the feasible set associated to nonlinear trajectory constraints. Superquadrics have been introduced in [1] as a generalization in n dimensions of the superellipses, and then applied to a number of problems. The main advantages are that few parameters are needed to describe the overall shape, and also that an explicit representation of the surface exists. In addition, compared to some other deformable objects, as hyperquadrics primitives, the number of attainable shapes is limited and, in some cases, additional convexity-preserving geometric transformations may be necessary. Thus, this subsection will focus on the n-dimensional extension of formulas and transformations derived only for three dimensional applications. Basically, a n-dimensional superquadrics may be described by the implicit ana-
5
lytical function Fn (x) = Λn,n (x) given by the following recursive expression: 2 2 Λn,2 (x) = x1 1 + x2 1 a1 a2 (7) 2 k−2 Λn,k (x) = (Fk−1 ) k−1 + xk k−1 ak where x is the vector of n-dimensional coordinates, a ∈ Rn the semi-major axes values, and ∈ Rn−1 are the roundness parameters. The expression Fn , so-called inside-outside function, enjoys the following properties: • Fn (x) < 1: the point x lies inside the superquadric, • Fn (x) = 1: the point x lies on the surface, • Fn (x) > 1: the point x lies outside the superquadric. The values of the roundness parameters will determine the convexity of the superquadric shape. It has been proved in [1] that the superquadrics are convex shapes if, ∀i, i ∈]0, 2]. In order to fully characterize the shapes in n dimensions, it is needed to introduce some definitions and notations first. The well-known explicit ”trigonometric“ parameterization corresponding to the inside-outside function (7) is given by: Definition 2 (n-D trigonometric parameterization [10]). Given a superquadric S, described by semi-major axes values a ∈ Rn , roundness parameters ∈ Rn−1 and anomalies θ ∈ Rn−1 , the associated trigonometric parameterization, with Cartesian coordinates xi , i = 1, . . . , n, is given by: n−1 a Y cosk θ i=1 i k k=1 n−1 xi = (8) Y cosk θk i = 2, . . . , n − 1 ai sini−1 θi−1 k=1 ai sini−1 θi−1 i=n where the vector of anomalies θ satisfies: θi
∈ [−π, π]
θi
[− π2 , π2 ]
∈
i=1 i = 2, . . . , n − 1
(9) (10)
The sampling points obtained with the trigonometric parameterization are not equally spaced at the surface of the superquadric. The objective here is to enlarge the deformable primitive within the feasible set. So, an equal-distance sampling of the superquadric surface would be preferable to reduce the number of sampling points needed. The n-D extension of the so-called ”angle-center“ representation provides a near-optimal sampling of the superquadric surface. Definition 3 (n-D angle-center parameterization). Given a superquadric S, described by semi-major axes values a ∈ Rn , roundness parameters ∈ Rn−1
6
and anomalies θ ∈ Rn−1 , the associated angle-center parameterization, with Cartesian coordinates xi , i = 1, . . . , n, is given by: n−1 Y r (θ ) cos θ i=1 k k k=1 n−1 (11) xi = Y r (θi−1 ) sin θi−1 r (θk ) cos θk i = 2, . . . , n − 1 k=1 r (θi−1 ) sin θi−1 i=n where the radii are defined by: r (θ1 )
=
r (θi )
=
cos θ1 a1
cos θi ai+1
2
1
2
i
+
sin θ1 a2
+
sin θi ai+1
2 − 21 1
2 − 2i i
i=1
(12)
i = 2, . . . , n − 1
(13)
and θi
∈ [−π, π]
θi
[− π2 , π2 ]
∈
i=1
(14)
i = 2, . . . , n − 1
(15)
Another important aspect linked to the use of superquadrics concerns the number of attainable shapes, which is limited due to the intrinsically weak number of available degrees of freedom. Therefore, some convexity-preserving geometric transformations have been introduced to encompass a wider number of nonconvex nonlinear constraints, namely n-D rotation, translation, and linear pinching along a single axis (not detailed here). Hence, the set Ψ, containing all the sizing parameters needed to obtain a positioned, oriented and bended superquadric shape, may be defined as: Ψ
= {a1 , . . . , an , 1 , . . . , n−1 , Φ1 , . . . , Φn(n+1)/2 , | {z } | {z } | {z } semi-axes
roundness
rotation
(16)
d1 , . . . , dn , v1 , . . . , vn−1 } | {z } | {z } translation
pinching
The inside-outside function (7) has also been modified in order to account for the previous n-D transformations.
Vn (Ψ) = 2an
n−1 Y i=1
ai i
n−1 X
Card Ckn−1
X
n Y
j=1
m=1
k=1
vp Υn−m,m−1 B
i k+1 i , i + 1 2 2 (23)
7
Definition 4 (inside-outside function). Let S be the class of nth order superquadric shapes parameterized by the set Ψ defined by (16). In this case, the (implicit) inside-outside function Fn (Ψ, x) = Λn,n (Ψ, x) is given by the recursive expression: 2 2 1 1 x x 1 2 “ ” “ ” Λn,2 (Ψ, x) = + v1 v2 a1 ap xp +1 a2 ap xp +1 (17) 2 k−2 k−1 x k−1 k “ ” + Λn,k (Ψ, x) = (Λn,k−1 (x)) vk ak
ap
xp +1
where vp = 0 in the pinching direction. Moreover, the volume of a superquadric shape S of order n can be obtained from the explicit parameterization (8) or (11). The superquadric volume will be used later in the objective function of the optimization process. Definition 5 (superquadric volume). Let S be the class of superquadric shape parameterized by the set Ψ defined in (16). The volume Vn (Ψ) of S is given by (23), where Υs,t = 0Υs−1,t , 1ΥR (19) s,t−1 represents the revolving-door gray code with Υs,0 = 0s and Υ0,t = 1t . Moreover, vp = 0 in the pinching direction. The beta function B(x, y) is related to the gamma function by: Z B(x, y) = 2
π/2
sin2x−1 φ cos2y−1 φdφ =
0
where the typical gamma function is defined by: Z ∞ Γ(x) = exp−t tx−1 dt
Γ(x)Γ(y) Γ(x + y)
(20)
(21)
0
In addition, to avoid numerical issues during the optimization process, a normalized volume V˜n is preferably computed such that: 1
V˜n (Ψ) = Vn (Ψ) n
(22)
where n is the order of the superquadric shape. Finally, the n-dimensional Euclidean radial distance may be useful to compute the shortest distance from an arbitrary point to the surface of the superquadric shape. Definition 6 (n-D radial Euclidean distance). The radial Euclidean distance d is defined as a distance between a point x0 and the corresponding point
8
xs at the surface of a superquadric, along a line through the point and the center of the shape. For an arbitrary superquadric shape parameterized by Ψ, a closed-form expression is given by: d (Ψ)
= |x0 − xs | = |x0 − βx0 | = |x0 |.|1 − β| −
= |x0 |.|1 − (Fn (Ψ, x0 ))
n−1 2
|
(23)
or also: d (Ψ) = |x0 − xs | = |
n−1 xs − xs | = |xs |.| (Fn (Ψ, x0 )) 2 − 1| β
(24)
where β is a scaling factor such that, for a point defined by a vector x0 in the canonical superquadric coordinate system, the tip of the scaled vector xs = βx0 lies on the surface of the shape. The constraints convexification problem can be stated as follows: Problem 1. Consider a superquadric shape S of order n, parameterized by the set Ψ defined in (16). The optimization problem then consists in finding the optimal parameters Ψ∗ corresponding to the largest superquadric shape Sopt contained inside a feasible domain (supposed to be nonconvex), such that: V˜n (Ψ) Fn (Ψ, x) ≤ 1 subject to: xli ≤ xi ≤ xui , max Ψ
(25) i = 1, . . . , n
where the set of tuning parameters Ψ is given by (16), the superquadric volume V˜n (Ψ) by (22), and the inside-outside function Fn (Ψ, x) is defined by (17). The optimal set of tuning parameters Ψ∗ is then found by a genetic algorithm, which is a robust and global optimization method (see [7] for a survey). As regards problem 1, the initial population of the GA is generated such that some elementary convex volumes matching the constraints (25) are drawn randomly over the whole search space. Then, near-uniform samplings of the superquadrics surfaces are performed thanks to the angle-center parameterization given in definition 3. Each individual fitness consists of two parts: a normalized volume term, which is computed by means of eq. (22), and an inside-outside function constraint violation term, computed by a modified tournament selection operator. The genetic operators which have been retained in this study are the simulated binary crossover (SBX) and a parameter-based mutation. Finally, a complete net migration topology has been adopted to obtain an extended multipopulation genetic algorithm. The overall convexification methodology presented in this section has been implemented in OCEANS (Optimal Convexification by Evolutionary Algorithm aNd Superquadrics), a Matlab software library developed at IMS lab/University of Bordeaux. All the superquadrics functions and GA operators mentioned in this section are currently implemented in the toolbox (selection, mutation, crossover, migration, ...). 9
3.1. Convex optimal control problem Assume that one or several superquadric shapes have been found to be solutions of Problem 1. Consider the OPC problem in the flat-output space given by (6). By using the inside-outside function (4), replace the nonlinear trajectory constraints and the cost functional by their approximating convex shapes. The goal is then to compute optimal trajectories lying inside the deformable shapes. The optimal control problem becomes: Z
tf
min J = G0 (z(t0 )) + z(t)
Gt (z(t)) dt + Gf (z(tf )) t0
subject to: l0 ≤ A˜0 z(t0 ) ≤ u0 lt ≤ A˜t z(t) ≤ ut , t ∈ [t0 , tf ] lf ≤ A˜f z(tf ) ≤ uf L0 ≤ c˜0 (z(t0 )) ≤ U0 0 ≤ Fn (Ψ∗ , z(t)) ≤ 1, t ∈ [t0 , tf ] Lf ≤ c˜f (z(tf )) ≤ Uf
(26)
It should be noted that the trajectory endpoints constraints Fn (Ψ∗ , z(t0 )) ≤ 1 and Fn (Ψ∗ , z(tf )) ≤ 1 must also be met. This may be achieved by checking that the endpoints are lying inside the superquadric shape by computing the corresponding n-D radial Euclidean distances via (6). The last step consists in discretizing the convex optimal control problem (26) in order to make it finite-dimensional. For this purpose, consider the uniform time partition t0 = ∆0 < ∆1 < ∆N∆−1 = tf , where N∆ is a predefined number of collocation points. The result of the previous discretization is a nonlinear programming problem where the unknowns are the active control points of all the B-spline curves. The resulting nonlinear programming problem can then be solved by using available solvers (as NPSOL, SNOPT or KNITRO for instance). 4. Application to terminal area energy management path planning 4.1. Description of the TAEM reentry phase The terminal area management phase, is principally aimed at dissipating the vehicle’s energy. The flight segment begins at the TAEM entry point (TEP), at the end of the hypersonic phase, and extends to the approach/landing capture zone, at the nominal energy point (NEP), defined when the vehicle is on glideslope, on airspeed, and on extended runway centerline (see Fig. 1). The path planning requirements during the TAEM flight segment are threefold. First, protection against excessive mechanical constraints (essentially the load factor and dynamic pressure) must be ensured. Second, final path constraints dictated by NEP kinematics conditions (velocity, flight path angle, alignment with the 10
extended runway centerline) must be met in order to ensure a safe autolanding. Third, to avoid possible actuator saturations in the flight control loop, guidance commands must stay within prescribed ranges. The main objective of the guidance software is then to generate the necessary commands to enable the vehicle to achieve the proper A&L conditions, while taking account of various energy conditions due to dispersions at TAEM entry point. The reader can found more details about all these requirements in [12, 9].
Figure 1: Typical TAEM path
4.2. Flatness property of the guidance model Under the assumption of a non-rotating Earth, which is licit with regards to the trajectory sizing, the vehicle can be located in a Lambert conformal conic projection frame (flat Earth coordinates) linked to the runway center (Xrwy , Yrwy , Zrwy ) [16]. Moreover, it is assumed that symmetric flight condition exists in the baseline guidance model, i.e. β = 0. Such a requirement is compliant with the path planner objectives. Moreover, as time is not a relevant parameter during atmospheric entry, a free trajectory duration λ is considered, the latter being strictly monotonic along any TAEM trajectory. Introducing 0 the normalized time τ = λt with 0 ≤ τ ≤ 1, then d(.)/dτ = λ d(.) dt = (.) , and the
11
point-mass equations become: x0 y0
= λV cos χ cos γ = λV sin χ cos γ
(27) (28)
h0
= λV sin γ D = λ − − g sin γ m L cos µ g = λ − cos γ mV V L sin µ = λ mV cos γ
(29)
V0 γ0 χ0
(30) (31) (32)
where (x, y, h, V, γ, χ) are the states and (α, µ) the inputs. The symbols L and D denote respectively the lift and drag forces, depending on the aerodynamic coefficients in clean configuration such as: 1 ρSV 2 CL (α, M ) 2 1 D(α, M ) = ρSV 2 CD (α, M ) 2 L(α, M ) =
(33) (34)
The aerodynamic coefficients must be approximated as smooth continuous analytic functions in order to be able to invert the nonlinear guidance equations properly. In clean configuration, the coefficients are provided by 2-dimensional look-up tables extracted from the open-source Shuttle Orbiter STS-1 aerodynamic design data book, freely available on NASA website1 . A fitting technique is then used, based on singular value decompositions and neural networks (not detailed here). Finally, a simple atmospheric model, well-suited to the TAEM phase, is considered with a constant gravitational acceleration and an exponential atmospheric density model given by: ρ = ρ0 exp (−h/Href ) (35) where ρ0 is the atmospheric density at sea level and Href is a Earth-relative constant atmospheric scale factor tuned for low altitude atmosphere layers. By choosing (x, y, h) as candidate flat outputs, it can be easily proved that the nonlinear guidance model is not flat, since it is not fully actuated [13]. However, all the states and inputs of the nonlinear point-mass model may be rewritten exclusively as functions of the independent variable λ, of the flat outputs and a 1 http://ntrs.nasa.gov
12
finite number of their time derivatives. Namely, the states are given by: p z102 + z202 + z302 V = λ ! z30 γ = arctan p 02 z1 + z202 0 z χ = arctan 20 z1
(36) (37) (38) (39)
and the control inputs are extracted from (31)-(32) such that: χ0 cos γ µ = arctan γ γ 0 + g cos λ V α=
2m cos γχ0 a0 − a1 fCL ρSV λ sin µ a1
(40)
(41)
However, since the model (27)-(32) is not flat for β = 0, the dynamic equation (30) must be strictly verified such that: 1 ρSV 2 CD (α, M ) V0 + g sin γ + =0 λ 2 m
(42)
where each term of the equality constraint (42) can be written exclusively as functions of the flat outputs and the free trajectory duration λ. 4.3. Convexification of nonlinear trajectory constraints We consider the nonlinear equation of the dynamic pressure constraint Q expressed as a function of the flat outputs, which must be lower than Qmax = 16kP a along the reference trajectory, i.e. p z102 + z202 + z302 1 z3 0 ≤ ρ0 exp − S ≤ Qmax (43) 2 Href λ From a geometrical point of view, this constraint can be seen as an exponentially contracting spherical shape (without the negative flight path angle constraint), the free trajectory duration λ acting as a homothety factor. In order to innerapproximate the dynamic pressure constraint by a convex shape, we consider a 5th order superquadric shape and the associated n-D geometric transformations introduced in section 3. Linear pinching will be performed along the altitude axis, which corresponds to the direction of the exponential decrease. The tuning parameters of the genetic algorithm have been chosen through trials and simulations. The optimized individual fitnesses Fi , i = 1, . . . , Nind along generations are depicted in Fig. 2 with a logarithmic scale. The parallel evolutions of the 3 subpopulations and exchanges of genetic material can be clearly seen until reaching the optimum. A projection of the resulting superquadric shape in the 13
Figure 2: GA individuals costs wrt generations.
Figure 3: Inner approximation of the dynamic pressure constraint by a superquadrics.
(z10 λ, z3 , z20 λ) frame is represented in Fig. 3 as well as the surface angle-center parameterization. The same convexification process has been applied to other nonlinear trajectory constraints such as bank angle, angle-of-attack and load factor. In order to solve the convex OCP in the flat-output space (26), the flat outputs
14
have been parameterized by 7th order B-splines with 5 intervals, and multiplicities of 3 to obtain continuous 1st and 2nd order flat outputs time derivatives. The free trajectory duration λ has been simply parameterized with a 1st order B-spline with zero multiplicity and a single interval. A number of 40 collocation points have been regularly distributed in the range [0, 1], associated to the normalized time τ . Then, the OCP (26) is transformed into a nonlinear programming problem by means of the Nonlinear Trajectory Generation (NTG) software tool [11]. Finally, the resulting NLP problem has been solved with NPSOL in about 200ms on a 2Go Intel Core 2 Duo processor. The optimal TAEM trajectory is depicted in Fig. 4, where the load factor along the trajectory is displayed in color. The same trajectory has been plotted inside the superquadric shape corresponding to the dynamic pressure constraint (see Fig. 3). The corresponding bank angle and angle-of-attack are depicted in Fig. 5.
Figure 4: Reference TAEM trajectory.
The guidance input profiles remain within the prescribed ranges defined by the superquadric shapes. The inside-outside functions associated to the bank angle and dynamic pressure superquadrics are depicted in Fig. 6. Clearly, the constraints Fni (Ψ∗ , z(t)) ≤ 1, i = 1, 2 are met along the reference TAEM trajectory. Finally, the mechanical constraints undergone by the vehicle during the TAEM phase are depicted in Fig. 7. The load factor and dynamic pressure never reach their maximum allowed values along the TAEM flight segment.
15
(a)
(b)
Figure 5: (a) Reference bank angle profile (b) Reference angle-of-attack profile.
(a)
(b)
Figure 6: (a) Dynamic pressure inside-outside function (b) Bank angle inside-outside function.
(a)
(b)
Figure 7: (a) Load factor constraint (b) Dynamic pressure constraint.
5. Conclusion The problem studied by this paper is that of designing an efficient modelbased strategy for onboard path planning for atmospheric reentry vehicles. A 16
convex optimal control problem is formulated by using flatness approach and annexation techniques. The optimization variables are parametrized by B-spline curves. The resulting nonlinear programming problem can be solved quickly, with low computational burden. Simulation results have shown the potential of the proposed approach for the terminal area energy management phase of the Shuttle orbiter STS-1 reentry mission. An appealing direction for further work is to make the path planner tolerant to potential single/multiple actuator faults occurring during the reentry mission. This is a topic of our current research. References [1] Barr, A., jan 1981. Superquadrics and angle-preserving transformations. IEEE Computer Graphics and Applications 1 (1), 11–23. [2] de Boor, C., 2001. A pratical guide to spline. Applied Mathematical Sciences. Springer, New York. [3] Faiz, N., Agrawal, S., Murray, R., 2001. Trajectory planning of differentially flat systems with dynamics and inequalities. Journal of Guidance, Control and Dynamics 24 (2), 219–227. [4] Fliess, M., Join, C., Mounier, H., 2004. An introduction to nonlinear fault diagnosis with an application to a congested internet router. Lecture Notes in Control and Information Sciences. Springer, London. [5] Fliess, M., Join, C., Sira-Ramirez, H., 2008. Non-linear estimation is easy. Int. J. Modelling, Identification and Control 3, 219–227. [6] Fliess, M., L´evine, J., Martin, P., Rouchon, P., 1995. Flatness and defect of non-linear systems: introduction theory and examples. Int. Journal of Control 61, 1327–1361. [7] Goldberg, D., January 1989. Genetic Algorithms in Search, Optimization, and Machine Learning. Addison-Wesley Professional. [8] Hagenmeyer, V., Delaleau, E., 2003. Robustness analysis of exact feedforward linearization based on differential flatness. Automatica 39 (11), 1941–1946. [9] Harpold, J., Graves, C., oct 1978. Shuttle entry guidance. In: Metzler, R. A., Powers, W. F. (Eds.), 25th Anniversary Conference,American Astronautical Society. [10] Louembet, C., Cazaurang, F., Zolghadri, A., Pittet, C., Charbonnel, C., 2007. Design of algorithms for satellite slew maneuvers by flatness and collocation. In: American Control Conference.
17
[11] Milam, M., Mushambi, K., Murray, R., 2000. A new computational approach to real-time trajectory generation for constrained mechanical systems. In: Proc. 39th IEEE Conference on Decision and Control. IEEE, Sydney, Australia. [12] Moore, T., 1991. Space shuttle entry terminal area energy management (NASA TM 104744). [13] Neckel, T., Talbot, C., Petit, N., 2003. Collocation and inversion for a reentry optimal control problem. In: Proceedings of the 5th Intern. Conference on Launcher Technology. [14] Petit, N., Milam, M., Murray, R., jul 2001. Inversion based trajectory optimization. In: Proc. 5th IFAC Symposium on Nonlinear Control Systems Design (NOLCOS). IFAC, St. Petersburg, Russia. [15] Ross, I., Fahroo, F., 2006. Issues in the real-time computation of optimal control. Mathematical and computer modelling 43 (9-10), 1172–1188. [16] Vinh, N. X., Busemann, A., Culp, R. D., 1980. Hypersonic and Planetary Entry Flight Mechanics. The university of Michigan Press.
18