An Artificial Intelligence Approach for the Kinodynamically Feasible Trajectory Planning of a Car-like Vehicle

Abstract

This work investigates the possibility to improve the computational efficiency of a set-based method for the trajectory planning of a car-like vehicle through artificial intelligence. Planning is performed on a graph that represents the operating scenario in which the vehicle moves, and the kinodynamic feasibility of the trajectories is guaranteed through a series of set-based arguments, which involve the solution of semi-definite programming problems. Navigation in the graph is performed through a hybrid A* algorithm whose performance metrics are improved through a properly trained classificator, which can forecast whether a candidate trajectory segment is feasible or not. The proposed solution is validated through numerical simulations, with a focus on the effects of different classificators features and by using two different kinds of artificial intelligence: a support vector machine (SVM) and a long-short term memory (LSTM). Results show up to a 28% reduction in computational effort and the importance of lowering the false negative rate in classification for achieving good planning performance outcomes.

1. Introduction

Among the various application areas, autonomous driving stands out for its strategic importance and the high level of innovation it demands. Trajectory planning consists of finding a sequence of positions, each one associated with a timed instant, from a starting position to a destination one [1]. It constitutes a key methodology across a wide range of technological domains, including robotics, advanced logistics, vehicular traffic, and the management of complex systems in dynamic environments, leading to intelligent urban transport networks. Industrial applications, such as the control of robotic arm movements [2,3,4] or the planning of routes for autonomous vehicles in automated warehouses [5,6], highlight the importance of optimizing trajectories under physical, dynamic, and safety constraints. In the evaluation of trajectories, another aspect that could be interesting to consider is the level of risk of the route, linked to environmental conditions, such as population density and hydrogeological risk. For example, an integrated risk model was proposed in [7]. Another area of growing interest lies in the management and prediction of airflow trajectories in confined environments [8,9], such as ventilation systems or environmental control frameworks. Here, trajectory planning principles can be applied to regulate flows efficiently, ensuring energy optimization and environmental comfort while addressing constraints similar to those found in mobility systems. These diverse contexts, along with many emerging applications, underscore the pivotal role of trajectory planning as a cross-disciplinary tool for tackling complex challenges. All the more so, this takes on primary importance in the context of smart cities, where the use of IoT technologies is strongly oriented toward process automation with the dual objective of maximizing energy efficiency and quality of service [10]. In this context, the European Union, through its latest directives, has set a clear goal of reducing greenhouse gasses emissions: initially by 30% and later increased to 55% by 2030 compared to 2021 levels. Achieving such an ambitious target cannot be reasonably accomplished without the widespread adoption of electric vehicles, the conscious and intelligent use of energy storage systems for electric traction [11], and the implementation of assisted and/or autonomous driving technologies.

The main push arises from its expected benefits, which include reducing incidents (94% of which are related to driver errors [12]) and emissions [13]. After years of extensive research, the SAE J3016[14] standard was finally published in 2014. This standard outlines six levels of vehicle automation (ranging from 0 to 5) for classification purposes. The third level, as defined by the SAE J3016, does not require constant human supervision and has been commercially available since the late 2010s. This level of automation is capable of performing complex safety-related tasks like lane-keeping with minimal human involvement [15]. Autonomous driving is a complex and interdisciplinary effort that involves finding solutions to a variety of challenges, particularly in urban environments [16]. The decision-making process in autonomous driving systems (ADSs) typically involves a layered approach that includes a local motion planning layer, which depends on the feedback control of vehicle systems [17]. This work tackles local motion planning, whose input is a structured environment in which both static and moving obstacles are represented by rectangular embeddings [18,19,20], hence assuming that a low-level control law exists, along with environment sensing and goals from the deliberative layer. Local motion planning is a foundational technology for autonomous driving, supporting the already commercially available J3016 level 2 features like self-parking, as well as level 3 applications where the automated driving system (ADS) handles intricate tasks within a specified operational design domain (ODD) with a human fallback, such as overtaking obstacles or slower vehicles. Motion planning for an autonomous driving system (ADS) usually consists of two phases [21,22]. The first phase is path planning, where an appropriate geometric path is determined between a starting position and a destination. The second phase involves associating this path with a suitable timing law [1]. From a computational perspective, these two phases can present significant challenges [23].

