# Constraint–Aware Mesh Refinement Method by Reachability Set Envelope of
Curvature Bounded Paths

Juho Bae<EMAIL_ADDRESS>Ji Hoon Bai<EMAIL_ADDRESS>Byung–Yoon Lee<EMAIL_ADDRESS>Jun–Yong Lee
<EMAIL_ADDRESS>Korea Advanced Institute of Science and Technology,
Daejeon, 34141, Republic of Korea  Nearthlab, Seoul, 06246, Republic of Korea

###### Abstract

This paper presents an enhanced direct-method-based approach for the real-time
solution of optimal control problems to handle path constraints, such as
obstacles. The principal contributions of this work are twofold: first, the
existing methods for constructing reachability sets in the literature are
extended to derive the envelope of these sets, which determines the region
swept by all feasible trajectories between adjacent sample points. Second, we
propose a novel method to guarantee constraint violation–free between discrete
states in two dimensions through mesh refinement approach. To illustrate the
effectiveness of the proposed methodology, numerical simulations are conducted
on real-time path planning for fixed–wing unmanned aerial vehicles.

###### keywords:

Reachability Set; Envelope; Direct Collocation Method; Inter–Sample
Collision–Avoidance; Mesh Refinement; Sequential Convex Programming.

††thanks: This paper was not presented at any conference.

, , , ††thanks: Corresponding author at: Nearthlab, Seoul, 06246, Republic of
Korea. Tel +821063088045.

##  1 Introduction

Over the past decade, the need for rapid and efficient real–time algorithms to
solve optimal control problems has intensified, particularly for onboard
guidance problems such as the powered descent landing problem in [2], [27],
and [25], the re–entry guidance problem in [15], [32], [33], and [3], and
planning of unmanned–aerial vehicle(UAV) trajectories in [21] and [30].

Recent advancements in real–time optimal control involve direct methods
combined with sequential convex programming (SCP) [2], [15], [28], [19], [29],
and [3]. The direct method partitions the domain into multiple intervals
separated by sample points, or nodes. Subsequently, polynomial interpolation
is employed to determine the values of state and control variables between
these nodes, with the specific interpolation method chosen based on the
circumstances. In a typical optimal control problem(OCP) that involves path
constraints, the forbidden regions, where the path constraint restrains the
state variables to enter, is a key focus of our study. Obstacles from the UAV
trajectory planning problems, dynamics pressure bound from the re–entry
guidance problem are specific examples of such forbidden regions.

Direct method formulation applies path constraints at each mesh points,
preventing state variables from entering forbidden regions. However, the
generated trajectory in between the mesh points may trespass the forbidden
regions. This is commonly known as the inter–sample collision problem [7]. Not
only confined in the SCP framework based approaches, it is seriously
considered as one of the most critical difficulties in practical application
of the general direct method based algorithms [30]. Despite the term
collision, path constraints extend beyond collision avoidance, as seen in re-
entry guidance problem in [15] and [3]. In this context, we will refer such an
issue as the problem of obtaining a path constraint violation–free trajectory.

To address this challenge, a method for implementing path constraints
involving cylindrical obstacles was proposed in [7]. The core idea involved
estimating the shortest distance between an obstacle’s center and the path
connecting two adjacent nodes. By ensuring that this minimum distance exceeds
the obstacle’s radius, a collision-free constraint was established. However,
this method relies on linearization of the dynamics, which introduces
truncation errors. Moreover, the most significant drawback of this method is
the loss of optimality. When there are only a limited number of nodes in close
proximity to an obstacle, the resulting collision-free constraints tend to
enforce a trajectory that maintains a significant distance from the obstacle
in an overly cautious manner. This can result in discrete trajectories that
are far from continuous time optimal trajectory, the actual correct solution.

Motivated by these trends, our research offers improved solutions for the
general inter–sample collision problems, guaranteeing path constraint
violation–free. Considering the aforementioned issues, we recognized the
importance of incorporating additional nodes, or refining mesh, to effectively
apply collision-free constraints.

Previous studies have adopted additional nodes in direct methods, primarily to
reduce dynamics interpolation errors during discretization. Several
collocation methods have been explored, primarily centered around the Pseudo-
Spectral method [8]. Various frameworks have been developed in [26], [13],
[14], and [23] to adaptively refine the mesh intervals between the nodes by
strategically inserting additional nodes in suitable locations. It is worth
noting that the conventional mesh refinement methods primarily focus on
addressing interpolation errors arising from specific attributes of the
dynamics constraints, such as smoothness, but fail to consider the arbitrary
placement of forbidden regions during the mesh refinement process [13].
Consequently, potential risk of violation into the forbidden region cannot be
detected using these methods. Additionally, arbitrarily increasing the number
of nodes is not a viable solution as it leads to a significant increase in
computational costs.

Our proposed method targets path constraint violation–free trajectories, while
minimizing mesh insertion for computational efficiency. The desired property
is attained by analytically determining the bounds within which the
continuous-time trajectory can exist between two fixed adjacent nodes. Such
concept is an expansion the idea of reachability set, or attainability set, of
curvature bounded paths in [22], [5], and [9] into the envelope of multiple
reachability sets. Whenever a solution is obtained from the direct method, the
violation of the aforementioned trajectory bound into the forbidden region
indicates the potential risk of path constraint violation at such mesh
interval. Our approach starts with formulating and proving a method to
construct such trajectory bounds, followed by a technique to assess any bound
violations into forbidden regions. Furthermore, a proof of the convergence of
the overall proposed algorithm is provided, along with a numerical
demonstration of its computational cost, particularly in light of its intended
use in onboard applications.

Let us first introduce some notations in the related literatures regarding
curvature bounded paths in plane. From hereon, whenever a planar curve has an
upper bound of $\kappa_{m}$ on its curvature, let us denote the part of the
trajectory with constant signed curvature of $\pm\kappa_{m}$ by C, as it is
part of a circular arc. The part of the trajectory with zero curvature is
denoted by S, as it is a straight segment. For instance, if a trajectory
initially follows a circular arc and then subsequently follows a segment, then
such trajectory is denoted by CS. It was further proved in [22] that the
corresponding control input that brings the endpoint of the trajectory to the
boundary of the reachability set in a sense of terminal location and
direction, contains at most two switchings in the control. Moreover, it was
also proved that if the trajectory contains two switchings, then it is in the
class of CSC or CCC. For simpler case when dealing only terminal location, it
was proved that the boundary of the reachability set can be reached by
trajectories of class CC and CS [5].

Unlike the ingenious geometric arguments in [6] and [5], the optimal control
theory based approaches in [12] and [22] further motivates us to formulate the
considered problem of obtaining the trajectory bound in a form of optimal
control problem. By solving the necessary conditions of optimality given by
the Pontryagin Maximum Principle(PMP), it is proved that the boundary of the
trajectory bound can be constructed using trajectories consist of at most five
C and S segments. In addition to the mathematical proof, further intuitive
insight is also provided. Consequently, we adopt the idea of covering the
obtained trajectory bound by multiple rectangular patches, thereby developing
a method to distinguish whether the constructed bounds encroach upon the
forbidden region. This analytical approach enables efficient and precise
assessment of whether additional mesh insertion is required at a certain mesh
interval, thereby achieving path constraint violation–free trajectories while
minimizing the number of additional mesh insertion. The proposed method can be
easily integrated into general direct formulation of optimal control problems,
and the termination of the algorithm in a finite number of iteration is
proved. The performance and computational efficiency of the algorithm is
validated through numerical simulations with 2D fixed–wing dynamics.

The remaining sections of this paper are organized as follows. Section 2
presents the problem statement and direct formulation of the Bolza optimal
control problem. In Section 3, the construction method of the reachability set
envelope of curvature bounded paths is derived. Section 4 describes the
methodology to detect intersection between the forbidden region and the
obtained envelope. Section 5 provides numerical demonstration results to
further validate the effectiveness of the proposed approach. Finally, in
Section 6, we conclude this work by highlighting the key contributions of this
paper.

##  2 Direct Formulation of the Optimal Control Problem

In this paper, the below form of Bolza optimal control problem is considered:

$\begin{array}[]{l}{\rm{minimize}}\quad
J=\Phi\left({\boldsymbol{z}\left({-1}\right),t_{0},\boldsymbol{z}\left({+1}\right),t_{f}}\right)\\\
+\frac{t_{f}-t_{0}}{2}\int_{-1}^{+1}{F\left({\boldsymbol{z}\left(\tau\right),\boldsymbol{u}(\tau),\tau}\right)d\tau}\\\
\text{subject
to}\quad\frac{d\boldsymbol{z}}{d\tau}=\frac{t_{f}-t_{0}}{2}f\left({\boldsymbol{z}\left(\tau\right),\boldsymbol{u}\left(\tau\right),\tau}\right),\\\
h\left({\boldsymbol{z}\left(\tau\right)}\right)\geq
0,\quad\Psi\left({\boldsymbol{z}\left({-1}\right),t_{0},\boldsymbol{z}\left({+1}\right),t_{f}}\right)\geq
0\\\ \end{array}$ (1)

where the state variables are denoted by
$\boldsymbol{z}(\tau)\in\mathbb{R}^{n_{\boldsymbol{z}}}$ and the control by
$\boldsymbol{u}(\tau)\in\mathbb{R}^{n_{\boldsymbol{u}}}$. The functions
$f:\mathbb{R}^{n_{\boldsymbol{z}}+n_{\boldsymbol{u}}+1}\mapsto\mathbb{R}^{n_{\boldsymbol{z}}}$,
$F:\mathbb{R}^{n_{\boldsymbol{z}}+n_{\boldsymbol{u}}+1}\mapsto\mathbb{R}$,
$\Phi:\mathbb{R}^{2n_{\boldsymbol{z}}+2}\mapsto\mathbb{R}$,
$h:\mathbb{R}^{n_{\boldsymbol{z}}}\mapsto\mathbb{R}^{n_{h}}$,
$\Psi:\mathbb{R}^{2n_{\boldsymbol{z}}+2}\mapsto\mathbb{R}^{n_{\Psi}}$ denotes
the dynamics, cost integrand, boundary cost, path inequality constraint, and
boundary constraint functions where $n_{\Psi}$ and $n_{h}$ denotes the number
of each constraints. If the final time $t_{f}$ is considered as a free
variable, it is also considered as one of the control variable. Throughout the
paper, the $i$–th component function of any function $g(\cdot)$ is denoted by
$g_{i}(\cdot)$. An assumption on the path constraints is that each component
functions are of bivariate form. (i.e. For each $k\in\\{1,\dots,n_{h}\\}$, the
function $h_{k}$ depends on two state variables and remains constant with
respect to the other remaining state variables.) Such formulation includes
numerous practical examples such as the re-entry guidance problem in [15],
problems with 2–dimensional dynamics under holonomic constraints [7], and
missile guidance problems with no–fly–zones. Extension of the proposed method
to higher dimensions is left for the future work. Suppose now the interval
$[-1,+1]$ is discretized by mesh points(nodes) :
$-1=\tau_{1},\tau_{2},\dots,\tau_{N-1},\tau_{N}=+1$ into total of $N-1$ number
of mesh intervals(meshes) $[\tau_{j},\tau_{j+1}],\;\;j=1,\dots,N-1$. The
lengths of the mesh intervals are denoted as
$\Delta\tau_{j}=\tau_{j+1}-\tau_{j},\;\;j=1,\dots,N-1$. Let
$\boldsymbol{z}_{j}$ and $\boldsymbol{u}_{j}$ denote the values at $\tau_{j}$,
$\boldsymbol{z}(\tau_{j})$ and $\boldsymbol{u}(\tau_{j})$, respectively. Then
the discretization step based on numerical integration with trapezoidal rule
yields the below discretized problem Eq. (2).

$\begin{array}[]{l}{\rm{minimize}}\quad
J=\Phi\left({\boldsymbol{z}_{1},t_{0},\boldsymbol{z}_{N},t_{f}}\right)\\\
+\Bigg{[}\frac{{\Delta\tau_{1}}}{2}F\left({\boldsymbol{z}_{1},\boldsymbol{u}_{1},\tau_{1}}\right)+\left({\sum\limits_{j=2}^{N-1}{\frac{{\Delta\tau_{j-1}+\Delta\tau_{j}}}{2}}}F\left({\boldsymbol{z}_{j},\boldsymbol{u}_{j},\tau_{j}}\right)\right)\\\
+\frac{{\Delta\tau_{N-1}}}{2}F\left({\boldsymbol{z}_{N},\boldsymbol{u}_{N},\tau_{N}}\right)\Bigg{]}\times\frac{t_{f}-t_{0}}{2}\\\
\text{subject
to}\quad\boldsymbol{z}_{j+1}-\boldsymbol{z}_{j}=\frac{t_{f}-t_{0}}{2}\frac{{\Delta\tau_{j}}}{2}\left[f\left({\boldsymbol{z}_{j},\boldsymbol{u}_{j},\tau_{j}}\right)\right.\\\
\left.+f\left({\boldsymbol{z}_{j+1},\boldsymbol{u}_{j+1},\tau_{j+1}}\right)\right],\\\
h\left({\boldsymbol{z}_{j}}\right)\geq 0,\quad j=1,\ldots,N-1,\\\
\Psi\left({\boldsymbol{z}_{1},t_{0},\boldsymbol{z}_{N},t_{f}}\right)\geq 0\\\
\end{array}$ (2)

