applied

sciences
Article
Efficient Optimization-Based Trajectory Planning for
Truck–Trailer Systems
Stepan Ozana , Filip Krupa and Zdenek Slanina *

Faculty of Electrical Engineering and Computer Science, VSB–Technical University of Ostrava,

708 00 Ostrava, Czech Republic; [email protected] (S.O.); [email protected] (F.K.)
* Correspondence: [email protected]

Abstract: This paper tackles the complex problem of trajectory planning for trucks with multiple
trailers, with a specific focus on autonomous parking assistance applications. These systems aim
to autonomously guide vehicles from a starting position to a target location while effectively navi-
gating real-world obstacles. We propose a novel six-phase approach that combines global and local
optimization techniques, enabling the efficient and accurate generation of reference trajectories. Our
method is validated in a case study involving a truck with two trailers, illustrating its capability to
handle intricate parking scenarios requiring precise obstacle avoidance and high maneuverability.
Results demonstrate that the proposed strategy significantly improves trajectory planning efficiency
and robustness in challenging environments.

Keywords: assisted parking; autonomous vehicles; trajectory planning; path planning; truck-trailer
systems; articulated vehicles

1. Introduction
Nowadays, we have been experiencing significant development in the field of au-
tonomous driving. From the point of view of the general public, this is a hot topic that has
only started to come to the forefront of the automotive industry.
From the point of view of the scientific community, however, it is a well-established
Citation: Ozana, S.; Krupa, F.; issue that has been addressed by scientific teams around the world for more than a decade.
Slanina, Z. Efficient Optimization-
This article focuses on assisted parking for a system consisting of a main vehicle (truck)
Based Trajectory Planning for
and articulated links (trailers).
Truck–Trailer Systems. Appl. Sci. 2024,
In particular, finding collision-free trajectories is a key element for such types of
14, 11675. https://doi.org/10.3390/
problems. The most commonly used methods and procedures for setting up the problem
app142411675
of path planning and trajectory planning are briefly summarized.
Academic Editor: Suchao Xie The model of the system plays a crucial role here, which is referred to as model-
based design. In case of truck–trailer modeling, models can be divided into kinematic
Received: 14 November 2024
Revised: 10 December 2024
and dynamic models. Kinematic models are especially useful for slow movements where
Accepted: 11 December 2024
the dynamic forces can be neglected. Such models are suitable for maneuvering between
Published: 13 December 2024 obstacles at low speeds.
Dynamic force-based models are only applicable when it is necessary to model a
fast-moving car, where movement outside the direction of rotation of the wheels may
occur, especially when taking curves. The higher complexity of these models is their
Copyright: © 2024 by the authors. main disadvantage, leading kinematic models to be preferred in most cases, including in
Licensee MDPI, Basel, Switzerland. this paper.
This article is an open access article
distributed under the terms and 1.1. Problem of Path Planning
conditions of the Creative Commons
Path planning is the process of finding a sequence of points between a start and
Attribution (CC BY) license (https://
a destination in a coordinate system. These points then represent the path the vehicle
creativecommons.org/licenses/by/
4.0/).

Appl. Sci. 2024, 14, 11675. https://doi.org/10.3390/app142411675 https://www.mdpi.com/journal/applsci

Appl. Sci. 2024, 14, 11675 2 of 23

must follow. Finding such a path usually falls into the problem of finding a global min-
imum. The usual requirement for the found path is its minimum length, but this can be
customized accordingly.
The first possible approach to path planning is to search for the shortest path in the
graph. This approach is often used when the user requires fast path planning and is trying
to ensure that the resulting vehicle control can be implemented in real time. The most
commonly used algorithm is A∗ , along with its modified variants such as [1] which focus on
improving smoothness and shortening the found path; for examples of this algorithm’s use,
see [2–4]. A very similar principle has been used by some authors for their own specialized
path-finding algorithms; for example, in [5] the authors split the path planning for the
vehicle exiting the parking space into four parts.
The other option for path planning is to use the Rapidly-Exploring Random Trees (RRT)
algorithm. This algorithm uses a random tree to explore an unknown environment and find
a path from a starting point to an ending point. The disadvantage of this algorithm, among
other things, is that the resulting path may not be optimal. In the case of path planning
for vehicles, authors have often resorted to various modifications of this algorithm, for
example [6]. Perhaps the most well-known modification of the RRT is the variant called
Closed-Loop RRT. This algorithm was developed as part of the DARPA Urban Challenge;
its main contribution is its use of an internal stabilizing controller. In this approach, the
RRT algorithm looks for a path that produces a stable system, even though the vehicle
system itself is often unstable. This makes it possible to reduce the number of states of the
whole system and extend the distances between each point of the tree, allowing the whole
computation to be accelerated [7].
Another possibility is to make the path planning problem into an NLP (nonlinear
programming) problem. By finding the minimum of a suitably specified problem, a path
between two points can be found [8]. The disadvantage of this approach is the need to use
a global solver for the optimization problem; otherwise, it is very likely to find only a local
minimum. Thus, in the case of path planning, we may find a link between two points, but
not necessarily the shortest one. Another problem is the time-consuming nature of such
procedures, which is why this approach has not been used recently.
A very common way to find a path is to use a ready-made software package that
contains one or a combination of the above-described procedures. This approach has the
advantage of allowing the author to concentrate on other aspects of vehicle control. The
most widely used such software appears to be the Robot Operating System (ROS). This
operating system includes a global path-finding package, which primarily uses the A∗
algorithm to find the path, but also relies on others. This approach was used in [9–11].

1.2. Problem of Trajectory Planning

By trajectory planning, we mean the process of finding control inputs that cause
the states to change from initial to target states when applied to a truck-trailer system.
The initial or target value of the state variables may be specified with small hysteresis to
ensure feasibility.
It is common to use an optimization problem for trajectory planning. The published
procedures can be further divided into two parts. Several authors have searched for
trajectories based on an already-found path. Another group of authors combines the path
planning and trajectory problems; instead of finding a path, this approach tries to find
trajectories that are suitable for vehicle movement.
A major problem in trajectory planning is the requirement that the the found trajectory
be both continuous and feasible. This problem can be easily solved using curvilinear
coordinates [12], and similar approaches have been taken using spline curves [13,14].

1.3. Path-Based Trajectory Search

Trajectory search based on a pre-existing path is most often used when the goal is
to achieve a real-time implementation. This approach allows for the use of a local NLP
Appl. Sci. 2024, 14, 11675 3 of 23

solver. Using a local solver provides faster solution-finding than a global solver. In the
case of trajectories, additional requirements arise: the usual goal of moving a vehicle from
location A to location B is complemented by the requirement of admissible trajectories
for the system; furthermore, the vehicle following the found trajectories must avoid all
obstacles. In the case of a truck with trailers, it is necessary to avoid the problem of the
vehicle hitting its own trailer (jackknife effect).
Existing papers have presented a large variety of solutions, with each team of authors
approaching the problem individually; thus, it is not possible to clearly identify any one
solution as the best. For example, in [2,3] the authors decided to split the solution into
several smaller optimization problems and link them together such that the trajectories
found in the previous problem could be used as the initial estimate of the next problem.
This approach allows for finding initial trajectories that do not respect collisions with
obstacles, which can then be recomputed in the next optimization problem, which takes
collisions with obstacles into account. If optimal trajectories still cannot be found, the next
step is to change the size of the obstacles and run the optimization problem in a cycle until
the desired collision-free trajectories are found.

1.4. Finding Trajectories Without a Known Path

Finding trajectories without an a priori known path implies the use of global optimiza-
tion. This global optimization problem has the advantage of not needing an initial guess;
however, a major disadvantage is that it relies on the use of stochastic algorithms. These are
mainly a group of rapidly expanding biologically-inspired optimization algorithms [15–17].
However, there are a variety of other stochastic algorithms [18,19]. The use of these algo-
rithms can lead to very interesting solutions where solving a single optimization problem
ensures that suitable trajectories are found.
In [20], the authors presented a method for finding trajectories using a stochastic
algorithm (SFS, Stochastic Fractal Search) within a single optimization problem. The
authors themselves pointed out that such trajectories are not necessarily optimal, but they
are very close to optimal.
Another disadvantage, probably the biggest from the applied point of view, is the time
needed to find such a solution; in the case of finding a global minimum without an initial
estimate, it is not currently possible to talk about real-time deployment.