Numerous solutions have been proposed in the literature [24], many of which have been adapted from robotic motion planning techniques [25] or share similarities with Unmanned Aerial Vehicle (UAV) motion planning to some extent [26,27]. However, unlike robotic motion, autonomous vehicles have greater demands when it comes to performance and safety. Motion planning solutions can be categorized based on how they ensure the feasibility of trajectories, particularly in terms of adhering to specific constraints. For instance, some solutions are grounded in geometric considerations, frequently utilizing Bézier curves or Lagrange polynomials [28] or ad hoc Dubin paths, which can embed complex constraints [29]. Other methods involve graph-based solutions that construct decision trees of potential trajectories, which are selected based on a cost function that can target different aspects of planning, ranging from fuel consumption [30] to passengers’ comfort [31]. There are also approaches based on model-predictive control (MPC) [32]. In the realm of graph-based methods, optimization algorithms such as Dijkstra’s algorithm or A* and its variants [33] are notable for their applicability to replanning [34]. These algorithms can utilize complex heuristic functions that consider various factors, including penalties for risky maneuvers [35]. However, these approaches typically yield [24] a straightforward geometric path that must later be accompanied by an appropriate timing law in a subsequent planning phase. In the existing literature, these two processes are often treated separately, focusing on one phase at a time. Nevertheless, for instance in [36], the challenge of determining a suitable timing law has been addressed as part of the path planning problem by incorporating an auxiliary time coordinate within the configuration space. This paper, however, proposes a method that integrates both steps, as was done in some of the authors’ previous works; see [1] and the references therein. Further classification of existing solutions depends on how the configuration space for planning is discretized using various methods. These methods aim to generate a set of candidate collision-free points, cells, or lattices [24] for the planning process. Approaches include probabilistic roadmaps [37], vector field histograms—which have shown robustness against uncertainty in [38]—and the widely used [22] visibility graph method. The latter is specifically adapted for autonomous vehicle systems (AVSs) in [39] by accurately defining obstacle approximations that consider the kinematics of AVSs. Various techniques can be integrated with different approximations of obstacles or no-go areas, such as rectangular shapes [40] or ellipsoids [41]. These approaches can successfully handle, to a certain extent, replanning in unstructured scenarios [42]. In addition to the classification previously presented, motion planning algorithms for vehicles frequently depend on the definition of worst-case state sets [17] that are proven to be collision-free and consider various phenomena that may affect vehicle performance. Finally, the interested reader can find further information in the survey [43]. This study employs a solution tested on robots [1], which is characterized by its ability to ensure specific constraints on vehicle states (e.g., maximum lateral velocity) that are often recommended for defining the Operational Design Domain (ODD) and enhancing driving comfort. Even before the explosion of interest in artificial intelligence, robotics and autonomous driving technologies have benefited from automated learning mainly in the form of supervised learning. Among the first two categories, most AI-based solutions for motion planning deal with environment sensing [44] and path planning rather than trajectory planning [45]. For example, ref. [46] relied on two different neural networks, one of which was used for path planning and another of which providea a collision avoidance feature to the motion of a ground vehicle. Even further, in planning, the greatest part of artificial intelligence-based solution focuses on the deliberative layer, for example, ref. [47], wherein they relied on model predictive control, with a focus on providing a natural driving style, and [48] which reached similar results in a line-change scenario. In [49], a hybrid-A* was proposed in which the heuristic function of the A* algorithm was improved through an artificial intelligence approach, even though it was limited to path planning. Only a handful works combine kinodynamic feasibility with artificial intelligence for improving search speed. For example, an interesting approach can be found in [50], wherein they performed the trajectory planning of a car, performed through an A* in a 4D space, by pruning the search space through a support vector machine (SVM), while [51] focused on solving the inherently nonprovable safety of trajectories obtained by an artificial intelligence model. Apart from supervised and unsupervised learning, reinforcement learning (RL)-based approaches offer interesting advantages in terms of adaptability. For example, ref. [52] introduced a deep RL layered framework in which planning at the higher level is performed through $A^{*}$; then, in proximity of the target, the lower-level policy builds a reachability graph through previously learned policies. In [53] RL was used for long-term planning; ref. [54] shifted the RL paradigm by considering the rollout of the model as a decision-making problem through considering the model itself as a decision maker and the current policy as the dynamics, showing good adaptability while tackling uncertainty. These approaches, however, do not focus on kynodynamic feasibility. Furthermore, existing approaches are often tightly coupled with the specific control technique used in trajectory tracking, while the proposed approach, instead, only requires a few hypotheses about the closed loop model of the system.

This work stems out of a series of previous works by the authors, which were focused on kinodynamically feasible trajectory planning for skid-steered tracked mobile robots that had been experimentally validated in [1] and later applied to heterogeneous vehicles, such as cars (see [55]). While the previous works were focused on finding and proving the feasibility of trajectories, this work, instead, assumes that such a procedure exists and focuses on improving planning. Without loss of generality, this work uses the authors’ results in trajectory planning for the training of a classifier that can speed up planning by giving precedence to the exploration of the most likely feasible candidate trajectories. The main novelties and enhancements to the existing literature are the following:

• It embeds provable kinodynamic feasibility in artificial intelligence-planned trajectories;
• It offers a sensitivity analysis of the most important parameters of the AI training with respect to the planning;
• It offers an analysis of some of AI feature effects upon planning.

This work is structured as follows. In Section 2, the considered trajectory planning problem is presented. In Section 2.1, a vehicle system model is discussed, highlighting its features that are of interest in terms of trajectory planning, while Section 2.2 formalizes the required mathematical structure of system model and constraints for planning. Then in Section 2.3, the set-based procedure for proving the feasibility of a trajectory is described. In Section 3, the planning algorithm is introduced, while in Section 3.1, the proposed AI-based enhancement is presented, followed by its validation in Section 4 and discussion in Section 5.

2. Trajectory Planning Problem

Suppose a two-dimensional operational scenario $\Delta \subseteq {\mathcal{R}}^2$ that can be discretized by a grid of safe positions of finite size ${\Delta }_D$. Let us assume the undirected weighted graph $\mathcal{G}$, whose nodes represent the points in ${\Delta }_D$.

Let us consider two generic points $M=(x_1,y_1)$ and $N=(x_2,y_2)$ in ${\Delta }_D$ that are closer than $\overline{L}$ and whose connecting segment is fully contained in $\Delta$. Thus, if ${\mathcal{G}}_M$ and ${\mathcal{G}}_N$ are the nodes of $\mathcal{G}$ associated, respectively, to points M and N, then $T_{MN}$ is defined as the arc of the graph $\mathcal{G}$ that links ${\mathcal{G}}_M$ and ${\mathcal{G}}_N$. Moreover, $T_{MN}$ represents the segment of the trajectory $MN$ of length $l_{MN}$ that is traversed at a constant speed $\overline{V}$. Consider $n_{MN}=⌈{\displaystyle \frac{l_{MN}}{\overline{V}{T}_S}}⌉$, i.e., the rounded number of time steps associated with the $T_{MN}$ trajectory segment, and let $l_{MN}$ be the weight of the arc $T_{MN}$. Hence, we have the following definition:

$$\mathcal{G}=\left(\left\{{\mathcal{G}}_A,\dots ,{\mathcal{G}}_N,N\in {\Delta }_D\right\},\left\{T_{AB},T_{AC},\dots ,T_{MN},\overline{MN}\in \Delta \right\}\right)$$

(1)

Let the nodes ${\mathcal{G}}_A,{\mathcal{G}}_B,{\mathcal{G}}_C$ be associated with the points $A,B,C\in {\Delta }_D$. And assume that the two arcs $T_{AB}$ and $T_{BC}$ (associated with segments $\overline{AB}$ and $\overline{BC}$) both exist and are part of a trajectory, as shown in Figure 1.

