This paper was converted on www.awesomepapers.org from LaTeX by an anonymous user.
Want to know more? Visit the Converter page.

Minimum-Time Trajectory Optimization With
Data-Based Models: A Linear Programming Approach

Nan Li [email protected]    Ehsan Taheri [email protected]    Ilya Kolmanovsky [email protected]    Dimitar Filev [email protected] Department of Aerospace Engineering, Auburn University, Auburn, AL, USA Department of Aerospace Engineering, University of Michigan, Ann Arbor, MI, USA Hagler Institute for Advanced Study, Texas A&M University, College Station, TX, USA
Abstract

In this paper, we develop a computationally-efficient approach to minimum-time trajectory optimization using input-output data-based models, to produce an end-to-end data-to-control solution to time-optimal planning/control of dynamic systems and hence facilitate their autonomous operation. The approach integrates a non-parametric data-based model for trajectory prediction and a continuous optimization formulation based on an exponential weighting scheme for minimum-time trajectory planning. The optimization problem in its final form is a linear program and is easy to solve. We validate the approach and illustrate its application with a spacecraft relative motion planning problem.

keywords:
Trajectory Optimization, Time-Optimal Control, Data-Driven Control, Linear Programming
thanks: This paper was not presented at any IFAC meeting. Corresponding author N. Li. Email: [email protected]

, , ,

1 Introduction

Minimum-time trajectory optimization (also known as “time-optimal control”) is frequently involved in robot path planning and tracking (Lepetič et al., 2003; Verscheure et al., 2009), space mission design and operation (Shirazi et al., 2018), and other disciplines wherever a task is desired to be accomplished in a least amount of time, typically subject to limited resources. Due to its significant relevance, the investigation into this topic has been extensive over decades (Kalman and Bertram, 1959; LaSalle, 1959; O’Reilly, 1981).

The formulation of a minimum-time trajectory optimization problem can be either in continuous time or in discrete time. In continuous time, solution techniques are usually based on indirect methods (also known as “variational methods”): One first applies Pontryagin’s maximum principle to reduce the trajectory optimization problem to a two-point boundary value problem (TPBVP), and then solves the TPBVP using a shooting method (Trélat, 2012; Taheri et al., 2017). In discrete time, in contrast, direct methods are more often considered. Various direct methods for minimum-time trajectory optimization in discrete time have been proposed in the literature: In the approach of Carvallo et al. (1990), a minimum-time problem is reformulated into a mixed-integer program, where a set of Boolean variables are used to indicate if a given target condition is reached at each time step over the planning horizon. In the approaches of Van den Broeck et al. (2011) and Zhang et al. (2014), a minimum-time problem is solved using a bi-level algorithm, where the lower level solves a fixed-horizon trajectory planning problem treating the target condition as a terminal constraint and the upper level adjusts the planning horizon of the lower-level problem to find the minimum horizon length such that the lower-level problem admits a feasible solution. In the approaches of Rösmann et al. (2015) and Wang et al. (2017), the time is scaled and the scaling factor is treated as an optimization variable. This way, minimizing the final time of the original free-final-time problem is achieved through minimizing the scaling factor in a related fixed-final-time problem. However, in such an approach based on time scaling, the optimization problem is necessarily nonlinear and nonconvex, even for linear systems. In the approach of Verschueren et al. (2017), the deviation from the target condition is penalized with a weight that increases exponentially with time. It is shown that a minimum-time trajectory solution can be obtained if the weight parameter is chosen to be sufficiently high. The approaches reviewed above typically use a state-space model of the considered dynamic system to predict the trajectories. A common approach to obtaining a state-space model, to be used for trajectory optimization, is to first derive the model from first principles and then calibrate its parameters according to prior knowledge and/or data of the system.

Recently, a non-parametric modeling approach based on behavioral systems theory is gaining attention due to its unique applicability to emerging data-driven paradigms (Markovsky and Dörfler, 2021). This approach uses input-output time-series data of a given system to build up a (non-parametric) predictive model, which is referred to as a data-based model in this paper, rather than using the data to identify/calibrate a (parametric) state-space model. The integration of such data-based models into predictive control algorithms has been investigated by various researchers, e.g., Coulson et al. (2019, 2021); Berberich et al. (2020); Baros et al. (2022); Huang et al. (2023), and has demonstrated superior performance than conventional control methods in a number of applications (Elokda et al., 2021; Huang et al., 2021; Chinde et al., 2022). Along with bypassing the step of state-space model identification/calibration, such a predictive control algorithm using an input-output data-based model determines control input values directly based on output measurements and does not require state estimation using an observer. Therefore, it provides an “end-to-end” data-to-control solution, which may be simpler to implement from the point of view of practitioners and may facilitate fully autonomous operation of future intelligent systems.

In the above context, the goal of this paper is to develop an approach to minimum-time trajectory optimization based on input-output data-based models, to produce an end-to-end data-to-control solution to many time-optimal planning and control problems in robotics, aerospace, and other disciplines. To the best of our knowledge, there has been no previous work addressing this topic. We focus on linear time-invariant systems, for which we adopt the approach based on behavioral systems theory to build up a non-parametric data-based model for trajectory prediction. We then exploit an exponential weighting scheme extended from the one of Verschueren et al. (2017) to solve for minimum-time trajectories. We show that the optimization problem in its final form is a linear program and hence is easy to solve. Main contributions of this paper are as follows:

  • We develop an approach to minimum-time trajectory optimization based on input-output data-based models, which is the first such approach in the literature.

  • We extend the trajectory prediction method in previous predictive control algorithms using data-based models (e.g., the ones in Coulson et al. (2019, 2021); Berberich et al. (2020); Baros et al. (2022); Huang et al. (2023)) to enable predicting trajectories over an extended planning horizon without relying on a high-dimensional model. This extension is especially relevant to trajectory optimization because a long planning horizon is not rare in a trajectory optimization setting. Our method is inspired by the multiple shooting method (Trélat, 2012).

  • Using an exponential weighting scheme extended from that of Verschueren et al. (2017), we formulate the minimum-time trajectory optimization problem into a linear program, which is easy to solve. We prove that minimum-time trajectories can be obtained with the linear program if a weight parameter is chosen to be sufficiently high. In particular, we make the following extensions to the exponential weighting scheme: 1) The scheme was used in Verschueren et al. (2017) for minimum-time trajectory planning based on a state-space model; we extend the scheme to apply it to minimum-time trajectory planning based on an input-output data-based model. 2) The scheme was used in Verschueren et al. (2017) for point-to-point trajectory planning; we extend the scheme to more general point-to-set trajectory planning. To enable these extensions, different assumptions are made, and, correspondingly, new proofs are developed to show the ability of this exponential weighting scheme to produce minimum-time trajectory solutions.

  • We validate the developed approach to minimum-time trajectory optimization and illustrate its application with an aerospace example.

Organization: The minimum-time trajectory optimization problem addressed in this paper is described in Section 2. We introduce the method using a non-parametric input-output data-based model to predict trajectories over an extended planning horizon in Section 3. We reformulate the minimum-time trajectory optimization problem into a linear program based on an exponential weighting scheme and prove its theoretical properties in Section 4. A spacecraft relative motion planning problem is considered in Section 5 to illustrate the approach. The paper is concluded in Section 6.

Notations: The symbol \mathbb{R} denotes the set of real numbers and \mathbb{Z} the set of integers; n\mathbb{R}^{n} denotes the set of nn-dimensional real vectors, n×m\mathbb{R}^{n\times m} the set of nn-by-mm real matrices, and a\mathbb{Z}_{\geq a} the set of integers that are greater than or equal to aa; InI_{n} denotes the nn-dimensional identity matrix, 0n,m0_{n,m} the nn-by-mm zero matrix, and 1n1_{n} the nn-dimensional column vector of ones. Given multiple vectors vknkv_{k}\in\mathbb{R}^{n_{k}} or matrices with the same number of columns Mknk×mM_{k}\in\mathbb{R}^{n_{k}\times m}, k=1,,Kk=1,...,K, the operator col()\text{col}(\cdot) stack them on top of one another, i.e., col(v1,,vK)=[v1,,vK]\text{col}(v_{1},...,v_{K})=[v_{1}^{\top},...,v_{K}^{\top}]^{\top} and col(M1,,MK)=[M1,,MK]\text{col}(M_{1},...,M_{K})=[M_{1}^{\top},...,M_{K}^{\top}]^{\top}. For a discrete-time signal z():nz(\cdot):\mathbb{Z}\to\mathbb{R}^{n}, we use 𝐳[a:b]{\bf z}_{[a:b]}, with a,ba,b\in\mathbb{Z} and aba\leq b, to denote col(z(a),,z(b))\text{col}(z(a),...,z(b)). We call both the sequence z(a),,z(b)z(a),...,z(b) and the column vector 𝐳[a:b]=col(z(a),,z(b)){\bf z}_{[a:b]}=\text{col}(z(a),...,z(b)) a trajectory (of length l=ba+1l=b-a+1).

2 Problem Statement

We study trajectory optimization problems associated with finite-dimensional linear time-invariant systems which can be represented in state-space form as

x(t+1)\displaystyle x(t+1) =Ax(t)+Bu(t)\displaystyle=Ax(t)+Bu(t) (1a)
y(t)\displaystyle y(t) =Cx(t)+Du(t)\displaystyle=Cx(t)+Du(t) (1b)

where tt\in\mathbb{Z} denotes the discrete time step, x(t)nx(t)\in\mathbb{R}^{n} represents the system state at time tt, u(t)mu(t)\in\mathbb{R}^{m} is the control input, y(t)py(t)\in\mathbb{R}^{p} is the output, and AA, BB, CC and DD are matrices of appropriate dimensions. We make the following assumption about the system:

Assumption 1: The system is controllable and observable.

Given a state-space model (1) of the system, we can write the following equation that relates an input trajectory of length ll, l1l\in\mathbb{Z}_{\geq 1}, to its corresponding output trajectory:

𝐲[0:l1]=𝒪lx(0)+𝒞l𝐮[0:l1]{\bf y}_{[0:l-1]}=\mathcal{O}_{l}x(0)+\mathcal{C}_{l}{\bf u}_{[0:l-1]} (2)

where x(0)x(0) is the system state at the initial time of the trajectory, and 𝒪l\mathcal{O}_{l} and 𝒞l\mathcal{C}_{l} are matrices defined as follows:

𝒪l=[CCACAl1]𝒞l=[DCBDCAl2BCBD]\mathcal{O}_{l}=\begin{bmatrix}C\\ CA\\ \vdots\\ CA^{l-1}\end{bmatrix}\quad\mathcal{C}_{l}=\begin{bmatrix}D&&&\\ CB&D&&&\\ \vdots&\ddots&\ddots&\\ CA^{l-2}B&\cdots&CB&D\end{bmatrix} (3)