1.5. Selecting Approaches and Solvers for Optimization Problems

Solving the optimal control problem is a key aspect of trajectory planning. Due to the
nonlinear nature of trajectory planning, linearization can be performed at one or multiple
operational points, as demonstrated by the authors of [8,9,11,21]. However, if the problem
is to be tackled using nonlinear methods, numerical approaches become a reasonable choice,
as they can address a broader range of optimization problems. A widely adopted technique
is to transform the optimal control problem into a nonlinear programming problem, then
leverage a ready-to-use solver.
Many researchers have utilized MATLAB’s built-in functions such as f mincon (or
f minunc) [22], which are highly efficient solvers incorporating numerous advancements
from prior studies such as [23,24]. These solvers are particularly suitable for simulation-
based tasks or early design phases prior to implementation.
When transitioning to real-time algorithm deployment, additional requirements
for solvers must be considered, particularly compatibility with the target operating
system (Linux being a preferred choice), availability on Linux platforms, and integration
capability with supervised control systems. Such implementations frequently rely on
programming languages such as Python, C/C++, Julia, or FORTRAN. For both trajectory
planning and broader nonlinear optimization problems, gradient-free solvers are often
essential in practice. This requirement significantly narrows the pool of potential solvers,
especially when there is a need to ensure smooth transition from offline simulations to
real-time applications.
Appl. Sci. 2024, 14, 11675 4 of 23

In our proposed approach, we select the f mincon solver due to its efficiency and
robust performance. Additionally, we tested the SOLNP solver [25], which is gradient-free
and supports multiple interfaces, including MATLAB, Python, and C++. The SOLNP
solver demonstrated excellent performance and robustness on our benchmarks, which
included nonlinear optimization tasks such as trajectory planning. Real-time operation
was successfully tested on a Raspberry Pi 4. However, all tests of the SOLNP solver were
conducted as part of preparations for future work, and are beyond the scope of this paper.

2. Methods
2.1. Problem Formulation
The problem involved in this paper is considered and demonstrated as a case study
focused on trajectory planning for a system of a truck and two trailers that moves forward
between a given initial position of the system and given target rectangular area while
respecting additional constraints. Such a problem can be referred to as an assisted parking
problem, as the movement starts and ends with zero velocity.
There are ten static obstacles within the area where the system is supposed to move.
The situation is depicted in Figure 1, showing the given 2D region, where the black cross
on the left determines the given initial position of the system (represented by reference
point R0 of the truck) and the red rectangle determines the target area to fit the truck-trailer
system at the end of the parking process.
In addition to presenting our solution to this problem, we also compare our solution
to the case study presented in [20], where the authors made use of a global stochastic
optimizer (SFS, Stochastic Fractal Search) to find optimal trajectories. Comparison with this
previous work is possible thanks to the fact that the authors provided their source code,
including comments and a detailed description of the functionality of their entire solution.

Figure 1. Graphical representation of the problem formulation, taken from [20].

2.2. Mathematical Model

In order to obtain a nonlinear state model, we consider the model of a truck with two
trailers depicted in Figure 2 and the choice of state variables according to Equation (1):

x1 X0
   
 x2   Y0 
   
 x3   ϕ0 
 x4  ≡  θ0 
⃗x =  (1)
  
   
 x5   ϕ1 
x6 ϕ2
where:
• X0 , Y0 refer to the x- and y-coordinates of the truck’s reference point R0 [m].
Appl. Sci. 2024, 14, 11675 5 of 23

• ϕ0 . . . is the angle of rotation of the wheels relative to the axis of the truck [rad].
• ϕ1 . . . is the angle between the axis of the truck and the axis of the first trailer [rad].
• ϕ2 . . . is the angle between the axis of the first trailer and the axis of the second
trailer [rad].
• θ0 . . . is the angle of rotation of the truck relative to the x-axis [rad].
Then, the mathematical model of such a system can be represented by a nonlinear
state model according to the set of differential equations provided below in Equation (2):

Figure 2. System of a truck and two trailers.

ẋ1 = u1 · cos( x4 )
ẋ2 = u1 · sin( x4 )
1 1
ẋ3 = − · x3 + · u2
τ τ
u
ẋ4 = 1 · tan( x3 )
d
 0  (2)
u u a
ẋ5 = 1 · tan( x3 ) − 1 · sin( x5 ) − · cos( x5 ) · tan( x3 )
d0 d1 d0
 
u1 a
ẋ6 = · sin( x5 ) − · cos( x5 ) · tan( x3 )
d1 d0
 
u1 a
− · sin( x6 ) · cos( x5 ) + · sin( x5 ) · tan( x3 )
d2 d0
where:
• u1 is the first system’s input, i.e., the velocity of the truck v0 [m · s−1 ].
• u2 is the second system’s input, i.e., the setpoint for the front wheel steering angle
ϕ0 [rad].
• d0 determines the distance of points | R0 S|, representing the distance between the axles
of the truck [m].
• a determines the distance between the points | R0 Q| of the rotary joint Q and the
reference point of the truck R0 [m].
• d1 determines the distance of the points | QR1 | representing the first trailer’s reference
point R1 and the rotary joint of the truck Q [m].
• d2 determines the distance of points | R1 R2 | representing the distance of reference
points of two consecutive trailers [m].
• τ is the front wheel steering servo time constant, which approximates the dynamics of
a first-order system between the steering setpoint and the actual steering [s].

2.3. Proposed Solution

The proposed solution suggests a six-phase procedure combining global optimization
for path planning (Phase 1) and local optimization for trajectory planning (Phases 2–6).
The problem of trajectory planning considered in this paper can be formulated as
continuous optimal control problem (OCP) in Bolza form; see Equation (3):
Appl. Sci. 2024, 14, 11675 6 of 23

Z tF
J = Φ[t0 , t F , x(t0 ), x(t F )] + L[ x(t), u(t), t]dt. (3)
t0

The original OCP is converted to a general nonlinear programming problem (NLP) in

the form of Equation (4), described as finding an optimal solution z∗ which minimizes the
cost function J depending on the decision vector variable z which contains both control
inputs and states. We find its optimal value, provided as Equation (4):

z∗ = arg min{ J (z)}. (4)

z

The optimization task includes the nonlinear equality constraints in Equation (5), the
nonlinear inequality constraints in Equation (6), and the simple lower and upper bounds in
Equation (7):
ceq (z) = 0 (5)
c(z) ≤ 0 (6)
zl ≤ z ≤ zu . (7)
The set of equality constraints ceq corresponding to Equation (5) is composed of
equality boundary constraints ψ and equality differential defect constraints ξ, as follows:
 
ψ
ceq = . (8)
ξ

Converting the OCP into an NLP is achieved through a direct transcription discretiza-
tion scheme using the fourth-order Runge–Kutta numerical method (RK4) in the form
of Equation (9). To remain consistent with notation in Equations (5) and (8), we express
Equation (9) as the set of differential defect constraints provided in Equation (10), where
k ∈ [0, N − 2] ∈ N0 stands for the current step of the computation, N is the number of nodes,
k1 , k2 , k3 , and k4 are determined by Equation (11), and h represents the sampling period.

1
x k +1 = x k + · h ( k1 + 2 · k2 + 2 · k3 + k4 ) (9)
6

1
ξ k = x k +1 − x k − · h · ( k1 + 2 · k2 + 2 · k3 + k4 ) (10)
6

