Introduction

Nowadays, road traffic accidents represent a major public health concern as they are the primary cause of death and disability worldwide, resulting in over a million deaths annually and between 20 and 50 million injuries, some of which can lead to permanent disabilities. Moreover, expenditure on traffic accidents can account for up to 3% of a country’s GDP, which is a significant financial burden, especially for developing countries [1, 2]. Therefore, it is imperative to explore innovative solutions that can create a safer driving and transportation environment from both a humanitarian and pragmatic perspective.

Autonomous Driving is a promising solution to this problem, as it has the potential to eliminate traffic accidents induced by human-driver error, such as speeding, drink-driving, delayed reaction time, and other forms of inattentive or aggressive driving [3]. In addition, it contributes to optimizing traffic flow, reducing fuel consumption, and enhancing passenger comfort. However, since it requires collaborative research across several domains, including computer vision, sensor data fusion, networks, and control theory, high-level automated driving has yet to be fully realized [4]. In this paper, we focus on the control aspects of the autonomous vehicles (AV) and present our research contribution to further advance the state-of-the-art in AV control, i.e., a generic, highly compatible architecture that can be applied for a variety of control systems and vehicle types with minor configurations.

Model Predictive Control (MPC) has recently become one of the most prevalent control methods in autonomous driving applications, owing to its flexibility, reliability, and effectiveness when dealing with multi-objective problems [5]. For example, it has been used in path following and lane keeping systems to promote driver safety and comfort while guaranteeing efficient fuel consumption, thus improving traffic flow and reducing air pollution [6, 7].

In a previous study [8], we introduced a deterministic MPC-based path tracker with collision-avoidance capabilities, in which multiple controllers were developed and fine-tuned for generic urban driving situations. Moreover, a Finite-State Machine (FSM) was implemented to analyze the current driving situation and appropriately activate the most suitable controller, as well as ensure smooth switching between the different controllers. This approach required modeling the path to be followed as a spline curve \({\gamma }_{ref}{:}\,[{s}_{0},{s}_{f}] \rightarrow {{\mathbb {R}}}^{2}\) that spans from the desired start location A to the destination B and passes through the way-points \({\gamma }_{ref}(s):=[{{x}_{ref}(s),{y}_{ref}(s)}]^{\text {T}}\). Accordingly, the control system was responsible for traversing this path as fast as possible while fulfilling some objectives (like minimizing the system controls) and adhering to some constraints (like speed limits).

Here, we continue to explore this idea by following a more standardized approach, in which we construct a configurable routing and tracking architecture that can control different vehicles in real-life urban driving scenarios. We take inspiration from sources like [9, 10] to understand how a typical autonomous driving architecture is designed, including expected system inputs and outputs and the characterization and responsibilities of each of the system components. To improve the applicability of our approach, we utilize the open-source geographic database OpenStreetMap (OSM) [11] and the open-source routing library GraphHopper [12] to construct feasible travel paths between desired locations across real-world street maps. Moreover, we consider sources like [13,14,15] to model the ego-vehicle and appropriately specify the system dynamics and constraints. Accordingly, we build vehicle- and objective-specific optimal reference path and velocity trajectories, which can be used as a baseline for online tracking. Finally, with the aid of [16, 17], we make some adaptations to our controller to create a real-time capable control system, which tracks the previously computed reference path and velocity trajectories while guaranteeing collision avoidance. This rounds up our proposed architecture, as it includes both path routing and real-time trajectory tracking while being configurable for different vehicle types and control objectives.

This paper is structured as follows. In “Control Architecture” section, we describe the overall system architecture and the inputs, outputs, and functionality of each of its components. We also briefly introduce the open-source services used in this study, namely OSM and GraphHopper. In “Trajectory Generation” section, we focus on the trajectory planning components, where we explain the algorithm that generates the reference path to be followed formulate the offline control problem that computes the vehicle-specific optimal velocity trajectory across the reference path. Afterwards, we discuss the tracking component in “Trajectory Tracking” section, with an emphasis on the modifications done to the system model and constraints to support real-time tracking of the reference trajectories. Finally, we evaluate the proposed architecture in numerical simulations and display the achieved results in “Numerical Simulation and Results” section, then summarize our work and ideas for future work in “Conclusion and Future Work” section.

Control Architecture

Autonomous driving has been the subject of extensive research over the past few decades, yet some challenges remain unsolved that hinder the development of fully autonomous vehicles (AVs) to this day, such as legal, technical, and ethical issues [18]. For example, one of the more controversial topics is the creation of a standardized development architecture for AVs, as it has been proven difficult to construct a single architecture suitable for all possible AV applications. Therefore, we can find multiple architectures that were developed to accommodate a specific scope, objective, or field of application [9, 10, 19], but on inspecting these architectures conjointly with an industrial approach to this problem [20], we realize that there are some characteristic components that recurrently appear in every architecture, such as the trajectory generation and trajectory execution components-albeit under different names. So in our proposed architecture, we primarily focus on the implementation of these two components, as they represent cornerstone elements that can be easily integrated into other existing architectures.