Consider the intervals $\left[t_A,t_A+n_{AB}·T_s\right]$ and $\left(t_B,t_B+n_{BC}·T_s\right]$, which represent the time frames during which the vehicle traverses the trajectory segments $AB$ and $BC$, respectively. Since these segments are contiguous, the following temporal relationship holds:

$$t_B=t_A+n_{AB}·T_S$$

(2)

At the moment $t_B$, the vehicle’s position is $(x\left(t_B\right),y\left(t_B\right))$, and its orientation is $\theta \left(t_B\right)$. These quantities are expressed in a local reference frame centered at point A, where segment $AB$ is aligned with the x axis. At $t_B$, a new local reference frame is established at point B, which is oriented according to ${\theta }_{BC}$. Given that the segment $BC$ with orientation ${\theta }_{BC}$ must be traversed at velocity $\overline{V}$, the state of (19) at $t_B$ undergoes the following instantaneous update:

$$\Pi ={[x_D\left(t_B\right)-x_B,0,-{\theta }_{BC}]}^T$$

(3)

where $x_D\left(t_B\right)$ denotes the desired position along segment $AB$ at time $t_B$. Refer to Figure 1 for additional details.

Let us thus introduce the following condition:

$${\delta }_z\left(t_B\right)+\Pi \in {\Gamma }_0$$

(4)

with $\delta z\left(t_B\right)$ being the state of the system at the time instant $t_B$ and ${\Gamma }_0$ being the robust positively invariant region [56] associated with its dynamics. If (4) is held, the sequence of trajectory segments corresponding to arcs $T_{AB}$ and $T_{BC}$ is deemed admissible, enabling the evaluation of another candidate trajectory segment.

2.1. System Model

For trajectory planning purposes, a suitable mathematical description of the dynamics of the vehicle must be used. The modeling solutions that can be found in the literature lie in a trade-off between complexity and descriptivity, ranging from simple kinematic models (which show a good level of accuracy for low-speed cases [17]) to increasingly complex ones, which embed the description of a greater share of physical phenomena [57]. For the purposes of this work, the model presented in [58] was used. The model relies on a few simplifying assumptions, namely, the following:

• The longitudinal forces affecting the tires are due to brakes and engine torque only;
• The considered vehicle uses front-wheel drive, with an equal distribution of torque between front tires, and the braking torque is, instead, evenly distributed among all four tires;
• Tire aligning torques are neglected;
• Dissipative force is made up of drag, rolling friction, and gravity force.

Further assumptions are made in this work:

• The slope is considered as an uncertain parameter;
• The vehicle forward velocity is constant.

Let us introduce the kinematic description of the trajectory tracking. Through some straightforward geometric consideration, the subsequent kinematic model can be formulated to represent the motion of the vehicle:

$$\left[\begin{array}{c}{\dot{x}}_E\\ {\dot{y}}_E\\ {\dot{\theta }}_E\end{array}\right]=\left[\begin{array}{ccc}cos\left({\theta }_E\right)& & 0\\ sin\left({\theta }_E\right)& & 0\\ 0& & 1\end{array}\right]\left[\begin{array}{c}{v_x}_E\\ {\omega }_E\end{array}\right]$$

(5)

where $x_E$, $y_E$, and ${\theta }_E$ define the pose of the vehicle in the inertial reference frame (E), ${v_x}_E$ is the longitudinal forward velocity, and the yaw rate ${\omega }_E$ is considered positive if it is counterclockwise. For trajectory tracking, Equation (5) is expressed in a local reference frame (L) centered in $\{{\overline{x}}_E,{\overline{y}}_E\}$ and oriented with angle ${\overline{\theta }}_E$ see Figure 2.

Thus, let us define the position of the vehicle in L through the following roto-translation:

$$\left[\begin{array}{c}{x}_L\\ y_L\\ {\theta }_L\end{array}\right]=\left[\begin{array}{ccc}cos\left({\overline{\theta }}_E\right)& sin\left({\overline{\theta }}_E\right)& 0\\ -sin\left({\overline{\theta }}_E\right)& cos\left({\overline{\theta }}_E\right)& 0\\ 0& 0& 1\end{array}\right]·\left[\begin{array}{c}{x}_E-{\overline{x}}_E\\ y_E-{\overline{y}}_E\\ {\theta }_E-{\overline{\theta }}_E\end{array}\right]$$

(6)

where $x_L$, $y_L$, and ${\theta }_L$ are the pose of center of mass of the vehicle in the local reference frame. Let the desidered pose be ${\left[\begin{array}{c}{x}_L^D{y}_L^D{\theta }_L^D\end{array}\right]}^T$, and the let $V_D$ be the nominal forward velocity associated with a trajectory segment; the tracking error in L, centered in $\{{\overline{x}}_E,{\overline{y}}_E\}$ and oriented according to the angle ${\overline{\theta }}_E$, becomes the following:

$$\left[\begin{array}{c}\dot{e_x}\\ \dot{e_y}\\ \dot{e_{\theta }}\end{array}\right]=\left[\begin{array}{cc}cos\left(e_{\theta }\right)& 0\\ sin\left(e_{\theta }\right)& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{v}_x\\ \omega \end{array}\right]-\left[\begin{array}{c}{V}_D\\ 0\\ 0\end{array}\right]$$

(7)

where $e_x=x_L-x_L^D$, $e_y=y_L-y_L^D$, and $e_{\theta }={\theta }_L-{\theta }_L^D$.

In [58], the dynamics of the vehicle are described in terms of a nonlinear system whose state vector is the following:

$$z={\left[v_x{v}_y\omega xy\theta \right]}^T$$

(8)

and whose input variables are

$$u={\left[TP\beta \right]}^T$$

(9)

i.e., the torque provided by the motor T, the braking force P, and the steering angle $\beta$, and the road–tire friction coefficient $\mu$ is considered as an unknown but bounded external input:

$$\dot{z}\left(t\right)=\tilde{A}(z\left(t\right),u\left(t\right))z\left(t\right)+\tilde{B}(z\left(t\right),u\left(t\right))u\left(t\right)+{\tilde{B}}_d\left(t\right)\mu \left(t\right)$$

(10)

with $\tilde{A}$, $\tilde{B}$, and ${\tilde{B}}_d$ being matrices of proper dimensions.