k1 = f ( x k , u k )
h u + uk
k2 = f ( x k + · k 1 , k +1 )
2 2 (11)
h u + uk
k 3 = f ( x k + · k 2 , k +1 )
2 2
k 4 = f ( x k + h · k 3 , u k +1 )
Altogether, the solution of both path planning and trajectory planning that we propose
in this paper consists of the six phases introduced in Table 1, which are each described in
detail in separate subsections.

Table 1. The six phases of the proposed solution.

Phase Phase Identification

1 Path Planning
2 Trajectory Planning with Constant Velocity Profile
3 Velocity Profile Planning
4 Truck Dimensions and Collision Checking
5 Adding Trailers
6 Fine-Tuning the Trajectories
Appl. Sci. 2024, 14, 11675 7 of 23

2.3.1. Phase 1: Path Planning

Dijkstra’s algorithm according to [26] is used to find the shortest path. The input to
this algorithm is the matrix PEN containing the transition values between individual
 nodes.
The density of the grid created by nodes can be expressed by parameter k g k g ∈ N , which
indicates the number of nodes per unit of length.
Supposing N nodes, the PEN matrix contains N × N elements. As the size N of the
square matrix increases, the number of elements grows quadratically, leading to a rapid
increase in memory or storage requirements which heavily affects the computational load.
If no transition between nodes is possible, then the transition value is set to infinity.
The numbers of nodes denoting the initial and target points of the path are also used as
inputs to the algorithm. The output of the algorithm is a vector containing the numbers of
individual points of the shortest found path. This vector then needs to be decoded into the
format of the coordinates of individual points, leading to the next part of the solution, in
−
→
which we obtain the vector Cx containing the x-coordinates of the found points and vector
−
→
Cy containing their y-axis counterparts.
Prior to carrying out the path search, it is necessary to expand the obstacles and define
the individual nodes. The parameter k g is chosen as k g = 1, indicating one node per meter.
A graphical representation of the considered problem is shown in Figure 3, where the
black cross on the left represents an initial position of R0 and the black cross on the right
represents the target position. Note that the target area is no longer a rectangular shape,
but instead a reference point R0 at the appropriate location in accordance with Figure 1.
Note that due to simplicity of processing of the weighted graph, the entire grid is shifted in
both directions compared to Figure 1.

Figure 3. Node graph with initial and final states.

2.3.2. Phase 2: Trajectory Planning with Constant Velocity Profile

The problem of finding a trajectory based on a path supposes a known sequence of
points that need to be passed through on the way from the start to the target. The searched
result is the vector of control inputs to be applied to the system. To find the velocity, we
need to know the total time needed to reach the destination, which is currently unknown.
Therefore, the goal of this phase of the trajectory planning is not to find the speed profile
yet, but to find the total time required to realize the trajectory. The velocity profile is found
in the subsequently Phase 3. In Phase 2, we search for trajectories with a constant velocity
profile. In this phase, there is no need to consider vehicle dimensions or collisions with
obstacles; therefore the model solution for this phase only considers the truck, and uses the
first four differential equations in Equation (2), as referenced later on by Equation (18).
Such a simplification is sufficient for the first design of the trajectories, which can
serve as a basis for further optimization runs when we have a more complex model of
the system. The described truck model has only four states. In this case, for the purpose
of incorporating time optimization it is necessary to add a fifth state x T , representing the
Appl. Sci. 2024, 14, 11675 8 of 23

total time needed to realize the desired trajectory. We first select an appropriate constant
velocity profile u1 for use in Phase 2. This selection adheres to widely accepted kinematic
assumptions. Linear velocities below 1–2 m/s (approximately 3.6–7.2 km/h) are generally
considered suitable for ensuring minimal dynamic forces such as centripetal acceleration.
For urban-like scenarios or controlled environments (e.g., robotics or autonomous systems),
a maximum linear velocity of 0.5–1.5 m/s is often applied. In tighter spaces such as parking
or obstacle avoidance, the lower end of this range (0.5–1 m/s) is preferred. Based on these
considerations, we set u1 = 1. The total time x T is determined through an optimization
process representing the time required to travel from the initial to the target position. The
total travel distance depends on the path determined in Phase 1 while respecting the
vehicle’s dynamics. Using the constant velocity profile, the travel distance can be expressed
as u1 · x T .
The total time x T is subsequently utilized in Phase 3, where the velocity profile is
refined under the constraints of zero initial and target velocities.
To find the total time x T , the simulation step size h corresponding to the sampling
period Ts is supposed to be unitary and to be scaled by the state x T , which is part of
decision vector of variables in this phase. The resulting dynamics equations are written in
Equation (12).

ẋ1 = x T · u1 · cos( x4 )
ẋ2 = x T · u1 · sin( x4 )
1 1
ẋ3 = x T · (− · x3 + · u2 ) (12)
τ τ
u1
ẋ4 = x T · · tan( x3 )
d0
ẋ T = 0
Fulfillment of the initial state can be ensured by a simple equality boundary condition,
where the corresponding elements of the decision vector ar be equal to the given initial
state −
→
x0 .
   
x 11 x 01
 x2   x0 
 x3  −  x0 
ψ= 1  2 (13)
1 3
x 41 x 04
Satisfying the desired final state can also be ensured by similar equality constraints;
however, much less restrictive inequality constraints are used here due to the need to fit the
system into a predefined target area. This allows for a certain level of relaxation, and signif-
icantly increases the chance of finding an admissible solution to the optimization problem.
The target area has a rectangular shape containing the truck’s target reference point
R0 , with dimensions denoted by xtgt and ytgt . These dimensions are appropriately chosen
such that the entire system can be placed inside without colliding with obstacles. Therefore,
this approach leads to the introduction of the two inequality conditions expressed by
Equation (14). These constraints represent the positional limitation of the truck’s reference
point in the last time step of the searched trajectories with respect to the desired outcome.
Here, the vector − x→
t f is a vector of state variables indicating the desired value at the end of
the procedure.
xtgt xtgt
xt f 1 − ≤z3N ≤ xt f 1 +
2 2 (14)
ytgt ytgt
xt f 2 − ≤z4N ≤ xt f 2 +
2 2
The cost function used here is defined by Equation (15). This function penalizes the
distance between truck’s reference point R0 and the corresponding path point, and also
minimizes the total time. Numerical time integration is computed using the trapezoidal
Appl. Sci. 2024, 14, 11675 9 of 23

−
→
method. The variables Cxn and Cyn correspond to the coordinates of the found path Cx and
−
→
Cy , respectively, as described in Section 2.3.1.
!
N q N −1
1 1 2 1 2
J = 50 · ∑ ( x1n − Cxn ) + x2n − Cyn +
2 T1 n∑
2
2 2
· x + x Tn + x TN (15)
n =1
N =2 2

If a solution is successfully found, it is necessary to extract individual control inputs

and state variables from decision vector. The state variable − x→
T becomes constant, i.e.,
−
→
x T ≡ x T , and represents the total travel time of the system. For further steps, it is neces-
sary to determine the sampling period from these data, which is computed according to
Equation (16).
xT
Ts = (16)
N−1
Simple bounds corresponding to Equation (7) are defined by Equation (17). Note that
only relevant pairs are used throughout particular phases of solution.
The cost function in Equation (15), differential defect constraints in Equation (10)
and related dynamics in Equation (12), equality boundary constraints in Equation (13),
inequality constraints in Equation (14), and simple bounds in Equation (17) form the full
optimization task for this phase of the solution.

−3 ≤ u1 ≤ 3
π π
− ≤ u2 ≤
3 3
−1 ≤ x1 ≡ X0 ≤ 50
−1 ≤ x2 ≡ Y0 ≤ 40
π π
− ≤ x3 ≡ ϕ0 ≤ (17)
3 3
−2π ≤ x4 ≡ θ0 ≤ 2π
20 ≤ x T ≤ 60
π π
− ≤ x5 ≡ ϕ1 ≤
3 3
π π
− ≤ x6 ≡ ϕ2 ≤
3 3

2.3.3. Phase 3: Velocity Profile Planning

