Next Article in Journal
Web-Based Architecture for Automating Quantity Surveying Construction Cost Calculation
Next Article in Special Issue
Travelers’ Perceptions on Significance of Travel Time Saving Attributes in Travel Behavior: A Case Study in Oman
Previous Article in Journal
Railway Transition Curves: A Review of the State-of-the-Art and Future Research
Previous Article in Special Issue
Path and Control Planning for Autonomous Vehicles in Restricted Space and Low Speed
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:

Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways

Department of Civil Engineering, Ryerson University, Toronto, ON M5B 2K3, Canada
Author to whom correspondence should be addressed.
Infrastructures 2020, 5(5), 44;
Submission received: 5 April 2020 / Revised: 14 May 2020 / Accepted: 14 May 2020 / Published: 18 May 2020
(This article belongs to the Special Issue Smart Mobility)


In passing maneuvers on two-lane highways, assessing the needed distance and the potential power reserve to ensure the required speed mode of the passing vehicle is a critical task of speed planning. This task must meet several mutually exclusive conditions that lead to successful maneuvers. This paper addresses three main aspects. First, the issues associated with a rational distribution of the speed of the passing vehicle for overtaking a long commercial vehicle on two-lane highways are discussed. The factors that affect the maneuver effectiveness are analyzed, considering the safety and cost. Second, a heuristic algorithm is proposed based on the rationale for choosing the necessary space and time for overtaking. The initial prediction’s sensitivity to fluctuations of the current measurements of the position and speed of the overtaking participants is examined. Third, an optimization technique for the passing vehicle speed distribution during the overtaking time using the finite element method is presented. Adaptive model predictive control is applied for tracking the references being generated. The presented model is illustrated using a simulation.

Graphical Abstract

1. Introduction

The concept of autonomous vehicles (AVs) has been under development since the 1990s [1,2], when the first field experiment performed on a freeway was conducted in San Diego, California. Autonomous vehicles have already started to appear on roads across the globe. Clearly, as the AV market expands, transportation professionals and researchers must address an array of challenges before AV becomes a reality. Several government and industry entities have begun to deploy demonstrations and field tests of the technology [3,4,5,6]. Centers for testing and validation, education, products, and standards for AV have been established and a variety of conferences are being organized to discuss the planning and modeling for AVs [6]. Currently, researchers, scientists, and engineers are investing significant resources to develop supporting technologies. This vibrant state of development has motivated the proposed program.
Autonomous vehicles have numerous advantages [7]. Since more than 90% of fatal vehicle collisions are attributed to human error, AV technologies can substantially reduce deaths and injuries from vehicle collisions. Autonomous vehicles can also reduce traffic congestion, reduce carbon dioxide emissions, increase the highway capacity, reduce fuel consumption, improve public transportation services, and provide more efficient parking. On two-lane highways, the driver’s decision to overtake may be risky, since a driver has to judge the operational behavior of opposing and impeding vehicles and decide to pass in a very short amount of time. In addition, the speed plan of the passing vehicle is based on the driver’s perception of its dynamic properties.
In autonomous overtaking, prediction, decision making, and continuous monitoring of vehicle performance are performed by an onboard system. The operation of autonomous vehicles is based on planning the movement trajectory, as well as other reference trajectories. The system searches for an optimal way to bypass obstacles while maintaining traffic safety. For overtaking on two-lane highways, the vehicle’s on-board system must estimate the distance and time required for the passing maneuver and distribute the passing vehicle speed and steering angle based on conflicting criteria. Therefore, the optimization of passing vehicle control for safe and efficient passing maneuvers of autonomous vehicles can be achieved.
Numerous researchers have addressed the issues of planning motion reference lines and control parameters for autonomous vehicles and attempted to find the best trajectories, speed plans, and state-space sequences. Schwarting et al. [8] considered the concept of parallel autonomy, where autonomous control works as an option to monitor and correct driver errors (called shared control). The work initially focused on curvilinear road profiles for which the optimal trajectory of motion was determined using nonlinear model predictive control (MPC), which allowed a consideration of turns and avoidance of moving and static obstacles. Both kinematic and dynamic models were used as vehicle models. The optimization model included a probabilistic collision estimate, and geometric and physical (tire-road adhesion) constraints. The intervention parameter was used to assess the degree to which the system is involved in driver actions. The optimization algorithm provided fast convergence. The only limitation of this work is that the acceleration input parameter is not related to the engine’s potential.
Talamino and Sanfeliu [9] presented a technique for planning a movement trajectory and speed plan of an autonomous vehicle in urban areas based on G2-splines. The polynomial fitting involved iterations equivalent to the optimization of curvature parameters. To simulate a sufficiently long path, a fifth-degree polynomial was used. However, such polynomials are often unstable between nodes. For the speed distribution, a third-degree polynomial was proposed, where the transition time was determined based on the values of parameters (speeds and accelerations) at the end points. Acceleration was limited to the maximum value and was not related to the parameters of the power plant. Not enough information about the overtaking maneuver parameters was provided. González et al. [10] reviewed the methods used to plan autonomous vehicle movement. Graph algorithms were mainly used to determine the minimum maneuver path on the surrounding space grid. The State Lattice algorithm executed path searching using state-space mesh generation. Sampling-Based Planners generated random state-spaces and looked for their ties. A rapidly exploring random tree (RRT) made it possible to use structured spaces. Lines and circles, closed curves, polynomial curves, Bézier curves, and spline curves were used to represent the path forecast. A numerical method for optimizing a function subject to different constraints was used.
Gu et al. [11] proposed a planning method that automatically discovers tactical maneuver patterns and fuses pattern reasoning trajectories based on the idea of using pseudo-homology along with characterizing workspace regions. Different patterns can be extracted, depending on the spatial area where the trajectory terminates (region-based distinction), how it gets there around the obstacles (homology-based distinction), and what overtaking (if any) order it follows (sequence-based distinction). A series of virtual tests were conducted and confirmed the effectiveness of the method. Wang et al. [12] considered the process of building an optimal overtaking route based on minimizing the probability of the vehicles’ presence in the area with close coordinates. An integration process was used to solve the nonlinear optimization. Kala and Warwick [13] considered the process of overtaking based on the conditions of maximum speed movement, but limited acceleration. A speed plan was not considered for overtaking. Testing of the model was carried out at low speeds, with a large distance between the approaching vehicles.
Babu et al. [14] presented an MPC framework based on path speed decomposition for autonomous driving. The concept of a time scaled collision cone, which constrains and formulates forward-speed quadratic optimization, was presented. Collision modeling between rectangular objects was presented. The planned vehicle was reduced to a point and the dynamic obstacle was enlarged using the concept of the Minkowski sum. The autonomous driving scenarios were validated with computations of lane change, overtaking, and merging maneuvers among multiple dynamic obstacles. Tomas-Gabarron et al. [15] considered how to trace the optimum trajectory of a high-speed vehicle that changed its lateral position within a time interval. Four different functions were proposed, along with their relative merits. The presence of Gaussian noise in the sensors’ measurements was studied regarding its influence on the final trajectories. Different performance criteria for the optimization of such maneuvers were presented, as well as an analysis on how path deviations can be minimized by using trajectory smoothing techniques, such as the Kalman filter. Liu et al. [16] focused on speed profile planning for a given path represented by a set of waypoints. The speed profile was generated using temporal optimization that searched the time stamps for all waypoints. Non-convex temporal optimization was approximated by a set of quadratic programs that were solved iteratively using a slack convex feasible algorithm to speed up computations. Other interesting developments of motion planning and control can be found elsewhere [17,18,19,20,21].
This paper presents a new technique of speed planning for the overtaking of autonomous vehicles on two-lane highways. The methodology consists of two main analytical tools. The first tool is a heuristic algorithm that determines the time and distance required for a safe passing maneuver. The algorithm relies on uncertainty-based thresholds of the opposing and impeding vehicles and the minimum and maximum performances of the passing vehicle. The second tool is a quadratic optimization model that determines the optimal speed distribution to ensure a smooth path for the passing vehicle. If needed, the speed plan is updated during the maneuver. The proposed method focuses on finding a rational scheme for vehicle limiting performances. The method aims to simultaneously satisfy several competing objectives, such as a sufficient overtaking time, trajectory smoothness, energy consumption, and collision avoidance. The reason for implementing this approach is that if overtaking is performed with the maximum vehicle performance, there will be a risk of losing lateral stability (due to random external forces) and energy consumption will be high. However, this scenario provides the minimum time and reduces the probability of a head-on collision. On the other hand, if overtaking is performed slowly, a good stability and controllability will be ensured, but there may not be enough of a safety margin at the end of the maneuver.

2. System Description

2.1. Overtaking Phases

