Successive Convexification for Trajectory Optimization with Continuous-Time Constraint Satisfaction
Purnanand Elango Dayou Luo Abhinav G. Kamath Samet Uzun Taewan Kim Behçet Açıkmeşe



SCP Traj. Opt. CTCS



[footnoteinfo]Corresponding author: Purnanand Elango.












^*pelango@uw.edu,  
^†dayoul@uw.edu,  
^*agkamath@uw.edu,  
^*samet@uw.edu,
^*twankim@uw.edu,  and  
^*behcet@uw.edu

^*William E. Boeing Department of Aeronautics & Astronautics
^†Department of Applied Mathematics
University of Washington, Seattle, WA, 98195

                                                      
Trajectory optimization; optimal control;                            
continuous-time constraint satisfaction; sequential convex programming 


 
We present successive convexification, a real-time-capable solution method for nonconvex trajectory optimization, with continuous-time constraint satisfaction and guaranteed convergence, that only requires first-order information. The proposed framework combines several key methods to solve a large class of nonlinear optimal control problems: (i) exterior penalty-based reformulation of the path constraints; (ii) generalized time-dilation; (iii) multiple-shooting discretization; (iv) ℓ_1 exact penalization of the nonconvex constraints; and v) the prox-linear method, a sequential convex programming (SCP) algorithm for convex-composite minimization. The reformulation of the path constraints enables continuous-time constraint satisfaction even on sparse discretization grids and obviates the need for mesh refinement heuristics. Through the prox-linear method, we guarantee convergence of the solution method to stationary points of the penalized problem and guarantee that the converged solutions that are feasible with respect to the discretized and control-parameterized optimal control problem are also Karush-Kuhn-Tucker (KKT) points. Furthermore, we highlight the specialization of this property to global minimizers of convex optimal control problems, wherein the reformulated path constraints cannot be represented by canonical cones, i.e., in the form required by existing convex optimization solvers. In addition to theoretical analysis, we demonstrate the effectiveness and real-time capability of the proposed framework with numerical examples based on popular optimal control applications: dynamic obstacle avoidance and rocket landing.









§ INTRODUCTION




Trajectory optimization forms an important part of modern guidance, navigation, and control (GNC) systems, wherein, it is used to generate reference trajectories for onboard use and also in offline design and analysis. State-of-the-art trajectory optimization methods do not yet check all the boxes with regard to desirable features: continuous-time feasibility, real-time performance, convergence guarantees, and numerical robustness <cit.>. Among these, continuous-time feasibility, which refers to the feasibility of the state trajectory and the control input with respect to the system dynamics and path constraints, is especially challenging due to the infinite-dimensional nature of optimal control problems. However, continuous-time feasibility is essential for meeting safety and performance requirements, which is a prerequisite for the deployment of autonomous systems such as space exploration vehicles, reusable rockets, dexterous robotic manipulators, etc <cit.>. Aerospace applications, in particular, typically prioritize feasibility and real-time performance over optimality <cit.>.

Trajectory optimization methods can be broadly classified into indirect and direct methods <cit.>. Invoking the maximum principle to solve optimal control problems with general nonlinear dynamics subject to path constraints is challenging. Such solution methods, i.e., indirect methods, usually take an optimize-then-discretize approach, and can only handle a restricted class of problems. The solutions they generate, however, naturally ensure continuous-time feasibility <cit.>. Methods that take the discretize-then-optimize approach, i.e., direct methods, on the other hand, are capable of handling general optimal control problems and are more reliable numerically (they are less sensitive to initialization). Since discretization is a crucial step in direct methods, the resulting solutions invariably suffer from so-called inter-sample constraint violations <cit.> (see Figure <ref>).

A majority of the existing direct methods, such as MISER <cit.>, DIRCOL <cit.>, 
PSOPT <cit.>, PROPT <cit.>, and GPOPS-II <cit.>, transcribe optimal control problems to nonlinear programs (NLP) and call standard NLP solvers, such as SNOPT <cit.>, IPOPT <cit.>, and Knitro <cit.>, which require second-order information, lack convergence guarantees, and are unsuitable for real-time, embedded applications.  In contrast, software such as  <cit.> and ACADO <cit.> interface with custom quadratic programming (QP), sequential quadratic programming (SQP), and NLP solvers that exploit the structure of the underlying problem. However, they neither guarantee convergence nor enforce continuous-time constraint satisfaction.

Several direct methods either—(i) assume a discrete-time formulation with path constraints imposed at finitely-many time nodes, i.e., they disregard the conversion of continuous-time problem descriptions to discrete-time ones (e.g., TrajOpt <cit.>, PANOC <cit.>, CALIPSO <cit.>); (ii) provide convergence guarantees by restricting the class of path constraints <cit.>; or (iii) ensure continuous-time constraint satisfaction for a  restricted class of dynamical systems <cit.>. 

 

Sequential convex programming (SCP) algorithms for trajectory optimization have received attention in the recent years <cit.> as a competitive alternative to sequential quadratic programming (SQP) and IPM-based NLP algorithms, especially since SCP is a multiplier-free method and does not require second-order information. In trajectory optimization, computing the second-order sensitivities of nonlinear dynamics (which form a part of the Hessian of the Lagrangian within SQP and IPM) is expensive <cit.>. Recent work has explored convergence guarantees for SCP, in the context of both trajectory optimization <cit.> and general nonconvex optimization <cit.>. SCP-based trajectory optimization methods, without theoretical guarantees, have been applied to a wide range of robotics and aerospace applications, with domain-specific heuristics that ensure effective practical performance <cit.>. Widespread adoption of SCP for performance- and safety-critical applications, however, will necessitate the development of a general framework with rigorous certification of its capabilities.

We propose successive convexification, an SCP-based, real-time-capable solution method for nonconvex trajectory optimization, with continuous-time feasibility and guaranteed convergence. The proposed framework combines several key methods to solve a large class of nonlinear optimal control problems: (i) exterior penalty-based reformulation of the path constraints; (ii) generalized time-dilation; (iii) multiple-shooting discretization; (iv) ℓ_1 exact penalization of the nonconvex constraints; and (v) the prox-linear method, an SCP algorithm for convex-composite minimization. 

The reformulation of path constraints involves integrating the continuous-time constraint violation using a smooth exterior penalty, which is transformed into an auxiliary dynamical system with boundary conditions. The reformulation combined with multiple-shooting discretization <cit.> enables continuous-time feasibility even on sparse discretization grids, and obviates the need for mesh refinement heuristics. While such constraint reformulations have appeared in the optimal control literature since the 1960s <cit.> with specific choices of penalty functions <cit.>, the full extent of its capabilities for enabling SCP-based convergence-guaranteed, continuous-time-feasible trajectory optimization have not been explored, to the best of the authors' knowledge. We address the consequences of the reformulation on a constraint qualification that is important in numerical optimization, and provide a rigorous quantification of the extent of continuous-time constraint satisfaction. 

Through the prox-linear method <cit.>, we guarantee convergence of the solution method to stationary points of the ℓ_1-penalized problem and guarantee that the converged solutions that are feasible with respect to the discretized and control-parameterized optimal control problem are also Karush-Kuhn-Tucker (KKT) points. Furthermore, we highlight the specialization of this property to global minimizers of convex optimal control problems, wherein the reformulated path constraints cannot be represented by canonical cones, i.e., in the form required by existing convex optimization solvers. 

In addition to theoretical analysis, we demonstrate the effectiveness of the proposed framework with numerical examples based on popular optimal control applications: two nonconvex problems—dynamic obstacle avoidance and 6-DoF rocket landing, and one convex problem—3-DoF rocket landing using lossless convexification. We also demonstrate the real-time capability of the proposed framework on the nonconvex examples considered, by executing C code generated using , an in-house-developed general-purpose real-time trajectory optimization software with customized code-generation support. The C codebase generated by  uses the proposed framework to solve the optimal control problem at hand. 

Simplified implementations of the proposed framework were recently demonstrated for specific applications, ranging from GPU-accelerated trajectory optimization for six-degree-of-freedom (6-DoF) powered-descent guidance <cit.> and nonlinear model predictive control (NMPC) for obstacle avoidance <cit.>, to trajectory optimization for 6-DoF aircraft approach and landing <cit.>.




§ PROBLEM FORMULATION


This section describes the transformation of a path-constrained, free-final-time optimal control problem to a fixed-final-time optimal control problem through generalized time-dilation and constraint reformulation. The constraint reformulation involves the conversion of path constraints into a two-point boundary value problem for an auxiliary dynamical system.




 §.§ Notation


We adopt the following notation in the remainder of the discussion. 
The set of real numbers is denoted by , the set of nonnegative real numbers by , the set of real n× m matrices by ^n× m, and the set of real n× 1 vectors by ^n. 
The concatenation of vectors v∈^n and u∈^m is denoted  by (v,u)∈^n+m, the concatenation of matrices A∈^l× m and B∈^l× n by [A B] ∈^l×(m+n), the Cartesian product of sets C and D by C× D, and the Kronecker product by ⊗. 
The vector of ones in ^n is denoted by n, the identity matrix in ^n× n by n, and the matrix of zeros in ^n× m by n× m. Whenever the subscript is omitted, the size is inferred from context. 
For any scalar v, we define v = max{0,v}. The operations □, |□|, □^2 (and their compositions) apply elementwise for a vector. 
The Euclidean norm of a vector v is denoted by v. 
The indicator function of a convex set D is denoted by _D (see <cit.>), and its normal cone at x by _D(x) <cit.>. 
The subdifferential of a function f, evaluated at x, is a set denoted by ∂f(x), and its members are called subgradients. 
The gradient of a differentiable function g:^n→^l with respect to z∈^m, evaluated at x∈^n, is denoted by ∇_z g(x)∈^n× m, where the elements of z can include a subset or superset of the arguments of g (irrespective of the order), and the subscript z is omitted if it coincides with the list of arguments of g. The (sub)gradient of a scalar-valued function at a point is defined to be a row vector (for e.g., from ^1× m). The (sub)gradient of a vector-valued function is a matrix consisting of the (sub)gradients of its scalar-valued elements along the rows. In particular, the partial derivatives of function (u,v)↦ h(u,v), evaluated at u̅,v̅, are denoted by hu(u̅,v̅), hv(u̅,v̅), respectively (with the argument inferred from context whenever omitted). Furthermore, the notation □(ς)ς represents the derivative with respect to a scalar variable.




 §.§ Optimal Control Problem


We consider a class of free-final-time optimal control problems for nonlinear dynamical systems with nonconvex constraints on the state and input, given by




    x, u, minimize      L(,x()) 
    subject to     ẋ(t) = f(t,x(t),u(t))  æ t∈
        g(t,x(t),u(t)) ≤n_g     æ t∈
        h(t,x(t),u(t)) = n_h     æ t∈
        P(,x(),,x()) ≤n_P
        Q(,x(),,x()) = n_Q