The aim of this phase is to find the missing velocity profile. Solving this problem is
possible using information about the total time required for realization of the trajectories,
which is one of the outputs of Phase 2. Furthermore, using the trajectories found in Phase
2 as an initial estimate, the actual velocity profile is derived as part of the optimization
process in this phase. This velocity profile must satisfy the condition that the total travel
distance equal the integral of the velocity over the given time interval. The velocity profile’s
shape and maximum are both influenced by the τ parameter, which determines the rate of
change of the velocity signal.
As the formulation and solution to OCP in this phase is similar to Phase 2, below we
only mention the differences between these two phases.
The dynamic model in this phase does not include any additional state variables, and
the total optimal time requirement is known from Phase 2. Therefore, the dynamics used
here are formed by the first four equations of Equation (2), and as suvh are described by
Equation (18).
Appl. Sci. 2024, 14, 11675 10 of 23

ẋ1 = u1 · cos( x4 )
ẋ2 = u1 · sin( x4 )
1 1 (18)
ẋ3 = − · x3 + · u2
τ τ
u
ẋ4 = 1 · tan( x3 )
d0
The differential defect constraints corresponding to Equation (10) are now related to
the dynamics in Equation (18).
The equality boundary constraints needed for this phase can be formed fro those from
Phase 2 according to Equation (13) and extended by constraints related to zero initial and
zero final velocity according to Equation (19).
   
u11 0
= (19)
u1 N 0
Therefore, the overall equality boundary constraints for Phase 3 are provided by Equation (20).

x 11 x 01
   
 x 21   x 02 
   
 x3   x0 
ψ=
 1  −  3 (20)
 x 41   x 04 
  
 u1   0 
1
u1 N 0
The inequality constraints stay unchanged compared to Phase 2, and are provided by
Equation (14).
The cost function must now also consider the input u1 , which represents the truck’s
velocity. The cost function mainly aims to minimize the rate of control inputs in time for
both control inputs. It also penalizes the distance between the truck and the corresponding
path point, leading to the cost function provided by Equation (21).
!
N −2
1 1 1
J =100 · · ( u − u 12 ) 2 + ∑ ( u 1 n − u 1 n + 1 ) 2 + ( u 1 N − 1 − u 1 N ) 2 +
N 2 11 n =2
2
!
N −2
1 1 1
+10 · · ( u 21 − u 22 ) + ∑ ( u 2 n − u 2 n + 1 ) + ( u 2 N − 1 − u 2 N ) +
2 2 2 (21)
N 2 n =2
2
N q
+ ∑ ( x1n − Cxn )2 + x2n − Cyn

2
n =1

2.3.4. Phase 4: Truck Dimensions and Collision Check

The outputs of previous Phase 3 are feasible trajectories for the simplified model of
the truck according to Equation (18). The goal of Phase 4 is to find trajectories with respect
to the dimensions of the truck and the presence of obstacles. The results of the previous
Phase 3 are used here as an initial estimate. The most significant change is that the path
points are no longer considered in the cost function. Similar to Phase 3, the dynamics here
relate to the model of the truck provided by Equation (18). Regarding the constraints, all
angles defining the entire truck–trailer system’s position are newly introduced.
In this phase of the solution, collision of the truck with static obstacles placed be-
tween the initial and target points is newly detected. Collision detection consists of four
sub-algorithms (Algorithms 1–4), as described below. The output q from the last one
(Algorithm 4) enters the used cost function provided by Equation (25), which ensures the
penalization of collisions between the truck and obstacles.
The collision detection approach used here is based on the approximation of all objects
by rectangles. Such a simplification is effective in the case of a truck with trailers, as real
Appl. Sci. 2024, 14, 11675 11 of 23

vehicles are frequently rectangular. Integrating potential collisions into the cost function is
a very important part of the solution, and represents one of the main benefits of this paper.
The procedure for detecting the collision of two quadrilaterals described by Equation (22)
and Algorithms 1 and 2 is taken from [20]. The remaining parts of the collision detection
(Algorithms 3 and 4) are our own contribution.
Two quadrilaterals collide only if one or more vertices of one of the quadrilaterals
is inside the other quadrilateral. We can reduce the collision problem to the question of
whether the point K representing any vertex of one of the quadrilaterals is inside the other
quadrilateral. If a collision occurs, Equation (22) must apply. We repeat this procedure for
all four points of the quadrilateral.
 −→ −→ −→ −→  −→ −→ −→ −→
0 < AK · AB < AB · AB ∩ 0 < AK · AD < AD · AD (22)

Assuming that a regular quadrilateral has vertices A, B, C, D, we can construct a func-

tion to detect its collision with point K. The detection procedure is captured in Algorithm 1.

Algorithm 1 Collision detection of a regular quadrilateral and a point

A, B, C, D ← obstacle points
K ← point for which a collision is detected
function C OLLISION R ECTANGLE P OINT(A, B, C, D, K)
−→ −→
ABAK ← AB −→· AK
−→
ADAK ← − → ·−→
AD AK
ABAB ← AB · AB
−→ −→
ADAD ← AD · AD
▷ (0 < AMAB < ABAB) ∧ (0 < AMAD < ADAD)
temp1 ← min(ABAB − ABAK, ABAK)
temp2 ← min(ADAD − ADAK, ADAK)
temp3 ← min(temp1, temp2)
return max(temp3, 0) ▷ 0 ← no collision (otherwise positive value returned)
▷ returns a value in case of a collision > 0
end function

Assuming that point K is one vertex of the quadrilateral K, L, M, N, we can construct

a function for detecting the collision of two regular quadrilaterals (Algorithm 2) based on
the function for the collision of a quadrilateral and a point (Algorithm 1).

Algorithm 2 Collision detection of two regular quadrilaterals

A, B, C, D ← vertices of the first quadrilateral
K, L, M, N ← vertices of the second quadrilateral
k≡0
function C OLLISION R CT R CT(A, B, C, D, K, L, M, N)
k ≡ k + CollisionRectanglePoint( A, B, C, D, K )
k ≡ k + CollisionRectanglePoint( A, B, C, D, L)
k ≡ k + CollisionRectanglePoint( A, B, C, D, M )
k ≡ k + CollisionRectanglePoint( A, B, C, D, N )
k ≡ k + CollisionRectanglePoint(K, L, M, N, A)
k ≡ k + CollisionRectanglePoint(K, L, M, N, B)
k ≡ k + CollisionRectanglePoint(K, L, M, N, C )
k ≡ k + CollisionRectanglePoint(K, L, M, N, D )
return k ▷ k > 0 ← collision occurred
end function

Multiple tests showed that detecting two quadrilaterals may be insufficient in terms of
convergence; thus, we added assessment of the proximity between two quadrilaterals. This
uses a logarithmic barrier function that penalizes situations when any part of the system
Appl. Sci. 2024, 14, 11675 12 of 23

is approaching obstacles. This procedure uses similar input information as the previous
collision detection, and is captured in Algorithm 3.

Algorithm 3 Proximity detection of two regular quadrilaterals

A, B, C, D ← vertices of the first quadrilateral
K, L, M, N ← vertices of the second quadrilateral
⃗ = [0, 1, 2, . . . , 15]
Q ▷ vector for the distances of all combinations of sides in two
rectangles
function D ISTANCE R CT R CT(A, B, C, D, K, L, M, N)
Q[0] = MinDistnc( A, B, K, L); Q[1] = MinDistnc( A, B, K, N );
Q[2] = MinDistnc( A, B, M, N ); Q[3] = MinDistnc( A, B, M, L)
Q[4] = MinDistnc( A, D, K, L); Q[5] = MinDistnc( A, D, K, N )
Q[6] = MinDistnc( A, D, M, N ); Q[7] = MinDistnc( A, D, M, L)
Q[8] = MinDistnc(C, B, K, L); Q[9] = MinDistnc(C, B, K, N )
Q[10] = MinDistnc(C, B, M, N ); Q[11] = MinDistnc(C, B, M, L)
Q[12] = MinDistnc(C, D, K, L); Q[13] = MinDistnc(C, D, K, N )
Q[14] = MinDistnc(C, D, M, N ); Q[15] = MinDistnc(C, D, M, L)
q = −W1 · log(min( Q ⃗ ))
return q
end function