Let ${\mu }_N$ be the nominal friction coefficient value and $T_N$ be the torque required to hold the forward speed $V_D$. Using the classical linearization arguments, the behavior of the system (10) around the nominal condition $\overline{z}={\left[V_{D}00000\right]}^T$, $\overline{u}={\left[T_{N}00\right]}^T$, $\overline{\mu }={\mu }_N$ is described by the following linear time-invariant model, which can be written as follows:

$${\dot{\delta }}_z\left(t\right)=A_C{\delta }_z\left(t\right)+B_C{\delta }_u\left(t\right)+B_{dC}{\delta }_{\mu }\left(t\right)$$

(11)

with $A_C$, $B_C$, and $B_{dC}$ being matrices of proper dimensions.

With $T_s>0$ being the uniform sampling step, it is possible to obtain the following discrete-time mathematical model:

$${\delta }_z(k+1)=A_D{\delta }_z\left(k\right)+B_D{\delta }_u\left(k\right)+B_{dD}{\delta }_{\mu }\left(k\right)$$

(12)

where $A_D=e^{A_C{T}_S}$, $B_D={\int }_0^{T_s}{e}^{A_c\tau }{B}_{C}d\tau$, and $B_{dD}={\int }_0^{T_s}{e}^{A_c\tau }{B}_{dC}d\tau$. As previously introduced, this work assumes that some elements of the model (12) may be subject to uncertainties. In particular, the mass of the vehicle (which also affects the moment of inertia around the yaw axis) and cornering stiffness of the tires are assumed to fall withing a bounded interval. Such uncertainty can be conveniently embedded in a polytopic mathematical representation [59] through the following convex hull:

$$Co\{{\psi }_1,{\psi }_2,\cdots {\psi }_{N_v}\}:=\left\{\sum _{n=1}^{N_v}{\alpha }_n{\psi }_n|\sum _{n=1}^{N_v}{\alpha }_n=1\right\}$$

(13)

with $N_v$ vertices and ${\psi }_n=(A_{Dn},B_{Dn},B_{dD})$ being its n-th vertex.

2.2. Constrained Control Problem

Let us assume that system (13) must abide by the the following ellipsoidal constraints for state and actuation:

$${\delta }_z\in {\Omega }_z,{\Omega }_z=\{{\delta }_z\in {\mathcal{R}}^{n_z}:{\delta }_z^T{S}_z{\delta }_z\le 1,S_z>0\}$$

(14)

$${\delta }_u\in {\Omega }_u,{\Omega }_u=\{{\delta }_u\in {\mathcal{R}}^{n_u}:{\delta }_u^T{S}_u{\delta }_u\le 1,S_u>0\}$$

(15)

Moreover, let us assume that the following bounds apply to the external disturbance ${\delta }_{\mu }$:

$${\delta }_{\mu }\in {\Omega }_{\mu },{\Omega }_{\mu }=\{{\delta }_{\mu }\in {\mathcal{R}}^{n_{\mu }}:{\delta }_{\mu }^T{M}_{\mu }{\delta }_{\mu }\le 1,M_{\mu }>0\}$$

(16)

The system closed-loop dynamics indeed must take into account the relevant control law which, without loss of generality, is assumed in this work to have the following structure:

$${\delta }_u\left(k\right)=K·{\delta }_z\left(k\right)$$

(17)

It is also assumed that it satisfies, for the system (13), the constraints (14) and (15) and that it guarantees quadratic stability in the positively invariant ellipsoidal region:

$${\Gamma }_0=\{{\delta }_z\in {\mathcal{R}}^{n_z}:{\delta }_z^T{P}_0{\delta }_z\le 1,P_0>0\}$$

(18)

which can be found using different procedures, such as the one described in [56].

Thus, taking into account (17), the closed-loop system can be rewritten in the following polytopic form:

$${\delta }_z(k+1)={\Phi }_n{\delta }_z\left(k\right)+B_{dD}{\delta }_{\mu }\left(k\right),n=1\cdots N_v$$

(19)

being ${\Phi }_n=A_{Dn}+B_{Dn}K$ with $n=1\cdots N_v$.

2.3. Trajectory Feasibility

The search for a feasible trajectory for a vehicle with closed-loop dynamics described by (19) and constrained by (14)–(16) can be performed using the methodology developed in [1].

In particular, to address the uncertainties of the model (19), the influence of disturbances (16) and the uncertainty in the initial state ${\delta }_z\left(t_A\right)\in S_A$, where the set $S_A$

$$S_A=\{{\delta }_z\in {\Gamma }_0:{\delta }_z^T{P}_A{\delta }_z\le 1,P_A\ge 0\}$$

(20)

lets us calculate, following the procedure reported in Appendix A, the set

$$S_{n_{AB}}=\{{\delta }_z\in {\Gamma }_0,{\delta }_z^T{P}_{n_{AB}}{\delta }_z\le {\gamma }_{n_{AB}},P_{n_{AB}}\ge 0,{\gamma }_{n_{AB}}\ge 0\}$$

(21)

which accounts for the projection of $S_A$ at time $t_B$ while incorporating the uncertainties in vehicle dynamics (19) and allowable disturbances (16).

For the trajectory segment $T_{BC}$ to follow $T_{AB}$, the following Minkowski sum condition must hold:

$$S_{N_{AB}}\oplus \Pi \subseteq {\Gamma }_0$$

(22)

Under this condition, Equation (4) is satisfied regardless of the initial state ${\delta }_z\left(t_A\right)\in S_A$ and for any disturbance ${\delta }_{\mu }\in {\Omega }_{\mu }$. This guarantees the feasibility of transitioning between the two trajectory segments, since it means, in practical terms, that the state of the vehicle will not leave the positively invariant region ${\gamma }_0$ associated with the constraints. If all trajectory segment sequences meet the admissibility criteria, the entire trajectory is validated as feasible.

Moreover, condition (22) can be reformulated as a Linear Matrix Inequality (LMI) feasibility problem. Specifically, (22) is satisfied if there exists a scalar $\tau >0$ such that the following LMI is valid:

$$\left[\begin{array}{cc}1-{\Pi }^T{P}_0\Pi -\tau {\gamma }_{n_{AB}}& -{\Pi }^T{P}_0\\ \ast & -P_0+\tau P_{n_{AB}}\end{array}\right]\ge 0$$

(23)

The proofs are provided in [1].

3. Feasible Trajectory Planning Algorithm

Let the two nodes ${\mathcal{G}}_S$ and ${\mathcal{G}}_F$ represent the starting point S and end point F of a trajectory. A straightforward choice to find (if it exists) the shortest trajectory between them is using the classical $A^{*}$ algorithm, whose output is a sequence of segments connecting S and F. Such segments are, according to the planning procedure which is used in this work, traversed at a constant forward velocity $\overline{V}$.

The $A^{*}$ algorithm employs a heuristic function to evaluate the trajectory linking the initial node ${\mathcal{G}}_S$ to an arbitrary node ${\mathcal{G}}_A$. This trajectory, denoted by $cSA$, comprises the sequence of segments connecting S to A at a predefined speed. The length of this trajectory is represented as $l\left(cSA\right)$. Using the notation introduced earlier, the heuristic function is defined as follows:

$$f(c_{SA},T_{AB},{\mathcal{G}}_B,{\mathcal{G}}_F)=l\left(c_{SA}\right)+l_{AB}+\parallel B-F\parallel$$

(24)

where $|·|$ represents the Euclidean norm. This function estimates a lower bound on the total length of the path from S to the final node F, incorporating the segments $c_{SA}$ and $T_{AB}$. Based on (24), the solution to the feasible trajectory planning problem is implemented as a recursive algorithm. The process, starting from the initial node ${\mathcal{G}}_S$ and its associated tracking error $S_S$, is summarized as follows:

• Identify and evaluate the adjacent node ${\mathcal{G}}_A$ with the most favorable heuristic value relative to ${\mathcal{G}}_S$;
• Compute the projection of $S_S$ to the end point of the trajectory segment $T_{SA}$;
• If condition (22) is satisfied and A is not the destination node, calculate $S_A⨁{\Pi }_{SA}$ and repeat the process.

The resulting algorithm is described by the following pseudocode (Algorithm 1).

Algorithm 1 The pseudocode for the planning algorithm.

1: $c_{opt}=⌀$;   2: $c_{exp}=⌀$;   3: $dup=false$   4: ${\mathcal{G}}_A={\mathcal{G}}_S$;   5: function trajectoryPlanner($\mathcal{G},S_A,{\mathcal{G}}_A,{\mathcal{G}}_F,c_{opt},c_{exp},dup$)   6:     $S_A\leftarrow \mathrm{initial}\mathrm{tracking}\mathrm{error}\mathrm{as}\mathrm{defined}\mathrm{in}\left(20\right)$;   7:     ${\mathcal{G}}_A\leftarrow \mathrm{current}\mathrm{node}$;   8:     $c_{opt}\leftarrow \mathrm{candidate}\mathrm{optimal}\mathrm{trajectory}$;   9:     $c_{exp}\leftarrow \mathrm{trajectory}\mathrm{being}\mathrm{explored}$; 10:   for all arcs $T_{AB}$ whose origin is ${\mathcal{G}}_A$, if $\neg ({\mathcal{G}}_B\in c_{exp}\wedge dup)$ do 11:         if $f(c_{exp},T_{AB},{\mathcal{G}}_B,{\mathcal{G}}_F)$ as specified in (24) < $l\left(c_{opt}\right)$ then 12:            Given $S_A$ find $S_{n_{AB}}$ according to Appendix A 13:            if $S_{n_{AB}}⨁\Pi \subseteq {\Gamma }_0$ then 14:                append $T_{AB}$ to $c_{exp}$ 15:                if ${\mathcal{G}}_B$ is the destination node then 16:                    $c_{opt}=c_{exp}$ 17:                else 18:                    if ${\mathcal{G}}_B\in c_{exp}$ then 19:                        $dup=true$ 20:                    end if 21:                    trajectoryPlanner$(\mathcal{G},S_{n_{AB}},{\mathcal{G}}_B,{\mathcal{G}}_F,c_{opt},c_{exp},dup)$ 22:                end if 23:            end if 24:         end if 25:     end forreturn $c_{opt}$ 26: end function

It should be noted that only one duplicate node per trajectory is allowed through the $dup$ flag so that some trajectories, including a loop—which can be useful in practical scenarios—are possible. While the classical $A^{*}$ algorithm provides an effective foundation for trajectory planning, its heuristic function does not take into account kinodynamic constraints. Its computational complexity in time is, however, in its worst case exponential in the length l (number of nodes) of the shortest path: $O\left(b^l\right)$, with b being the branching factor (i.e., the average number of arcs per node). The heuristics can lower the practical branching factor (i.e., the number of nodes that are actually explored), thus reducing the complexity [60]. Algorithms derived from $A^{*}$ usually try enhancing the heuristic; among them, the hybrid $A^{*}$ adds kinematics-related penalty factors. The following section introduces a hybrid $A^{*}$ algorithm that embeds additional insights into its heuristic function, ensuring both improved efficiency and guaranteed convergence properties by improving the priority in exploring nodes by using properly trained classificators as predictors.

3.1. Artificial Intelligence-Based Heuristic

The $A^{*}$ algorithm has shown strong performances in solving trajectory problems in systems such as videogames, where the overall state of the machine is not subject to the physical and temporal constraints inherent in nonholonomic systems. While classic A* methods can successfully find optimal trajectories in general cases, these trajectories may be suboptimal or infeasible for applications such as autonomous vehicles. Introducing mechanical constraints requires the penalization or favoring of specific configurations, requiring hybrid $A^{*}$ systems to compute the cost of individual states differently.

As demonstrated in [50], the $A^{*}$ algorithm can achieve significant improvements in efficiency by employing an SVM as a heuristic function rather than relying on the standard Euclidean distance heuristic. In this approach, SVMs are utilized to classify different trajectories, defining a nonlinear separation hyperplane that distinguishes acceptable trajectories from nonacceptable ones.