Among the various interpolation methods available, we opted for the standard
collocation method instead of the well-known Pseudo–Spectral method in this
paper. Such decision is based on two main reasons. Firstly, the
Pseudo–Spectral method tends to significantly improve dynamics interpolation
error at the cost of increase in computational burden [25]. However, in the
context of our method, minimizing dynamics interpolation error is not the
primary objective. As a result, incorporating the Pseudo-Spectral method would
only add computational overhead without the contribution of enhancing the
efficiency of mesh refinement method by minimizing the number of additional
meshes. Secondly, the scope of application of the proposed method in this
paper need not to be limited by the collocation methods. Therefore, we aimed
to keep the basic problem formulation as simple as possible. By adopting the
standard collocation method within the SCP framework, we strike a balance
between computational efficiency and achieving path constraint violation–free
trajectories, while maintaining flexibility to extend the method to Pseudo-
Spectral formulations if needed.

##  3 Reachability Set Envelope of Curvature Bounded Paths

In this section, we develop a construction methodology for the region in which
a trajectory connecting two adjacent mesh points can exist while the state
variables at the two mesh points are assumed to be given. This construction
method is developed by introducing the concept of the Reachability Set and its
envelope. Let us commence by presenting the problem settings and associated
notations.

###  3.1 The Reachability Set of Curvature Bounded Paths

Let us denote $z_{i}(\tau_{j})$, the $i$–th state variable at $\tau_{j}$, by
$z_{i,j}$. Since the path inequality constraints are of bivariate form, we may
further assume without loss of generality that the path constraints are
applied on the first and second state variables, $h_{k}(z_{1,j},z_{2,j})\geq
0$ and $h_{k}(z_{1,j+1},z_{2,j+1})\geq 0$, at two adjacent mesh points
$\tau_{j}$ and $\tau_{j+1}$. Define a projection map
$\pi:\mathbb{R}^{n_{\boldsymbol{z}}}\mapsto\mathbb{R}^{2}$,
$\pi(z_{1},z_{2},\dots,z_{n_{\boldsymbol{z}}})=(z_{1},z_{2})$. Whenever the
states, $\boldsymbol{z}_{j}$ and $\boldsymbol{z}_{j+1}$, and the controls,
$u_{j}$ and $u_{j+1}$, at the mesh points, $\tau_{j}$ and $\tau_{j+1}$, are
determined respectively, we can consider a two–dimensional curve
$(\pi\circ\boldsymbol{z})(\tau)=(z_{1}(\tau),z_{2}(\tau)):[\tau_{j},\tau_{j+1}]\mapsto\mathbb{R}^{2}$
that connects the two points
$(\pi\circ\boldsymbol{z})(\tau_{j})=(z_{1,j},z_{2,j})$ and
$(\pi\circ\boldsymbol{z})(\tau_{j+1})=(z_{1,j+1},z_{2,j+1})\in\mathbb{R}^{2}$,
which is depicted in Fig. 1.

Figure 1: Example of a curve $(z_{1}(\tau),z_{2}(\tau))$ and a forbidden
region

Let us denote the curvature of such curve by $\kappa(\tau)$. We additionally
assume that the curve is regular, the function $f$ is $C^{1}$, and the control
input is piecewise $C^{1}$, which guarantees that the curve is piecewise
$C^{2}$. Furthermore, it is assumed that an upper bound on the curvature
exists, denoted by
$\kappa_{m}\geq\sup\limits_{\tau\in[\tau_{j},\tau_{j+1}]}\kappa(\tau)$. The
ultimate goal of this section is to find the reachable region by the curve
$(\pi\circ\boldsymbol{z})$ under such curvature constraint. For further
procedure, we normalize the curve $\pi\circ\boldsymbol{z}$ by its arc length
$L=\int_{\tau_{j}}^{\tau_{j+1}}\|(\pi\circ\boldsymbol{z})^{\prime}(\tau)\|d\tau$
for sake of simplicity. Then the trajectory, or the image of the normalized
curve, $\frac{1}{L}(\pi\circ\boldsymbol{z})([\tau_{j},\tau_{j+1}])$, of length
one can be parametrized by a regular, curvature bounded curve parametrized by
arc length. We further represent such curve as
$\alpha_{u}=(x,y):[0,1]\mapsto\mathbb{R}^{2}$, where
$\alpha_{u}([0,1])=\frac{1}{L}(\pi\circ\boldsymbol{z})([\tau_{j},\tau_{j+1}])$.
Then $\alpha_{u}$ can be characterized by the below dynamics:

$\dot{\boldsymbol{\chi}}=\begin{pmatrix}\dot{x}\\\ \dot{y}\\\
\dot{\gamma}\end{pmatrix}=\begin{pmatrix}\cos\gamma\\\ \sin\gamma\\\
u\end{pmatrix}\equiv f_{\chi}(\boldsymbol{\chi},u)$ (3)

where $\gamma$ is the direction of the curve $\alpha_{u}$, and the state
variables $[x\;y\;\gamma]^{T}$ are denoted by $\boldsymbol{\chi}$. $u$ is
regarded as the signed curvature of the planar curve $\alpha_{u}$, and the
upper bound on curvature $|u|\leq\kappa_{m}^{\prime}=L\kappa_{m}$ is
considered. Let $\Omega$ denote the admissible control set,
$\Omega=\\{u:[0,1]\mapsto[-\kappa_{m}^{\prime},\kappa_{m}^{\prime}]\enspace|\enspace\text{piecewise
}C^{1}\\}$. Since
$\dot{z}_{1}(\tau)=f_{1}(\boldsymbol{z},\boldsymbol{u},\tau)$ and
$\dot{z}_{2}(\tau)=f_{2}(\boldsymbol{z},\boldsymbol{u},\tau)$, it follows
directly that the initial and final directions, $\gamma(0)$ and $\gamma(1)$,
of the trajectory depicted in Fig. 1 can be determined by appropriately
applying inverse tangent to the ratios
$f_{2}(\boldsymbol{z}_{j},\boldsymbol{u}_{j},\tau_{j})/f_{1}(\boldsymbol{z}_{j},\boldsymbol{u}_{j},\tau_{j})$
and
$f_{2}(\boldsymbol{z}_{j+1},\boldsymbol{u}_{j+1},\tau_{j+1})/f_{1}(\boldsymbol{z}_{j+1},\boldsymbol{u}_{j+1},\tau_{j+1})$.
Hence, the values of $\boldsymbol{\chi}(0)$ and $\boldsymbol{\chi}(1)$ can be
specified whenever the state and controls at the both mesh points are given.
This dynamics is often referred as Dubin’s Car dynamics [6]. For the
projection $\pi_{1}:\mathbb{R}^{3}\mapsto\mathbb{R}^{2}$,
$\pi_{1}(x,y,\gamma)=(x,y)$, it is evident that firstly,
$\pi_{1}(\boldsymbol{\chi})=\alpha_{u}$ and secondly, the image
$\pi_{1}(\boldsymbol{\chi})([0,1])=\alpha_{u}([0,1])$ is the image
$(\pi\circ\boldsymbol{z})([\tau_{j},\tau_{j+1}])$ scaled to $\frac{1}{L}$
size. Hence, we aim to first find the reachable region of the unit length
curve $\alpha_{u}$, and scale it back to find the reachable region of
$\pi\circ\boldsymbol{z}$. By applying an appropriate rigid motion, we may
presume that the initial state
$\boldsymbol{\chi}_{0}=\left[{x_{0},\;y_{0},\;\gamma_{0}}\right]^{T}=\boldsymbol{0}$.
(i.e. $\alpha_{u}(0)=(0,0)$ and $\alpha_{u}^{\prime}(0)=(1,0)$). Consequently,
we can introduce the below definition of Reachability Set, which was
introduced and studied extensively in [12] and [22].

######  Definition 1.

Reachability Set [12] and [22]

$\mathcal{G}(T)\equiv\\{\boldsymbol{z}:\exists u\in\Omega\text{ s.t.
}\boldsymbol{\chi}(T)=\boldsymbol{z}\\}$

The set $\mathcal{G}(T)$ represents the extent to which the state variables
can either deviate from or remain close to the initial point over a given time
$T$. It is worth noting that the terminal state $\boldsymbol{\chi}(1)$ is not
considered in this definition; instead, the primary emphasis is placed on
determining the region of feasible terminal states. It was further stated and
proved in [12] that any trajectory $\\{\bar{\boldsymbol{\chi}},\bar{u}\\}$
reaching the boundary point of $\mathcal{G}(T)$ at time $T$ satisfies the
below PMP:

$\begin{array}[]{l}\dot{\boldsymbol{p}}(s)=-\frac{\partial\mathcal{H}}{\partial\boldsymbol{\chi}}\\\
\mathcal{H}(\boldsymbol{p},\bar{\boldsymbol{\chi}},\bar{u})=\max\limits_{u\in\Omega}\mathcal{H}(\boldsymbol{p},\bar{\boldsymbol{\chi}},u)\quad\text{almost
everywhere}\end{array}$ (4)

where the Hamiltonian is defined as $\mathcal{H}=\boldsymbol{p}^{T}f_{\chi}$.
The aforementioned statement is not restricted to the dynamics of Dubins’ car
but is applicable to general dynamics of class $C^{1}$ that have bounded
response for a measurable admissible control set [12]. For the Dubins’ car
dynamics specifically, it was proved in [22] that any trajectory reaching the
boundary of $\mathcal{G}(T)$ is of class CCC or CSC.

When the focus of reachability is solely on the locations, the following set,
which is essentially the projection of $\mathcal{G}(T)$ onto the $xy$-plane,
can be defined:

######  Definition 2.

$\mathcal{G}^{\prime}(T)\equiv\\{\pi_{1}(\boldsymbol{z}):\exists
u\in\Omega\text{ s.t. }\boldsymbol{\chi}(T)=\boldsymbol{z}\\}$

It was proved in [5] that any boundary point of $\mathcal{G}^{\prime}(T)$ can
be reached by a trajectory of class CC or CS. With such considerations, let us
define the set below, which is of our primary interest in this paper.
Additionally, we denote the tangent bundle of a regular surface $S$ by
$\mathbb{T}S$.

######  Definition 3.

Given $\mathbb{X}\in\mathbb{T}\mathbb{R}^{2}$ and
$\kappa_{m}\in\mathbb{R}_{>0}$, define the below sets where the curve $\chi$
is defined by $\alpha_{u}$ as Eq. (3).

$\begin{array}[]{l}\mathcal{B}(\mathbb{X},\kappa_{m})\equiv\cup\boldsymbol{\chi}([0,1])\text{
s.t. }u\in\Omega,\\\
(\alpha_{u}(0),\dot{\alpha}_{u}(0))=((0,0),(1,0)),(\alpha_{u}(1),\dot{\alpha}_{u}(1))=\mathbb{X}\end{array}$
$\begin{array}[]{l}\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})\equiv\cup\alpha_{u}([0,1])\text{
s.t. }u\in\Omega,\\\
(\alpha_{u}(0),\dot{\alpha}_{u}(0))=((0,0),(1,0)),(\alpha_{u}(1),\dot{\alpha}_{u}(1))=\mathbb{X}\end{array}$

The set $\mathcal{B}(\mathbb{X},\kappa_{m})$ is the collection of all points
such that there exists some curve passing through the point that connects
$((0,0),(1,0))\in\mathbb{T}\mathbb{R}^{2}$ and $\mathbb{X}$, with a curvature
bounded above $\kappa_{m}$. The boundary of
$\mathcal{B}(\mathbb{X},\kappa_{m})$ is the envelope of those possible curves.
The set $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ is the projection of
$\mathcal{B}(\mathbb{X},\kappa_{m})$ onto $xy$–plane, neglecting the bound on
the last component $\gamma$. By the aforementioned definitions, the task of
identifying the bounded region in which the state variables of interest can
exist is transformed into the problem of determining the set
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. Therefore, the remaining
subsections are focused on developing a methodology to construct the boundary
of $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$.

###  3.2 The Original Optimal Control Problem

Consider the OCP [P0] below, where $f_{\chi}(\cdot)$ is as defined in Eq. (3).

[P0] : maximize

$\enspace J=\Phi\left(\boldsymbol{\chi}\left(T\right)\right)$ (5)

subject to

$\begin{array}[]{l}\dot{\boldsymbol{\chi}}\left(\tau\right)=f_{\chi}\left(\boldsymbol{\chi}\left(\tau\right),u\left(\tau\right)\right),\\\
\boldsymbol{\chi}\left(0\right)=\boldsymbol{\chi}_{0},\\\ -\kappa_{m}\leq
u\leq\kappa_{m}\end{array}$ (6)

In the above formulation, a terminal cost function
$\Phi:\mathbb{R}^{3}\mapsto\mathbb{R}$ with a nonzero constant gradient field
in an arbitrary direction is considered. The OCP [P0] is defined per every
such $\Phi$. The below proposition provides some intuition for Eq. (4).

######  Proposition 4.

The collection of PMP sense solutions of every [P0] is the set of solutions of
Eq. (4).