The combination of previous collision detections is then captured in Algorithm 4. The

output variable q from this algorithm is used in the cost function of the current phase.

Algorithm 4 Resulting detection algorithm

j←0
q←0 ▷ the resulting penalty parameter
for i = 1, 2, . . . ,number of obstacles do
for k = 1, 2, . . . ,the number of vehicles of the system do
j = j + CollisionRctRct(obstacle pointsi , vehicle pointsk )
q = q + DistanceRctRct(obstacle pointsi , vehicle pointsk )
end for
end for
if q ≥ qin f then
q = W2 · j
end if
return q

Here, the parameter qin f represents either an infinitely large value, the maximum
value feasible within a given software environment, or a user-defined value. Regarding
parameters W1 and W2 , we chose W1 = 0.001 and W2 = 10. These values represent
weighting factors used for scaling the optimization task. Real-world optimization problems
often comprise parameters of vastly different orders of magnitudes. In many applications,
parameters with very large start values will vary over a wide range, and a change in that
parameter will only lead to a relatively small change in the criterion function. If this is the
case, the scaling of the optimization problem can be improved by some measures, usually
based on heuristics. There are several approaches, such as simply dividing all parameter
vectors by the start parameters or re-mapping all parameters such that the bounds are
[0, 1] for all parameters. For the case study presented here, we used expert choice of these
values. Additional tuning or applying a more sophisticated mechanism to determine these
parameters might be needed in future modifications of the presented case study.
The system model and constraints stay the same as in Phase 3. Because the truck is
no longer treated as a point in this phase, but rather as a quadrilateral in space, it makes
sense to penalize the vehicle’s initial and target angular positions. Furthermore, the target
Appl. Sci. 2024, 14, 11675 13 of 23

position of the truck’s reference point is no longer penalized; instead, all truck vertices are
required to match within the target area.
The rotation angle at the initial point is an extension of the equality constraints for the
initial state provided by Equation (23). Adding them allows us to find feasible trajectories
even if the truck is not aligned precisely with the x-axis at the beginning of movement.
This assumption, which was used in the previous phases, is replaced here by specifying the
desired initial rotation of the truck with respect to the x-axis.
   
x 11 x 01
 x2   x0 
 1  2
 x 31  −  x 03 
ψ= (23)
  
 x4   x0 
1 4
x 51 x 05
The final rotation and position of the truck is written in the form of an inequality
constraint, which is less strict, introduces some relaxation for the optimization task, and im-
proves the existence and convergence likelihood of the solution. The inequality constraints
defined in Equation (24) represent a 2D region in which X ∈ [−6; 6] and Y ∈ [−1.5; 1.5] [m].
In the first eight constraints, all vertices of the truck (modeled as a rectangle with vertices
K, L, M, N) need to be located within the desired target region. The last constraint concerns
the angle of rotation of the truck. Because the endpoints of the truck are not included in
the vector of state variables of the dynamics, we need to compute their coordinates at the
final time point (Kxt f , Kyt f , L xt f , Lyt f , Mxt f , Myt f , Nxt f , Nyt f ) in the same way as in the case
of collision detection with obstacles. Compared to the previous phase, the target area is
slightly enlarged, meaning that its dimensions are larger than the dimensions of the system.

xt f 1 − 6 ≤ K xt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ L xt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ Mxt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ Nxt f ≤ xt f 1 + 6
xt f 2 − 1.5 ≤Kyt f ≤ xt f 2 + 1.5 (24)
xt f 2 − 1.5 ≤ Lyt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ Myt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ Nyt f ≤ xt f 2 + 1.5
pi pi
xt f 4 − ≤ x4 N ≤ x t f 4 +
2 2
The cost function can already fulfill the original requirement expressed by Equation (25) of
minimizing the distance traveled and energy spent on the control action; furthermore, we
retain the variable q, ensuring that collision between the truck–trailer system and obstacles
are penalized.
!
N −2
1 1 1
J =10 · · ( u − u 12 ) + ∑ ( u 1 n − u 1 n + 1 ) + ( u 1 N − 1 − u 1 N ) +
2 2 2
N 2 11 n =2
2
!
N −2
1 1 1
+10 · · (u21 − u22 ) + ∑ (u2n − u2n+1 ) + (u2N −1 − u2N ) +
2 2 2 (25)
N 2 n =2
2
N −1 q
+ ∑

2
2
x 1n − x 1n +1 + x 2n − x 2n +1 + q
n =1
Appl. Sci. 2024, 14, 11675 14 of 23

2.3.5. Phase 5: Adding Trailers

This phase presents the solution to the trajectory planning for the entire truck–trailer
system described by Equation (2). It uses the output from the previous Phase 4 as an initial
estimate. We now require the desired rotation of the entire truck–trailer system at both the
initial and target locations. The initial conditions reflect the initial state of the entire system,
provided by Equation (26).
x1 N x 01
   
 x 2 N   x 02 
   
 x3   x0 
ψ= N −  3
(26)
 x4   x0 
 N  4
 x5   x0 
N 5
x6 N x 06
New constraints on the angles between individual trailers and between the first trailer
and the truck at the final time point are introduced in Equation (27).
pi pi
xt f 4 − ≤ x4 N ≤ x t f 4 +
2 2
pi pi
xt f 5 − ≤ x5 N ≤ x t f 5 + (27)
2 2
pi pi
xt f 6 − ≤ x6 N ≤ x t f 6 +
2 2
In addition to penalizing the location of the final truck’s endpoints as defined by
Equation (28), it is possible to add constraints on the locations of the endpoints of all
trailers within the target area, as expressed by Equation (29).

xt f 1 − 6 ≤ K xt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ L xt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ Mxt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ Nxt f ≤ xt f 1 + 6
(28)
xt f 2 − 1.5 ≤Kyt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ Lyt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ Myt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ Nyt f ≤ xt f 2 + 1.5
The endpoints are referred to as K1, L1, M1, N1 for the first trailer and K2, L2, M2, N2
for the second trailer. Due to the mutual connection of all the vehicles in the truck–trailer
system, it is possible to omit the constraints on the endpoints of the first trailer. This reduces
the number of constraints and speeds up the computation.

xt f 1 − 6 ≤K2xt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ L2xt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ M2xt f ≤ xt f 1 + 6
xt f 1 − 6 ≤ N2xt f ≤ xt f 1 + 6
(29)
xt f 2 − 1.5 ≤K2yt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ L2yt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ M2yt f ≤ xt f 2 + 1.5
xt f 2 − 1.5 ≤ N2yt f ≤ xt f 2 + 1.5
Appl. Sci. 2024, 14, 11675 15 of 23

The cost function used in this phase does not change in this regards, and stays as
described in Equation (25). However, a fundamental change is applied in the case of
collision detection, which is now extended to detect both collisions between obstacles with
the truck and collisions between obstacles and individual trailers.

2.3.6. Phase 6: Fine-Tuning of the Trajectories