The phases of overtaking on two-lane highways are shown in Figure 1. In Phase a (obstacle rear reach), the passing vehicle will start to drive into the oncoming traffic lane (State 1) and reaches the rear edge of the impeding vehicle (State 2), as shown in Figure 1a. Therefore, the longitudinal component of the passing vehicle path during the bypassing phase is Xpb, and during this time, the impeding vehicle travels the distance Xib. In Phase b (obstacle front reach), the passing vehicle travels from the bypass point (State 2) to the critical point (State 3), where its front aligns with that of the impeding vehicle. During this time, the passing and impeding vehicles travel distances Xpo and Xio, respectively.
In Phase c (maneuver completion), the passing vehicle passes the longitudinal distance Xpf to ensure an adequate safe distance df between itself and the impeding vehicle, traveling the distance Xif, as shown in Figure 1c. The passing and opposing vehicles must provide a safety margin distance Xm between their fronts which is equivalent to the minimum safety margin time tmm. Note that in this phase, the lateral movement back to the original lane will not start until the passing vehicle has already overtaken the impeding vehicle.

2.2. Assumptions

The developed system involves the following assumptions:
The roadway is assumed to be straight, with ideal surface friction and no external forces, such as gusts of wind. In addition, the road is assumed to have very gentle vertical curvature that would not obstruct sensor measurements;
Before the overtaking maneuver starts, it is assumed that the estimation of the vehicles’ positions has already been carried out, the forecast has been made, and the passing vehicle is ready to start the passing maneuver. In this state, the distance D0 between the passing and opposing vehicles and the distance d0 between the impeding and passing vehicles are estimated using long- and short-range radars, respectively;
The length of the impeding vehicle is estimated using machine vision technology. The sensor measurements (distance and angle) are assumed to be independent;
The AV is assumed to possess the input data required for the proposed model: roadway data (e.g., lane width and speed limit) and vehicle characteristics (e.g., acceleration–speed relation);
Only the passing vehicle is autonomous, and the impeding and opposing vehicles are assumed to be human-driven vehicles. The overtaking maneuver involves only one impeding vehicle;
In the heuristic algorithm, it is assumed that the actual speed fluctuations detected by the radars remain within the uncertainty thresholds, for which the forecast is considered reliable. Therefore, the changes of vehicle locations will occur within the boundaries determined by the specified measurement thresholds. If the threshold is exceeded, the forecast should be recalculated;
Uncertainty is considered in the estimation of the speeds of the passing and impeding vehicles. Based on the uncertainty thresholds, possible violations of the thresholds are analyzed. No uncertainty propagation throughout the prediction process is considered in this study;
The algorithm continuously works with the vehicle sensory system within the maneuver and only estimates new overtaking parameters when thresholds are violated, thus helping to reduce the load on the on-board computer system.

2.3. Logic of the Speed Control Model

The logic of the speed control model is shown in Figure 2. The model involves four main tasks. First, the operational thresholds of the passing, opposing, and impeding vehicles are established. These thresholds include (1) uncertainty-based thresholds for the predicted speeds of the opposing and impeding vehicles, (2) the minimum performance limit of the passing vehicle, and (3) the maximum performance limit of the passing vehicle. Second, the speed profiles of the three involved vehicles are established and initial values of the time tp and distance Xp required for safe maneuver completion are established using a heuristic algorithm, as described later. These variables are used for determining the optimal distribution of speeds and trajectory planning of the maneuver. Third, quadratic optimization is used to develop a smooth curve for the path of the passing vehicle that can serve as a reference for the control law implementation during maneuver realization.
During maneuver execution, if the upper (in terms of speed values) confidence thresholds of the predicted speed of the opposing or impeding vehicles are violated, the time and distance required for completing the maneuver safely are updated. As noted in Figure 2, if t < tp, the system continues with the last prediction and is updated if the speed thresholds are violated. Otherwise, the maneuver is successfully completed.

2.4. Establishing Operational Speed Thresholds

The measured speeds of the opposing and impeding vehicles have uncertainty. The speed of the opposing vehicle is estimated using radar sensors located in the passing vehicle. Using four signals, Δt apart, four distances to the opposing vehicle di and the corresponding azimuth angles θi are recorded, where the polar coordinates used with the origin point lie with the sensor location. The deterministic distance crossed by the vehicle in consecutive time intervals was presented by Hassein et al. [22] (2018):
d V o p p i = d i 2 + d i + 1 2 2 · d i · d i + 1 · c o s ( θ i + 1 θ i ) d p i , i = 1   t o   3 ,
where dVoppi denotes the distance traveled by the opposing vehicle during Δt and dpi denotes the distance traveled by the passing vehicle during Δt. The speed of the opposing vehicle, Vopp, can then be derived as
V o p p = 1 3 · Δ t · d V o p p 1 7 6 · Δ t · d V o p p 2 + 11 6 · Δ t · d V o p p 3 .
Let the errors in di and θi measurements of the radar be denoted by ed and ea, respectively. Then, these errors will propagate and produce an error in Vopp of Equation (2). To calculate this error, let the four variables of Equation (1) (di, θi, di+1, and θi+1) be denoted by xi, where i denotes 1 to 3. Using the Taylor series, the standard deviation of Y, σy, was given by Benjamin and Cornell [23]:
σ y = i = 1 n ( f x i ) 2 · σ x i 2 ,
where σxi denotes the standard deviation (SD) of random variable xi. By applying Equation (3) to Equation (1), the standard deviation of dVoppi, σdVoppi, can be derived as
σ d V o p p i = k d · e d 2 + k a · e a 2 d V o p p i + d p i ,
k d = ( ( d i 2 + d i + 1 2 ) · ( 1 + c o s 2 ( θ i + 1 θ i ) ) 4 · d i · d i + 1 · c o s ( θ i + 1 θ i ) ) , k a = 2 · d i 2 · d i + 1 2 · s i n 2 ( θ i + 1 θ i ) .
Then, by applying Equation (3) to Equation (2), the standard deviation of the speed of the opposing vehicle, σVopp, is obtained as
σ V o p p = ( 1 3 · Δ t ) 2 · σ d v 1 o p p 1 + ( 7 6 · Δ t ) 2 · σ d v 2 o p p 2 + ( 11 6 · Δ t ) 2 · σ d v 3 o p p 3 .
Similarly, the deterministic distance travelled by the impeding vehicle between consecutive time intervals before overtaking is
d V i m p i = d i + 1 d i + d p i , i = 1   t o   3 .
The standard deviation of the distance dvi − impi, σdVimpi, of Equation (6) is given by
σ d V i m p i = d i 2 + d i + 1 2 .
Then, the speed of the impeding vehicle Vimp and its standard deviation σVimp are calculated using Equations (2) and (5), after replacing dVoppi with dVimpi (i denotes 1, 2, and 3).
For the 95% confidence level, the true speed of the opposing or impeding vehicle lies within approximately two standard deviations from the measured value. Therefore, the confidence ranges used in the proposed heuristic algorithm for the opposing and impeding speeds are calculated as Vopp ± 2·σVopp and Vimp ± 2·σVimp, respectively.

3. Heuristic Algorithm

3.1. General