Proof. The PMP condition of [P0] consists of the conditions of Eq. (4), the
transversality condition
$\boldsymbol{p}(T)=p_{0}\nabla_{\boldsymbol{\chi}}\Phi$, $p_{0}\geq 0$, and
the nontriviality condition applied to $\\{\boldsymbol{p},p_{0}\\}$. Hence for
any solution of Eq. (4), nontriviality of $\boldsymbol{p}$ directly implies
nontriviality of the pair $\\{\boldsymbol{p},p_{0}\\}$. The transversality
condition $\boldsymbol{p}(T)=p_{0}\nabla_{\boldsymbol{\chi}}\Phi$ can be met
by choosing appropriate $p_{0}\geq 0$ and
$\nabla_{\boldsymbol{\chi}}\Phi\in\mathbb{R}^{3}\setminus\\{\boldsymbol{0}\\}$.
Therefore, if $\\{\boldsymbol{\chi},u,\boldsymbol{p}\\}$ is a solution of Eq.
(4), then it is also a PMP sense solution of some [P0]. Conversely, suppose
the pair $\\{\boldsymbol{\chi},u,\boldsymbol{p},p_{0}\\}$ is a solution of
some [P0]. If $\boldsymbol{p}\equiv 0$, then the transversality condition
further implies $p_{0}=0$ since
$\nabla_{\boldsymbol{\chi}}\Phi\neq\boldsymbol{0}$. This violates the
nontriviality condition of [P0]. Therefore, the nontriviality condition must
hold for $\boldsymbol{p}$ alone, and hence the pair
$\\{\boldsymbol{\chi},u,\boldsymbol{p}\\}$ becomes a solution of Eq. (4). ∎
The fact that any boundary point of $\mathcal{G}(T)$ is a solution of [P0] is
intuitive in a sense that [P0] measures how far or close the endpoint of the
trajectory can be located in a certain direction. To invoke the concept of
‘direction’, it should be noted that the nonvanishing gradient of $\Phi$ must
be presumed. Indeed, such condition played an important role related to the
nontriviality condition in the above proof. It is important to note some
specific discriminations between the existing literatures [22], [9], and [5],
and our situation. The bound we aim to obtain is not the reachability set
itself at a certain moment, but it is the envelope, or the collection, of the
reachability sets per each terminal times in $[0,1]$. In other words, the
problem considered in the existing literatures utilized the necessary
conditions that optimize the terminal state, while the considered problem in
this paper requires optimizing the state at a certain moment, $T$, during the
trajectory. Then collecting all such optimized state over $T\in[0,1]$ would
form an envelope, which is the desired bound for mesh refinement. The
subsequent subsection is devoted to handle such differences.

###  3.3 Augmentation of Two Optimal Control Problems

Let us denote the desired terminal state by
$\boldsymbol{\chi}_{f}=\left[{x_{f},\;y_{f},\;\gamma_{f}}\right]^{T}$ where
$\gamma_{f}\in(-\pi,\pi]$. Then considering potential cycles, any curve with
terminal value $\left[{x_{f},\;y_{f},\;\gamma_{f}+2k\pi}\right]^{T}$ where
$k\in\mathbb{Z}$ satisfies the terminal state constraint. Consider the below
region where the points on a trajectory can be located at a specific time
$T\in[0,1]$.

######  Definition 5.

$\begin{array}[]{l}\mathcal{R}_{k}(T)\equiv\\{\boldsymbol{z}:\exists
u\in\Omega\text{ s.t. }\boldsymbol{\chi}(0)=\boldsymbol{\chi}_{0},\\\
\boldsymbol{\chi}(T)=\boldsymbol{z},\boldsymbol{\chi}(1)=\left[{x_{f},\;y_{f},\;\gamma_{f}+2k\pi}\right]^{T}\\}\end{array}$

Similar definition as $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ is made as
follows: $\mathcal{R}_{k}^{\prime}(T)\equiv\pi_{1}(\mathcal{R}_{k}(T))$. In
relation to finding such regions, we take into account the following OCP:

[$\textbf{P0}_{T,k}$] : maximize

$\enspace J=\Phi\left(\boldsymbol{\chi}\left(T\right)\right)$ (7)

subject to

$\begin{split}&\dot{\boldsymbol{\chi}}\left(\tau\right)=f_{\chi}\left(\boldsymbol{\chi}\left(\tau\right),u\left(\tau\right)\right),\\\
&\boldsymbol{\chi}\left(0\right)=\boldsymbol{\chi}_{0},\\\
&\boldsymbol{\chi}\left(1\right)=\left[{x_{f},\;y_{f},\;\gamma_{f}+2k\pi}\right]^{T},\\\
&-\kappa_{m}\leq u\leq\kappa_{m}\end{split}$ (8)

It is evident that
$\bigcup\limits_{k\in\mathbb{Z}}\bigcup\limits_{T\in[0,1]}\mathcal{R}_{k}(T)=\mathcal{B}(\mathbb{X},\kappa_{m})$
and hence we devote the remaining part of this subsection to identify the
boundary of $\mathcal{R}_{k}(T)$. It is noteworthy that it is not yet trivial
that any trajectory reaching the boundary point of $\mathcal{R}_{k}(T)$ is a
PMP sense solution of $[\textbf{P0}_{T,k}]$.

Whenever the time $\tau$ reaches $T$, the remaining time–to–go must be $1-T$,
as the final time is set to be 1. Since the conventional PMP only permits the
consideration of cost functions at the terminal time, and not at any point
along the trajectory, we will address two OCPs. One is defined over the time
domain $[0,T]$ and the other is defined over $[0,1-T]$. For the second part of
the trajectory on interval $[0,1-T]$, we consider a trajectory with
orientation–reversing reparametrization, starting from initial state
$\left[{x_{f},\;y_{f},\;\gamma_{f}+2k\pi}\right]^{T}$. By adding a terminal
state constraints that the two trajectories have same terminal location and
direction at $T$, we can define an augmented OCP.

Now, let us define the two aforementioned OCPs as follows. After appropriate
reparametrization, the both OCPs are parametrized by $s\in[0,1]$ instead of
$\tau$, where $T$ is some constant in $(0,1)$.
$\boldsymbol{\chi}_{1}(s)=\left[{x_{1}(s),\;y_{1}(s),\;\gamma_{1}(s)}\right]^{T}$
and
$\boldsymbol{\chi}_{2}(s)=\left[{x_{2}(s),\;y_{2}(s),\;\gamma_{2}(s)}\right]^{T}$
represent the state variables of the two OCPs respectively.

[P1] : maximize

$\enspace J=\Phi\left(\boldsymbol{\chi}_{1}\left(1\right)\right)$ (9)

subject to

$\begin{split}&\frac{d}{ds}\boldsymbol{\chi}_{1}\left(s\right)=Tf_{\chi}\left(\boldsymbol{\chi}_{1}\left(s\right),u_{1}\left(s\right)\right),\\\
&\boldsymbol{\chi}_{1}\left(0\right)=\boldsymbol{\chi}_{0},\\\
&-\kappa_{m}\leq u\leq\kappa_{m}\end{split}$ (10)

[P1] is the OCP for the time interval $[0,T]$. We define the OCP [P2]
corresponding to the time interval $[T,1]$ as below, with consideration of the
terminal state constraints.

[$\textbf{P2}_{k}$] : maximize

$\enspace J=\Phi\left(\boldsymbol{\chi}_{2}\left(1\right)\right)$ (11)

subject to

$\begin{split}&\frac{d}{ds}\boldsymbol{\chi}_{2}\left(s\right)=-(1-T)f_{\chi}\left(\boldsymbol{\chi}_{2}\left(s\right),u_{2}\left(s\right)\right),\\\
&\boldsymbol{\chi}_{2}\left(0\right)=\left[{x_{f},\;y_{f},\;\gamma_{f}+2k\pi}\right]^{T},\\\
&-\kappa_{m}\leq u\leq\kappa_{m}\end{split}$ (12)

It is noteworthy that the curvature constraint remains the same in the
normalized domain of $s\in[0,1]$ since curvature is invariant under
reparametrization. Consequently, the augmented OCP below is considered.

[$\textbf{P3}_{k}$] : maximize

$\enspace
J=\Phi\left(\boldsymbol{\chi}_{1}\left(1\right),\boldsymbol{\chi}_{2}\left(1\right)\right)$
(13)

subject to

$\frac{d}{ds}\boldsymbol{\chi}\left(s\right)=\begin{bmatrix}\dot{\boldsymbol{\chi}}_{1}\left(s\right)\\\
\dot{\boldsymbol{\chi}}_{2}\left(s\right)\end{bmatrix}=\begin{bmatrix}Tf_{\chi}\left(\boldsymbol{\chi}_{1}\left(s\right),u_{1}\left(s\right)\right)\\\
-(1-T)f_{\chi}\left(\boldsymbol{\chi}_{2}\left(s\right),u_{2}\left(s\right)\right)\end{bmatrix}$
(14)
$\boldsymbol{\chi}\left(0\right)=\begin{bmatrix}\boldsymbol{\chi}_{1}\left(0\right)\\\
\boldsymbol{\chi}_{2}\left(0\right)\end{bmatrix}=\left[{x_{0},\;y_{0},\;\gamma_{0},\;x_{f},\;y_{f},\;\gamma_{f}+2k\pi}\right]^{T}$
(15) $-\kappa_{m}\leq u_{1},\;u_{2}\leq\kappa_{m}$ (16)
$m(\boldsymbol{\chi}(1))\equiv\boldsymbol{\chi}_{1}\left(1\right)-\boldsymbol{\chi}_{2}\left(1\right)=\boldsymbol{0}$
(17)

where $\Phi(\boldsymbol{\chi}_{1},\boldsymbol{\chi}_{2})$ in Eq. (13) is the
extension of the cost function in Eq. (9) to the domain $\mathbb{R}^{6}$ and
$m:\mathbb{R}^{6}\mapsto\mathbb{R}^{3}$ is the terminal state constraint
function. Similar to the definition of [P0], $\Phi$ is assumed to have
constant gradient in $\mathbb{R}^{6}$, nonzero on the manifold defined by Eq.
(17). The rationale behind such augmentation is to enable the consideration of
cost functions applied at an arbitrary time $T$, not only at the terminal time
$1$. Among the concatenated components, the problem [$\textbf{P2}_{k}$] part
is regarded as an orientation–reversed one of the original curve. It is
important to note that the invariance property of curvature under
reparametrization implies that if any curve on $[0,1]$ is in the search space
of [$\textbf{P3}_{k}$] in sense of concatenating
$\boldsymbol{\chi}_{1}([0,1])$ and $\boldsymbol{\chi}_{2}([0,1])$, then it is
contained in the search space of [$\textbf{P0}_{T,k}$] and vice versa.
Therefore, the search space of [$\textbf{P3}_{k}$] is identical with the
original OCP, [$\textbf{P0}_{T,k}$]. Such preservation of search space may not
hold if additional constraints besides curvature bound that discriminates the
orientation of the trajectory are applied.

Now, consider the dynamical system defined by Eqs. (14)$\sim$(16). Let us
denote the reachability set of such dynamical system at final time $1$ as
$\mathcal{D}_{k}(1)$. The boundary points of $\mathcal{D}_{k}(1)$ can be
obtained by Eq. (4) applied to the dynamics Eq. (14). Among the points in
$\mathcal{D}_{k}(1)$, the set of points that are on the plane defined by Eq.
(17) is $\mathcal{R}_{k}(T)$. Then the below theorem and corollary can be
proved.

######  Theorem 6.

The below sets are identical for each $k\in\mathbb{Z}$.

  * •

The set of solutions of Eq. (4) applied to obtain the boundary points of
$\mathcal{D}_{k}(1)$, such that the endpoint of the trajectory is on the plane
defined by Eq. (17)

  * •

The set of PMP sense solutions of [$\textbf{P3}_{k}$]

Proof. Let us begin by introducing the necessary conditions of optimality(PMP)
of [$\textbf{P3}_{k}$]. The costate variables are denoted by
$\boldsymbol{p}=[p_{x,1}\;p_{y,1}\;p_{\gamma,1}\;p_{x,2}\;p_{y,2}\;p_{\gamma,2}]^{T}$.
Then the necessary conditions are as follows [10]:

  1. 1.

Costate Differential

$\begin{split}&\dot{p}_{x,1}(s)=0\\\ &\dot{p}_{y,1}(s)=0\\\
&\dot{p}_{\gamma,1}(s)=T[p_{x,1}(s)\sin\gamma_{1}(s)-p_{y,1}(s)\cos\gamma_{1}(s)]\\\
&\dot{p}_{x,2}(s)=0\\\ &\dot{p}_{y,2}(s)=0\\\
&\dot{p}_{\gamma,2}(s)=-(1-T)[p_{x,2}(s)\sin\gamma_{2}(s)-p_{y,2}(s)\cos\gamma_{2}(s)]\end{split}$
(18)

  2. 2.

Pointwise Maximum Condition