The smallest integer ll such that the matrix 𝒪l\mathcal{O}_{l} defined above has full rank is called the lag of the system and denoted as lminl_{\min}. Under Assumption 1, lminl_{\min} exists and satisfies 1lminn1\leq l_{\min}\leq n.

Given an arbitrary pair of input trajectory 𝐮[0:l1]{\bf u}_{[0:l-1]} and output trajectory 𝐲[0:l1]{\bf y}_{[0:l-1]}, both of length ll, if (2) holds for some x(0)nx(0)\in\mathbb{R}^{n}, then the pair (𝐮[0:l1],𝐲[0:l1])({\bf u}_{[0:l-1]},{\bf y}_{[0:l-1]}) is called admissible by the system. In particular, an admissible pair of input-output trajectories of length ll, (𝐮[0:l1],𝐲[0:l1])({\bf u}_{[0:l-1]},{\bf y}_{[0:l-1]}), with llminl\geq l_{\min}, corresponds to a unique initial state x(0)x(0), which can be determined according to (2) as follows:

x(0)=𝒪l(𝐲[0:l1]𝒞l𝐮[0:l1])x(0)=\mathcal{O}_{l}^{\dagger}({\bf y}_{[0:l-1]}-\mathcal{C}_{l}{\bf u}_{[0:l-1]}) (4)

where ()(\cdot)^{\dagger} denotes the Moore-Penrose pseudoinverse.

In this paper, we focus on minimum-time trajectory optimization problems given in the following form:

minu(),y(),T0\displaystyle\min_{u(\cdot),y(\cdot),T\geq 0} T\displaystyle\quad T (5a)
s.t. (𝐮[Ki:1],𝐲[Ki:1])=(𝐮i,𝐲i)\displaystyle\quad({\bf u}_{[-K_{i}:-1]},{\bf y}_{[-K_{i}:-1]})=({\bf u}_{i},{\bf y}_{i}) (5b)
𝐲[T:T+Kf1]Yf\displaystyle\quad{\bf y}_{[T:T+K_{f}-1]}\in Y_{f} (5c)
c(u(t),y(t))0,t=0,,T+Kf1\displaystyle\quad c(u(t),y(t))\leq 0,\quad t=0,...,T+K_{f}-1 (5d)

where KilminK_{i}\geq l_{\min}; 𝐮imKi{\bf u}_{i}\in\mathbb{R}^{mK_{i}} and 𝐲ipKi{\bf y}_{i}\in\mathbb{R}^{pK_{i}} represent given initial conditions for the input and output trajectories; Kf1K_{f}\geq 1; YfpKfY_{f}\subset\mathbb{R}^{pK_{f}} represents a target set for the output trajectory; and c(u(t),y(t))0c(u(t),y(t))\leq 0 represents prescribed path constraints for the trajectory to satisfy. The goal represented by the cost function in (5a) and the constraint in (5c) is to drive the output trajectory to reach the target set YfY_{f} in the minimum time, starting with the initial condition in (5b), while satisfying the path constraints in (5d). We first make the following assumption about the initial condition (𝐮i,𝐲i)({\bf u}_{i},{\bf y}_{i}):

Assumption 2: The pair (𝐮i,𝐲i)({\bf u}_{i},{\bf y}_{i}) represents an admissible pair of input-output trajectories (of length KiK_{i}) that satisfies the path constraints c(u(t),y(t))0c(u(t),y(t))\leq 0 at all times.

Assumption 2 is reasonable because 𝐮i{\bf u}_{i} and 𝐲i{\bf y}_{i}, according to (5b), represent the input and output trajectories of the system over the past KiK_{i} time steps, and hence, the pair (𝐮i,𝐲i)({\bf u}_{i},{\bf y}_{i}) is supposed to be admissible and satisfies any path constraints. Under Assumption 2, given a state-space model (1) of the system, (5b) corresponds to the following initial condition for the state:

x(0)=AKi𝒪Ki(𝐲i𝒞Ki𝐮i)+[AKi1BABB]𝐮ix(0)=A^{K_{i}}\mathcal{O}_{K_{i}}^{\dagger}({\bf y}_{i}-\mathcal{C}_{K_{i}}{\bf u}_{i})+\begin{bmatrix}A^{K_{i}-1}B&\cdots&AB&B\end{bmatrix}{\bf u}_{i} (6)

where 𝒪Ki\mathcal{O}_{K_{i}} and 𝒞Ki\mathcal{C}_{K_{i}} are the matrices defined in (3) with l=Kil=K_{i}.

We consider polyhedral target set YfpKfY_{f}\subset\mathbb{R}^{pK_{f}} and linear-inequality path constraints c(u(t),y(t))0c(u(t),y(t))\leq 0, i.e., they can be written as:

Yf={𝐲fpKf:G𝐲fg,H𝐲f=h}\displaystyle Y_{f}=\{{\bf y}_{f}\in\mathbb{R}^{pK_{f}}:G{\bf y}_{f}\leq g,H{\bf y}_{f}=h\} (7a)
c(u(t),y(t))=[SuSy][u(t)y(t)]s0\displaystyle c(u(t),y(t))=\begin{bmatrix}S_{u}&S_{y}\end{bmatrix}\begin{bmatrix}u(t)\\ y(t)\end{bmatrix}-s\leq 0 (7b)

where G,H,Su,SyG,H,S_{u},S_{y} and g,h,sg,h,s are matrices and vectors of compatible dimensions. Furthermore, we make the following “controlled invariance” assumption about YfY_{f}:

Assumption 3: Let K¯f=max(Kf,lmin)\bar{K}_{f}=\max(K_{f},l_{\min}). For any admissible pair of input-output trajectories of length K¯f\bar{K}_{f}, (𝐮[0:K¯f1],𝐲[0:K¯f1])({\bf u}_{[0:\bar{K}_{f}-1]},{\bf y}_{[0:\bar{K}_{f}-1]}), that satisfies the path constraints c(u(t),y(t))0c(u(t),y(t))\leq 0 for t=0,,K¯f1t=0,...,\bar{K}_{f}-1 and the target condition 𝐲[K¯fKf:K¯f1]Yf{\bf y}_{[\bar{K}_{f}-K_{f}:\bar{K}_{f}-1]}\in Y_{f}, there exist an input u(K¯f)u(\bar{K}_{f}) and its corresponding output y(K¯f)y(\bar{K}_{f}) such that they satisfy c(u(K¯f),y(K¯f))0c(u(\bar{K}_{f}),y(\bar{K}_{f}))\leq 0 and 𝐲[K¯fKf+1:K¯f]Yf{\bf y}_{[\bar{K}_{f}-K_{f}+1:\bar{K}_{f}]}\in Y_{f}.

In Assumption 3, because K¯flmin\bar{K}_{f}\geq l_{\min}, an admissible pair of input-output trajectories (𝐮[0:K¯f1],𝐲[0:K¯f1])({\bf u}_{[0:\bar{K}_{f}-1]},{\bf y}_{[0:\bar{K}_{f}-1]}) corresponds to a unique initial state x(0)x(0) and a unique state trajectory x(0),x(1),,x(K¯f)x(0),x(1),...,x(\bar{K}_{f}). Hence, for an input u(K¯f)u(\bar{K}_{f}), the corresponding output y(K¯f)y(\bar{K}_{f}) is also unique. Assumption 3 means that for any pair of input-state trajectories the output trajectory of which enters the target set YfY_{f} while satisfying the path constraints, there exists a control to maintain the output trajectory in YfY_{f} while satisfying the path constraints. Hence, it specifies a “controlled invariance” property of YfY_{f} with respect to the system dynamics and the path constraints.

Remark 1: While in many conventional trajectory optimization problem formulations the initial condition is a given value xix_{i} for the state at the initial time t=0t=0, a given value for the output or a given pair of input-output at a single time is not sufficient for uniquely determining the trajectory. Therefore, we consider a pair of input-output trajectories of length KiK_{i} that satisfies KilminK_{i}\geq l_{\min} as the initial condition which uniquely determines the initial state according to (6) and hence the trajectory. For the terminal condition, we consider a target set YfY_{f} instead of a single point to represent a larger class of problems which includes a single target point as a special case.

Lastly, we assume that a state-space model of the considered system (i.e., the matrices (A,B,C,D)(A,B,C,D) in (1)) is not given and only input-output trajectory data of the system are available. For instance, we may deal with a real system that has uncertain parameters while can generate input-output data (see the example in Section 5). The goal of this paper is to develop a computationally-efficient approach to the minimum-time trajectory optimization problem (5) for such a setting. We note that although it is possible to first identify the matrices (A,B,C,D)(A,B,C,D) using input-output data and system identification techniques and then solve the problem (5) based on the identified state-space model, this two-step approach may be cumbersome from the point of view of practitioners and (A,B,C,D)(A,B,C,D) that is compatible with given data is in general not unique. Therefore, we pursue an end-to-end solution – directly from input-output data to a solution to (5).

3 Data-based Model for Long-term Trajectory Prediction

Assume that we have input-output trajectory data of length MM:

𝒟M={[ud(0)yd(0)],[ud(1)yd(1)],,[ud(M1)yd(M1)]}\mathcal{D}_{M}=\left\{\begin{bmatrix}u^{d}(0)\\ y^{d}(0)\end{bmatrix},\begin{bmatrix}u^{d}(1)\\ y^{d}(1)\end{bmatrix},...,\begin{bmatrix}u^{d}(M-1)\\ y^{d}(M-1)\end{bmatrix}\right\} (8)

where the superscript dd indicates “data.” Note that these input-output pairs (ud(t),yd(t))(u^{d}(t),y^{d}(t)), t=0,,M1t=0,...,M-1, should be sampled from a single trajectory111If the data are from multiple trajectories, then the inputs shall satisfy a collectively persistently exciting condition (van Waarde et al., 2020).. Construct the following Hankel data matrices:

L(ud)=[ud(0)ud(1)ud(ML)ud(1)ud(2)ud(ML+1)ud(L1)ud(L)ud(M1)]\displaystyle\!\mathcal{H}_{L}(u^{d})\!=\!\begin{bmatrix}u^{d}(0)&u^{d}(1)&\cdots&u^{d}(M-L)\\ u^{d}(1)&u^{d}(2)&\cdots&u^{d}(M-L+1)\\ \vdots&\vdots&\ddots&\vdots\\ u^{d}(L-1)&u^{d}(L)&\cdots&u^{d}(M-1)\end{bmatrix} (9a)
L(yd)=[yd(0)yd(1)yd(ML)yd(1)yd(2)yd(ML+1)yd(L1)yd(L)yd(M1)]\displaystyle\!\mathcal{H}_{L}(y^{d})\!=\!\begin{bmatrix}y^{d}(0)&y^{d}(1)&\cdots&y^{d}(M-L)\\ y^{d}(1)&y^{d}(2)&\cdots&y^{d}(M-L+1)\\ \vdots&\vdots&\ddots&\vdots\\ y^{d}(L-1)&y^{d}(L)&\cdots&y^{d}(M-1)\end{bmatrix} (9b)

where LL indicates the number of stacks of the signal udu^{d} or ydy^{d} in each column. The control input trajectory ud(0),ud(1),,ud(M1)u^{d}(0),u^{d}(1),...,u^{d}(M-1) is said to be persistently exciting of order LL if L(ud)\mathcal{H}_{L}(u^{d}) has full rank.

Our approach uses a result known as the fundamental lemma of Willems et al. (2005). The following lemma is an equivalent statement of Willems’ fundamental lemma in a state-space context (van Waarde et al., 2020):

Lemma 1: If the system (1) is controllable and the control input trajectory ud(0),ud(1),,ud(M1)u^{d}(0),u^{d}(1),...,u^{d}(M-1) is persistently exciting of order L+nL+n, then a pair of input-output trajectories of length LL, (𝐮[t:t+L1],𝐲[t:t+L1])({\bf u}_{[t:t+L-1]},{\bf y}_{[t:t+L-1]}), is admissible by the system (1) if and only if

[𝐮[t:t+L1]𝐲[t:t+L1]]=[L(ud)L(yd)]ζ\begin{bmatrix}{\bf u}_{[t:t+L-1]}\\ {\bf y}_{[t:t+L-1]}\end{bmatrix}=\begin{bmatrix}\mathcal{H}_{L}(u^{d})\\ \mathcal{H}_{L}(y^{d})\end{bmatrix}\zeta (10)

for some vector ζML+1\zeta\in\mathbb{R}^{M-L+1}.

Assume L>KiL>K_{i} and let t=Kit=-K_{i}. Then, (10) can be written as

[𝐮[Ki:1]𝐮[0:LKi1]𝐲[Ki:1]𝐲[0:LKi1]]=[L(ud)L(yd)]ζ\begin{bmatrix}{\bf u}_{[-K_{i}:-1]}\\ {\bf u}_{[0:L-K_{i}-1]}\\ {\bf y}_{[-K_{i}:-1]}\\ {\bf y}_{[0:L-K_{i}-1]}\end{bmatrix}=\begin{bmatrix}\mathcal{H}_{L}(u^{d})\\ \mathcal{H}_{L}(y^{d})\end{bmatrix}\zeta (11)

Given (𝐮[Ki:1],𝐲[Ki:1])=(𝐮i,𝐲i)({\bf u}_{[-K_{i}:-1]},{\bf y}_{[-K_{i}:-1]})=({\bf u}_{i},{\bf y}_{i}), (11) is a linear equation of variables (𝐮[0:LKi1],𝐲[0:LKi1],ζ)m(LKi)×p(LKi)×ML+1({\bf u}_{[0:L-K_{i}-1]},{\bf y}_{[0:L-K_{i}-1]},\zeta)\in\mathbb{R}^{m(L-K_{i})}\times\mathbb{R}^{p(L-K_{i})}\times\mathbb{R}^{M-L+1}. In Coulson et al. (2019), (11) is used as a model for predicting the outputs y(t)y(t) over the entire planning horizon t=0,,N1t=0,...,N-1. This requires L=N+KiL=N+K_{i}. When the planning horizon NN is large, which is not rare in a trajectory optimization setting, such a strategy requires high-dimensional data matrices (L(ud),L(yd))(\mathcal{H}_{L}(u^{d}),\mathcal{H}_{L}(y^{d})) and requires the control input trajectory ud(0),ud(1),,ud(M1)u^{d}(0),u^{d}(1),...,u^{d}(M-1) to be persistently exciting of an order of L+n=N+Ki+nL+n=N+K_{i}+n. Inspired by the multiple shooting method for trajectory optimization over an extended planning horizon (Trélat, 2012), we consider partitioning the entire trajectory of length N=K(LKi)N=K(L-K_{i}) into KK segments:

𝐮[0:N1]=[𝐮[0:LKi1]𝐮[LKi:2(LKi)1]𝐮[(K1)(LKi):N1]]\displaystyle{\bf u}_{[0:N-1]}=\begin{bmatrix}{\bf u}_{[0:L-K_{i}-1]}\\ {\bf u}_{[L-K_{i}:2(L-K_{i})-1]}\\ \vdots\\ {\bf u}_{[(K-1)(L-K_{i}):N-1]}\end{bmatrix} (12a)
𝐲[0:N1]=[𝐲[0:LKi1]𝐲[LKi:2(LKi)1]𝐲[(K1)(LKi):N1]]\displaystyle{\bf y}_{[0:N-1]}=\begin{bmatrix}{\bf y}_{[0:L-K_{i}-1]}\\ {\bf y}_{[L-K_{i}:2(L-K_{i})-1]}\\ \vdots\\ {\bf y}_{[(K-1)(L-K_{i}):N-1]}\end{bmatrix} (12b)

For each segment kk, k=1,,Kk=1,...,K, we stack its previous KiK_{i} points on top to get the following vectors:

𝗎k\displaystyle{\sf u}_{k} =[𝐮[(k1)(LKi)Ki:(k1)(LKi)1]𝐮[(k1)(LKi):k(LKi)1]]mL\displaystyle=\begin{bmatrix}{\bf u}_{[(k-1)(L-K_{i})-K_{i}:(k-1)(L-K_{i})-1]}\\ {\bf u}_{[(k-1)(L-K_{i}):k(L-K_{i})-1]}\end{bmatrix}\in\mathbb{R}^{mL} (13a)
𝗒k\displaystyle{\sf y}_{k} =[𝐲[(k1)(LKi)Ki:(k1)(LKi)1]𝐲[(k1)(LKi):k(LKi)1]]pL\displaystyle=\begin{bmatrix}{\bf y}_{[(k-1)(L-K_{i})-K_{i}:(k-1)(L-K_{i})-1]}\\ {\bf y}_{[(k-1)(L-K_{i}):k(L-K_{i})-1]}\end{bmatrix}\in\mathbb{R}^{pL} (13b)

According to Lemma 1, for each k=1,,Kk=1,...,K, the pair (𝗎k,𝗒k)({\sf u}_{k},{\sf y}_{k}) is an admissible pair of input-output trajectories if and only if

[𝗎k𝗒k]=[L(ud)L(yd)]ζk\begin{bmatrix}{\sf u}_{k}\\ {\sf y}_{k}\end{bmatrix}=\begin{bmatrix}\mathcal{H}_{L}(u^{d})\\ \mathcal{H}_{L}(y^{d})\end{bmatrix}\zeta_{k} (14)

for some ζkML+1\zeta_{k}\in\mathbb{R}^{M-L+1}. We refer to (14) for k=1,,Kk=1,...,K as equality dynamic constraints with ζk\zeta_{k} as auxiliary variables. Then, we impose the following equality matching conditions for k=1,,K1k=1,...,K-1 to piece together the segments and form a long admissible trajectory:

[ImKi0m(LKi),mKi]𝗎k+1\displaystyle\begin{bmatrix}I_{mK_{i}}\\ 0_{m(L-K_{i}),mK_{i}}\end{bmatrix}^{\top}{\sf u}_{k+1} =[0m(LKi),mKiImKi]𝗎k\displaystyle=\begin{bmatrix}0_{m(L-K_{i}),mK_{i}}\\ I_{mK_{i}}\end{bmatrix}^{\top}{\sf u}_{k} (15a)
[IpKi0p(LKi),pKi]𝗒k+1\displaystyle\begin{bmatrix}I_{pK_{i}}\\ 0_{p(L-K_{i}),pK_{i}}\end{bmatrix}^{\top}{\sf y}_{k+1} =[0p(LKi),pKiIpKi]𝗒k\displaystyle=\begin{bmatrix}0_{p(L-K_{i}),pK_{i}}\\ I_{pK_{i}}\end{bmatrix}^{\top}{\sf y}_{k} (15b)

i.e., we enforce the first KiK_{i} points of 𝗎k+1{\sf u}_{k+1} (resp. 𝗒k+1{\sf y}_{k+1}) to be equal to the last KiK_{i} points of 𝗎k{\sf u}_{k} (resp. 𝗒k{\sf y}_{k}). Note that because KilminK_{i}\geq l_{\min}, an admissible pair of input-output trajectories of length KiK_{i} corresponds to a unique state trajectory of length Ki+1K_{i}+1. Specifically, given a state-space model (1) of the system, for an admissible pair of input-output trajectories of length KiK_{i}, with KilminK_{i}\geq l_{\min}, the unique corresponding initial state can be determined by (4) with l=Kil=K_{i}, and then the states over the next KiK_{i} steps are uniquely determined by this initial state and the given input trajectory. Therefore, matching the first KiK_{i} points of (𝗎k+1,𝗒k+1)({\sf u}_{k+1},{\sf y}_{k+1}) to the last KiK_{i} points of (𝗎k,𝗒k)({\sf u}_{k},{\sf y}_{k}) creates a continuous state trajectory x((k1)(LKi)Ki),,x((k+1)(LKi))x((k-1)(L-K_{i})-K_{i}),...,x((k+1)(L-K_{i})).

 

𝐮[Ki:N1]\displaystyle{\bf u}_{[-K_{i}:N-1]} =col([ImKi0m(LKi),mKi]𝗎1,[0mKi,m(LKi)Im(LKi)]𝗎1,,[0mKi,m(LKi)Im(LKi)]𝗎K)\displaystyle=\text{col}\left(\begin{bmatrix}I_{mK_{i}}\\ 0_{m(L-K_{i}),mK_{i}}\end{bmatrix}^{\top}{\sf u}_{1},\begin{bmatrix}0_{mK_{i},m(L-K_{i})}\\ I_{m(L-K_{i})}\end{bmatrix}^{\top}{\sf u}_{1},...,\begin{bmatrix}0_{mK_{i},m(L-K_{i})}\\ I_{m(L-K_{i})}\end{bmatrix}^{\top}{\sf u}_{K}\right) (18a)
𝐲[Ki:N1]\displaystyle{\bf y}_{[-K_{i}:N-1]} =col([IpKi0p(LKi),pKi]𝗒1,[0pKi,p(LKi)Ip(LKi)]𝗒1,,[0pKi,p(LKi)Ip(LKi)]𝗒K)\displaystyle=\text{col}\left(\begin{bmatrix}I_{pK_{i}}\\ 0_{p(L-K_{i}),pK_{i}}\end{bmatrix}^{\top}{\sf y}_{1},\begin{bmatrix}0_{pK_{i},p(L-K_{i})}\\ I_{p(L-K_{i})}\end{bmatrix}^{\top}{\sf y}_{1},...,\begin{bmatrix}0_{pK_{i},p(L-K_{i})}\\ I_{p(L-K_{i})}\end{bmatrix}^{\top}{\sf y}_{K}\right) (18b)

 

