Efficient Optimization-Based Trajectory Planning for Truck–Trailer Systems

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 navigating 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.

1. Introduction

Nowadays, we have been experiencing significant development in the field of autonomous 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 issue that has been addressed by scientific teams around the world for more than a decade.

This article focuses on assisted parking for a system consisting of a main vehicle (truck) and articulated links (trailers).

In particular, finding collision-free trajectories is a key element for such types of problems. The most commonly used methods and procedures for setting up the problem of path planning and trajectory planning are briefly summarized.

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 and dynamic models. Kinematic models are especially useful for slow movements where the dynamic forces can be neglected. Such models are suitable for maneuvering between 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 main disadvantage, leading kinematic models to be preferred in most cases, including in this paper.

1.1. Problem of Path Planning

Path planning is the process of finding a sequence of points between a start and a destination in a coordinate system. These points then represent the path the vehicle must follow. Finding such a path usually falls into the problem of finding a global minimum. 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,3,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,10,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 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 optimization. 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,16,17]. However, there are a variety of other stochastic algorithms [18,19]. The use of these algorithms 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 $fmincon$ (or $fminunc$) [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.

In our proposed approach, we select the $fmincon$ 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 $R_0$ 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.

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):

(1)$\overrightarrow{x}=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\\ x_5\\ x_6\end{array}\right]\equiv \left[\begin{array}{c}{X}_0\\ Y_0\\ {\varphi }_0\\ {\theta }_0\\ {\varphi }_1\\ {\varphi }_2\end{array}\right]$

where:

• $X_0,Y_0$ refer to the x- and y-coordinates of the truck’s reference point $R_0$ [m].

• ${\varphi }_0$... is the angle of rotation of the wheels relative to the axis of the truck [rad].

• ${\varphi }_1$... is the angle between the axis of the truck and the axis of the first trailer [rad].

• ${\varphi }_2$... is the angle between the axis of the first trailer and the axis of the second trailer [rad].

• ${\theta }_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):

(2)$\begin{array}{c}{\dot{x}}_1=u_1·cos\left(x_4\right)\\ {\dot{x}}_2=u_1·sin\left(x_4\right)\\ {\dot{x}}_3=-{\displaystyle \frac{1}{\tau }}·x_3+{\displaystyle \frac{1}{\tau }}·u_2\\ {\dot{x}}_4={\displaystyle \frac{u_1}{d_0}}·tan\left(x_3\right)\\ {\dot{x}}_5={\displaystyle \frac{u_1}{d_0}}·tan\left(x_3\right)-{\displaystyle \frac{u_1}{d_1}}·\left[sin\left(x_5\right)-{\displaystyle \frac{a}{d_0}}·cos\left(x_5\right)·tan\left(x_3\right)\right]\\ {\dot{x}}_6={\displaystyle \frac{u_1}{d_1}}·\left[sin\left(x_5\right)-{\displaystyle \frac{a}{d_0}}·cos\left(x_5\right)·tan\left(x_3\right)\right]\\ -{\displaystyle \frac{u_1}{d_2}}·sin\left(x_6\right)·\left[cos\left(x_5\right)+{\displaystyle \frac{a}{d_0}}·sin\left(x_5\right)·tan\left(x_3\right)\right]\end{array}$

where:

• $u_1$ is the first system’s input, i.e., the velocity of the truck $v_0$ [$\mathrm{m}·{\mathrm{s}}^{-1}$].

• $u_2$ is the second system’s input, i.e., the setpoint for the front wheel steering angle ${\varphi }_0$ [rad].

• $d_0$ determines the distance of points $|R_{0}S|$, representing the distance between the axles of the truck [m].

• a determines the distance between the points $|R_{0}Q|$ of the rotary joint Q and the reference point of the truck $R_0$ [m].

• $d_1$ determines the distance of the points $|Q{R}_1|$ representing the first trailer’s reference point $R_1$ and the rotary joint of the truck Q [m].

• $d_2$ determines the distance of points $|R_1{R}_2|$ representing the distance of reference points of two consecutive trailers [m].

