# Time Optimal Control for High-Order Chain-of-Integrators Systems with Full
State Constraints and Arbitrary Terminal States

Yunan Wang, Chuxiong Hu, Zeyang Li, Shize Lin, Suqin He, Ze Wang, and Yu Zhu,
Corresponding author: Chuxiong Hu. He is with the Department of Mechanical
Engineering, Tsinghua University, Beijing, China (e-mail:
cxhu@tsinghua.edu.cn).

###### Abstract

High-order chain-of-integrators systems have been universally utilized in the
industry. However, the time-optimal control problem for high-order chain-of-
integrators systems with full state constraints and arbitrary given terminal
states remains a challenging problem in the optimal control theory, yet to be
resolved. To obtain further understanding of the problem, this paper
establishes a novel notation system and theoretical framework, successfully
providing the switching manifold for high-order problems in the form of
switching law. Through deriving properties of switching laws on signs and
dimension, this paper proposes a definite condition for time-optimal control.
Guided by the developed theory, a trajectory planning method named the
manifold-intercept method (MIM) is proposed. MIM can plan time-optimal jerk-
limited trajectories with full state constraints. MIM can also plan near-
optimal higher-order trajectories with negligible extra motion time. Numerical
results indicate that the proposed MIM outperforms all baselines in
computational time, computational accuracy, and trajectory quality with a
large gap.

###### Index Terms:

Optimal control, linear systems, variational methods, switched systems,
trajectory planning.

##  I Introduction

High-order chain-of-integrators systems have been universally utilized in
computer numerical control (CNC) machining [1, 2], robotic motion control [3,
4, 5], semiconductor device fabrication [6, 7], and autonomous driving [8].
Trajectory planning has a significant influence on motion efficiency and
accuracy in those scenarios [9, 10]. However, the time-optimal control problem
for high-order chain-of-integrators systems with full state constraints and
arbitrary terminal states remains a challenging and significant open problem
in the optimal control theory, yet to be resolved.

The time-optimal control problem for chain-of-integrators requires planning a
trajectory with minimum motion time from a given initial state vector to a
terminal state vector, where the system state vector is composed of components
such as position, velocity, acceleration, and so forth. State constraints
require that all system state components are limited by given upper bounds,
while the initial states and the terminal states are specified arbitrarily. A
fifth-order trajectory planned by the proposed Algorithm 1 is shown in Fig. 1.
The system states and the input, i.e., position, velocity, acceleration, jerk,
snap, and crackle, are bounded by the given constraints. The crackle is always
maximum, minimum, or zero along the planned trajectory, satisfying the Bang-
Singular-Bang control law [11].

Figure 1: A fifth-order trajectory planned by our proposed method.

In optimal control, numerous research has been conducted on time-optimal
control for chain-of-integrators. Proved by Pontryagin’s minimum principle
(PMP) [12], theorems on the Bang-Bang control [13] and the analytic expression
of the optimal control [14] are well-known, fully solving the problem with
only input saturation. Marchand et al. [15] proposed a discrete-time control
law based on the above problem. However, the time-optimal problem with input
saturation and full state constraints remains unsolved, especially when the
initial and terminal states are arbitrarily given. The 2nd-order problem is
trivial [16], but 3rd-order or higher-order problems have not been solved
well. Haschke et al. [17] proposed an online time-optimal jerk-limited
trajectory planning method without position constraints. Kröger [18] developed
the Reflexxes motion libraries, solving the third-order problem without given
terminal acceleration and position constraints. Berscheid et al. [19] fully
considered third-order chain-of-integrators with arbitrary terminal states and
no position constraints, resulting in a widely-used jerk-limited trajectory
planning package, i.e., Ruckig. Yury [20] obtained the time-optimal
trajectories for third-order integrators based on the phase map in part. Some
approaches on continuous path following time-optimal trajectory planning are
proposed in [21], with an order lower than 3. However, to the best of our
knowledge, there has been no mature method available for planning 4th-order
optimal trajectories with full state constraints so far.

For higher-order problems, the control invariant set [22] and the switching
surface [23] are significant tools, which provide the nature of the objective
time-optimal problem [24]. Mitchell et al. [25] calculated the invariant set
based on level sets of a partial differential equation. Tahir et al. [26]
employed a polyhedral approximation to characterize the invariant set. Doeser
et al. [27] constructed the third-order invariant sets for integrators. He et
al. [11] excellently provided the explicitly analytic expressions of the
complete switching surfaces for the third-order problem with zero terminal
states. However, the investigation of switching surfaces for 3rd-order
problems remains incomplete, while limited studies on switching manifold have
been conducted for 4th-order or higher-order problems.

Since higher-order problems are lack of theoretical results, scholars either
apply optimization methods on a time-discrete problem or plan feasible-but-
not-optimal trajectories in engineering. Direct methods have been widely
applied in numerical optimal control problems, and some optimization solvers
have been built, such as CasADi [28], ICLOCS2 [29], and Yop [30]. However, the
time-optimal control problem in the 4th or higher order is non-convex in
discrete time, leading to high computational time and failure in obtaining
optimal solutions. Leomanni et al. [31] transformed the time-optimal problem
into sequential convex optimization problems, but the solved control exhibits
serious oscillations. Solving the time-optimal problem for high-order chain-
of-integrators systems efficiently and accurately is still challenging.

Quasi-optimal trajectories, especially in the S-curve form, are widely used in
industry. Erkorkmaz et al. [32] applied jerk-limited trajectories in the
S-curve form in high-speed CNC machining. Dai et al. [33] planned snap-limited
trajectories by solving motion time in S-curves. Ezair et al. [34] proposed a
greedy recursive trajectory planning method for the high-order problem with
full state constraints, but the method would fail for cases where the terminal
position is close to the initial position, while the terminal velocity is far
from the initial velocity. Oland et al. [35] planned suboptimal 4th-order
trajectories without any constraints to obtain a zero terminal based on
exponential activation functions. However, the current methods either
underutillize the inertia of system states or fall short of attaining full
state constraints, thereby leading to unsatisfactory in motion time.

This paper theoretically studies the time-optimal control problem for high-
order chain-of-integrators systems with full state constraints and arbitrary
initial as well as terminal states. A novel notation system and theoretical
framework is established in Section III, providing key concepts, namely, the
switching law and the optimal-trajectory manifold. A trajectory planning
method named the manifold-intercept method (MIM) is proposed in Section IV.
The contributions of this paper include the following aspects:

  1. 1.

This paper establishes a novel notation system and theoretical framework for
the classical and longstanding problem in optimal control, namely, the time-
optimal control problem for high-order chain-of-integrators systems. Through
comprehensive analysis of states and costates, the framework successfully
provides the switching manifold for high-order problems in the form of
switching laws. Notably, limited studies on the switching hypersurface have
been conducted even for 4th-order problems. This paper derives properties of
switching laws on signs as well as dimension and proposes a definite condition
of augmented switching laws, imposing a necessary-and-more-strict condition on
optimal control compared to the Bellman’s principle of optimality.

  2. 2.

Guided by the developed framework, this paper proposes an efficient trajectory
planning algorithm called manifold-intercept method (MIM). The proposed MIM
can plan near-time-optimal trajectories for 4th-order or higher-order problems
with only negligible extra motion time compared to time-optimal trajectories,
outperforming all baselines on computational time, computational accuracy, and
trajectory quality with a large gap. To the best of our knowledge, there has
been no mature method available for planning 4th-order optimal trajectories
with full state constraints so far.

  3. 3.

For 3rd-order problems, the proposed MIM can plan strictly time-optimal
trajectories with full state constraints, noting that 3rd-order trajectories
are broadly applied in the industry [1, 27]. According to the available
literature, the proposed MIM is the first 3rd-order trajectory planning method
with full state constraints. While it is claimed that Ruckig in pro version
[36] can achieve 3rd-order trajectories with full state constraints, it is
neither open-source nor explicit publishes its underlying principles.

##  II Problem Formulation

As mentioned in Section I, the time-optimal control problem for chain-of-
integrator systems is a classical problem in the theory of optimal control,
remaining challenging to plan trajectories with input saturation, full state
constraints [11], and arbitrary terminal states [18, 19]. Generally, the
problem can be summarized as

$\displaystyle\min\quad$ $\displaystyle
J=\int_{0}^{{t_{\mathrm{f}}}}\mathrm{d}t={t_{\mathrm{f}}}$ (1a)
$\displaystyle{\mathrm{s.t.}}\quad$
$\displaystyle\dot{\boldsymbol{x}}\left(t\right)={\boldsymbol{A}}{\boldsymbol{x}}\left(t\right)+{\boldsymbol{B}}u\left(t\right),\,\forall
t\in\left[0,t_{\mathrm{f}}\right],$ (1b)
$\displaystyle{\boldsymbol{x}}\left(0\right)={\boldsymbol{x}}_{0},\,{\boldsymbol{x}}\left({t_{\mathrm{f}}}\right)={\boldsymbol{x}}_{\mathrm{f}},$
(1c) $\displaystyle\left|x_{k}\left(t\right)\right|\leq M_{k},\,\forall
t\in\left[0,t_{\mathrm{f}}\right],\,k=1,2,\dots,n,$ (1d)
$\displaystyle\left|u\left(t\right)\right|\leq M_{0},\,\forall
t\in\left[0,t_{\mathrm{f}}\right],$ (1e)

where
${\boldsymbol{x}}=\left(x_{1},x_{2},\dots,x_{n}\right)\in{\mathbb{R}}^{n}$ is
the state vector of the system, $u\in{\mathbb{R}}$ is the control input.
${\boldsymbol{x}}_{0}=\left(x_{01},x_{02},\dots,x_{0n}\right)$ and
${\boldsymbol{x}}_{\mathrm{f}}=\left(x_{{\mathrm{f}}1},x_{{\mathrm{f}}2},\dots,x_{{\mathrm{f}}n}\right)$
are the assigned initial state vector and terminal state vector. $n$ is called
the order of problem (1a). The notation $\left(\bullet\right)$ means
$\left[\bullet\right]^{\top}$. In (1a),

${\boldsymbol{A}}=\left[\begin{array}[]{cccc}0&\cdots&0&0\\\ 1&\cdots&0&0\\\
\vdots&\ddots&\vdots&\vdots\\\
0&\cdots&1&0\end{array}\right]\in{\mathbb{R}}^{n\times
n},\,{\boldsymbol{B}}=\left[\begin{array}[]{c}1\\\ 0\\\ \vdots\\\
0\end{array}\right]\in{\mathbb{R}}^{n}.$ (2)

In order words, $\dot{x}_{1}=u$, and $\forall k>1$, $\dot{x}_{k+1}=x_{k}$.
When $n=4$, $x_{4}$, $x_{3}$, $x_{2}$, $x_{1}$, and $u$ are the position,
velocity, acceleration, jerk, and snap of the 1-DOF motion system,
respectively.