A key advantage of using an SVM as a heuristic in a hybrid $A^{*}$ lies in the significant reduction of search complexity. Traditional heuristics, such as the Euclidean distance, often require the exploration of thousands of nodes in the search space, leading to increased computational time and complexity. Conversely, when an SVM-based heuristic is applied, the search space can be drastically reduced to fewer nodes, particularly when higher values of $\gamma$ (which controls the complexity of the SVM boundary) are used [50]. This reduction occurs because $\gamma$ is a hyperparameter that shapes the decision boundary and influences the complexity of the learned model. Specifically, $\gamma$ determines the influence of individual data points on the decision boundary according to the following kernel formulation:

$$K({\chi }_i,{\chi }_j)=exp{\displaystyle \frac{-\left|\right|{\chi }_i-{\chi }_j{\left|\right|}^2}{2{\gamma }^2}}$$

(25)

The technical advantage of integrating SVMs into the the $A^{*}$ algorithm lies, in the proposed algorithm, in bypassing the need to check the feasibility of unfeasible trajectories. Instead, the SVM provides a priori decision making for each state, thereby shortening the exploration process, through a soft pruning of the search space.

Beyond the search efficiency gains achieved by employing SVMs, the convergence properties of hybrid $A^{*}$ algorithms are crucial for ensuring their practical applicability. To this end, this work proposes an improved hybrid A* heuristic function designed to leverage insights from a classificator that has been trained to identify the most promising (in terms of kinodynamic feasibility) trajectory segments, as detailed below. The proposed heuristics are the following:

$$F(c_{SA},T_{AB},{\mathcal{G}}_B,{\mathcal{G}}_F)=f(c_{SA},T_{AB},{\mathcal{G}}_B,{\mathcal{G}}_F)+k_{f}p(c_{SA},T_{AB},{\mathcal{G}}_B)f(c_{SA},T_{AB},{\mathcal{G}}_B,{\mathcal{G}}_F)$$

(26)

with $k_f\in [0,1]$ and the prediction $p(c_{SA},T_{AB},{\mathcal{G}}_B)\in \{0,1\}$. The prediction p is the binary output of a properly trained classifier, and its value is 0 if the trajectory segment $T_{AB}$ is classified as nonadmissible, whereas it is 1 if otherwise. The coefficient $k_f$ (whose positiveness ensures the admissibility of the heuristics) is a weight factor that dictates the effect of p: when $k_f=0$, p does not affect the order in which adjacent nodes are explored, while $k_f=1$ moves nodes with $p=1$ to the top of the list of nodes to be explored. The following subparagraphs will focus on the way p is found.

3.2. SVM-Improved Heuristics

The SVM classifier employs a polynomial kernel to classify the eligibility of a trajectory segment stating the nonlinear relationships between three predictors:

• The length of the candidate segment;
• The change in direction;
• The x value in the relevant $\Pi$ vector.

The SVM decision function is expressed as

$$f\left(\chi \right)=\sum _{i=1}^N{\alpha }_i{\upsilon }_{i}K({\chi }_i,\chi )+b$$

(27)

Therein, we have the following:

• $\chi =\{{\chi }_1,\dots {\chi }_i\}$ is the input feature vector;
• N is the number of support vectors;
• ${\alpha }_h$ defines the Lagrange multipliers for each support vector;
• ${\upsilon }_g\in \{0,1\}$ defines the class labels of the support vectors, determining whether the input is acceptable or not for a trajectory;
• $K({\chi }_i,\chi )$ is the polynomial kernel function;
• b is the bias term.

The sequential minimal optimization (SMO) algorithm is used to solve the convex optimization problem in SVMs due to the nonlinear assumption of the problem. The dual form of the soft-margin SVM is expressed as

$$\underset{\alpha }{max}\sum _{i=1}^N{\alpha }_i-{\displaystyle \frac{1}{2}}\sum _{i=1}^N\sum _{j=1}^N{\alpha }_i{\alpha }_j{\upsilon }_i{\upsilon }_{j}K({\mathsf{Ø}}_i,{\mathsf{Ø}}_j)$$

(28)

It is subject to the following constraints:

$$\sum _{i=1}^N{\alpha }_i{\upsilon }_i=0\mathrm{and}0\le {\alpha }_i\le C$$

(29)

where ${\alpha }_i$ defines the Lagrange multipliers associated with the support vectors, and $K({\mathsf{Ø}}_i,{\mathsf{Ø}}_j)$ is the second-order polynomial kernel. SMO iteratively solves two ${\alpha }_i$ values at a time until meeting its convergence criterion. The misclassification cost is adopted here in the dual form by modifying the bounds of the corresponding Lagrange multipliers $\left({\alpha }_i\right)$ by class-specific penalty terms $\left(C_i\right)$. By assigning different $\left(C_i\right)$ values to different classes, the optimization process penalizes misclassifications of certain classes more than others. This adjustment indirectly modifies the influence of the support vectors and thus the decision boundary during optimization.

3.3. LSTM-Improved Heuristics

The features allowing classification or regression using classical machine learning approaches have the major limitation of being manually designed by developers using a combination of domain expertise and iterative experimentation. Cummins et al. [61] proposed a deep neural network capable of learning optimization heuristics, achieving performance improvements of up to 14% over traditional machine learning approaches. In fact, deep learning can extract information directly from raw data without the need for human experts to label the dataset. It can also use transfer learning to adapt knowledge from one optimization problem to another. A long short-term memory (LSTM) neural network is a deep learning-based approach developed by Hochreiter and Schmidhuber [62]. It is a type of recurrent neural network designed to increase the storage capacity of the network and preserve historical information over subsequent time steps. It is particularly well suited to nonlinear prediction problems and is able to capture temporal dependencies by correlating time series data. A four-layer neural network was designed with a sequence input layer to process the sequential data of three features, followed by a bidirectional LSTM (BiLSTM) recurrent layer that processed the input sequence in both forward and backward directions to capture dependencies in the data over time as follows. The bidirectional LSTM (BiLSTM) layer processes the input sequence in both the forward and backward directions to capture temporal dependencies in the data. The output at each timestep t can be expressed as