• $\tau$ 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):

(3)$J=\Phi [t_0,t_F,\mathit{x}\left(t_0\right),\mathit{x}\left(t_F\right)]+{\int }_{t_0}^{t_F}L[\mathit{x}\left(t\right),\mathit{u}\left(t\right),t]\mathrm{d}t.$

The original OCP is converted to a general nonlinear programming problem (NLP) in the form of Equation (4), described as finding an optimal solution ${\mathit{z}}^{*}$ which minimizes the cost function J depending on the decision vector variable $\mathit{z}$ which contains both control inputs and states. We find its optimal value, provided as Equation (4):

(4)${\mathit{z}}^{*}=\underset{\mathit{z}}{argmin}\left\{J\left(\mathit{z}\right)\right\}.$

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):

(5)${\mathit{c}}_{eq}\left(\mathit{z}\right)=\mathbf{0}$

(6)$\mathit{c}\left(\mathit{z}\right)\le \mathbf{0}$

(7)${\mathit{z}}_l\le \mathit{z}\le {\mathit{z}}_u.$

The set of equality constraints ${\mathit{c}}_{eq}$ corresponding to Equation (5) is composed of equality boundary constraints $\mathit{\psi }$ and equality differential defect constraints $\mathit{\xi }$, as follows:

(8)${\mathit{c}}_{eq}=\left[\begin{array}{c}\mathit{\psi }\\ \mathit{\xi }\end{array}\right].$

Converting the OCP into an NLP is achieved through a direct transcription discretization 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\in [0,N-2]\in {\mathbb{N}}^0$ stands for the current step of the computation, N is the number of nodes, $k_1$, $k_2$, $k_3$, and $k_4$ are determined by Equation (11), and h represents the sampling period.

(9)${\mathit{x}}_{\mathit{k}+\mathbf{1}}={\mathit{x}}_{\mathit{k}}+{\displaystyle \frac{1}{6}}·h({\mathit{k}}_1+2·{\mathit{k}}_2+2·{\mathit{k}}_3+{\mathit{k}}_4)$

(10)${\mathit{\xi }}_k={\mathit{x}}_{k+1}-{\mathit{x}}_k-{\displaystyle \frac{1}{6}}·h·({\mathit{k}}_1+2·{\mathit{k}}_2+2·{\mathit{k}}_3+{\mathit{k}}_4)$

(11)$\begin{array}{cc}\hfill {\mathit{k}}_{\mathbf{1}}& =f({\mathit{x}}_k,{\mathit{u}}_k)\hfill \\ \hfill {\mathit{k}}_{\mathbf{2}}& =f({\mathit{x}}_k+{\displaystyle \frac{h}{2}}·{\mathit{k}}_{\mathbf{1}},{\displaystyle \frac{{\mathit{u}}_{k+1}+{\mathit{u}}_k}{2}})\hfill \\ \hfill {\mathit{k}}_{\mathbf{3}}& =f({\mathit{x}}_k+{\displaystyle \frac{h}{2}}·{\mathit{k}}_{\mathbf{2}},{\displaystyle \frac{{\mathit{u}}_{k+1}+{\mathit{u}}_k}{2}})\hfill \\ \hfill {\mathit{k}}_{\mathbf{4}}& =f({\mathit{x}}_k+h·{\mathit{k}}_{\mathbf{3}},{\mathit{u}}_{k+1})\hfill \end{array}$

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.

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\left\{k_g\in \mathbb{N}\right\}$, which indicates the number of nodes per unit of length.

Supposing N nodes, the $PEN$ matrix contains $N\times 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 $\overrightarrow{C_x}$ containing the x-coordinates of the found points and vector $\overrightarrow{C_y}$ 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 $R_0$ 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 $R_0$ 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.

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 total time needed to realize the desired trajectory. We first select an appropriate constant velocity profile $u_1$ 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 $u_1=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 $u_1·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 $T_s$ 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).