The heuristic algorithm estimates the rational time and distance required for overtaking. The logic of the algorithm is presented in Figure 3. For each relative position of the vehicles involved in the overtaking maneuver, a combination of measurements will be unique, and the number of possible maneuvers will be numerous. Suppose that at a certain time, the speed values (sample) of the impeding and opposing vehicles have been estimated using the passing vehicle sensory system (noise variances are supposed to be known). At the initial time, the impeding and opposing vehicles are located at distances d(−m) and D(−m), respectively, relative to the passing vehicle, where the index (−m) means the backward number of radar measurement cycles required before the prediction is made. Considering the time required for prediction, the values at the maneuver’s beginning time t0 will become d0 and D0, respectively. If the opposing and impeding vehicles continue their motions at speeds close to the measured ones, then the change in their positions will be approximately linear, which determines the slope (dX/dt) of the corresponding curves (blue and green curves in Figure 3). In turn, the speed measurements also have uncertainty.
Point a (tmin, Xpmin) corresponds to the minimum time and distance, while Point b (tmax, Xpmax) corresponds to the maximum time and distance. Consider the vehicle’s maximum performance with a full fuel supply along the oa curve. Considering the mean performance at Point f (tp, Xp), a distance (df + Lp) would be needed to complete the maneuver, where the linear segment ab represents a set of solutions that correspond to the desired time and distance of overtaking. To predict the minimum distance required for overtaking completion, the passing vehicle speed at the maneuver’s end, and the minimum safe distance between the passing and impeding vehicles (Figure 1c), the space dfmin (Figure 3) should be predetermined.
The safety margin tmm guarantees a distance between the passing and opposing vehicles after maneuver completion. This corresponds to Point c on the segment ab. The oc curve will represent the lower boundary of the field oac of the valid time–distance realizations. If the speed changes of the opposing and impeding vehicles remain within the threshold values, the lower boundary of the opposing vehicle’s distance dependency with the basis (instant mean) line yp will not reach Point c, maintaining the safety margin until time ts. This may be the key point for determining the threshold conditions.
The main idea of the rational point search is to simultaneously meet the criteria of the safety margin and vehicle performance reserve for unpredicted circumstances. Suppose the opposing vehicle moves according to the nominal straight line yp, and the impeding vehicle moves along the nominal straight line rp. Then, the nominal distance (df + Lp) for completing the maneuver (segment bk) will correspond to ta. Considering tmm, the new boundary will imply the safety limit at time ts. Now, it is necessary to choose a point on segment ac that would meet the required criteria. There are many approaches that can be employed to achieve this. One of the possible approaches is to use the trapezoid hlnj to proportionally split segment ac. That is, the vertical lines of the intersection Point z of the trapezoid hlnj diagonals correspond to intersect line ac at Point f. Segments fm and fi characterize the distances to the opposing and impeding vehicles, respectively. In this case, Point f determines the tp and Xp required for the passing vehicle. At this moment, the distance traveled by the opposing vehicle is Xo, which corresponds to its final position Xof (Point x). This approach ensures a stable and gradual redistribution of Point f by increasing the minimum safety margin.
The feasible vehicle acceleration performance corresponds to an upper limit oa and a lower limit that achieves the safety margin oc. The search for rational values and the law of speed change in the third phase (Figure 1c) depends on the difference between the speeds of the passing and impeding vehicles, which may exhibit values from the admissible minimum to the maximum being stipulated by the full performance mode (i.e., the upper limit oa at the beginning and completion of the lane change, Figure 3). Obviously, there is a need to optimize the movement trajectory in such a way as to ensure that both criteria (safety and power margins) account for possible changes in maneuver conditions (e.g., vehicle speeds and/or unpredictable forces).
Note that the geometric average scheme of Figure 3 is justifiable from two perspectives. First, when overtaking with full use of the engine energy, there is no more power reserve for unforeseen circumstances (e.g., gusts of wind, random forces, or control system failure). That is, in a critical situation, it would not be possible to compensate for their influence, especially if one (or both) of the other two vehicles suddenly increases their speed. Second, the maximum performance modes are undesirable in terms of the vehicle lateral stability and lateral sliding. If the speed is high, a stable lane change requires a greater distance, according to the criterion of limited lateral acceleration.

3.2. Vehicle Performance Thresholds

The lower limit of the vehicle acceleration performance is determined based on the velocity plan ensuring the minimum safety. The criterion of the minimization of energy consumption can be applied to the search for the time–distance curve adjusted for the curvilinear trajectory of the vehicle. The upper limit (curve oa) represents the vehicle potential provided with a full fuel supply under the ideal conditions of motion. The definition of the upper limit can be based on the characteristic of the vehicle dynamic factor (specific free traction force) restricted by the conditions of road surface adhesion and the reduced total movement resistance, including the road macro-profile. This upper limit may be estimated using mapping and GPS. The excess of the dynamic factor can be employed to accelerate the vehicle. Therefore, it is possible to build the speed–time and time–distance dependences for the acceleration mode, by which the necessary overtaking time and distance can be determined using the iterative method considered by Diachuk at al. [24]. The obtained values will represent the vehicle performance on a straight road section. To account for the curvilinear trajectory of the maneuver, the values of the time–distance curve can be adjusted.
An important stage of the forecasting is determination of the maximum vehicle acceleration capabilities (speed-acceleration). For this purpose, it is necessary to have a diagram of the free traction force (air resistance is subtracted) or a diagram of the dynamic factor (specific free traction force). If there is access to GPS signals and digitized terrain maps, it is possible to track the motion condition changes to correct control signals (throttle). In addition, a diagram of possible vehicle accelerations in the current conditions (Figure 4) is necessary considering movement resistance (surface quality) and adhesion to the road surface (weather conditions), since restrictions are needed for the optimal distribution of the speed plan. If the initial speed and the average speed are known, then for the linear constraints used in linear quadratic programming (LQP), the limiting values of acceleration achievable under given conditions can be determined.
If the measurements are known and the time–distance curve is determined for the case of the passing vehicle’s maximum performance [24], it is possible to immediately define whether the maneuver is feasible. For this, it is necessary to set, rightward of Point a (Figure 5), the safety margin tmm to Point b. The ratio of the inclination angles of opposing and impeding vehicles’ curves, providing an intersection in Point p and adequate space df + Lp to complete the maneuver, will ensure the time limit and the possible path of the maneuver. It is obvious that with a larger inclination angle of linear prediction for the opposing vehicle, the sensitivity’s influence on the remaining time safety margin t’mm is decreased. This does not mean that the vehicle will not be able to use the power corresponding to the segment ab; however, the maneuver execution in this mode will be associated with a decrease in the guaranteed level of safety. Therefore, the condition of the maneuver possibility is ta > tmin + t’mm, where t’mm ≤ tmm due to the lower sensitivity.
Another important point concerns the maximum allowable time required for overtaking. It is also possible that the time range tmin − tl according to the measured and evaluated data will be quite wide, which may lead to the calculation of large values of tp having no real meaning because of predicting a protracted maneuver. In this regard, the maximum maneuver time should also be limited based on the ratio of the times tmin and ta, tl.

3.3. Mathematical Formulation

To describe the movement laws of overtaking using the performance curve of, suppose that the measurements are evaluated at intervals of Δtm based on the preset frequency of the radar system. Then, the current discrete time will be Δtm, where n takes both positive and negative values relative to the starting point of the maneuver (sample n = 0). The results of the relative measurements at time tn are as follows:
( D | t n v o | t n d | t n v i | t n X p | t n V p | t n ) = ( D n v o n d n v i n X p n V p n ) ,
where Dn denotes the current measured distance between the passing and opposing vehicles; von denotes the current measured opposing vehicle speed; dn denotes the current measured distance between the passing vehicle and the rear of the impeding vehicle; vin denotes the current measured impeding vehicle relative speed; and Xpn and Vpn denote the estimated current self-position and speed of the passing vehicle, respectively.
Nevertheless, the speed forecast processing requires more time Δtpr and computational resources. In this regard, the algorithm should be organized to avoid frequent recalculations that do not significantly affect the quality of the forecast. Therefore, the time Δtpr must be a multiple of the time Δtmtpr = Δtm), where m is the factor of cycle multiplicity. This is provided that to process the forecast, the time Δtpr = t0T−1 is needed (where T−1 is the time before the forecast is made until t0) and that during this period, the vehicle speed does not change significantly, i.e.,
d X p d t | T 1 d X p d t | t 0 ,   d X o d t | T 1 d X o d t | t 0 ,   d X i d t | T 1 d X i d t | t 0
For the opposing and impeding vehicles, linear predictions can be made from a tangent angle k and the values at the point as (y − y0) = k (x − x0). The potential global positions of the opposing and impeding vehicles are determined relatively through the predicted movement of the passing vehicle. Therefore, while performing the maneuver, the current state vector assessment (Xpn, Vpn)T is periodically recalculated based on the sensor fusion technology. Then,
( X o n X i n ) = ( ( D n v o n d n + L i v i n ) + ( X p n V p n X p n V p n ) ) · ( 1 t t n )
To determine the locking time tl within the overtaking pocket, the condition for the intersection of linear predictions at Point p corresponding to tl is Xo = Xi. Note that Vo and the time at the moment before the forecast are negative, thus T−1 = − Δtpr = − m·Δtm. Consequently, the initial measurement is carried out in m cycles before the maneuver starts. That is,
( 1 1 ) T · ( X o X i ) = ( 1 1 ) T · ( ( D ( m ) v o ( m ) d ( m ) + L i v i ( m ) ) + ( X p ( m ) V p ( m ) X p ( m ) V p ( m ) ) ) · ( 1 t l + Δ t p r )
The passing vehicle position at time T−1 relative to t0 is estimated as Xp(−m) ≈ −Vp(−m)·Δtpr.
t l = D ( m ) d ( m ) L i v i ( m ) v o ( m ) m · Δ t m
To determine ta, the minimum distance Xo − Xi = dfmin+Lp is the distance between the impeding and passing vehicles. Therefore, ta can be defined by the difference (dfmin + Lp) between the functions yp and rp, similar to Equation (12). That is,
t a = t l d f m i n + L p v i ( m ) v o ( m )
To determine Point z of the diagonals’ intersection, the distances at Points l, h, n, and j corresponding to tmin, ts, using Equation (12), are
( X l X n X h X j ) = ( D ( m ) V p ( m ) · Δ t p r V p ( m ) + v o ( m ) d ( m ) + L i V p ( m ) · Δ t p r V p ( m ) + v i ( m ) ) · ( 1 1 t m i n + Δ t p r t s + Δ t p r )
The values of the coefficients k and b are determined from the matrix relations:
( X l X h X j X n ) = ( t m i n 1 t s 1 ) · ( k l j k h n b l j b h n )   and   ( k l j k h n b l j b h n ) = 1 t s t m i n · ( 1 1 t s t m i n ) · ( X l X h X j X n )
The intersection condition at tp is the equality of the ordinates of segments Xlj and Xhn:
X l j X h n = ( t p 1 ) T · ( k l j b l j ) ( t p 1 ) T · ( k h n b h n ) = ( t p 1 ) T · ( k l j k h n b l j b h n ) = 0   and   t p = b l j b h n k h n k l j
The equation of a line passing through the segment ab can be determined based on the coordinates of two points:
( X a X b ) = ( t m i n 1 t a 1 ) · ( k a b b a b )   and   ( k a b b a b ) = ( t m i n 1 t a 1 ) 1 · ( X a X b )
Therefore, the distance Xp corresponding to time tp is
X p = ( t p 1 ) T · ( k a b b a b ) = 1 t a t m i n · ( t p 1 ) T · ( 1 1 t a t m i n ) · ( X a X b )
The aim of this stage is to obtain the values of tp and Xp (Figure 3), which can be used for the optimal distribution of speeds and trajectory planning of the maneuver.