$\begin{split}&T\left[p_{x,1}(s)\cos\gamma_{1}(s)+p_{y,1}(s)\sin\gamma_{1}(s)+p_{\gamma,1}(s)u_{1}^{*}(s)\right]\\\
&-(1-T)\left[p_{x,2}(s)\cos\gamma_{2}(s)+p_{y,2}(s)\sin\gamma_{2}(s)\right.\\\
&\left.+p_{\gamma,2}(s)u_{2}^{*}(s)\right]\\\
&=\max\limits_{|u_{1}|,|u_{2}|\leq\kappa_{m}}T\left[p_{x,1}(s)\cos\gamma_{1}(s)+p_{y,1}(s)\sin\gamma_{1}(s)\right.\\\
&\left.+p_{\gamma,1}(s)u_{1}(s)\right]-(1-T)\left[p_{x,2}(s)\cos\gamma_{2}(s)\right.\\\
&\left.+p_{y,2}(s)\sin\gamma_{2}(s)+p_{\gamma,2}(s)u_{2}(s)\right]\end{split}$
(19)

almost everywhere on the interval $[0,1]$.

  3. 3.

Transversality Condition

$\boldsymbol{p}(1^{-})=\lambda_{0}\nabla_{\boldsymbol{\chi}}\Phi(1)+\left[I_{3};\;-I_{3}\right]^{T}\boldsymbol{\beta}$
(20)

where $\lambda_{0}\geq 0$,
$\boldsymbol{\beta}\equiv[\beta_{x}\;\beta_{y}\;\beta_{\gamma}]^{T}\in\mathbb{R}^{3}$,
and $I_{n}$ denotes the $n\times n$ identity matrix.

The substitution
$\nabla_{\boldsymbol{\chi}}m^{T}=\left[I_{3};\;-I_{3}\right]^{T}$ was made for
the transversality condition. The solution pair of the above necessary
conditions is denoted by
$\\{\boldsymbol{\chi},\boldsymbol{u}^{*},\boldsymbol{p},\lambda_{0},\boldsymbol{\beta}\\}$.

It is evident that when the conditions of Eq. (4) are applied to obtain
$\mathcal{D}_{k}(1)$, it reduces to the analogous costate differential and
maximum condition as above. The only difference is the transversality
condition, expressed as:

$\tilde{\boldsymbol{p}}(1^{-})=\tilde{\lambda}_{0}\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)$
(21)

where
$\\{\tilde{\boldsymbol{\chi}},\tilde{\boldsymbol{u}},\tilde{\boldsymbol{p}},\tilde{\lambda}_{0}\\}$
denotes the solution pair of the conditions of Eq. (4). $\tilde{\Phi}$ denotes
the terminal cost function as a counterpart of $\Phi$, utilizing the idea of
Proposition 4 interpreting Eq. (4) as [P0].

Now suppose a fixed vector
$\tilde{\lambda}_{0}\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)$ is given, where
$\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)$ is nonzero. First suppose that
$\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)$ is in the column space of
$\left[I_{3};\;-I_{3}\right]^{T}$. If $\tilde{\lambda}_{0}\neq 0$, then
$\exists\tilde{\boldsymbol{\beta}}\neq\boldsymbol{0}$ such that
$\tilde{\lambda}_{0}\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)=\left[I_{3};\;-I_{3}\right]^{T}\tilde{\boldsymbol{\beta}}$.
Then choice of $\lambda_{0}=0$,
$\boldsymbol{\beta}=\tilde{\boldsymbol{\beta}}$,
$\boldsymbol{p}=\tilde{\boldsymbol{p}}$,
$\boldsymbol{\chi}=\tilde{\boldsymbol{\chi}}$,
$\boldsymbol{u}^{*}=\tilde{\boldsymbol{u}}$, and arbitrary $\Phi$ having
nonvanishing gradient on the manifold Eq. (17) satisfies the condition
$\tilde{\lambda}_{0}\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)=\lambda_{0}\nabla_{\boldsymbol{\chi}}\Phi(1)+\left[I_{3};\;-I_{3}\right]^{T}\boldsymbol{\beta}$.
The nontriviality condition of the pair
$\\{\boldsymbol{p},\lambda_{0},\boldsymbol{\beta}\\}$ is satisfied by
$\boldsymbol{\beta}$. If $\tilde{\lambda}_{0}=0$, then
$\tilde{\boldsymbol{p}}$ satisfies the nontriviality condition alone.
Consequently, we can set $\lambda_{0}=0$, $\boldsymbol{\beta}=\boldsymbol{0}$,
$\boldsymbol{p}=\tilde{\boldsymbol{p}}$,
$\boldsymbol{\chi}=\tilde{\boldsymbol{\chi}}$, and
$\boldsymbol{u}^{*}=\tilde{\boldsymbol{u}}$ to meet the transversality and the
nontriviality conditions. Again, $\Phi$ is considered arbitrary with nonzero
constant gradient on the manifold Eq. (17). If
$\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)$ is not in the column space of
$\left[I_{3};\;-I_{3}\right]^{T}$, then $\tilde{\Phi}(1)$ has nonvanishing
gradient on the manifold Eq. (17). Then the choice of
$\lambda_{0}=\tilde{\lambda}_{0}$, $\boldsymbol{\beta}=\boldsymbol{0}$,
$\boldsymbol{p}=\tilde{\boldsymbol{p}}$,
$\boldsymbol{\chi}=\tilde{\boldsymbol{\chi}}$,
$\boldsymbol{u}^{*}=\tilde{\boldsymbol{u}}$, and $\Phi=\tilde{\Phi}$ satisfies
the transversality condition. For this case, the nontriviality condition of
$\\{\tilde{\boldsymbol{p}},\tilde{\lambda}_{0}\\}$ implies the nontriviality
of such choice of $\\{\boldsymbol{p},\lambda_{0},\boldsymbol{\beta}\\}$.
Hence, any PMP sense solution of Eq. (4) applied to obtain
$\mathcal{D}_{k}(1)$, with its endpoint of the trajectory satisfying Eq. (17),
is also a solution of some [$\textbf{P3}_{k}$].

Conversely, for a fixed vector
$\lambda_{0}\nabla_{\boldsymbol{\chi}}\Phi(1)+\left[I_{3};\;-I_{3}\right]^{T}\boldsymbol{\beta}\neq\boldsymbol{0}$,
$\exists\\{\tilde{\boldsymbol{\chi}},\tilde{\boldsymbol{u}},\tilde{\boldsymbol{p}},\tilde{\lambda}_{0}\\}$
and $\nabla_{\boldsymbol{\chi}}\tilde{\Phi}$ such that
$\lambda_{0}\nabla_{\boldsymbol{\chi}}\Phi(1)+\left[I_{3};\;-I_{3}\right]^{T}\boldsymbol{\beta}=\tilde{\lambda}_{0}\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)$.
This is directforward from the fact that the choice of nonzero
$\nabla_{\boldsymbol{\chi}}\tilde{\Phi}$ is arbitrary. The nontriviality
condition is also directly satisfied by choosing nonzero $\tilde{\lambda}_{0}$
arbitrarily. Hence, any PMP sense solution of [$\textbf{P3}_{k}$] is also a
solution of Eq. (4) applied to obtain $\mathcal{D}_{k}(1)$. ∎

######  Corollary 7.

Any trajectory that reaches the boundary point of $\mathcal{R}_{k}(T)$ is a
PMP sense solution of [$\textbf{P3}_{k}$].

In the subsection followed, we solve the necessary conditions of
optimality(PMP) of [$\textbf{P3}_{k}$] to derive the subsequent conclusions.

###  3.4 Necessary Conditions of the Augmented Problem

By dropping the constant terms from Eq. (19), the maximum condition reduces to

$\begin{split}&p_{\gamma,1}(s)Tu_{1}^{*}(s)-p_{\gamma,2}(s)(1-T)u_{2}^{*}(s)\\\
&=\max\limits_{|u_{1}|,|u_{2}|\leq\kappa_{m}}p_{\gamma,1}(s)Tu_{1}(s)-p_{\gamma,2}(s)(1-T)u_{2}(s)\end{split}$

As the control variables are independent, it is evident that

$\begin{split}&u_{1}^{*}(s)=\begin{cases}\kappa_{m},\quad\left(p_{\gamma,1}(s)>0\right)\\\
\text{(Indetermediate)},\quad\left(p_{\gamma,1}(s)=0\right)\\\
-\kappa_{m},\quad\left(p_{\gamma,1}(s)<0\right)\end{cases}\\\
&u_{2}^{*}(s)=\begin{cases}-\kappa_{m},\quad\left(p_{\gamma,2}(s)>0\right)\\\
\text{(Indetermediate)},\quad\left(p_{\gamma,2}(s)=0\right)\\\
\kappa_{m},\quad\left(p_{\gamma,2}(s)<0\right)\end{cases}\end{split}$ (22)

Then by substituting Eq. (14) and integrating the costate differential Eq.
(18), it follows

$\begin{split}&p_{\gamma,1}(s)=Tp_{x,1}y_{1}(s)-Tp_{y,1}x_{1}(s)+C_{1}\\\
&p_{\gamma,2}(s)=(1-T)p_{x,2}y_{2}(s)-(1-T)p_{y,2}x_{2}(s)+C_{2}\end{split}$

Therefore, whenever $p_{\gamma,1}(s)$ or $p_{\gamma,2}(s)=0$, the below
equations hold respectively.

$\begin{split}&Tp_{x,1}y-Tp_{y,1}x+C_{1}=0\\\
&(1-T)p_{x,2}y-(1-T)p_{y,2}x+C_{2}=0\end{split}$ (23)

In other words, $p_{\gamma,1}(s)=0$(or $p_{\gamma,2}(s)=0$) if and only if the
location $(x_{1}(s),y_{1}(s))$(or $(x_{2}(s),y_{2}(s))$) is on the line Eq.
(23). Similar observation for the problem [P0] was made in [22].

######  Remark 8.

The costate differential condition of Eq. (18) and the maximum condition of
Eq. (22) are the same necessary conditions (3.1) and (3.2) in [22].
Consequently, the nontriviality condition of [$\textbf{P3}_{k}$] indicates
that either one of the decomposed trajectories $(x_{1},y_{1},\gamma_{1})$ or
$(x_{2},y_{2},\gamma_{2})$ meet the switching condition outlined in the
theorem from [22]. Hence, any boundary point of the set $\mathcal{R}_{k}(T)$
can be reached by a trajectory which at least one of the decomposed trajectory
is of class CSC or CCC or their subsegments. In the subsequent lemma, we
utilize such observation to construct the boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$.

As mentioned in Sec. 3.1, we are interested in the bounds applied for $x$ and
$y$, or the set $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. Hence, we can
set $\nabla\Phi=[(\cdot)\;(\cdot)\;0\;(\cdot)\;(\cdot)\;0]^{T}$ where
$(\cdot)$ are unspecified values. The third and last components are zero as we
are not considering the cost dependency on $\gamma_{1}$ and $\gamma_{2}$. It
is important to note that when there are such restrictions on $\nabla\Phi$,
the ‘converse’ part of the proof of Theorem 6 does not hold trivially.
However, the nontrivial part of the transversality condition of Eq. (20)
reduces to
$p_{\gamma,1}(1^{-})+p_{\gamma,2}(1^{-})=\beta_{\gamma}-\beta_{\gamma}=0$.
Then, for $\nabla_{\boldsymbol{\chi}}\tilde{\Phi}$ such that
$\lambda_{0}\nabla_{\boldsymbol{\chi}}\Phi(1)+\left[I_{3};\;-I_{3}\right]^{T}\boldsymbol{\beta}=\tilde{\lambda}_{0}\nabla_{\boldsymbol{\chi}}\tilde{\Phi}(1)$,
it is evident that
$\tilde{\lambda}_{0}\nabla_{\boldsymbol{\chi}}\tilde{\Phi}=[(\cdot)\;(\cdot)\;\beta_{\gamma}\;(\cdot)\;(\cdot)\;-\beta_{\gamma}]^{T}$.
Consequently, the constraint Eq. (17) ensures that
$\tilde{\lambda}_{0}\tilde{\Phi}=(\cdot)+\beta_{\gamma}\gamma_{1}-\beta_{\gamma}\gamma_{2}=(\cdot)+\beta_{\gamma}\gamma_{1}-\beta_{\gamma}\gamma_{1}=(\cdot)$.
Hence, the cost dependency on $\gamma$ is not present in $\tilde{\Phi}$ as
well and Theorem 6 still holds. In light of such observations, we first state
and prove the below Lemma 9, followed by derivation of the construction method
of $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ in Theorem 10.

######  Lemma 9.

Suppose both of the trajectories $\boldsymbol{\chi}_{1}$ and
$\boldsymbol{\chi}_{2}$ satisfy the nontriviality condition with
$p_{\gamma,i}(1)=0$, $i=1,2$. If the point
$\pi_{1}(\boldsymbol{\chi}_{1}(1))=\pi_{1}(\boldsymbol{\chi}_{2}(1))$ is on
the boundary of $\mathcal{R}_{k}^{\prime}(T)$, it can be reached by curves
belonging to the following classes: CCCCC, CSCCC, CCCSC, CSCSC, or their
subsegments.

Proof. As stated in Remark 8, both of the trajectories $\boldsymbol{\chi}_{1}$
and $\boldsymbol{\chi}_{2}$ are of classes CSC or CCC or their subsegments.
Among the possible classes, it suffices to show only the cases where
concatenation is done among CSC or CCC, with the two C’s that are being
concatenated having opposite winding directions. Otherwise, the two C’s can be
reduced to a single arc, or a single C, and the overall concatenation belongs
to the classes stated. The condition $p_{\gamma,i}(1)=0$ further implies that
the points of switching and the endpoints are on the line Eq. (23) for both
trajectories.