(12)$\begin{array}{cc}\hfill {\dot{x}}_1& =x_T·u_1·cos\left(x_4\right)\hfill \\ \hfill {\dot{x}}_2& =x_T·u_1·sin\left(x_4\right)\hfill \\ \hfill {\dot{x}}_3& =x_T·(-{\displaystyle \frac{1}{\tau }}·x_3+{\displaystyle \frac{1}{\tau }}·u_2)\hfill \\ \hfill {\dot{x}}_4& =x_T·{\displaystyle \frac{u_1}{d_0}}·tan\left(x_3\right)\hfill \\ \hfill {\dot{x}}_T& =0\hfill \end{array}$

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 $\overrightarrow{x_0}$.

(13)$\psi =\left[\begin{array}{c}{x}_{1_1}\\ x_{2_1}\\ x_{3_1}\\ x_{4_1}\end{array}\right]-\left[\begin{array}{c}{x}_{0_1}\\ x_{0_2}\\ x_{0_3}\\ x_{0_4}\end{array}\right]$

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 significantly 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 $R_0$, with dimensions denoted by $x_{\mathrm{tgt}}$ and $y_{\mathrm{tgt}}$. 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 $\overrightarrow{x_{tf}}$ is a vector of state variables indicating the desired value at the end of the procedure.

(14)$\begin{array}{cc}\hfill x_{{tf}_1}-{\displaystyle \frac{x_{\mathrm{tgt}}}{2}}\le & z_{3N}\le x_{{tf}_1}+{\displaystyle \frac{x_{\mathrm{tgt}}}{2}}\hfill \\ \hfill x_{{tf}_2}-{\displaystyle \frac{y_{\mathrm{tgt}}}{2}}\le & z_{4N}\le x_{{tf}_2}+{\displaystyle \frac{y_{\mathrm{tgt}}}{2}}\hfill \end{array}$

The cost function used here is defined by Equation (15). This function penalizes the distance between truck’s reference point $R_0$ and the corresponding path point, and also minimizes the total time. Numerical time integration is computed using the trapezoidal method. The variables $C_{xn}$ and $C_{yn}$ correspond to the coordinates of the found path $\overrightarrow{C_x}$ and $\overrightarrow{C_y}$, respectively, as described in Section 2.3.1.

(15)$J=50·\sum _{n=1}^N\sqrt{{\left(x_{1_n}-C_{x_n}\right)}^2+{\left(x_{2_n}-C_{y_n}\right)}^2}+{\displaystyle \frac{1}{N}}·\left({\displaystyle \frac{1}{2}}{x}_{T_1}^2+\sum _{n=2}^{N-1}{x}_{T_n}^2+{\displaystyle \frac{1}{2}}{x}_{T_N}^2\right)$

If a solution is successfully found, it is necessary to extract individual control inputs and state variables from decision vector. The state variable $\overrightarrow{x_T}$ becomes constant, i.e., $\overrightarrow{x_T}\equiv x_T$, and represents the total travel time of the system. For further steps, it is necessary to determine the sampling period from these data, which is computed according to Equation (16).

(16)$T_s={\displaystyle \frac{x_T}{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.

(17)$\begin{array}{cc}\hfill -3\le & u_1\le 3\hfill \\ \hfill -{\displaystyle \frac{\pi }{3}}\le & u_2\le {\displaystyle \frac{\pi }{3}}\hfill \\ \hfill -1\le & x_1\equiv X_0\le 50\hfill \\ \hfill -1\le & x_2\equiv Y_0\le 40\hfill \\ \hfill -{\displaystyle \frac{\pi }{3}}\le & x_3\equiv {\varphi }_0\le {\displaystyle \frac{\pi }{3}}\hfill \\ \hfill -2\pi \le & x_4\equiv {\theta }_0\le 2\pi \hfill \\ \hfill 20\le & x_T\le 60\hfill \\ \hfill -{\displaystyle \frac{\pi }{3}}\le & x_5\equiv {\varphi }_1\le {\displaystyle \frac{\pi }{3}}\hfill \\ \hfill -{\displaystyle \frac{\pi }{3}}\le & x_6\equiv {\varphi }_2\le {\displaystyle \frac{\pi }{3}}\hfill \end{array}$

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 $\tau$ 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).

(18)$\begin{array}{cc}\hfill {\dot{x}}_1& =u_1·cos\left(x_4\right)\hfill \\ \hfill {\dot{x}}_2& =u_1·sin\left(x_4\right)\hfill \\ \hfill {\dot{x}}_3& =-{\displaystyle \frac{1}{\tau }}·x_3+{\displaystyle \frac{1}{\tau }}·u_2\hfill \\ \hfill {\dot{x}}_4& ={\displaystyle \frac{u_1}{d_0}}·tan\left(x_3\right)\hfill \end{array}$

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).