Refer to caption
Figure 1: Illustration of the structure of the variables.

The initial condition for the input-output trajectories in (5b) can be imposed through the following equality constraints on (𝗎1,𝗒1)({\sf u}_{1},{\sf y}_{1}):

[ImKi0m(LKi),mKi]𝗎1\displaystyle\begin{bmatrix}I_{mK_{i}}\\ 0_{m(L-K_{i}),mK_{i}}\end{bmatrix}^{\top}{\sf u}_{1} =𝐮i\displaystyle={\bf u}_{i} (16a)
[IpKi0p(LKi),pKi]𝗒1\displaystyle\begin{bmatrix}I_{pK_{i}}\\ 0_{p(L-K_{i}),pK_{i}}\end{bmatrix}^{\top}{\sf y}_{1} =𝐲i\displaystyle={\bf y}_{i} (16b)

Also, the path constraints in (5d) can be imposed through the following inequality constraints for k=1,,Kk=1,...,K and l=Ki+1,,Ll=K_{i}+1,...,L:

Su[0m(l1),mIm0m(Ll),m]𝗎k+Sy[0p(l1),pIp0p(Ll),p]𝗒ksS_{u}\begin{bmatrix}0_{m(l-1),m}\\ I_{m}\\ 0_{m(L-l),m}\end{bmatrix}^{\top}{\sf u}_{k}+S_{y}\begin{bmatrix}0_{p(l-1),p}\\ I_{p}\\ 0_{p(L-l),p}\end{bmatrix}^{\top}{\sf y}_{k}\leq s (17)

The input-output trajectories over the entire planning horizon t=Ki,,N1t=-K_{i},...,N-1 can be constructed from the vectors 𝗎k{\sf u}_{k} and 𝗒k{\sf y}_{k}, k=1,,Kk=1,...,K, through the equations in (18). And we arrive at the following result:

Lemma 2: Suppose the system (1) is controllable and the control input trajectory ud(0),ud(1),,ud(M1)u^{d}(0),u^{d}(1),...,u^{d}(M-1) is persistently exciting of order L+nL+n. Then, a pair of input-output trajectories (𝐮[Ki:N1],𝐲[Ki:N1])({\bf u}_{[-K_{i}:N-1]},{\bf y}_{[-K_{i}:N-1]}), where NN can be written as N=K(LKi)N=K(L-K_{i}) for some K1K\in\mathbb{Z}_{\geq 1}, is admissible by the system (1) and satisfies the initial condition in (5b) and the path constraints in (5d) if and only if there exist vectors (𝗎k,𝗒k,ζk)mL×pL×ML+1({\sf u}_{k},{\sf y}_{k},\zeta_{k})\in\mathbb{R}^{mL}\times\mathbb{R}^{pL}\times\mathbb{R}^{M-L+1}, k=1,,Kk=1,...,K, that satisfy the constraints in (14), (15), (16) and (17) and (𝐮[Ki:N1],𝐲[Ki:N1])({\bf u}_{[-K_{i}:N-1]},{\bf y}_{[-K_{i}:N-1]}) relate to the vectors (𝗎k,𝗒k)({\sf u}_{k},{\sf y}_{k}), k=1,,Kk=1,...,K, according to (18).

Proof: This result follows from Lemma 1 and the constructions of the vectors (𝗎k,𝗒k)({\sf u}_{k},{\sf y}_{k}), k=1,,Kk=1,...,K, in (12)–(13) and the constraints in (14)–(17). \blacksquare

4 Linear Program for Minimum-time Trajectory Optimization

Now we deal with the target condition 𝐲[T:T+Kf1]Yf{\bf y}_{[T:T+K_{f}-1]}\in Y_{f}. Recall that the goal is to minimize the time TT at which the output trajectory reaches the target set Yf={𝐲fpKf:G𝐲fg,H𝐲f=h}Y_{f}=\{{\bf y}_{f}\in\mathbb{R}^{pK_{f}}:G{\bf y}_{f}\leq g,H{\bf y}_{f}=h\}.

Assume that the minimum time TT^{*} is in the range of [T0,T1][T_{0},T_{1}]. For a given problem, such a range may be estimated based on prior knowledge or set to be sufficiently large (e.g., T0=0T_{0}=0 and T1T_{1} being a large number). It will become clear soon that a smaller range (i.e., a closer estimate of TT^{*}) makes the formulated problem simpler in terms of having less decision variables and constraints. For each t[T0,T1]t\in[T_{0},T_{1}], define a slack variable εtqg+qh\varepsilon_{t}\in\mathbb{R}^{q_{g}+q_{h}}, where qgq_{g} is the dimension of gg and qhq_{h} is that of hh. Impose the following constraints for εt\varepsilon_{t}:

G𝐲[t:t+Kf1]gεt,1:qg\displaystyle G{\bf y}_{[t:t+K_{f}-1]}-g\leq\varepsilon_{t,1:q_{g}} (19a)
H𝐲[t:t+Kf1]h=εt,qg+1:qg+qh\displaystyle H{\bf y}_{[t:t+K_{f}-1]}-h=\varepsilon_{t,q_{g}+1:q_{g}+q_{h}} (19b)

where εt,1:qg\varepsilon_{t,1:q_{g}} denotes the first qgq_{g} rows of εt\varepsilon_{t} and εt,qg+1:qg+qh\varepsilon_{t,q_{g}+1:q_{g}+q_{h}} denotes the remaining rows of εt\varepsilon_{t}. Then, 𝐲[t:t+Kf1]Yf{\bf y}_{[t:t+K_{f}-1]}\in Y_{f} if and only if 0 is a feasible value for εt\varepsilon_{t}.

Consider the following function:

J=t=T0T1θtT0εt1J=\sum_{t=T_{0}}^{T_{1}}\theta^{t-T_{0}}\|\varepsilon_{t}\|_{1} (20)

where θ>1\theta>1 is a sufficiently large constant and 1\|\cdot\|_{1} denotes the 11-norm. The analysis in what follows shows that minimizing (20) can lead to a minimum-time trajectory solution:

Assume T[T0,T1]T^{*}\in[T_{0},T_{1}] is known and consider the following problem parameterized by η={ηt}t=T,,T1\eta=\{\eta_{t}\}_{t=T^{*},...,T_{1}}:

minu(),y(),ε()\displaystyle\min_{u(\cdot),y(\cdot),\varepsilon_{(\cdot)}}\!\! J0(θ,{εt}t=T0,,T1)=t=T0T1θtT0εt1\displaystyle\,J_{0}(\theta,\{\varepsilon_{t}\}_{t=T_{0},...,T^{*}-1})=\sum_{t=T_{0}}^{T^{*}-1}\theta^{t-T_{0}}\|\varepsilon_{t}\|_{1} (21a)
s.t. (𝐮[Ki:1],𝐲[Ki:1])=(𝐮i,𝐲i)\displaystyle\,({\bf u}_{[-K_{i}:-1]},{\bf y}_{[-K_{i}:-1]})=({\bf u}_{i},{\bf y}_{i}) (21b)
c(u(t),y(t))0,t=0,,N1\displaystyle\,c(u(t),y(t))\leq 0,\,\,\,t=0,...,N-1 (21c)
G𝐲[t:t+Kf1]gεt,1:qg\displaystyle\,G{\bf y}_{[t:t+K_{f}-1]}-g\leq\varepsilon_{t,1:q_{g}} (21d)
H𝐲[t:t+Kf1]h=εt,qg+1:qg+qh,t=T0,,T1\displaystyle\,H{\bf y}_{[t:t+K_{f}-1]}-h=\varepsilon_{t,q_{g}+1:q_{g}+q_{h}},\,\,t=T_{0},...,T_{1} (21e)
εt=ηt,t=T,,T1\displaystyle\,\varepsilon_{t}=\eta_{t},\,\,\,t=T^{*},...,T_{1} (21f)

where NN represents a planning horizon that satisfies NT1+KfN\geq T_{1}+K_{f}, and ηtqg+qh\eta_{t}\in\mathbb{R}^{q_{g}+q_{h}}, t=T,,T1t=T^{*},...,T_{1}, are parameters with the nominal value ηt=0\eta_{t}^{*}=0. The following result clarifies the relation between the minimum-time problem of interest, (5), and the above problem (21):

Theorem 1: (i) Suppose Assumptions 2 and 3 hold. Let (𝐮[Ki:T+Kf1],𝐲[Ki:T+Kf1],T)({\bf u}_{[-K_{i}:T^{*}+K_{f}-1]}^{*},{\bf y}_{[-K_{i}:T^{*}+K_{f}-1]}^{*},T^{*}) be an optimal solution to the minimum-time problem (5) with T[T0,T1]T^{*}\in[T_{0},T_{1}]. Then, (21) with η=0\eta=0 has a feasible solution (𝐮[Ki:N1],𝐲[Ki:N1],{εt}t=T0,,T1)({\bf u}_{[-K_{i}:N-1]}^{\prime},{\bf y}_{[-K_{i}:N-1]}^{\prime},\{\varepsilon_{t}^{\prime}\}_{t=T_{0},...,T_{1}}) that satisfies (𝐮[Ki:T+Kf1],𝐲[Ki:T+Kf1])=(𝐮[Ki:T+Kf1],𝐲[Ki:T+Kf1])({\bf u}_{[-K_{i}:T^{*}+K_{f}-1]}^{\prime},{\bf y}_{[-K_{i}:T^{*}+K_{f}-1]}^{\prime})=({\bf u}_{[-K_{i}:T^{*}+K_{f}-1]}^{*},{\bf y}_{[-K_{i}:T^{*}+K_{f}-1]}^{*}) and εt=0\varepsilon_{t}^{\prime}=0 for t=T,,T1t=T^{*},...,T_{1}.