First, consider the case when the concatenation is done with two CCC curves.
If a CCC curve contains no cycles, then assertion (a) of lemma 1 from [22]
implies that the second and last C have same length. Then lemma 2 from [22]
implies that there exists an auxiliary trajectory that has the same endpoint
but cannot satisfy the nontriviality condition. Consequently, if the
concatenation consists of two CCC curves without a cycle, then both
trajectories will have such auxiliary trajectories. As a result, the endpoint
must lie within the interior of $\mathcal{D}_{k}(T)$, and therefore also
within the interiors of both $\mathcal{R}_{k}(T)$ and
$\mathcal{R}_{k}^{\prime}(T)$. If a CCC curve has a cycle, assertion (b) of
lemma 1 from [22] implies that the geometric coordinates of the endpoint and
the switching points coincide. This implies that the second and last C are
full cycles. Then reversing the winding direction of the last C component does
not alter the endpoints, $\pi_{1}(\boldsymbol{\chi}_{1}(1))$ or
$\pi_{1}(\boldsymbol{\chi}_{2}(1))$. Hence, the CCCCCC curve reaching the
endpoint can be reduced to a CCCCC curve.

For CSC curves, the endpoint must lie on the line containing S segment. Hence,
assertion (d) of lemma 1 from [22] implies that the last C is a cycle.
Therefore, analogous steps by reversing the winding direction of the last C
component reduces the concatenation with any CCC or CSC curve into the stated
classes. ∎

######  Theorem 10.

Any boundary point of $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ can be
reached by a curve belonging to one of the following classes: CCCCC, CSCCC,
CCCSC, CSCSC, or their subsegments.

Proof. First, suppose that the nontriviality condition is applied to both of
the concatenated trajectories. Then it suffices to show only the cases where
concatenation is done among CSC or CCC. By the argument in Lemma 9, we may
presume that $p_{\gamma,1}(1)>0$ and $p_{\gamma,2}(1)<0$ without loss of
generality. Then continuity of the adjoint variables implies that
$\exists\varepsilon>0$ such that $p_{\gamma,1}(s)>0$ and $p_{\gamma,2}(s)<0$
on $(1-\varepsilon,1]$. Therefore, $p_{\gamma,1}(s)$ and $p_{\gamma,2}(s)$
have opposite signs on $(1-\varepsilon,1]$. This implies that the substrings
on the interval $(1-\varepsilon,1]$ are both of class C, with $u_{1}$ and
$u_{2}$ same signs. Hence, the last C part of the two plane curves are of a
single arc. Therefore, the concatenated curves CCCCCC, CSCCCC, CCCCSC, and
CSCCSC can be reduced to CCCCC, CSCCC, CCCSC, and CSCSC respectively.

Before proving the remaining part, let us introduce some additional notations.
For a set $S$, let us denote the interior points of $S$ by $int(S)$ and the
boundary points by $bd(S)$. For some fixed $T$, let us denote the reachability
set for the $\boldsymbol{\chi}_{1}$ component of Eq. (14) by
$\mathcal{G}_{1}(T)$, the forward reachability set. Similarly, the
reachability set for the $\boldsymbol{\chi}_{2}$ component of Eq. (14), with
its initial condition at
$\left[{x_{f},\;y_{f},\;\gamma_{f}+2k\pi}\right]^{T}$, the backward
reachability set, is denoted as $\mathcal{G}_{2,k}(1-T)$. Similar definitions
are made for $\mathcal{G}_{1}^{\prime}(T)$ and
$\mathcal{G}_{2,k}^{\prime}(1-T)$ as well, by applying the projection map
$\pi_{1}$ to $\mathcal{G}_{1}(T)$ and $\mathcal{G}_{2,k}(1-T)$. It follows
trivially that
$\mathcal{R}_{k}(T)=\mathcal{G}_{1}(T)\cap\mathcal{G}_{2,k}(1-T)$ and
$\mathcal{R}_{k}^{\prime}(T)\subset\mathcal{G}_{1}^{\prime}(T)\cap\mathcal{G}_{2,k}^{\prime}(1-T)$.
In addition, we denote the set of points reached from the initial point by
curves of class CS or CC, or their subsegments, by
$\mathcal{G}_{1}^{\prime\prime}(T)$.

Without loss of generality, suppose some $\boldsymbol{x}\in
bd(\mathcal{R}_{k}(T))$ is in the interior of $\mathcal{G}_{2,k}(1-T)$ and
boundary of $\mathcal{G}_{1}(T)$. In other words, $\boldsymbol{x}$ is reached
by a trajectory such that the nontriviality condition is met for the
$\boldsymbol{\chi}_{1}$ part, but not necessarily on the
$\boldsymbol{\chi}_{2}$ part.

The problem of identifying the boundary of $\mathcal{G}_{1}^{\prime}(T)$ was
first solved in [5] by geometric means, that any boundary point can be reached
by curves of class CS or CC, or their subsegments. In terms of our notations,
it was proved that
$bd(\mathcal{G}_{1}^{\prime}(T))\subset\pi_{1}(\mathcal{G}_{1}^{\prime\prime}(T))$.
Analogous statement can be derived by solving the PMP conditions of OCP [P0]
by neglecting the cost dependency of $\Phi$ on $\gamma$, or with an additional
condition, $p_{\gamma}(T)=0$. Hence, if $\boldsymbol{x}$ cannot be reached by
curves of class CS or CC, or their subsegments, there exists a neighborhood
$V$ of $\boldsymbol{x}$ in $\mathcal{G}_{1}(T)$ such that the point
$\pi_{1}(\boldsymbol{x})$ is in the interior of $\pi_{1}(V)$. Then if $V$ is
small enough that $V\subset int(\mathcal{G}_{2,k}(1-T))$, it follows
$V\subset\mathcal{R}_{k}(T)$ and hence
$\pi_{1}(V)\subset\mathcal{R}_{k}^{\prime}(T)$. Therefore,
$\pi_{1}(\boldsymbol{x})\in int(\pi_{1}(V))\subset
int(\mathcal{R}_{k}^{\prime}(T))$ and it follows $\pi_{1}(\boldsymbol{x})\in
int(\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m}))$. Hence, it suffices to
consider the case when $\boldsymbol{x}$ can be reached from the initial point
by curves of class CS or CC, or their subsegments. Taking the winding
directions of C’s into consideration, there are total of four possible classes
of curves that belong to CS or CC, and seven including their subsegments. It
remains to show that if such $\pi_{1}(\boldsymbol{x})\in
bd(\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m}))$, then $\boldsymbol{x}$ can be
reached by trajectories belonging to one of the classes of CCCCC, CSCCC,
CCCSC, CSCSC, or their subsegments.

Since the variation of the boundary of $\mathcal{G}_{2,k}(1-T)$ is continuous
in $T$ [12], it is directforward that $\exists\varepsilon_{1}>0$ such that
$\boldsymbol{x}\in\mathcal{G}_{2,k}(1-T\pm\varepsilon)$ for all
$0<\varepsilon<\varepsilon_{1}$. This indicates that the point
$\boldsymbol{x}$ can also be reached by any time in
$(1-T-\varepsilon_{1},1-T+\varepsilon_{1})$ from $\mathbb{X}$.

First, assume that the points of $\pi_{1}(\mathcal{G}_{1}^{\prime\prime}(T))$
in a neighborhood of $\pi_{1}(\boldsymbol{x})$ are reached by curves belonging
to four of the classes, CC or CS. As we are assuming that the curve is not a
single C, there exist other curves that achieve the same terminal direction.
Hence, the cross–section of $\mathcal{G}_{1}(T)$ by the terminal direction of
$\boldsymbol{x}$ is not a singleton set. Instead, it is a bounded region with
boundary points reached by curves of class CSC or CCC, or their subsegments.
(e.g., see Figs. 9 and 10 in [22].) Furthermore, it should be noted that
$\pi_{1}(\boldsymbol{x})$ may or may not belong to the boundary of
$\mathcal{G}_{1}^{\prime}(T)$. However, regardless if
$\pi_{1}(\boldsymbol{x})$ is in $bd(\mathcal{G}_{1}^{\prime}(T))$ or not,
appropriate elongation or shortening of C or S component by a small length
$\varepsilon<\varepsilon_{1}$ can always perturb the points of
$\mathcal{G}_{1}^{\prime\prime}(T)$ near $\pi_{1}(\boldsymbol{x})$ to a single
direction without changing the terminal direction $\gamma_{1}(1)$, so that
$\pi_{1}(\boldsymbol{x})\in int(\mathcal{G}_{1}^{\prime}(T\pm\varepsilon))$
and $\boldsymbol{x}\in int(\mathcal{G}_{1}(T\pm\varepsilon))$. In particular,
for CS curves, elongation or shortening can be done at the S segment. For CC
curves, same amount of elongation or shortening of both C components maintain
the terminal direction while perturbing the geometric coordinate of the
terminal point. Then $\boldsymbol{x}\in int(\mathcal{G}_{1}(T\pm\varepsilon))$
implies that $\boldsymbol{x}\in int(\mathcal{R}_{k}(T\pm\varepsilon))$ because
$\boldsymbol{x}\in int(\mathcal{G}_{2,k}(1-T\mp\varepsilon))$, and hence
$\boldsymbol{x}\in int(\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m}))$.

Such assumption is not available when $\pi_{1}(\boldsymbol{x})$ is reached by
curve of class C or S. For the case when $\boldsymbol{x}$ is reached by a
single S, then the points of $\pi_{1}(\mathcal{G}_{1}^{\prime\prime}(T))$ in
the neighborhood of $\pi_{1}(\boldsymbol{x})$ are reached by CS. In other
words, the transition between two CS classes happen at
$\pi_{1}(\boldsymbol{x})$. (e.g., see Fig. 2 in [22].) Then similar to the
previous case, appropriate elongation or shortening of S component by
$\varepsilon<\varepsilon_{1}$ perturbs the points of
$\mathcal{G}_{1}^{\prime\prime}(T)$ so that $\pi_{1}(\boldsymbol{x})\in
int(\mathcal{G}_{1}^{\prime}(T\pm\varepsilon))$ and $\boldsymbol{x}\in
int(\mathcal{G}_{1}(T\pm\varepsilon))$. Hence, such $\boldsymbol{x}$ cannot
contribute in forming the boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. However, such statement is not
available when $\boldsymbol{x}$ is reached by a single C, especially when
$T<\frac{\pi}{r}$, as the transition between CS and CC classes has to be made
at $\pi_{1}(\boldsymbol{x})$. For such cases, $\boldsymbol{x}$ could
contribute in forming the boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. Then, consider a point
$\boldsymbol{x}+t(\cos\gamma_{1}(1),\sin\gamma_{1}(1),0)$ and the set
$\mathcal{G}_{2,k}(1-T-t)$. At $t=0$, the point is in the interior of
$\mathcal{G}_{2,k}(1-T)$. As $t$ increases, $\mathcal{G}_{2,k}(1-T-t)$
converges continuously to the endpoint, $(x_{f},y_{f},\gamma_{f}+2k\pi)$.
Hence, the boundary of $\mathcal{G}_{2,k}(1-T-t)$ and the point
$\boldsymbol{x}+t(\cos\gamma_{1}(1),\sin\gamma_{1}(1),0)$ must meet at some
$t>0$. Since the boundary of $\mathcal{G}_{2,k}(1-T-t)$ can be reached by
curves of class CSC or CCC, or their subsegments, concatenation with the
remaining part, which is the line
$\boldsymbol{x}+t(\cos\gamma_{1}(1),\sin\gamma_{1}(1),0)$, yields SCSC or
SCCC, or their subsegments. Consequently, concatenation with the C in
$\boldsymbol{\chi}_{1}$ part gives CSCSC or CSCCC or their subsegments. This
completes the proof. ∎ It is noteworthy that any other combinations can be
obtained with some substrings dropped from one of the classes
$\\{CCCCC,CSCCC,CCCSC,CSCSC\\}$. Hence, we treat these four classes as the
fundamental classes of curves that construct the boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. Similar to the characteristics
of reachability sets and their projections in [22] and [5],
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ may contain ‘holes’ inside. To
clarify, some of the boundaries derived from the envelopes of the fundamental
classes might refer to the inner boundary rather than the outer boundary, as
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ is not necessarily simply
connected. However, it is trivial from the definition that
$\mathcal{B}(\mathbb{X},\kappa_{m})$ and
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ are both path connected.

###  3.5 Schematic Examples