(19)$\left[\begin{array}{c}{u}_{1_1}\\ u_{1_N}\end{array}\right]=\left[\begin{array}{c}0\\ 0\end{array}\right]$

Therefore, the overall equality boundary constraints for Phase 3 are provided by Equation (20).

(20)$\psi =\left[\begin{array}{c}{x}_{1_1}\\ x_{2_1}\\ x_{3_1}\\ x_{4_1}\\ u_{1_1}\\ u_{1_N}\end{array}\right]-\left[\begin{array}{c}{x}_{0_1}\\ x_{0_2}\\ x_{0_3}\\ x_{0_4}\\ 0\\ 0\end{array}\right]$

The inequality constraints stay unchanged compared to Phase 2, and are provided by Equation (14).

The cost function must now also consider the input $u_1$, 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).

(21)$\begin{array}{cc}\hfill J=& 100·{\displaystyle \frac{1}{N}}·\left({\displaystyle \frac{1}{2}}{(u_{1_1}-u_{1_2})}^2+\sum _{n=2}^{N-2}{(u_{1_n}-u_{1_{n+1}})}^2+{\displaystyle \frac{1}{2}}{(u_{1_{N-1}}-u_{1_N})}^2\right)+\hfill \\ \hfill +& 10·{\displaystyle \frac{1}{N}}·\left({\displaystyle \frac{1}{2}}{(u_{2_1}-u_{2_2})}^2+\sum _{n=2}^{N-2}{(u_{2_n}-u_{2_{n+1}})}^2+{\displaystyle \frac{1}{2}}{(u_{2_{N-1}}-u_{2_N})}^2\right)+\hfill \\ \hfill +& \sum _{n=1}^N\sqrt{{\left(x_{1_n}-C_{x_n}\right)}^2+{\left(x_{2_n}-C_{y_n}\right)}^2}\hfill \end{array}$

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 between 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 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.

(22)$\left(0<\overrightarrow{AK}·\overrightarrow{AB}<\overrightarrow{AB}·\overrightarrow{AB}\right)\cap \left(0<\overrightarrow{AK}·\overrightarrow{AD}<\overrightarrow{AD}·\overrightarrow{AD}\right)$