Other notable components are the environment- and self-perception modules, such as cameras, radar sensors, and data fusion modules. These modules may operate in a localized or cooperative manner with neighboring AVs, leveraging the vehicle’s sensor set and communication devices, as well as the available Internet of Vehicles (IoV) environment [21]. However, the configuration and selection of perception modules as well as their cost represent separate research topics that are beyond the scope of this study. Similarly, the actuation modules are typically vehicle and model-dependent and require considerable scientific contribution [22], so they will only be briefly discussed in the sequel.

Figure 1 illustrates a comprehensive summary of the typical components present in AV system architectures [19] from a control perspective, and can be explained as follows:

  • Perception In this stage, the AV uses various sensors and algorithms to gather information about itself and its environment [23]. For example, camera modules produce images that, when processed by the lane marks recognition module, yield feature points about the admissible driving area. By combining this with a localization algorithm, we can accurately determine the position of the ego-vehicle with respect to its travel lane to ensure safe driving [19]. The data fusion module is reserved for more complex functions, like pedestrian recognition and tracking, which require information from multiple sensors (e.g., cameras and LiDAR) to reliably extract the desired features [10].

  • Decision and planning Here, the AV uses the acquired information from the perception modules to plan and decide the motion and behavior of the ego-vehicle, which includes trajectory planning, obstacle and collision avoidance, action prediction, and so on [24]. We can categorize these components into two groups: trajectory generation, i.e., the high-level path planning, and trajectory execution, i.e., the low-level real-time path tracking. Furthermore, some interactive modules exist that enable the driver to influence the decision-making process of the AV, e.g., to set a desired travel speed or to override a planned autonomous maneuver [9].

  • Actuation After determining the desired driving maneuvers, the control modules convert these actions into physical controls of the ego-vehicle, such as steering and accelerating/braking. Consequently, they are transferred to and executed by the hardware drivers, as they directly interface with the chassis components of the AV (e.g., the steering wheel motor and the accelerator pedal motor) [10]. In some cases, the actuation modules may also be used to alert an inattentive driver with the aid of vibrant lights or loud sounds if the system detects an emergency [23].

Fig. 1
figure 1

System architecture of AVs with generic components

Full size image

In order to maximize compatibility with existing architectures, we propose a simplistic architecture to implement the decision and planning components, in which we assume prior knowledge of the required road data (e.g., driving lanes), ego data (e.g., speed and steering angle), and traffic data (e.g., positions and velocities of other traffic participants). Moreover, we configure our system to compute generic control actions, such that they can be converted later into the specific controls of different vehicles. Thus, the proposed architecture is displayed in Fig. 2 and the components of interest are highlighted in gray.

Fig. 2
figure 2

Proposed routing and tracking architecture

Full size image

The implementation of these components will be discussed in detail in the upcoming sections, but first we will give a brief description of the open-source services we intend to use, namely OSM and GraphHopper. It is pertinent to mention that these services offer free solutions for academic purposes and were accordingly used at no cost in this study, so further research may be necessary to assess their financial viability in commercial applications.

OpenStreetMap

OSM [11] is a collaborative, volunteer-driven project that was launched in 2004 with the aim of creating a freely available geographic database of the world, that can be used in mapping, navigation, or similar applications without being limited by proprietary or monetary restrictions [25]. Since then, it has become one of the largest and most recognized Volunteered Graphical Information (VGI) projects, with millions of registered contributors and billions of data points covering countries across the entire world [25, 26]. Moreover, the OSM datasets are constantly increasing both in quality and magnitude, since their contributors include not only volunteers and non-profit organizations, but also professional editors from major tech corporations such as Facebook, Amazon, and Apple [27]. Finally, OSM datasets have already been utilized in navigation systems [26] and autonomous vehicle applications [27], which further supports our decision to use OSM in the path planning components of this study.

GraphHopper

GraphHopper is an efficient, open-source routing engine that offers multiple free and commercial solutions for traffic routing, such as turn-by-turn navigation, route planning and optimization, geocoding, and map matching [28]. It primarily utilizes OSM datasets for its road networks, and it can be configured to use different path-finding algorithms, such as Dijkstra, A*, and their bidirectional variations, to determine the fastest/shortest path between two given locations. GraphHopper works pretty efficiently, where it can calculate routes across hundreds of kilometers in mere seconds [29]. Additionally, it analyzes the map routes’ metadata (e.g., road type, speed limits, and road dimensions), and thus can be configured for custom routing of different vehicles, cyclists, or pedestrians [26]. It can also be adapted to offer custom routing for specific driving objectives, such as finding the shortest path while optimizing nitrogen oxide emissions [30]. Overall, GraphHopper presents us with a free, fast, and customizable method for route planning, which perfectly fits our current problem.

Trajectory Generation

As previously discussed, we need to develop two components to generate the reference path and velocity trajectories that will be tracked by the ego-vehicle, which we will discuss in the following sections.

Path Planning