$$h_t=tanh(W_h·{\chi }_t+b_h)$$

(30)

Therein, we have the following:

• ${\chi }_t$ is the input instance at time step t, where ${\chi }_t=\{{\chi }_{t1},{\chi }_{t2},{\chi }_{t3}\}$ comprises the length of the candidate segment, the change in direction, and the x value in the relevant $\Pi$ vector.
• $W_h$ is the weight matrix associated with the input features.
• $b_h$ is the bias term.
• $h_t$ is the output (hidden state) at time t produced by applying the hyperbolic tangent ($tanh$) activation function.

Next, a fully connected layer is used to classify the data into the target classes, while a softmax is used layer to compute class probabilities using a softmax function as follows:

$$P(y=c\mid \chi )={\displaystyle \frac{e^{z_c}}{{\sum }_j{e}^{z_j}}}$$

(31)

where $z_c$ is the score in terms of the weighted sum of the input features for class c, and j iterates over the two classes (feasible and unfeasible). The LSTM architecture used in this paper is shown in Figure 3.

4. Numerical Results and Discussion

This section illustrates some of the results of a numerical simulation campaign carried out in the MATLAB environment running on an Intel i7-based platform PC. Table 1 lists the relevant parameters that pertain to the simulations. The aim was to carry out a sensitivity analysis against some of the tunable parameters of the proposed architecture.

Section 4.1 describes how the algorithm was trained and the scenario employed. In Section 4.2, we review the algorithm being tested using four different models (model A, B, C, and D) and four couples of destination poses (target A, B, C, and D). The tests in Section 4.3, Section 4.4 and Section 4.5 relied on model A and target A. The subsequent tests did not change neither the destination pose nor the model such that they highlight the effects of a relevant parameter. Namely, in Section 4.3, the effects of the $k_f$ parameter are discussed with tests carried out using the same SVM. In Section 4.4, the effects of the false negative rate and false detection rate are shown while leaving the same accuracy for both classifiers. Finally in Section 4.5, the LSTM-based planning is and its performance is described, discussing the effects of the number of hidden layers in the LSTM.

4.1. Training and Scenario

The planning considered the motion of the vehicle in a 250 m long road segment of a two-lane road that is $7.1$ m wide. Following common procedures in the literature [18,19,20], fixed and moving obstacles (traffic flow) were assumed to be represented by rectangular no-go areas (red boxes in Figure 4). The operational scenario is covered with a grid of 1836 points (graph nodes) and a maximum length of trajectory segments of 10 m, resulting in arcs.

Table 1. System parameters values (vehicle A is taken from [58]) for tests.

Parameter	Value	Unit	Uncertainty (%)
Vehicle A
Vehicle mass	1550	kg	8
Yaw-axis moment of inertia of the vehicle	1260	kg/m2	10
Rear axle tires stiffness	43,300	N/rad	15
Front axle tires stiffness	63,700	N/rad	15
Forward velocity	25	m/s	10
Vehicle B
Vehicle mass	1800	kg	8
Yaw-axis moment of inertia of the vehicle	1500	kg/m2	10
Rear axle tires stiffness	43,300	N/rad	15
Front axle tires stiffness	63,700	N/rad	15
Forward velocity	20	m/s	10
Vehicle C
Vehicle mass	1550	kg	8
Yaw-axis moment of inertia of the vehicle	1260	kg/m2	10
Rear axle tires stiffness	40,000	N/rad	15
Front axle tires stiffness	60,000	N/rad	15
Forward velocity	20	m/s	10
Vehicle D
Vehicle mass	1300	kg	8
Yaw-axis moment of inertia of the vehicle	1056	kg/m2	10
Rear axle tires stiffness	43,300	N/rad	15
Front axle tires stiffness	63,700	N/rad	15
Forward velocity	25	m/s	10

Table 1. System parameters values (vehicle A is taken from [58]) for tests.

Parameter	Value	Unit	Uncertainty (%)
Vehicle A
Vehicle mass	1550	kg	8
Yaw-axis moment of inertia of the vehicle	1260	kg/m2	10
Rear axle tires stiffness	43,300	N/rad	15
Front axle tires stiffness	63,700	N/rad	15
Forward velocity	25	m/s	10
Vehicle B
Vehicle mass	1800	kg	8
Yaw-axis moment of inertia of the vehicle	1500	kg/m2	10
Rear axle tires stiffness	43,300	N/rad	15
Front axle tires stiffness	63,700	N/rad	15
Forward velocity	20	m/s	10
Vehicle C
Vehicle mass	1550	kg	8
Yaw-axis moment of inertia of the vehicle	1260	kg/m2	10
Rear axle tires stiffness	40,000	N/rad	15
Front axle tires stiffness	60,000	N/rad	15
Forward velocity	20	m/s	10
Vehicle D
Vehicle mass	1300	kg	8
Yaw-axis moment of inertia of the vehicle	1056	kg/m2	10
Rear axle tires stiffness	43,300	N/rad	15
Front axle tires stiffness	63,700	N/rad	15
Forward velocity	25	m/s	10

Figure 4. Two planned trajectories: the SVM-ehnanced one (green line) and the non-SVM-enhanced one (blue line). The trajectories start from the circles on the right. The two red boxes denote two forbidden areas (vehicle A and target A).

Figure 4. Two planned trajectories: the SVM-ehnanced one (green line) and the non-SVM-enhanced one (blue line). The trajectories start from the circles on the right. The two red boxes denote two forbidden areas (vehicle A and target A).

The training for the AIs used a dataset consisting of approximately 17,000 instances of the feasible/unfeasible trajectory classes. The dataset was obtained by setting different start and target points with the proper kinds of vehicles and collecting data from the planning procedure using the Euclidean (nonhybrid) $A^{*}$. The collected dataset consists of partial trajectories (i.e., the trajectory from the starting point to a candidate segment) in terms of the following:

• The change in pose (angle and tracking error) associated with each segment of the trajectory;
• The length of each trajectory segment;
• The feasible/unfeasible label for the trajectory followed by a candidate segment.