Assuming that a regular quadrilateral has vertices $A,B,C,D$, we can construct a function 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\leftarrow$ obstacle points $K\leftarrow$ point for which a collision is detected function  CollisionRectanglePoint($A,B,C,D,K$)     $ABAK\leftarrow \overrightarrow{AB}·\overrightarrow{AK}$     $ADAK\leftarrow \overrightarrow{AD}·\overrightarrow{AK}$     $ABAB\leftarrow \overrightarrow{AB}·\overrightarrow{AB}$     $ADAD\leftarrow \overrightarrow{AD}·\overrightarrow{AD}$     ▹$(0<\mathrm{AMAB}<\mathrm{ABAB})\wedge (0<\mathrm{AMAD}<\mathrm{ADAD})$     $\mathrm{temp}1\leftarrow min(\mathrm{ABAB}-\mathrm{ABAK},\mathrm{ABAK})$     $\mathrm{temp}2\leftarrow min(\mathrm{ADAD}-\mathrm{ADAK},\mathrm{ADAK})$     $\mathrm{temp}3\leftarrow min(\mathrm{temp}1,\mathrm{temp}2)$     return $max(\mathrm{temp}3,0)$    ▹$0\leftarrow$ 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\leftarrow$ vertices of the first quadrilateral $K,L,M,N\leftarrow$ vertices of the second quadrilateral $k\equiv 0$ function  CollisionRctRct($A,B,C,D,K,L,M,N$)     $k\equiv k+CollisionRectanglePoint(A,B,C,D,K)$     $k\equiv k+CollisionRectanglePoint(A,B,C,D,L)$     $k\equiv k+CollisionRectanglePoint(A,B,C,D,M)$     $k\equiv k+CollisionRectanglePoint(A,B,C,D,N)$     $k\equiv k+CollisionRectanglePoint(K,L,M,N,A)$     $k\equiv k+CollisionRectanglePoint(K,L,M,N,B)$     $k\equiv k+CollisionRectanglePoint(K,L,M,N,C)$     $k\equiv k+CollisionRectanglePoint(K,L,M,N,D)$     return k    ▹$k>0\leftarrow$ 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 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\leftarrow$ vertices of the first quadrilateral $K,L,M,N\leftarrow$ vertices of the second quadrilateral $\overrightarrow{Q}=[0,1,2,\dots ,15]$    ▹ vector for the distances of all combinations of sides in two rectangles function  DistanceRctRct($A,B,C,D,K,L,M,N$)     $Q\left[0\right]=MinDistnc(A,B,K,L);Q\left[1\right]=MinDistnc(A,B,K,N);$     $Q\left[2\right]=MinDistnc(A,B,M,N);$ $Q\left[3\right]=MinDistnc(A,B,M,L)$     $Q\left[4\right]=MinDistnc(A,D,K,L);$ $Q\left[5\right]=MinDistnc(A,D,K,N)$     $Q\left[6\right]=MinDistnc(A,D,M,N);$ $Q\left[7\right]=MinDistnc(A,D,M,L)$     $Q\left[8\right]=MinDistnc(C,B,K,L);$ $Q\left[9\right]=MinDistnc(C,B,K,N)$     $Q\left[10\right]=MinDistnc(C,B,M,N);$ $Q\left[11\right]=MinDistnc(C,B,M,L)$     $Q\left[12\right]=MinDistnc(C,D,K,L);$ $Q\left[13\right]=MinDistnc(C,D,K,N)$     $Q\left[14\right]=MinDistnc(C,D,M,N);$ $Q\left[15\right]=MinDistnc(C,D,M,L)$     $q=-W_1·log\left(min\left(\overrightarrow{Q}\right)\right)$     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\leftarrow 0$ $q\leftarrow 0$    ▹ the resulting penalty parameter for $i=1,2,\dots ,$ number of obstacles do     for $k=1,2,\dots ,$ the number of vehicles of the system do         $j=j+CollisionRctRct(\mathrm{obstacle}{\mathrm{points}}_i,\mathrm{vehicle}{\mathrm{points}}_k)$         $q=q+DistanceRctRct(\mathrm{obstacle}{\mathrm{points}}_i,\mathrm{vehicle}{\mathrm{points}}_k)$     end for end for if  $q\ge q_{inf}$  then     $q=W_2·j$ end if return  q

Here, the parameter $q_{inf}$ represents either an infinitely large value, the maximum value feasible within a given software environment, or a user-defined value. Regarding parameters $W_1$ and $W_2$, we chose $W_1=0.001$ and $W_2=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 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.

(23)$\psi =\left[\begin{array}{c}{x}_{1_1}\\ x_{2_1}\\ x_{3_1}\\ x_{4_1}\\ x_{5_1}\end{array}\right]-\left[\begin{array}{c}{x}_{0_1}\\ x_{0_2}\\ x_{0_3}\\ x_{0_4}\\ x_{0_5}\end{array}\right]$

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 improves the existence and convergence likelihood of the solution. The inequality constraints defined in Equation (24) represent a 2D region in which $X\in [-6;6]$ and $Y\in [-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 ($K_{x_{tf}},K_{y_{tf}},L_{x_{tf}},L_{y_{tf}},M_{x_{tf}},M_{y_{tf}},N_{x_{tf}},N_{y_{tf}}$) 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.

(24)$\begin{array}{cc}\hfill x_{{tf}_1}-6\le & K_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & L_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & M_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & N_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_2}-1.5\le & K_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & L_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & M_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & N_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_4}-{\displaystyle \frac{pi}{2}}\le & x_{4_N}\le x_{{tf}_4}+{\displaystyle \frac{pi}{2}}\hfill \end{array}$

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.