In this subsection, schematic examples of the curves of fundamental classes
are outlined. Examples of the curves of class CSCSC and CCCCC are depicted in
Fig. 2. The red arrows indicate the initial and terminal locations and
directions while the black dotted circles indicate the tangentially bounded
region due to curvature bound. The black solid line in Fig. 2 (a) represents a
curve of class CSCSC. The curve is generated by stretching the red dotted
circle to its maximum extent to reach the boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. This curve bears a striking
resemblance to the construction of an ellipse. As the curvature bound
approaches infinity, it becomes clear that there are no curvature constraints.
Consequently, the set $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ would
gradually converge into an ellipse with its foci located at the initial and
terminal points, and with a major axis length equal to the prescribed length
of the curve. Consequently, the trajectory that reaches a certain boundary
point of the ellipse becomes unique, consist of two segments connecting each
endpoints and the boundary respectively, as the unique minimum–length
trajectory connecting any two points is a straight line linking the two
points. This trajectory exhibits infinite curvature at both the initial and
terminal points, as well as at the boundary of the ellipse. Thus, we can
incorporate circles of curvature $\kappa_{m}$ at these specific points to
ensure the desired curvature constraints. These circles are represented by the
black and red dotted circles in Fig. 2 (a). Following this, by maneuvering the
red dotted circle while maintaining the trajectory’s tension (i.e., stretching
it to its maximum extent), an envelope can be formed. This envelope is
schematically illustrated in Fig. 2 using blue dotted lines. On the contrary,
the black solid line shown in Fig. 2 (b) represents a curve of class CCCCC.
This curve is formed by compressing the curve to its maximum extent. In a
similar manner, by maneuvering the red dotted circles, an envelope will be
generated. Depending on the relative length of the curve, it may establish the
inner boundary of $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$.

(a) Class CSCSC

(b) Class CCCCC

Figure 2: Schematic examples of the curves of fundamental classes

##  4 Rectangular Patch Cover and the Mesh Refinement Algorithm

Although we have revealed the four fundamental classes of curves necessary to
construct the boundary of $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$, the
challenge remains in establishing whether this boundary actually intersects
the forbidden region. In this section, we present a technique for identifying
the existence of such intersections by introducing the concept of rectangular
patches. The fundamental concept is outlined as follows.

Given the challenge of analytically determining the boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$, our approach involves
identifying a set of rectangles that collectively encompass the entire region
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. An illustrative representation
of this covering concept is depicted in Fig. 3.

Figure 3: Example of rectangular patches covering
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$

The blue dotted line represents the boundary of the set
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$, or the envelopes of the curves
of fundamental classes. Consequently, the set
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$ is entirely covered by the
yellow rectangular patches. Individually, each patch does not encompass the
entire $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$; however, when considered
collectively, the entire set of patches covers the entirety of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. The rationale behind the choice
of the geometric shapes of such patches by rectangle is as follows. Firstly,
whenever the path inequality constraint function $h(\boldsymbol{z})$ is
concave, we recognize that the feasible region is convex. Leveraging such
prior understanding, we can readily ascertain whether a rectangular patch
intersects the forbidden region by evaluating the constraint
$h(\boldsymbol{x})\geq 0$ at its vertices. Secondly, even in scenarios where
the concavity of $h(\boldsymbol{z})$ is not confirmed, it is generally more
manageable to ascertain whether a segment or a line intersects the boundary of
the forbidden region, compared to other geometric shapes. Nevertheless, making
a blanket statement about tractability is challenging due to the versatility
of the function $h(\boldsymbol{z})$, which varies depending on the specific
problem. Therefore, it is highly advisable to explore other shapes in addition
to rectangles when applying the methodology presented in this paper to
specific situations.

At the boundary points of the curve, among the black dotted circles depicted
in Fig. 2, we label the circle with a signed curvature of
$\kappa_{s}=\kappa_{m}$ as the left circle, and the circle with
$\kappa_{s}=-\kappa_{m}$ as the right circle. Throughout this paper, the $x$
and $y$ coordinates in the figures are arranged such that the $x$–axis is
parallel to the common tangent of the two left circles. The origin is
positioned so that both left circles have $y$–coordinates equal to
$r=\frac{1}{\kappa_{m}}$. The below Fig. 4 depicts the four possible scenarios
that may reach the upmost boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. Scenarios that reach the
bottommost boundary can be easily understood by flipping Fig. 4.

(a) Left–Left

(b) Left–Right

(c) Right–Left

(d) Right–Right

Figure 4: Four scenarios of reaching the upmost boundary of
$\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$

If a curve initiates from its starting point by following the left (right)
circle and proceeds to its terminal point via the left (right) circle, it is
denoted as ‘left (right) - left (right)’. The fundamental strategy for
obtaining rectangular patches is to identify bounding rectangles for each
left(right)–left(right) scenarios. In this section, for sake of brevity, we
outline a technique for identifying patches in the left–left scenario for
illustrative purposes. Nevertheless, the same approach is applicable to the
other three cases as well. Additionally, it should be noted that the
feasibility of each scenarios varies with the length of the curve, potentially
resulting in the absence of corresponding rectangular patches.

###  4.1 Upmost Bound of CSCSC Curves

We begin by obtaining the patch that includes the highest point(largest
$y$–coordinate) in $\mathcal{B}^{\prime}(\mathbb{X},\kappa_{m})$. There are
two possible scenarios in which a curve of class CSCSC may attain the maximum
$y$–coordinate. The first scenario is when the winding direction of the middle
C is opposite to that of the remaining two C’s. The second scenario is when
the winding directions of all three C’s are the same. The winding directions
of all C’s are predetermined in the remaining CCCCC, CSCCC, and CCCSC curves
and hence there is no need for such case study. For sake of brevity, we defer
the discussion of such non–CSCSC curves until Subsection 4.3.

####  4.1.1 Winding direction of the middle C is opposite

The Fig. 5 below illustrates an instance of the CSCSC curve that attains the
maximum $y$–coordinate where the winding direction of the middle C is opposite
to that of the remaining two C’s. Where $r=\frac{1}{\kappa_{m}}$, the center
of the left circle at departure is positioned at $(0,r)$. Here, $\phi_{1}$ and
$\phi_{2}$ denote the departure and arrival angles of the curve measured
counterclockwise from the $x$–axis, respectively.

(a) Local maximum case

(b) Boundary case

Figure 5: Illustration of the CSCSC curves that attain the maximum $y$
coordinate

At the local maximum case in Fig. 5 (a), symmetry indicates that the maximum
height is achieved at $\frac{x}{2}$. As a result, we can derive the following
length equation as follows:

$\begin{split}&f(h)=2\sqrt{\frac{x^{2}}{4}+(h-r)^{2}-4r^{2}}\\\
&+r\left[2\lambda+(\lambda-\phi_{1}(\mathrm{mod}\
{2\pi}))+(\lambda+\phi_{2}(\mathrm{mod}\ {2\pi}))\right]\\\
&-\ell=0\end{split}$ (24)

where $\ell$ denotes the length of the curve and
$\lambda=\sin^{-1}\left(\frac{2r}{\sqrt{\frac{x^{2}}{4}+(h-r)^{2}}}\right)+\tan^{-1}\left(\frac{2(h-r)}{x}\right)$.
The terms preceding $\ell$ represent the total length of the curve. While the
previous sections focused on curves of unit length, this section considers
curves of arbitrary length, $\ell$, for convenience. The only distinction lies
in the normalization with respect to the scale of $\ell$. The modular
operation is used to exclude the possibility of cycles that wrap around the
circles. The square root term measures the length of the S part, while the
latter term measures the C part. The length equation can be efficiently solved
using numerical methods(e.g., Newton–Raphson), typically requiring only a few
iterations. For the boundary case scenario depicted in Fig. 5 (b), the length
equation is derived by assuming that the red dotted circle is tangent to the
line containing the departure angle and the initial point. Then the solution
$h$ obtained by solving the Eq. (24) must be compared to the value at the
boundary cases, such as the one illustrated in Fig. 5 (b). This comparison
leads to the determination of the maximum $y$–coordinate, given by
$h_{max}=\max(h+r,r(1-\cos\phi_{1}),r(1-\cos\phi_{2}))$. The max operation is
to consider the cases when the concatenation point is on the first or last C,
as outlined in the proof of Theorem 10. Similar comparisons need to be
conducted when establishing the horizontal and bottommost bounds as well,
following the determination of the location of the middle C.

####  4.1.2 Winding directions of all three C’s are the same

The Fig. 6 below illustrates an instance of the CSCSC curve which attains the
maximum $y$–coordinate where the winding directions of all three C’s are the
same. The length equation in this case becomes as below:

$\begin{split}&f(h)=2\sqrt{\frac{x^{2}}{4}+(h-r)^{2}}\\\
&+r\left[(2\pi-2\lambda)+(\lambda-\phi_{1}(\mathrm{mod}\ {2\pi}))\right.\\\
&+\left.(\lambda+\phi_{2}(\mathrm{mod}\ {2\pi}))\right]-\ell=0\end{split}$
(25)

where $\ell$ denotes the length of the curve and
$\lambda=\tan^{-1}\left(\frac{2(h-r)}{x}\right)$. The length equation can be
solved by similar means with Eq. (24), followed by routine comparison with the
boundary cases as those depicted in Fig. 6 (b) and Fig. 8 (b). However, this
scenario usually does not contribute to the bound, as the height of the red
dotted circles depicted in Fig. 5 are higher compared to the ones depicted in
Fig. 6. The scenarios where such a curve can achieve the maximum height are
limited to two cases. First is where $x<4r$, and the length of the curve falls
between the black and blue lines as depicted in Fig. 7. Such situation is
known as the closed region [4]. Within this range of $x$ value and length, the
CSCSC curve in the previous scenario becomes infeasible. Detailed conditions
regarding such length and terminal position is described in [4]. As a result,
the CSCSC curve shown in Fig. 6 (a) may attain the highest $y$–coordinate
instead. In such scenarios, the CSCSC curves depicted in Fig. 5, with possible
cycles, should also be taken into consideration and compared. Second is when
the $x$–coordinate of the endpoint is small enough; so that it is close enough
to the initial point, or even behind the initial point. Such scenario is
depicted in Fig. 8.

(a) Local maximum case

(b) Boundary case

Figure 6: Illustration of the CSCSC curves that attain the maximum
$y$–coordinate. Figure 7: Illustration of a scenario where there exists
infeasible lengths.

(a) Local maximum case

(b) Boundary case

Figure 8: Illustration of a scenario where the initial point is on the right
side of the endpoint.

In the subsequent subsection, we introduce a method for determining horizontal
bounds, the maximum and minimum $x$–coordinates.

###  4.2 Horizontal Bound of CSCSC Curves

Illustrative examples CSCSC curves that achieve the minimum $x$–coordinate in
the left–left scenario are depicted in Fig. 9. Similar to the previous
subsection, such scenario is when the three C’s have same winding directions.
For sufficient lengths that such case is feasible, symmetry implies that the
curve attains its minimum $x$–coordinate where the two S’s have same slopes of
different signs, denoted by $\beta$, as depicted in Fig. 9 (a). Such condition
simplifies to $\beta=0$. It is important to note that the location of the
terminal left circle in Fig. 9 (a) is adjusted for illustrative purposes; in
reality, the two left circles and the red dotted circle must be aligned with
the $x$–axis. The remaining case is when the middle C has opposite winding
direction. For shorter lengths where the previous shape is not feasible, the
minimum $x$ coordinate is achieved by the curves shown in Fig. 9 (b) and (c).
Therefore, we can formulate a similar length equation as Eq. (24) and compare
the local and boundary cases to determine the minimum $x$–coordinate on the
middle C. It should be noted that for the curves consist of four or lesser
components as in Fig. 9 (c), the location of the middle C is uniquely
determined only by the length equation, irrelevant to the optimal condition.
Hence, only a finite number of scenarios need to be obtained and compared. In
practical applications, the scenario depicted in Fig. 9 (c) is highly
desirable than others. This is because a large distance between adjacent mesh
points implies a significant time (or any other parametrization variable
depending on the problem) difference between these points. Consequently, this
can lead to potential errors in dynamics interpolation, irrespective of any
violation of path constraints. As a result, the obtained mesh points
themselves might be inaccurate in terms of fulfilling the dynamics constraint.

(a) Local minimum case, large $\ell$, $\beta=0$

(b) Small $\ell$

(c) Small $\ell$, CCSC

(d) Boundary case

Figure 9: Illustration of the CSCSC curves that attain the optimal
$x$–coordinates

###  4.3 Bottommost Bound and the Non–CSCSC Curves

In this subsection, we present a method for determining the minimum
$y$–coordinate, or the bottommost bound. Upmost and horizontal bounds
constructed by non–CSCSC curves are discussed as well. Illustrative examples
of curves of class CSCSC that achieve minimum $y$–coordinate in the left–left
scenario is depicted in Fig. 10. A similar case study as the upmost bound
scenario is possible. Scenario where the winding directions of all three C’s
are the same is depicted in Fig. 10 (a) and (b). The other scenario is
depicted in Fig. 10 (c) and (d). For the first scenario, the length equation
is given as Eq. (25), and Eq. (24) for the second scenario. Then the lower
bound obtained by solving the Eqs. (24) and (25) must be compared to the value
at the boundary cases depicted in Fig. 10 (b) and (d).

(a) Local minimum case

(b) Boundary case

(c) Local minimum case

(d) Boundary case

Figure 10: Illustration of the CSCSC curves that attain minimum $y$–coordinate