The SVM training was performed considering just the last segment of a partial trajectory, while the LSTM was trained on the whole sequences from the starting point.

It should be noted that uncertainties regarding the vehicle model and road conditions were taken into account by the feasibility check; thus, they were not explicitly changed while creating the dataset, i.e., for a certain vehicle, if a certain uncertain parameter is considered during modeling, the feasibility of a trajectory is guaranteed (and thus it is not influenced) for any allowable value of the parameter in question.

The data were split into 90% for training and 10% for testing for any trained classifier. For the LSTM, the network was trained using the Adam optimizer for a maximum of 200 epochs, with a learning rate of $0.001$, gradient clipping at a threshold of 1, and the cross-entropy loss function. The data were not shuffled during training, and accuracy was used as the evaluation metric. For the SVM, feature scaling ensured normalization using the mean and standard deviation. The model utilized 64 support vectors to define the decision boundary.

4.2. Algorithm Efficiency

Table 2. Number of LMI calls varying the vehicle and the target.

		Target A	Target B	Target C	Target D
Vehicle A	Baseline	5430	6002	4001	3097
	$K_f=1$	4003	7389	4769	3712
Vehicle B	Baseline	4654	5132	3399	2604
	$K_f=1$	5584	6055	3990	2922
Vehicle C	Baseline	4490	5002	3288	2506
	$K_f=1$	5239	5563	3941	3057
Vehicle D	Baseline	5012	5540	3688	2799
	$K_f=1$	5919	6795	4724	3507

shows the number of LMIs solved in a series of tests while varying the vehicle and the target. The baseline used was the classical $A^{*}$ with Euclidean heuristics, while the tests with $K_f=1$ used the hybrid $A^{*}$ heuristics with the SVM classifier. The improvement ranged from roughly $12\%$ (vehicle B/target D) to $28\%$ (vehicle D/target C).

4.3. Kf Parameter

Figure 4 shows the results of two plannings with vehicle A and target A. The blue line represents the one performed by setting the $K_f$ parameter to 0, while the green line represents a planning with the $K_f$ parameter set to 1. A further planning with the $K_f$ parameter set to $0.5$ was performed, but it resulted in the same trajectory obtained while considering the $K_f=1$. The following results were obtained:

• $K_f=0$ required the expansion of 181 nodes, with 5430 evaluations of LMIs;
• $K_f=0.5$ required the expansion of 174 nodes, with 5394 evaluations of LMIs;
• $K_f=1$ required the expansion of 174 nodes, with 4003 evaluations of LMIs.

The computational effort related to the classification process was roughly a tenth of the overall computational time, and the workstation used was capable, during tests of running 140,000 classifications per second. Training was carried out in 7 min. The relevant confusion matrix for the SVM used is shown in Figure 5.

4.4. Training Weights in SVM

A series of tests was carried by training the SVM with different weights matrices $K_m$ of misclassification costs. To isolate the effect of $K_m$ over planning, two matrices were chosen aiming at holding the same accuracy: one lowering the false discovering rate (FDR) at the expense of the false negative rate (FNR) and the other one doing exactly the opposite, which was chosen such that the achieved accuracy would be the same in both cases.

The two confusion matrices are reported in Figure 6. In the first case, the misclassification cost was 0.01 for the true class versus 1 for the false class. In the second case, the misclassification cost was 0.12 for the false class versus 1 for the true class. The measured accuracy came out to $96.4\%$ and $96.5\%$, respectively.

The “higher FDR” case resulted in the benchmark trajectory in the exploration of 182 nodes and the solution of 4389 LMI problems. Conversely, the “higher FNR” case resulted in the exploration of 350 nodes while requiring solving 14,611 LMI; thus, the FNR appears to be the critical parameter in classifier training for trajectory planning.

4.5. Number of LSTM Layers

This subsection shows the tests obtained by considering 40, 5, and 2 hidden layers in the LSTM, with an accuracy of $100\%$, $99.99\%$, and $96\%$, respectively. The training process took 16, 10, and 8 min, respectively. The process for the 40-layer LSTM is reported in Figure 7, while the two-layer one is reported in Figure 8.

Although the results in terms of expanded nodes were better in terms of both expanded nodes and solved LMIs with respect to the SVM-enhanced planning, the higher computational cost of LSTM-based classification exceeded this benefit. Namely, while the number of node expansions was reduced to 141 and the solved LMIs were 3651, the classification process brought an extra burden of roughly four times more computational effort.

5. Discussion and Conclusions

The scope of this work lies in bringing the formally ensured safety of set-based methods for trajectory planning into artificial intelligence-based ones, with the aim being to exploit the effectiveness of performing the complex tasks that characterize the latter.

By combining a graph-based trajectory planning technique used in robotics with an artificial intelligence-based hybrid $A^{*}$, the technique can improve the performance of the baseline set-based method in terms of the time needed to find a first solution (regardless of its optimality) and the time needed to find the optimal solution.

Namely, the SVM-based planning showed up to a 28% reduction in computation effort with respect to the classical “Euclidean” $A^{*}$ planning (i.e., the $A^{*}$ planning). Furthermore, it has been shown that higher accuracy is tied with the performance outcomes of the algorithm but also that the false negative rate can significantly hamper the performances.

Furthermore, the possibility to enhance planning through the use of a long-short term memory has been evaluated: although it can improve the efficiency of the algorithm while exploring the environment, its higher computational cost exceeds this benefit. It is interesting to note that the highest accuracy of the LSTM can be reached with just five hidden layers. This result could probably be linked to the fact that the kynodynamic feasibility of a trajectory depends, from a physical point of view, on a part of the trajectory that has been already crossed, and further investigation could exploit this result for improving planning performances.

The results obtained in this work contribute to validating a methodological approach that, while focused on trajectory planning for autonomous vehicles, offers insights and potential for future applications in various scientific and technological contexts. Its possible use in autonomous driving could deal with the local planning of trajectories, not only from the autonomous driving perspective but also in terms of traffic management. In particular, starting from SAE J3016 level 2 features, AVSes require kynodynamically feasible trajectories.

Further deployments will look forward in embedding incremental learning features in the planning framework, without hampering computational efficiency.