(ii) Suppose (5) is feasible and has a minimum time T[T0,T1]T^{*}\in[T_{0},T_{1}]. Let (𝐮[Ki:N1],𝐲[Ki:N1],{εt}t=T0,,T1)({\bf u}_{[-K_{i}:N-1]}^{\prime},{\bf y}_{[-K_{i}:N-1]}^{\prime},\{\varepsilon_{t}^{\prime}\}_{t=T_{0},...,T_{1}}) be an arbitrary feasible solution to (21) with η=0\eta=0. Then, the triple (𝐮[Ki:T+Kf1],𝐲[Ki:T+Kf1],T)({\bf u}_{[-K_{i}:T^{*}+K_{f}-1]}^{\prime},{\bf y}_{[-K_{i}:T^{*}+K_{f}-1]}^{\prime},T^{*}) is an optimal solution to (5).

Proof: For (i), let (𝐮[Ki:T+Kf1],𝐲[Ki:T+Kf1],T)({\bf u}_{[-K_{i}:T^{*}+K_{f}-1]}^{*},{\bf y}_{[-K_{i}:T^{*}+K_{f}-1]}^{*},T^{*}) be an optimal solution to (5). Because KilminK_{i}\geq l_{\min} and T0T^{*}\geq 0, K¯f=max(Kf,lmin)\bar{K}_{f}=\max(K_{f},l_{\min}) must satisfy KiT+KfK¯fT+Kf1-K_{i}\leq T^{*}+K_{f}-\bar{K}_{f}\leq T^{*}+K_{f}-1. Then, under Assumption 2, (𝐮[T+KfK¯f:T+Kf1],𝐲[T+KfK¯f:T+Kf1])({\bf u}_{[T^{*}+K_{f}-\bar{K}_{f}:T^{*}+K_{f}-1]}^{*},{\bf y}_{[T^{*}+K_{f}-\bar{K}_{f}:T^{*}+K_{f}-1]}^{*}) is an admissible pair of input-output trajectories of length K¯f\bar{K}_{f} that satisfies c(u(t),y(t))0c(u(t),y(t))\leq 0 for t=T+KfK¯f,,T+Kf1t=T^{*}+K_{f}-\bar{K}_{f},...,T^{*}+K_{f}-1 and 𝐲[T:T+Kf1]Yf{\bf y}_{[T^{*}:T^{*}+K_{f}-1]}\in Y_{f}. In this case, under Assumption 3, there exist inputs u(T+Kf),,u(N1)u^{\prime}(T^{*}+K_{f}),...,u^{\prime}(N-1) and the corresponding outputs y(T+Kf),,y(N1)y^{\prime}(T^{*}+K_{f}),...,y^{\prime}(N-1) such that c(u(t),y(t))0c(u(t),y(t))\leq 0 for t=T+Kf,,N1t=T^{*}+K_{f},...,N-1 and 𝐲[t:t+Kf1]Yf{\bf y}_{[t:t+K_{f}-1]}\in Y_{f} for t=T+1,,T1t=T^{*}+1,...,T_{1}. Now let 𝐮[Ki:N1]=col(𝐮[Ki:T+Kf1],u(T+Kf),,u(N1)){\bf u}_{[-K_{i}:N-1]}^{\prime}=\text{col}({\bf u}_{[-K_{i}:T^{*}+K_{f}-1]}^{*},u^{\prime}(T^{*}+K_{f}),...,u^{\prime}(N-1)) and 𝐲[Ki:N1]=col(𝐲[Ki:T+Kf1],y(T+Kf),,y(N1)){\bf y}_{[-K_{i}:N-1]}^{\prime}=\text{col}({\bf y}_{[-K_{i}:T^{*}+K_{f}-1]}^{*},y^{\prime}(T^{*}+K_{f}),...,y^{\prime}(N-1)), and let εt\varepsilon_{t}^{\prime} be defined according to εt,1:qg=G𝐲[t:t+Kf1]g\varepsilon_{t,1:q_{g}}^{\prime}=G{\bf y}_{[t:t+K_{f}-1]}^{\prime}-g and εt,qg+1:qg+qh=H𝐲[t:t+Kf1]h\varepsilon_{t,q_{g}+1:q_{g}+q_{h}}^{\prime}=H{\bf y}_{[t:t+K_{f}-1]}^{\prime}-h for t=T0,,T1t=T_{0},...,T^{*}-1 and εt=0\varepsilon_{t}^{\prime}=0 for t=T,,T1t=T^{*},...,T_{1}. Then, the triple (𝐮[Ki:N1],𝐲[Ki:N1],{εt}t=T0,,T1)({\bf u}_{[-K_{i}:N-1]}^{\prime},{\bf y}_{[-K_{i}:N-1]}^{\prime},\{\varepsilon_{t}^{\prime}\}_{t=T_{0},...,T_{1}}) is a feasible solution to (21) with η=0\eta=0. This proves part (i).

For part (ii), let (𝐮[Ki:N1],𝐲[Ki:N1],{εt}t=T0,,T1)({\bf u}_{[-K_{i}:N-1]}^{\prime},{\bf y}_{[-K_{i}:N-1]}^{\prime},\{\varepsilon_{t}^{\prime}\}_{t=T_{0},...,T_{1}}) be a feasible solution to (21) with η=0\eta=0. Due to the constraints in (21d)–(21f), this solution satisfies 𝐲[T:T+Kf1]Yf{\bf y}_{[T^{*}:T^{*}+K_{f}-1]}^{\prime}\in Y_{f}. Therefore, the triple (𝐮[Ki:T+Kf1],𝐲[Ki:T+Kf1],T)({\bf u}_{[-K_{i}:T^{*}+K_{f}-1]}^{\prime},{\bf y}_{[-K_{i}:T^{*}+K_{f}-1]}^{\prime},T^{*}) is an optimal solution to the minimum-time problem (5). \blacksquare

Remark 2: From the proof of Theorem 1 the necessity of both Assumptions 2 and 3 for guaranteeing the result of part (i) should be clear. For instance, suppose the second half of Assumption 2 does not hold, i.e., the pair (𝐮i,𝐲i)({\bf u}_{i},{\bf y}_{i}) does not satisfy the path constraints c(u(t),y(t))0c(u(t),y(t))\leq 0 at all times, and suppose Kf<lminK_{f}<l_{\min} and T<lminKfT^{*}<l_{\min}-K_{f}. Then, the pair (𝐮[T+KfK¯f:T+Kf1],𝐲[T+KfK¯f:T+Kf1])({\bf u}_{[T^{*}+K_{f}-\bar{K}_{f}:T^{*}+K_{f}-1]}^{*},{\bf y}_{[T^{*}+K_{f}-\bar{K}_{f}:T^{*}+K_{f}-1]}^{*}) may not satisfy c(u(t),y(t))0c(u(t),y(t))\leq 0 for all t=T+KfK¯f,,T+Kf1t=T^{*}+K_{f}-\bar{K}_{f},...,T^{*}+K_{f}-1. Consequently, for such a pair of input-output trajectories of length K¯f\bar{K}_{f}, there may not exist an input u(T+Kf)u^{\prime}(T^{*}+K_{f}) that is able to maintain the output trajectory in YfY_{f} while satisfying the path constraints even if Assumption 3 holds. In contrast, the result of part (ii) does not rely on Assumptions 2 and 3.

Theorem 1 indicates that minimum-time trajectory solutions (i.e., optimal solutions to (5)) can be obtained through (21). However, the formulation of (21) relies on the exact knowledge of the minimum time TT^{*}, which is typically not known a priori (otherwise the problem reduces to a fixed-time trajectory planning problem). In what follows we show that an optimal solution to (21) can be obtained through another related problem the formulation of which does not rely on exact knowledge of TT^{*}. The technique originates from exact penalty methods for handling constraints in constrained optimization.

Now let (𝐮[Ki:N1](θ),𝐲[Ki:N1](θ),{εt(θ)}t=T0,,T1)({\bf u}_{[-K_{i}:N-1]}^{*}(\theta),{\bf y}_{[-K_{i}:N-1]}^{*}(\theta),\{\varepsilon_{t}^{*}(\theta)\}_{t=T_{0},...,T_{1}}) be an optimal solution to (21) with η={ηt}t=T,,T1=0\eta=\{\eta_{t}\}_{t=T^{*},...,T_{1}}=0 and a certain value of θ\theta. Let λk(θ)qg+qh\lambda_{k}(\theta)\in\mathbb{R}^{q_{g}+q_{h}} be the Lagrange multiplier associated with the constraint εk=ηk\varepsilon_{k}=\eta_{k}, k=T,,T1k=T^{*},...,T_{1}. Its value satisfies

λk,i(θ)=dJ0(θ,{εt(θ,ηk,i)}t=T0,,T1)dηk,i,i=1,,qg+qh\lambda_{k,i}(\theta)\!=\!\frac{\text{d}J_{0}(\theta,\{\varepsilon_{t}^{*}(\theta,\eta_{k,i})\}_{t=T_{0},...,T^{*}-1})}{\text{d}\eta_{k,i}},\,\,i\!=\!1,...,q_{g}+q_{h} (22)

where εt(θ,ηk,i)\varepsilon_{t}^{*}(\theta,\eta_{k,i}) denotes the perturbed value of εt(θ)\varepsilon_{t}^{*}(\theta) due to a perturbation ηk,i\eta_{k,i}\in\mathbb{R} to the iith entry of the parameter ηk\eta_{k}, i.e., the Lagrange multiplier represents the sensitivity of the cost to perturbations to the constraint (Büskens and Maurer, 2001). We make the following assumption about the optimal solution (𝐮[Ki:N1](θ),𝐲[Ki:N1](θ),{εt(θ)}t=T0,,T1)({\bf u}_{[-K_{i}:N-1]}^{*}(\theta),{\bf y}_{[-K_{i}:N-1]}^{*}(\theta),\{\varepsilon_{t}^{*}(\theta)\}_{t=T_{0},...,T_{1}}) to (21):

Assumption 4: For sufficiently large θ\theta and t=T0,,T1t=T_{0},...,T^{*}-1, the sensitivity of εt(θ)\varepsilon_{t}^{*}(\theta) to perturbations to the constraints εk=ηk\varepsilon_{k}=\eta_{k}, k=T,,T1k=T^{*},...,T_{1}, is bounded by a constant RR, i.e.,

dεt(θ,ηk,i)dηk,i1R,i=1,,qg+qh\left\|\frac{\text{d}\varepsilon_{t}^{*}(\theta,\eta_{k,i})}{\text{d}\eta_{k,i}}\right\|_{1}\leq R,\quad i=1,...,q_{g}+q_{h} (23)

where 1\|\cdot\|_{1} denotes the 11-norm.