(25)$\begin{array}{cc}\hfill J=& 10·{\displaystyle \frac{1}{N}}·\left({\displaystyle \frac{1}{2}}{(u_{1_1}-u_{1_2})}^2+\sum _{n=2}^{N-2}{(u_{1_n}-u_{1_{n+1}})}^2+{\displaystyle \frac{1}{2}}{(u_{1_{N-1}}-u_{1_N})}^2\right)+\hfill \\ \hfill +& 10·{\displaystyle \frac{1}{N}}·\left({\displaystyle \frac{1}{2}}{(u_{2_1}-u_{2_2})}^2+\sum _{n=2}^{N-2}{(u_{2_n}-u_{2_{n+1}})}^2+{\displaystyle \frac{1}{2}}{(u_{2_{N-1}}-u_{2_N})}^2\right)+\hfill \\ \hfill +& \sum _{n=1}^{N-1}\sqrt{{\left(x_{1_n}-x_{1_{n+1}}\right)}^2+{\left(x_{2_n}-x_{2_{n+1}}\right)}^2}+q\hfill \end{array}$

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).

(26)$\psi =\left[\begin{array}{c}{x}_{1_N}\\ x_{2_N}\\ x_{3_N}\\ x_{4_N}\\ x_{5_N}\\ x_{6_N}\end{array}\right]-\left[\begin{array}{c}{x}_{0_1}\\ x_{0_2}\\ x_{0_3}\\ x_{0_4}\\ x_{0_5}\\ x_{0_6}\end{array}\right]$

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).

(27)$\begin{array}{cc}\hfill x_{{tf}_4}-{\displaystyle \frac{pi}{2}}\le & x_{4_N}\le x_{{tf}_4}+{\displaystyle \frac{pi}{2}}\hfill \\ \hfill x_{{tf}_5}-{\displaystyle \frac{pi}{2}}\le & x_{5_N}\le x_{{tf}_5}+{\displaystyle \frac{pi}{2}}\hfill \\ \hfill x_{{tf}_6}-{\displaystyle \frac{pi}{2}}\le & x_{6_N}\le x_{{tf}_6}+{\displaystyle \frac{pi}{2}}\hfill \end{array}$

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).

(28)$\begin{array}{cc}\hfill x_{{tf}_1}-6\le & K_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & L_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & M_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & N_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_2}-1.5\le & K_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & L_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & M_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & N_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \end{array}$

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.

(29)$\begin{array}{cc}\hfill x_{{tf}_1}-6\le & K{2}_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & L{2}_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & M{2}_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_1}-6\le & N{2}_{x_{tf}}\le x_{{tf}_1}+6\hfill \\ \hfill x_{{tf}_2}-1.5\le & K{2}_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & L{2}_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & M{2}_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \\ \hfill x_{{tf}_2}-1.5\le & N{2}_{y_{tf}}\le x_{{tf}_2}+1.5\hfill \end{array}$

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 $W_1$ 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 $x_1$ is provided in Equations (30) and (31), where $x_{1_{max}}$ and $x_{1_{min}}$ 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.