For non–CSCSC curves, let us denote the $i$–th C component by $C_{i}$. The
concatenation point of the two trajectories from problem [P3] must exist
within the first, middle, or last C. Because the locations of the first and
last C are fixed, it suffices to determine the location of the middle C. For
the CCCCC curves, suppose the angles $\theta_{1}$ and $\theta_{2}$ in Fig. 11
are given. Then the centers of $C_{2}$ and $C_{4}$, denoted as $(x_{2},y_{2})$
and $(x_{4},y_{4})$ respectively, are determined and hence the two possible
locations of $C_{3}$ as well. Two such C’s are indicated as $C_{3}$ and
$C_{3}^{\prime}$ in Fig. 11. Let us denote its center by
$(x_{3}(\theta_{1},\theta_{2}),y_{3}(\theta_{1},\theta_{2}))$ and the boundary
points by
$(x_{3}^{\prime}(\theta_{1},\theta_{2}),y_{3}^{\prime}(\theta_{1},\theta_{2}))$
and
$(x_{3}^{\prime\prime}(\theta_{1},\theta_{2}),y_{3}^{\prime\prime}(\theta_{1},\theta_{2}))$.
Then
$(x_{3}(\theta_{1},\theta_{2}),y_{3}(\theta_{1},\theta_{2}))=\left(\frac{x_{2}+x_{4}}{2},\frac{y_{2}+y_{4}}{2}\right)\pm\boldsymbol{h}$
where $\boldsymbol{d}=(x_{4},y_{4})-(x_{2},y_{2})$,
$\boldsymbol{h}=\sqrt{4r^{2}-\left(\frac{|\boldsymbol{d}|}{2}\right)^{2}}R_{90}(\hat{\boldsymbol{d}})$,
and $R_{90}$ represents the counterclockwise $90^{\circ}$ rotation in
$\mathbb{R}^{2}$. The length of the curve can also be obtained explicitly by
similar means in the previous subsections. Then numerical techniques(e.g.,
Newton–Raphson) can be applied to find the bounds of $x_{3}$,
$x_{3}^{\prime}$, $x_{3}^{\prime\prime}$, $y_{3}$, $y_{3}^{\prime}$, and
$y_{3}^{\prime\prime}$, and hence the bound of the envelope of the middle C.
Analogous approach is available for the CSCCC and CCCSC curves as well.

Building on the previous discussions, a schematic illustration of a
rectangular patch in a left–left scenario is shown in Fig. 12. The boundary of
the rectangular patch is depicted by a yellow solid line.

Figure 11: Illustration of CCCCC curve given $\theta_{1}$ and $\theta_{2}$
Figure 12: Schematic illustration of a rectangular patch in a left–left
scenario

###  4.4 Mesh Refinement Algorithm and the Definiteness Theorem

Based on the previous discussions, the comprehensive mesh refinement algorithm
integrated with NLP solvers is outlined as below Algorithm 1.

Algorithm 1 Mesh Refinement Algorithm(NLP Solver)

1:Initial mesh points $\\{t_{j}\\}_{1:N^{(0)}}$, tolerance $\varepsilon>0$

2:for k = 1:MaxIteration do

3: Solve the discretized problem, Eq. (2), with mesh points
$\\{t_{j}\\}_{1:N^{(k-1)}}$

4: for i = 1:N-1 do

5: Obtain rectangular patches at the $i$–th mesh interval.

6: if Any rectangular patch intrudes the forbidden region more than
$\varepsilon$ then

7: Divide the mesh interval $\left[t_{i},t_{i+1}\right]$ into two equi-sized
subintervals.

8: end if

9: end for

10: Update $N^{(k)}$

11: Update mesh points, $\\{t_{j}\\}_{1:N^{(k)}}$

12: if No mesh interval is refined in current loop then

13: break

14: end if

15:end for

The process continues until a sufficient number and locations of mesh points
are obtained to ensure that the rectangular patches do not intersect the
forbidden regions. Importantly, this algorithm does not introduce a
significant computational burden that could impact real-time feasibility. The
following theorem proves that the process of mesh refinement does not continue
indefinitely for any given tolerance $\varepsilon>0$. It is important to
highlight that the assumption that the mesh points remain outside the
forbidden region is essentially a restatement of the path constraint applied
to the discretized problem, as shown in Eq. (2). In practical applications,
the forbidden region that is imposed as a path constraint should be expanded
by an amount of $\varepsilon$ compared to the original forbidden region that
is intended.

######  Theorem 11.

Suppose that the dynamics function $f$ in Eq. (1) is bounded within the mesh
intervals $[\tau_{j},\tau_{j+1}]$. If the state variables at the mesh points
remain outside of the forbidden region, then for any $\varepsilon>0$, it is
possible to achieve mesh points that ensure a violation of the forbidden
region between adjacent mesh points by an amount less than $\varepsilon$.
Moreover, such mesh points can be obtained through a finite number of
refinements.

Proof. Boundedness of $f$ implies Lipschitz continuity of the state variables.
Hence, there exists an upper bound, $V_{M}$, for the speed that the trajectory
can travel within the mesh interval. Consequently, for an arbitrary mesh
interval, if $\Updelta\tau<\frac{\varepsilon}{V_{M}}$, then the amount of
violation into the forbidden region cannot exceed
$V_{M}\cdot\Updelta\tau<\varepsilon$. Hence, the interval
$[\tau_{j},\tau_{j+1}]$ can be refined into multiple mesh intervals such that
the maximum interval size is less than $\frac{\varepsilon}{V_{M}}$. Such
condition can be achieved by at most
$N=\left\lceil\log_{2}\frac{V_{M}\Updelta\tau_{j}}{\varepsilon}\right\rceil$
number of refinements. ∎ Furthermore, integration of the mesh refinement
algorithm into iterative methods such as SCP framework in [18] and [34] is
presented in the below Algorithm 2. The refinement process can be incorporated
during the iterations while the solution is progressively converging to
optimal.

Algorithm 2 Mesh Refinement Algorithm(SCP Framework)

1:Initial mesh points $\\{t_{j}\\}_{1:N^{(0)}}$, initial guess
$\boldsymbol{z}^{(0)}$, tolerances $\varepsilon,\varepsilon_{TRC}>0$

2:for k = 1:MaxIteration do

3: Solve the convexified discrete problem using mesh points
$\\{t_{j}\\}_{1:N^{(k-1)}}$ and initial guess $\boldsymbol{z}^{(k-1)}$

4: for i = 1:$N^{(k-1)}-1$ do

5: Obtain rectangular patches at the $i$–th mesh interval.

6: if Any rectangular patch intrudes the forbidden region more than
$\varepsilon$ then

7: Divide the mesh interval $\left[t_{i},t_{i+1}\right]$ into two equi-sized
subintervals.

8: end if

9: end for

10: Update $N^{(k)}$

11: Update mesh points, $\\{t_{j}\\}_{1:N^{(k)}}$

12: Update initial guess, $\boldsymbol{z}^{(k)}$

13: if
$\left|\boldsymbol{z}^{(k)}-\boldsymbol{z}^{(k-1)}\right|<\varepsilon_{TRC}$
and No mesh interval is refined in current loop then

14: break

15: end if

16:end for

The convexified discrete problem in Algorithm 2 refers to the problem Eq. (2)
with its component functions convexified, typically through linearization
around the initial guess. Detailed convexification process in a practical
example is outlined in Appendix. A.

Upon updating the initial guess with the expanded mesh points, suitable
interpolation is necessary due to the newly appended meshes. While more
sophisticated methods may exist for general hp-adaptive mesh refinement, we
utilize straightforward linear interpolation for our purposes in trapezoidal
method. In fact, any interpolation methods are acceptable for obtaining the
updated initial guess. This is because the interpolation step is merely
reconstruction step of the initial guess, not the actual solution process.

##  5 Numerical Demonstration

Throughout this section, the numerical demonstration of the proposed mesh
refinement algorithm is presented. For an illustrative example, the dynamics
of the fixed wing unmanned aerial vehicles(UAVs) is considered, which is a
typical example of curvature bounded dynamics applied under presence of
holonomic constraints. Other practical examples include missile guidance
problems [11] and autonomous underwater vehicles [16].

###  5.1 Path Planning for Fixed–Wing UAVs with No-Fly Zones

The problem is formulated with path constraints composed of multiple bounded
regions, known as No–Fly Zones (NFZs). These regions represent areas that the
vehicle is prohibited from traversing. Similar problems have been extensively
explored in various studies, such as [31]. In the context of this problem, the
objective function is to minimize energy consumption, while circular NFZs are
considered as forbidden regions. The problem can be formulated in the
structure of Eq. (1) as follows:

minimize

$\enspace J=\int_{0}^{t_{f}}\|u(t)\|^{2}\enspace dt$ (26)

subject to

$\begin{cases}\dot{x}=V\cos\gamma\\\ \dot{y}=V\sin\gamma\\\
\dot{\gamma}=\frac{u}{V}\end{cases}$ (27)
$\begin{split}&\left[{x(0),\;y(0),\;\gamma(0),\;x(t_{f}),\;y(t_{f}),\;\gamma(t_{f})\;}\right]\\\
&=\left[{x_{0},\;y_{0},\;\gamma_{0},\;x_{f},\;y_{f},\;\gamma_{f}\;}\right]\end{split}$
(28) $-u_{max}\leq u\leq u_{max}$ (29) $(x-x_{c,i})^{2}+(y-y_{c,i})^{2}\geq
r_{i}^{2},\quad i=1,\dots,N_{NFZ}$ (30)

where $t_{f}$ is considered as a free variable. The total number of NFZs is
represented as $N_{NFZ}$, and the center and the radius of the $i$–th NFZ are
denoted as $x_{c,i}$, $y_{c,i}$, and $r_{i}$, respectively. To further
validate the real–time effectiveness of the proposed algorithm, we utilize the
SCP framework for the direct formulation of this problem. As the convex
formulation of this problem falls outside the scope of this paper, the direct
formulation of this problem within the SCP framework is provided in Appendix
A.

The numerical demonstration is conducted using Matlab 2019b on an Apple
Macbook Air with an Apple M2 processor. A state–of–the–art dual–primal
interior point algorithm is employed by calling the MOSEK solver [1] within
Matlab. The maximum number of iterations for the SCP framework is set to 100.
The initial guess for the state variables were obtained through linear
interpolation between the initial and terminal values. The initial guess for
the flight time was set at 300 seconds. Detailed information about the model
parameters is summarized in Table 1.

Table 1: Model Parameters Parameter | Value
---|---
$u_{max}$ | 100 $[m/s^{2}]$
$V$ | 300 $[m/s]$
$x_{0}$ | 0 $[km]$
$x_{f}$ | 50 $[km]$
$y_{0}$ | 0 $[km]$
$y_{f}$ | 50 $[km]$
$\gamma_{0}$ | $\frac{\pi}{4}$
$\gamma_{f}$ | $\frac{\pi}{2}$
$t_{f}$ | Free
NFZ Tolerance | 50 $[m]$
$[$NFZ 1 Location, Radius$]$ |  $[$(8, 30), 10$]$ $[km]$
$[$NFZ 2 Location, Radius$]$ |  $[$(33, 30), 12$]$ $[km]$
$[$NFZ 3 Location, Radius$]$ |  $[$(18, 7), 10$]$ $[km]$
$[$NFZ 4 Location, Radius$]$ |  $[$(41, 41), 6$]$ $[km]$
$[$NFZ 5 Location, Radius$]$ |  $[$(3, 8), 2.5$]$ $[km]$
$[$NFZ 6 Location, Radius$]$ |  $[$(35, 15), 18$]$ $[km]$

###  5.2 Results

A comparison result between the solutions obtained through the SCP framework
with and without the proposed mesh refinement algorithm is presented
throughout this subsection. The initial mesh is given by 10 equi–spaced mesh
points. It is essential to recognize that the prevailing literature on mesh
refinement algorithms, exemplified by studies such as [13] and [17], intimates
that a mere 10 mesh points may not suffice to attain precise solutions.
Nonetheless, for illustrative purposes, this analysis employs an initial mesh
of 10 equi–spaced mesh points to accentuate the significance of the proposed
algorithm. The converged trajectories obtained from the SCP framework are
depicted in Figs. 13 and 14. The rectangular patches are depicted in Fig. 15.

Figure 13: Results Without the Proposed Mesh Refinement Algorithm

Figure 14: Results With the Proposed Mesh Refinement Algorithm

In the absence of the proposed mesh refinement algorithm, it required 6
iterations for the SCP framework to converge. It is evident that the mesh
points depicted in Fig. 13 exhibits potential collision with the NFZs.
Conversely, using the proposed method, the entire SCP framework converged
within 8 iterations. Notably, a convergent mesh consisting of 29 mesh points
is attained after the 5–th iteration. The following points necessitate more
detailed explanations:

  1. 1.

At the vicinity of the NFZ boundaries, high mesh point density is often
observed, but not consistently.

  2. 2.

There are typically two rectangular patches at each mesh intervals that
contribute to forming the overall bound.

The first aspect can be attributed as follows. The proposed algorithm does not
eliminate mesh points, even when the mesh density appears sufficient to
prevent violations into forbidden regions. This cautious approach is adopted
because the removal of a mesh point requires careful consideration; it could
exacerbate interpolation errors and lead to inaccuracies in the solution.
Inadvertent elimination might even result in unintended violations into
forbidden regions. On the other hand, these concerns do not apply when adding
a mesh point, as this typically enhances the solution’s accuracy without
introducing such risks. Therefore, as the refinement process is incorporated
during the iterations when the solution is converging to the optimum, those
points that were previously in close proximity to the boundary of the
forbidden region could exhibit high mesh densities, even when it is not near
the forbidden region in the convergent solution. In this regards, the
development of more sophisticated algorithms for eliminating unnecessary mesh
points warrants further investigation in future works.