In order to find optimal trajectories in the shortest possible computational time, and
consequently lower the dimension of the decision vector, a small number of nodes are
considered for the discretization scheme. The bottleneck in this approach is the possibility
of finding trajectories that collide with an obstacle even though the optimization algorithm
considers them collision-free. To suppress this problem, the final Phase 6 is added to the
solution, during which the number of nodes is considerably increased and the optimization
problem is solved again. Setting the weighting factor W1 in Algorithm 3 can significantly
reduce the risk of finding collision trajectories. When collisions are penalized in the cost
function, the constraints can be characterized as either “soft” or “hard”, and there is no
absolute guarantee of non-collision trajectories. The primary reason for potential collision
is due to by discretization process; therefore, we double the number of nodes here.
Because the initial estimate is based on the previously-found optimal trajectories, the
computation time needed here is significantly reduced. A larger number of nodes where
the found optimal trajectories are defined is also beneficial, and is more effective for the
trajectory tracking problem because the sampling period becomes smaller.
Reducing the sampling period increases the number of time points at which control
inputs and trajectories are determined. In this phase, an initial estimate is obtained by
interpolating with cubic splines based on the solution from the previous phase.
First, the resulting optimal decision vector must be decomposed into individual
variables before applying interpolation. Additionally, it is necessary to ensure that the
interpolated points remain within the range specified by the simple bounds in Equation (17),
as interpolation can sometimes lead to minor violations of these bounds. An example of
how to apply this correction for x1 is provided in Equations (30) and (31), where x1max and
x1min represent the simple bounds for this variable. A tolerance threshold of 0.001 can be
used to enforce these bounds, which is essential because the solver requires the initial guess
to meet the constraints on the design variables. During interpolation, newly determined
points may not inherently satisfy these limits.

x 1n , x1n < x1max
x 1n = (30)
x1max − 0.001, x1n ≥ x1max
(
x 1n , x 1n > x 1
x 1n = min (31)
x1 + 0.001, x1n ≤ x1
min min

The system’s model remains the same as in the previous phase, and is again described
by Equation (2). The constraints on the initial state provided by Equation (26) and the
differential defect constraints for fulfilling the dynamics according to Equation (10) also
remain unchanged.
The form of the cost function is based on the previous step, but is extended by sup-
pressing the difference between the original and newly searched trajectories (between Phase
5 and Phase 6). The implementation of this penalty is complicated by the different lengths of
the decision vector ⃗z, where the original one is always shorter than the current one. In order
to compute the difference, a newly defined initial estimate z⃗0 is used which has the same
length as the searched vector. In order to prevent some values from being unintentionally
penalized against the values found by the spline function, a newly introduced weighting
vector z⃗w is introduced to indicate the positions for difference computation. It contains
values of 1 at positions where the difference is to be computed and zeros at positions
where the difference is not required. Both vectors are multiplied by this vector before the
Appl. Sci. 2024, 14, 11675 16 of 23

actual difference is computed, ensuring that only the values found in the previous step are
penalized. The resulting form of the cost function is provided by Equation (32).
!
N −2
1 1 1
J =10 · · ( u − u 12 ) 2 + ∑ ( u 1 n − u 1 n + 1 ) 2 + ( u 1 N − 1 − u 1 N ) 2 +
N 2 11 n =2
2
!
N −2
1 1 1
+10 · · (u21 − u22 ) + ∑ (u2n − u2n+1 ) + (u2N −1 − u2N ) +
2 2 2
N 2 n =2
2
(32)
N
+ ∑ (zwn · z0n − zwn · zn )+
n =1
N −1 q
∑

2
2
+ x 1n − x 1n +1 + x 2n − x 2n +1 +q
n =1

3. Results and Discussion

3.1. Resulting Optimal Path (Phase 1)
As described in Section 2.3.1, the most significant result from Phase 1 related to path
planning is the optimal path found by Dijkstra algorithm. This path can be described by
−
→ −
→
Equation (33), representing Cx and Cy .
−
→
Cx = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 18, 19, 20, 21,
22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39}
−
→ (33)
Cy = {23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 24, 25, 25, 24, 23, 22, 21, 20,
19, 18, 17, 16, 15, 14, 14, 14, 14, 14, 15, 15, 15, 15, 14, 13, 13, 13, 13, 13}

The optimal path can also be visualized, as seen in Figure 4. It consists of piece-wise
linear lines connecting grid points that refer to the reference point R0 of the truck over time.

Figure 4. Phase 1 optimal found path (left: starting point, right: target point).

3.2. Resulting Optimal Trajectories (Phase 6) and Comparison with the Reference Solution
This subsection presents the final results selected from Phase 6 related to trajectory
planning, namely, the control input u1 representing the truck’s velocity and the state
Appl. Sci. 2024, 14, 11675 17 of 23

variable x3 ≡ ϕ0 representing steering angle of the truck’s front wheels. This also implicitly
represents partial results from Phase 2–5, as they are consecutive throughout the workflow
of the trajectory planning solution. The found trajectories are compared to the reference
solution presented in [20].
Figure 5 shows a comparison of the driving progress for both solutions in a 2D space
over time, with a trailing path illustrating movement up to the selected time points.
Figure 6 illustrates the truck’s movement in detail, with a trailing path provided for
enhanced visibility.
15

10

Y [m]
0

Trajectory of Truck
Trajectory of Trailer 1
Trajectory of Trailer 2

0 10
X [m]

Figure 5. Comparison of the trajectories: proposed solution (left) and reference solution (right).

Figure 6. Comparison of the trajectories (detail)

For clarity, the color legends of the truck and individual trailers are the same for both
solutions. Figure 7 presents a comparison of the resulting optimal control inputs. In the
upper sections, the truck’s speed exhibits distinct signal characteristics; one graph displays
a more oscillatory pattern, with peaks resembling sinusoidal waves, while the other shows
a smoother parabolic-like curve. A similar though less pronounced effect can be observed
in the lower section, which represents the steering angle of the truck’s front wheels.
Appl. Sci. 2024, 14, 11675 18 of 23

1.5

u 1 [m s-1]

u 1 [m s-1]
1

1
0.5

0 0
0 10 20 30 40 0 10 20 30
t [s] t [s]

0.2
0.1

0
x 3 [rad]

x 3 [rad]
0

0 10 20 30 40 0 10 20 30
t [s] t [s]

Figure 7. Comparison of control inputs: proposed solution (left) and reference solution (right).

3.3. Summary of Features of the Proposed Solution

Several relevant indicators were selected for comparison of the proposed and reference
solutions, as summarized in Table 2, which provides a numerical comparison between the
proposed solution and reference solution across several performance metrics, including
computational time, cost function value, total driving time, trajectory point count, distance
traveled, and cumulative sum angle differences of the front wheels. The table highlights
differences in efficiency and effectiveness, with a note indicating that the reference solution’s
computational time includes the use of an initial estimate.

Table 2. Numerical comparison of the proposed solution and reference solution.

Proposed Reference
Comparison Indicator
Solution Solution
Computational time for solution [s] 363.8 409.9 a
The value of the cost function 42.31 44.26
Total driving time [s] 42.35 31.58
Number of points in the trajectory 42 33
Total distance traveled [m] 44.42 44.26
Cumulative sum angle diff of front wheels 2.89 20.09
a time required to find a solution with a supplied initial estimate.

Finding the complete solution requires approximately 364 s, which represents a

marked improvement over the reference solution. Although the authors did not spec-
ify exact times, they provided the Matlab code for their solution. Experimental testing of
this code shows that it also reaches a solution in minutes (409 s), but only when the supplied
initial guess is used. When this is omitted, the reference solution proved unsuccessful, as
the provided code ran for over 12 h without convergence. Therefore, the authors’ initial
guess was adopted for comparison, though including it as part of the solution with the
global SFS solver as the authors did may not be fully accurate.
Appl. Sci. 2024, 14, 11675 19 of 23

Proposing multiple optimization phases proved beneficial for the problem addressed
in this paper. To support this, we also tested the solution without Phase 4 in order to
examine its importance. Although constraints related to the truck and its possible collision
with obstacles as a part of Phase 4 are also included in Phase 5, incorporating Phase 4
makes the computation much faster. Omitting this led to an almost tenfold increase in
computational time, as documented in Table 3. The times are shown in two columns, with
and without Phase 4, indicated by a dash symbol.

Table 3. Computation times for the proposed solution with Phase 4 included (left column) and
omitted (right column)

Phase Time [s] Time [s]

Phase 1: Path Planning 0.2 0.2
Phase 2: Trajectory Planning with Constant Velocity profile 1.4 1.4
Phase 3: Velocity Profile Planning 9.5 9.5
Phase 4: Truck dimensions and collision check 9.2 -
Phase 5: Adding Trailers 343.5 3357
Phase 6: fine tuning of the trajectories 0.3 0.3
Overall value 363.8 3368