We start with the path planning component, which, when given a start location A and a destination B, must generate a feasible travel path \({\gamma }_{ref}\) between them through the road network. To achieve this, we write the locations A and B in terms of their geographic coordinates \(({lat}_{A/B},{lon}_{A/B})\) and use the GraphHopper Routing service to calculate the fastest route connecting them. If needed, we may also use the GraphHopper Geocoding service to convert desired addresses into (lat, lon) points. Figure 3 displays a sample of the different vehicle-specific fastest routes generated by GraphHopper between given start and end locations.

Fig. 3
figure 3

Generated routes for a car (left) and a bicycle (right) between the same points using the GraphHopper web interface [12]

Full size image

The web interface offers basic routing options, such as setting multiple stops between A and B and optimizing for a selected traffic participant type (e.g., car, truck, pedestrian, etc.). Accordingly, the generated route is a set of labeled geographic way-points with navigational instructions, as well as some rudimentary information like traveled distance and elevation data. However, the GraphHopper Routing API offers more capabilities, such as specifying a custom routing vehicle profile and selecting the language for navigational instructions, but more importantly, it can embed additional information in the metadata of the generated way-points, like road class, number of lanes, maximum speed, street name, and many more [12].

This API can be called over HTTP, where it responds with a JSON formatted data file containing the generated routing way-points with the requested metadata. To simplify the API calling process, we used MATLAB to develop the GUI depicted in Fig. 4, where the user can specify the routing options and desired metadata. Then, the GUI calls the API and exports the received generated route either to an external data file or to the MATLAB workspace for further data manipulation. Also, we added a simple map displaying the generated way-points data in the GUI for clarity purposes.

Fig. 4
figure 4

Developed GUI to facilitate calling the GraphHopper Routing API

Full size image

On observing the generated route data, we notice two main issues: first, the way-points are specified in the geographic coordinates system, which is unsuitable for local positioning and trajectory following. To tackle this problem, we must redefine the 3D geodetic data in a local Cartesian coordinates system, which is achieved by using a map projection (e.g., the Universal Transverse Mercator) to transform the route map from ellipsoidal into plane coordinates [31]. We can then create a local Cartesian map with the projected way-points, which, in accordance with a GPS sensory module and an adequate vehicle model, enables system localization and tracking [4]. Luckily, MATLAB readily offers the function latlon2local that directly converts (lat, lon, alt) data points into (x, y, z) coordinates, where the local coordinate system is anchored around a desired origin point.

The second issue is that the received way-points resemble simple positions on a coordinates system, but we need a spline curve \({\gamma }_{ref}\) for the path tracking component. We can solve this by constructing continuous natural cubic splines between the way-points to create \({\gamma }_{ref}\) from A to B, which theoretically works, yet may yield infeasible routes due to the nature of the received way-points. As GraphHopper attempts to minimize the number of returned data points, it omits sequential points in a straight line, which results in having poorly defined road segments with few way-points, such that generating splines for these road segments may result in routes that span outside the allowed road network. We address this by analyzing the locations of each pair of sequential way-points and if their inter-distance is less than a specific threshold, we use interpolation to insert additional way-points between them. Thus, we can ensure that all of our road segments are adequately defined.

Algorithm 1
figure a

Compute Reference Path

Full size image

Additionally, GraphHopper returns way-points exactly in the center of road segments, which may yield incorrect paths in case of multi-lane roads. So, by checking the number of lanes (from the metadata) in all road segments, we can appropriately shift the way-points’ locations to favor traveling in the rightmost lane to promote safe driving. Similarly, we can analyze navigational instructions to identify turning maneuvers to the right/left and correspondingly shift the way-points to produce smoother travel paths. Lastly, we create continuous splines across the adapted way-points to form our reference path \({\gamma }_{ref}\). The complete process of way-points manipulation is summarized in Algorithm 1 and an example of the resulting spline is demonstrated in Fig. 5.

Fig. 5
figure 5

Original way-points (blue) versus final spline from adapted way-points (red)

Full size image

It is important to mention that the generated reference path serves only as a baseline for the ego-vehicle and that during real-time tracking, the sensor modules will provide the AV with additional data about the lane constraints, which will be used to ensure driving in admissible areas.

Velocity Planning

After generating the reference path, we now move unto calculating the optimal velocity trajectory corresponding to this path. We take the road data and the vehicle-specific dynamics and constraints into account and accordingly compute an explicit baseline for the velocity trajectory across this path. This ensures a safe, efficient, and comfortable trip, as well as simplifies the tracking problem, which facilitates achieving real-time trajectory tracking. We formulate this as an optimal control problem and solve it using Nonlinear MPC to guarantee feasible and smooth trajectories [32]. Since we intend to create generalized components for modeling various AVs, we need to use a generic model that describes the dynamic motion of the ego-vehicle while being independent of vehicle-specific parameters. So, we utilize the kinematic vehicle model [13] in coordination with a curvilinear coordinate system to describe the AV’s motion across a reference curve \({\gamma }_{ref}{:}\, [0,L] \rightarrow {{\mathbb {R}}}^{2}\) as follows:

$$\begin{aligned} {s}'(t)&= \frac{v(t) \cos \chi (t)}{1 - d(t) \cdot {\kappa }_{ref}(s(t))} \end{aligned}$$
(1a)
$$\begin{aligned} {d}'(t)&= v(t) \sin \chi (t) \end{aligned}$$
(1b)
$$\begin{aligned} {\chi }'(t)&= {\psi }'(t) - {{\psi }'_{ref}}(t) = v(t) \kappa (t) - {s}'(t) {\kappa }_{ref}(s(t)) \end{aligned}$$
(1c)
$$\begin{aligned} {\kappa }'(t)&= {{u}_{1}}(t) \end{aligned}$$
(1d)
$$\begin{aligned} {v}'(t)&= {{u}_{2}}(t) \end{aligned}$$
(1e)

This yields the system state vector \({x}^{T} = [s,d,\chi ,\kappa ,v]\), where s is the arc length of a reference point on the ego-vehicle relative to \({\gamma }_{ref}\), d is the normal distance between this point and \({\gamma }_{ref}\), \(\chi\) is the deviation in orientation with respect to \({\gamma }_{ref}\), and \(\kappa\) and \({\kappa }_{ref}\) represent the curvature of the vehicle’s trajectory and reference trajectory respectively. Another benefit of using the kinematic model is that there exists a subset of the system states \({y}^{T} = [d, \chi ]\) that represent tracking errors to the reference trajectory, such that the path tracking problem equates to stabilizing y(t) to 0. In addition, this model offers generic quantities for the system inputs (\({\kappa }'\), \(v'\)), and they can later be converted to the controls of different vehicles by using the mapping \((x,u,X) \mapsto U=\mu (x,u,X)\) [8].

To define the system constraints, we first need to revisit the metadata of the way-points we received from GraphHopper, as they include information on the speed limits at the start and end of each of the road segments in the reference path. We use a simple heuristic to create continuous functions for the speed limits \({v}_{min}\) and \({v}_{max}\), in which we assume constant values across a road segment if the speed limits remain unchanged, and otherwise use interpolation to determine the intermediate speed limit values. Consequently, we can impose the following restriction on the vehicle’s velocity:

$$\begin{aligned} \begin{aligned} {v}_{min}(s(t)) \le v(t) + {\eta }_{v} \le {v}_{max}(s(t)) \end{aligned} \end{aligned}$$
(2)

such that \({v}_{min}(s(t))\) and \({v}_{max}(s(t))\) are computed with respect to the traveled arc length, and \({\eta }_{v}\) is an additional slack variable for constraint relaxation, which increases the chances of finding feasible solutions without sacrificing system stability [33]. Second, we consider the AV’s physical construction that may limit the behavior of its components, e.g., by imposing restrictions on its maximum acceleration/deceleration or its driving curvature. So, we add:

$$\begin{aligned} {u}_{min} \le {u}(t) \le {u}_{max} \end{aligned}$$
(3a)
$$\begin{aligned} {\kappa }_{min} \le {\kappa }(t) \le {\kappa }_{max} \end{aligned}$$
(3b)

which are vehicle-specific constraints that are determined and set for different vehicles independently [15]. Finally, we introduce a safety constraint that restricts the vehicle’s lateral acceleration to a maximum allowed value \({a}_{n,max}\), in order to avoid excessive lateral forces when making sharp turns [13]:

$$\begin{aligned} \begin{aligned} -{a}_{n,max} \le {\kappa (t)}{v(t)}^{2} \le {a}_{n,max} \end{aligned} \end{aligned}$$
(4)

As for the MPC cost function, we use the weighted multi-objective function:

$$\begin{aligned} \begin{aligned} min \int _{0}^{T}&{\omega }_{y} ||y|| + {\omega }_{s} \Bigr (\frac{{s}_{f}-s}{{s}_{f}-{s}_{0}}\Bigl )^{2} +{\omega }_{u} ||u|| + {\omega }_{v} {{\eta }_{v}}^{2}dt \end{aligned} \end{aligned}$$
(5)

in which the first term incorporates minimization of the tracking errors to the reference trajectory, the second is a normalized incentive to minimize the difference between the vehicle’s traveled distance s and the destination \(s_f\) [8], the third is a traditional objective to reduce energy consumption and maximize passenger comfort, and the fourth minimizes the slack variable for velocity relaxation \({\eta }_{v}\). By solving the problem, we get the desired reference path and velocity trajectories that will be later used in online tracking.

Trajectory Tracking

Nonlinear system models are suitable for describing the motion and dynamical behavior of complex systems, yet they impose an additional layer of complexity that optimal solvers may struggle against, which limits their usability in real-time applications [34]. Therefore, it is often beneficial to linearize the system dynamics around an operating point and attempt to find an optimal solution for the linearized model instead, as the problem structure can then be exploited to reach feasible solutions faster [35].

As the ego-vehicle is expected to closely follow the reference path, we can safely assume that its deviation in orientation \(\chi\) with respect to \({\gamma }_{ref}\) is small enough, such that \(sin(\chi ) \approx \chi\) and \(cos(\chi ) \approx 1\). Moreover, we can assume that its lateral deviation d is small with respect to the reference curve (which is defined as \({r}_{ref} = \frac{1}{{\kappa }_{ref}}\)), such that \(\frac{d}{{r}_{ref}}=d\cdot {\kappa }_{ref}\ll 1\) [16]. This simplifies the model introduced in Equation 1 and yields:

$$\begin{aligned} {s}'(t)&= v(t) \end{aligned}$$
(6a)
$$\begin{aligned} {d}'(t)&= v(t) \chi (t) \end{aligned}$$
(6b)
$$\begin{aligned} {\chi }'(t)&= v(t) \cdot (\kappa (t) - {\kappa }_{ref}(s(t))) \end{aligned}$$
(6c)
$$\begin{aligned} {\kappa }'(t)&= {{u}_{1}}(t) \end{aligned}$$
(6d)
$$\begin{aligned} {v}'(t)&= {{u}_{2}}(t) \end{aligned}$$
(6e)

Nevertheless, this model still includes some nonlinearities due to the coupled system dynamics in d and \(\chi\). To address this problem, some approaches proposed to assume a constant velocity profile [36], others suggested decoupling the longitudinal and lateral system dynamics and solving them in two sequential phases [37], however we argue that this can only yield sub-optimal trajectories. A more interesting idea would be to locally linearize the coupled system model using Taylor expansion at each time step and then solve for the fully linearized model, which may yield better results provided that the model is discretized with a small enough time step \(T_s\), but we leave this for future studies. For the sake of this study and the application at hand, we proceed with the simplified model in Eq. 6 as it is sufficient to achieve acceptable real-time results.

For trajectory tracking, we use the constraints introduced in Eqs. 2 and 3, but ignore the constraint for \({a}_{n,max}\) as it has already been considered in computing the optimal velocity trajectory \({v}_{ref}\) to simplify our problem further. In addition, we primarily reuse the constraints for safe following and driving within the permissible area previously proposed in [8], which we summarize in the following.

We define the occupancy region of the ego-vehicle as a rectangular area encompassing its complete 2-D footprint and optimally cover it with circular disks, then use a simple heuristic for collision detection, i.e., a collision occurs if an object (or lane markings) overlaps with the footprint of any of the covering disks. Accordingly, we guarantee driving within the admissible area by adding a set of clearance constraints \({clear}_{LN}\) that restrict the lateral deviation of the disks covering the ego-vehicle as demonstrated in Fig. 6. These constraints are defined as \({d}_{min,i} + r \le {d}_{i} \le {d}_{max,i} - r, \forall i \in {1,\ldots ,{nrDisk}_{ego}}\), where \({d}_{i}\) is calculated using trigonometric functions for each disk and r is the common disk radius. Determining \({d}_{max,i}\) and \({d}_{min,i}\) depends on the data we receive from the sensory modules [24], e.g., on receiving way-points corresponding to the lane markings, we may create the splines \({\gamma }_{min}\) and \({\gamma }_{max}\), from which we can compute the minimum/maximum allowed lateral deviation with respect to arc length \({d}_{min/max}(s(t))\). Nonetheless, this is a rather irrelevant task to the MPC as it can be completely realized by an independent component that takes s and returns the corresponding \({d}_{min/max}\) values. So, in this work we rewrite the constraints to guarantee driving in one permissible lane with width \({w}_{LN}\), such that \({\gamma }_{ref}\) lies in the middle of it, which yields the linear constraints:

$$\begin{aligned} r - \frac{{w}_{LN}}{2}&\le d \le \frac{{w}_{LN}}{2} - r \end{aligned}$$
(7a)
$$\begin{aligned} r - \frac{{w}_{LN}}{2}&\le d + \frac{{l}_{d}}{2}\chi \le \frac{{w}_{LN}}{2} - r \end{aligned}$$
(7b)
$$\begin{aligned} r - \frac{{w}_{LN}}{2}&\le d + {l}_{d}\chi \le \frac{{w}_{LN}}{2} - r \end{aligned}$$
(7c)
Fig. 6
figure 6

The clearance constraints for driving inside the permissible area [8]

Full size image

Moreover, we introduced a probabilistic component that determines the closest road user driving in the same lane as the ego-vehicle in [8]. We employ this component in accordance with a modified constant time headway policy to specify the safety distance \({s}_{SF}(t) = max({s}_{SF,min}, v(t){t}_{h})\), which must always be kept to avoid collisions. Subsequently, we add a constraint for safe following:

$$\begin{aligned} \begin{aligned} {s}_{rel} \ge {s}_{SF} + {\eta }_{SF} \end{aligned} \end{aligned}$$
(8)

in which \({s}_{rel}\) is the relative distance between the arc length of the leading traffic participant and the ego-vehicle respectively, and \({\eta }_{SF}\) is the slack variable for safe following. Finally, we formulate the modified MPC objective function as:

$$\begin{aligned}{} & {} min (v({t}_{f}) - {v}_{ref}(s({t}_{f})))^{2} + \int _{{t}_{0}}^{{t}_{f}}{\omega }_{y} ||y|| \nonumber \\{} & {} \quad +\, {\omega }_{u} ||u|| + {\omega }_{SF}{{\eta }_{SF}}^{2} dt \end{aligned}$$
(9)

which operates in the time span \(t \in [{t}_{0}, {t}_{f}]\) and consists of a terminal term for following the reference velocity trajectory \({v}_{ref}\), in addition to a running cost for tracking the path trajectory as well as minimization of system controls and slack variables. We now solve the optimal control problem iteratively, where we start at \(t={t}_{0}\) and solve for the states, constraints, and cost function on the interval \(\Delta T = {t}_{f} - {t}_{0}\) to determine the optimal control \({u}^{*}\), where \({u}^{*}\) is applied for one time step \({T}_{s} = \frac{\Delta T}{N}\). Afterwards, we shift the prediction horizon by one time step and measure the system states, then solve the problem again for the updated states, constraints, and cost function. This is repeated until an exit condition is met, which, in our case, is reaching the destination. Notice that the terminal term \(v(T) - {v}_{ref}(s(T))\) in Eq. 9 now replaces the normalized incentive \(\frac{{s}_{f}-s}{{s}_{f}-{s}_{0}}\), as the vehicle shall continue to move with the optimal velocity trajectory until it finally stops at its destination.

FSM Operating Strategy

Similarly, we revise the FSM architecture previously discussed in [8] and add some modifications to incorporate the changes we made to our control problem due to introducing the velocity planning component. We present the modified configuration in Fig. 7 and switch conditions in Table 1, as we briefly discuss the formulation of the transition conditions in the following equations. For more details on the FSM modes, we refer to “Appendix”.

Fig. 7
figure 7

FSM state configuration and possible transitions

Full size image
Table 1 Conditions for switching between the different FSM modes
Full size table

On switching from one state to the other, we may have multiple exit conditions and in this case, we prioritize the conditions according to their indices, i.e., \({\tau }_{1}\) is evaluated before \({\tau }_{2}\). In case no exit condition is satisfied, the active state remains unchanged, which is denoted by OW in Table 1. Moreover, we primarily use sigmoid functions \(f(x) = \frac{1}{1 + {e}^{\alpha + \beta x}}\) to calculate smoothing factors \({\omega }_{\tau }\) for state exit conditions, such that we prevent aggressive switching from one state to the other, as well as guarantee smooth reversible switching if a condition is no longer satisfied. Notice that we will switch from one state to the other only when a condition is fully satisfied \(\tau = 1\), otherwise, we remain in the same state and calculate a transition factor \({\omega }_{\tau }\) to gradually modify the system objectives and constraints, then accordingly formulate a new control problem to find the optimal system controls [8].

For the first state XP, we have two exit conditions: \({\tau }_{1}\) to PF and \({\tau }_{2}\) to PU. Both are initially triggered on leaving the initial parking area (i.e., \(s \ge {s}_{XP}\)), in which \({s}_{XP}\) is a traveled distance threshold for the parking area that is determined based on geographic data in accordance with \({\gamma }_{ref}\). Likewise, both transitions are completed after the ego-vehicle passes the relaxed threshold \(s \ge {s}_{XP,\eta }\). The main difference between \({\tau }_{1}\) and \({\tau }_{2}\) is the maximum speed limit in each of their respective states, which appropriately affects the acceleration and deceleration limits for driving comfort. We define \({\tau }_{1}\) as:

$$\begin{aligned} {\tau }_{1} {:}{=} {\left\{ \begin{array}{ll} 1, &{}\quad (s \ge {s}_{XP,\eta }) \wedge ({v}_{max} \ge 13.5[{\rm m/s}])\\ {\omega }_{{\tau }_{1}}, &{}({s}_{XP} \le s< {s}_{XP,\eta }) \wedge ({v}_{max} \ge 13.5[{\rm m/s}])\\ 0, &{}\quad s < {s}_{XP} \end{array}\right. } \end{aligned}$$
(10)

where \({\omega }_{{\tau }_{1}} {:}{=} f(s)\) is a sigmoid function, such that \(\lim _{s \rightarrow {s}_{XP}} {\omega }_{{\tau }_{1}} = 0\) and \(\lim _{s \rightarrow {s}_{XP,\eta }} {\omega }_{{\tau }_{1}} = 1\). Note that \({\tau }_{2}\) is defined in a similar manner by using \({\omega }_{{\tau }_{1}}\) as the smoothing function, but with a different speed limit \({v}_{max} \ge 8[{\rm m/s}]\). Next, we have the common exit condition \({\tau }_{3}\) for both states PF and PU, which is defined in a complementary manner to \({\tau }_{1}\) as:

$$\begin{aligned} {\tau }_{3} {:}{=} {\left\{ \begin{array}{ll} 1, &{}\quad s \ge {s}_{NP}\\ {\omega }_{{\tau }_{3}}, &{}{s}_{NP,\eta } \le s< {s}_{NP}\\ 0, &{}\quad s < {s}_{NP,\eta } \end{array}\right. } \end{aligned}$$
(11)

with \({s}_{NP}\) and \({s}_{NP,\eta }\) being the distance threshold and relaxed threshold for the final parking area respectively. We now move unto the transition condition \({\tau }_{4}\) from PF to PU, which is triggered either on following/approaching a slow road user, or when the road regulations specify a lower maximum speed limit than the current, i.e., \(30\,[\mathrm{km/h}]\) instead of \(50\,[\mathrm{km/h}]\). Subsequently, we set \({\tau }_{4}\) as:

$$\begin{aligned} {\tau }_{4} {:}{=} {\left\{ \begin{array}{ll} 1, &{}\quad \lnot {\tau }_{3} \wedge (v \le 8) \wedge (({O}_{inLN} \ne \emptyset ) \vee ({v}_{max} \le 8.4))\\ {\omega }_{{\tau }_{4}}, &{}\quad \lnot {\tau }_{3} \wedge (({v}_{max}< 13.5) \vee (({O}_{inLN} \ne \emptyset ) \wedge (v < 13.5)))\\ 0, &{}\quad \text {otherwise} \end{array}\right. } \end{aligned}$$
(12)

with the velocity-dependent sigmoid function \({\omega }_{{\tau }_{4}} {:}{=} f(v[{\rm m/s}])\), such that \(\lim _{v \rightarrow 13.5} {\omega }_{{\tau }_{4}} = 0\) and \(\lim _{s \rightarrow 8} {\omega }_{{\tau }_{4}} = 1\). Note that we use marginally lower values for the speed limits (\(8\,[\mathrm{m/s}]\), \(13.5\,[\mathrm{m/s}]\) instead of \(30\,[\mathrm{km/h}]\), \(50\,[\mathrm{km/h}]\)), since it promotes safe driving without sacrificing speed, while being slightly easier to formulate programmatically. Complementary to \({\tau }_{4}\), we define the return transition \({\tau }_{5}\) from PU to PF as:

$$\begin{aligned} {\tau }_{5} {:}{=} {\left\{ \begin{array}{ll} 1, &{}\quad \lnot {\tau }_{3} \wedge ({v}_{max} \ge 13.5) \wedge (({O}_{inLN} = \emptyset ) \vee (v \ge 13.5))\\ {\omega }_{{\tau }_{5}}, &{}\quad \lnot {\tau }_{3} \wedge ({v}_{max} > 8.4) \wedge (({O}_{inLN} = \emptyset ) \vee (v \ge 8))\\ 0, &{}\quad \text {otherwise} \end{array}\right. } \end{aligned}$$
(13)

where \({\omega }_{{\tau }_{5}} = 1 - {\omega }_{{\tau }_{4}}\). Afterwards, we examine the transitions for the utility state SS, which can be sufficiently implemented as hard constraints due to the nature of the state itself. The transitions for entering and exiting the state are:

$$\begin{aligned} \begin{aligned} {\tau }_{6}:= \lnot {\tau }_{5} \wedge (v \le 0.5[{\rm m/s}]) \wedge (a \le 0[{\rm m/s}^{2}]) \wedge ({s}_{rel} \le {s}_{SF}) \end{aligned} \end{aligned}$$
(14)
$$\begin{aligned} \begin{aligned} {\tau }_{7}:= ({O}_{inLN} = \emptyset ) \vee ({s}_{rel} \ge {s}_{SF} + {s}_{SF,\eta }). \end{aligned} \end{aligned}$$
(15)

Finally, we define the final transition \({\tau }_{8}\) by specifying some threshold \({s}_{ND}\) for reaching the destination \({s}_{f}\), such that:

$$\begin{aligned} \begin{aligned} {\tau }_{8}:= ({s}_{f}-s \le {s}_{ND}) \end{aligned} \end{aligned}$$
(16)

after which the ND state is fully activated and promptly decelerates the ego-vehicle until it comes to a complete halt, thus completing the trip.

Numerical Simulation and Results

Next, we will assess the performance of the proposed architecture under the following assumptions: first, that the ego-vehicle is equipped with the necessary sensor set to accurately determine the vehicle’s position and obtain the control-relevant states, such as velocity and curvature. Second, that the sensor data is high-fidelity, reliable, and instantaneously available without any data loss or delay after being filtered (similarly for the communication with the virtual traffic light). Finally, we assume that the vehicle control is consistent and prompt with no delays.

As a compromise between simplicity and performance, we implemented the proposed architecture as a combination of MATLAB and FORTRAN code, in which the software OCPID-DAE1 [38] is used to solve the optimal control problems, namely velocity trajectory planning and real-time tracking. The simulation results were obtained on a Linux system with the processor i5-5200U of 2.20 GHz and 8 GB of RAM. For the MPC parameters, we selected a prediction horizon of \(\Delta T=3.0\,[\textrm{s}]\) with \(N=15\) control points and a time step of \({T}_{s} = 0.2\,[\textrm{s}]\) for the velocity planning and \(\Delta T=2.0\,[\textrm{s}]\), \(N=10\), and \({T}_{s} = 0.2\,[\textrm{s}]\) for the real-time tracking. Model parameters were primarily defined using a variety of sources [15, 39] to guarantee feasibility and maximize passenger comfort, where we created two different sets of parameters for the kinematic vehicle to resemble a car and a small truck. Furthermore, the weights in the MPC objective function in Eqs. 5 and 9 were reused from [8], then slightly modified to improve performance based on trial and error.

Using the knowledge of local streets around the University of the Bundeswehr Munich, we formed a basic scenario with a route of about \(1.5\,[\textrm{km}]\) and a lane width of \({w}_{LN}=3.25\,\textrm{m}\) that ensures activation of the main FSM states. This route is displayed in Fig. 8, where it is obvious that the path planning for both vehicles yields very similar routes, yet we get different velocity trajectories based on the model parameters and system constraints. This is especially true for the constraint due to \({a}_{n,max}\), which is responsible for the sharp dip in the velocity of both vehicles at around \(s\approx 215\,\textrm{m}\) and \(s\approx 520\,\textrm{m}\). For evaluating the performance of the real-time tracking controller, we augment the scenario with a virtual traffic light that is located at \(s=1270\,\textrm{m}\) and gets activated (turns red) during the time interval \(t=[150\,\textrm{s},180\,\textrm{s}]\), so as to verify the approach in a typical urban environment.

Fig. 8
figure 8

Reference path (left) and velocity (right) trajectories generated for the different vehicle types

Full size image

As shown in Fig. 9, both vehicles are able to travel across the reference path with minimal deviation from start to finish. In addition, they travel with smooth velocity trajectories that generally adhere to their respective maximum and optimal velocity trajectories as depicted in Fig. 10. Note that these figures are displayed with respect to the arc length s and not time t in order to present a meaningful comparison.

Fig. 9
figure 9

Error states d, \(\chi\) for both vehicles in the test scenario

Full size image
Fig. 10
figure 10

Velocity trajectories for both vehicles with respect to their maximum and optimal velocities

Full size image

Subsequently, Fig. 11 provides additional insight into the vehicles’ real-time motion, as we see that both vehicles halt in the time period when the traffic light is activated. Finally, we can see in Fig. 12 that trajectory tracking problem was successfully solved in real-time, in which a solution was always found before our specified time step \({T}_{s}=200\,[\textrm{ms}]\) had passed. To be precise, the average CPU time for solving the car problem was \(15.33\,[\textrm{ms}]\) with a maximum of \(96.2\,[\textrm{ms}]\), and the average CPU time for the truck problem was \(15.53\,[\textrm{ms}]\) with a maximum of \(92.03\,[\textrm{ms}]\), which solidifies the validity of our proposed approach for real-time applications.

Fig. 11
figure 11

The time-dependent velocity trajectories signify the effect of the traffic light in our test scenario

Full size image
Fig. 12
figure 12

Execution time for solving the online tracking control problem

Full size image

Conclusion and Future Work

In this paper, we introduced a multi-component flexible architecture for autonomous driving, which integrates path routing, velocity planning, and real-time tracking of pre-computed path and velocity trajectories for different vehicle types. We utilized open-source services, i.e., GraphHopper and OSM, to receive preliminary routing way-points between desired start and end locations, and applied a manipulation algorithm that analyzes the way-points’ metadata to construct an initial continuous spline that can be followed by a traditional MPC controller. Moreover, we divided the optimal control problem into two steps, the first of which is solved offline to calculate the optimal path and velocity trajectories for a nonlinear model, and the second is solved online for a simplified model to follow the trajectories in real-time while guaranteeing safe following of leading traffic participants. The architecture was tested successfully and it shows promising results with respect to real-time autonomous driving.

Ideas for future studies include more comprehensive simulations in a virtual environment with realistic sensor models and system delays, as well as optimization of the system models to achieve improved real-time capabilities, e.g., support longer prediction horizons and shorter time steps. Ultimately, the architecture shall be tested on physical hardware to verify its applicability in real-life, for instance on scale RC cars initially, then on actual test vehicles. Screenshots in Fig. 3 are excerpted from the web interface of GraphHopper https://graphhopper.com/maps. The developed routing Algorithm 1 and corresponding GUI in Figs. 4, 5 are publicly available at the following repository: https://doi.org/10.5281/zenodo.10882122. Moreover, this repository includes the code and data files used in the test scenario demonstrated in Figs. 8, 9, 10, 11, 12, namely: the FORTRAN code files for velocity planning and trajectory tracking using the FSM approach, as well as the generated data files.