where the derivative with respect to time t∈ in (<ref>) is denoted by □̇ = □t. We assume that the terminal state cost function L:×→, the dynamics function f:××→, the path constraint functions g:××→^, h:××→^, and the boundary condition constraint functions P:×××→^n_P, Q:×××→^n_Q, are continuously differentiable. The inequality and equality constraints in (<ref>)-(<ref>) are interpreted elementwise. The initial time ∈ is fixed, while the final time ∈ is a free (decision) variable. For simplicity, we omit parameters <cit.> of the system dynamics and constraints in (<ref>). With minimal modifications, the subsequent development can handle parameters as decision variables as well. Note that the above problem can be a nonconvex trajectory optimization problem due to: i) nonlinear dynamics, i.e., f is a nonlinear function; ii) final time being free; iii) nonlinear functions h and Q; iv) nonconvex functions g and P.  

We assume that the control input u:→ is a piecewise continuous function.  Since f is continuously differentiable, this  ensures the existence and uniqueness of an absolutely continuous function x : →,  the state trajectory, which satisfies (<ref>) almost everywhere and the following integral equation


    x(t) = x_i + ∫_^tf(γ,x(γ),u(γ))dγ


where x() = x_i is the initial state. Note that existence and uniqueness of the state trajectory can be ensured with weaker assumptions; we refer the reader to <cit.> and <cit.> for detailed discussions. We also  require the following boundedness assumption based on Gronwall's Lemma <cit.>.


For any compact U⊂ and > ≥ 0, there exist positive  ϑ and ϱ such that f(t, x, u)≤ϑx + ϱ
for all (t, x, u) ∈ [, ] ××U.


Note that Assumption <ref> may not always hold. Consider f(t,x,u) = x^2. For any positive ϑ and ϱ, when x becomes sufficiently large, Assumption <ref> is invalid. Nonetheless, for physical systems, one can define a compact set 𝕏⊂, containing all physically meaningful states. Any state not contained in 𝕏 can be regarded as infeasible. Consequently, we create a modified dynamics function so that it satisfies Assumption <ref> and is consistent with the original dynamics function on 𝕏. To be precise, for any open set 𝕎 containing 𝕏, there exists a smooth function ω : → such that ω(x) = 1 for all x∈𝕏 and the support of ω is compact and contained in 𝕎, as shown in <cit.>. Therefore, we can replace function f in (<ref>) with ω f, which is compactly supported in , ensuring Assumption <ref>. Note that the inclusion of ω is only for the purpose of proofs of results discussed in subsequent sections; it is not necessary in a practical implementation.


The optimal control problem (<ref>) is posed in the Mayer form <cit.>, without loss of generality. The original problem could be provided in the Bolza or Lagrange forms where the objective function contains the integral of a continuously differentiable running cost function L_r:×^n_x×^n_u→, given by


    ∫_^L_r(t,x(t),u(t))dt


which is well-defined when x is a state trajectory and u is the corresponding piecewise continuous control input. The Mayer form is obtained from the Lagrange and Bolza forms by equivalently reformulating the integral of L_r as a new dynamical system


    l̇(t) = L_r(t,x(t),u(t))


and adding l() as a terminal state cost to the objective function, along with specifying the initial condition l() = 0.


We propose an SCP and multiple-shooting based solution method <cit.> (which only requires first-order information). The gradient of the integral of L_r can be computed through the first-order sensitivities <cit.> of dynamical system (<ref>)—a qualitatively similar effort to linearizing (<ref>). Therefore, converting to the Mayer form is beneficial for consolidating majority of the effort of gradient computation in one place, which is efficient for numerical implementations. Furthermore, the integral of running cost function can be either convex or nonconvex. In the convex case, however, the integral often cannot be reformulated as a canonical conic constraint with the help of additional slack variables (see Section <ref> for further details). Moreover, a convex running cost function in a free-final-time problem will become nonconvex after conversion to a fixed-final-time problem via the time-dilation technique described in Section <ref>. In the subsequent development, unless stated otherwise, we assume that the dynamical system (<ref>), the terminal state cost function L, and the boundary condition (<ref>) already encode any desired running cost terms. 




 §.§ Constraint Reformulation


To reformulate path constraints (<ref>) and (<ref>), we consider continuous exterior penalty functions

    
    q_i : ××→,  i=1,…,
        q_i(z)  = 0    if z ≤ 0 
    > 0    otherwise
        p_j : ××→,  j=1,…,
        p_j(z)  = 0    if z = 0 
    > 0    otherwise


and the penalty function : ××→ 

    (t,x,u) = ∑_i=1^ q_i(g_i(t,x,u)) + ∑_j=1^ p_j(h_j(t,x,u))

Note that g_i, i=1,…,, and h_j, j=1,…,, denote the scalar-valued elements of functions g and h, respectively.

It is straightforward to show that t↦(t,x(t),u(t)) is piecewise continuous over  when x is a state trajectory and u is the corresponding piecewise continuous control input.

 Path constraints (<ref>) and (<ref>) are satisfied by x(t) and u(t) æ t∈ iff

    ∫_^(t,x(t),u(t))dt = 0




For each t∈, (t,x(t),u(t)) is nonnegative since q_i(g_i(t,x(t),u(t))) and p_j(h_j(t,x(t),u(t))) are nonnegative. Since t↦(t,x(t),u(t)) is piecewise continuous, (<ref>) holds iff (t,x(t),u(t)) = 0 æ t∈.

Then, q_i(g_i(t,x(t),u(t))) = 0 and p_j(h_j(t,x(t),u(t))) = 0 æ t∈, which is equivalent to g_i(t,x(t),u(t)) ≤ 0 and h_j(t,x(t),u(t)) = 0 æ t∈.


Next, we augment the system dynamics with an additional state to accumulate the constraint violations, and impose periodic boundary conditions on it to ensure continuous-time constraint satisfaction.


The differential-algebraic system



    ẋ(t) = f(t,x(t),u(t))
        g(t,x(t),u(t)) ≤ 0
        h(t,x(t),u(t)) = 0



is satisfied by x and u a.e. in  iff x and u solve the boundary value problem on 



    ẋ(t) =    f(t,x(t),u(t))
    ẏ(t) =   (t,x(t),u(t))
    
    y(0) =    y()





 Suppose that x and u satisfy (<ref>), (<ref>), and (<ref>) almost everywhere in  . Since t↦(t,x(t),u(t)) is piecewise continuous over , there exists a unique state trajectory y:→ which satisfies (<ref>) almost everywhere in  with initial condition y() = 0. Then, y() = 0 iff (<ref>) holds. Therefore, from Lemma (<ref>), the path constraints (<ref>) and (<ref>) are satisfied almost everywhere in .

For the other direction, we let x, y, and u be the solution to (<ref>). Then, (<ref>) implies that (<ref>) holds. We obtain the desired result from Lemma <ref>.


 Since  a gradient-based solution method will be adopted,  the exterior penalty functions q_i and p_j, for i=1,…, and j=1,…,, must be  continuously differentiable in order to compute first-order sensitivities <cit.> of (<ref>). For e.g., 



    q_i(z) =   z^2, p_j(z) = z^2
   

for any z∈, are valid choices. Then,  can be compactly written as


    (t,x,u) = n_g^⊤g(t,x,u)^2 + n_h^⊤ h(t,x,u)^2


for any t∈, x∈, and u∈.



The partial derivatives of  are given by

    □ = ∑_i=1^∇ q_ig_i□ + ∑_i=1^∇ p_jh_j□

where □ = t, x, and u. A consequence of the continuous differentiability of q_i and p_j, and their construction in (<ref>), is that their derivatives in (<ref>) are zero wherever g_i ≤ 0 and h_j = 0.     



The penalty function  can be defined more generally as a vector-valued function of the form

    (t,x,u) = M[[ q(g(t,x,u)); p(h(t,x,u)) ]]

for any t∈, x∈, and u∈, where matrix M ∈^n_y × (+), which we refer to as the mixing matrix, has nonnegative entries, and functions q : ^→^ and p:^→^ are defined as


    q((z_g^1,…,z_g^))    = (q_1(z_g^1),…,q_(z_g^))
    
        p((z_h^1,…,z_h^))    = (p_1(z_h^1),…,p_(z_h^))


for any z^i_g ∈ and z_h^j ∈, with i=1,…, and j=1,…,. The mixing matrix has at most + rows, at most one positive entry per column, and exactly + positive entries in total. We obtain (<ref>) and a scalar-valued state y in (<ref>) by choosing M = +^⊤. In general, choosing (<ref>) will lead to a vector-valued state y∈^n_y for quantifying the total constraint violation. While (<ref>) is suitable for most cases, choosing (<ref>) for certain applications can be numerically more reliable. In the remainder of the discussion, we adopt the definition in (<ref>). However, the results also apply to the general representation in (<ref>).





 §.§ Generalized Time-Dilation



Time-dilation is a transformation technique for posing a free-final-time optimal control problem as an equivalent fixed-final-time problem <cit.>. Let x be a state trajectory for (<ref>) over  obtained with control input u and some initial condition. We define a strictly increasing, continuously differentiable mapping t : [0,1]→, with boundary conditions t(0)=, t(1)=, and treat the derivative of the map 


    s(τ) = t(τ)τ = t(τ)


for τ∈[0,1], as an additional control input, which we refer to as the dilation factor. Note that the derivative with respective τ∈[0,1] is denoted by □ = □τ. Previously, dilation factors were either treated as a single parameter decision variable (as in <cit.>) or multiple discrete parameter decision variables (as in <cit.>). The approach proposed herein generalizes it by treating dilation factor as a continuous-time control input. We note that this approach bears resemblance to the time-scaling transformation proposed in <cit.>. Further, we treat t as an additional state, and define functions ũ and x̃ as 



    ũ(τ) = (u(t(τ)), s(τ))∈^n_u+1
       x̃(τ) = (x(t(τ)), y(t(τ)), t(τ))∈^n_x+2


for τ∈[0,1]. Then, using chain-rule, we obtain 


    x̃(τ) = [ ẋ(t); ẏ(t);    1 ]t(τ)τ = F(x̃(τ),ũ(τ))


for τ∈[0,1], where 


    F(x̃(τ),ũ(τ)) = 
    [ f(t(τ),x(t(τ)),u(t(τ)));  (t(τ),x(t(τ)),u(t(τ)));                       1 ]s(τ)

We refer to (<ref>) as the augmented system, with augmented state x̃, and augmented control input ũ.

In the subsequent development, we define =n_x+2 and use matrices x, y, and t to select the first n_x elements (x), the penultimate element (y), and the last element (t), respectively, of x̃. Similarly, matrices u and s select the first n_u elements (u) and the last element (s), respectively, of ũ. Note that generalized time-dilation transforms non-autonomous dynamical systems into equivalently autonomous ones (without explicit dependence on time) by increasing the dimensions of the state and control input. If the final time is fixed in (<ref>), then the generalized time-dilation step can be skipped. Henceforth, we drop the qualifier “generalized” for brevity.


When the system dynamics and constraint functions in (<ref>) are time-varying, we use time-dilation to augment the system with an extra state and control input. However, when the system and constraint descriptions are time-invariant, it suffices to augment only the control input.     
 




 §.§ Reformulated Optimal Control Problem




Subjecting (<ref>) to constraint reformulation and time-dilation results in the following optimal control problem




    x̃, ũminimize    L̃(x̃(1)) 
    subject to    x̃(τ) = F(x̃(τ),ũ(τ))       æ τ∈[0,1]
        sũ(τ)>0       æ τ∈[0,1] 
        y(x̃(1)-x̃(0)) = 0
        P̃(x̃(0),x̃(1)) ≤ 0 
        Q̃(,x̃(0),x̃(1)) = 0