For convenience, denote
${\boldsymbol{M}}=\left(M_{0},M_{1},\dots,M_{n}\right)\in{\mathbb{R}}_{++}\times\overline{{\mathbb{R}}}_{++}^{n}$,
where
$\overline{{\mathbb{R}}}_{++}={\mathbb{R}}_{++}\cup\left\\{\infty\right\\}$ is
the strictly positive part of the extended real number line. $M_{k}=\infty$ if
$x_{k}$ is unconstrained. Problem (1a) is denoted as
${\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$.

In order to solve the optimal control problem (1a), according to PMP, the
Hamiltonian is constructed as

$\displaystyle{\mathcal{H}}\left({\boldsymbol{x}}\left(t\right),u\left(t\right),{\boldsymbol{\lambda}}\left(t\right),{\boldsymbol{\eta}}\left(t\right),t\right)$
(3) $\displaystyle=$ $\displaystyle
1+\lambda_{1}u+\sum_{k=2}^{n}\lambda_{k}x_{k-1}+\sum_{k=1}^{n}\eta_{k}\left(\left|x_{k}\right|-M_{k}\right),$

where
${\boldsymbol{\lambda}}\left(t\right)=\left(\lambda_{1}\left(t\right),\lambda_{2}\left(t\right),\dots,\lambda_{n}\left(t\right)\right)\in{\mathbb{R}}^{n}$
is the costate vector, satisfying the Euler-Lagrange equations [37]:

$\dot{\lambda}_{k}=-\frac{\partial{\mathcal{H}}}{\partial
x_{k}},\,k=1,2,\cdots,n,$ (4)

i.e.,

$\begin{dcases}\dot{\lambda}_{k}=-\lambda_{k+1}-\eta_{k}\,{\mathrm{sgn}}\left(x_{k}\right),\,k<n,\\\
\dot{\lambda}_{n}=-\eta_{n}\,{\mathrm{sgn}}\left(x_{n}\right).\end{dcases}$
(5)

The initial costates ${\boldsymbol{\lambda}}\left(0\right)$ and the terminal
costates ${\boldsymbol{\lambda}}\left({t_{\mathrm{f}}}\right)$ are not
assigned since ${\boldsymbol{x}}\left(0\right)$ and
${\boldsymbol{x}}\left({t_{\mathrm{f}}}\right)$ are given in problem (1a).

In (3), ${\boldsymbol{\eta}}$ is the multiplier vector induced by inequality
constraints (1d), satisfying

$\eta_{k}\geq
0,\,\eta_{k}\left(\left|x_{k}\right|-M_{k}\right)=0,\,k=1,2,\cdots,n.$ (6)

Equivalently, $\forall t\in\left[0,t_{\mathrm{f}}\right]$,
$\eta_{k}\left(t\right)\not=0$ only if
$\left|x_{k}\left(t\right)\right|=M_{k}$.

PMP gives the results that the input control $u\left(t\right)$ minimizes the
Hamiltonian ${\mathcal{H}}$, i.e.,

$u\left(t\right)\in\mathop{\arg\min}\limits_{\left|U\right|\leq
M_{0}}{\mathcal{H}}\left({\boldsymbol{x}}\left(t\right),U,{\boldsymbol{\lambda}}\left(t\right),{\boldsymbol{\eta}}\left(t\right),t\right).$
(7)

Therefore,

$u\left(t\right)=\begin{dcases}M_{0},&\lambda_{1}\left(t\right)<0\\\
*,&\lambda_{1}\left(t\right)=0\\\
-M_{0},&\lambda_{1}\left(t\right)>0\end{dcases},$ (8)

where $u\left(t\right)$ is undetermined during $\lambda_{1}\left(t\right)=0$.
It is evident that the value of $u\left(t\right)$ in a zero-measure set does
not make sense. However, a singular condition occurs when
$\lambda_{1}\left(t\right)=0$ holds for a period of time. The control law for
the triple order version of (1a) based on (8) was previously reasoned and
named the Bang-Singular-Bang time-optimal control law in [11].

The continuity of the system is guaranteed in two folds. On one hand,

$\forall
t\in\left[0,t_{\mathrm{f}}\right],\,{\mathcal{H}}\left({\boldsymbol{x}}\left(t\right),u\left(t\right),{\boldsymbol{\lambda}}\left(t\right),{\boldsymbol{\eta}}\left(t\right),t\right)\equiv
0$ (9)

holds along the time-optimal trajectory. On the other hand, the junction
condition [12] is induced as a guarantee of (9). More specifically,
${\boldsymbol{\lambda}}$ might jump when the state vector ${\boldsymbol{x}}$
enters or exits the boundaries of the inequality constraints (1d). In other
words, ${\boldsymbol{\lambda}}$ might not be continuous when
${\boldsymbol{x}}$ switches between $\left|x_{k}\right|<M_{k}$ and
$\left|x_{k}\right|=M_{k}$ for some $k$.

##  III System Behavior Analysis and Switching Laws for the Time Optimal
Control Problem

Section II has provided the problem form and some properties of system states
and costates. The Bang-Singular-Bang control law (8) indicates the importance
of the costate analysis. Section III-A and Section III-B analyze costates and
system behaviors, respectively. Then, system behaviors of problem (1a) are
classified into finite ones and the theory of the switching law is developed
in Section III-C. Finally, the definite condition of problem (1a) is induced
by the proposed augmented switching law in Section III-D.

###  III-A Jump Condition of Costates ${\boldsymbol{\lambda}}$

As mentioned in Section II, the costate vector ${\boldsymbol{\lambda}}$ might
not be continuous when the state ${\boldsymbol{x}}$ moves into or leaves the
boundary of constraints, i.e., $\left|x_{k}\right|$ increases onto $M_{k}$ or
decreases from $M_{k}$ for some $k$. The above behavior is called the junction
condition (or jump condition) [12] of costates. The junction condition has a
significant influence on the switching law of the optimal control $u$.

Assume the junction condition for $\left|x_{k}\right|\leq M_{k}$ occurs at
time $t_{1}$. Then, $\exists\mu\in{\mathbb{R}}$, s.t.

${\boldsymbol{\lambda}}\left(t_{1}^{+}\right)-{\boldsymbol{\lambda}}\left(t_{1}^{-}\right)=\mu\frac{\partial\left(\left|x_{k}\right|-M_{k}\right)}{\partial{\boldsymbol{x}}}=\mu\,{\mathrm{sgn}}\left(x_{k}\right){\boldsymbol{e}}_{k},$
(10)

where the $k$-th component of ${\boldsymbol{e}}_{k}\in{\mathbb{R}}^{n}$ is 1,
and other components of ${\boldsymbol{e}}_{k}$ are 0. In order words, $\forall
i\not=k$, $\lambda_{i}$ is continuous at $t_{1}$, while $\lambda_{k}$ might
jump at $t_{1}$.

Two cases for the junction condition exist where (a) $\left|x_{k}\right|\equiv
M_{k}$ for a period of time, and where (b) $\left|x_{k}\right|$ touches
$M_{k}$ at a single time point. The above two cases will be discussed in
Section III-A1 and Section III-A2. Among them, Case 1 induces the system
behavior defined in Definition 1, while Case 2 induces the tangent marker in
Definition 8.

####  III-A1 Case 1. $\left|x_{k}\right|\equiv M_{k}$ for A Period of Time

Without loss of generality, assume that $\forall
t\in\left[t_{1},t_{2}\right]$, $x_{k}\left(t\right)\equiv M_{k}$, and
$\exists\delta>0$, $\forall t\in\left(t_{1}-\delta,t_{1}\right)$,
$x_{k}\left(t\right)<M_{k}$. The case where $x_{k}$ leaves $M_{k}$ or
$x_{k}\equiv-M_{k}$ can be reasoned similarly.

During $t\in\left[t_{1},t_{2}\right]$, $x_{k}\equiv M_{k}$, $\dot{x}_{1}=u$,
and $\forall i<k$, $\dot{x}_{i+1}=x_{i}$. Hence, $u\equiv 0$ and $\forall
i<k$, $x_{i}\equiv 0$. Therefore, $\forall i>k$, $\left|x_{i}\right|<M_{i}$
holds except at finite time points, and $\forall i<k$,
$\left|x_{i}\right|\equiv 0<M_{i}$. From (6) and (8), $\lambda_{1}\equiv 0$,
and $\forall i\not=k$, $\eta_{i}\equiv 0$ almost everywhere. It can be
reasoned from (5) that $\forall i\leq k$, $\lambda_{i}\equiv 0$. The term
“almost everywhere” means a proposition holds for all points except for a
zero-measure set [38].

Furthermore, for the case where $k=1$, $\lambda_{1}$ keeps continuous despite
the junction condition (10). According to (9),

${\mathcal{H}}^{\pm}=1+\lambda_{1}^{+}u^{\pm}+\sum_{k=2}^{n}\lambda_{k}x_{k-1}=0.$
(11)

Hence, $\lambda_{1}^{+}u^{+}=\lambda_{1}^{-}u^{-}$. The notation
$\bullet^{\pm}$ means the left and right-hand limits of variable $\bullet$ at
the junction time. Note that $u^{+}=0$, $u^{-}=M_{0}$, $\lambda_{1}^{+}=0$, so
$\lambda_{1}^{-}=0$. Therefore, $\lambda_{1}^{+}=\lambda_{1}^{-}=0$, i.e.,
$\lambda_{1}$ keeps continuous at the junction time.

As a corollary, (8) can be written as follows:

######  Proposition 1 (Bang-Singular-Bang Control Law).

The optimal control $u\left(t\right)$ of (1a) satisfies

$u\left(t\right)=\begin{dcases}M_{0},&\lambda_{1}\left(t\right)<0\\\
0,&\lambda_{1}\left(t\right)=0\\\
-M_{0},&\lambda_{1}\left(t\right)>0\end{dcases}$ (12)

almost everywhere.

###### Proof.

$\lambda_{1}\left(t\right)\equiv 0$ occurs only if $\exists k$, s.t.
$\left|x_{k}\left(t\right)\right|\equiv M_{k}$, or else
${\boldsymbol{\lambda}}\left(t\right)\equiv{\boldsymbol{0}}$. Note that
${\boldsymbol{\lambda}}\left(t\right)\equiv{\boldsymbol{0}}$ contradicts (9).
Therefore, $u\left(t\right)\equiv 0$ if $\lambda_{1}\left(t\right)\equiv 0$.
Hence, (12) is equivalent to (8) almost everywhere. ∎

It is noted that the case where $\left|x_{n}\right|\equiv M_{n}$ for a period
of time does not exist. By contradiction, if $\exists t_{1}<t_{2}$, s.t.
$\forall t\in\left[t_{1},t_{2}\right]$,
$\left|x_{n}\left(t\right)\right|\equiv M_{n}$, then
$x_{1}\left(t\right)=x_{2}\left(t\right)=\dots=x_{n-1}\left(t\right)=0$ during
$t\in\left[t_{1},t_{2}\right]$. In other words, the system state vector
${\boldsymbol{x}}$ parks at $\pm M_{n}{\boldsymbol{e}}_{n}$ for a period of
time, which contradicts the time-optimality. As a corollary, (5) can be
written as

$\begin{dcases}\dot{\lambda}_{k}=-\lambda_{k+1}-\eta_{k}\,{\mathrm{sgn}}\left(x_{k}\right),\,k<n,\\\
\dot{\lambda}_{n}=0,\end{dcases}$ (13)

since $\eta_{n}=0$ almost everywhere. It should be pointed out that
$\lambda_{n}$ might not be constant though $\dot{\lambda}_{n}=0$, because
$\lambda_{n}$ might jump when $\left|x_{n}\right|$ touches $M_{n}$ in the
following Case 2.

####  III-A2 Case 2. $\left|x_{k}\right|$ Touches $M_{k}$ at A Single Time
Point

Without loss of generality, assume that $x_{k}$ touches $M_{k}$ at $t_{1}$,
i.e., $x_{k}\left(t_{1}\right)=M_{k}$, and $\exists\delta>0$, $\forall
t\in\left(t_{1}-\delta,t_{1}\right)\cup\left(t_{1},t_{1}+\delta\right)$,
$x_{k}\left(t\right)<M_{k}$. The case where $x_{k}$ touches $-M_{k}$ can be
reasoned similarly.

The case where $k=1$ is evident to Case 1 in Section III-A1. For the case
where $k\geq 2$, $\exists\varepsilon\in\left(0,\delta\right)$, s.t. $\forall
t\in\left(0,\varepsilon\right)$, $u\left(t_{1}+t\right)\equiv u^{+}$, and
$u\left(t_{1}-t\right)\equiv u^{-}$. According to (1b), $\forall
t\in\left[0,\varepsilon\right]$,

$\displaystyle
x_{k}\left(t_{1}+t\right)=\sum_{i=1}^{k}\frac{x_{k-i}\left(t_{1}\right)}{i!}t^{i}+\frac{u^{+}}{k!}t^{k}\leq
M_{k},$ (14aa) $\displaystyle
x_{k}\left(t_{1}-t\right)=\sum_{i=1}^{k}\frac{x_{k-i}\left(t_{1}\right)}{i!}\left(-t\right)^{i}+\frac{u^{-}}{k!}\left(-t\right)^{k}\leq
M_{k}.$ (14ab)

(14aa) and (14ab) hold with equality if and only if $t=0$. Considering the
Taylor expansions of $x_{k}$ at $t_{1}$ on each side, it is evident that
$\exists l\in{\mathbb{N}}$, s.t. $x_{k-1}=x_{k-2}=\dots=x_{k-2l+1}=0$ at
$t_{1}$, and (a) if $2l<k$, then $x_{k-2l}<0$, or (b) if $2l=k$, then
$u\left(t_{1}^{+}\right)=-M_{0}$,
$u\left(t_{1}^{-}\right)=\left(-1\right)^{k-1}M_{0}$.

Case 2 does not lead to the junction of ${\boldsymbol{\lambda}}$ for cases
where (a) $n\leq 2$, and where (b) $n=3$ with $M_{3}=\infty$. For the case
where $n=3$ with $M_{3}<\infty$, the jerk-limited time-optimal problem under
position constraints is still not well solved yet. He et al. [11] developed
the analytic expression of switching surfaces in triple order with zero
terminal states, where Case 2, fortunately, does not exist with zero terminal
states. Reflexxes [18] and Ruckig in community version [19], the two most
famous open-source online trajectory planning packages, are not able to plan
jerk-limited trajectories under position constraints.

However, Case 2 is common in high-order problems. Some examples will be
provided in Section III-D.

###  III-B System Behavior

The case where the inequality constraints (1d) hold strictly, i.e., $\forall
k$, $\left|x_{k}\right|<M_{k}$, is first considered. Bellman’s principle of
optimality [39] is necessary to introduce.

######  Lemma 1 (Bellman’s Principle of Optimality).

An optimal path has the property that whatever the initial conditions and
control variables are, the remaining chosen control must be optimal for the
remaining problem, with the state resulting from the early controls taken to
be the initial condition.

For problem (1a), i.e.,
${\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$,
Bellman’s principle of optimality gives the results that if
$\forall\left[t_{1},t_{2}\right]\subset\left[0,{t_{\mathrm{f}}}\right]$,
$u=u^{*}\left(t\right)$ is the optimal control and
${\boldsymbol{x}}={\boldsymbol{x}}^{*}\left(t\right)$ is the optimal
trajectory, then $\left.u^{*}\right|_{t\in\left[t_{1},t_{2}\right]}$ is the
optimal control of problem
${\mathcal{P}}\left({\boldsymbol{x}}^{*}\left(t_{1}\right),{\boldsymbol{x}}^{*}\left(t_{2}\right);{\boldsymbol{M}}\right)$.

For the case where $\forall t\in\left[t_{1},t_{2}\right]$, $\forall k$,
$\left|x_{k}\left(t\right)\right|<M_{k}$, it is evident that
${\boldsymbol{\lambda}}$ does not jump and
${\boldsymbol{\eta}}\equiv{\boldsymbol{0}}$ in this period. Therefore,

$\begin{dcases}\dot{\lambda}_{k}=-\lambda_{k+1},\,k<n\\\
\dot{\lambda}_{n}=0\end{dcases},\text{ if }\forall
t\in\left[t_{1},t_{2}\right],\,\left|{\boldsymbol{x}}\left(t\right)\right|<{\boldsymbol{M}}.$
(15)

Since ${\boldsymbol{\lambda}}$ is continuous, (15) indicates that
$\lambda_{k}\left(t\right)$ is an $\left(n-k\right)$-th degree polynomial for
$t\in\left[t_{1},t_{2}\right]$. According to the fundamental theorem of
algebra [38], $\lambda_{k}$ has no more than $\left(n-k\right)$ roots during
$t\in\left[t_{1},t_{2}\right]$. A corollary, also well-known in previous works
[13], is reasoned as follows:

######  Proposition 2.

Assume $\forall t\in\left[t_{1},t_{2}\right]$, $\forall k$,
$\left|x_{k}\right|<M_{k}$ in problem (1a). Then, the optimal control $u$
switches between $M_{0}$ and $-M_{0}$ with no more than $\left(n-1\right)$
times.

###### Proof.

If $\forall t\in\left[t_{1},t_{2}\right]$, $\forall k$,
$\left|x_{k}\right|<M_{k}$, then ${\boldsymbol{\lambda}}$ is continuous.
Specifically, $\lambda_{n}\equiv\mathrm{const}$. By (15), $\lambda_{1}$ is an
$\left(n-1\right)$-th degree polynomial for $t\in\left[t_{1},t_{2}\right]$.
Note that $\lambda_{1}\equiv 0$ contradicts (9) when
${\boldsymbol{\eta}}\equiv 0$,. Therefore, $\lambda_{1}$ has no more than
$\left(n-1\right)$ roots. According to Proposition 1, $u$ switches between
$M_{0}$ and $-M_{0}$ with no more than $\left(n-1\right)$ times. ∎

The properties of states and costates reasoned in Section III-A and Section
III-B is summarized as follows:

######  Theorem 1.

The following propositions hold for the optimal control of problem (1a).

  1. 1.

$u\left(t\right)=-{\mathrm{sgn}}\left(\lambda_{1}\left(t\right)\right)$ almost
everywhere. Specifically, $u\left(t\right)=0$ if
$\lambda_{1}\left(t\right)=0$.

  2. 2.

$\forall t\in\left[0,{t_{\mathrm{f}}}\right]$,
${\boldsymbol{\lambda}}\left(t\right)\not={\boldsymbol{0}}$.

  3. 3.

$\lambda_{1}$ and ${\boldsymbol{x}}$ are continuous, while
$\lambda_{k}\left(k>1\right)$ might jump at junction time.

  4. 4.

$\lambda_{k}$ consists of $\left(n-k\right)$-th degree polynomials and zero.
Specifically, $\lambda_{1}\equiv 0$ for a period of time if and only if
$\exists k,\left|x_{k}\right|\equiv M_{k}$ during the period of time.

  5. 5.

For $k<n$, cases might exist where $\exists t_{1}<t_{2},\delta>0$, s.t.
$\forall t\in\left[t_{1},t_{2}\right]$,
$\left|x_{k}\left(t\right)\right|\equiv M_{k}$, while $\forall
t\in\left(t_{1}-\delta,t_{1}\right)\cup\left(t_{2},t_{2}+\delta\right)$,
$\left|x_{k}\left(t\right)\right|<M_{k}$. Then, $\forall
t\in\left[t_{1},t_{2}\right]$,

    1. (a)

$\forall i\leq k$, $\lambda_{i}\left(t\right)\equiv 0$, while $\forall i>k$,
$\lambda_{i}\left(t\right)$ is an $\left(n-i\right)$-th degree polynomial.
Furthermore, $\lambda_{k+1}\left(t\right)$ is not always zero.

    2. (b)

$u\left(t\right)\equiv 0$. $\forall i<k$, $x_{i}\left(t\right)\equiv 0$.

    3. (c)

$\forall 1<k<n$, $\lambda_{k}$ might jump at $t_{1}$ and $t_{2}$.

  6. 6.

For $k>2$, case might exist where $\exists
t_{1}\in\left(0,{t_{\mathrm{f}}}\right)$, $\delta>0$, s.t.
$\left|x_{k}\left(t_{1}\right)\right|=M_{k}$, while $\forall
t\in\left(t_{1}-\delta,t_{1}\right)\cup\left(t_{1},t_{1}+\delta\right)$,
$\left|x_{k}\left(t\right)\right|<M_{k}$. Then, at time $t_{1}$, $\exists
l\leq\frac{k}{2}$, s.t. $\left|x_{k}\right|=M_{k}$,
$x_{k-1}=x_{k-2}=\dots=x_{k-2l+1}=0$.

    1. (a)

If $2l<k$, $x_{k-2l}\left(t_{1}\right)\not=0$.

    2. (b)

If $2l=k$,
$u\left(t_{1}^{+}\right)=\left(-1\right)^{k}u\left(t_{1}^{-}\right)=-\frac{M_{0}}{M_{k}}x_{k}\left(t_{1}\right)$.

###### Proof.

Based on discussion in Section III-A and Section III-B, Theorem 1 is evident.
∎

Theorem 1 provides all options of system behavior in a single stage. The
system behavior is classified into finite ones and is proposed formally as
follows:

######  Definition 1.

System behavior of problem (1a) at a single stage is denoted as follows:

  1. 1.

The stage where $u\equiv M_{0}\,\left(-M_{0}\right)$ is denoted as
$\overline{0}\,\left(\underline{0}\right)$.

  2. 2.

The stage where $x_{k}\equiv M_{k}\,\left(-M_{k}\right),u\equiv 0$, and
$\forall i<k,x_{i}\equiv 0$, is denoted as
$\overline{k}\,\left(\underline{k}\right)$.

  3. 3.

$\forall 0\leq k\leq n$, the signs of $\overline{k}$ and $\underline{k}$ are
denoted as ${\mathrm{sgn}}\left(\overline{k}\right)=1$,
${\mathrm{sgn}}\left(\underline{k}\right)=-1$. The value of $\overline{k}$ and
$\underline{k}$ is denoted as
$\left|\overline{k}\right|=\left|\underline{k}\right|=k$.

Based on Theorem 1 and Definition 1, system behaviors during the whole moving
process can be studied. In the following, the sign of a system behavior can be
left out if no ambiguity exists.

###  III-C Switching Law and Optimal-Trajectory Manifold

Building on the analysis of the system behavior in Section III-A and Section
III-B, this section focuses on how the system behavior switches along the
time-optimal trajectory. The core idea in this section is the switching law
and the optimal-trajectory manifold for time-optimal control, which are
defined in Section III-C1. The properties of the switching law on dimension
and sign are reasoned in Section III-C2 and Section III-C3, respectively.

####  III-C1 Definitions

######  Definition 2.

Given a problem ${\mathcal{P}}$, assume the time-optimal trajectory passes
through system behaviors $s_{1},s_{2},\dots,s_{N}$ successively. Then, the
series of system behaviors $S=s_{1}s_{2}\cdots s_{N}$ is called the switching
law w.r.t. ${\mathcal{P}}$, denoted as
$S={\mathcal{S}}\left({\mathcal{P}}\right)$, where $N\in{\mathbb{N}}$ is
called the length of $S$.

As an example, two second-order optimal problems with the same terminal state
vector are shown in Fig. 2. The switching law of
${\mathcal{P}}_{1}={\mathcal{P}}\left({\boldsymbol{x}}_{0}^{\left(1\right)},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$
is
${\mathcal{S}}\left({\mathcal{P}}_{1}\right)=\underline{0}\underline{1}\overline{0}$.
In other word, along the time-optimal trajectory from
${\boldsymbol{x}}_{0}^{\left(1\right)}$ to ${\boldsymbol{x}}_{\mathrm{f}}$,
$\exists 0<t_{1}<t_{2}<t_{3}<{t_{\mathrm{f}}}$, s.t.

  1. 1.

$\underline{0}$: $\forall t\in\left(0,t_{1}\right),u\left(t\right)\equiv-
M_{0}$. ${\boldsymbol{x}}$ starts from
${\boldsymbol{x}}\left(0\right)={\boldsymbol{x}}_{0}^{\left(1\right)}$, and
$x_{1}$ enters $-M_{1}$ at $t_{1}$.

  2. 2.

$\underline{1}$: $\forall
t\in\left(t_{1},t_{2}\right),x_{1}\left(t\right)\equiv-M_{1}$, while
$u\left(t\right)\equiv-M_{0}$.

  3. 3.

$\overline{0}$: $\forall
t\in\left(t_{2},t_{\mathrm{f}}\right),u\left(t\right)\equiv M_{0}$. $x_{1}$
exits $-M_{1}$ at $t_{2}$, and the system state vector ${\boldsymbol{x}}$
reaches the terminal state vector ${\boldsymbol{x}}_{\mathrm{f}}$ at
${t_{\mathrm{f}}}$.

A similar analysis applies to
${\mathcal{P}}_{2}={\mathcal{P}}\left({\boldsymbol{x}}_{0}^{\left(2\right)},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$.
From examples shown in Fig. 2, it can be observed that the switching law
depends on the initial states.

Notably, a switching law focuses on how the system behavior switches along the
trajectory, without information on motion time. The optimal control of problem
(1a) consists of a switching law and motion time for every system behavior,
the definite condition of which will be discussed in Section III-D.

For an $n$-th order problem (1a), a natural approach is to employ mathematical
induction, utilizing the solutions of lower-order problems to solve the $n$-th
order problem. The above idea induces the definitions of sub-problem and
optimal-trajectory manifold.

For convenience of discussion, the optimal trajectory, the optimal control,
and the terminal time of a problem ${\mathcal{P}}$ are denoted as
${\boldsymbol{x}}^{*}\left(t;{\mathcal{P}}\right),u^{*}\left(t;{\mathcal{P}}\right)$,
and $t_{\mathrm{f}}^{*}\left({\mathcal{P}}\right)$, respectively.

Figure 2: Switching laws for 2 optimal problems with the same terminal state
vector ${\boldsymbol{x}}_{\mathrm{f}}$, i.e.,
${\mathcal{P}}_{1}={\mathcal{P}}\left({\boldsymbol{x}}_{0}^{\left(1\right)},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$
and
${\mathcal{P}}_{2}={\mathcal{P}}\left({\boldsymbol{x}}_{0}^{\left(2\right)},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$.
Among them, $n=2$, ${\boldsymbol{M}}=\left(1,1,1\right)$,
$\mathcal{S}\left({\mathcal{P}}_{1}\right)=\underline{0}\underline{1}\overline{0}$,
and $\mathcal{S}\left({\mathcal{P}}_{2}\right)=\underline{0}\overline{0}$.

######  Definition 3.

For an $n$-th order problem
${\mathcal{P}}={\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)$
and $1\leq a_{i}\leq b_{i}\leq n$, a sub-problem of ${\mathcal{P}}$ is
$\hat{{\mathcal{P}}}={\mathcal{P}}\left({\boldsymbol{x}}_{0}^{a_{1}:b_{1}},{\boldsymbol{x}}_{\mathrm{f}}^{a_{2}:b_{2}};{\boldsymbol{M}}^{a_{3}:b_{3}}\right)$,
i.e., the time-optimal problem for chain-of-integrators with the following
constraints:

$\begin{dcases}x_{k}\left(0\right)=x_{0k},&a_{1}\leq k\leq b_{1}\\\
x_{k}\left({t_{\mathrm{f}}}\right)=x_{{\mathrm{f}}k},&a_{2}\leq k\leq
b_{2},\\\ \left|x_{k}\right|\leq M_{k},&a_{3}\leq k\leq b_{3}.\end{dcases}$
(16)

######  Definition 4.

Two time-optimal problems ${\mathcal{P}}_{1},{\mathcal{P}}_{2}$ are
equivalent, denoted as ${\mathcal{P}}_{1}\Leftrightarrow{\mathcal{P}}_{2}$, if
${\mathcal{P}}_{1}$ and ${\mathcal{P}}_{2}$ have the same solution of optimal
control, i.e.,

$\begin{dcases}t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)=t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{2}\right),\\\
u^{*}\left(t,{\mathcal{P}}_{1}\right)=u^{*}\left(t,{\mathcal{P}}_{2}\right),\forall
t\in\left[0,t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)\right].\end{dcases}$
(17a)

Two problems are equivalent when their boundary conditions can replace each
other. Specifically, if a problem ${\mathcal{P}}$ is equivalent to its sub-
problem $\hat{{\mathcal{P}}}$, then some boundary conditions and constraints
of ${\mathcal{P}}$ is unnecessary. The above observation contributes to the
following definition:

######  Definition 5.

Given ${{\boldsymbol{x}}_{\mathrm{f}}}\in{\mathbb{R}}^{n}$ and
${\boldsymbol{M}}\in{\mathbb{R}}_{++}\times\overline{{\mathbb{R}}}_{++}^{n}$,
define

$\displaystyle{\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)\triangleq\bigcup\left\\{{\boldsymbol{x}}_{0}\in{\mathbb{R}}^{n}:{\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)\Leftrightarrow\right.$
(18)
$\displaystyle\left.{\mathcal{P}}\left({\boldsymbol{x}}_{0}^{1:k},{\boldsymbol{x}}_{\mathrm{f}}^{1:k};{\boldsymbol{M}}\right)\text{
are feasible}\right\\}$

as the $k$-th order optimal-trajectory manifold of
${{\boldsymbol{x}}_{\mathrm{f}}}$ under ${\boldsymbol{M}}$.

“$\Leftrightarrow$” is evidently an equivalence relation among the set of
feasible time-optimal control problems, which ensures the well-definedness of
(18). Intuitively,
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
consists of time-optimal trajectories for $k$-th order sub-problem, with high-
dimensional components of states additionally. The following proposition
provides the nature of
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.

######  Proposition 3.

If
${\mathcal{F}}_{k}={\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)\not=\varnothing$
in (18), then ${\mathcal{F}}_{k}$ is an embedding of ${\mathbb{R}}^{k}$ into
${\mathbb{R}}^{n}$, and ${\mathcal{F}}_{k}$ is of dimension $k$.

###### Proof.

$\forall{\boldsymbol{x}}_{0},{\boldsymbol{y}}_{0}\in{\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
$\forall i\leq k$, $x_{0i}=y_{0i}$. According to the definition in (18),
${\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)\Leftrightarrow{\mathcal{P}}\left({\boldsymbol{y}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$,
whose optimal control is assumed as $u=u^{*}\left(t\right)$,
$t\in\left[0,t_{\mathrm{f}}^{*}\right]$. Note that ${\boldsymbol{x}}_{0}$ and
${\boldsymbol{y}}_{0}$ have the same control and terminal time; hence,
${\boldsymbol{x}}_{0}={\boldsymbol{y}}_{0}$. Therefore, elements in
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
are uniquely determined by their first $k$ components. Considering the
continuity of optimal control,
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
is an embedding of ${\mathbb{R}}^{k}$ into ${\mathbb{R}}^{n}$. ∎

######  Definition 6.

Assume
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)\not=\varnothing$
in (18). The switching-law representation of
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
is defined as

$\mathcal{SF}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)=\bigcup_{{\boldsymbol{x}}_{0}\in{\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)}\left\\{{\mathcal{S}}\left({\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)\right)\right\\}.$
(19)

An example of the second-order optimal-trajectory manifold is shown in Fig.
3(a). It is indicated in Fig. 3(a) that
$\mathcal{SF}_{2}\left({\boldsymbol{0}},{\boldsymbol{M}}\right)=${$\overline{0}$,
$\underline{0}\overline{0}$, $\underline{1}\overline{0}$,
$\underline{0}\underline{1}\overline{0}$, $\underline{0}$,
$\overline{0}\underline{0}$, $\overline{1}\underline{0}$,
$\overline{0}\overline{1}\underline{0}$}. Fig. 3(a) is corresponding to
$\mathcal{F}_{2}\left({\boldsymbol{0}},{\boldsymbol{M}}\right)$ in Fig. 3(b).
It can be observed that
$\mathcal{F}_{2}\left({\boldsymbol{0}},{\boldsymbol{M}}\right)\subset\mathcal{F}_{3}\left({\boldsymbol{0}},{\boldsymbol{M}}\right)$
is a sub-manifold of 2 dimension, which confirms the validity of Proposition
3. Furthermore, every switching law induces a sub-manifold, i.e., any given
sub-manifold of
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
can be represented by a subset of
$\mathcal{SF}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.

####  III-C2 Dimension Property of the Switching Law

The dimension analysis of the optimal-trajectory manifold is indispensable to
solve the time-optimal problem (1a). For a given sub-manifold represented by a
switching law $S$, the motion time of every stage can be solved only if $S$ is
of $n$ dimension.

######  Theorem 2.

Given a sub-manifold represented by a switching law $S=s_{1}s_{2}\dots
s_{N}\in\mathcal{SF}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
the dimension of the manifold is

$\dim S=\sum_{i=1}^{N}\left(1-s_{i}\right)=N-\sum_{i=1}^{N}s_{i}.$ (20)

Furthermore, if $\left|s_{i}\right|\not=0$, then
$\left|s_{i-1}\right|=\left|s_{i+1}\right|=0$.

###### Proof.

Without loss of generality, assume $k=n$, or else it can be considered as
$S\in\mathcal{SF}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}}^{1:k},{\boldsymbol{M}}\right)$
equivalently.

Assume ${\boldsymbol{x}}_{i}\in{\mathbb{R}}^{n},i=0,1,\dots,N$ and $t_{i}\geq
0,i=1,2,\dots,N$. ${\boldsymbol{x}}_{i-1}$ moves to ${\boldsymbol{x}}_{i}$
after time $t_{i}$ under the system behavior $s_{i}$, $\forall i=1,2,\dots,N$.
The terminal state vector
${\boldsymbol{x}}_{N}={{\boldsymbol{x}}_{\mathrm{f}}}$ is given.

Under the system behavior $s_{i}$, the control $u\equiv u_{i}$, where

$u_{i}=\begin{dcases}{\mathrm{sgn}}\left(s_{i}\right)M_{0},&\left|s_{i}\right|=0\\\
0,&\text{otherwise}\\\ \end{dcases}.$ (21)

Under the system behavior $\left|s_{i}\right|=0$, $Nn$ constraints on state
transformation exists, i.e., $\forall i=1,2,\dots,N,k=1,2,\dots,n$,

$x_{i,k}=\sum_{j=1}^{k}\dfrac{1}{\left(k-j\right)!}x_{i-1,j}t_{i}^{k-j}+\dfrac{1}{k!}u_{i}t_{i}^{k}.$
(22)

Under the system behavior $\left|s_{i}\right|\not=0$, the states satisfy

$\begin{dcases}x_{i-1,j}=0,j<\left|s_{i}\right|\\\
x_{i-1,s_{i}}={\mathrm{sgn}}\left(s_{i}\right)M_{\left|s_{i}\right|}\end{dcases},\forall
i=1,2,\dots,N.$ (23)

(23) provides
$\sum_{\left|s_{i}\right|\not=0}\left|s_{i}\right|=\sum_{i=1}^{N}\left|s_{i}\right|$
constraints.

In summary, there exists $Nn+\sum_{i=1}^{N}\left|s_{i}\right|$ constraints and
$\left(n+1\right)N$ variables in total. Therefore, the degree of freedom,
i.e., the dimension of $S$, is

$\dim
S=\left(n+1\right)N-\left(Nn+\sum_{i=1}^{N}\left|s_{i}\right|\right)=N-\sum_{i=1}^{N}\left|s_{i}\right|.$
(24)

Assume $\left|s_{i}\right|,\left|s_{i-1}\right|\not=0$. Note that
$s_{i-1}\not=s_{i}$, and assume $\left|s_{i}\right|<\left|s_{i-1}\right|$.
According to (23),
$\left|x_{\left|s_{i}\right|}\left(T_{i}^{+}\right)\right|=M_{\left|s_{i}\right|}$,
where $T_{i}$ is the switching time between $s_{i-1}$ and $s_{i}$. However,
$x_{\left|s_{i}\right|}\left(T_{i}^{-}\right)=0$; hence,
$x_{\left|s_{i}\right|}$ is not continuous at $T_{i}$, which leads to a
contradiction. Therefore, if $\left|s_{i}\right|\not=0$, then
$\left|s_{i-1}\right|=\left|s_{i+1}\right|=0$. ∎

Figure 3: Examples of the optimal-trajectory manifolds. (a) Second order.
${\boldsymbol{M}}=\left(1,1,1.5\right),{{\boldsymbol{x}}_{\mathrm{f}}}={\boldsymbol{0}}$.
(b) Third order.
${\boldsymbol{M}}=\left(1,1,1.5,4\right),{{\boldsymbol{x}}_{\mathrm{f}}}={\boldsymbol{0}}$.

Take the optimal-trajectory manifold in Fig. 3(b) as an example. $0$, $10$,
and $2010$ are of 1 dimension. $00$, $1010$, and $02010$ are of 2 dimension.
$000$, $0010$, and $0102010$ are of 3 dimension. Similar analysis applies to
other sub-manifolds.

####  III-C3 Sign Property of the Switching Law

From Fig. 3(b), it is observed that $\underline{2}\overline{01}\underline{0}$
exists while $\overline{201}\underline{0}$ and
$\underline{20}\overline{1}\underline{0}$ do not exist. The analysis of the
sign is given as the following theorem.

######  Theorem 3.

$\forall S=s_{1}s_{2}\dots
s_{N}\in\mathcal{SF}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
$\forall i=2,3,\dots,N$,

${\mathrm{sgn}}\left(s_{i-1}\right)=\begin{dcases}{\mathrm{sgn}}\left(s_{i}\right),&\text{if
}\left|s_{i}\right|\text{ is odd}\\\
-{\mathrm{sgn}}\left(s_{i}\right),&\text{if }\left|s_{i}\right|\text{ is
even}\end{dcases}.$ (25)

###### Proof.

For the case where $\left|s_{i-1}\right|=0,\,\left|s_{i}\right|=0$, note that
$s_{i}\not=s_{i-1}$. Hence,
${\mathrm{sgn}}\left(s_{i-1}\right)=-{\mathrm{sgn}}\left(s_{i}\right)$.

For the case where $\left|s_{i-1}\right|\not=0$, according to Theorem 2,
$\left|s_{i}\right|=0$. Denote the switching time between $s_{i-1}$ and
$s_{i}$ as $T_{i}$. If ${\mathrm{sgn}}\left(s_{i-1}\right)=+1$, then $\forall
k<\left|s_{i-1}\right|$, $x_{k}\left(T_{i}\right)=0$ and
$x_{\left|s_{i-1}\right|}\left(T_{i}\right)=M_{\left|s_{i-1}\right|}$. Assume
that ${\mathrm{sgn}}\left(s_{i}\right)=+1$, i.e., $u\equiv u_{i}=M_{0}$ during
$s_{i}$. Then, $\exists\delta>0$, $\forall t\in\left(0,\delta\right)$,
$x_{\left|s_{i-1}\right|}\left(T_{i}+t\right)=M_{\left|s_{i-1}\right|}+\frac{1}{\left|s_{i-1}\right|!}M_{0}t^{\left|s_{i-1}\right|}>M_{\left|s_{i-1}\right|}$
since $u\equiv M_{0}$, which leads to a contradiction. Therefore,
${\mathrm{sgn}}\left(s_{i-1}\right)=+1$ if
${\mathrm{sgn}}\left(s_{i}\right)=-1$. For the similar analysis,
${\mathrm{sgn}}\left(s_{i-1}\right)=-1$ if
${\mathrm{sgn}}\left(s_{i}\right)=+1$.

For the case where $\left|s_{i}\right|\not=0$, according to Theorem 2,
$\left|s_{i-1}\right|=0$. Denote the switching time between $s_{i-1}$ and
$s_{i}$ as $T_{i}$. $x_{k}\left(T_{i}\right)=0$ for $k<\left|s_{i}\right|$ and
$x_{\left|s_{i}\right|}\left(T_{i}\right)=M_{\left|s_{i}\right|}$. Then,
$\exists\delta>0$, $\forall t\in\left(0,\delta\right)$,
$u\left(T_{i}-t\right)\equiv{\mathrm{sgn}}\left(s_{i}\right)M_{0}$; hence,
$x_{\left|s_{i}\right|}\left(T_{i}-t\right)={\mathrm{sgn}}\left(s_{i}\right)M_{\left|s_{i}\right|}+\frac{1}{\left|s_{i}\right|!}M_{0}t^{\left|s_{i}\right|}\cdot\left(-1\right)^{\left|s_{i}\right|}{\mathrm{sgn}}\left(s_{i-1}\right)$.
The constraint where
$\left|x_{\left|s_{i}\right|}\left(T_{i}-t\right)\right|\leq
M_{\left|s_{i}\right|}$ resulting that
$\left(-1\right)^{\left|s_{i}\right|}{\mathrm{sgn}}\left(s_{i-1}\right)=-{\mathrm{sgn}}\left(s_{i}\right)$.∎

Theorem 3 indicates that signs of all system behaviors in a switching law is
uniquely determined by the sign of the last system behavior. For a switching
law $S$ with a length of $N$, Theorem 3 reduces the possible signs of all
system behaviors from $2^{N}$ to $2$.

###  III-D Augmented Switching Law and Tangent Marker

For a given initial state vector ${\boldsymbol{x}}_{0}\in{\mathbb{R}}^{n}$ and
the switching law
$S=\mathcal{S}\left({\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)\right)$,
Section III-C2 has pointed out that the optimal control can be solved only if
$\dim S=n$. For example, in Fig. 3(a),
${\boldsymbol{x}}_{0}\in{\mathbb{R}}^{2}$ is given. If
$S=\overline{0}\underline{0}$, then the full definite conditions are provided
by (21), (22), and (23). However, $S=\overline{0}\underline{0}\overline{0}$ is
underdefined, while $S=\overline{0}$ is overdefined.

######  Definition 7.

Given a problem ${\mathcal{P}}$, the augmented switching law w.r.t.
${\mathcal{P}}$, denoted as $S=\mathcal{AS}\left({\mathcal{P}}\right)$, is the
switching law of ${\mathcal{P}}$ attached full definite conditions.

In the above example, $\overline{0}\underline{0}$ can be an augmented
switching law, while $\overline{0}\underline{0}\overline{0}$ and
$\overline{0}$ are not. As a trick, the switching law $\overline{0}$ can be
represented by an augmented switching law $\underline{0}\overline{0}$, where
the motion time of $\underline{0}$ is 0 by (21), (22), and (23).

However, the above trick of adding zero is not enough to provide all augmented
switching laws. According to discussion on Case 2 in Section III-A2, the
tangent marker is necessary to define and precisely exists in the time-optimal
problem (1a).

######  Definition 8.

For a time-optimal trajectory, assume $x_{k}$ touches $\left|M_{k}\right|$ at
$t_{1}$ with an even degree $2l\leq k$ as described in Theorem 6,
$\left|s\right|\geq 3$. Then, the tangent marker is denoted as
$\left(s,2l\right)$, where $\left|s\right|=k$ and
${\mathrm{sgn}}\left(s\right)={\mathrm{sgn}}\left(x_{k}\left(t_{1}\right)\right)$.

######  Theorem 4.

For an augmented switching law $S=S_{1}\left(s,2l\right)S_{2}$, where
$S_{i}=s_{1}^{\left(i\right)}s_{2}^{\left(i\right)}\dots
s_{N_{i}}^{\left(i\right)}$ are augmented switching laws, $i=1,2$, then,

  1. 1.

$\left|s_{N_{1}}^{\left(1\right)}\right|=\left|s_{1}^{\left(2\right)}\right|=0$.

  2. 2.

$\left(s,2l\right)$ contributes $-2l<0$ dimension.

###### Proof.

According to Section III-A2,
$\left|s_{N_{1}}^{\left(1\right)}\right|=\left|s_{1}^{\left(2\right)}\right|=0$,
and $\left(s,2l\right)$ does not provide extra motion time. Assume the state
vector at $\left(s,2l\right)$ is ${\boldsymbol{x}}_{1}$. Then,
$\left(s,2l\right)$ contributes $2l$ extra constraints, i.e.,
$x_{1,\left|s\right|}={\mathrm{sgn}}\left(s\right)M_{\left|s\right|}$, and
$x_{1,\left|s\right|-k}=0$, $k=1,2,\dots,2l-1$. Hence, $\left(s,2l\right)$
contributes $-2l$ dimension. ∎

Fig. 4 shows an example of the tangent marker. From Fig. 4(a), it is observed
that
$\mathcal{S}\left({\mathcal{P}}\right)=\overline{0}\underline{0}\overline{0}\underline{0}$.
Therefore, $\dim\mathcal{S}\left({\mathcal{P}}\right)=4>3$. Hence, the problem
${\mathcal{P}}$ is not determined by $\mathcal{S}\left({\mathcal{P}}\right)$.
Opportunely,
$\mathcal{AS}\left({\mathcal{P}}\right)=\overline{0}\underline{0}\left(\underline{4},2\right)\underline{0}\overline{0}\underline{0}$,
where the tangent marker $\left(\underline{4},2\right)$ induced 2 constraints,
i.e., $x_{2,4}=-M_{2}$ and $x_{2,3}=0$. Therefore,
$\dim\mathcal{AS}\left({\mathcal{P}}\right)=5-2=3$, and ${\mathcal{P}}$ is
determined by $\mathcal{AS}\left({\mathcal{P}}\right)$. As shown in Fig. 4(f),
$\underline{0}\left(\underline{4},2\right)\underline{0}$ with time
$t_{2},t_{3}$ performs the same as $\underline{0}$ with time $t_{2}+t_{3}$ on
the surface, and $\left(\underline{4},2\right)$ means the system state vector
passes $x_{3}=-4$, $x_{2}=0$ during the stage $\underline{0}$.

Considering the full discussion in Section III-A, a conjecture is proposed
that system behaviors and tangent markers can provide full definite condition
for time optimal trajectories.

######  Conjecture 1.

$\forall S$ is an augmented switching law of the time-optimal problem (1a),
$S$ consists of system behaviors in Definition 1 and tangent markers in
Definition 8.

Obviously, if Conjecture 1 can be proven constructively, then the time optimal
problem for high-order chain-of-integrator systems, an open problem in the
optimal control theory, would be solved completely.

Figure 4: Time-optimal trajectory with a tangent marker.
${\mathcal{P}}={\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$,
where ${\boldsymbol{M}}=\left(1,1,1.5,4\right)$. The augmented switching law
is
$\mathcal{AS}\left({\mathcal{P}}\right)=\overline{0}\underline{0}\left(\underline{3},2\right)\underline{0}\overline{0}\underline{0}$,
where the tangent marker $\left(\underline{4},2\right)$ means
$x_{2,4}=-4,x_{2,3}=0$, and $x_{2,2}>0$. (a), (b) and (c) are the jerk,
velocity, and position plots, respectively. (d) is the trajectories of system
states ${\boldsymbol{x}}$. (e) and (f) are the flow charts for
$\mathcal{AS}\left({\mathcal{P}}\right)$ and
$\mathcal{S}\left({\mathcal{P}}\right)$.

##  IV Manifold-Intercept Method

Section III studies the properties of time optimal control and builds a novel
notation system of the time-optimal problem for chain-of-integrators. However,
it is a daunting task to analyze augmented switching laws for arbitrary given
problems
${\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)$.
Based on conclusions of optimal control reasoned in Section III, this section
proposed a trajectory planning method for high-order chain-of-integrators
systems, named the manifold-intercept method (MIM).

It should be pointed out that MIM is an efficient and quasi-optimal method.
Section V will indicate that MIM is near-optimal for 4th or higher-order
problems. Furthermore, for 3rd or lower-order problems, MIM achieves time-
optimality. In this section, the switching law and the optimal-trajectory
manifold are corresponding to those induced by MIM. Propositions in Section
III except Conjecture 1 still hold true, while theorems in this section might
not be true for time optimal control unless emphasized.

###  IV-A Manifold-Intercept Method

The key idea of MIM is that when the current state vector is “higher” (or
“lower”) than the lower-order optimal-trajectory manifold of the terminal
state vector, the control tends to choose the minimum (or maximum) value to
allow the system to enter the constant velocity phase of minimum (or maximum)
velocity, i.e., $\underline{n-1}$ (or $\overline{n-1}$) as quick as possible.

######  Definition 9.

The proper-position function is defined as
$p^{*}:{\mathrm{dom\,}}p^{*}\to{\mathbb{R}}$,
$\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)\mapsto\hat{x}_{n}$,
where

$\displaystyle{\mathrm{dom\,}}p^{*}=\left\\{\left({\boldsymbol{x}}_{0};{{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right):{\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)\text{
is feasible},\right.$
$\displaystyle\left.{\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}}\in{\mathbb{R}}^{n},{\boldsymbol{M}}\in{\mathbb{R}}_{++}\times\overline{{\mathbb{R}}}_{++}^{n}\right\\},$
(26)

and
$\hat{{\boldsymbol{x}}}_{0}=\left(x_{0,1},x_{0,2},\dots,x_{0,n-1},\hat{x}_{n}\right)\in{\mathcal{F}}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.

######  Definition 10.

For a feasible problem
${\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)$
of $n$-th order, ${\boldsymbol{x}}_{0}$ is called higher (or lower) than
${\mathcal{F}}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
if $x_{0,n}>$ (or $<$)
$p^{*}\left({\boldsymbol{x}}_{0};{{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.
If
$x_{0,n}=p^{*}\left({\boldsymbol{x}}_{0};{{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
then ${\boldsymbol{x}}_{0}$ is called to have a proper position.

The proper-position function is well-defined according to the proof process of
Proposition 3. In Definition 9, $\hat{{\boldsymbol{x}}}_{0}$ can be regarded
as the projection of ${\boldsymbol{x}}_{0}$ along the $x_{n}$-axis onto the
manifold
${\mathcal{F}}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.
In Definition 10, the relationship between ${\boldsymbol{x}}_{0}$ and
${\mathcal{F}}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
is represented by the positional relationship between ${\boldsymbol{x}}_{0}$
and its projection, i.e., $\hat{{\boldsymbol{x}}}_{0}$.

A third order example is shown in Fig. 5.
${\boldsymbol{x}}_{0}^{\left(1\right)}$ is higher than
${\mathcal{F}}_{2}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
while ${\boldsymbol{x}}_{0}^{\left(2\right)}$,
${\boldsymbol{x}}_{0}^{\left(3\right)}$, and
${\boldsymbol{x}}_{0}^{\left(4\right)}$ are lower than
${\mathcal{F}}_{2}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.
In Fig. 5, $\forall i=1,2,3,4$,
$\hat{{\boldsymbol{x}}}_{0}^{\left(i\right)}\in{\mathcal{F}}_{2}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
has a proper position.

Now MIM is introduced by mathematical induction.

For the base case, i.e., $n=1$, the optimal control has a trivial analytic
expression, i.e.,

$u^{*}\left(t\right)=M_{0}\,{\mathrm{sgn}}\left(x_{\mathrm{f}}-x_{0}\right),\,\forall
0\leq t\leq{t_{\mathrm{f}}}=\frac{\left|x_{\mathrm{f}}-x_{0}\right|}{M_{0}}.$
(27)

Assume that $\forall 1\leq k<n$, the $k$-th order trajectories with arbitrary
${\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}$ can be
planned by MIM. For the case where $n\geq 2$ and $M_{n}=\infty$,
$\hat{x}_{n}=p^{*}\left({\boldsymbol{x}}_{0};{{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
can be calculated by solving
${\mathcal{P}}\left({\boldsymbol{x}}_{0}^{1:\left(n-1\right)},{{\boldsymbol{x}}_{\mathrm{f}}}^{1:\left(n-1\right)};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$.
By Definition 9, it can be judged whether ${\boldsymbol{x}}_{0}$ is higher or
lower than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
by comparing $x_{0,n}$ and $\hat{x}_{n}$.

  1. 1.

If ${\boldsymbol{x}}_{0}$ has a proper position, according to (18),
${\mathcal{P}}\triangleq{\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)$
has the same solution as the $\left(n-1\right)$-th order problem
${\mathcal{P}}\left({\boldsymbol{x}}_{0}^{1:\left(n-1\right)},{{\boldsymbol{x}}_{\mathrm{f}}}^{1:\left(n-1\right)};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$.

  2. 2.

If $x_{0}$ is higher than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
the system tends to obtain a state vector in minimum uniform speed $-M_{n-1}$,
and keeps the velocity as $-M_{n-1}$ until entering
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.

    1. (a)

Firstly, the system state vector moves along the MIM-trajectory of the
$\left(n-1\right)$-order problem
${\mathcal{P}}_{1}={\mathcal{P}}\left({\boldsymbol{x}}_{0}^{1:\left(n-1\right)},-M_{n-1}{\boldsymbol{e}}_{n-1};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$,
i.e.,
${\boldsymbol{x}}\left(t\right)={\boldsymbol{x}}^{*}\left(t;{\mathcal{P}}_{1}\right)$.

    2. (b)

If
${\boldsymbol{x}}\left(t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)\right)$
is still higher than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
denote
$\hat{x}_{n}=p^{*}\left(-M_{n-1}{\boldsymbol{e}}_{n-1};{{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.
Then, $u\left(t\right)\equiv 0$ for
$t\in\left[t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right),t_{1}+t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)\right]$,
where
$t_{1}=\dfrac{x_{n}\left(t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)\right)-\hat{x}_{n}}{M_{n-1}}$.
Therefore,
${\boldsymbol{x}}\left(t_{1}\right)=-M_{n-1}{\boldsymbol{e}}_{n-1}+\hat{x}_{n}{\boldsymbol{e}}_{n}$
has a proper position. Denote
${\mathcal{P}}_{2}={\mathcal{P}}\left(-M_{n-1}{\boldsymbol{e}}_{n-1},{{\boldsymbol{x}}_{\mathrm{f}}}^{1:\left(n-1\right)};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$.
Let ${\boldsymbol{x}}$ moves along the MIM-trajectory of ${\mathcal{P}}_{2}$,
i.e.,
${\boldsymbol{x}}\left(t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)+t_{1}+t\right)={\boldsymbol{x}}^{*}\left(t;{\mathcal{P}}_{2}\right),0\leq
t\leq t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{2}\right)$. Finally,
${\boldsymbol{x}}\left(t_{\mathrm{f}}^{*}\left({\mathcal{P}}\right)\right)$
reach ${{\boldsymbol{x}}_{\mathrm{f}}}$, i.e.,

$t_{\mathrm{f}}^{*}\left({\mathcal{P}}\right)=t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)+t_{1}+t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{2}\right).$
(28)

    3. (c)

If
${\boldsymbol{x}}\left(t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)\right)$
is lower than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
i.e., ${\boldsymbol{x}}\left(t\right)$ enters
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
at some time
$t_{2}\in\left(0,t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{1}\right)\right)$,
then ${\boldsymbol{x}}$ can move along the MIM-trajectory of the
$\left(n-1\right)$-th order problem
${\mathcal{P}}_{3}={\mathcal{P}}\left({\boldsymbol{x}}^{1:\left(n-1\right)}\left(t_{2}\right),{{\boldsymbol{x}}_{\mathrm{f}}}^{1:\left(n-1\right)};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$.
Finally,
${\boldsymbol{x}}\left(t_{\mathrm{f}}^{*}\left({\mathcal{P}}\right)\right)$
reach ${{\boldsymbol{x}}_{\mathrm{f}}}$, i.e.,

$t_{\mathrm{f}}^{*}\left({\mathcal{P}}\right)=t_{2}+t_{\mathrm{f}}^{*}\left({\mathcal{P}}_{3}\right).$
(29)

  3. 3.

If $x_{0}$ is lower than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
a similar analysis as above applies to this case.

Figure 5: Some examples of proper positions. The parameters and optimal-
trajectory manifold is the same as Fig. 3(b). $\forall
i=1,2,3,4,\hat{{\boldsymbol{x}}}_{0}^{\left(i\right)}$ has a proper position
and the same $x_{1},x_{2},x_{3}$ as that of
${\boldsymbol{x}}_{0}^{\left(i\right)}$.

If $M_{n-1}=\infty$ above, then the first terminal state vector in Case 2 is
modified as $-M_{k}{\boldsymbol{e}}_{k}$ where $k<n-1$ is maximum index
satisfying $M_{k}\not=\infty$. Specifically, if $k=0$, the problem degenerates
into a time-optimal problem without state constraints, which can be easily
solved by an $n$-th order polynomial system [14].

An example of Case 2b is ${\boldsymbol{x}}^{\left(1\right)}$ in Fig. 2, where
${\boldsymbol{x}}^{\left(1\right)}$ slides under a minimum velocity $-M_{1}$
uniformly until ${\boldsymbol{x}}$ enters
$\mathcal{F}_{1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.
An example of Case 2c is ${\boldsymbol{x}}^{\left(2\right)}$ in Fig. 2, where
${\boldsymbol{x}}^{\left(2\right)}$ is intercepted by
$\mathcal{F}_{1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
before ${\boldsymbol{x}}^{\left(2\right)}$ reaches $x_{1}=-M_{1}$.

The above $n$-th order process meets the Bang-Singular-Bang control law and
can be solved by sequential $\left(n-1\right)$-th order problems. Furthermore,
the trajectory
${\boldsymbol{x}}\left(t\right)={\boldsymbol{x}}^{*}\left(t,{\mathcal{P}}\right)$
meets the constraints $\forall k=1,2,\dots,n$, $\left|x_{k}\right|\leq M_{k}$.
Specifically, $\left|x_{n}\right|\leq M_{n}=\infty$.

For the case where $n\geq 2$ and $M_{n}<\infty$, the optimal trajectory
${\boldsymbol{x}}\left(t\right)={\boldsymbol{x}}^{*}\left(t;{\mathcal{P}}_{\infty}\right)$
is generated based on the above process, where
${\mathcal{P}}_{\infty}={\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$.

  1. 1.

If the trajectory
${\boldsymbol{x}}\left(t\right)={\boldsymbol{x}}^{*}\left(t;{\mathcal{P}}_{\infty}\right)$
meets the constraint $\left|x_{n}\right|\leq M_{n}$, then the constraint
$\left|x_{n}\right|\leq M_{n}$ is deactivated. Hence,
${\boldsymbol{x}}\left(t\right)={\boldsymbol{x}}^{*}\left(t;{\mathcal{P}}_{\infty}\right)$
is also the optimal trajectory of
${\mathcal{P}}={\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)$.

  2. 2.

If the trajectory
${\boldsymbol{x}}\left(t\right)={\boldsymbol{x}}^{*}\left(t;{\mathcal{P}}_{\infty}\right)$
does not meet the constraint, i.e., $\left|x_{n}\right|>M_{n}$ at some time,
then a tangent marker w.r.t. $x_{n}$ occurs in the optimal problem
${\mathcal{P}}$. Considering the dimension of the augmented switching law and
according to Theorem 1, $\exists l\leq\frac{n}{2}$, s.t. ${\boldsymbol{x}}$
reaches a tangent marker $\left(n,2l\right)$ through an augmented switching
law of $2l$ dimension in optimal time, i.e., by solving a $2l$-th order
problem. Assume the system state vector is $\hat{{\boldsymbol{x}}}$ when
reaching the tangent marker. Then, the system state vector can move from
$\hat{{\boldsymbol{x}}}$ to ${{\boldsymbol{x}}_{\mathrm{f}}}$ in optimal time
by solving
${\mathcal{P}}\left(\hat{{\boldsymbol{x}}},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$.

By mathematical induction, $\forall n\in{\mathbb{N}}$, trajectories for high-
order chain-of-integrators systems with full state constraints
${\boldsymbol{M}}\in{\mathbb{R}}_{++}\times\overline{{\mathbb{R}}}_{++}^{n}$
and arbitrary terminal state vector
${{\boldsymbol{x}}_{\mathrm{f}}}\in{\mathbb{R}}^{n}$ can be planned by MIM.

###  IV-B Virtual System Behavior

The augmented switching law in this section refers to the switching law in MIM
with full definite conditions. The virtual system behavior is defined for the
case where (29) occurs.

######  Definition 11.

In MIM, assume an augmented switching law is
$S=\mathcal{AS}\left({\mathcal{P}}\left({\boldsymbol{x}}_{0},{\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}\right)\right)=S_{1}s\left(S_{2}\right)S_{3}$,
where $s$ is a system behavior, and
$S_{i}=s_{1}^{\left(i\right)}s_{2}^{\left(i\right)}\dots
s_{N_{i}}^{\left(i\right)},i=1,2,3$ are augmented switching laws.
$\left(S_{2}\right)$ is called a virtual system behavior, if
$\exists{\boldsymbol{t}}^{\left(i\right)}=\left(t_{j}^{\left(i\right)}\right)_{j=1}^{N_{i}}\geq{\boldsymbol{0}},i=1,2,3$
and $t_{1},t_{2}\geq 0$, s.t.

  1. 1.

${\boldsymbol{x}}_{0}$ moves to ${{\boldsymbol{x}}_{\mathrm{f}}}$ successively
passing through $S_{1}$ by time ${\boldsymbol{t}}^{\left(1\right)}$, $s$ by
time $t_{1}$, and $S_{3}$ by time ${\boldsymbol{t}}^{\left(3\right)}$. In
order words, $S_{1}sS_{3}$ with time
$\left({\boldsymbol{t}}^{\left(1\right)},t_{1},{\boldsymbol{t}}^{\left(3\right)}\right)$
is the solution of ${\mathcal{P}}$.

  2. 2.

${\boldsymbol{x}}_{0}$ can move successively passing through $S_{1}$ by time
${\boldsymbol{t}}^{\left(1\right)}$, $s$ by time $t_{2}$, and $S_{2}$ by time
${\boldsymbol{t}}^{\left(2\right)}$. In order words, $S_{1}sS_{2}$ with time
${\boldsymbol{t}}^{\left(1\right)},t_{2},{\boldsymbol{t}}^{\left(2\right)}$ is
a feasible solution under constraints (22) and (23).

An example is shown in Fig. 6, where
$\mathcal{S}\left({\mathcal{P}}\right)=\underline{01}\overline{0}\underline{2}\overline{01}\underline{0}\overline{01}\underline{0}\overline{2}\underline{01}\overline{0}$;
hence, $\dim\mathcal{S}\left({\mathcal{P}}_{2}\right)=6>4$. In MIM,
$\mathcal{AS}\left({\mathcal{P}}\right)=\underline{01}\overline{0}\underline{2}\overline{01}\underline{0}\left(\underline{3}\right)\overline{01}\underline{0}\overline{2}\underline{01}\overline{0}$;
hence, $\dim\mathcal{AS}\left({\mathcal{P}}\right)=4$. As shown in Fig. 6(d),
${\boldsymbol{x}}_{i-1}$ moves to ${\boldsymbol{x}}_{i}$ under the
corresponding system state vector for time $t_{i}$, $i\not=8,9$, while
${\boldsymbol{x}}_{7}$ moves to ${\boldsymbol{x}}_{9}$ under $\overline{0}$
for time $t_{9}$. Furthermore, the ability of ${\boldsymbol{x}}_{7}$ that can
moves to ${\boldsymbol{x}}_{8}$ under $\overline{0}$ for a time period $t_{9}$
provides definite condition for ${\mathcal{P}}_{2}$, where
${\boldsymbol{x}}_{8}$ is determined by the virtual system behavior
$\left(\underline{3}\right)$, i.e., $x_{8,1}=x_{8,2}=0$, $x_{8,3}=-M_{3}=-4$.
${\boldsymbol{x}}$ is intercepted by
$\mathcal{F}_{3}\left({\boldsymbol{x}}_{\mathrm{f}},{\boldsymbol{M}}\right)$
at ${\boldsymbol{x}}_{7}$.

Figure 6: An example of the virtual system behavior.
${\boldsymbol{M}}=\left(1,1,1.5,4,20\right)$, and
$\mathcal{AS}\left({\mathcal{P}}\right)=\underline{01}\overline{0}\underline{2}\overline{01}\underline{0}\left(\underline{3}\right)\overline{01}\underline{0}\overline{2}\underline{01}\overline{0}$,
where $\left(\underline{3}\right)$ is a virtual system behavior. (a) and (b)
are the snap and velocity plots, while (c) is the trajectory of the first
three components of system states ${\boldsymbol{x}}^{1:3}$. (d) is the flow
charts for $\mathcal{AS}\left({\mathcal{P}}\right)$.

######  Theorem 5.

In MIM, apply notations in Definition 11.

  1. 1.

The dimension of $S$ can be calculated like (20), i.e., $\left(S_{2}\right)$
contributes
$\sum_{i=1}^{N_{2}}\left(1-\left|s_{i}^{\left(2\right)}\right|\right)<0$
dimension to $S$.

  2. 2.

$\left|s_{1}^{\left(3\right)}\right|=0$, and
${\mathrm{sgn}}\left(s\right)=-{\mathrm{sgn}}\left(s_{1}^{\left(3\right)}\right)$.

  3. 3.

$\left|s_{N_{2}}^{\left(2\right)}\right|\not=0$, and $\forall k<N_{2}$,
$\left|s_{N_{2}}^{\left(2\right)}\right|>\left|s_{k}^{\left(2\right)}\right|$.
Furthermore,
${\mathrm{sgn}}\left(s_{N_{2}}^{\left(2\right)}\right)=-{\mathrm{sgn}}\left(s_{1}^{\left(3\right)}\right)$.

  4. 4.

Except the tangent marker, $S_{2}$ has an even number of even numbers, and
(25) holds.

  5. 5.

If $\exists N_{2}^{\prime}<N_{2}$, s.t. $\dim
S_{2,\left(N_{2}^{\prime}+1\right):N_{2}}=0$, then $\left(S_{2}\right)$ is
equivalent to $\left(S_{2,1:N_{2}^{\prime}}\right)$, i.e., $S$ is equivalent
to $S_{1}s\left(S_{2,1:N_{2}^{\prime}}\right)S_{3}$.

###### Proof.

Theorem 5-1 holds by the same analysis as Theorem 2. Theorem 5-5 holds
evidently.

Denote
${\mathcal{P}}={\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)$.
According to Section IV-A, the virtual system behavior $\left(S_{2}\right)$
occurs when the optimal trajectory represented by $S_{1}sS_{2}$ is intercepted
by a low-dimensional optimal-trajectory manifold $\mathcal{F}$, and
${\boldsymbol{x}}$ moves in $\mathcal{F}$ along an optimal trajectory
represented by $S_{3}$ after that. By induction, we only need to prove Theorem
5 for the case where the terminal state vector of $S_{1}sS_{2}$ has a maximum
velocity, i.e., $\left|s_{N_{2}}^{\left(2\right)}\right|=n-1$ for an $n$-th
order problem.

According to discussion above, $S_{3}\in\mathcal{AF}_{n-1}$; hence,
$\left|s_{1}^{\left(3\right)}\right|=0$. Note that $S_{1}sS_{3}$ induces
$\mathcal{S}\left({\mathcal{P}}\right)$ by removing all brackets, and $s$ as
well as $s_{1}^{\left(3\right)}$ are system behaviors. According to Theorem 3,
${\mathrm{sgn}}\left(s\right)=-{\mathrm{sgn}}\left(s_{1}^{\left(3\right)}\right)$.
Theorem 5-2 holds.

Note that $S_{1}sS_{2}^{1:\left(N_{2}-1\right)}$ is an augmented switching law
with the terminal states $\pm M_{n-1}{\boldsymbol{e}}_{n-1}$. Therefore,
$\left|s_{N_{2}}^{\left(2\right)}\right|=n-1\not=0$, and
$\left|s_{N_{2}}^{\left(2\right)}\right|>\left|s_{k}^{\left(2\right)}\right|,\forall
k<N_{2}$. Furthermore,
$\left|s_{1}^{\left(1\right)}\right|=\left|s_{N_{3}}^{\left(3\right)}\right|=0$
and
${\mathrm{sgn}}\left(s_{N_{3}}^{\left(3\right)}\right)=\left(-1\right)^{n-1}{\mathrm{sgn}}\left(s_{1}^{\left(1\right)}\right)$.
Note that $S_{3}\in\mathcal{AF}_{n-1}$; hence,
${\mathrm{sgn}}\left(s_{N_{3}}^{\left(3\right)}\right)=\left(-1\right)^{n-2}{\mathrm{sgn}}\left(s_{1}^{\left(3\right)}\right)$.
Assume ${\boldsymbol{x}}_{0}$ is higher than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.
Then,
${\mathrm{sgn}}\left(s_{N_{2}}^{\left(2\right)}\right)={\mathrm{sgn}}\left(s_{1}^{\left(1\right)}\right)=-1$;
hence,
${\mathrm{sgn}}\left(s_{N_{2}}^{\left(2\right)}\right)=-{\mathrm{sgn}}\left(s_{N_{3}}^{\left(3\right)}\right)$.
If ${\boldsymbol{x}}_{0}$ is lower than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
then
${\mathrm{sgn}}\left(s_{N_{2}}^{\left(2\right)}\right)=-{\mathrm{sgn}}\left(s_{N_{3}}^{\left(3\right)}\right)=1$.
Therefore, Theorem 5-3 holds.

According to discussion above, (25) holds for $S_{1}sS_{2}$, $S_{1}sS_{3}$,
and $s_{N_{2}}^{\left(2\right)}S_{3}$. In other words, the sign of
$S_{1}s\left(S_{2}\right)S_{3}$ is the same as $S_{1}sS_{2}S_{3}$ and
satisfies (25). Note that
${\mathrm{sgn}}\left(s\right)=-{\mathrm{sgn}}\left(s_{1}^{\left(3\right)}\right)={\mathrm{sgn}}\left(s_{N_{2}}^{\left(2\right)}\right)$;
hence, $S_{2}$ has an even number of even numbers. Therefore, Theorem 5-4
holds. ∎

As an example for Theorem 5-5,
$\underline{01}\overline{0}\underline{2}\left(\overline{01}\underline{03}\right)\overline{01}\underline{0}\overline{2}\underline{01}\overline{0}$
is equivalent to
$\underline{01}\overline{0}\underline{2}\overline{01}\underline{0}\overline{2}\underline{01}\overline{0}$.
The tangent marker also has some properties in MIM.

######  Theorem 6.

Apply notations in Theorem 4. In MIM,

  1. 1.

${\mathrm{sgn}}\left(s_{N_{1}}^{\left(1\right)}\right)={\mathrm{sgn}}\left(s_{1}^{\left(2\right)}\right)={\mathrm{sgn}}\left(s\right)$.

  2. 2.

$2l<\left|s\right|$.

###### Proof.

Similar to the proof of Theorem 5, we only need to prove the case where
$\left|s\right|=n$ for an $n$-th problem. Assume ${\boldsymbol{x}}_{0}$ is
higher than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
and the tangent marker occurs. According to Theorem 6,
$\left|s_{N_{1}}^{\left(1\right)}\right|=\left|s_{1}^{\left(2\right)}\right|=0$.
According to Section IV-A,
${\mathrm{sgn}}\left(s_{1}\right)=\left(-1\right)^{d-2}{\mathrm{sgn}}\left(s_{1}^{\left(1\right)}\right)=-1$,
and ${\mathrm{sgn}}\left(s\right)=-1$. When ${\boldsymbol{x}}$ reaches
$\left(s,d\right)$ as $\tilde{{\boldsymbol{x}}}$, $\tilde{{\boldsymbol{x}}}$
is still higher than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
while $s_{2}S_{2}$ is the augmented switching law from
$\tilde{{\boldsymbol{x}}}$ to ${{\boldsymbol{x}}_{\mathrm{f}}}$. Therefore,
$s_{2}=0$, ${\mathrm{sgn}}\left(s_{1}\right)=-1$. Applying a similar analysis
to the case where ${\boldsymbol{x}}_{0}$ is lower than
$\mathcal{F}_{n-1}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$,
Theorem 6-1 holds.

Assume $2l=\left|s\right|$ and the system state vector reaches
$\left(s,2l\right)$ at $t$. Then, $u$ is continuous at $t$ and
$u\left(t\right)={\mathrm{sgn}}\left(s\right)M_{0}$ according to Theorem 6.
However, according to Section III-A2,
${\mathrm{sgn}}\left(u\left(t\right)\right)=-{\mathrm{sgn}}\left(s\right)$ to
avoid $x_{s}$ crossing the boundary, which causes contradiction. Therefore,
$2l<\left|s\right|$.∎

Theorems 5 and 6 indicate that signs of elements of an augmented switching law
with a length of $N$ is determined by the sign of the last element in MIM,
which also reduces the possible signs of all elements from $2^{N}$ to $2$.
Furthermore, augmented switching laws can be enumerated fully, as reasoned in
Section IV-C.

###  IV-C Full Enumeration of Augmented Switching Laws in MIM

######  Definition 12.

In MIM, assume
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)\not=\varnothing$
in (18). The augmented-switching-law representation of
${\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
is

$\displaystyle\mathcal{AF}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)=\bigcup\left\\{S=\mathcal{AS}\left({\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right)\right):\right.$
$\displaystyle\left.{\boldsymbol{x}}_{0}\in{\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right),\dim
S=k\right\\}.$ (30)

$\forall{\boldsymbol{x}}_{0}\in{\mathcal{F}}_{k}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right),{\mathcal{P}}={\mathcal{P}}\left({\boldsymbol{x}}_{0},{{\boldsymbol{x}}_{\mathrm{f}}};{\boldsymbol{M}}\right),S=\mathcal{AS}\left({\mathcal{P}}\right)$,
if $\dim S=k^{\prime}<k$, $\left(k-k^{\prime}\right)$ zero can be added before
$S$ to increase the dimension to $k$. For example, if
$S=\overline{0}\underline{0}$ in Fig. 3(b), the switching law can be seen as
$\underline{0}\overline{0}\underline{0}$, and the solved motion time of the
first $\underline{0}$ is 0. If some augmented switching laws do not end with
0, then some 0 can also be added after the augmented switching law with motion
time 0.

######  Definition 13.

In MIM,

$\mathcal{AF}_{n}\triangleq\bigcup_{\begin{subarray}{c}{{\boldsymbol{x}}_{\mathrm{f}}}\in{\mathbb{R}}^{n}\\\
{\boldsymbol{M}}\in{\mathbb{R}}_{++}\times\overline{{\mathbb{R}}}_{++}^{n}\end{subarray}}\mathcal{AF}_{n}\left({{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$
(31)

is called the $n$-th order set of augmented switching laws.

According to analysis in Section IV-A, $\mathcal{AF}_{n}$ can be constructed
as follows:

######  Theorem 7.

In MIM, $\mathcal{AF}_{1}=\\{0\\}$. $\forall n\geq 1,\mathcal{AF}_{n+1}\supset
F_{n,1}\cup F_{n,2}\cup F_{n,3}$, where

$\displaystyle F_{n,1}=\\{$ $\displaystyle
S_{1}sS_{2}:S_{1},S_{2}\in\mathcal{AF}_{n},\left|s\right|=n\\},$ (32)
$\displaystyle F_{n,2}=\\{$ $\displaystyle
S_{1}\left(S_{2}s\right)S_{3}:S_{1}S_{2},S_{3}\in\mathcal{AF}_{n},\left|s\right|=n,$
$\displaystyle S_{2}s\text{ satisfies Theorem
\ref{thm:TheFirstBracket}-\ref{thm:TheFirstBracket_evennumber}}\\},$ (33)
$\displaystyle F_{n,3}=\\{$ $\displaystyle
S_{1}\left(s,d\right)S_{2}:S_{1}\in\mathcal{AF}_{d},S_{2}\in F_{n,1}\cup
F_{n,2},$ $\displaystyle\left|s\right|=n+1,d<\left|s\right|\text{ is
even}\\}.$ (34)

The signs of system behaviors, tangent markers, and virtual system behaviors
satisfy Theorems 3, 4, 5, and 6.

###### Proof.

According to Theorems 5, 6, and discussion in Section IV-A, Theorem 7 holds
evidently. ∎

For example, $\mathcal{AF}_{2}$={00, 010}. $\mathcal{AF}_{3}$={000, 0010,
0100, 01010, 00200, 002010, 010200, 0102010, 00(3,2)000, 00(3,2)0010,
00(3,2)0100, 00(3,2)01010, 00(3,2)00200, 00(3,2)002010, 00(3,2)010200,
00(3,2)0102010, 010(3,2)000, 010(3,2)0010, 010(3,2)0100, 010(3,2)01010,
010(3,2)00200, 010(3,2)002010, 010(3,2)010200, 010(3,2)0102010}.
$\mathcal{AF}_{4}$= {0000, 010(4,2)0102010(3)0102010, $\dots$}.
$\mathcal{AF}_{n}$ for $n\geq 4$ can be obtained by Theorem 7.

All augmented switching laws for any trajectory planning problem for high-
order chain-of-integrators systems have been fully enumerated so far. In order
words, the suboptimal solution of problem (1a) with arbitrary initial states,
terminal states, and full state constraints can be solved even by enumeration.

###  IV-D Calculation of Motion Time and Feasibility Verification

According to Definition 12 and 13, $\forall S\in\mathcal{AF}_{n}$, $\dim S=n$.
Therefore, the motion time can be determined by a given initial state vector
${\boldsymbol{x}}_{0}$ and an augmented switching law $S$. Denote
${\boldsymbol{f}}:{\mathbb{R}}^{n}\times{\mathbb{R}}\times{\mathbb{R}}\to{\mathbb{R}}^{n}$
whose $k$-th component is

$f_{k}\left({\boldsymbol{x}},u,t\right)=\frac{1}{k!}ut^{k}+\sum_{i=1}^{k}\frac{1}{i!}x_{k-i}t^{i}.$
(35)

${\boldsymbol{y}}={\boldsymbol{f}}\left({\boldsymbol{x}},u,t\right)$ means
system state vector moves from ${\boldsymbol{x}}$ to ${\boldsymbol{y}}$ under
a constant control $u$ by a period of time $t$. Next, describe equations
induced by a given $S=s_{1}s_{2}\dots s_{N}\in\mathcal{AF}_{n}$. Assume the
system state vector reaches ${\boldsymbol{x}}_{m-1}$ before $s_{m}$. The
control and time in stages of $s_{m}$ are $u_{m}$ and $t_{m}$, respectively.
$u_{m}$ and ${\mathrm{sgn}}\left(s_{m}\right)$ can be calculated by Theorem 3
and Theorem 5.

If $s_{m}$ and $s_{m+1}$ are system behaviors, or $s_{m}$ and $s_{m+1}$ are
the virtual system behaviors, or $m=N$, then

$\begin{dcases}{\boldsymbol{x}}_{m}={\boldsymbol{f}}\left({\boldsymbol{x}}_{m-1},u_{m},t_{m}\right),\\\
x_{m,\left|s_{m}\right|}={\mathrm{sgn}}\left(s_{m}\right)M_{\left|s_{m}\right|},\text{
if }\left|s_{m}\right|\not=0,\\\ x_{m,k}=0,\text{ if
}\left|s_{m}\right|\not=0,k<\left|s_{m}\right|.\end{dcases}$ (36)

For $s_{l}\left(s_{l+1}\dots s_{r-1}\right)s_{r}$, $s_{l}$ and $s_{r}$ are
system behaviors, while $\left(s_{l+1}\dots s_{r-1}\right)$ is a virtual
system behavior. According to Theorem 5-2, $s_{r}=0$. Then,

$\begin{dcases}{\boldsymbol{x}}_{l}={\boldsymbol{f}}\left({\boldsymbol{x}}_{l-1},u_{l},t_{l}\right),\\\
{\boldsymbol{x}}_{r-1}={\boldsymbol{f}}\left({\boldsymbol{x}}_{l-1},u_{l},t_{r-1}\right),\\\
{\boldsymbol{x}}_{r}={\boldsymbol{f}}\left({\boldsymbol{x}}_{r-1},u_{r},t_{r}\right),\\\
x_{l,\left|s_{l}\right|}={\mathrm{sgn}}\left(s_{l}\right)M_{\left|s_{l}\right|},\text{
if }\left|s_{l}\right|\not=0,\\\ x_{l,k}=0,\text{ if
}\left|s_{l}\right|\not=0,k<\left|s_{l}\right|.\end{dcases}$ (37)

For $s_{m-1}\left(s_{m},d_{m}\right)s_{m+1}$, $u_{m-1}=u_{m+1}$, and $u_{m}$
is not defined. Then,

$\begin{dcases}{\boldsymbol{x}}_{m}={\boldsymbol{f}}\left({\boldsymbol{x}}_{m-2},u_{m-1},t_{m-1}\right),\\\
x_{m,\left|s_{m}\right|}={\mathrm{sgn}}\left(s_{m}\right)M_{\left|s_{m}\right|},\\\
x_{m,\left|s_{m}\right|-k}=0,\,k<d_{m}.\end{dcases}$ (38)

Finally, ${\boldsymbol{x}}_{N}$ is substituted as
${{\boldsymbol{x}}_{\mathrm{f}}}$.

0: $n\in{\mathbb{N}}$, ${\boldsymbol{x}}_{0}$,
${{\boldsymbol{x}}_{\mathrm{f}}}\in{\mathbb{R}}^{n}$,
${\boldsymbol{M}}\in{\mathbb{R}}_{++}\times\overline{{\mathbb{R}}}_{++}^{n}$.

0: Optimal control $u=u^{*}\left(t\right)$ of problem (1a).

if $n=1$ then

Solve the problem by (27) and return.

end if

if ${\boldsymbol{x}}_{0}$ is higher than
$\mathcal{F}_{n-1}\left({\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$
then

Obtain $u=\hat{u}^{*}\left(t\right)$ by
${\mathcal{P}}\left(-{\boldsymbol{x}}_{0},-{{\boldsymbol{x}}_{\mathrm{f}}},{\boldsymbol{M}}\right)$.

return $u^{*}\left(t\right)=-\hat{u}^{*}\left(t\right)$.

end if

if $M_{1}=M_{2}=\dots=M_{n-1}=\infty$ then

Obtain ${\boldsymbol{x}}^{*}\left(t\right)$, $u^{*}\left(t\right)$,
$t_{\mathrm{f}}$ by solving $n$ tandem (35).

else

$m\leftarrow\arg\max\\{k<n:M_{k}<\infty\\}$.

Obtain ${\boldsymbol{x}}^{\left(1\right)}\left(t\right),t_{{\mathrm{f}}1}$ by
${\mathcal{P}}\left({\boldsymbol{x}}_{0}^{1:\left(n-1\right)},M_{m}{\boldsymbol{e}}_{m},{\boldsymbol{M}}^{0:m}\right)$.

if $x_{n}^{\left(1\right)}\left(t_{{\mathrm{f}}1}\right)$ is lower than
$\mathcal{F}_{n-1}\left({\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$
then

Obtain ${\boldsymbol{x}}^{\left(2\right)}\left(t\right)$ by
${\mathcal{P}}\left(M_{m}{\boldsymbol{e}}_{m},{\boldsymbol{x}}_{\mathrm{f}}^{1:\left(n-1\right)},{\boldsymbol{M}}^{0:m}\right)$.

Obtain ${\boldsymbol{x}}^{*}\left(t\right)$ by connecting
${\boldsymbol{x}}^{\left(1\right)}$, $x_{m}\equiv M_{m}$,
${\boldsymbol{x}}^{\left(2\right)}$.

else

Solve $t_{1}$ when ${\boldsymbol{x}}$ enters
$\mathcal{F}_{n-1}\left({\boldsymbol{x}}_{\mathrm{f}};{\boldsymbol{M}}^{0:\left(n-1\right)}\right)$.

Obtain ${\boldsymbol{x}}^{\left(3\right)}\left(t\right)$ by
${\mathcal{P}}\left({\boldsymbol{x}}^{1:\left(n-1\right)}\left(t_{1}\right),{\boldsymbol{x}}_{\mathrm{f}}^{1:\left(n-1\right)},{\boldsymbol{M}}^{0:m}\right)$
.

Obtain ${\boldsymbol{x}}^{*}\left(t\right)$ by connecting
${\boldsymbol{x}}^{\left(1\right)}$, ${\boldsymbol{x}}^{\left(3\right)}$.

end if

end if

if $M_{n}<\infty$ and $\exists t\in\left(0,t_{{\mathrm{f}}}\right)$,
$\left|x_{n}^{*}\left(t\right)\right|>M_{n}$ then

for $d\leftarrow 2,4,\dots,2\left\lfloor\frac{n-1}{2}\right\rfloor$ do

for $S\in\mathcal{AF}_{d}$ do

Obtain
${\boldsymbol{x}}={\boldsymbol{x}}^{\left(0\right)}\left(t\right),t_{{\mathrm{f}}0}$
by $S$, where ${\boldsymbol{x}}$ moves from ${\boldsymbol{x}}_{0}$ to
$\left(n,d\right)$;

Obtain ${\boldsymbol{x}}={\boldsymbol{x}}^{\left(4\right)}\left(t\right)$ by
${\mathcal{P}}\left({\boldsymbol{x}}\left(t_{{\mathrm{f}}0}\right),{\boldsymbol{x}}_{\mathrm{f}},{\boldsymbol{M}}\right)$.

Update the best feasible trajectory ${\boldsymbol{x}}^{*}\left(t\right)$ by
connecting
${\boldsymbol{x}}^{\left(0\right)},{\boldsymbol{x}}^{\left(4\right)}$.

end for

end for

end if

Algorithm 1 Trajectory planning for high-order chain-of-integrators systems by
MIM.

Assume $S\in\mathcal{AF}_{n}$ has $N_{0}$ system behaviors, $N_{1}$ virtual
system behaviors, and $N_{2}$ tangent markers. Then, the number of equations
equals the number of variables, i.e.,
$\left(N_{0}+N_{1}\right)\left(n+1\right)-n$. $N_{2}$ satisfies $\dim S=n$.

Feasibility verification is trivial. On one hand, $\forall m=1,2,\dots,N$,
$t_{m}\geq 0$. On the other hand, $\forall m=1,2,\dots,N$, $\forall
t\in\left[0,t_{m}\right]$, $\forall k=1,2,\dots,n$,
$\left|x_{m,k}\left(t\right)\right|\leq M_{k}$, where $x_{m,k}\left(t\right)$
is the system state after entering $s_{m}$ for a period of time $t$. The
latter feasibility condition can be verified by
$x_{k}\left(t\right)=f_{k}\left({\boldsymbol{x}}_{p},u_{m},t\right)$, where
${\boldsymbol{x}}_{p}$ is the previous system state vector of $s_{m}$
according to discussion above. Furthermore, checking
$\left|x_{m,k}\left(t\right)\right|\leq M_{k}$ at stationary points is enough
[27].

Based on MIM in Section IV-A and enumeration of $\mathcal{AF}_{n}$ in Section
IV-C, a trajectory planning algorithm for high-order chain-of-integrators
systems with arbitrary initial states, terminal states, and full state
constraints are developed in Algorithm 1.

##  V Numerical Results and Discussion

Figure 7: Some trajectories planned by the proposed and baselines. (a) A jerk-
limited trajectory represented by
$\underline{0}\underline{1}\overline{0}\underline{2}\overline{0}\overline{1}\underline{0}$.
${\boldsymbol{x}}_{0}=\left(1,-0.375,4\right)$,
${{\boldsymbol{x}}_{\mathrm{f}}}=\left(0,0,0\right)$,
${\boldsymbol{M}}=\left(1,1,1.5,4\right)$. (b) A jerk-limited trajectory
represented by
$\underline{0}\overline{0}\left(\overline{3},2\right)\overline{0}\underline{0}\overline{0}$.
${\boldsymbol{x}}_{0}=\left(1,-0.375,3.999\right)$,
${{\boldsymbol{x}}_{\mathrm{f}}}=\left(0,0,4\right)$,
${\boldsymbol{M}}=\left(1,1,1.5,4\right)$. (c) A snap-limited trajectory
represented by
$\underline{0}\underline{1}\overline{0}\underline{2}\overline{0}\overline{1}\underline{0}(\underline{3})\overline{0}\overline{1}\underline{0}\overline{0}$.
${\boldsymbol{x}}_{0}=\left(0.75,-0.375,2,9\right)$,
${{\boldsymbol{x}}_{\mathrm{f}}}=\left(0.25,0.5,-2,-5\right)$,
${\boldsymbol{M}}=\left(1,1,1.5,4,20\right)$. (d) A snap-limited trajectory
represented by
$\underline{0}\overline{0}(\overline{3},2)\overline{0}\underline{0}\overline{0}\overline{3}\underline{0}\underline{1}\overline{0}\underline{0}$.
${\boldsymbol{x}}_{0}=\left(1,-0.375,4,-10\right)$,
${{\boldsymbol{x}}_{\mathrm{f}}}=\left(0.75,-0.375,2,16\right)$,
${\boldsymbol{M}}=\left(1,1,1.5,4,20\right)$. The trajectories of MIM and
Ruckig almost coincide in (a), while $x_{3},x_{4}$ of all methods look to
coincide. Figure 8: Quantitative results of 100 3rd-order trajectories and 100
4th-order trajectories with random initial states and terminal states. (a)
Success rate $R_{\mathrm{s}}$. (b) Computational time $T_{\mathrm{c}}$. (c)
Error of terminal states $E_{\mathrm{s}}$. (d) Normalized MSE $E_{\mathrm{m}}$
between input and the Bang-Singular-Bang control law. (e) Normalized total
variation $T_{\mathrm{v}}$ of the planned control $u$. Ruckig is only applied
in 3-order trajectories because of disability to plan snap-limited
trajectories. $y$-axes in (b) and (c) are in logarithmic scales.

###  V-A Experimental Setup

Baselines. To verify the performance of the proposed MIM method, i.e.,
Algorithm 1, simulation experiments for trajectories are conducted. The
baselines are as follows.

  * •

Ruckig [19] in community version: a jerk-limited time-optimal trajectory
solver without position constraints.

  * •

SOCPs [31]: a time-optimal control method based on solving sequential convex
second-order cone problems. The SOCPs method is achieved by Gurobi [40].

  * •

Yop [30]: a MATLAB toolbox for numerical optimal control problems based on
CasADi [28] by direct methods.

The control period of Ruckig is set as 1 ms. The number of time points is set
as 500 and 150 in SOCPs and Yop, respectively. In practice, if the number of
time points is set more than 600 and 200 in SOCPs and Yop, respectively, then
the computational time will increase obviously.

Metrics. For trajectory planning methods, the computational efficiency, the
computational error, and the trajectory quality are significant performance
metrics.

  * •

The computational time $T_{\mathrm{c}}$. All experiments are conducted on a
computer with an AMD Ryzen 7 5800H @ 3.20 GHz processor.

  * •

The error of the terminal states $E_{\mathrm{s}}$. For a solved final state
vector $\hat{{\boldsymbol{x}}}_{\mathrm{f}}$, $E_{\mathrm{s}}$ is defined in
normalization as

$E_{\mathrm{s}}\triangleq\sqrt{\sum_{k=1}^{n}\left(\frac{x_{{\mathrm{f}}k}-\hat{x}_{{\mathrm{f}}k}}{M_{k}}\right)^{2}}.$
(39)

  * •

The success rate to obtain a feasible solution $R_{\mathrm{s}}$. A result is
determined to be successful if the states along the planned trajectories are
feasible and $E_{\mathrm{s}}\leq 0.1$.

  * •

The normalized mean-squared error (MSE) $E_{\mathrm{m}}$ between the solved
$u=u\left(t\right)$ and the Bang-Singular-Bang control law [11] is defined to
describe the trajectory quality. According to Proposition 1, the optimal
control $u^{*}\left(t\right)\in\left\\{0,M_{0},-M_{0}\right\\}$ almost
everywhere. Define

$E_{\mathrm{m}}\triangleq\sqrt{\dfrac{4}{M_{0}^{2}t_{\mathrm{f}}}\int_{0}^{t_{\mathrm{f}}}u\left(t\right)^{2}\wedge\left(\left|u\left(t\right)\right|-M_{0}\right)^{2}\,\mathrm{d}t}.$
(40)

Then, the smaller $E_{\mathrm{m}}$ is, the closer $u=u\left(t\right)$ is to a
Bang-Singular-Bang control law, i.e., the better trajectory quality is.
Specifically, $E_{\mathrm{m}}\in\left[0,1\right]$. Furthermore,
$E_{\mathrm{m}}=0$ if and only if $\forall t\in\left[0,t_{\mathrm{f}}\right]$,
$u\left(t\right)\in\left\\{0,M_{0},-M_{0}\right\\}$ satisfies the Bang-
Singular-Bang control law, while $E_{\mathrm{m}}=1$ if and only if $\forall
t\in\left[0,t_{\mathrm{f}}\right]$,
$\left|u\left(t\right)\right|=\frac{1}{2}M_{0}$.

  * •

The normalized total variation [38] $T_{\mathrm{v}}$ of the solved control is
defined to describe the stability of the trajectory. For
$u\left(\frac{k}{n}t_{\mathrm{f}}\right)=u_{k}$ with $\left(n+1\right)$
waypoints, define

$T_{\mathrm{v}}\triangleq\frac{1}{2nM_{0}}\sum_{k=1}^{n}\left|u_{k}-u_{k-1}\right|.$
(41)

Then, as $T_{\mathrm{v}}$ decreases, the trajectory exhibits increased
oscillations, i.e., the stability of the trajectory decreases. Specifically,
$T_{\mathrm{v}}\in\left[0,1\right]$. Furthermore, $T_{\mathrm{v}}=0$ if and
only if $u_{k}\equiv\mathrm{const}$, while $T_{\mathrm{v}}=1$ if and only if
$u_{k}=\left(-1\right)^{k}M_{0}$ or $u_{k}=\left(-1\right)^{k-1}M_{0}$.

###  V-B Simulation Experiments

Several trajectories planned by the proposed MIM and baselines are shown in
Fig. 7 as examples, and the quantitative results of 100 jerk-limited
trajectories and 100 snap-limited trajectories are shown in Fig. 8. Among
them, 3rd-order trajectories in Fig. 8 contain no tangent markers, since
Ruckig in community version is not able to plan trajectories with position
constraints. Furthermore, tangent markers occur in 3rd-order trajectories only
if the initial position and the terminal position are both close to
boundaries, which occurs with a low probability.

It is noteworthy that the time-optimal problem (1a) with an order $n\geq 3$ is
non-convex if it is solved in discretion directly [41]. Hence, discrete
methods like Yop might fail to obtain an optimal trajectory, as shown in Fig.
7(a). Though SOCPs successfully transforms problem (1a) into sequential convex
problems, SOCPs might fail to obtain a feasible solution during iteration in
some cases, as shown in Fig. 7(d). Furthermore, trajectories planned by Yop
and SOCPs does not meet constraints (1d), causing failure as well. As shown in
Fig. 8(a), Yop and SOCPs has a limited success rate, while Ruckig can plan all
3th-order trajectories. Supported by theoretical results in Section III and
Section IV, the proposed MIM succeeds in planning all randomly selected
trajectories in 3rd and 4th order.

As shown in Fig. 8(c) and (d), the proposed MIM outperforms all baselines on
computational time and computational accuracy. It can be observed that the
proposed MIM achieves a computational time reduction of approximately 2 orders
of magnitude compared to SOCPs and Yop, while improving computational accuracy
by 90.8% (79.0%), 99.6% (99.0%), and 52.1% compared to SOCPs, Yop, and Ruckig,
respectively, for 3rd-order (4th-order) problem in average. Specifically, MIM
is not based on discrete time; hence, its computational efficiency keeps
constant no matter what the control period is. On the contrary, as a real-time
method, Ruckig requires longer computational time with a higher control
frequency, but Ruckig is able to achieve a short computational time than the
proposed MIM if its control frequency is set lower than 100 Hz, which is not
conducive to precise motion control. Furthermore, the upper bound of the
computational accuracy of MIM is determined only by the control frequency.

$E_{\mathrm{m}}$ and $T_{\mathrm{v}}$ shown in Fig. 8(d) and (e) describe the
trajectory quality. $E_{\mathrm{m}}$ and $T_{\mathrm{v}}$ of MIM and Ruckig
are evidently lower than those of SOCPs and Yop. Therefore, the proposed MIM
and Ruckig achieve much higher trajectory quality. Compared with SOCPs and
Yop, trajectories generated by MIM are strict Bang-Singular-Bang. In addition
to necessary switching, the input $u\left(t\right)$ of the proposed MIM does
not exhibit oscillations like those of SOCPs, as shown in Fig. 7(a) and (c).
When the trajectory is short enough like Fig. 7(b), SOCPs has a short step
width and can avoid oscillations. Quantitatively, MIM achieves a normalized
MSE of control $E_{\mathrm{m}}$ that is 80.78% (92.3%), 91.9% (92.2%), and
57.2% lower than that of SOCPs, Yop, and Ruckig, respectively, for 3rd-order
(4th-order) problems in average. Taking oscillations into consideration, MIM
achieves a normalized total variation of control $T_{\mathrm{v}}$ that is
99.6% (99.5%), 97.1% (99.5%), and 10.1% lower than that of SOCPs, Yop, and
Ruckig, respectively, for 3rd-order (4th-order) problems in average.

###  V-C Discussion

By comparing Algorithm 1 of 3rd-order and Ruckig in community version [19]
with 1-DOF, it can be examined that the proposed MIM plans the same trajectory
with Ruckig for a given 3rd-order problem. Furthermore, in conjunction with
discussion in Section III, the proposed can plan strictly time-optimal
trajectories in 3rd-order and lower-order with full state constraints.

For 4th-order and higher-order problems, MIM plans quasi-optimal trajectories
since the virtual system behavior in Definition 11 does not exist in time-
optimal trajectories. In fact, the virtual system behavior contributes to high
computational efficiency. It is noteworthy that the motion time of the MIM-
trajectory is close to that of the time-optimal trajectory. For example, the
MIM-trajectory shown in Fig. 7(b) can be represented by
$\underline{0}\underline{1}\overline{0}\underline{2}\overline{0}\overline{1}\underline{0}(\underline{3})\overline{0}\overline{1}\underline{0}\overline{0}$
with motion time 9.8604 s, while the optimal trajectory can be represented by
$\underline{0}\underline{1}\overline{0}\underline{2}\overline{0}\overline{1}\underline{0}(\underline{3},2)\overline{0}\overline{1}\underline{0}\overline{0}$
with motion time 9.8543 s. In this example, the motion time of MIM is only
0.6% longer than that of the optimal trajectory. Furthermore, the motion time-
optimal trajectory is difficult to solve since
$\underline{0}\underline{1}\overline{0}\underline{2}\overline{0}\overline{1}\underline{0}(\underline{3},2)\overline{0}\overline{1}\underline{0}\overline{0}$
can not be decoupled.

It is meaningful to strictly solve the time optimal problem (1a) in further
study, which would be a landmark achievement in the optimal control theory. To
the best of our knowledge, the theoretical framework established in Section
III provides unprecedented insights into problem (1a), surpassing current
literature. Therefore, it is believed that the theoretical framework
established in Section III would be a noteworthy mathematical tool for the
final resolution of the time optimal problem (1a).

##  VI Conclusion

This paper has set out to theoretically study a classical and challenging
problem in the optimal control theory, namely, the time-optimal control
problem for high-order chain-of-integrators systems. To this end, this paper
establishes a novel notation system and theoretical framework, successfully
providing the switching manifold for high-order problems in the form of
switching laws. The framework derives properties of switching laws on signs as
well as dimension and reasons a definite condition of augmented switching
laws. Guided by the developed framework, a near-time-optimal trajectory
planning method named the manifold-intercept method (MIM) is proposed,
outperforming all baselines on computational time, computational accuracy, and
trajectory quality with a large gap. The proposed MIM can achieve time-optimal
trajectories for 3rd-order or lower-order problems with full state
constraints. MIM can also plan near-optimal trajectories efficiently and
accurately with negligible extra motion time compared to time-optimal
trajectories that have been lack of mature algorithms up to now.

## References

  * [1] W. Wang, C. Hu, K. Zhou, S. He, and L. Zhu, “Local asymmetrical corner trajectory smoothing with bidirectional planning and adjusting algorithm for cnc machining,” _Robotics and Computer-Integrated Manufacturing_ , vol. 68, p. 102058, 2021.
  * [2] Z. Wang, R. Zhou, C. Hu, and Y. Zhu, “Real-time iterative compensation based contouring control method for polar coordinate motion systems,” _IEEE/ASME Transactions on Mechatronics_ , vol. 27, no. 5, pp. 3517–3526, 2022.
  * [3] Y. Wang, J. Wang, Y. Li, C. Hu, and Y. Zhu, “Learning latent object-centric representations for visual-based robot manipulation,” in _2022 International Conference on Advanced Robotics and Mechatronics (ICARM)_. IEEE, 2022, pp. 138–143.
  * [4] G. Zhao and M. Zhu, “Pareto optimal multirobot motion planning,” _IEEE Transactions on Automatic Control_ , vol. 66, no. 9, pp. 3984–3999, 2020.
  * [5] J. Wang, C. Hu, Y. Wang, and Y. Zhu, “Dynamics learning with object-centric interaction networks for robot manipulation,” _IEEE Access_ , vol. 9, pp. 68 277–68 288, 2021.
  * [6] C. Ni, M. Zhang, Y. Zhu, C. Hu, S. Ding, and Z. Jia, “Sinusoidal phase-modulating interferometer with ellipse fitting and a correction method,” _Applied Optics_ , vol. 56, no. 13, pp. 3895–3899, 2017.
  * [7] M. Li, Y. Zhu, K. Yang, L. Yang, C. Hu, and H. Mu, “Convergence rate oriented iterative feedback tuning with application to an ultraprecision wafer stage,” _IEEE Transactions on Industrial Electronics_ , vol. 66, no. 3, pp. 1993–2003, 2018.
  * [8] S. Güler, B. Fidan, S. Dasgupta, B. D. Anderson, and I. Shames, “Adaptive source localization based station keeping of autonomous vehicles,” _IEEE Transactions on Automatic Control_ , vol. 62, no. 7, pp. 3122–3135, 2016.
  * [9] Y. Wang, C. Hu, Z. Wang, S. Lin, Z. Zhao, W. Zhao, K. Hu, Z. Huang, Y. Zhu, and Z. Lu, “Optimization-based non-equidistant toolpath planning for robotic additive manufacturing with non-underfill orientation,” _Robotics and Computer-Integrated Manufacturing_ , vol. 84, p. 102599, 2023.
  * [10] M. Li, K. Yang, Y. Zhu, H. Mu, and C. Hu, “State/model-free variable-gain discrete sliding mode control for an ultraprecision wafer stage,” _IEEE Transactions on Industrial Electronics_ , vol. 64, no. 8, pp. 6695–6705, 2016\.
  * [11] S. He, C. Hu, Y. Zhu, and M. Tomizuka, “Time optimal control of triple integrator with input saturation and full state constraints,” _Automatica_ , vol. 122, p. 109240, 2020.
  * [12] R. F. Hartl, S. P. Sethi, and R. G. Vickson, “A survey of the maximum principles for optimal control problems with state constraints,” _SIAM review_ , vol. 37, no. 2, pp. 181–218, 1995.
  * [13] E. B. Lee and L. Markus, “Foundations of optimal control theory,” Minnesota Univ Minneapolis Center For Control Sciences, Tech. Rep., 1967.
  * [14] G. Bartolini, S. Pillosu, A. Pisano, E. Usai _et al._ , “Time-optimal stabilization for a third-order integrator: A robust state-feedback implementation,” _Lecture notes in control and information sciences_ , pp. 131–144, 2002.
  * [15] N. Marchand, A. Hably, and A. Chemori, “Global stabilization with low computational cost of the discrete-time chain of integrators by means of bounded controls,” _IEEE Transactions on Automatic Control_ , vol. 52, no. 5, pp. 948–952, 2007.
  * [16] Z. Ma and S. Zou, _Optimal Control Theory: The Variational Method_. Springer, 2021.
  * [17] R. Haschke, E. Weitnauer, and H. Ritter, “On-line planning of time-optimal, jerk-limited trajectories,” in _2008 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2008, pp. 3248–3253.
  * [18] T. Kröger, “Opening the door to new sensor-based robot applications–the reflexxes motion libraries,” in _2011 IEEE International Conference on Robotics and Automation_. IEEE, 2011, pp. 1–4.
  * [19] L. Berscheid and T. Kröger, “Jerk-limited real-time trajectory generation with arbitrary target states,” _Robotics: Science and Systems XVII_ , 2021\.
  * [20] M. Yury, “Quasi-time-optimal control of third-order integrators with phase constraints,” in _2016 International Conference Stability and Oscillations of Nonlinear Control Systems (Pyatnitskiy’s Conference)_. IEEE, 2016, pp. 1–4.
  * [21] S. He, C. Hu, S. Lin, Y. Zhu, and M. Tomizuka, “Real-time time-optimal continuous multi-axis trajectory planning using the trajectory index coordination method,” _ISA transactions_ , vol. 131, pp. 639–649, 2022.
  * [22] Y. Gao, K. H. Johansson, and L. Xie, “Computing probabilistic controlled invariant sets,” _IEEE Transactions on Automatic Control_ , vol. 66, no. 7, pp. 3138–3151, 2020.
  * [23] U. Walther, T. T. Georgiou, and A. Tannenbaum, “On the computation of switching surfaces in optimal control: A grobner basis approach,” _IEEE Transactions on Automatic Control_ , vol. 46, no. 4, pp. 534–540, 2001.
  * [24] E. Khmelnitsky and M. Caramanis, “One-machine n-part-type optimal setup scheduling: analytical characterization of switching surfaces,” _IEEE Transactions on Automatic Control_ , vol. 43, no. 11, pp. 1584–1588, 1998.
  * [25] I. M. Mitchell, A. M. Bayen, and C. J. Tomlin, “A time-dependent hamilton-jacobi formulation of reachable sets for continuous dynamic games,” _IEEE Transactions on automatic control_ , vol. 50, no. 7, pp. 947–957, 2005\.
  * [26] F. Tahir and I. M. Jaimoukha, “Low-complexity polytopic invariant sets for linear systems subject to norm-bounded uncertainty,” _IEEE Transactions on Automatic Control_ , vol. 60, no. 5, pp. 1416–1421, 2014.
  * [27] L. Doeser, P. Nilsson, A. D. Ames, and R. M. Murray, “Invariant sets for integrators and quadrotor obstacle avoidance,” in _2020 American Control Conference (ACC)_. IEEE, 2020, pp. 3814–3821.
  * [28] J. A. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “Casadi: a software framework for nonlinear optimization and optimal control,” _Mathematical Programming Computation_ , vol. 11, pp. 1–36, 2019.
  * [29] Y. Nie, O. Faqir, and E. C. Kerrigan, “Iclocs2: Try this optimal control problem solver before you try the rest,” in _2018 UKACC 12th international conference on control (CONTROL)_. IEEE, 2018, pp. 336–336.
  * [30] V. Leek, “An optimal control toolbox for matlab based on casadi,” 2016.
  * [31] M. Leomanni, G. Costante, and F. Ferrante, “Time-optimal control of a multidimensional integrator chain with applications,” _IEEE Control Systems Letters_ , vol. 6, pp. 2371–2376, 2022.
  * [32] K. Erkorkmaz and Y. Altintas, “High speed cnc system design. part i: jerk limited trajectory generation and quintic spline interpolation,” _International Journal of machine tools and manufacture_ , vol. 41, no. 9, pp. 1323–1345, 2001.
  * [33] L. Dai, X. Li, Y. Zhu, M. Zhang, and C. Hu, “The generation mechanism of tracking error during acceleration or deceleration phase in ultraprecision motion systems,” _IEEE Transactions on Industrial Electronics_ , vol. 66, no. 9, pp. 7109–7119, 2018.
  * [34] B. Ezair, T. Tassa, and Z. Shiller, “Planning high order trajectories with general initial and final conditions and asymmetric bounds,” _The International Journal of Robotics Research_ , vol. 33, no. 6, pp. 898–916, 2014\.
  * [35] E. Oland and R. Kristiansen, “Controlling a chain of integrators with constrained actuation using exponential activation functions,” in _2019 IEEE 10th International Conference on Mechanical and Aerospace Engineering (ICMAE)_. IEEE, 2019, pp. 258–261.
  * [36] L. Berscheid, “Ruckig - motion generation for robots and machines,” https://ruckig.com/, 2023.
  * [37] C. Fox, _An introduction to the calculus of variations_. Courier Corporation, 1987.
  * [38] E. M. Stein and R. Shakarchi, _Real analysis: measure theory, integration, and Hilbert spaces_. Princeton University Press, 2009.
  * [39] R. Bellman, “On the theory of dynamic programming,” _Proceedings of the national Academy of Sciences_ , vol. 38, no. 8, pp. 716–719, 1952.
  * [40] L. Gurobi Optimization, “Gurobi optimizer reference manual,” https://www.gurobi.com, 2023.
  * [41] F. Debrouwere, W. Van Loock, G. Pipeleers, Q. T. Dinh, M. Diehl, J. De Schutter, and J. Swevers, “Time-optimal path following for robots with convex–concave constraints using sequential convex programming,” _IEEE Transactions on Robotics_ , vol. 29, no. 6, pp. 1485–1495, 2013.

|  Yunan Wang (S’22) received the B.S. degree in mechanical engineering, in
2022, from the Department of Mechanical Engineering, Tsinghua University,
Beijing, China. He is currently working toward the Ph.D. degree in mechanical
engineering. His research interests include optimal control, trajectory
planning, toolpath planning, and precision motion control. He was the
recipient of the Best Conference Paper Finalist at the 2022 International
Conference on Advanced Robotics and Mechatronics, and 2021 Top Grade
Scholarship for Undergraduate Students of Tsinghua University.
---|---
|  Chuxiong Hu (S’09-M’11-SM’17) received his B.S. and Ph.D. degrees in
Mechatronic Control Engineering from Zhejiang University, Hangzhou, China, in
2005 and 2010, respectively. He is currently an Associate Professor (tenured)
at Department of Mechanical Engineering, Tsinghua University, Beijing, China.
From 2007 to 2008, he was a Visiting Scholar in mechanical engineering with
Purdue University, West Lafayette, USA. In 2018, he was a Visiting Scholar in
mechanical engineering with University of California, Berkeley, CA, USA. His
research interests include precision motion control, high-performance
multiaxis contouring control, precision mechatronic systems, intelligent
learning, adaptive robust control,neural networks, iterative learning control,
and robot. Prof. Hu was the recipient of the Best Student Paper Finalist at
the 2011 American Control Conference, the 2012 Best Mechatronics Paper Award
from the ASME Dynamic Systems and Control Division, the 2013 National 100
Excellent Doctoral Dissertations Nomination Award of China, the 2016 Best
Paper in Automation Award, and 2018 Best Paper in AI Award from the IEEE
Internal Conference on Information and Automation. He is now a Technical
Editor for the IEEE/ASME Transactions on Mechatronics.
---|---
|  Zeyang Li received the B.S. degree in mechanical engineering in 2021, from
the School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai,
China. He is currently working toward the M.S. degree in mechanical
engineering with the Department of Mechanical Engineering, Tsinghua
University, Beijing, China. His current research interests include the areas
of optimal control and reinforcement learning.
---|---
|  Shize Lin received the B.S. degree in mechanical engineering, in 2020, from
the Department of Mechanical Engineering, Tsinghua University, Beijing, China,
where he is currently working toward the Ph.D. degree in mechanical
engineering. His research interests include robotics, motion planning and
precision motion control.
---|---
|  Suqin He received the B.S. degree in mechanical engineering from Department
of Mechanical Engineering, Tsinghua University, Beijing, China, in 2016, and
the Ph.D. degree in mechanical engineering from the Department of Mechanical
Engineering, Tsinghua University, Beijing, China, in 2023. His research
interests include multi-axis trajectory planning and precision motion control
on robotics and CNC machine tools. He was the recipient of the Best Automation
Paper from IEEE Internal Conference on Information and Automation in 2016.
---|---
|  Ze Wang (S’16-M’21) received the B.S. degree in automation from the School
of Electricity Engineering and Automation, Tianjin University, Tianjin, China,
in 2015, and the Ph.D. degree in mechanical engineering from the Department of
Mechanical Engineering, Tsinghua University, Beijing, China, in 2020. He is
currently an assistant professor with the Department of Mechanical
Engineering, Tsinghua University. His research interests include precision
motion control, neural network adaptive control, and high-performance multi-
axis contouring control. Dr. Wang was the recipient of the Best Student Paper
Finalist at the 2017 IEEE/ASME International Conference on Advanced
Intelligent Mechatronics, the 2017 Best Student Paper from the IEEE Internal
Conference on Information and Automation, and 2019 Top Grade Scholarship for
Graduate Students of Tsinghua University.
---|---
|  Yu Zhu (Member, IEEE) received the B.S. degree in radio electronics from
Beijing Normal University, Beijing, China, in 1983, and the M.S. degree in
computer applications and the Ph.D. degree in mechanical design and theory
from the China University of Mining and Technology, Beijing, in 1993 and 2001,
respectively. He is currently a Professor with the State Key Laboratory of
Tribology, Department of Mechanical Engineering, Tsinghua University, China.
He has authored or coauthored more than 180 technical papers. He is also the
holder of more than 140 warranted invention patents. His research interests
include precision measurement and motion control, ultraprecision mechanical
design and manufacturing, two-photon microfabrication, and electronics
manufacturing technology and equipment.
---|---