4. Quadratic Optimization Model

4.1. General

After the rational values of tp and Xp are determined using the heuristic algorithm previously described, the desired trajectory of motion is determined using a kinematic model. Such a model makes path planning simpler and faster and provides a smooth curve that can be adjusted, depending on the priorities of the kinematic parameters. This curve can serve as a reference for the control implementation of laws of the autonomous vehicle during maneuver realization.
Suppose that the overtaking maneuver is being planned for a relatively straight road section. Then, the formation of the longitudinal and transversal components of the speed plan can be considered independent. Consider the process of forming the longitudinal component of vehicle speed according to the direction of road marking lines (Figure 6). The values of the time and distance required for overtaking (tp, Xp) are determined before the maneuver starts at time t0. Obviously, there are many realizations of the distribution of the speed’s longitudinal component, such that their integral over the time interval (tp − t0) equals the distance Xp. These curves will at least differ in the value of VX and its derivative dVX/dt (acceleration) at the nodal points.
The speed distribution must also satisfy various requirements, such as technical, operational, economic, and safety requirements, which may be conflicting. The technical requirements are associated with the propulsion system to ensure the required vehicle’s performance under external constraints, such as a slope, road resistance, tire adhesion, and head wind. That is, the curvature of the speed plan should be comparable to the curvature of the output characteristics of a power unit operating in the transient mode. The operational requirements ensure that the traction and steering controls are smooth by imposing restrictions related to the vehicle’s steerability and stability. Therefore, significant speed and acceleration are undesirable at the moments of lane change. The economic requirements aim to minimize the work of the vehicle power unit by minimizing vehicle acceleration, since more fuel consumption occurs at the moments of speed change. From this perspective, the cumulative derivative of the speed plan curve should also be minimal. The safety requirements limit the maximum speed, acceleration, angular speed of the steering wheel, and time spent near the impeding vehicle.

4.2. Objective Function

The objective function of the model minimizes the speed variation, acceleration variation, and sharpness (differences between tangent coefficients of adjacent nodes). Therefore, the optimal speed distribution is written as
m i n V X J = W V · J V + W A · J A + W S · J S
where J denotes the objective function; WV, WA, and WS denote the weighting factors of speed, acceleration, and sharpness, respectively; and JV, JA, and JS denote corresponding integral functions.
The speed integral function is given by
J V = t 0 t p ( V X ( t ) V X a ) 2 d t = t 0 t p V X 2 ( t ) d t J V 2 · V X a · t 0 t p V X ( t ) d t J V + V X a 2 · t 0 t p d t c o n s t
where the last member that does not contain the variable qi (see Equation (A4), Appendix A) is omitted.
The speed integral must correspond to the distance Sp, considering Equations (A1), (A3), (A8), and (A10) (Appendix A). For the integral function JV’’,
t 0 t p V X ( t ) d t = i = 1 n ( 0 Δ T i f b T d τ ) · q i = i = 1 n g bi T · q i = g f T · q f = g T · E T · q f = g T · E T · M q · q
where g = (gb1, gb2, …, gbn)T, E = (E4, E4, …, E4)T, E4 denotes the identity matrix of the dimension 4 × 4, and Mq denotes the transition matrix from the vector q of degrees of freedom to the vector qf of repeating degrees of freedom of all finite elements (FE). Note that in adjacent FE, the values of the nodes on the right and left are repeated in the vector qf (e.g., q3i and q4i are equal to q1(i+1) and q2(i+1), respectively). Therefore, the excess degrees of freedom must be reduced by grouping node values instead of FE. That is, qf = Mq·q.
Now consider the integral of the square of speed V2(t), considering Equation (A11) (Appendix A):
J V = t 0 t p V X 2 ( t ) d t = i = 1 n q i T · D b i · q i
In vector-matrix form, Equation (22) can be written as
J V = ( q 1 q 2 q n ) q f T · ( D b 1 Z 4 Z 4 Z 4 Z 4 D b 2 Z 4 Z 4 Z 4 Z 4 Z 4 Z 4 D b n ) D · ( q 1 q 2 q n ) q f = q f T · D · q f
where Z4 = zero matrix (4 × 4).
The integral functions of the acceleration and sharpness are similar to Equation (22). Considering Equation (23) and Equations (A12) and (A13) (Appendix A), they yield
J A = t 0 t p ( d V X ( t ) d t ) 2 d t = i = 1 n q i T · G b i · q i = q f T · G · q f
J S = t 0 t p ( d 2 V X ( t ) d t 2 ) 2 d t = i = 1 n q i T · K b i · q i = q f T · K · q f
where G and K denote the matrices formed in the same format as D in Equation (23).
By leaving only the members containing the variables, the problem becomes equivalent to quadratic optimization. By substituting Equations (20)–(25), the objective function of Equation (19) becomes
m i n V X J = q T · H · q + 2 · L · q
where H denotes the equivalent of Hessian for combined speed, acceleration, and sharpness factors, and L denotes the vector reflecting the average level of speed.

4.3. Constraints

4.3.1. Lane Change-Related Constraint

The lane change is the first phase depicted in Figure 1a. The maneuver starts from a position that satisfies the condition of avoiding a blind spot and determines the length of the impeding vehicle (i.e., extremely close to the dashed marking line). The main requirement is that the full departure in the opposite lane should be completed before reaching the rear of the impeding vehicle. This approach ensures maximum security, especially before outrunning a long impeding vehicle, since its lateral behavior (possible swinging of a semitrailer) is unpredictable. Additionally, the tracking of the opposing vehicle’s position and speed is retained until the departure to the opposite lane. In an extreme case, a transverse movement may be allowed, where the minimum safe side distance between the passing and impeding vehicles is provided.
The time required for the passing vehicle to pass from State 1 to 2 (Figure 1a) is tt (Figure 3). Then, the longitudinal component of the path is
X p b = t 0 t t V X ( t ) d t
The condition for achieving State 2 (Figure 1a) is
X p b d 0 + X i b
The time tt can be determined iteratively (Figure 3) after the distribution of the longitudinal speed, based on Equations (27) and (28), considering Xpb = Xp(tt). According to a possible slight decrease in the impeding vehicle speed, Xpb may correspond to a small longitudinal gap between the front of the passing vehicle and the rear of the impeding vehicle. Thus,
X p ( t t ) d 0 ( V p 0 + v i 0 ) · t t 0
The path’s transverse component is
Y p b = t 0 t t V Y ( t ) d t
where VY(t) is represented similar to Equation (21).

4.3.2. Location in Opposite Lane Constraint

The upper limit of the passing vehicle transversal movement Ypo may correspond to its position in the middle of the opposite lane. Possible deflections of this position are restricted by safe clearance to the road edge (Figure 1b). A lower limit of this clearance is the minimum safe distance between passing and impeding vehicles. Therefore, for Xpo and Ypo,
X p o = t t t c V X ( t ) d t   and   Y p o = t t t c V Y ( t ) d t = 0
where tc can be found iteratively according to the condition when the passing and impeding vehicles are abreast at the critical point (State 3, Figure 1b), considering Xp(tc) = Xpb + Xpo (Point e, Figure 3).
According to a possible slight increase in the impeding vehicle speed, the passing vehicle’s front can be a little ahead of the front of the impeding vehicle. Thus,
X p ( t c ) ( d 0 + L i ) ( V p 0 + v i 0 ) · t c 0

4.3.3. Maneuver Completion

The lane change planning is similar to the first phase. Thus,
X p f = t c t p V X ( t ) d t   and   Y p f = t c t p V Y ( t ) d t

4.4. Preparing the Reference Trajectories