All computations and timing measurements were performed using the same hardware
and software setup for consistency: Matlab R2021b on a laptop with an Intel Core i7-11390H
processor and 16 GB of RAM. To ensure repeatability and validate results, additional tests
were also conducted with different hardware and software configurations.
The goal of testing on different hardware and software configurations was to analyze
the following issues:
• Repeatability and Validation: Here, the objective is to ensure reliability by making
sure that the proposed solution consistently produces similar results across different
configurations.
• Hardware Limitations: The effect of RAM size on computation speed was tested, with
no observed improvement for this specific task. However, a limitation of the proposed
solution lies in its high RAM requirements as the number of nodes in Dijkstra’s
algorithm increases. In the presented case study, a single node per meter (k g = 1) was
used. If this number was increased to k g = 5, then 64 GB of RAM would be required,
with RAM demand growing exponentially as more nodes are added. This issue can be
effectively addressed by implementing an adaptive grid, which reduces complexity by
using a finer grid near obstacles and a coarser grid elsewhere, significantly lowering
the computational load. This mainly relates to Phase 1. The computational load for
the rest of phases significantly depends on CPU performance when considering our
PC, Windows OS, and Matlab environment.
To demonstrate behavior on different hardware and software configurations, we
added a table showing some basic relations between CPU benchmarking score (obtained
via the GeekBench utility) and total computational times on three other PC platforms. We
added benchmarking for Raspberry Pi4 and Raspberry Pi5 for consistency in cases where
algorithms should be supposed to work on these platforms. Here, the computational times
remain unknown, as the implementation is currently conducted in the Matlab environment.
To reach a relevant conclusion on performance on Rpi4, Rpi5, and other Linux targets,
the solution would have to be ported there. The most crucial phase would be using an
appropriate NLP solver. We carried out basic tests on simple benchmarking optimal control
problems using the pysolnp NLP solver in a Python environment and COBYLA solver
using FORTRAN language. These tests revealed that good overall efficiency and very
favorable computational times can be achieved in these environments, despite lower CPU
performance as indicated by GeekBench CPU benchmarking.
Table 4 summarizes the tests we carried out on different platforms. Regarding the
RPi4 and RPi5, “U” stands for unknown, as the entire proposed algorithm is currently
Appl. Sci. 2024, 14, 11675 20 of 23

only available in the Matlab environment. The table shows both single-core and multi-core
values obtained with the GeekBench CPU benchmarking utility.

Table 4. Testing results with different SW and HW configurations.

Model RAM Score Single-Core Score Multicore Comp. Time

Dell Inspiron 5379 16 GB 1365 3430 319.477452
HeavyHorse 512 GB 1303 13,310 308.716627
MS-7D76 32 GB 2839 14,566 139.697665
RPi4 4 GB 268 738 U
RPi5 8 GB 566 to 806 1520 to 1650 U

3.4. Challenging Examples, Edge Cases, and Limitations

In addition to the single case study presented in this paper, the proposed approach was
successfully tested on a modified case where reverse movement was considered instead of
forward movement. To address the robustness and limitations of the proposed algorithm
for trajectory planning of truck–trailer systems, additional more complex scenarios could
also be considered. The proposed methodology can help in highly congested environments
with high density of static obstacles, which would showcase its ability to find feasible paths
under complex spatial constraints. Reverse parking with tight constraints poses another
interesting challenge for a truck with multiple trailers in a scenario where spatial constraints
and orientation precision are critical. Under real conditions, adverse initial alignments
may occur, meaning that the truck–trailer system starts in a non-aligned or challenging
initial orientations; such scenarios can help to evaluate convergence to a feasible solution.
A typical use case would be a dense warehouse with multiple static obstacles such as
shelves or parked vehicles, leaving only narrow paths for maneuvering. In such cases,
the truck and trailers may start in a non-optimal initial orientation or with the trailers in
a jackknifed configuration. This would help test the algorithm’s capacity to recover from
adverse initial conditions, and additional evaluation metrics such as time to reach a stable
configuration, computational cost, and success rate could be included. Each phase in the
proposed solution can be tested with increasing complexity to reveal how the algorithm
builds robustness step-by-step. In addition, the results of the proposed algorithm can be
compared to baseline solutions (e.g., RRT) in order to highlight differences in computational
efficiency, solution optimality, and success rates under challenging scenarios.
Regarding the limitations of the proposed solution, we point out the following
main issues:
• Scenarios with dynamic moving obstacles can be used to test algorithms’ adaptability
in the case of real-time trajectory adjustments. However, this is currently impossible
with the proposed approach, as we focus only on parking assistance.
• The current methodology was not tested on scenarios that explicitly showcase areas
where the algorithm might struggle, such as non-convex obstacle spaces.
• The tradeoff between trajectory smoothness and computational time represents an
open topic for further analysis and validation tests that could demonstrate how the
algorithm prioritizes these aspects.

4. Conclusions
The presented results depicted in Figure 5 document our solution to the problem
formulated in Section 2.1. The findings presented in this study illustrate a robust solution
to the truck–trailer maneuverability challenge as defined in the problem formulation. The
primary focus has been on achieving forward movement control of a two-trailer truck
system, with additional experiments involving a three-trailer configuration conducted
to test the flexibility and robustness of the proposed solution. These additional tests,
Appl. Sci. 2024, 14, 11675 21 of 23

including scenarios with reverse movement, validate the adaptability and effectiveness of
our approach across a broader spectrum of real-world applications.
Our approach contributes to the domain by addressing the complexity inherent in
multi-trailer systems, where maintaining precise control of each segment is critical for
stability and operational efficiency. The algorithms and control strategies developed
herein demonstrate significant promise for applications in industries where automated and
semi-automated trailer maneuvering is essential, such as logistics, freight transport, and
yard operations.
While the focus of this work has primarily been on achieving a solution under for-
ward driving conditions, our extended experiments with multiple trailers and reversing
maneuvers suggest that the proposed control system could potentially be generalized to
even more complex scenarios. Testing both forward and reverse movements of a truck
with three trailers in a real-world environment using the educational physical description
in [27] further supports the validity of the proposed concept. Future work could build
upon these foundations to explore refinements in control algorithms that can accommo-
date varying load distributions and trailer lengths, allowing for real-time adaptability in
dynamic environments.
As future work, more sophisticated mechanism related to increasing number of nodes
could be applied when refining the trajectories in Phase 6, which would apply more
increasing steps in iterations and evaluate progress between steps. By treating evaluation
as a relative progress under a certain relative tolerance, the algorithm could end after
reaching some very high chance of finding a non-collision trajectory. Such mechanism
could also be combined with other measures, such as considering obstacles as somewhat
larger then they really are, thereby introducing a certain relaxation and safety margin.
In addition to what is mentioned in this paper regarding evaluation of the results, ad-
ditional metrics such as trajectory length, control input smoothness, and error from desired
endpoints could be used to evaluate the algorithm’s capabilities and limitations more com-
prehensively. In the future, we may test the algorithm’s sensitivity to parameter variations
in order to provide a holistic evaluation. For example, it would be possible to explore a
truck with multiple trailers of varying lengths navigating a given course to test how the
algorithm handles varying parameters and the robustness of the solution. After the trajec-
tory planning problem is solved, the next step is trajectory tracking. While this paper does
not address trajectory tracking in detail, standard methods such as using Linear Quadratic
Regulator (LQR) over a finite horizon can be employed. Alternatively, more advanced
algorithms such as the approach proposed in [28] can be utilized, which focuses on path
tracking control for autonomous vehicles while accounting for network-induced delays
and incorporating roll dynamics to enhance both driving safety and passenger comfort.
In summary, this paper contributes a foundational approach to enhancing the maneu-
verability of multi-trailer systems, presenting both theoretical and practical insights that
can pave the way for further research and industrial applications.