Under Assumption 4, we can derive the following bound on λk,i(θ)\lambda_{k,i}(\theta):

|λk,i(θ)|=|dJ0(θ,{εt(θ,ηk,i)}t=T0,,T1)dηk,i|\displaystyle|\lambda_{k,i}(\theta)|=\left|\frac{\text{d}J_{0}(\theta,\{\varepsilon_{t}^{*}(\theta,\eta_{k,i})\}_{t=T_{0},...,T^{*}-1})}{\text{d}\eta_{k,i}}\right|
t=T0T1|J0εtεtηk,i|t=T0T1θtT0dεtdηk,i1\displaystyle\leq\sum_{t=T_{0}}^{T^{*}-1}\left|\frac{\partial J_{0}}{\partial\varepsilon_{t}^{*}}\cdot\frac{\partial\varepsilon_{t}^{*}}{\partial\eta_{k,i}}\right|\leq\sum_{t=T_{0}}^{T^{*}-1}\theta^{t-T_{0}}\left\|\frac{\text{d}\varepsilon_{t}^{*}}{\text{d}\eta_{k,i}}\right\|_{1}
Rt=T0T1θtT0Rθ1θTT0\displaystyle\leq R\sum_{t=T_{0}}^{T^{*}-1}\theta^{t-T_{0}}\leq\frac{R}{\theta-1}\theta^{T^{*}-T_{0}} (24)

Hence, if θR+1\theta\geq R+1, we have

λk(θ)=maxi|λk,i(θ)|θTT0\|\lambda_{k}(\theta)\|_{\infty}=\max_{i}|\lambda_{k,i}(\theta)|\leq\theta^{T^{*}-T_{0}} (25)

for k=T,,T1k=T^{*},...,T_{1}, where \|\cdot\|_{\infty} denotes the sup-norm. We arrive at the following result:

Theorem 2: Suppose (5) is feasible and has a minimum time T[T0,T1]T^{*}\in[T_{0},T_{1}]. Let (𝐮[Ki:N1](θ),𝐲[Ki:N1](θ),({\bf u}_{[-K_{i}:N-1]}^{*}(\theta),{\bf y}_{[-K_{i}:N-1]}^{*}(\theta), {εt(θ)}t=T0,,T1)\{\varepsilon_{t}^{*}(\theta)\}_{t=T_{0},...,T_{1}}) be an optimal solution to (21) with η=0\eta~{}=~{}0 and θ>1\theta>1. Suppose Assumption 4 holds and θ\theta is sufficiently large. Then, (𝐮[Ki:N1](θ),𝐲[Ki:N1](θ),({\bf u}_{[-K_{i}:N-1]}^{*}(\theta),{\bf y}_{[-K_{i}:N-1]}^{*}(\theta), {εt(θ)}t=T0,,T1)\{\varepsilon_{t}^{*}(\theta)\}_{t=T_{0},...,T_{1}}) is an optimal solution to the following problem, the formulation of which does not rely on TT^{*}:

minu(),y(),ε()\displaystyle\min_{u(\cdot),y(\cdot),\varepsilon_{(\cdot)}}\!\! J=t=T0T1θtT0εt1\displaystyle\,J=\sum_{t=T_{0}}^{T_{1}}\theta^{t-T_{0}}\|\varepsilon_{t}\|_{1} (26a)
s.t. (𝐮[Ki:1],𝐲[Ki:1])=(𝐮i,𝐲i)\displaystyle\,({\bf u}_{[-K_{i}:-1]},{\bf y}_{[-K_{i}:-1]})=({\bf u}_{i},{\bf y}_{i}) (26b)
c(u(t),y(t))0,t=0,,N1\displaystyle\,c(u(t),y(t))\leq 0,\,\,\,t=0,...,N-1 (26c)
G𝐲[t:t+Kf1]gεt,1:qg\displaystyle\,G{\bf y}_{[t:t+K_{f}-1]}-g\leq\varepsilon_{t,1:q_{g}} (26d)
H𝐲[t:t+Kf1]h=εt,qg+1:qg+qh,t=T0,,T1\displaystyle\,H{\bf y}_{[t:t+K_{f}-1]}-h=\varepsilon_{t,q_{g}+1:q_{g}+q_{h}},\,\,t=T_{0},...,T_{1} (26e)

Proof: The cost function of (26) can be written as J=J0+t=TT1θtT0εt1J=J_{0}+\sum_{t=T^{*}}^{T_{1}}\theta^{t-T_{0}}\|\varepsilon_{t}\|_{1}, where J0J_{0} is the cost function of (21). If Assumption 4 holds and θR+1\theta\geq R+1, then according to (25), for t=T,,T1t=T^{*},...,T_{1}, the Lagrange multiplier λt(θ)\lambda_{t}(\theta) associated with the constraint εt=ηt=0\varepsilon_{t}=\eta_{t}=0 of (21) satisfies λt(θ)θTT0θtT0\|\lambda_{t}(\theta)\|_{\infty}\leq\theta^{T^{*}-T_{0}}\leq\theta^{t-T_{0}}. This implies that the term θtT0εt1\theta^{t-T_{0}}\|\varepsilon_{t}\|_{1} in the cost function of (26) is an exact penalty for the constraint εt=0\varepsilon_{t}=0. Therefore, (26) is a reformulation of (21) by replacing all constraints in (21f) with exact penalties and hence the result follows (Han and Mangasarian, 1979). \blacksquare

Theorem 2 indicates that optimal solutions to (21), which, according to Theorem 1, are optimal solutions to (5) (i.e., minimum-time trajectories), can be obtained through (26). The cost function of (26), which is non-smooth due to the 11-norms, can be readily converted into a smooth function using a linear programming reformulation. We combine the results of the previous section on data-based trajectory prediction and of this section on minimum-time trajectory planning and arrive at the following problem formulation:

min{𝗎k,𝗒k,ζk}k=1,K,{εt0}t=T0,,T1J=t=T0T1θtT0(1qg+qhεt)\displaystyle\min_{\{{\sf u}_{k},{\sf y}_{k},\zeta_{k}\}_{k=1,...K},\{\varepsilon_{t}\geq 0\}_{t=T_{0},...,T_{1}}}\!J=\sum_{t=T_{0}}^{T_{1}}\theta^{t-T_{0}}(1_{q_{g}+q_{h}}^{\top}\varepsilon_{t})
s.t. (27)
dynamic constraints: (14) for k=1,,K\displaystyle\text{dynamic constraints: \eqref{equ:14} for }k=1,...,K
matching conditions: (15) for k=1,,K1\displaystyle\text{matching conditions: \eqref{equ:15} for }k=1,...,K-1
initial condition: (16)
path constraints: (17) for k=1,..,K and l=Ki+1,,L\displaystyle\text{path constraints: \eqref{equ:17} for }k=1,..,K\text{ and }l=K_{i}+1,...,L
terminal condition:
G𝐲[t:t+Kf1]gεt,1:qg\displaystyle G{\bf y}_{[t:t+K_{f}-1]}-g\leq\varepsilon_{t,1:q_{g}}
H𝐲[t:t+Kf1]hεt,qg+1:qg+qh\displaystyle H{\bf y}_{[t:t+K_{f}-1]}-h\leq\varepsilon_{t,q_{g}+1:q_{g}+q_{h}}
hH𝐲[t:t+Kf1]εt,qg+1:qg+qh for t=T0,,T1\displaystyle h-H{\bf y}_{[t:t+K_{f}-1]}\leq\varepsilon_{t,q_{g}+1:q_{g}+q_{h}}\text{ for }t=T_{0},...,T_{1}

in which the dimensions of the decision variables are 𝗎kmL{\sf u}_{k}\in\mathbb{R}^{mL}, 𝗒kpL{\sf y}_{k}\in\mathbb{R}^{pL}, ζkML+1\zeta_{k}\in\mathbb{R}^{M-L+1}, εtqg+qh\varepsilon_{t}\in\mathbb{R}^{q_{g}+q_{h}}, and the number KK should be chosen as K=T1+KfLKiK=\lceil\frac{T_{1}+K_{f}}{L-K_{i}}\rceil, where \lceil\cdot\rceil means rounding up to the nearest integer. As discussed in the second paragraph of this section, the range [T0,T1][T_{0},T_{1}] may be estimated based on prior knowledge about the problem or set as [T0,T1]=[0,T][T_{0},T_{1}]=[0,T^{\prime}] with TT^{\prime} sufficiently large. It can be seen that the cost function of (4) is a linear equation of decision variables and all constraints of (4) are either linear equalities or linear inequalities of decision variables. Hence, (4) is a linear program (LP) and can be easily solved using off-the-shelf LP solvers.

Remark 3: In (4), the auxiliary variables ζkML+1\zeta_{k}\in\mathbb{R}^{M-L+1} only appear in the dynamic constraints (14). Using the following approach we can drop these variables from the problem: Let 𝗏k=col(𝗎k,𝗒k)(m+p)L{\sf v}_{k}=\text{col}({\sf u}_{k},{\sf y}_{k})\in\mathbb{R}^{(m+p)L} and =col(L(ud),L(yd))(m+p)L×(ML+1)\mathcal{H}=\text{col}(\mathcal{H}_{L}(u^{d}),\mathcal{H}_{L}(y^{d}))\in\mathbb{R}^{(m+p)L\times(M-L+1)}. Then, (14) can be written as 𝗏k=ζk{\sf v}_{k}=\mathcal{H}\zeta_{k}. Assume rank()=r\text{rank}(\mathcal{H})=r. We partition the rows of \mathcal{H} into two groups: 1r×(ML+1)\mathcal{H}_{1}\in\mathbb{R}^{r\times(M-L+1)} collects rr linearly independent rows and 2((m+p)Lr)×(ML+1)\mathcal{H}_{2}\in\mathbb{R}^{((m+p)L-r)\times(M-L+1)} collects the remaining rows. Since rank()=r\text{rank}(\mathcal{H})=r, the rows of 2\mathcal{H}_{2} are linear combinations of the rows of 1\mathcal{H}_{1}, i.e., 2\mathcal{H}_{2} can be written as 2=Γ1\mathcal{H}_{2}=\Gamma\mathcal{H}_{1}, where Γ((m+p)Lr)×r\Gamma\in\mathbb{R}^{((m+p)L-r)\times r} is uniquely determined by Γ=21\Gamma=\mathcal{H}_{2}\mathcal{H}_{1}^{\dagger}. Then, (14) can be written as