As a result of the optimization, the components of speeds (VX, VY)T in global coordinates are determined. Therefore, it is necessary to transfer the speeds to the local coordinates of the passing vehicle (Vx, Vy)T, in order to allow it to consider its maneuvering. Since the yaw angle φ is small,
( V X V Y ) = ( c o s ( ϕ ) s i n ( ϕ ) s i n ( ϕ ) c o s ( ϕ ) ) · ( V x V y )
( V x V y ) = ( c o s ( ϕ ) s i n ( ϕ ) s i n ( ϕ ) c o s ( ϕ ) ) · ( V X V Y ) ( 1 ϕ ϕ 1 ) · ( V X V Y )
The ideal forecast for the yaw angle can be obtained as a tangent to the motion trajectory:
ϕ = a r c t g ( d Y p d X p ) V Y V X
Therefore, to track a virtual trajectory, a state vector corresponding to the capabilities of the current measurements may be used: absolute displacements in global coordinates Xp, Yp; speeds in local coordinates Vx, Vy; and the yaw angle φ. As additional parameters, which can be directly measured on a vehicle, the accelerations that are components of the optimized speed plans reduced to the vehicle local coordinates may be used, as well as the yaw rate, which can be estimated indirectly as dφ/dt considering Equation (36).

5. Updating the Speed Plan