(30)$x_{1_n}=\left\{\begin{array}{cc}{x}_{1_n},& x_{1_n}<{x_1}_{\max }\\ {x_1}_{\max }-0.001,& x_{1_n}\ge {x_1}_{\max }\end{array}\right.$

(31)$x_{1_n}=\left\{\begin{array}{cc}{x}_{1_n},& x_{1_n}>{x_1}_{\min }\\ {x_1}_{\min }+0.001,& x_{1_n}\le {x_1}_{\min }\end{array}\right.$

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 suppressing 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 $\overrightarrow{z}$, where the original one is always shorter than the current one. In order to compute the difference, a newly defined initial estimate $\overrightarrow{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 $\overrightarrow{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 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).

(32)$\begin{array}{cc}\hfill J=& 10·{\displaystyle \frac{1}{N}}·\left({\displaystyle \frac{1}{2}}{(u_{1_1}-u_{1_2})}^2+\sum _{n=2}^{N-2}{(u_{1_n}-u_{1_{n+1}})}^2+{\displaystyle \frac{1}{2}}{(u_{1_{N-1}}-u_{1_N})}^2\right)+\hfill \\ \hfill +& 10·{\displaystyle \frac{1}{N}}·\left({\displaystyle \frac{1}{2}}{(u_{2_1}-u_{2_2})}^2+\sum _{n=2}^{N-2}{(u_{2_n}-u_{2_{n+1}})}^2+{\displaystyle \frac{1}{2}}{(u_{2_{N-1}}-u_{2_N})}^2\right)+\hfill \\ \hfill +& \sum _{n=1}^N(z_{w_n}·z_{0_n}-z_{w_n}·z_n)+\hfill \\ \hfill +& \sum _{n=1}^{N-1}\sqrt{{\left(x_{1_n}-x_{1_{n+1}}\right)}^2+{\left(x_{2_n}-x_{2_{n+1}}\right)}^2}+q\hfill \end{array}$

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 $\overrightarrow{C_x}$ and $\overrightarrow{C_y}$.

(33)$\begin{array}{cc}\hfill \overrightarrow{C_x}=\{& 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,18,19,20,21,\hfill \\ \hfill & 22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39\}\hfill \\ \hfill \overrightarrow{C_y}=\{& 23,23,23,23,23,23,23,23,23,23,23,23,24,25,25,24,23,22,21,20,\hfill \\ \hfill & 19,18,17,16,15,14,14,14,14,14,15,15,15,15,14,13,13,13,13,13\}\hfill \end{array}$

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 $R_0$ of the truck over time.

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 $u_1$ representing the truck’s velocity and the state variable $x_3\equiv {\varphi }_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.

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.

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.

Finding the complete solution requires approximately 364 s, which represents a marked improvement over the reference solution. Although the authors did not specify 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.

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.

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 only available in the Matlab environment. The table shows both single-core and multi-core values obtained with the GeekBench CPU benchmarking utility.

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, 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 forward 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 accommodate 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, additional 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 comprehensively. 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 trajectory 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 maneuverability 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.: methodology, writing—original draft; F.K.: methodology, visualization, data curation; Z.S.: editing, supervision. All authors have read and agreed to the published version of the manuscript.

Data Availability Statement

Dataset available on request from the authors.

Conflicts of Interest

The authors declare no conflicts of interest.

Footnotes

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.

Figures and Tables

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

Figure 2. System of a truck and two trailers.

Figure 3. Node graph with initial and final states.

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

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

Figure 6. Comparison of the trajectories (detail).

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

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

Table 2

Numerical comparison of the proposed solution and reference solution.

Comparison Indicator	Proposed Solution	Reference 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.

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

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

References

1. Chang, T.; Tian, G. Hybrid A-Star Path Planning Method Based on Hierarchical Clustering and Trichotomy. Appl. Sci.; 2024; 14, 5582. [DOI: https://dx.doi.org/10.3390/app14135582]

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, pp. 414-425. [DOI: https://dx.doi.org/10.1109/TIV.2019.2960943]

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; [DOI: https://dx.doi.org/10.48550/arXiv.1902.06201] arXiv: 1902.06201

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. Proceedings of the ASME 2018 Dynamic Systems and Control Conference, DSCC 2018; Atlanta, GA, USA, 30 September–3 October 2018; Volume 3, [DOI: https://dx.doi.org/10.1115/DSCC2018-9195]

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, pp. 241-246. [DOI: https://dx.doi.org/10.5194/isprs-archives-XLII-2-W7-241-2017]

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, pp. 1105-1118. [DOI: https://dx.doi.org/10.1109/TCST.2008.2012116]

8. Divelbiss, A.W.; Wen, J.T. Trajectory tracking control of a car-trailer system. IEEE Trans. Control Syst. Technol.; 1997; 5, pp. 269-278. [DOI: https://dx.doi.org/10.1109/87.572125]

9. Xu, H.; Yu, Z.; Lu, X.; Wang, S.; Li, S.; Wang, S. Model predictive control-based path tracking control for automatic guided vehicles. 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. [DOI: https://dx.doi.org/10.1109/CVCI51460.2020.9338578]

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. 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. [DOI: https://dx.doi.org/10.1109/ITSC.2017.8317775]

13. Ghilardelli, F.; Lini, G.; Piazzi, A. Path Generation Using η⁴-Splines for a Truck and Trailer Vehicle. IEEE Trans. Autom. Sci. Eng.; 2013; 11, pp. 187-203. [DOI: https://dx.doi.org/10.1109/TASE.2013.2266962]

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, pp. 659-670. [DOI: https://dx.doi.org/10.2478/v10006-011-0052-8]

15. Mirjalili, S.; Mirjalili, S.M.; Lewis, A. Grey wolf optimizer. Adv. Eng. Softw.; 2014; 69, pp. 46-61. [DOI: https://dx.doi.org/10.1016/j.advengsoft.2013.12.007]

16. Mirjalili, S.; Lewis, A. The whale optimization algorithm. Adv. Eng. Softw.; 2016; 95, pp. 51-67. [DOI: https://dx.doi.org/10.1016/j.advengsoft.2016.01.008]

17. Meraihi, Y.; Ramdane-Cherif, A.; Acheli, D.; Mahseur, M. Dragonfly algorithm: A comprehensive review and applications. Neural Comput. Appl.; 2020; 32, pp. 16625-16646. [DOI: https://dx.doi.org/10.1007/s00521-020-04866-y]

18. Salimi, H. Stochastic fractal search: A powerful metaheuristic algorithm. Knowl.-Based Syst.; 2015; 75, pp. 1-18. [DOI: https://dx.doi.org/10.1016/j.knosys.2014.07.025]

19. Rocha, A.M.A.; Fernandes, E.M. A Stochastic Augmented Lagrangian Equality Constrained-Based Algorithm for Global Optimization. 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. [DOI: https://dx.doi.org/10.1063/1.3498657]

20. Li, B.; Shao, Z. Precise trajectory optimization for articulated wheeled vehicles in cluttered environments. Adv. Eng. Softw.; 2016; 92, pp. 40-47. [DOI: https://dx.doi.org/10.1016/j.advengsoft.2015.10.008]

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. [DOI: https://dx.doi.org/10.1177/1729881419852201]

22. Mathworks. Fmincon. 2021; Available online: https://www.mathworks.com/help/optim/ug/fmincon.html (accessed on 13 November 2024).

23. Zadachyn, V.; Dorokhov, O. Calculation of optimal path for parallel car parking. Transp. Telecommun.; 2012; 13, 303. [DOI: https://dx.doi.org/10.2478/v10244-012-0026-4]

24. Lin, L.; Zhu, J.J. Trajectory Generation From Paths for Autonomous Ground Vehicles. Proceedings of the Dynamic Systems and Control Conference; Virtual, Online, 5–7 October 2020; ASME: New York, NY, USA, 2020; Volume 84287, V002T31A004. [DOI: https://dx.doi.org/10.1115/DSCC2020-3300]

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. Proceedings of the Communications in Computer and Information Science; Chennai, India, 20–22 April 2023; Volume 2202 CCIS, pp. 32-44. [DOI: https://dx.doi.org/10.1007/978-3-031-73065-8_3]

27. REX Controls. Autonomous Reversing T3T. 2020; Available online: https://www.rexcontrols.com/autonomous-reversing (accessed on 13 November 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, pp. 14743-14756. [DOI: https://dx.doi.org/10.1109/TITS.2023.3321415]