[𝗏k,1𝗏k,2]=[12]ζk=[1ζkΓ1ζk]=[1ζkΓ𝗏k,1]\begin{bmatrix}{\sf v}_{k,1}\\ {\sf v}_{k,2}\end{bmatrix}=\begin{bmatrix}\mathcal{H}_{1}\\ \mathcal{H}_{2}\end{bmatrix}\zeta_{k}=\begin{bmatrix}\mathcal{H}_{1}\zeta_{k}\\ \Gamma\mathcal{H}_{1}\zeta_{k}\end{bmatrix}=\begin{bmatrix}\mathcal{H}_{1}\zeta_{k}\\ \Gamma{\sf v}_{k,1}\end{bmatrix} (28)

where 𝗏k,1{\sf v}_{k,1} (resp. 𝗏k,2{\sf v}_{k,2}) collects the entries of 𝗏k{\sf v}_{k} corresponding to the rows of 1\mathcal{H}_{1} (resp. 2\mathcal{H}_{2}), and the last equality is obtained by substituting 𝗏k,1=1ζk{\sf v}_{k,1}=\mathcal{H}_{1}\zeta_{k} in the first row into the second row. Recall that, according to Lemma 1, 𝗏k=col(𝗎k,𝗒k){\sf v}_{k}=\text{col}({\sf u}_{k},{\sf y}_{k}) represents an admissible pair of input-output trajectories if and only if there exists ζkML+1\zeta_{k}\in\mathbb{R}^{M-L+1} such that (14) (equivalently, (28)) holds. Since 𝗏k,1r{\sf v}_{k,1}\in\mathbb{R}^{r} and 1\mathcal{H}_{1} has a rank of rr, for any 𝗏k,1{\sf v}_{k,1} there exists ζk\zeta_{k} such that the equation 𝗏k,1=1ζk{\sf v}_{k,1}=\mathcal{H}_{1}\zeta_{k} in the first row of (28) is satisfied. In this case, there exists ζkML+1\zeta_{k}\in\mathbb{R}^{M-L+1} such that (28) holds if and only if 𝗏k,1{\sf v}_{k,1} and 𝗏k,2{\sf v}_{k,2} satisfy the equation 𝗏k,2=Γ𝗏k,1{\sf v}_{k,2}=\Gamma{\sf v}_{k,1} in the second row of (28). Therefore, we can replace the dynamic constraints (14) in (4) with the linear-equality constraints 𝗏k,2=Γ𝗏k,1{\sf v}_{k,2}=\Gamma{\sf v}_{k,1} for k=1,,Kk=1,...,K, after which we obtain a linear program that is equivalent to (4) while does not involve auxiliary variables ζk\zeta_{k}. Note that the partition of =col(L(ud),L(yd))\mathcal{H}=\text{col}(\mathcal{H}_{L}(u^{d}),\mathcal{H}_{L}(y^{d})) into 1\mathcal{H}_{1} and 2\mathcal{H}_{2} and the calculation of Γ=21\Gamma=\mathcal{H}_{2}\mathcal{H}_{1}^{\dagger} can be done offline and they are independent of kk.

5 Example: Spacecraft Relative Motion Planning

To illustrate the approach, we consider a motion planning problem for a low-thrust spacecraft relative to a target body on a nominal circular orbit. The continuous-time dynamics are represented by the Clohessy-Wiltshire-Hill (CWH) equations:

x˙=Acx+Bcu\dot{x}=A_{c}x+B_{c}u (29)

with

Ac=[0001000000100000013ω20002ω00002ω0000ω2000]Bc=[03,3TmaxmsI3]A_{c}=\begin{bmatrix}0&0&0&1&0&0\\ 0&0&0&0&1&0\\ 0&0&0&0&0&1\\ 3\omega^{2}&0&0&0&2\omega&0\\ 0&0&0&-2\omega&0&0\\ 0&0&-\omega^{2}&0&0&0\end{bmatrix}\quad B_{c}=\begin{bmatrix}0_{3,3}\\ \frac{T_{\max}}{m_{s}}I_{3}\end{bmatrix} (30)

where ω=μ/ro3\omega=\sqrt{\mu/r_{o}^{3}} is the orbital rate of the target (in radians/s), μ=398,600\mu=398,600 km3/s2 is the gravitational parameter, ro=6,928r_{o}=6,928 km is the radius of the target’s circular orbit, ms=50m_{s}=50 kg is the mass of the ego spacecraft, and Tmax=2×104T_{\max}=2\times 10^{-4} kN represents the ego spacecraft’s maximum thrust. The first three entries of the state vector x6x\in\mathbb{R}^{6} represent the relative positions of the ego spacecraft with respect to the target body along the 𝗑{\sf x}-, 𝗒{\sf y}-, and 𝗓{\sf z}-axes of the CWH frame (in km), and the last three entries represent the relative velocity components (in km/s). The entries of the control input vector u3u\in\mathbb{R}^{3} represent the thrust level components of the ego spacecraft along the 𝗑{\sf x}-, 𝗒{\sf y}-, and 𝗓{\sf z}-axes. For simplicity, we discretize the continuous-time model (29) using the forward Euler method with a sampling period of Δt=10\Delta t=10 s and obtain the discrete-time model:

x(t+1)=(I6+AcΔt)Ax(t)+(BcΔt)Bu(t)x(t+1)=\underbrace{(I_{6}+A_{c}\Delta t)}_{A}x(t)+\underbrace{(B_{c}\Delta t)}_{B}u(t) (31)

We treat the discrete-time model (31) as the actual system and use it for both data generation and simulation. Also for simplicity, we assume a sup-norm bound on the control input vector: u(t)1\|u(t)\|_{\infty}\leq 1, which can be equivalently expressed as individual bounds on each thrust level component:

1ui(t)1,i{x,y,z}-1\leq u_{i}(t)\leq 1,\quad i\in\{x,y,z\} (32)

We remark that higher-fidelity modeling is possible: For piecewise-constant or piecewise-linear control inputs, exact discrete-time models can be obtained through the zero- or first-order hold method. For a 2-norm bound on the thrust level vector, u(t)21\|u(t)\|_{2}\leq 1, one can use a polygonal approximation (Blackmore et al., 2012). The considered task is for the ego spacecraft to travel from the initial condition xi=(1,0,1,0,0,0)x_{i}=(-1,0,-1,0,0,0) to the target condition xf=(0,0,0,0,0,0)x_{f}=(0,0,0,0,0,0) in the minimum time.

Assume we do not have the model (31). This may be due to uncertainty about the target’s orbital rate ω\omega or uncertainty about the ego spacecraft’s precise mass msm_{s}, the maximum thrust TmaxT_{\max} its propulsion system produces, or thruster alignment. And assume we are only able to measure relative positions, i.e.,

y(t)=Cx(t)=[I303,3]x(t)y(t)=Cx(t)=\begin{bmatrix}I_{3}&0_{3,3}\end{bmatrix}x(t) (33)

We apply the approach developed in this paper to solve the minimum-time trajectory planning problem for the ego spacecraft in such a setting.

First, we build up a data-based model (L(ud),L(yd))(\mathcal{H}_{L}(u^{d}),\mathcal{H}_{L}(y^{d})) with L=40L=40 using a pair of input-output trajectories (ud,yd)(u^{d},y^{d}) of length M=10,000M=10,000. It is checked that the persistent excitation of order L+n=46L+n=46 condition is satisfied by this trajectory. The lag of this system is lmin=2l_{\min}=2. Therefore, we choose Ki=2K_{i}=2 and Kf=2K_{f}=2 and consider the following initial condition for (𝐮[2:1],𝐲[2:1])({\bf u}_{[-2:-1]},{\bf y}_{[-2:-1]}) and terminal condition for 𝐲[T:T+1]{\bf y}_{[T:T+1]}:

𝐮i=col((0,0,0),(0,0,0)),𝐲i=col((1,0,1),(1,0,1))\displaystyle{\bf u}_{i}\!=\!\text{col}\big{(}(0,0,0),(0,0,0)\big{)},\,{\bf y}_{i}\!=\!\text{col}\big{(}(-1,0,-1),(-1,0,-1)\big{)} (34a)
Yf=𝐲f=col((0,0,0),(0,0,0))\displaystyle Y_{f}={\bf y}_{f}=\text{col}\big{(}(0,0,0),(0,0,0)\big{)} (34b)

These conditions equivalently express the initial and terminal conditions xix_{i} and xfx_{f} for the state vector xx. We estimate that the minimum time TT^{*} is in the range of [T0,T1]=[100,140][T_{0},T_{1}]=[100,140], and hence we choose K=T1+KfLKi=4K=\lceil\frac{T_{1}+K_{f}}{L-K_{i}}\rceil=4. In the LP formulation (4), we use θ=2\theta=2, which is shown to be sufficiently large to yield a minimum-time trajectory solution.

To validate the result obtained by our approach, we also implement a (state-space) model-based mixed-integer programming (MIP) approach for minimum-time trajectory optimization, which is modified from the approach of Carvallo et al. (1990). The MIP formulation for the considered spacecraft relative motion planning problem is given as:

minu(),x(),δ()\displaystyle\min_{u(\cdot),x(\cdot),\delta(\cdot)}\!\! t=T0T1tδ(t)\displaystyle\quad\sum_{t=T_{0}}^{T_{1}}t\,\delta(t) (35a)
s.t. dynamic constraints: (35b)
x(t+1)=Ax(t)+Bu(t),t=0,,T11\displaystyle x(t+1)=Ax(t)+Bu(t),\,\,t=0,...,T_{1}-1
initial condition: x(0)=xi\displaystyle\text{initial condition: }\,x(0)=x_{i} (35c)
path constraints:
1mu(t)1m,t=0,,T11\displaystyle-1_{m}\leq u(t)\leq 1_{m},\,\,t=0,...,T_{1}-1 (35d)
terminal condition:
x(t)xf(1δ(t))W1n\displaystyle x(t)-x_{f}\leq(1-\delta(t))W1_{n}
xfx(t)(1δ(t))W1n\displaystyle x_{f}-x(t)\leq(1-\delta(t))W1_{n} (35e)
δ(t){0,1},t=T0,,T1\displaystyle\delta(t)\in\{0,1\},\,\,\,\,t=T_{0},...,T_{1}
t=T0T1δ(t)=1\displaystyle\sum_{t=T_{0}}^{T_{1}}\delta(t)=1 (35f)

where n=6n=6, m=3m=3, δ(t){0,1}\delta(t)\in\{0,1\}, t=T0,,T1t=T_{0},...,T_{1}, are indicator variables, W>0W>0 is a large constant, the constraints in (35e) mean that the state reaches the target condition xfx_{f} at time tt if δ(t)=1\delta(t)=1, the constraint in (35f) makes sure that a feasible trajectory must reach xfx_{f} at some t[T0,T1]t\in[T_{0},T_{1}], and the goal represented by the cost function in (35a) is to minimize the time to reach xfx_{f}.