Each subsequent measurement determines the new position of the linear forecast. As previously mentioned, the influence of the fluctuations in the participants’ speed on the forecast reliability during the maneuver should be analyzed to avoid a redundant number of predictions. In Figure 3, the deviations in the proximity of Point f are shown, where the threshold values of changes in speeds of the opposing and impeding vehicles are reached. In Figure 7a1, the speed of the impeding vehicle increases in such a way that the linear curve exceeds the upper boundary prior to the moment ta, and the segment b’k’ slightly goes up (green), along the path curve of the impeding vehicle. Basically, the value of the minimum required distance dfmin depends on the difference between the speeds of the passing and impeding vehicles, and, thus, will vary with the fluctuations in movement modes of the overtaking participants. However, the changes will not have a significant affect, and therefore, dfmin can be considered constant in the vicinity of Point ta. The determination of dfmin is described elsewhere [24,25]. The bias of the intersection point of the trapezoid diagonals in z’ leads to shifting of the optimal Point f’ up to the left. The required time t’p becomes shorter and the needed space X’p becomes larger. This may be explained by the significant sensitivity of the forecast to the impeding vehicle’s speed changes.
In the case of Figure 7a2, the opposing vehicle speed increases and the upper boundary limit is violated. In this case, the inclination angles of the segments ab and ab’ are practically the same. However, even though the required time has decreased t’p < tp, unlike the previous case, the required space X’p decreases due to the larger space needed for the opposing vehicle. The most critical case is when the speed fluctuations of both the opposing and impeding vehicles reach the threshold boundaries simultaneously (Figure 7a3). The displacement of the minimum space segment dfmin + Lp for completing the maneuver can be so significant that the time t’s approaches the preset time tp, and the margin tmm in relation to the minimum performance mode will not be provided. Point t’p is located the furthest from Point tp, even though the space required for the maneuver may remain almost unchanged X’p ≈ Xp. Similarly, a decrease in the speed of opposing and impeding vehicles will give a lower limit of time fluctuations fL (Figure 3). However, such decreases are not dangerous, and it makes sense to only recalculate the forecast to save energy and increase the movement stability. Based on the described scheme, it is possible to determine the allowable level of deviations, at which the margin of minimum safety is kept without the necessary recalculation.
Therefore, for the case depicted in Figure 7b1, the diminished impeding vehicle speed means that a shorter distance and longer period are needed: X’p < Xp and t’p > tp. For the case in Figure 7b2, the diminished opposing vehicle speed demonstrates the need for a longer distance and time because of the reduced space Xo for the opposing vehicle: X’p > Xp and t’p > tp. For the case in Figure 7b3, the diminished speeds of both opposing and impeding vehicles move Point z’ quite far from z, providing a longer time in almost the same space: X’p ≈Xp and t’p > tp. This may cause the double margin time tmm with unreasonable energy consumption. Therefore, the passing vehicle speed mode may be reduced.
Other possible combinations, where one of the vehicles increases its speed and the other decreases its speed approximately simultaneously, give solutions that are not superior in nature to the changes discussed in Figure 7. Note that measurements of dn are available prior to the moment of alignment with the rear of the impeding vehicle, after which the last dn value may be fixed, and measurements of Dn can be carried out until the critical point. Recalculation after the critical point is possible if the next impeding vehicle appears in the lane, which does not provide a proper pocket or harshly reduces its speed.
Obviously, even in the automatic mode of maneuver execution, deviations of the passing vehicle speed are possible due to the influence of various random factors. However, within the thresholds set by fU, fL (Figure 3), forecast recalculation is not required. Therefore, the autonomous control system must adjust the speed mode of the passing vehicle not only according to measurement changes, but also considering the matching with its own reference curve (of in Figure 3).
The condition under which the specified safety level is retained and the forecast does not require recalculation is
{ t a > t p + t m m + t p r + t u n , i f t a < t a t a > t a + t m m ( t p r + t u n ) , i f t a > t a
where t’a denotes the instantaneous value of the hypothetical accident time compatible with predefined ta; tpr = Δtpr + Δtm = (m+p)·Δtm, where p denotes the number of spare measurement cycles, by default, p = 2; and tun denotes the unaccounted time expenses (e.g., engine transition mode and control delay).
Hence, t’a can be recalculated using Equation (13) for every n-th measurement at tn as follows:
t a = D n d n L i d f m i n L p v i n v o n + t n
As can be seen, the expression does not contain any absolute value, except for vehicles’ lengths, and only uses relative measurements, making it independent of the passing vehicle state parameters, including variance regarding its reference curve.

6. Overtaking Scenario modeling

6.1. Vehicle Model Description

Consider the simple linear single track (“bicycle”) vehicle model in the standard form of state-space:
d x / d t = A · x + B · u   and   y = C · x + D · u
where x denotes the state vector; y denotes the output vector; u denotes the control vector; and A, B, C, and D are matrices.
Suppose that the vehicle maintains its longitudinal speed Vx from Equation (34), and the state vector x only contains parameters for the front wheel steering control u = Θf. Therefore, x = (Vy, Y, ω, φ)T, where Y denotes the lateral displacement in global coordinates, and ω = dφ/dt denotes the yaw rate. Other parameters are denoted above. Provided D = 0, the matrices A, B, and C can be derived as
A = ( k f + k r m · V x 0 V x k f · x f + k r · x r m · V x 0 1 0 0 V x k f · x f + k r · x r I · V x 0 k f · x f 2 + k r · x r 2 I · V x 0 0 0 1 0 ) ,   B = ( k f m 0 x f · k f I 0 ) ,   C = ( 0 0 1 0 0 0 0 1 ) T
where m and I denote the vehicle mass and inertia, respectively; kf and kr denote the front and rear tires’ side stiffness, respectively; and xf and xr denote the local longitudinal coordinates of the front and rear tire spots, correspondingly. Therefore, the output variables for reference tracking are y = (Y, φ)T, which, in the real world, can be measured using a camera and sensors.

6.2. Adaptive Model Predictive Control Tracking Optimization Problem

According to the tracking problem, the control parameters should provide the values closest to the reference signals. Therefore, the cost function for the Adaptive Model Predictive Control (AMPC) [26] controller may be composed of the minimization of the sum of the squared errors, as follows:
m i n u J ( z k ) = ρ ε · ε k 2 + i = 0 p 1 ( e y , k + i T · Q y · e y , k + i + e u , k + i T · Q u · e u , k + i + e Δ u , k + i T · Q Δ u · e Δ u , k + i )
subject   to   e y , k + i = y k + i + 1 | k * y k + i + 1 | k ,   e u , k + i = u k + i | k * u k + i | k ,   e Δ u , k + i = u k + i | k u k + i 1 | k
where Qy, Qu, and Q∆u denote positive semi-defined weight matrices; y*k+i+1|k denotes the Plant output reference signals at the ith prediction horizon step; yk+i+1|k denotes the Plant outputs at the ith prediction horizon step; u*k+i|k denotes the Plant target reference signals at the ith prediction horizon step; uk+i|k denotes the Plant inputs (manipulated variables) at the ith prediction horizon step; zk = (uTk|k, uTk+1|k, ··· uTk+p−1|k, εk) denotes the solution; εk denotes the scalar dimensionless slack variable used for constraint softening at control interval k; ρε denotes the constraint violation penalty weight; k denotes the current control interval; and p denotes the prediction horizon (number of intervals).
The system of constraints is written as
{ y j , m i n ( i ) ε k · h j , m i n ( i ) ( y ) y j , k + i | k y j , m a x ( i ) + ε k · h j , m a x ( i ) ( y ) , i = 1 p , i = 1 n y u j , m i n ( i ) ε k · h j , m i n ( i ) ( u ) u j , k + i 1 | k u j , m a x ( i ) + ε k · h j , m a x ( i ) ( u ) , i = 1 p , i = 1 n u Δ u j , m i n ( i ) ε k · h j , m i n ( i ) ( Δ u ) Δ u j , k + i 1 | k Δ u j , m a x ( i ) + ε k · h j , m a x ( i ) ( Δ u ) , i = 1 p , i = 1 n Δ u
where yj,min(i) and yj,max(i) denote the minimum and maximum values of the jth output at the ith prediction horizon step, respectively; uj,min(i) and uj,max(i) denote the minimum and maximum values of the jth input at the ith prediction horizon step, respectively; Δuj,min(i) and Δuj,max(i) denote the minimum and maximum values of the jth input rate at the ith prediction horizon step, respectively; h(y)j,min(i) and h(y)j,max(i) denote the minimum and maximum values of the jth output’s hard constraints at the ith prediction horizon step, respectively; h(u)j,min(i) and h(u)j,max(i) denote the minimum and maximum values of the jth input’s hard constraints at the ith prediction horizon step, respectively; h(∆u)j,min(i) and h(∆u)j,max(i) denote the minimum and maximum values of the jth input rates’ hard constraints at the ith prediction horizon step, respectively; ny denotes the number of output parameters; nu denotes the number of input parameters; and nΔu denotes the number of input rate parameters.6.3. Simulink Model
A simplified Simulink model (Figure 8) that implements a virtual overtaking scenario on a two-lane highway has been developed. The main block 1 (SUV Plant) calculates the state vector of the vehicle’s continuous dynamic model, which presents a real vehicle and its sensor system measurements. Block 2 (SUV Model) calculates the vector of discrete states of the vehicle dynamic model, updating the necessary matrices and vectors at each time step. For simplicity, the same bicycle vehicle model is used as a Plant and Model. Block 3 (MPC) implements the Adaptive Model Predictive Controller, which calculates the optimal control values (steering angle), based on minimizing the sum of the square of the differences measured and predicted parameters: lateral displacement Y and yaw angle Phi, which is extracted from the Plant state vector by block 5 (MO Extractor). The vectors of the reference tracks Ref, being the desirable values of the vehicle model state parameters, are stored in the memory after optimization for reading at the corresponding time step. Block 4 (Conditions) sets the values for the vehicle’s local longitudinal speed Vx and the desired reference values Ref = (Y, Phi)T at the current time. Block 6 (Result) accumulates the calculated outputs. The model does not comprise external disturbances and measurement noise.

7. Application

The purpose of this application is to achieve stable control and ensure that the Plant state parameters fit those generated by the proposed methodology. The application involved setting initial conditions data, defining the parameters of the AMPC controller, and determining the desired reference tracks for speed, acceleration, and displacement in the global coordinates.

7.1. Initial Conditions Data

The Matlab/Simulink example for simulating overtaking was used [26]. According to the measurements, at time T−1 = −0.1 s, the initial data vector is formed as follows:
(D(−1), Vo(−1), d(−1), Vi(−1), Li(−1), Xp(−1), Vp(−1)) = (480, 70, 35, 65, 22.5, 0, 70),
where the linear dimensions are given in m and speeds in km/h. Using the technique described in [22], for the case of ideal motion conditions, the necessary values yield tmin = 7.79 s and Sfmin = 25 m. The minimum time margin was set as tmm = 1 s. Substituting these values into Equations (12)–(18) and (27)–(33) gives the following rational values (Figure 9a,b): overtaking global longitudinal projection Xp = 250 m, overtaking time tp = 8.9 ≈ 9 s, bypass time during lane change tt = 5.1 s, and time to the critical Point tc = 6.9 s.

7.2. Parameters of the AMPC Controller

The sampling time = 0.1 s, prediction horizon = 10 s, and control horizon = 2 s. The plant model has four states with two measured outputs. Weights: manipulated variable (steering angle) = 0 and manipulated variable rate (steering angle rate) = 0.1; output variables: lateral displacement = 0.8 and yaw angle = 0.1; constraints: −0.2 ≤ steering angle (rad) ≤ 0.2, −0.2 ≤ steering angle (rad/s) ≤ 0.2, 0 ≤ lateral displacement (m) ≤ 3.6, and −0.1 ≤ yaw angle (rad) ≤ 0.1.

7.3. Reference Speeds, Accelerations, and Displacements

Using the proposed optimization model, the desired reference tracks for speed, acceleration, and displacement in the global coordinates were determined, using the time grid with the increment of 0.1 s (Figure 9c–e). Note that the setting of linear constraints in Equation (40) should be consistent with the thresholds of the vehicle performance set by the distribution of acceleration upon speed (Figure 4) for the current conditions. This means that each speed value to be optimized in a time grid node is tied to the maximum possible acceleration at this speed, which creates non-linear constraints. In this regard, for each set of speed and acceleration thresholds in the optimization process, some rationale is needed, as previously described. Another difficult point before optimization is setting the final values of the speed and acceleration, since they significantly affect the trend of the entire speed plan. The speed value in the last node close to the average speed VXa (Figure 9d) may lead to the appearance of such a peak near the critical point, when the longitudinal accelerations in the phase of maneuver completion are negative and larger than the absolute value of 0.5 m/s2. That would mean the use of service braking and activation of the vehicle’s working brake system. From the point of view of ensuring the maximum vehicle stability during the lane change, it is undesirable to use the tire longitudinal force values close to those which may considerably reduce the tire’s lateral adhesion. In connection with the foregoing, it may be recommended to focus on the value of the speed Vpf at which the distribution of the speed plan requires decelerations, provided only by limiting the engine power consumption. In this case, the selected value Vpf ensures maneuver completion with acceleration close to zero.
As can be seen, the combination of the plan for longitudinal speeds and accelerations fits the performance limitations well in Figure 4. At the same time, the projection curve of the overtaking path on the global X axis clearly corresponds to 250 m (Figure 9d). This, when copied to Figure 9a’s curve, shows that in the initial phase, the vehicle uses a potential close to the upper limit (black line). The next important point is the conditionality of the weighting factors in the optimization of Equation (19). Note that the ratio of weight coefficients significantly changes the optimization picture in connection with the change of priorities. Increasing the WV coefficient very much reduces the speed consumption, but significantly increases the need for acceleration at the beginning of the maneuver. The increase of the WA coefficient reduces the cumulative consumption of acceleration, but does not provide smoothness in the boundary zones of the speed plan, and the peak speed value rises. Increasing the WS coefficient distributes speeds evenly over time.
Therefore, in the current case of optimizing the longitudinal plan, the stable engine’s performance is the most important, minimizing abrupt transitions in its control; respectively, the values for the entire overtaking maneuver are chosen: WV = 0.2, WA = 0.2, and WS = 0.6. In the distribution of transverse speeds of the bypass phase, the priority is divided between the control smoothness and the cumulative acceleration intake: WV = 0.2, WA = 0.4, and WS = 0.4. In the final phase, due to the lane change at high speeds, the main priority is focused on reducing the lateral accelerations, respectively: WV = 0.1, WA = 0.6, and WS = 0.3. It is obvious, however, that priorities may vary, depending on the situation.
It should also be noted that the value of the vehicle’s initial lateral position does not correspond to the lane center, but is offset by 0.4 m from the dashed line to ensure the conditions previously described.
Figure 10 shows the overtaking results by predicting the lateral offset and yaw angle. As can be seen in Figure 10a, at the 9th second of overtaking, the trajectory longitudinal component practically corresponds to the pre-set one, with a final value of Xp = 250 ≈ 249.4 m, and the transverse component Yp is strictly within 3.6 m, but has a residual of 0.23 m at the time tp = 9 s. At this moment, the passing vehicle is almost in the middle of its lane and continues stable movement, i.e., the situation is uncritical. The AMPC controller calculates the discrete control signal based on the information on the previous value and reference tracks. However, it is almost impossible to avoid tracking delay completely. The same effect can be observed in relation to the lateral speed VY (Figure 10b), which coincides in terms of shape and values with the initial one in Figure 9d, but lags a bit in time.
In general, by using the vehicle refined models of Plant and Model (Figure 8) and including a larger number of state parameters for tracking and other measures, the model convergence can be improved.

8. Concluding Remarks

This article has presented a methodology for distributing the speed in the longitudinal and lateral directions when a vehicle is overtaking on two-lane highways in an automated mode. An advantage of the kinematic technique used in the model is its ability to predict both speed and acceleration references, providing subsequent tracking control based on sensor measurements. In addition, this technique can be successfully used as a component of the model predictive control for generating reference trajectories. Based on this study, the following comments are offered.
In this study, the ‘kinematic model’ was not exactly used in the sense of a vehicle model, but the output kinematic parameters of the vehicle motion were used to predict both the trajectory and the overtaking mode, considering the speed distribution directly and acceleration distribution indirectly, as part of the FE speed model. In this regard, we followed the well-established trend of planning motion paths with various curves and graphs, as presented in the literature [17]. However, we obtained trajectories as integrals of the velocity plans. Unlike other studies, the proposed approach focused on using an FE grid that simultaneously ensured a high accuracy and compliance with the real capabilities of the vehicle engine-drive system. In particular, the interaction between possible speeds and the respective accelerations was considered.
In the quadratic optimization model for the speed-plan distribution, three measures were considered (speed, acceleration, and sharpness). The two parameters (tp and Xp) characterize the average speed Vxa and are obviously not enough, because many curves integrable in the interval (t0, tp) can give the same Xp. In addition, the vehicle’s ability to increase speed is a function of the speed and depends on the vehicle’s characteristics (i.e., acceleration is a 3D surface as a function of vehicle speed and throttle activation level). Therefore, in this study, it was assumed that the speed nodes are interconnected by curves over time, differentiable at least twice (i.e., providing smoothness). Then, the derivatives at the nodes which reflect the slope (acceleration) and the curvature (sharpness) were included as members of the objective function, along with their weights. Since this is an FE model of the curve, no other parameters except for the nodal Vx, dVx/dt, and d2Vx/dt2 can be included, because only the nodes need to be distributed, consistent with the vehicle technical features.
The general idea of using the overtaking heuristic algorithm is to guarantee maneuver completion, prior to the maneuver beginning, with at least the minimum safety margin. The algorithm emanates from individual vehicle operational properties. This allows the maximum and minimum performances to be predicted for given initial conditions. Therefore, considering possible changes due to random factors, for example, the algorithm will provide a decision to only pass when maneuver safety is ensured. In addition, the algorithm can foresee the situation when a vehicle must have a power margin, and whether maneuver completion requires additional acceleration to prevent a possible collision. Note that cancelling the overtaking maneuver is possible. However, within the framework of the presented algorithm, cancellation could happen in occasional situations (e.g., sudden swinging of the impeding vehicle). In this case, a switch to another algorithm is needed and this should be investigated in the future.
Determination of the required overtaking time is based on the safety margin and adequate engine power. These criteria allow possible adjustments of the power consumption and safety time, depending on the situation changes and priorities. A simple, but quite effective, technique was proposed in the heuristic algorithm for finding one rational curve from an infinite number of possible realizations. This is the reason why two additional derivatives (acceleration and sharpness) were used for finding the best speed distribution. All these measures aim at ensuring strict vehicle power stability and consequently, safety. In addition, by considering the lateral acceleration and yaw rate (indirectly) in the steering control prognosis, motion instability and possible sideslip can be prevented. Therefore, maneuver safety has been adequately incorporated in the proposed framework.
In this study, the roadway is assumed to be straight, with ideal surface friction and no external forces (e.g., gusts of wind). In addition, the road is assumed to have very gentle vertical curvature. These assumptions are necessary, since sensor measurements can be particularly affected by the slope and direction of the road, being unable to measure the position and speed of the opposing vehicles in various circumstances. Fortunately, this does not represent a limitation of the model, since overtaking maneuvers on two-lane highways are not permitted on sharp vertical curves because they require very long vertical curves that are expensive to construct [27,28].
Further research can be conducted to improve the algorithm for finding the optimal speed distribution for overtaking. Areas of focus may include: (1) influence of the final speed of the maneuver on the nature of the optimal speed plan; (2) influence of the weight coefficients on the speed plan; and (3) modeling of obstacle avoidance in autonomous overtaking. By grouping the longitudinal and lateral components of the reference trajectories, the kinematic technique can be used to simulate the obstacle-avoidance trace for autonomous vehicles. In addition, a sensitivity analysis of the proposed framework can be conducted to understand the impact of certain problem parameters on the overall results, including the effect of uncertainty.

Author Contributions

Conceptualization, S.M.E. and M.D.; methodology, M.D. and S.M.E.; software, M.D.; validation, S.M.E.; formal analysis, S.M.E.; investigation, M.D.; resources, S.M.E.; data curation, S.M.E.; writing-original draft preparation, M.D. and S.M.E.; writing-review and editing, S.M.E. and M.D.; visualization, M.D.; supervision, S.M.E.; project administration, S.M.E.; funding acquisition, S.M.E. All authors have read and agreed to the published version of the manuscript.


This research was sponsored by the Natural Sciences and Engineering Research Council of Canada (NSERC).


The authors are grateful to three anonymous reviewers for their thorough and most helpful comments.

Conflicts of Interest

The authors declare no conflicts of interest. The funders had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript; or in the decision to publish the results.


The following abbreviations are used in this manuscript:
NMPCModel Predictive Control
RRTRapid Random Tree
GPSGlobal Positioning System
LQPLinear Quadratic Programming
AMPCAdaptive Model Predictive Control
SUVSingle Unit Vehicle
MOMeasured Outputs
FEFinite Element

Appendix A: Representing the Speed Function by Finite Elements

Suppose that the speed of the passing vehicle within the time interval (t0, tp) varies along the X-coordinate of the road segment 0Xp, according to the law VX(t). Then, for a grid of n time intervals (t0, t1, t2, …, tn),
X p = t 0 t p V X ( t ) d t = i = 1 n t i 1 t i V X ( t ) d t = i = 1 n 0 Δ T i V X i ( τ , Δ T i ) d τ
where ΔTi is the time interval (ti − ti−1), which is generally variable.
Using the FE method for the piecewise representation of the speed function of each interval, for the i-th time segment (ti−1, ti),
V X i ( τ , Δ T i ) = k = 1 4 q k i · f τ k ( τ , Δ T i )
where τ [0, ΔTi] is the FE local time; q1i, q2i, q3i, and q4i represent impact coefficients, where q1i and q3i are speeds at FE nodes and q2i and q4i are accelerations (derivatives) at the corresponding nodes; and fτ1, fτ2, fτ3, and fτ4 are basis functions.
Thus, using matrix notation,
V X i ( τ , Δ T i ) = f b T ( τ , Δ T i ) · q i = f b T · q i
f b = ( f τ 1 f τ 2 f τ 3 f τ 4 ) T ,   q i = ( q 1 i q 2 i q 3 i q 4 i ) T
The normalized basis function fξ for an FE of a unitary length (ΔT = 1) is based on the cubic polynomial with two degrees of freedom at a node, providing smoothness and continuous differentiability, as follows:
f ξ = ( f ξ 1 f ξ 2 f ξ 3 f ξ 4 ) = ( ( 2 · ξ + 1 ) · ( ξ 1 ) 2 ξ · ( ξ 1 ) 2 ξ 2 · ( 2 · ξ 3 ) ξ 2 · ( ξ 1 ) ) ,   l Δ T = d i a g ( 1 Δ T 1 Δ T )
where ξ [0, 1] is the normalized coordinate.
Let τ = ξ·ΔT. Then, the transition between the absolute and normalized basis functions is given by
f b = f b ( τ , Δ T ) = f b ( Δ T · ξ , Δ T ) = l Δ T · f ξ .
Considering the basis functions defined only within the FE interval, the relation for the entire speed can be written as
V X ( t ) = i = 1 n f b T ( τ , Δ T i ) · q i = i = 1 n f b i T · q i .
Since the function also uses (up to second) derivatives of FE basis functions, the first and second derivatives are obtained, respectively, as
d f b d τ = l Δ T Δ T · d f ξ d ξ
d 2 f b d τ 2 = l Δ T Δ T 2 · d 2 f ξ d ξ 2
Consider the formation of common integrals, replacing the differential dτ = dξ·ΔT and thresholds. Since qi does not depend on τ, only the basis functions (Equation (A5)) are integrated.
g b = 0 Δ T f b ( τ , Δ T ) d τ = Δ T · l Δ T · 0 1 f ξ d ξ
D b = 0 Δ T f b · f b T d τ = Δ T i · l Δ T · ( 0 1 f ξ · f ξ T d ξ ) · l Δ T T
G b = 0 Δ T d f b d τ · d f b T d τ d τ = l Δ T Δ T · ( 0 1 d f ξ d ξ · d f ξ T d ξ d ξ ) · l Δ T T
K b = 0 Δ T d 2 f b d τ 2 · d 2 f b T d τ 2 d τ = l Δ T Δ T 3 · ( 0 1 d 2 f ξ d ξ 2 · d 2 f ξ T d ξ 2 d ξ ) · l Δ T T


  1. Easa, S.M. Automated highways. In Encyclopaedias of Electrical and Electronics Engineering; Webster, J.G., Ed.; John Wiley & Sons: New York, NY, USA, 2017; pp. 79–88. [Google Scholar]
  2. Al-Qaysi, Q.; Easa, S.M.; Ali, N. Proposed Canadian automated highway system architecture: Object-oriented approach. Can J. Civ. Eng. 2003, 30, 955–969. [Google Scholar] [CrossRef]
  3. Narla, S.; Stowell, H. Connected and Autonomous Vehicles; Institute of Transportation Engineers: Washington, DC, USA, 2019. [Google Scholar]
  4. Ozbay, K. Developments in connected and automated vehicles. Intell. Transp. Syst. J. 2018, 22, 187–189. [Google Scholar] [CrossRef]
  5. Shladover, S. Connected and automated vehicle systems: Review. Intell. Transp. Syst. J. 2018, 22, 190–200. [Google Scholar] [CrossRef]
  6. National Cooperative Highway Research Program. Connected and Autonomous Vehicles and Transportation Infrastructure Readiness; Project 20–24(111); Transportation Research Board: Washington, DC, USA, 2017. [Google Scholar]
  7. Goldin, P. 10 Advantages of Autonomous Vehicles. ITSdigest. 2018. Available online: (accessed on 13 May 2020).
  8. Schwarting, W.; Alonso-Mora, J.; Paull, L.; Karaman, S.; Rus, D. Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model. IEEE Trans. Intell. Transp. Syst. 2018, 19, 2994–3008. [Google Scholar] [CrossRef] [Green Version]
  9. Talamino, J.P.; Sanfeliu, A. Anticipatory kinodynamic motion planner for computing the best path and velocity trajectory in autonomous driving. Robot. Auton. Syst. 2019, 114, 93–105. [Google Scholar] [CrossRef]
  10. González, D.; Pérez, J.; Milanés, V.; Nashashibi, F. A review of motion planning techniques for automated vehicles. IEEE Trans. Intell. Transp. Syst. 2016, 17, 1135–1145. [Google Scholar] [CrossRef]
  11. Gu, T.; Dolan, J.M.; Lee, J. Automated tactical maneuver discovery, reasoning and trajectory planning for autonomous driving. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Daejeon, Korea, 9–14 October 2016; pp. 5474–5480. [Google Scholar]
  12. Wang, F.; Yang, M.; Yang, R. Conflict-probability-estimation-based overtaking for intelligent vehicles. IEEE Trans. Intell. Transp. Syst. 2009, 10, 366–370. [Google Scholar] [CrossRef] [Green Version]
  13. Kala, R.; Warwick, K. Motion planning of autonomous vehicles on a dual carriageway without speed lanes. Electronics 2015, 4, 59–81. [Google Scholar] [CrossRef] [Green Version]
  14. Babu, M.; Oza, Y.; Singh, A.K.; Krishna, K.M.; Medasani, S. Model predictive control for autonomous driving based on time scaled collision cone. arXiv 2018, arXiv:1712.04965. [Google Scholar]
  15. Tomas-Gabarron, J.-B.; Egea-Lopez, E.; Garcia-Haro, J. Optimization of vehicular trajectories under Gaussian noise disturbances. Future Internet 2013, 5, 1–20. [Google Scholar] [CrossRef] [Green Version]
  16. Liu, C.; Zhan, W.; Tomizuka, M. Speed profile planning in dynamic environments via temporal optimization. In Proceedings of the IEEE Intelligent Vehicles Symposium IV, Los Angeles, CA, USA, 11–14 June 2017; pp. 154–159. [Google Scholar]
  17. Berntorp, K.; Weiss, A.; Danielson, C.; Kolmanovsky, I.V.; Cairano, S.D. Automated driving: Safe motion planning using positively invariant sets. In Proceedings of the IEEE 20th International Conference on Intelligent Transportation Systems, Yokohama, Japan, 16–l9 October 2017; pp. 1–6. [Google Scholar]
  18. Hu, X.; Chen, L.; Tang, B.; Cao, D.; He, H. Dynamic path planning for autonomous driving on various roads with avoidance of static and moving obstacles. Mech. Syst. Signal Process. 2018, 100, 482–500. [Google Scholar] [CrossRef]
  19. Molinari, F.; Anh, N.N.; Re, L.D. Efficient mixed integer programming for autonomous overtaking. In Proceedings of the American Control Conference, Seattle, WA, USA, 1 May 2017; pp. 2303–2308. [Google Scholar]
  20. Murgovski, N.; Sjöberg, J. Predictive cruise control with autonomous overtaking. In Proceedings of the 54th IEEE Conference on Decision and Control, Osaka, Japan, 15–18 December 2015; pp. 644–649. [Google Scholar]
  21. Németh, B.; Gáspár, P.; Hegedűs, T. Optimal control of overtaking maneuver for intelligent vehicles. J. Adv. Transp. 2018. [Google Scholar] [CrossRef]
  22. Hassein, U.; Diachuk, M.; Easa, S.M. In-vehicle passing collision warning system for two-lane highways considering driver characteristics. Transp. Res. Rec. 2018, 2672, 101–112. [Google Scholar] [CrossRef]
  23. Benjamin, J.R.; Cornell, C.A. Probability, Statistics, and Decision for Civil Engineers; Dover Publications: Mineola, NY, USA; McGraw-Hill: New York, NY, USA, 2014; pp. 1–704. [Google Scholar]
  24. Diachuk, M.; Easa, S.M.; Hassein, U.; Shihundu, D. Modeling passing maneuver based on vehicle characteristics for in-vehicle collision warning systems on two-lane highways. Transp. Res. Rec. 2019, 2673, 165–178. [Google Scholar] [CrossRef]
  25. Diachuk, M.; Easa, S.M. Guidelines for roundabout circulatory and entry widths based on vehicle dynamics. J. Traffic Transp. Eng. 2018, 5, 361–371. [Google Scholar] [CrossRef]
  26. MathWorks. Available online: (accessed on 1 October 2019).
  27. American Association of State Highway and Transportation Officials (AASHTO). A Policy on Geometric Design of Highway and Streets; AASHTO: Washington, DC, USA, 2018. [Google Scholar]
  28. Easa, S.M. Geometric design. In Civil Engineering Handbook; Chapter 63; Chen, W.F., Liew, J.Y., Eds.; CRC Press: Boca Raton, FL, USA, 2002. [Google Scholar]
Figure 1. Phases of overtaking: (a) obstacle-rear reach, (b) obstacle-front reach, and (c) maneuver completion.
Figure 1. Phases of overtaking: (a) obstacle-rear reach, (b) obstacle-front reach, and (c) maneuver completion.
Infrastructures 05 00044 g001
Figure 2. Logic of the proposed speed control model for the overtaking of autonomous vehicles.
Figure 2. Logic of the proposed speed control model for the overtaking of autonomous vehicles.
Infrastructures 05 00044 g002
Figure 3. Logic for selecting rational values of time and distance for overtaking.
Figure 3. Logic for selecting rational values of time and distance for overtaking.
Infrastructures 05 00044 g003
Figure 4. Vehicle acceleration as a function of speed (example of Audi A4 Quattro).
Figure 4. Vehicle acceleration as a function of speed (example of Audi A4 Quattro).
Infrastructures 05 00044 g004
Figure 5. Limiting case of the maximum performance.
Figure 5. Limiting case of the maximum performance.
Infrastructures 05 00044 g005
Figure 6. Different speed plan distributions with fixed values of tp and Xp.
Figure 6. Different speed plan distributions with fixed values of tp and Xp.
Infrastructures 05 00044 g006
Figure 7. Influence of threshold values of speed fluctuations on the prediction reliability: (a) Increase in speeds of opposing and impeding vehicles (Cases 1-3); (b) decrease in speeds of opposing and impeding vehicles (Cases 1-3).
Figure 7. Influence of threshold values of speed fluctuations on the prediction reliability: (a) Increase in speeds of opposing and impeding vehicles (Cases 1-3); (b) decrease in speeds of opposing and impeding vehicles (Cases 1-3).
Infrastructures 05 00044 g007
Figure 8. Simulink model of overtaking scenario execution.
Figure 8. Simulink model of overtaking scenario execution.
Infrastructures 05 00044 g008
Figure 9. Planning reference tracks for state parameters: (a) Vehicle path’s prognosis; (b) definition of point tp; (c) predicted passing vehicle’s global displacements; (d) plan of global velocities; (e) plan of global accelerations.
Figure 9. Planning reference tracks for state parameters: (a) Vehicle path’s prognosis; (b) definition of point tp; (c) predicted passing vehicle’s global displacements; (d) plan of global velocities; (e) plan of global accelerations.
Infrastructures 05 00044 g009
Figure 10. Simulation results of vehicle steering control prognosis during overtaking: (a) Global displacements; (b) steering control and lateral speed projection.
Figure 10. Simulation results of vehicle steering control prognosis during overtaking: (a) Global displacements; (b) steering control and lateral speed projection.
Infrastructures 05 00044 g010

Share and Cite

MDPI and ACS Style

Easa, S.M.; Diachuk, M. Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways. Infrastructures 2020, 5, 44.

AMA Style

Easa SM, Diachuk M. Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways. Infrastructures. 2020; 5(5):44.

Chicago/Turabian Style

Easa, Said M., and Maksym Diachuk. 2020. "Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways" Infrastructures 5, no. 5: 44.

Article Metrics

Back to TopTop