Author Contributions: S.O. (Stepan Ozana): methodology, writing—original draft; F.K. (Filip Krupa):
methodology, visualization, data curation; Z.S. (Zdenek Slanina): editing, supervision; All authors
have read and agreed to the published version of the manuscript.
Funding: This work was supported by the project SP2024/021, “Development of algorithms and
systems for control, measurement, and safety applications X” under the Student Grant System,
VSB-TU Ostrava. This article was produced with the financial support of the European Union
under the REFRESH – Research Excellence for Region Sustainability and High-tech Industries project
number CZ.10.03.01/00/22_003/0000048 through the Operational Programme Just Transition. This
work was also supported by the European Regional Development Fund for the Research Centre of
Advanced Mechatronic Systems project, project number CZ.02.1.01/0.0/0.0/16_019/0000867 under
the Operational Programme Research, Development and Education.
Data Availability Statement: Dataset available on request from the authors.
Conflicts of Interest: The authors declare no conflicts of interest.
Appl. Sci. 2024, 14, 11675 22 of 23

References
1. Chang, T.; Tian, G. Hybrid A-Star Path Planning Method Based on Hierarchical Clustering and Trichotomy. Appl. Sci. 2024,
14, 5582. [CrossRef]
2. Li, B.; Acarman, T.; Zhang, Y.; Zhang, L.; Yaman, C.; Kong, Q. Tractor-trailer vehicle trajectory planning in narrow environments
with a progressively constrained optimal control approach. IEEE Trans. Intell. Veh. 2019, 5, 414–425. [CrossRef]
3. Li, B.; Zhang, Y.; Ouyang, Y.; Liu, Y.; Zhong, X.; Cen, H.; Kong, Q. Real-time trajectory planning for agv in the presence of moving
obstacles: A first-search-then-optimization approach. arXiv 2019, arXiv:1902.06201. [CrossRef]
4. Erraji, O. Model Predictive Control System Design of a Passenger Car for Valet Parking Scenario. Ph.D. Thesis, University of
Windsor, Windsor, ON, Canada, 2018.
5. Lin, L.; Jim Zhu, J. Path planning for autonomous car parking. In Proceedings of the ASME 2018 Dynamic Systems and Control
Conference, DSCC 2018, Atlanta, GA, USA, 30 September–3 October 2018; Volume 3. [CrossRef]
6. Lee, B.; Wei, Y.; Guo, I.Y. Automatic parking of self-driving car based on lidar. Int. Arch. Photogramm. Remote. Sens. Spat. Inf. Sci.
2017, 42, 241–246. [CrossRef]
7. Kuwata, Y.; Teo, J.; Fiore, G.; Karaman, S.; Frazzoli, E.; How, J.P. Real-time motion planning with applications to autonomous
urban driving. IEEE Trans. Control. Syst. Technol. 2009, 17, 1105–1118. [CrossRef]
8. Divelbiss, A.W.; Wen, J.T. Trajectory tracking control of a car-trailer system. IEEE Trans. Control. Syst. Technol. 1997, 5, 269–278.
[CrossRef]
9. Xu, H.; Yu, Z.; Lu, X.; Wang, S.; Li, S.; Wang, S. Model predictive control-based path tracking control for automatic guided
vehicles. In Proceedings of the 4th CAA International Conference on Vehicular Control and Intelligence (CVCI), Hangzhou,
China, 18–20 December 2020; IEEE: Piscataway, NJ, USA, 2020; pp. 627–632. [CrossRef]
10. Qian, X. Model Predictive Control for Autonomous and Cooperative Driving. Ph.D. Thesis, PSL Research University, Paris,
France, 2017.
11. Salmon, J. Guidance of an off-road tractor-trailer system using model predictive control. Ph.D. Thesis, Auburn University,
Auburn, AL, USA, 2013.
12. Hery, E.; Masi, S.; Xu, P.; Bonnifait, P. Map-based curvilinear coordinates for autonomous vehicles. In Proceedings of the 2017
IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), Yokohama, Japan, 16–19 October 2017; IEEE:
Piscataway, NJ, USA, 2017; pp. 1–7. [CrossRef]
13. Ghilardelli, F.; Lini, G.; Piazzi, A. Path Generation Using η 4 -Splines for a Truck and Trailer Vehicle. IEEE Trans. Autom. Sci. Eng.
2013, 11, 187–203. [CrossRef]
14. Szynkiewicz, W.; Błaszczyk, J. Optimization-based approach to path planning for closed chain robot systems. Int. J. Appl. Math.
Comput. Sci. 2011, 21, 659–670. [CrossRef]
15. Mirjalili, S.; Mirjalili, S.M.; Lewis, A. Grey wolf optimizer. Adv. Eng. Softw. 2014, 69, 46–61. [CrossRef]
16. Mirjalili, S.; Lewis, A. The whale optimization algorithm. Adv. Eng. Softw. 2016, 95, 51–67. [CrossRef]
17. Meraihi, Y.; Ramdane-Cherif, A.; Acheli, D.; Mahseur, M. Dragonfly algorithm: A comprehensive review and applications. Neural
Comput. Appl. 2020, 32, 16625–16646. [CrossRef]
18. Salimi, H. Stochastic fractal search: A powerful metaheuristic algorithm. Knowl.-Based Syst. 2015, 75, 1–18. [CrossRef]
19. Rocha, A.M.A.; Fernandes, E.M. A Stochastic Augmented Lagrangian Equality Constrained-Based Algorithm for Global
Optimization. In Proceedings of the AIP Conference Proceedings, Rhodes, Greece, 19–25 September 2010; American Institute of
Physics: College Park, MD, USA, 2010; Volume 1281, pp. 967–970. [CrossRef]
20. Li, B.; Shao, Z. Precise trajectory optimization for articulated wheeled vehicles in cluttered environments. Adv. Eng. Softw. 2016,
92, 40–47. [CrossRef]
21. Ye, H.; Jiang, H.; Ma, S.; Tang, B.; Wahab, L. Linear model predictive control of automatic parking path tracking with soft
constraints. Int. J. Adv. Robot. Syst. 2019, 16, 1729881419852201. [CrossRef]
22. Mathworks. Fmincon. 2021. Available online: https://www.mathworks.com/help/optim/ug/fmincon.html (accessed on 13
December 2024 ).
23. Zadachyn, V.; Dorokhov, O. Calculation of optimal path for parallel car parking. Transp. Telecommun. 2012, 13, 303. [CrossRef]
24. Lin, L.; Zhu, J.J. Trajectory Generation From Paths for Autonomous Ground Vehicles. In Proceedings of the Dynamic Systems
and Control Conference, Virtual, Online, 5–7 October 2020; ASME: New York, NY, USA, 2020; Volume 84287, p. V002T31A004.
[CrossRef]
25. Ge, D.; Liu, T.; Liu, J.; Tan, J.; Ye, Y. SOLNP+: A Derivative-Free Solver for Constrained Nonlinear Optimization. arXiv 2022,
arXiv:2210.07160. .
26. Arumugam, V.; Algumalai, V. Topological Navigation of Path Planning Using a Hybrid Architecture in Wheeled Mobile Robot. In
Proceedings of the Communications in Computer and Information Science, 2023, Chennai, India ; Volume 2202 CCIS, pp. 32–44.
[CrossRef]
Appl. Sci. 2024, 14, 11675 23 of 23

27. REX Controls. Autonomous Reversing T3T. 2020. Available online: https://www.rexcontrols.com/autonomous-reversing
(accessed on 13 December 2024).
28. Viadero-Monasterio, F.; Nguyen, A.T.; Lauber, J.; Boada, M.J.L.; Boada, B.L. Event-Triggered Robust Path Tracking Control
Considering Roll Stability Under Network-Induced Delays for Autonomous Vehicles. IEEE Trans. Intell. Transp. Syst. 2023,
24, 14743–14756. [CrossRef]

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual
author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to
people or property resulting from any ideas, methods, instructions or products referred to in the content.