The second aspect concerns the ratio between the prescribed lengths and the
distance between the mesh intervals. The lengths of the curves were relatively
short compared to the spacing between the mesh points, typically falling
within a ratio scale of $1.00\mathrm{xx}$. Consequently, the initial and
terminal points establish the horizontal boundaries as depicted in Fig. 9 (c),
whereas the CSCSC curves define the vertical boundaries. This leads to a
situation where two CSCSC curves which determine the vertical boundaries
originate from two of the four possible scenarios of left and right
combination. As a result, only the two scenarios out of the four typically
contribute to the formation of the overall boundary.

Figure 15: Rectangular Patches

###  5.3 Analysis on Computational Time

To evaluate the viability of onboard implementation of the entire algorithm
for real–time applications, examination of computational load imposed by the
proposed algorithm is imperative. The average computational time over
iterations is detailed in Table. 2. In the table, the mesh refinement
component, corresponding to lines $3\sim 11$ in Algorithm 2, is referred to as
MR, an abbreviation for mesh refinement. The outcomes demonstrate that the
proposed algorithm does not introduce significant computational overhead that
might hinder real–time feasibility. Moreover, the inclusion of additional mesh
points did not significantly elevate the computational burden, resulting in
similar CPU times for the SCP subproblems, both with and without the mesh
refinement. Given that traditional NLP algorithms [24] require considerably
more computational cost than SCP algorithms, the proposed algorithm can be
seamlessly integrated with a range of direct method algorithms without
incurring substantial computational expenses.

Table 2: Average Computational Time Over Iterations Average Time [sec] | MR | SCP Subproblem
---|---|---
Without MR | – | 0.0011
With MR | 0.0040 | 0.0019

##  6 Conclusion

This paper introduces a novel strategy to resolve inter–sample collision
problem of 2–dimensional state constraints by dint of mesh refinement.
Compliance of the path constraint in between the mesh points is proved, as
well as the convergence of the mesh refinement process. The proposed method
can be applied to general trajectory optimization problems formulated in
direct method, independent of the solver types: NLP or convex. Through
numerical demonstration on fixed wing dynamics problem, the potential of
real–time application is confirmed as well. Throughout this development, we
propose and prove a method for constructing the reachable region by curves
with a prescribed length, curvature bound, and specified initial and terminal
locations and directions. This result expands the upon concept of the
reachability set as covered in the literature, including [22], [6], [5], and
[4], extending these concepts into their envelope. We gratefully acknowledge
Nearthlab for providing wholehearted support and assistance to this paper.

##  Appendix A Convex Formulation of the Fixed–Wing UAV Problem

The non–convex constraints in the fixed–wing UAV problem are Eqs. (27) and
(30). These constraints are casted into suitable constraints of a convex
optimization problem through linearization. Following the standard
linearization process in [20], we first linearize the nonlinear dynamics
function. The dynamics function in Eq. (27) is in control affine form of
$f(\boldsymbol{z},u)=\tilde{f}(\boldsymbol{z})+Bu$ where
$\tilde{f}(\boldsymbol{z})=[V\cos\gamma,\;V\sin\gamma,\;0]^{T}$ and
$B=\left[0,\;0,\;\frac{1}{V}\right]^{T}$ for state variable
$\boldsymbol{z}=[x,\;y,\;\gamma]^{T}$. Then for the solution pair,
$\left\\{\boldsymbol{z}^{(k)},u^{(k)}\right\\}$, obtained from the previous
$k$–th iteration, define

$A\left(\boldsymbol{z}^{(k)}\right)\equiv\left.\frac{\partial\tilde{f}}{\partial\boldsymbol{z}}\right|_{\boldsymbol{z}=\boldsymbol{z}^{(k)}}=\begin{pmatrix}0&0&-V\sin\gamma^{(k)}\\\
0&0&V\cos\gamma^{(k)}\\\ 0&0&0\end{pmatrix}$ (31)
$b\left(\boldsymbol{z}^{(k)}\right)=\tilde{f}\left(\boldsymbol{z}^{(k)}\right)-A\left(\boldsymbol{z}^{(k)}\right)\boldsymbol{z}^{(k)}$
(32)

Then the nonlinear dynamics is linearized as below:

$f(\boldsymbol{z},u)\approx
A\left(\boldsymbol{z}^{(k)}\right)\boldsymbol{z}+Bu+b\left(\boldsymbol{z}^{(k)}\right)$
(33)

The nonconvex path inequality constriant, Eq. (29), is linearized as below:

$\begin{split}&2\left(x^{(k)}-x_{c,i}\right)\left(x-x^{(k)}\right)+2\left(y^{(k)}-y_{c,i}\right)\left(y-y^{(k)}\right)\\\
&-r_{i}^{2}\geq 0,\quad i=1,\dots,N_{NFZ}\end{split}$ (34)

After such linearization, the trust region constraint(TRC) is added to prevent
unboundedness of the convexified problem as below [20] :

$|\boldsymbol{z}-\boldsymbol{z}^{(k)}|\leq\varepsilon_{TRC}$ (35)

where $\varepsilon_{TRC}\in\mathbb{R}^{3}$ is a constant representing
termination criterion. The inequality is applied componentwise.

To summarize, the optimal control problem described by Eqs. (26) $\sim$ (30)
is linearized through two steps. Firstly, the nonlinear functions given by
Eqs. (27) and (30) are replaced with their linearized counterparts, as shown
in Eqs. (33) and (34). Secondly, the trust region constraint, as represented
by Eq. (35), is introduced. Then the subsequent discretization process is as
described in Sec. 2.

## References

  * [1] Erling Andersen and Knud Andersen.  The mosek interior point optimizer for linear programming: An implementation of the homogeneous algorithm.  33, 01 1999.
  * [2] Behçet Açıkmeşe and Lars Blackmore.  Lossless convexification of a class of optimal control problems with non-convex control constraints.  Automatica, 47(2):341–347, 2011.
  * [3] Juho Bae, Sang-Don Lee, Young-Won Kim, Chang-Hun Lee, and Sung-Yug Lim.  Convex optimization-based entry guidance for spaceplane.  International Journal of Control, Automation and Systems, 2022.
  * [4] Zheng Chen, Kun Wang, and Heng Shi.  Elongation of curvature-bounded path.  Automatica, 151:110936, 2023.
  * [5] E. J. Cockayne and G. W. C. Hall.  Plane motion of a particle subject to curvature constraints.  SIAM Journal on Control, 13(1):197–220, 1975.
  * [6] L. E. Dubins.  On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents.  American Journal of Mathematics, 79(3):497–516, 1957.
  * [7] Daniel Dueri, Yuanqi Mao, Zohaib Mian, Jerry Ding, and Behçet Açıkmeşe.  Trajectory optimization with inter-sample obstacle avoidance via successive convexification.  pages 1150–1156, 12 2017.
  * [8] Divya Garg, Michael Patterson, William W. Hager, Anil V. Rao, David A. Benson, and Geoffrey T. Huntington.  A unified framework for the numerical solution of optimal control problems using pseudospectral methods.  Automatica, 46(11):1843–1851, 2010.
  * [9] Mikhail Gusev and Igor Zykov.  On extremal properties of boundary points of reachable sets for a system with integrally constrained control.  IFAC-PapersOnLine, 50:4082–4087, 07 2017.
  * [10] Richard F. Hartl, Suresh P. Sethi, and Raymond G. Vickson.  A survey of the maximum principles for optimal control problems with state constraints.  SIAM Review, 37(2):181–218, 1995.
  * [11] Chang-Hun Lee, Tae-Hun Kim, Min-Jea Tahk, and Ick-Ho Whang.  Polynomial guidance laws considering terminal impact angle and acceleration constraints.  IEEE Transactions on Aerospace and Electronic Systems, 49(1):74–92, 2013.
  * [12] Ernest Bruce Lee and Lawrence Markus.  Foundations of optimal control theory.  1967\.
  * [13] Fengjin Liu, William W. Hager, and Anil V. Rao.  Adaptive mesh refinement method for optimal control using nonsmoothness detection and mesh size reduction.  Journal of the Franklin Institute, 352(10):4081–4106, 2015.
  * [14] Fengjin Liu, William W. Hager, and Anil V. Rao.  Adaptive mesh refinement method for optimal control using decay rates of legendre polynomial coefficients.  IEEE Transactions on Control Systems Technology, 26(4):1475–1483, 2018.
  * [15] Xinfu Liu, Zuojun Shen, and Ping Lu.  Entry trajectory optimization by second-order cone programming.  Journal of Guidance, Control, and Dynamics, 39(2):227–241, 2016\.
  * [16] Yanji Liu, Jie Ma, Ning Ma, and Guichen Zhang.  Path planning for underwater glider under control constraint.  Advances in Mechanical Engineering, 9(8):1687814017717187, 2017\.
  * [17] Lin Ma, Zhijiang Shao, Weifeng Chen, and Zhengyu Song.  Trajectory optimization for lunar soft landing with a hamiltonian-based adaptive mesh refinement strategy.  Advances in Engineering Software, 100:266–276, 2016.
  * [18] Danylo Malyuta, Taylor Reynolds, Michael Szmuk, Thomas Lew, Riccardo Bonalli, Marco Pavone, and Behçet Açıkmeşe.  Convex optimization for trajectory generation : A tutorial on generating dynamically feasible trajectories reliably and efficiently.  IEEE Control Systems, 42(5):40–113, 10 2022.
  * [19] Yuanqi Mao, Michael Szmuk, and Behçet Açıkmeşe.  Successive convexification: A superlinearly convergent algorithm for non-convex optimal control problems.  04 2018.
  * [20] Skye Mceowen, Abhinav G Kamath, Purnanand Elango, Taewan Kim, Samuel C Buckner, and Behcet Acikmese.  High-accuracy 3-dof hypersonic reentry guidance via sequential convex programming.  In AIAA SCITECH 2023 Forum, page 0300, 2023.
  * [21] Young-Jae Oh, Heekun Roh, and Min-Jea Tahk.  Fast trajectory optimization using sequential convex programming with no-fly zone constraints.  IFAC-PapersOnLine, 52(12):298–303, 2019.  21st IFAC Symposium on Automatic Control in Aerospace ACA 2019.
  * [22] Valerii Patsko, S.G. Pyatko, and Andrey Fedotov.  Three-dimensional reachability set for a nonlinear control system.  Journal of Computer and Systems Sciences International, 42, 05 2003\.
  * [23] Michael A Patterson, William W Hager, and Anil V Rao.  A ph mesh refinement method for optimal control.  Optimal Control Applications and Methods, 36(4):398–421, 2015.
  * [24] Michael A. Patterson and Anil V. Rao.  Gpops-ii: A matlab software for solving multiple-phase optimal control problems using hp-adaptive gaussian quadrature collocation methods and sparse nonlinear programming.  41(1), oct 2014.
  * [25] Marco Sagliano.  Pseudospectral convex optimization for powered descent and landing.  Journal of Guidance, Control, and Dynamics, 41(2):320–334, 2018\.
  * [26] Marco Sagliano.  Generalized hp pseudospectral-convex programming for powered descent and landing.  Journal of Guidance, Control, and Dynamics, 42(7):1562–1570, 2019\.
  * [27] Marco Sagliano, Erwin Mooij, and Stephan Theil.  Onboard trajectory generation for entry vehicles via adaptive multivariate pseudospectral interpolation.  Journal of Guidance, Control, and Dynamics, 40(2):466–476, 2016\.
  * [28] Michael Szmuk, Utku Eren, and Behcet Acikmese.  Successive convexification for mars 6-dof powered descent landing guidance.  In AIAA Guidance, Navigation, and Control Conference, page 1500, 01 2017.
  * [29] Michael Szmuk, Taylor Reynolds, and Behçet Açıkmeşe.  Successive convexification for real-time six-degree-of-freedom powered descent guidance with state-triggered constraints.  Journal of Guidance, Control, and Dynamics, 43(8):1399–1413, 06 2020.
  * [30] Jesus Tordesillas, Brett T. Lopez, and Jonathan P. How.  Faster: Fast and safe trajectory planner for flights in unknown environments.  In 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 1934–1940, 2019.
  * [31] Xiaoliang Wang, Peng Jiang, Deshi Li, and Tao Sun.  Curvature continuous and bounded path planning for fixed-wing uavs.  Sensors, 17(9), 2017.
  * [32] Zhenbo Wang and Michael J. Grant.  Constrained trajectory optimization for planetary entry via sequential convex programming.  Journal of Guidance, Control, and Dynamics, 40(10):2603–2615, 2017\.
  * [33] Zhenbo Wang and Ye Lu.  Improved sequential convex programming algorithms for entry trajectory optimization.  Journal of Spacecraft and Rockets, 57(6):1373–1386, 2020.
  * [34] Xiang Zhou, Rui-Zhi He, Hong-Bo Zhang, Guo-Jian Tang, and Wei-Min Bao.  Sequential convex programming method using adaptive mesh refinement for entry trajectory planning problem.  Aerospace Science and Technology, 109:106374, 2021.