where, 



    L̃(x̃) =    L(tx̃,xx̃)
    P̃(x̃,x̃^') =    P(tx̃,xx̃,tx̃^',xx̃^')
    Q̃(,x̃,x̃^') =   [ Q(,xx̃,tx̃^',xx̃^');               tx̃ - ]



for any x̃,x̃^'∈ℝ^n_x̃. Since time is treated as a state variable, its initial condition needs to be specified through Q̃. This is not needed in (<ref>) because (constant)  is explicitly passed to functions P and Q. Furthermore, positivity of the dilation factor is ensured using (<ref>).

Observe that, as a consequence of the constraint reformulation, the continuous-time constraints on the state and control input are not explicitly imposed in (<ref>); they are instead embedded within the augmented system in (<ref>) and boundary condition (<ref>).




§ PARAMETERIZATION AND DISCRETIZATION


This section describes the transformation of the (infinite-dimensional) reformulated optimal control problem (<ref>) into a (finite-dimensional) nonconvex optimization problem through parameterization of the augmented control input, and discretization of (<ref>) over the interval [0,1]. In addition, we introduce a relaxation to the constraint reformulation step after discretization to ensure favorable properties from an optimization viewpoint, and analyze the consequences of the relaxation.



We parameterize the augmented control input with finite-dimensional vectors via ν̃:[0,1]→^n_ũ, defined as


    ν̃(τ) = [ Γ_u(τ)⊗[    n_u; 1× n_u ] ] U + [ n_u× N_s;   Γ_s(τ) ]S


for τ∈[0,1], where 


    Γ_□ = [Γ_□^1 … Γ_□^N_□]


Each element in (<ref>), Γ_□^k : [0,1] →, is a polynomial basis function, for k=1,…,N_□ and □ = u,s. Vectors U∈^n_uN_u and S∈^N_s, which are the decision variables, are coefficients parameterizing the control input and dilation factor. The support of each of the basis functions in Γ_u and Γ_s can be pairwise disjoint and strictly contained in [0,1]. 


    ν̃(τ) = ( (Γ_u(τ) ⊗n_u)U, Γ_s(τ)S)



The parameterization (<ref>) for the augmented control input is general enough to subsume several control input parameterizations used in practice, such as zero- and first-order-hold, and pseudospectral polynomials in orthogonal collocation (see Appendix <ref>). We distinguish between the parameterizations for the control input and dilation factor because the former is allowed to have a general polynomial parameterization as long as U is elementwise bounded, whereas, the latter must be constrained to be bounded and positive due to (<ref>). Such a requirement should be straightforward to handle with convex constraints on S.


After parameterizing the augmented control input, we discretize (<ref>) via its integral form over a finite grid of size N in [0,1], denoted by 0=τ_1<…<τ_N=1. We treat the augmented states x̃_k at the node points τ_k also as decision variables, and denote them compactly as 


    X̃ = (x̃_1,…,x̃_N)∈^ N


Next, we define F_k:^×^×^n_uN_u×^N_s→^ 


    (x̃_k+1,x̃_k,U,S) ↦
                 x̃_k+1-x̃_k-∫_τ_k^τ_k+1F(x̃^k(τ),ν̃(τ))dτ


for k=1,…,N-1, where the augmented state trajectory x̃^k satisfies (<ref>) almost everywhere on [τ_k,τ_k+1] with augmented control input ν̃, and initial condition x̃_k. Then, the discretization of (<ref>), together with (<ref>), yields the constraints


    F_k(x̃_k+1,x̃_k,U,S) = 0
       y(x̃_k+1-x̃_k)=0


for k=1,…,N-1. The discretization in (<ref>), commonly referred to as multiple-shooting, is exact because there is no approximation involved in switching to the integral representation of the differential equation (<ref>), and it is numerically more stable compared to its precursor, single-shooting <cit.>.  


 The triplet X̃, U, and S is said to be continuous-time feasible if 
it satisfies (<ref>).


A continuous-time feasible triplet: X̃, U, and S corresponds to an augmented state trajectory generated by  integration of (<ref>) over [0,1] with augmented control input ν̃ and initial condition x̃_1, such that it passes through the node values x̃_k at τ_k, for k=2,…,N. Furthermore, due to (<ref>), the resulting state trajectory and control input satisfy the path constraints almost everywhere in .


Continuous-time feasibility is a crucial metric for assessing the quality of solutions computed by direct methods. They solve for finite-dimensional quantities (such as X̃, Ũ, and S) which parameterize the control input, and at times, even the state trajectory. In particular, direct collocation methods <cit.> approximate the state trajectory with a basis of polynomials (for e.g. orthogonal collocation methods <cit.>). They match the derivative of the polynomial approximant to the right-hand-side of (<ref>), and impose the path constraints at finitely-many nodes. As a result, such methods cannot guarantee that the computed solution is continuous-time feasible, necessitating computationally intensive heuristics such as mesh refinement <cit.>. On the other hand, the proposed framework leverages multiple-shooting and constraint reformulation to provide continuous-time feasible solutions, which is a key distinction  compared to existing direct methods <cit.>, <cit.>.




 §.§ Constraint Qualification and Relaxation


An issue with directly imposing (<ref>) is that it violates linear independence constraint qualification (LICQ) <cit.>. For each point in ^n_x̃N×^n_uN_u×^N_s, we can determine the active set <cit.> corresponding to the constraints in (<ref>). LICQ is said to hold at a point if the gradients of the constraints in the corresponding active set are linearly independent. The following result shows that LICQ is violated at all points feasible with respect to (<ref>).

 If X̃, U, and S are feasible with respect to (<ref>) and (<ref>), then for k=1,…,N-1


    yF_kx̃_k+1(x̃_k+1,x̃_k,U,S) =    [1× n_x 1 0]
    yF_kx̃_k(x̃_k+1,x̃_k,U,S) =    [1× n_x-1 0]
    yF_k U(x̃_k+1,x̃_k,U,S) =   1× n_uN_u
    yF_k S(x̃_k+1,x̃_k,U,S) =   1× N_s




Suppose X̃, U, and S satisfy (<ref>) and (<ref>). For each k=1,…,N-1, let x̃^k be the augmented state trajectory on [τ_k,τ_k+1] with augmented control input ν̃ and initial condition x̃_k. Then, due to Lemma <ref>, path constraints (<ref>) and (<ref>) are satisfied almost everywhere on the time interval [tx̃_k,tx̃_k+1]. Recall that F defined in (<ref>) is continuously differentiable and that ν̃ is piecewise continuous. This  implies that 


    τ↦F(x̃^k(τ),ν̃(τ))x̃, τ↦F(x̃^k(τ),ν̃(τ))ũ


are piecewise continuous functions. Therefore, they are integrable and the partial derivatives of F_k are well-defined. Elements of the penultimate row of Fx̃ and Fũ 
(i.e., the partial derivatives of ), are zero æ on [τ_k,τ_k+1] when evaluated on  x̃^k and ν̃. This is because all elements of this row contain factors of the derivatives of q_i or p_j, which are zeros due to Remark <ref>. Hence, only the partial derivatives of F_k with respect to x̃_k and x̃_k+1 contain nonzero elements in that row, as shown in (<ref>) and (<ref>).


Using Lemma <ref>, for  any X̃, U, and S satisfying (<ref>) and (<ref>), the gradient of the left-hand-side of (<ref>), and the penultimate row of the gradient of F_k (shown in (<ref>)) are identical. Hence, all feasible solutions of an optimization problem having both (<ref>) and (<ref>) as constraints will not satisfy LICQ. 
The proposed framework relies on the classical exact penalization result to determine KKT points of (<ref>) <cit.>, which are related to local minimizers when LICQ holds <cit.>. So, we remedy the pathological scenario due to (<ref>) by relaxing (<ref>) to an inequality with a positive constant ϵ. 

Then, (<ref>) transforms under augmented control input parameterization (<ref>) and discretization (<ref>) as follows 




    X̃, U, Sminimize      L̃(x̃_N)
    subject to     F_k(x̃_k+1,x̃_k,U,S) = 0
        y(x̃_k+1-x̃_k) ≤ϵ
        k=1,…,N-1 
        U∈U, S∈S
        P̃(x̃_1,x̃_N) ≤ 0, Q̃(,x̃_1,x̃_N) = 0




where U⊂^n_uN_u and S⊂^N_s are compact convex sets. 


If X̃, U, and S satisfy (<ref>) and (<ref>), then LICQ will not be trivially violated by the active set associated with (<ref>) and the penultimate row of (<ref>). If (<ref>) holds with strict inequality, for all k=1,…,N-1, then the result follows immediately. However, if for some k=1,…,N-1, (<ref>) holds with equality, then


    ∫_τ_k^τ_k+1sν̃(τ)(tx̃(τ),xx̃^k(τ),uν̃(τ))dτ = ϵ


where x̃^k is the augmented state trajectory with augmented control input ν̃ and initial condition x̃_k. Due to piecewise continuity of the integrand in (<ref>) with respect to τ∈[τ_k,τ_k+1], it implies that there exists an interval I_k ⊂ [τ_k,τ_k+1] where the integrand of (<ref>) is positive, i.e., some of the path constraints are violated on the segments of x̃^k and ν̃ corresponding to interval I_k. Therefore, unlike the case in Lemma <ref>, the partial derivatives of  evaluated on those segments of x̃^k and ν̃ will not be trivially zero. 



Besides addressing the LICQ issue, relaxation (<ref>) is essential for enabling exact penalization—a key step in the proposed framework. The combination of (<ref>) and (<ref>) is equivalent to the following constraint for each k=1,…,N-1


    y(x̃_k+1-x̃_k-F_k(x̃_k+1,x̃_k,U,S)) = 0


Exact penalization of (<ref>) involves penalizing the left-hand-side of (<ref>) using a nonsmooth exterior penalty function (such as p_j). However, the effect of exterior penalty is suppressed due to the positivity and continuous differentiability of , which destroys the exactness of the penalty term (see discussion in <cit.>). In other words, it may not be possible to recover a solution that is feasible with respect to (<ref>) using a finite weight for the penalty term.



Set U ensures that the control input lies in a compact convex set. Whenever the choice of parameterization permits (e.g., with zero- or first-order-hold), the convex control constraints in (<ref>) and (<ref>) could be captured with U, i.e., via imposing convex control constraints only at the node points (inter-sample constraint satisfaction is guaranteed); otherwise, U will encode elementwise bounds on U. Set S ensures that the dilation factor is positive and bounded. Besides handling convex control constraints directly, requiring the augmented control input to lie in a compact set via (<ref>) is necessary for deriving a pointwise constraint violation bound, given the ϵ-relaxation in (<ref>). 





 §.§ Gradient of Discretized Dynamics


A key step in any gradient-based solution method for (<ref>) is to compute the partial derivatives of F_k. We adopt the so-called variational method <cit.>, <cit.>, also referred to as inverse-free exact discretization <cit.>. 

Let X̅̃̅ = (x̅̃̅_1,…,x̅̃̅_N), U̅, and S̅ denote a reference solution, and let ν̅̃̅ denote the corresponding parameterization using (<ref>). For each k=1,…,N-1, consider the following initial value problem over [τ_k,τ_k+1]


    Φ^k_x̃(τ) = A(τ)Φ^k_x̃(τ)
       Φ^k_u(τ) = A(τ)Φ^k_u(τ) + Γ_u(τ)⊗ B(τ)
       Φ^k_s(τ) = A(τ)Φ^k_s(τ) + Γ_s(τ)⊗ C(τ)
       Φ_x̃^k(τ_k) = Φ
       Φ_u^k(τ_k) = × n_uN_uΦ
       Φ_s^k(τ_k) = × N_sΦ


where Φ_x̃^k(τ)∈^×, Φ_u^k(τ)∈^× n_uN_u, and Φ_s^k(τ)∈^× N_s, for τ∈[τ_k,τ_k+1]. The partial derivatives of F evaluated on the augmented state trajectory x̅̃̅^k for (<ref>) over [τ_k,τ_k+1], with augmented control input ν̅̃̅ and initial condition x̅̃̅_k, are compactly denoted with A(τ)∈^×, B(τ)∈^× n_u, and C(τ)∈^, i.e.,


    A(τ) =   Fx̃(x̅̃̅^k(τ),ν̅̃̅(τ))
    
     B(τ) =   Fũ(x̅̃̅^k(τ),ν̅̃̅(τ))u^⊤
    
     C(τ) =   Fũ(x̅̃̅^k(τ),ν̅̃̅(τ))s^⊤


for τ∈[τ_k,τ_k+1]. Then, the partial derivatives of F_k with respect to x̃_k, U, and S (denoted by A_k, B_k, and C_k) evaluated at the reference solution are related to the terminal value of the solution to (<ref>) as follows


    A_k =   Φ^k_x̃(τ_k+1) = - F_kx̃_k(x̅̃̅_k+1,x̅̃̅_k,U̅,S̅) 
    
    B_k =   Φ_u^k(τ_k+1) = -F_k U(x̅̃̅_k+1,x̅̃̅_k,U̅,S̅)
    
    C_k =   Φ_s^k(τ_k+1) = -F_k S(x̅̃̅_k+1,x̅̃̅_k,U̅,S̅)



Finally, the linearization of F_k is given by the mapping


    (x̃_k+1,x̃_k,U,S) ↦x̃_k+1 - x̅̃̅^k(τ_k+1) 
            - A_k(x̃_k-x̅̃̅_k) - B_k(U-U̅) - C_k(S-S̅)
 

Note that arbitrarily chosen X̅̃̅, U̅, and S̅ are, in general, not continuous-time feasible, i.e., x̅̃̅^k(τ_k+1) x̅̃̅_k+1, for k=1,…,N-1. We refer to <cit.> and <cit.> for further insights and related methods for discretization and linearization of nonlinear dynamics.




 §.§ Pointwise Constraint Violation Bound


Given a solution for (<ref>), we can obtain the state trajectory x, control input u, and final time . In addition, we can construct a time grid t_1<…<t_N, satisfying t_1 = and t_N =. Node t_k is the time instant corresponding to τ_k, for k=1,…,N. The length of interval [t_k,t_k+1] is denoted by Δ t_k, for k=1,…,N-1, and Δ t_min denotes a lower bound for the lengths of the N-1 intervals. 
Then we can establish an upper bound for the pointwise violation of the path constraints when (<ref>) is satisfied for a specified ϵ>0, and with the choice of exterior penalties (<ref>). 

 For any i=1,…,, j=1,…,, and k=1,…,N-1, there exist ω_g_i and ω_h_j such that 

    □(t,x(t),u(t)) ≤   δ_□(ϵ) = (4ϵω_□)^1/3

whenever ϵ in (<ref>) satisfies

    ϵ≤ω^2_□Δ t_min^3/4

for t∈[t_k,t_k+1] and □ = g_i,h_j.

 See Appendix <ref>.


While the analysis in Appendix <ref> assumes (<ref>), other valid exterior penalty functions can be handled in a similar manner. Further, note that the bound in (<ref>) is consistent because δ_□(ϵ) is strictly monotonic for ϵ∈ and δ_□(ϵ) → 0 as ϵ→ 0, where □=g_i,h_j. We can use these bounds to select an ϵ that is numerically significant yet physically insignificant for the underlying dynamical system and constraints. 


The pointwise bound for the inequality path constraints specifies the amount of constraint tightening required for the solutions obtained with relaxation (<ref>) to satisfy the constraints with no pointwise violation. More precisely,

    ∫_t_k^t_k+1g_i(t,x(t),u(t))+δ_g_i(ϵ)^2dt ≤ϵ
        g_i(t,x(t),u(t)) ≤ 0   ∀ t∈[t_k,t_k+1]

We cannot, however, specify such a tightening for the equality constraints that ensures pointwise satisfaction. We can only control the extent of pointwise violation using (<ref>). Specialized techniques such as the one proposed in <cit.> could be explored as a future direction.


 
The estimate of ω_g_i and ω_h_j can be conservative whenever it is challenging to tightly bound f(t,x(t),u(t)). As a result, the bound in (<ref>) can become conservative. In such cases, a solution to (<ref>) can be iteratively refined until the pointwise violations are within a desired tolerance. We can successively solve (<ref>) by reducing ϵ by a constant factor and warm-starting with the previous solution.  This refinement process will terminate since δ_□ is strictly monotonic and consistent (i.e., lim_ϵ→0δ_□(ϵ) = 0).



The key distinction between the approach in Remark <ref> and the mesh refinement techniques for alleviating inter-sample constraint violation <cit.> is that the former does not add more node points to the discretization grid, i.e., N remains fixed. 
The proposed framework can achieve continuous-time constraint satisfaction (up to a tolerance ϵ) on a sparse discretization grid. Most direct methods verify the extent of inter-sample constraint violation a posteriori, i.e., after solving the trajectory optimization problem for a chosen discretization grid. In contrast, the proposed framework specifies the extent of allowable inter-sample constraint violation within the optimization problem. 




§ PROX-LINEAR METHOD FOR NUMERICAL OPTIMIZATION


We propose a solution method which applies an SCP algorithm called the prox-linear method <cit.> to an unconstrained minimization obtained from transforming (<ref>) via ℓ_1 exact penalization of (<ref>) and (<ref>) <cit.>. 




 §.§ Exact Penalization
 

We can construct an unconstrained minimization problem from the constrained problem (<ref>) by penalizing the constraints. The penalty terms have weights as tunable hyperparameters in the unconstrained problem. The penalty functions are said to be exact whenever, for a finite penalty weight, the stationary points and local minimizers of the unconstrained problem can capture the KKT points and local minimizers of the constrained problem. A widely-used example of such penalty functions, which are typically nonsmooth, are the ℓ_1 penalty functions <cit.>. Quadratic penalty functions, in contrast, are not exact, i.e., the penalty weight must tend to infinity in order to obtain the first-order optimal points of the constrained problem. Clearly, adopting an exact penalization of constraints is practical for designing a numerical solution method since the solution to a single unconstrained problem can deliver a solution to the original constrained problem without needing to solve a sequence of problems where penalty weight is arbitrarily increased. 

Standard results on exact penalization <cit.>, <cit.> for general constrained optimization appeared close to three decades ago, and are widely used. However, they don't directly apply for the proposed formulation since the construction of (<ref>) is atypical, in that it possesses convex constraints: (<ref>), (<ref>) (represented with convex sets), and nonconvex constraints: (<ref>), (<ref>) (represented with functions). While transforming (<ref>) into an unconstrained minimization, we wish to subject only the nonconvex constraints to exact penalization, while using indicator functions to represent the convex constraints, so that they can be directly parsed without approximations to a conic optimization solver iteratively called within the prox-linear method. The standard exact penalty results cannot be directly applied to this hybrid case. To the best of authors' knowledge, an exposition on the modifications necessary to handle the hybrid case is not available in the literature. For completeness and clarity, this section presents the exact penalty results with required modifications along with the proofs. 

Given γ > 0,  function Θ:^n_z→ is defined as 



    Θ_γ(z) =   L̃(x̃_N) + _Z(z)  
         + γ^⊤P̃(x̃_1,x̃_N) + γQ̃(,x̃_1,x̃_N)_1 
        + γ∑_k=1^N-1F_k(x̃_k+1,x̃_k,U,S)_1



where n_z =  N+n_uN_u+N_s, z = (X̃,U,S), X̃ = (x̃_1,…,x̃_N), and Z = X̃×U×S is a closed convex set with 


    X̃ = {[ (x̃_1,…,x̃_N);       ∈^n_x̃N ]| [ y(x̃_k+1-x̃_k) ≤ϵ,;  x̃_k∈^, k=1,…,N-1 ]}


The ℓ_1-penalization of constraints (<ref>) and (<ref>) involves penalty functions  □↦□ and □↦□_1, respectively. 

Our goal then is to compute a local minimizer of Θ_γ where the penalty terms are zero. We start by seeking a stationary point of Θ_γ, since local minimizers are stationary points. Determining stationary points is a relatively simpler task than directly attempting to solve the nonconvex problem (<ref>). We can rewrite the indicator function for Z as explicit convex constraints[Commonly encountered convex sets such as polytope, ball, box, second-order cone, halfspace, hyperplane etc. can be directly represented in conic optimization solvers, such as ECOS <cit.> and MOSEK <cit.>, as the intersections of canonical cones: zero cone, nonnegative orthant, second-order cone, and the cone of positive semidefinite matrices.] while computing a stationary point of Θ_γ via SCP. A stationary point z^⋆ satisfies 0∈∂Θ_γ(z^⋆), i.e., there exists μ∈^n_z and λ∈^n_λ denoted by 


    λ =    (λ^P̃_1,…,λ^P̃_n_P,λ^Q̃_1,…,λ^Q̃_n_Q+1,…
          λ^F_1_1,…,λ^F_N-1_n_x̃)


with n_λ = n_P+n_Q+n_x̃(N-1)+1, such that


    λ^P̃_i ∈   ∂P̃_i(x̃^⋆_1,x̃_N^⋆),        i = 1,…,n_P
    λ^Q̃_i ∈   ∂|Q̃_i(,x̃^⋆_1,x̃_N^⋆)|,        i = 1,…,n_Q+1
    λ^F_k_i ∈   ∂|F_ki(x̃^⋆_k+1,x̃_k^⋆,U^⋆,S^⋆)|,        k=1,…,N-1
              i = 1,…,n_x̃
    


    0_1 × n_z =   ∇_z L(x̃^⋆_N) + μ^⊤
       + γ∑_i=1^n_Pλ^P̃_i ∇_z P̃_i(x̃_1^⋆,x̃_N^⋆)
       + γ∑_i=1^n_Q+1λ^Q̃_i ∇_z Q̃_i(,x̃_1^⋆,x̃_N^⋆) 
       + γ∑_k=1^N-1∑_i=1^n_x̃λ^F_k_i ∇_z F_ki(x̃_k+1^⋆,x̃_k^⋆,U^⋆,S^⋆)


where z^⋆ = (X̃^⋆,U^⋆,S^⋆) with X̃^⋆ = (x̃_1^⋆,…,x̃_N^⋆). The scalar-valued functions P̃_i, Q̃_i, and F_ki are elements of P̃, Q̃, and F_k, respectively, indexed by i. The gradients of P̃_i, Q̃_i, and F_ki are with respect to z = (x̃_1,…,x̃_N,U,S). The convex subdifferential of □↦ |□|_+ and □↦|□| are



    ∂|w|_+ =   {[   {1}-  w > 0; [0,1]-  w = 0;   {0}-  w < 0 ].
    ∂|w|_+ =   {[    {1}  w > 0; [-1,1]  w = 0;   {-1}  w < 0 ].



for any w∈. We refer the reader to <cit.> and the references therein, for the definition and properties of the subdifferential of a nonconvex function.



Next, we need the following lemma about the normal cone of a convex set.


Suppose that z^⋆∈^n, convex set C = {z∈^n | Ω̌_i(z) ≤ 0, i=1,…,N_C} is defined with continuously differentiable convex functions Ω̌_i:^n→, and convex set D = {z∈^n | a_j^⊤ z + b_j = 0, j=1,…,N_D} is defined with vectors a_j∈^n and scalars b_j∈. Assume that 1) the collection of vectors ∇Ω̌_i(z^⋆), for i∈ I_C(z^⋆), and a_j, for j=1,…,N_D are linearly independent, where I_C(z^⋆) = {i | Ω̌_i(z^⋆) = 0} is an index set, and 2) the intersection of D and the interior of C is nonempty. Then, we have


    _C(z^⋆) =   {∑_i∈ I_Cλ_i∇Ω̌_i(z^⋆)^⊤ | λ_i ∈}
    _D(z^⋆) =   {∑_j=1^N_Dλ_ja_j | λ_j∈}
    _C∩D(z^⋆) =   _C(z^⋆) + _D(z^⋆)


where the set addition in (<ref>) is a Minkowski sum.



The construction of _C(z^⋆) and _D(z^⋆) follow directly from <cit.>, and the expression for _C∩D(z^⋆) is derived from <cit.> and <cit.>, where we use the fact that ∂_□(z) = _□(z) for any z∈^n, where □=C, D.



Given a convex optimization problem with a feasible set described by C∩D in Lemma <ref>, the corresponding assumptions are regularity conditions similar to LICQ and strong Slater's assumption <cit.>, which are typically needed for a well-posed convex optimization problem. We require the assumptions in Lemma <ref> on convex set Z since the SCP approach will involve a sequence of convex problems with Z as the feasible set.


Then, we have the following result relating the stationary points of (<ref>) and KKT points of (<ref>).


Assume that Z and z^⋆ satisfy the assumptions of Lemma <ref>. If z^⋆ is a stationary point of Θ_γ and is feasible with respect to (<ref>), then z^⋆ is a KKT point of (<ref>). Conversely, if z^⋆ is a KKT point of (<ref>) and γ is sufficiently large, then z^⋆ is also a stationary point of Θ_γ.




Suppose that z^⋆ is feasible with respect to (<ref>), where z^⋆ = (X̃^⋆,U^⋆,S^⋆) with X̃^⋆ = (x̃_1^⋆,…,x̃_N^⋆). Note that z^⋆ is a KKT point of (<ref>) if and only if there exists μ∈_Z(z^⋆) (using Lemma <ref>) and λ = (λ_1, …, λ_n_λ)∈^n_λ such that 


    ∇_z L(x̃^⋆_N) + μ^⊤ + λ^⊤[              ∇_zP̃(x̃_1^⋆,x̃_N^⋆);             ∇_zQ̃(,x̃_1^⋆,x̃_N^⋆);     ∇_zF_1(x̃^⋆_2,x̃^⋆_1,U^⋆,S^⋆);                                 ⋮; ∇_zF_N-1(x̃^⋆_N,x̃^⋆_N-1,U^⋆,S^⋆) ]
       = 0_1× n_z


and 


    λ_i P̃_i(x̃_1^⋆,x̃_N^⋆) = 0


for i = 1, …, n_P. Similar to (<ref>), the gradients of P̃, Q̃, and F_k in (<ref>) are with respect to z = (x̃_1,…,x̃_N,U,S).

If z^⋆ is a stationary point of Θ_γ, then there exists μ∈_Z(z^⋆) and λ̂ = (λ̂_1,…,λ̂_n_λ)∈^n_λ such that (<ref>) holds, where


    1/γλ̂∈   ∂P̃_1×…×∂P̃_n_P×∂ |Q̃_1| ×…
       ×∂ |Q̃_n_Q+1| ×∂ |F_11| ×…×∂ |F_(N-1)n_x̃|


The arguments of P̃_i, Q̃_i, and F_ki, which are elements of z^⋆, are omitted in (<ref>) for brevity. Observe that λ̂ satisfies (<ref>) due to the feasibility of z^⋆ for (<ref>) and due to the definition of ∂□ in (<ref>). Therefore, z^⋆ is a KKT point of (<ref>).

Conversely, if z^⋆ is a KKT point of (<ref>), then there exists μ∈_Z(z^⋆) and λ = (λ_1,…,λ_n_λ) such that (<ref>) and (<ref>) hold. Note that z^⋆ is feasible with respect to (<ref>) and suppose that γ > λ_∞. Then, we obtain λ_n_P+i/γ∈∂ |Q̃_i(,x̃_1^⋆,x̃_N^⋆)| = [-1,1], for i = 1, …, n_Q+1, and λ_n_P+n_Q+n_x̃(k-1)+i+2/γ∈∂ |F_ki(x̃_k+1^⋆,x̃_k^⋆,U^⋆,S^⋆)| = [-1,1], for k=1,…,N-1, i = 1, …, n_x̃.
Furthermore, for i=1,…,n_P, we have the following. If P̃_i(x̃_1^⋆,x̃_N^⋆) < 0, then λ_i/γ∈∂P̃_i(x̃^⋆_1,x̃_N^⋆) = {0}. Alternatively, if P̃_i(x̃_1^⋆,x̃_N^⋆) = 0, then 0 ≤λ_i≤γ, i.e., λ_i/γ∈∂P̃_i(x̃^⋆_1,x̃_N^⋆) = [0,1].

Therefore, due to (<ref>) and (<ref>), z^⋆ is a stationary point of Θ_γ for a large enough γ.


Next we relate the local minimizers of (<ref>) to the stationary points and local minimizers of Θ_γ.


Suppose LICQ holds at z^⋆ and it is a strict local minimizer of (<ref>). Then, there exists γ̅ > 0 such that for all γ≥γ̅, z^⋆ is a local minimizer of Θ_γ. 



Moreover, there exist constants γ̃ > 0 and σ̃ > 0  such that, for all γ≥γ̃, if z is a stationary point of Θ_γ satisfying z-z^⋆≤σ̃, then z is feasible with respect to (<ref>).

 Consider a modification of Θ_γ, denoted by Θ̃_γ, where convex constraints due to Z are also ℓ_1-penalized instead of using the indicator function. 
The first statement follows from Theorem 4.4 in <cit.> if we first replace Θ_γ with Θ̃_γ, and note that Θ̃_γ(z) ≤Θ_γ(z) with equality iff z∈Z. The last statement follows from Propositions 3.1 and 3.3 in <cit.>.


Stronger versions of the exact penalization results also exist. For instance, it can be shown under certain assumptions that, given a local minimizer of (<ref>), there exists a neighborhood around it where the stationary points of Θ_γ are also local minimizers of (<ref>). We refer the reader to <cit.> for further details.




 §.§ Prox-Linear Method


We use the prox-linear method <cit.> developed for convex-composite minimization to compute stationary points of Θ_γ. The prox-linear method is applicable when Θ_γ can be fit into the following template


    Θ_γ(z) = J(z) + H(G(z))


where J is a proper closed convex function, H is an α-Lipschitz continuous convex function, and G is potentially nonconvex and continuously differentiable with a β-Lipschitz continuous gradient.

The prox-linear method finds a stationary point z^⋆ of Θ_γ by iteratively minimizing a convex approximation for Θ_γ given by


    Θ^ρ_γ(z;z_k) = J(z) + H(G(z_k) + ∇ G(z_k) (z-z_k))
                  + 1/2ρz-z_k^2

where z_k is the current iterate, with k ≥ 1, and ρ determines the proximal term weight. The nonconvex term H(G(z)) is convexified by linearizing G at z_k. We denote the unique minimizer <cit.> of the strongly convex function z ↦Θ^ρ_γ(z;z_k) as z_k+1. The iterative minimization of Θ^ρ_γ amounts to solving a sequence of convex subproblems, which is numerically very efficient and reliable. The convergence of this iterative approach is quantified using the prox-gradient mapping


    G_ρ(z) =  1/ρ(z-z^'argmin Θ^ρ_γ(z^';z))


This is because G_ρ(z)=0 iff z is a stationary point of Θ_γ <cit.>. Observe that Θ_γ defined in (<ref>) fits the template of (<ref>): the terminal cost function L̃ and the exact penalties for P̃, Q̃, and F_k are represented by the composition H∘ G, and the indicator function _Z is represented by J. More precisely, for any z=(x̃_1,…,x̃_N,U,S)∈^n_z, we choose



    J(z) =   _Z(z)
    
    H(ζ) =   ζ_L̃ + ^⊤ζ_P̃ + (ζ_Q̃,ζ_F)_1
    
    G(z) =   ( L̃(x̃_N), γP̃(x̃_1,x̃_N), γQ̃(,x̃_1,x̃_N),
         F_1(x̃_2,x̃_1,U,S),…,F_N-1(x̃_N,x̃_N-1,U,S))



where ζ = (ζ_L̃,ζ_P̃,ζ_Q̃,ζ_F) with ζ_L̃∈, ζ_P̃∈^n_P, ζ_Q̃∈^n_Q+1, and ζ_F∈^(N-1). While minimizing (<ref>), we translate _Z into explicit convex conic constraints that can be handled by

convex optimization solvers such as ECOS <cit.>, MOSEK <cit.>, and PIPG <cit.>.


 Under Assumption <ref>, the gradients of L̃, P̃, Q̃, and F_k are bounded on on Z.


Functions L̃, P̃, Q̃, and F_k are continuously differentiable. The domain of Θ_γ is Z. From Assumption <ref> and <cit.>, state trajectories generated with a bounded control input and dilation factor (due to (<ref>)) are bounded. Boundedness of the dilation factor also ensures that  is bounded. Then, there exists a constant, β, that bounds the norm of the gradients of L̃, P̃, Q̃, and F_k on Z.




Figure <ref> shows the complete solution method that we propose, and Algorithm <ref> describes the prox-linear method. The inputs to the algorithm are the maximum number of iterations, k_max, the termination tolerance, ε, and the proximal term weight, ρ. Note that the “Convexification” block in Figure <ref> constructs the subproblem based on (<ref>), which forms a convex approximation of the composition H ∘ G. Since the convex approximation is non-affine based on our choice of H, we call this step “Convexification” rather than “Linearization”. Furthermore, this step computes the gradient of the discretized dynamics (<ref>), as described in Section <ref>.  



We have the following result about the monotonic decrease of Θ_γ for a sufficiently small ρ.


At iteration k of Algorithm <ref>, the following holds

    Θ_γ(z_k) ≥Θ_γ(z_k+1) + ρ/2(2 - αβρ)G_ρ(z_k)^2

Therefore, Θ_γ(z_k) is monotonically decreasing if ρ≤1/αβ.


The magnitude of G_ρ(z_k) is both a practical and rigorous measurement for assessing whether z_k is close to a stationary point (see <cit.> for details). We then have the following finite-stop corollary.


Suppose Θ_γ is bounded below by Θ^⋆_γ and ρ≤1/αβ. For a given tolerance ε, prox-linear method achieves

    G_ρ(z_k)^2 < ε  for 
        k ≤2αβ(Θ_γ(z_1) -  Θ^⋆_γ)/ε



 In practice,  β can be computed by either implementing a line-search as described in <cit.> or by adopting the adaptive weight update strategy in <cit.>, where the equivalence between the trust-region and prox-linear methods is also discussed. The “Update Hyperparameters” block in Figure <ref> represents the use of such update techniques. The implementation of the prox-linear method can be further enhanced using the acceleration scheme described in <cit.>.


Note that the upper bound for ρ in Theorem <ref> is a sufficient condition for convergence to a stationary point. In general, if the prox-linear method converges with arbitrary user-specified weights ρ and γ, then the converged solution is guaranteed to be a stationary point, due to <cit.>. 

 The prox-linear method is susceptible to the crawling phenomenon exhibited by SCP algorithms <cit.>, wherein the iterates make vanishingly small progress towards a stationary point when they are not close to one. This phenomenon can happen when γ is chosen to be very large, which decreases the upper bound on ρ for ensuring monotonic decrease of Θ_γ. Then, the lower bound 1/2αβG_1/αβ(z_k) for Θ_γ(z_k)-Θ_γ(z_k+1) becomes very small even if G_1/αβ(z_k) is not small. The crawling phenomenon can be remedied by scaling the decision variables and the constraint functions so that their orders of magnitude are similar.


 The penalized trust region (PTR) SCP algorithm in <cit.> solves a sequence convex subproblems similar to the ones generated by the prox-linear method. The PTR algorithm also implements a class of time-dilation, discretization, and parameterization operations to the optimal control problem (<ref>), albeit in an order that is different from what we propose. The resulting sequence of convex subproblems that PTR solves can fit within the template of (<ref>) after minor modifications. The weights of the proximal and exact penalty terms are heuristically selected to obtain good convergence behavior.


In summary, the proposed framework based on the prox-linear method globally converges to a stationary point of Θ_γ (Theorem <ref>), and if the stationary point is feasible with respect to (<ref>), then it is a KKT point (Theorem <ref>). Furthermore, each strict local minimizer of (<ref>) with LICQ satisfied is a local minimizer of Θ_γ for a large-enough finite γ (Theorem <ref>). 




§ EXPLOITING CONVEXITY


Next we consider optimal control problem (<ref>) where both initial and final times ,  are fixed, the terminal cost function L is convex in the terminal state, the dynamical system (<ref>) is a linear nonhomogeneous ordinary differential equation, represented by

    ẋ(t) =    A(t)x(t) + B(t)u(t) + w(t)

for t∈, and the constraint functions in (<ref>), (<ref>), (<ref>), and (<ref>) are jointly convex in all arguments (except time). In particular, constraint functions h and Q are affine in state and control input. Furthermore, we deviate from the Mayer form by explicitly specifying a running cost term in the objective with a continuously differentiable function Y:××→ that is jointly convex in the state and control input. Embedding the running cost into the dynamics, as mentioned in Section <ref>, would turn the dynamical system nonlinear and hence destroy the convexity of the optimal control problem.
The resulting optimal control problem is now in the Bolza form <cit.>. 

To obtain a finite-dimensional optimization problem similar to (<ref>), we first parameterize the control input and discretize (<ref>). Time-dilation is not necessary in the fixed-final-time setting, and constraint reformulation is postponed to the end with a minor modification to exploit convexity of the constraints. We apply parameterization (<ref>) directly to the control input in the time domain, i.e., ν(t) = (Γ_u(t)⊗n_u)U, for t∈. To discretize (<ref>), we form a 
grid of size N in  denoted by =t_1<…<t_N=, and treat the states x_k at node points t_k as decision variables. Then, for each k=1,…,N-1, we exactly discretize (<ref>) via the integral form into 


    x^k(t) = Φ_x(t,t_k)x_k + Φ_u(t,t_k)U+ ϕ(t,t_k)


for t∈[t_k,t_k+1], where state trajectory x^k generated with control input ν and initial condition x_k satisfy (<ref>) æ t∈[t_k,t_k+1]. Moreover, t↦Φ_x(t,t_k), t↦Φ_u(t,t_k), and t↦ϕ(t,t_k) are computed as the solution to an initial value problem[Note that, unlike in (<ref>), superscript “k” is absent from Φ_x, Φ_u, and ϕ since they are independent of the state trajectory x^k.] similar to (<ref>), and we denote their terminal values as A_k = Φ_x(t_k+1,t_k), B_k = Φ_u(t_k+1,t_k), and w_k = ϕ(t_k+1,t_k).

Next, the path constraints (<ref>) and (<ref>) are reformulated as follows. For each k = 1,…,N-1, define g̃_k:×^n_x×^n_uN_u→^n_g and h̃_k:×^n_x×^n_uN_u→^n_h by substituting in the control parameterization and the discretized form (<ref>) into g and h


    □̃_k(t,x_k,U) = 
       □(t,Φ_x(t,t_k)x_k +Φ_u(t,t_k)U  +ϕ(t,t_k),ν(t))


where □ = g,h. Note that each scalar-valued element of g̃_k and h̃_k is jointly convex in x_k and U. Then, for each k=1,…,N-1, we obtain



    g̃_k(t,x_k,U) ≤ 0, h̃_k(t,x_k,U) = 0 ∀ t∈[t_k,t_k+1]
       ξ_k(x_k,U) ≤ 0



where ξ_k : ^n_x×^n_uN_u→ is defined as


    ξ_k(x_k,U) = ∫_t_k^t_k+1^⊤g̃_k(t,x_k,U)^2 + ^⊤h̃_k(t,x_k,U)^2dt


Similar to (<ref>), we need to relax (<ref>) with ϵ>0 in order to facilitate exact penalization.

 ξ_k is a convex function.

 
Observe that □↦max{0,□}^2 is a convex, non-decreasing function. Then, the composition ^⊤g̃_k^2 is a convex function <cit.>. Furthermore, ^⊤h̃_k^2 is a sum of convex quadratics of its arguments since h̃_k is an affine function.


For each k=1,…,N-1, the running cost over [t_k,t_k+1] can be compactly expressed with function Y_k : ×^n_uN_u→ by substituting in the control parameterization and the discretized form (<ref>) as


    Y_k(x_k,U) = ∫_t_k^t_k+1Y(t,x^k(t),ν(t))dt


where x^k is a state trajectory for (<ref>) over [t_k,t_k+1], generated with control input ν and initial condition x_k.

Finally, we obtain the convex optimal control problem subject to parameterization, discretization, and reformulation (<ref>) of path constraints with ϵ-relaxation.




    X,Uminimize      L(,x_N) + ∑_k=1^N-1Y_k(x_k,U)
    subject to     x_k+1 = A_kx_k + B_kU + w_k 
        ξ_k(x_k,U) ≤ϵ
        k=1,…,N-1 
        U∈U
        P(,x_1,,x_N) ≤ 0
        Q(,x_1,,x_N) = 0




where X = (x_1,…,x_N)∈^n_xN and the compact convex set U plays a role similar to that in Remark <ref>.

Although ξ_k(x_k,U) ≤ϵ is a convex constraint, we do not possess any structural or geometric information needed to classify it as a conic constraint of the form ÂX+B̂U+ĉ∈K, where K is a Cartesian product of canonical convex cones: zero cone, nonnegative orthant, second-order cone, cone of positive semidefinite matrices etc. In fact, we only have access to first-order oracles for Y_k and ξ_k that provide the function value and its gradient at a query point. As a consequence, we cannot directly use existing conic optimization solvers <cit.> to solve (<ref>). 

We adopt the prox-linear method after exactly penalizing (<ref>), since it is compatible with the first-order oracles for Y_k and ξ_k. Convexity of (<ref>) guarantees global convergence to the set of minimizers. 

Besides incorporating the ϵ-relaxation in (<ref>) to facilitate exact penalization, we also assume that representation (<ref>)-(<ref>) for the feasible set of (<ref>) satisfies the strong Slater's assumption <cit.>. This assumption is necessary and sufficient for the set of Lagrange multipliers associated with a minimizer of (<ref>) to be nonempty, compact, and convex <cit.>.




 §.§ Exact Penalization & Prox-Linear Method
 

Similar to (<ref>), for a given γ > 0, consider the convex function Θ_γ : ^n_xN+n_uN_u→ defined by 



    Θ_γ(z) =    L(x_N)  + ∑_k=1^N-1Y_k(x_k,U) 
        + _Z(z) + γ∑_k=1^N-1ξ_k(x_k,U)-ϵ



where z = (X,U) and Z = X×U with 


    X = {X ∈^n_xN|[   X = (x_1,…,x_N), x_k∈^n_x; x_k+1 = A_kx_k + B_kU + w_k;                   1≤ k≤ N-1;            P(,x_1,,x_N) = 0;            Q(,x_1,,x_N) ≤ 0 ]}


is the convex set encoding all state constraints except for (<ref>), which is subject to ℓ_1 penalization. Note that when ϵ=0 in (<ref>), i.e., when (<ref>) is not subject to relaxation, then its penalization is not exact since leads to a quadratic penalty on the violation of path constraints.


Under Assumption <ref>, the gradients of Y_k and ξ_k are bounded on Z.



The domain of Θ_γ is Z. Similar to Lemma <ref>, we can again invoke Assumption <ref> and <cit.> to infer that a bounded control input parameterization (due to (<ref>)) generates bounded state trajectories over the time interval . Then, there exists a constant, β, that bounds the norm of the gradients of Y_k and ξ_k on Z.


Due to the convexity of (<ref>), we can invoke a stronger exact penalization result than those in Section <ref>.

 Suppose strong Slater's assumption holds for the constraints in (<ref>). The set of stationary points of (<ref>) coincides with the set of minimizers of (<ref>) when γ is larger than the largest magnitude Lagrange multiplier associated with the minimizers of (<ref>). 

 The result follows from <cit.>. Note that strong Slater's assumption ensures that strong duality holds. 




Suppose strong Slater's assumption holds for the constraints in (<ref>) and ρ≤1/αβ. Then, Algorithm <ref> globally converges arbitrarily close to a solution of (<ref>) in a finite number of iterations.
 
 The monotonic decrease of (<ref>) guaranteed by Theorem <ref> when ρ≤1/αβ, together with Corollary <ref> and <cit.>, implies that Algorithm <ref> will get arbitrarily close to a stationary point in a finite number of iterations. Since stationary points of (<ref>) are identical to the minimizers of (<ref>), due to Theorem <ref>, we have that Algorithm <ref> will get arbitrarily close to a minimizer of (<ref>) in a finite number of iterations. 


Even though prox-linear method will globally converge to a minimizer of (<ref>), providing a good initial point z_1 will dramatically improve its practical convergence behavior. A solution to the closely related convex optimization problem where all path constraints are explicitly imposed as conic constraints at finitely-many time nodes will serve as a good initialization. Computing such a solution is very efficient in practice with existing convex solvers.




§ NUMERICAL RESULTS


We provide a numerical demonstration of the proposed framework with three examples: dynamic obstacle avoidance, six-degree-of-freedom (6-DoF) rocket landing, and three-degree-of-freedom (3-DoF) rocket landing with lossless convexification. The first two examples are nonconvex problems while the third one is convex. Appendices <ref> through <ref> describe the problem formulation (in the form of (<ref>)) and the algorithm parameters for each of the examples. The results in this section are generated with code in the following repository





https://github.com/UW-ACL/successive-convexification





For all examples considered, the convex subproblems of prox-linear method are QPs, which are solved using PIPG <cit.>. We also demonstrate the real-time capability of the proposed framework by using a C codebase generated by , a general-purpose SCP-based trajectory optimization software with customized code generation, in tandem with ECOS <cit.>.

The proximal term weight ρ is determined using a line search and the exact penalty weight γ is determined by gradually increasing it by factors of 10 (see the discussion in <cit.>). Unless otherwise specified, the initial guess z_1 for the prox-linear method consists of a linear interpolation between the initial and final augmented states, and a constant profile for the augmented control input. The nodes of the discretization grid in [0,1] for free-final-time problems, and in  for fixed-final-time problems, are uniformly spaced. To ensure reliable numerical performance, all decision variables and constraint functions g_i and h_j are scaled <cit.> to similar orders of magnitude. We refer the reader to <cit.> for a discussion on the effects of scaling in numerical optimal control. 

We compare solutions from the proposed framework with those from an implementation where path constraints are not reformulated (i.e., they are only imposed at the discretization nodes), which we refer to as the node-only approach. We demonstrate that the proposed framework can compute continuous-time feasible solutions on a sparse discretization grid without mesh-refinement, and that the node-only approach is susceptible to inter-sample constraint violation on the same sparse grid. Handling sparse discretization grids and eliminating mesh-refinement makes the proposed framework amenable to real-time applications. Moreover, the resulting state trajectory can activate constraints between discretization time nodes, which is a unique characteristic among other direct methods.

In the plots that follow, unless otherwise stated, we adopt the following convention. Dots denote the solution from prox-linear method: black (∙) for proposed framework and blue (∙) for node-only approach. Lines denote the continuous-time control input and state trajectory constructed using the solution from prox-linear method: black (-) for proposed framework and blue (-) for node-only approach. (We simulate/integrate (<ref>) with the parameterized control input to obtain the state trajectory.) Red lines (-) denote the constraint bounds. Inter-sample violation in the solutions from the node-only approach is highlighted with magnified inset.




 §.§ Dynamic Obstacle Avoidance






We consider two scenarios: one where the obstacles exhibit periodic motion and the other where they are static. Figure <ref> shows the magnitude of control input (acceleration) in the case with dynamic obstacles obtained with the proposed framework. An animation of the corresponding position trajectory is provided in the code repository. A solution from the node-only approach is not shown in Figure <ref>, since it fails to provide a physically meaningful solution on the same discretization grid due to aliasing in the obstacle motion. Due to the time variation in the obstacle positions, a dense discretization grid is required with the node-only approach. However, the number of distinct obstacle avoidance constraints imposed in the node-only approach grows with the discretization grid size and the number of obstacles, which is computationally expensive. On the other hand, the number of constraints imposed in the proposed framework only depends on the discretization grid size.

Figure <ref> shows the percentage of violation in the obstacle avoidance constraint by the simulated state trajectories obtained for different values of the constraint relaxation tolerance ϵ. As long as the constraint functions are well-scaled, a state trajectory with physically insignificant continuous-time constraint violation can be obtained by picking ϵ to be several orders of magnitude greater than machine precision, which is essential for reliable numerical performance. In practice, picking ϵ close to 10^-4 ensures that the continuous-time constraint violation does not exceed 1%. 



To compare the proposed and the node-only approaches, we turn to the case of static obstacles: Figure <ref> shows the position and Figure <ref> shows the acceleration magnitude. Note that the acceleration magnitude in the solution from the node-only approach is smaller than the lower bound (strictly smaller between the discretization nodes). As a result, the terminal state cost from the node-only approach, 5.06, is significantly smaller than that from the proposed framework, 47.91—the node-only approach optimizes the cost at the expense of inter-sample constraint violation.






 §.§ 6-DoF Rocket Landing




Figure <ref> shows the position of the 6-DoF rocket along with the attitude of its body-axis (in green) and the thrust vector (as an orange-yellow plume). Figure <ref> shows the thrust magnitude and Figure <ref> shows the tilt of the body axis—both of which show inter-sample violation with the node-only approach.



Similar to the obstacle avoidance example, the terminal state cost with the node-only approach, 170.1 kg, is smaller than that from the proposed framework, 180.5 kg.





 §.§ 3-DoF Rocket Landing (Lossless Convexification)




This example demonstrates the application of the proposed framework for solving a convex optimal control problem. The node-only approach, in this case, amounts to solving a single convex problem where all constraints are imposed via canonical convex cones in a convex optimization solver; we use ECOS <cit.>. 

The solution from the node-only approach is used to warm-start the proposed framework, which then “cleans up” the inter-sample constraint violations. As with the previous examples, we see that the trajectory cost (in this case, fuel consumption) with the node-only approach, 351 kg, is smaller than that from the proposed framework, 352.4 kg. Figure <ref> shows the position of the point-mass rocket along with the thrust vector (as an orange-yellow plume). The glideslope constraint shows inter-sample violation with the node-only approach. Figure <ref> confirms that lossless convexification holds for the solutions from both methods—proposed on the left and node-only on the right. The thrust lower bound constraint shows inter-sample violation with the node-only approach, since a zero-order-hold (ZOH) parameterization is used for the mass-normalized thrust. 

The node-only approach requires a dense discretization grid to ensure validity of the lossless convexification result <cit.>, which was proven for the continuous-time problem (prior to discretization). The proposed framework, in contrast, provides a valid solution despite using a sparse discretization grid. Future work will examine whether the continuous-time lossless convexification result can apply directly to (<ref>).





 §.§ Real-Time Performance


We provide real-time performance statistics for solving the two nonconvex examples in Table <ref>. These results were obtained by executing pure C code generated using the  software. The binary executables are provided in the code repository.




We note that these solve-times are on the same order-of-magnitude as the solve-times reported in the literature for real-time powered-descent guidance <cit.> and real-time quadrotor trajectory optimization <cit.>, and thus demonstrate that the proposed framework is amenable to online trajectory optimization for onboard/embedded applications.






§ CONCLUSIONS


We propose a novel trajectory optimization method that ensures continuous-time constraint satisfaction, guarantees convergence, and is real-time capable. The approach leverages an SCP algorithm called the prox-linear method along with ℓ_1 exact penalization. When the optimal control problem is convex, we show stronger convergence property of the prox-linear method.

Future work will consider assumptions weaker than LICQ for exact penalty to hold, with the goal of developing a solution method that directly handles (<ref>) without relaxation, and also releasing , the in-house-developed code-generation software for general-purpose real-time trajectory optimization that was used to generate pure C code for the examples considered in the paper.


The authors gratefully acknowledge Mehran Mesbahi, Skye Mceowen, and Govind M. Chari for helpful discussions and their feedback on the initial draft of the paper.
  




unsrturl      







§ APPENDIX







§ PROOF OF THEOREM <REF>


As a consequence of the relaxation in (<ref>), we wish to bound g_i and |h_j| on the interval [t_k, t_k+1], for any i=1,…, and j=1,…,.


Time derivative of path constraint functions g_i and h_j are bounded almost everywhere on the state trajectory x and control input u.



Note that the augmented control input parameterization (<ref>) is a piecewise polynomial, which is differentiable almost everywhere in [0,1]. Then, due to (<ref>) and Remark <ref>, u(t) and s(τ) are bounded for t∈, τ∈[0,1], and u̇(t) = u(t(τ))/s(τ) is well-defined and bounded æ τ∈[0,1]. The state trajectory x is bounded due to Assumption <ref>. Further, Δ t_k, for k=1,…,N-1, is bounded. Consequently, f, g_i□, and h_j□ are bounded, where □ = t, x, and u. Then, through the chain-rule

    □̇ =   □t + □xẋ + □uu̇
 
with □=g_i, h_j, we infer that ġ_i and ḣ_j are bounded almost everywhere.


We denote the upper bounds on the absolute values of ġ_i and ḣ_j (which hold almost everywhere) with ω_g_i and ω_h_j, respectively.

When (<ref>) and the penultimate row of (<ref>) are satisfied, for any k=1,…,N-1, we have that 

    ∫_τ_k^τ_k+1 s(τ) ^⊤g(t(τ),x(t(τ)),u(t(τ)))^2
             +s(τ) ^⊤ h(t(τ),x(t(τ)),u(t(τ)))^2 dτ≤ϵ
       ∫_t_k^t_k+1^⊤g(t,x(t),u(t))^2+^⊤ h(t,x(t),u(t))^2 dt ≤ϵ
       ∫_t_k^t_k+1g_i(t,x(t),u(t))^2dt ≤ϵ
       ∫_t_k^t_k+1h_j(t,x(t),u(t))^2dt ≤ϵ

for any i=1,…,n_g and j=1,…,n_h, where we use the change of variable t(τ) = t_k + ∫^τ_τ_ks(θ)dθ, for τ∈[τ_k,τ_k+1].



First, we examine the pointwise bound on h_j. We denote h_j(t,x(t),u(t)) compactly with h_j[t], and let ĥ_j denote the maximum absolute value of h_j within the interval [t_k, t_k+1]. We assume, without loss of generality, that ĥ_j is attained at t_max∈ [t_k, t_k + 0.5Δ t_k]. Next, we approximate the area under h_j[t]^2, for t∈[t_k,t_k+1], via an auxiliary function η:[t_max,t_k+1] →  given by


    η(t) = ĥ_j^2 - 2 ω_h_jĥ_j(t-t_max)
 

Note that, for t∈[t_max,t_k+1], we have  













    h_j[t]^2 - ĥ^2_j + 2ω_h_jĥ_j(t-t_max)
    ≥    h_j[t]^2 - ĥ^2_j + 2ĥ_j(ĥ_j-h_j[t])
    
       =    h_j[t]^2 + ĥ^2_j - 2ĥ_jh_j[t] = (h_j[t] - ĥ_j)^2
    ≥    0

Therefore, h_j[t]^2 - η(t) is positive for t > t_max and  h_j[t_max]^2 = η (t_max). If ĥ_j≤Δ t_k ω_h_j, we have that η(t_max^') = 0, where t_max^' = t_max + 0.5Δ t_k. The area under η(t), for t∈ [t_max, t_k+1], is triangular (Figure <ref>). Then, 



    ϵ≥∫_t_k^t_k+1 h_j[t]^2 dt ≥∫_t_max^t_k+1η(t)dt 
       =∫_t_max^t_max +ĥ_j/2ω_h_jĥ_j^2-2ω_h_jĥ_j(t-t_max)dt
       =∫_0^ĥ_j/2ω_h_jĥ_j^2-2ω_h_jĥ_jθ dθ = ĥ_j^3/4ω_h_j



Conversely, if ĥ_j > Δ t_k ω_h_j, we have that η(t_max^') > 0. The area under η(t) within [t_max,t_max^'] is trapezoidal (Figure <ref>). Then,



    ϵ≥∫_t_k^t_k+1 h_j[t]^2dt ≥∫_t_max^t_k+1η(t)dt
       >∫_t_max^t_max+Δ t_k/2ĥ_j^2-2ω_h_jĥ_j(t-t_max)dt
       =∫_0^Δ t_k/2ĥ_j^2 - 2 ω_h_jĥ_jθ dθ
       = ĥ^2_jΔ t_k/2 - ω_h_jĥ_jΔ t_k^2/4 >ĥ_j^2 Δ t_k/4 > ω_h_j^2Δ t_min^3/4


where Δ t_min > 0 is the lower bound for Δ t_k; it exists because the dilation factor is positive and τ_k+1-τ_k>0, for k=1,…,N-1. Note that ĥ_j > Δ t_k ω_h_j holds only if 

    ϵ > ω_h_j^2Δ t_min^3/4
 
This case can be ignored in practice since we are interested in small ϵ which is physically insignificant. Hence, from (<ref>), we have 

    ĥ_j≤δ_h_j(ϵ) = (4ϵω_h_j)^1/3  if  ϵ≤ω_h_j^2Δ t_min^3/4

for j=1,…,n_h. We can similarly bound the maximum value of g_i, denoted by ĝ_i, by approximating the area under g_i(t,x(t),u(t))^2, for t∈[t_k,t_k+1], to obtain 

    ĝ_i≤δ_g_i(ϵ) = (4ϵω_g_i)^1/3  if  ϵ≤ω_g_i^2Δ t_min^3/4

for i=1,…,n_g. 




§ CONTROL INPUT PARAMETERIZATION


Let U = (u_1,…,u_N_u) with u_k∈, for k=1,…,N_u. Then, according to (<ref>), the control input is given by

    u(τ) = ∑_k=1^N_uΓ_u^k(τ)u_k

for τ∈[0,1].




 §.§ First-Order-Hold

Choose N_u = N and  define 


    Γ_u^1(τ) =   {[  τ_2-τ/τ_2-τ_1 if τ∈[τ_1,τ_2];              0      otherwise ].
    Γ_u^k(τ) =   {[ τ-τ_k-1/τ_k-τ_k-1  if τ∈[τ_k-1,τ_k]; τ_k+1-τ/τ_k+1-τ_k  if τ∈[τ_k,τ_k+1];                 0         otherwise ].  k=2,…,N-1
    Γ_u^N(τ) =   {[ τ-τ_N-1/τ_N-τ_N-1  if τ∈[τ_N-1,τ_N];                 0         otherwise ].


In other words,


    u(τ) = (τ_k+1-τ/τ_k+1-τ_k)u_k + (τ-τ_k/τ_k+1-τ_k)u_k+1


whenever τ∈[τ_k,τ_k+1] for some k=1,…,N-1. Note that Γ_u^k(τ_j) = δ_jk is the Kronecker delta, for k,j=1,…,N. As a result, u(τ_k) = u_k, for k=1…,N. 




 §.§ Pseudospectral Methods

In pseudospectral methods, the control input is parameterized with a basis of orthogonal polynomials, i.e.,


    Γ_u^k(τ) = ∏_0pt1j=1j k^N_u2τ-1 - η_j/η_k-η_j


is the Lagrange interpolating polynomial <cit.> of degree N_u-1, where η_k∈[-1,1], for k=1,…,N_u, are related to the roots of orthogonal polynomials (such as members of the Jacobi family <cit.>). For e.g., choosing η_k to be the roots of the polynomial


    η↦ (1-η^2)T_N_u-1(η)η


would correspond to Chebyshev-Gauss-Lobatto (CGL) collocation <cit.>, where T_M:[-1,1] → defined by η↦cos(Marccos (η) ) is the Chebyshev polynomial of degree M <cit.>. The choice of basis functions (<ref>) ensures that Γ_u^k(η_j+1/2) = δ_jk, for each k,j=1,…,N_u. As a result, u(η_k+1/2) = u_k, for k=1,…,N_u.




§ DYNAMIC OBSTACLE AVOIDANCE


We consider a two-dimensional free-final-time path planning problem. The dynamical system describing the vehicle comprises of position r∈^2, velocity v∈^2, cumulative cost p̌∈, and acceleration input u∈^2. The state x∈^5, augmented state x̃∈^7, and augmented control input ũ∈^3 are given by



    x =    (r,v,p̌)
    x̃ =    (x,y,t)
    ũ =    (u,s)



We choose a first-order-hold (see Appendix <ref>) parameterization for the augmented control input. 

The augmented system is given by


    x̃ =   [[            v;    u - c_dvv;          u^2; ^⊤g(t,x,u)^2;            1 ]]s


where c_d∈_+ is the drag coefficient. The vehicle is subject to the following inequality path constraints: avoidance of =10 moving elliptical obstacles, speed upper bound, and acceleration upper- and lower bounds, which are encoded in the path constraint function g as follows


    g(t, x,u) =   [ [ 1 - Ȟ_1(r-q̌_1(t))^2;                    ⋮;   1 - Ȟ_(r-q̌_(t))^2;        v^2 - v^2_max;        u^2 - u^2_max;        u_min^2 - u^2;                      ]]


The shape matrix and center of the ith obstacle at time t are Ȟ_i and q̌_i(t), respectively, for i=1,…,. A sinusoidal motion is prescribed for the centers of the obstacles, i.e.,


    q̌_i(t) = ( ψ_i + δψ_i sin(θ_i t + ∠ψ_i ),0 )


with amplitude δψ_i, phase angle ∠ψ_i, and frequency θ_i, about the nominal center ψ_i. The speed upper bound and the acceleration upper- and lower bounds are v_max, u_max, and u_min, respectively. 
The boundary conditions are specified through function Q as follows


    Q(,x(),,x())  =   [     r() -; v() - v_i;      p̌();     r() -; v() - v_f ]


where  and v_i are the initial position and velocity, respectively, and  and v_f are the final position and velocity, respectively. We set the terminal state cost function to L(,x()) = p̌(). Constraint functions h and P are not utilized in this example.

Table <ref> shows the parameter values chosen for the system and the algorithm to generate the results in Section <ref>.  In addition, we choose the following parameter values for the sinusoidal motion given by (<ref>)


    ψ =   [ [  34 -32  42 -24  34 -32  42 -24  34 -32;  20  20  10  10   0   0 -10 -10 -20 -20 ]]
    δψ =    (10) 1×
    ∠ψ =   π/2[ 1 1 0 0 1 1 0 0 1 1 ]
    θ =   (π/20)1×


where □_i is the ith column of □, for □ = ψ,δψ,∠ψ,θ. The scenario with static obstacles in Section <ref> is generated by setting δψ = 1×.






§ 6-DOF ROCKET LANDING


We consider a free-final-time 6-DoF lunar landing scenario based on the formulation given in <cit.>, and adopt the notation therein. The state x∈^14, control input u∈^3, augmented state x̃∈^15, and augmented control input ũ∈^4 are given by



    x =    (m,r_I,v_I,q_B←I,ω_B)
    
    u =   T_B
    x̃ =    (x,y)
    ũ =    (T_B,s)



We choose a first-order-hold (see Appendix <ref>) parameterization for the augmented control input. 

The augmented dynamical system is given by


    x̃ = [                           -α̌T_B;                              v_I;         1/mC_I←B(q_B←I)T_B + g_I;                   1/2Ω(ω_B)q_B←I; J_B^-1( r_T,B×T_B - ω_B× J_Bω_B);                       ^⊤g(t,x,u) ]s


The path constraint function g is given by


    g(t,x,u) =   [                    m_dry - m; γ_gs H_γr_I^2 - (e_1^⊤r_I)^2;                    -e_1^⊤r_I;              v_I^2 - v_max^2; 2H_θq_B←I^2 - (cosθ_max-1)^2;              ω_B^2 - ω_max^2; cosδ_maxT_B^2 - (e_1^⊤T_B)^2;                    -e_1^⊤T_B;              T_B^2 - T_max^2;              T_min^2 - T_B^2 ]


where the second-order-cone constraints on r_I and T_B are equivalently reformulated to quadratic forms to ensure continuous differentiability <cit.>. The boundary conditions are specified through function Q as follows


    Q(,x(),,x()) =   [    m() - m_wet;    r_I() - r_i;    v_I() - v_i;          ω_B();    r_I() - r_f;    v_I() - v_f; q_B←I() - q_id;          ω_B();                ]


where r_i and v_i are the initial position and velocity, respectively, r_f and v_f are the final position and velocity, respectively, and q_id is the unit quaternion. The terminal state cost function is set to L(,x()) = -m(). Constraint functions h and P are not utilized in this example.

Table <ref> shows the parameter values chosen for the system and the algorithm to generate the results in Section <ref>. The definitions of H_γ and H_θ are taken from <cit.>.






§ 3-DOF ROCKET LANDING


We consider a convex, fixed-final-time Mars landing scenario based on the dynamical system and constraints in <cit.>. The state x∈^7 and control input u∈^4 are given by 


    x =    (r,v,z)
    
        u =    (Ť,σ̌)


where m = exp z is the vehicle mass, T = mŤ is the thrust vector, and σ̌ is an auxiliary control input (slack variable) introduced as a part of the lossless convexification procedure. We choose a zero-order-hold parameterization for the control input. 

We consider the following linear dynamical system


    ẋ =   [              v; Ť + (0,0,-g_m);          -α̌σ̌ ]
     
        =   [ 3× 3    3 3× 1;      3× 7     ;      1× 7      ]_Ax + [                     3× 4; [    3 3× 1; 1× 3  -α̌ ] ]_B u + [ 5× 1; -g_m;    0 ]_w


The path constraint function g is given by 


    g(t,x,u) =
       [                   γ_gsE_12r^2 - (e_3^⊤ r)^2;                                  -(e_3^⊤ r);                               v^2 - v_max^2;                                  z - z_1(t);                  -z + max{log m_dry,z_0(t)};                          -e_3^⊤Ť+σ̌cosθ_max;                                  Ť^2 - σ̌^2;                                         -σ̌; μ_min(t)(1-(z-z_0(t))+1/2(z-z_0(t))^2) - σ̌;                -μ_max(t)(1-(z-z_0(t))) + σ̌ ]


where E_12 = [2 2× 1], e_3 = (0,0,1), and 



    z_0(t) =   log (m_wet - α̌T_maxt)
    
        z_1(t) =   log (m_wet - α̌T_mint)
    μ_min(t) =    T_minexp(-z_0(t))
    μ_max(t) =    T_maxexp(-z_0(t))
    


Lossless convexification holds if T(t) = σ(t) = m(t)σ̌(t) æ t∈.

The boundary conditions are specified through function Q as follows


    Q(,x(),,x()) =[           r() -;       v() - v_i; z() - log m_wet;           r() -;       v() - v_f ]


where r_i and v_i are the initial position and velocity, respectively, and r_f and v_f are the final position and velocity, respectively. The terminal state cost function is set to L(,x()) = -z(). The running cost function Y, and constraint functions h and P, are not utilized in this example.

Table <ref> shows the parameter values chosen for the system and the algorithm to generate the results in Section <ref>.