Refer to caption
Figure 2: Spacecraft position trajectories from our LP approach (black solid) versus from the model-based MIP approach (red dotted).
Refer to caption
Figure 3: Thrust level vector time histories from our LP approach (black solid) versus from the model-based MIP approach (red dotted).

Figs. 24 illustrate the trajectory solutions from our LP approach based on data-based model (black solid curves) and the MIP approach based on state-space model (red dotted curves). Fig. 2 shows the ego spacecraft relative position trajectories, and Fig. 3 shows the thrust level vector time histories. It can be seen that the trajectories from the two approaches are different, especially for the z-direction. However, the two trajectories have the same time-of-flight, TOF=T×Δt=124×10=1240TOF=T^{*}\times\Delta t=124\times 10=1240 (s), where T=124T^{*}=124 is the minimum (discrete) time obtained by both approaches and Δt=10\Delta t=10 (s) is the sampling period, i.e., the two trajectories are both minimum-time trajectories. It is well-known that in a discrete-time setting minimum-time trajectories are typically not unique. While the MIP approach (35) returns an arbitrary minimum-time solution, our LP approach (4) returns a minimum-time solution that also minimizes the secondary objective function J0=t=T0T1θtT0𝐲[t:t+1]𝐲f1J_{0}=\sum_{t=T_{0}}^{T^{*}-1}\theta^{t-T_{0}}\|{\bf y}_{[t:t+1]}-{\bf y}_{f}\|_{1} (roughly, the cumulative distance from the target condition). Correspondingly, it can be seen from Fig. 3 that the thrust vector profile obtained by our LP approach is smoother than that from the MIP approach222To avoid obtaining a non-smooth control profile due to the non-uniqueness of minimum-time trajectories, one can add a small regularization term to the cost function (Carvallo et al., 1990). In our LP approach, the secondary objective function J0=t=T0T1θtT0𝐲[t:t+1]𝐲f1J_{0}=\sum_{t=T_{0}}^{T^{*}-1}\theta^{t-T_{0}}\|{\bf y}_{[t:t+1]}-{\bf y}_{f}\|_{1} plays the role of such a regularization term.. Fig. 4 shows the values of the slack variables: εt\varepsilon_{t} in our LP approach (4) and δ(t)\delta(t) in the MIP approach (35). At t=T=124t=T^{*}=124 (corresponding to continuous time T×Δt=1240T^{*}\times\Delta t=1240 (s)), εt\varepsilon_{t} converges to zero (according to the criterion εt1<103\|\varepsilon_{t}\|_{1}<10^{-3}) and δ(t)\delta(t) equals one, indicating that both approaches capture the minimum time T=124T^{*}=124.

Refer to caption
Figure 4: Slack variable values: εt\varepsilon_{t} in our LP approach (black solid) versus δ(t)\delta(t) in the model-based MIP approach (red dotted).

In terms of the computations, the difference between the two approaches is significant: We solve the problems in the MATLAB environment running on a Windows 10 Enterprise OS desktop with Intel Xeon Gold 2.30 GHz processor and 3232 GB of RAM. It takes the LP approach 7676 milliseconds (averaged over 1010 experiments) to find a minimum-time solution (after elimination of the auxiliary variables ζk\zeta_{k} using the approach in Remark 3 and solved with MATLAB linprog function and default settings); while it takes the MIP approach 4.434.43 seconds to find one (solved with MATLAB intlinprog function and default settings) – our LP approach based on data-based model is approximately 5858 times faster than the MIP approach based on state-space model.

6 Conclusions

In this paper, we developed an LP-based approach to minimum-time trajectory optimization using input-output data-based models. The approach was based on an effective integration of non-parametric data-based models for trajectory prediction and a continuous optimization formulation using an exponential weighting scheme for minimum-time trajectory planning. We proved that minimum-time trajectories could be obtained with the approach if the weight parameter was chosen to be sufficiently high. We validated the approach and illustrated its application with an aerospace example. Future work includes integrating the approach into a model predictive control framework to achieve repeated replanning and closed-loop control, where real-time data may be used to update the model, and extending the approach to handle noisy data and nonlinear systems.

References

  • Baros et al. (2022) Baros, S., Chang, C.-Y., Colon-Reyes, G. E., Bernstein, A., 2022. Online data-enabled predictive control. Automatica 138, 109926.
  • Berberich et al. (2020) Berberich, J., Köhler, J., Müller, M. A., Allgöwer, F., 2020. Data-driven model predictive control with stability and robustness guarantees. IEEE Transactions on Automatic Control 66 (4), 1702–1717.
  • Blackmore et al. (2012) Blackmore, L., Açıkmeşe, B., Carson III, J. M., 2012. Lossless convexification of control constraints for a class of nonlinear optimal control problems. Systems & Control Letters 61 (8), 863–870.
  • Büskens and Maurer (2001) Büskens, C., Maurer, H., 2001. Sensitivity analysis and real-time optimization of parametric nonlinear programming problems. Online Optimization of Large Scale Systems, 3–16.
  • Carvallo et al. (1990) Carvallo, F. D., Westerberg, A. W., Morari, M., 1990. MILP formulation for solving minimum time optimal control problems. International Journal of Control 51 (4), 943–947.
  • Chinde et al. (2022) Chinde, V., Lin, Y., Ellis, M. J., 2022. Data-enabled predictive control for building HVAC systems. Journal of Dynamic Systems, Measurement, and Control 144 (8), 081001.
  • Coulson et al. (2019) Coulson, J., Lygeros, J., Dörfler, F., 2019. Data-enabled predictive control: In the shallows of the DeePC. In: 18th European Control Conference (ECC). IEEE, pp. 307–312.
  • Coulson et al. (2021) Coulson, J., Lygeros, J., Dörfler, F., 2021. Distributionally robust chance constrained data-enabled predictive control. IEEE Transactions on Automatic Control 67 (7), 3289–3304.
  • Elokda et al. (2021) Elokda, E., Coulson, J., Beuchat, P. N., Lygeros, J., Dörfler, F., 2021. Data-enabled predictive control for quadcopters. International Journal of Robust and Nonlinear Control 31 (18), 8916–8936.
  • Han and Mangasarian (1979) Han, S. P., Mangasarian, O. L., 1979. Exact penalty functions in nonlinear programming. Mathematical Programming 17, 251–269.
  • Huang et al. (2021) Huang, L., Coulson, J., Lygeros, J., Dörfler, F., 2021. Decentralized data-enabled predictive control for power system oscillation damping. IEEE Transactions on Control Systems Technology 30 (3), 1065–1077.
  • Huang et al. (2023) Huang, L., Zhen, J., Lygeros, J., Dörfler, F., 2023. Robust data-enabled predictive control: Tractable formulations and performance guarantees. IEEE Transactions on Automatic Control 68 (5), 3163–3170.
  • Kalman and Bertram (1959) Kalman, R. E., Bertram, J., 1959. General synthesis procedure for computer control of single-loop and multiloop linear systems (an optimal sampling system). Transactions of the American Institute of Electrical Engineers, Part II: Applications and Industry 77 (6), 602–609.
  • LaSalle (1959) LaSalle, J., 1959. Time optimal control systems. Proceedings of the National Academy of Sciences 45 (4), 573–577.
  • Lepetič et al. (2003) Lepetič, M., Klančar, G., Škrjanc, I., Matko, D., Potočnik, B., 2003. Time optimal path planning considering acceleration limits. Robotics and Autonomous Systems 45 (3-4), 199–210.
  • Markovsky and Dörfler (2021) Markovsky, I., Dörfler, F., 2021. Behavioral systems theory in data-driven analysis, signal processing, and control. Annual Reviews in Control 52, 42–64.
  • O’Reilly (1981) O’Reilly, J., 1981. The discrete linear time invariant time-optimal control problem–an overview. Automatica 17 (2), 363–370.
  • Rösmann et al. (2015) Rösmann, C., Hoffmann, F., Bertram, T., 2015. Timed-elastic-bands for time-optimal point-to-point nonlinear model predictive control. In: European Control Conference. IEEE, pp. 3352–3357.
  • Shirazi et al. (2018) Shirazi, A., Ceberio, J., Lozano, J. A., 2018. Spacecraft trajectory optimization: A review of models, objectives, approaches and solutions. Progress in Aerospace Sciences 102, 76–98.
  • Taheri et al. (2017) Taheri, E., Li, N. I., Kolmanovsky, I., 2017. Co-state initialization for the minimum-time low-thrust trajectory optimization. Advances in Space Research 59 (9), 2360–2373.
  • Trélat (2012) Trélat, E., 2012. Optimal control and applications to aerospace: Some results and challenges. Journal of Optimization Theory and Applications 154, 713–758.
  • Van den Broeck et al. (2011) Van den Broeck, L., Diehl, M., Swevers, J., 2011. A model predictive control approach for time optimal point-to-point motion control. Mechatronics 21 (7), 1203–1212.
  • van Waarde et al. (2020) van Waarde, H. J., De Persis, C., Camlibel, M. K., Tesi, P., 2020. Willems’ fundamental lemma for state-space systems and its extension to multiple datasets. IEEE Control Systems Letters 4 (3), 602–607.
  • Verscheure et al. (2009) Verscheure, D., Demeulenaere, B., Swevers, J., De Schutter, J., Diehl, M., 2009. Time-optimal path tracking for robots: A convex optimization approach. IEEE Transactions on Automatic Control 54 (10), 2318–2327.
  • Verschueren et al. (2017) Verschueren, R., Ferreau, H. J., Zanarini, A., Mercangöz, M., Diehl, M., 2017. A stabilizing nonlinear model predictive control scheme for time-optimal point-to-point motions. In: 56th Conference on Decision and Control (CDC). IEEE, pp. 2525–2530.
  • Wang et al. (2017) Wang, Z., Liu, L., Long, T., 2017. Minimum-time trajectory planning for multi-unmanned-aerial-vehicle cooperation using sequential convex programming. Journal of Guidance, Control, and Dynamics 40 (11), 2976–2982.
  • Willems et al. (2005) Willems, J. C., Rapisarda, P., Markovsky, I., De Moor, B. L., 2005. A note on persistency of excitation. Systems & Control Letters 54 (4), 325–329.
  • Zhang et al. (2014) Zhang, X., Fang, Y., Sun, N., 2014. Minimum-time trajectory planning for underactuated overhead crane systems with state and control constraints. IEEE Transactions on Industrial Electronics 61 (12), 6915–6925.