Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways
Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways
Easa, Said M.;Diachuk, Maksym
2020-05-18 00:00:00
infrastructures Article Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways Said M. Easa * and Maksym Diachuk Department of Civil Engineering, Ryerson University, Toronto, ON M5B 2K3, Canada; maksym.diachuk@ryerson.ca * Correspondence: seasa@ryerson.ca; Tel.: +1-416-979-5000 (ext. 7868) Received: 5 April 2020; Accepted: 14 May 2020; Published: 18 May 2020 Abstract: 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 aect the maneuver eectiveness 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. Keywords: autonomous vehicles; speed planning; optimization; required passing time; two-lane highways 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–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 trac congestion, reduce carbon dioxide emissions, increase the highway capacity, reduce fuel consumption, improve public transportation services, and provide more ecient 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. Infrastructures 2020, 5, 44; doi:10.3390/infrastructures5050044 www.mdpi.com/journal/infrastructures Infrastructures 2020, 5, 44 2 of 25 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 trac 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 ecient 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 G -splines. The polynomial fitting involved iterations equivalent to the optimization of curvature parameters. To simulate a suciently 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 dierent 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. Dierent 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 eectiveness 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 Infrastructures 2020, 5, 44 3 of 25 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 dierent 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. Dierent 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–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 sucient 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 trac 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 X , and during this time, pb the impeding vehicle travels the distance X . In Phase b (obstacle front reach), the passing vehicle ib 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 X and po X , respectively. io In Phase c (maneuver completion), the passing vehicle passes the longitudinal distance X to pf ensure an adequate safe distance d between itself and the impeding vehicle, traveling the distance X , as shown in Figure 1c. The passing and opposing vehicles must provide a safety margin distance if X between their fronts which is equivalent to the minimum safety margin time t . Note that in m mm this phase, the lateral movement back to the original lane will not start until the passing vehicle has already overtaken the impeding vehicle. Infrastructures 2020, 5, 44 4 of 25 Infrastructures 2020, 5, x FOR PEER REVIEW 4 of 26 State 1 State 2 (a) State 2 State 3 (b) (c) Figure 1. Figure 1.Phases of o Phases of v oertaking: ( vertaking: a) obstacle-rear reach, ( (a) obstacle-rear reach, b () b obstacle-front ) obstacle-fron reach, and t reach, and(c () maneuv c) maneuv er e r completion. completion. 2.2. Assumptions (b) Before the overtaking maneuver starts, it is assumed that the estimation of the vehicles' positions ha The s alrea developed dy been ca system rried ou involves t, the fthe orecast following has been assumptions: made, and the passing vehicle is ready to start the passing maneuver. In this state, the distance D between the passing and opposing vehicles (a) The roadway is assumed to be straight, with ideal surface friction and no external forces, such as and the distance d between the impeding and passing vehicles are estimated using long- and gusts of wind. In addition, the road is assumed to have very gentle vertical curvature that would short-range radars, respectively; not obstruct sensor measurements; (c) The length of the impeding vehicle is estimated using machine vision technology. The sensor (b) Before the overtaking maneuver starts, it is assumed that the estimation of the vehicles’ positions measurements (distance and angle) are assumed to be independent; has already been carried out, the forecast has been made, and the passing vehicle is ready to start (d) The AV is assumed to possess the input data required for the proposed model: roadway data the passing maneuver. In this state, the distance D between the passing and opposing vehicles (e.g., lane width and speed limit) and vehicle characteristics (e.g., acceleration–speed relation); and the distance d between the impeding and passing vehicles are estimated using long- and (e) Only the passing vehicle is autonomous, and the impeding and opposing vehicles are assumed short-range radars, respectively; to be human-driven vehicles. The overtaking maneuver involves only one impeding vehicle; (c) The length of the impeding vehicle is estimated using machine vision technology. The sensor (f) In the heuristic algorithm, it is assumed that the actual speed fluctuations detected by the radars measurements (distance and angle) are assumed to be independent; remain within the uncertainty thresholds, for which the forecast is considered reliable. Therefore, (d) The AV is assumed to possess the input data required for the proposed model: roadway data the changes of vehicle locations will occur within the boundaries determined by the specified (e.g., lane width and speed limit) and vehicle characteristics (e.g., acceleration–speed relation); measurement thresholds. If the threshold is exceeded, the forecast should be recalculated; (e) Only the passing vehicle is autonomous, and the impeding and opposing vehicles are assumed to (g) Uncertainty is considered in the estimation of the speeds of the passing and impeding vehicles. be human-driven vehicles. The overtaking maneuver involves only one impeding vehicle; Based on the uncertainty thresholds, possible violations of the thresholds are analyzed. No uncertainty propagation throughout the prediction process is considered in this study; Infrastructures 2020, 5, 44 5 of 25 (f) 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; (g) 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; (h) 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 t and distance X required for safe maneuver completion are p p 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. Infrastructures 2020, 5, x FOR PEER REVIEW 6 of 26 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. ଶ ଶ ଶ ݇ ൌ ቀ ሺ ݀ ݀ ሻ ڄ ሺ ͳܿݏ ሺ ߠ െߠ െͶڄ݀ ڄ݀ ڄܿݏ ሺ ߠ െߠ ሻ ቁ ǡ ௗ ାଵ ାଵ ାଵ ାଵ ଶ ଶ ଶ ݇ ൌʹڄ݀ ڄ݀ ڄݏ݅݊ ሺ ߠ െߠ ሻ . ାଵ ାଵ Then, by applying Equation 3 to Equation 2, the standard deviation of the speed of the opposing vehicle, Η , is obtained as Vopp ଶ ଶ ଶ ଵ ଵଵ ට (5) ߪ ൌ ቀ ቁ ڄߪ ቀ ቁ ڄߪ ቀ ቁ ڄߪ . ௗ௩ ି ௗ௩ ି ௗ௩ ି భ భ మ మ య య ଷڄ௱௧ ௧ڄ௱ ڄ௱௧ Similarly, the deterministic distance travelled by the impeding vehicle between consecutive time intervals before overtaking is ܸ݀ ൌ݀ െ݀ ݀ ǡ ݅ ൌ (6) ାଵ The standard deviation of the distance dv - imp , Η , of Equation 6 is given by i i dV imp ଶ ଶ ߪ ൌ ඥ ݀ ݀ . (7) ௗ ାଵ Then, the speed of the impeding vehicle V and its standard deviation Η are calculated using imp Vimp Equations 2 and 5, after replacing dV with dV (i denotes 1, 2, and 3). oppi impi 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 V opp ± 2·Η and V ± 2·Η , respectively. Vopp imp Vimp 3. Heuristic Algorithm 3.1. General ͵Ǥͳݐ ሻሻ Infrastructures 2020, 5, 44 6 of 25 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 < t , 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, Dt apart, four distances to the opposing vehicle d and the corresponding azimuth angles are recorded, i i 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): 2 2 ( ) dV = d + d 2d d cos dp , i = 1 to 3, (1) opp i i+1 i+1 i i i i+1 where dV denotes the distance traveled by the opposing vehicle during Dt and dp denotes the oppi i distance traveled by the passing vehicle during Dt. The speed of the opposing vehicle, V , can then opp be derived as 1 7 11 V = dV dV + dV . (2) opp opp opp opp 1 2 3 3Dt 6Dt 6Dt Let the errors in d and measurements of the radar be denoted by e and e , respectively. Then, i i d a these errors will propagate and produce an error in V of Equation (2). To calculate this error, let the opp four variables of Equation (1) (d , , d , and ) be denoted by x , where i denotes 1 to 3. Using the i i i+1 i+1 i Taylor series, the standard deviation of Y, , was given by Benjamin and Cornell [23]: n 2 @ f = , (3) xi @x i=1 where denotes the standard deviation (SD) of random variable x . By applying Equation (3) to xi i Equation (1), the standard deviation of dV , , can be derived as oppi dVoppi 2 2 k e + k e d a = , (4) dV opp dV + dp opp i where 2 2 2 k = d + d 1 + cos ( ) 4d d cos( ) , d i+1 i i i+1 i+1 i i i+1 2 2 k = 2d d sin ( ). a i+1 i i i+1 Then, by applying Equation (3) to Equation (2), the standard deviation of the speed of the opposing vehicle, , is obtained as Vopp 2 2 2 1 7 11 = + + . (5) opp dv opp dv opp dv opp 1 1 2 2 3 3 3Dt 6Dt 6Dt Similarly, the deterministic distance travelled by the impeding vehicle between consecutive time intervals before overtaking is dV = d d + dp , i = 1 to 3. (6) imp i+1 i i i Infrastructures 2020, 5, 44 7 of 25 The standard deviation of the distance dv imp , , of Equation (6) is given by i i dVimpi 2 2 = d + d . (7) dV imp i i+1 Then, the speed of the impeding vehicle V and its standard deviation are calculated using imp Vimp Equations (2) and (5), after replacing dV with dV (i denotes 1, 2, and 3). oppi impi 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 V 2 and V 2 , respectively. opp Vopp imp Vimp 3. Heuristic Algorithm Infrastructures 2020, 5, x FOR PEER REVIEW 7 of 27 3.1. General The heuristic algorithm estimates the rational time and distance required for overtaking. The logic The heuristic algorithm estimates the rational time and distance required for overtaking. The of the algorithm is presented in Figure 3. For each relative position of the vehicles involved in the logic of the algorithm is presented in Figure 3. For each relative position of the vehicles involved in overtaking maneuver, a combination of measurements will be unique, and the number of possible 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 maneuvers will be numerous. Suppose that at a certain time, the speed values (sample) of the and opposing vehicles have been estimated using the passing vehicle sensory system (noise variances impeding and opposing vehicles have been estimated using the passing vehicle sensory system (noise are supposed to be known). At the initial time, the impeding and opposing vehicles are located at variances are supposed to be known). At the initial time, the impeding and opposing vehicles are distances d and D , respectively, relative to the passing vehicle, where the index ( m) means the located at distances d(-m) and D(-m), respectively, relative to the passing vehicle, where the index (-m) ( m) ( m) backward number of radar measurement cycles required before the prediction is made. Considering means the backward number of radar measurement cycles required before the prediction is made. the time required for prediction, the values at the maneuver ’s beginning time t will become d and D , Considering the time required for prediction, the values at the maneuver's beginning time t will 0 0 0 0 respectively. If the opposing and impeding vehicles continue their motions at speeds close to the become d and D , respectively. If the opposing and impeding vehicles continue their motions at 0 0 measured ones, then the change in their positions will be approximately linear, which determines speeds close to the measured ones, then the change in their positions will be approximately linear, the slope (dX/dt) of the corresponding curves (blue and green curves in Figure 3). In turn, the speed which determines the slope (dX/dt) of the corresponding curves (blue and green curves in Figure 3). measurements also have uncertainty. In turn, the speed measurements also have uncertainty. 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. Point a (t , X ) corresponds to the minimum time and distance, while Point b (t , X ) min pmin max pmax 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 (t , X ), a distance p p (d + L ) would be needed to complete the maneuver, where the linear segment ab represents a set of f p 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 d fmin (Figure 3) should be predetermined. The safety margin t guarantees a distance between the passing and opposing vehicles after mm 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 t . This may be the key point for determining the threshold conditions. Infrastructures 2020, 5, 44 8 of 25 Point a (t , X ) corresponds to the minimum time and distance, while Point b (t , X ) min pmin max pmax 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 (t , X ), a distance p p (d + L ) 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 d (Figure 3) fmin should be predetermined. The safety margin t guarantees a distance between the passing and opposing vehicles after mm 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 t . 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 (d + L ) for completing the maneuver (segment bk) will correspond to t . Considering t , the new boundary will imply the safety limit at time t . Now, it a mm s 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 t and X required for the passing vehicle. At this moment, the distance traveled by the opposing vehicle is X , which corresponds to its final position X (Point x). This approach ensures a stable and gradual of 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 dierence 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 Infrastructures 2020, 5, 44 9 of 25 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 Infrastructures 2020, 5, x FOR PEER REVIEW 9 of 27 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 constraints used in linear quadratic programming (LQP), the limiting values of acceleration iterative method considered by Diachuk at al. [24]. The obtained values will represent the vehicle achievable under given conditions can be determined. performance on a straight road section. To account for the curvilinear trajectory of the maneuver, If the measurements are known and the time–distance curve is determined for the case of the the values of the time–distance curve can be adjusted. passing vehicle's maximum performance [24], it is possible to immediately define whether the An important stage of the forecasting is determination of the maximum vehicle acceleration maneuver is feasible. For this, it is necessary to set, rightward of Point a (Figure 5), the safety margin capabilities (speed-acceleration). For this purpose, it is necessary to have a diagram of the free traction t to Point b. The ratio of the inclination angles of opposing and impeding vehicles' curves, providing mm force (air resistance is subtracted) or a diagram of the dynamic factor (specific free traction force). an intersection in Point p and adequate space d + L to complete the maneuver, will ensure the time f p If there is access to GPS signals and digitized terrain maps, it is possible to track the motion condition limit and the possible path of the maneuver. It is obvious that with a larger inclination angle of linear changes to correct control signals (throttle). In addition, a diagram of possible vehicle accelerations prediction for the opposing vehicle, the sensitivity's influence on the remaining time safety margin in the current conditions (Figure 4) is necessary considering movement resistance (surface quality) t' is decreased. This does not mean that the vehicle will not be able to use the power corresponding mm and adhesion to the road surface (weather conditions), since restrictions are needed for the optimal to the segment ab; however, the maneuver execution in this mode will be associated with a decrease distribution of the speed plan. If the initial speed and the average speed are known, then for the linear in the guaranteed level of safety. Therefore, the condition of the maneuver possibility is t > t + t' , a min mm constraints used in linear quadratic programming (LQP), the limiting values of acceleration achievable where t' ǂ t due to the lower sensitivity. mm mm under given conditions can be determined. 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). 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 t to Point b. The ratio of the inclination angles of opposing and impeding vehicles’ curves, providing mm an intersection in Point p and adequate space d + L to complete the maneuver, will ensure the time f p 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’ is decreased. This does not mean that the vehicle will not be able to use the power corresponding mm 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 t > t + t’ , a min mm where t’ t due to the lower sensitivity. mm mm Infrastructures 2020, 5, 44 10 of 25 Infrastructures 2020, 5, x FOR PEER REVIEW 10 of 27 Figure 5. Limiting case of the maximum performance. Figure 5. Limiting case of the maximum performance. Another important point concerns the maximum allowable time required for overtaking. It is Another important point concerns the maximum allowable time required for overtaking. It is also also possible that the time range t - t according to the measured and evaluated data will be quite possible that the time range t t minaccor l ding to the measured and evaluated data will be quite wide, min l wide, which may lead to the calculation of large values of t having no real meaning because of which may lead to the calculation of large values of t having no p real meaning because of predicting a pr predicting a protracted otracted maneuver. In this maneuver. regard, the In this maximum regard, maneuver the maximum time should maneuver time sho also be limited based uld also be on the ratio limited of based the times on the t rand atio o t f , th t . e times t and t , t . a min a l min l 3.3. Mathematical Formulation 3.3. Mathematical Formulation To describe the movement laws of overtaking using the performance curve of , suppose that the To describe the movement laws of overtaking using the performance curve of, suppose that the measurements are evaluated at intervals of Dt based on the preset frequency of the radar system. measurements are evaluated at intervals of ̇ t based on the preset frequency of the radar system. Then, the current discrete time will be nDt , where n takes both positive and negative values relative Then, the current discrete time will be n·̇ t , 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 to the starting point of the maneuver (sample n = 0). The results of the relative measurements at time t are as follows: t are as follows: Dj v j dj v j X V = D v d v X V , (8) ȁ o ȁ ȁ i ȁ p p ܦ t ݒ t ݀ t ݒ t ܺ ห ܸ ห ܦ n ݒ on ݀ n ݒ in ܺ pn ܸ pn n n n n ൌ ሺ ሻ , ቀ ௧ ௧ ௧ ௧ t t ቁ (8) n n ௧ ௧ wher where e D D denotes denotes the the curr cur ent rent measur measured distance ed distanc between e between the passing the paand ssinopposing g and opvehicles; posing vehic v denotes les; v n on n on the current measured opposing vehicle speed; d denotes the current measured distance between denotes the current measured opposing vehicle nspeed; d denotes the current measured distance the passing vehicle and the rear of the impeding vehicle; v denotes the current measured impeding between the passing vehicle and the rear of the impeding veh in icle; v denotes the current measured in vehicle relative speed; and X and V denote the estimated current self-position and speed of the pn pn impeding vehicle relative speed; and X and V denote the estimated current self-position and speed pn pn passing vehicle, respectively. of the passing vehicle, respectively. Nevertheless, the speed forecast processing requires more time Dt and computational resources. pr Nevertheless, the speed forecast processing requires more time ̇ t and computational resources. pr In this regard, the algorithm should be organized to avoid frequent recalculations that do not In this regard, the algorithm should be organized to avoid frequent recalculations that do not significantly aect the quality of the forecast. Therefore, the time Dt must be a multiple of the time pr significantly affect the quality of the forecast. Therefore, the time ̇ t must be a multiple of the time pr Dt (Dt = mDt ), where m is the factor of cycle multiplicity. This is provided that to process the m pr m ̇ t (̇ t = m·̇ t ), where m is the factor of cycle multiplicity. This is provided that to process the m pr m forecast, the time Dt = t T is needed (where T is the time before the forecast is made until t ) pr 0 1 1 0 forecast, the time ̇ t = t - T is needed (where T is the time before the forecast is made until t ) and pr 0 -1 -1 0 and that during this period, the vehicle speed does not change significantly, i.e., that during this period, the vehicle speed does not change significantly, i.e., dX ௗ dXௗ ௗ ௗ ௗ ௗ p p dX dX dX dX o o i i ቚ ൎ ቚ , ቚ ൎ ቚ , ቚ ൎ ቚ , , (9 (9) ) ௗ௧ ௗ௧ ௗ௧ ௗ௧ ௗ௧ ௗ௧ ் ௧ ் ௧ ் ௧ dt షభ dt బ dt షభ dt బ dtషభ dt బ T t T t T t 1 0 1 0 1 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 Infrastructures 2020, 5, 44 11 of 25 For the opposing and impeding vehicles, linear predictions can be made from a tangent angle k and the values at the point as (y y ) = k (x x ). The potential global positions of the opposing and 0 0 impeding vehicles are determined relatively through the predicted movement of the passing vehicle. Therefore, while performing the maneuver, the current state vector assessment (X , V ) is periodically pn pn recalculated based on the sensor fusion technology. Then, ! ! !! ! X D v X V 1 on n on pn pn = + (10) X d + L v X V t t in n i in pn pn n To determine the locking time t within the overtaking pocket, the condition for the intersection of linear predictions at Point p corresponding to t is X = X . Note that V and the time at the moment l o i o before the forecast are negative, thus T = Dt = mDt . Consequently, the initial measurement is pr m carried out in m cycles before the maneuver starts. That is, ! ! ! ! !! ! T T 1 X 1 D v X V 1 o ( m) o( m) p( m) p( m) = + (11) 1 X 1 d + L v X V t + Dt pr i ( m) i i( m) p( m) p( m) l The passing vehicle position at time T relative to t is estimated as X V Dt . 1 0 p ( m) p ( m) pr D d L ( m) ( m) i t = mDt (12) v v i( m) o( m) To determine t , the minimum distance X X = d +L is the distance between the impeding a o i fmin p and passing vehicles. Therefore, t can be defined by the dierence (d + L ) between the functions a p fmin yp and rp, similar to Equation (12). That is, d + L f min t = t (13) v v i( m) o( m) To determine Point z of the diagonals’ intersection, the distances at Points l, h, n, and j corresponding to t , t , using Equation (12), are min ! ! ! D V Dt V + v X X 1 1 n pr l ( m) p( m) p( m) o( m) = (14) X X d + L V Dt V + v t + Dt t + Dt h j i pr min pr s pr ( m) p( m) p( m) i( m) The values of the coecients k and b are determined from the matrix relations: ! ! ! ! ! ! X X t 1 k k k k 1 1 1 X X l h min lj hn lj hn l h = and = (15) X X t 1 b b b b t t t t X X n s s s n j lj hn lj hn min min j The intersection condition at t is the equality of the ordinates of segments X and X : p lj hn ! ! ! ! ! ! T T T b b t k t k t k k lj hn p lj p hn p lj hn X X = = = 0 and t = (16) lj hn 1 b 1 b 1 b b k k lj hn lj hn hn lj The equation of a line passing through the segment ab can be determined based on the coordinates of two points: ! ! ! ! ! ! X t 1 k k t 1 X a min min a ab ab = and = (17) X t 1 b b t 1 X b a ab ab a b Infrastructures 2020, 5, 44 12 of 25 Therefore, the distance X corresponding to time t is Infrastructures 2020, 5, x FOR PEER REVIpEW p 12 of 27 ! ! ! ! ! T T After the rational values of tp and Xp are determined using the heuristic algorithm previously t k 1 t 1 1 X p ab p a X = = (18) described, the desired p trajectory of motion is determined using a kinematic model. Such a model 1 b t t 1 t t X a min a min ab b 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 The aim of this stage is to obtain the values of t and X (Figure 3), which can be used for the p p implementation of laws of the autonomous vehicle during maneuver realization. optimal distribution of speeds and trajectory planning of the maneuver. 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 4. Quadratic Optimization Model 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 4.1. General 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 After the rational values of t and X are determined using the heuristic algorithm previously p p integral over the time interval (t - t ) equals the distance X . These curves will at least differ in the p 0 p described, the desired trajectory of motion is determined using a kinematic model. Such a model value of V and its derivative dV /dt (acceleration) at the nodal points. X X makes path planning simpler and faster and provides a smooth curve that can be adjusted, depending The speed distribution must also satisfy various requirements, such as technical, operational, on the priorities of the kinematic parameters. This curve can serve as a reference for the control economic, and safety requirements, which may be conflicting. The technical requirements are implementation of laws of the autonomous vehicle during maneuver realization. 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 Suppose that the overtaking maneuver is being planned for a relatively straight road section. Then, the speed plan should be comparable to the curvature of the output characteristics of a power unit the formation of the longitudinal and transversal components of the speed plan can be considered operating in the transient mode. The operational requirements ensure that the traction and steering independent. Consider the process of forming the longitudinal component of vehicle speed according controls are smooth by imposing restrictions related to the vehicle's steerability and stability. to the direction of road marking lines (Figure 6). The values of the time and distance required for Therefore, significant speed and acceleration are undesirable at the moments of lane change. The overtaking (t , X ) are determined before the maneuver starts at time t . Obviously, there are many p p economic requirements aim to minimize the work of the vehicle power unit by 0 minimizing vehicle realizations acceleration, of since the distribution more fuel consumption of the speed’s occurs longitudinal at the moments of speed component, such change. that their From th integral is over the perspective, the cumulative derivative of the speed plan curve should also be minimal. The safety time interval (t t ) equals the distance X . These curves will at least dier in the value of V and its p p 0 X requirements limit the maximum speed, acceleration, angular speed of the steering wheel, and time derivative dV /dt (acceleration) at the nodal points. spent near the impeding vehicle. Figure 6. Different speed plan distributions with fixed values of t and X . p p Figure 6. Dierent speed plan distributions with fixed values of t and X . p p 4.2. Objective Function The speed distribution must also satisfy various requirements, such as technical, operational, The objective function of the model minimizes the speed variation, acceleration variation, and economic, and safety requirements, which may be conflicting. The technical requirements are associated sharpness (differences between tangent coefficients of adjacent nodes). Therefore, the optimal speed with the propulsion system to ensure the required vehicle’s performance under external constraints, distribution is written as such as a slope, road resistance, tire adhesion, and head wind. That is, the curvature of the speed plan ݉݅݊ ܬൌܹ ڄ ܬ ܹ ڄܬ ܹ ڄ ܬ ௌ ௌ (19) should be comparable to the curvatur e 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 where J denotes the objective function; W , W , and W denote the weighting factors of speed, V A S by acceleration, and sharpne imposing restrictions s rs elated , respectively; and to the vehicle J , J ’s , and steerability J denote cand orrespondin stability g integr . Ther al func efore,tion significant s. speed V A S and acceleration are undesirable at the moments of lane change. The economic requirements aim The speed integral function is given by 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. Infrastructures 2020, 5, 44 13 of 25 4.2. Objective Function The objective function of the model minimizes the speed variation, acceleration variation, and sharpness (dierences between tangent coecients of adjacent nodes). Therefore, the optimal speed distribution is written as min J = W J + W J + W J (19) V V A A S S where J denotes the objective function; W , W , and W denote the weighting factors of speed, V A S acceleration, and sharpness, respectively; and J , J , and J denote corresponding integral functions. V A S The speed integral function is given by Z Z Z Z t t t t p p p p 2 2 J = (V (t) V ) dt = V (t)dt 2V V (t)dt+V dt (20) V X X Xa X X Xa t t t t 0 0 0 0 | {z }| {z }| {z } const J 0 J 00 V V where the last member that does not contain the variable q (see Equation (A4), Appendix A) is omitted. The speed integral must correspond to the distance S , considering Equations (A1), (A3), (A8), and (A10) (Appendix A). For the integral function J , V” Z Z n n t X DT X p i T T T T T T T V (t)dt = f d q = g q = g q = g E q = g E M q (21) X q i i f f b bi f t 0 i=1 i=1 T T where g = (g , g , ::: , g ) , E = (E , E , ::: , E ) , E denotes the identity matrix of the dimension 4 b1 b2 bn 4 4 4 4 4, and M denotes the transition matrix from the vector q of degrees of freedom to the vector q of q f 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 q (e.g., q and q are equal to q and q f 3i 4i 1 (i+1) 2 (i+1), respectively). Therefore, the excess degrees of freedom must be reduced by grouping node values instead of FE. That is, q = M q. f q Now consider the integral of the square of speed V (t), considering Equation (A11) (Appendix A): p X 2 T J 0 = V (t)dt = q D q (22) V bi X i i=1 In vector-matrix form, Equation (22) can be written as 0 1T 0 1 0 1 q D Z Z Z q B C B C B C b1 4 4 4 1 1 B C B C B C B C B C B C B C B C B C B C B C B C q Z D Z Z q B C B 4 b2 4 4 C B C 2 2 B C B C B C B C B C B C J = B C B CB C = q Dq (23) . . . V . B C B C B C f . . . . B C B C B C B C B . C B C . Z Z . . B C B 4 4 C B C B C B C B C @ A @ A @ A q Z Z D q 4 4 bn n n | {z } | {z } | {z } q q f f where Z = 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 Z ! 2 n p X dV (t) T T J = dt = q G q = q Gq (24) A bi i f i f dt i=1 Infrastructures 2020, 5, 44 14 of 25 2 n 2 X ( ) d V t T T J = dt = q K q = q Kq (25) S bi i i f 2 f dt i=1 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 min J = q Hq + 2Lq (26) 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 t (Figure 3). Then, the longitudinal component of the path is X = V (t)dt (27) pb X The condition for achieving State 2 (Figure 1a) is X d + X (28) pb 0 ib The time t can be determined iteratively (Figure 3) after the distribution of the longitudinal speed, based on Equations (27) and (28), considering X = X (t ). According to a possible slight decrease in pb p t the impeding vehicle speed, X may correspond to a small longitudinal gap between the front of the pb passing vehicle and the rear of the impeding vehicle. Thus, X (t ) d V + v t 0 (29) p t 0 p0 i0 t The path’s transverse component is Y = V (t)dt (30) pb Y where V (t) is represented similar to Equation (21). 4.3.2. Location in Opposite Lane Constraint The upper limit of the passing vehicle transversal movement Y may correspond to its position po in the middle of the opposite lane. Possible deflections of this position are restricted by safe clearance Infrastructures 2020, 5, 44 15 of 25 to the road edge (Figure 1b). A lower limit of this clearance is the minimum safe distance between passing and impeding vehicles. Therefore, for X and Y , po po Z Z t t c c X = V (t)dt and Y = V (t)dt = 0 (31) po X po Y t t t t where t 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 X (t ) = X + X (Point e, Figure 3). p c pb po 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 (t ) (d + L ) V + v t 0 (32) p c 0 i p0 i0 c 4.3.3. Maneuver Completion The lane change planning is similar to the first phase. Thus, Z Z t t p p X = V (t)dt and Y = V (t)dt (33) X Y p f p f t t c c 4.4. Preparing the Reference Trajectories As a result of the optimization, the components of speeds (V , V ) in global coordinates are X Y determined. Therefore, it is necessary to transfer the speeds to the local coordinates of the passing vehicle (V , V ) , in order to allow it to consider its maneuvering. Since the yaw angle ' is small, x y ! ! ! V cos() sin() V X x = (34) V sin() cos() V Y y and ! ! ! ! ! V cos() sin() V 1 V x X X = (35) ( ) ( ) V sin cos V 1 V y Y Y The ideal forecast for the yaw angle can be obtained as a tangent to the motion trajectory: dY p V = arctg (36) dX V p 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 X , Y ; speeds in local p p coordinates V , V ; and the yaw angle '. As additional parameters, which can be directly measured x y 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 t , 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 d depends on the dierence between the speeds of fmin Infrastructures 2020, 5, 44 16 of 25 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 aect, and therefore, d can be considered constant in the vicinity of Point t . The determination of d is described fmin a fmin 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’ becomes shorter and the needed space X’ p p becomes larger. This may be explained by the significant sensitivity of the forecast to the impeding Infrastructures 2020, 5, x FOR PEER REVIEW 17 of 27 vehicle’s speed changes. (a) (b) Figure 7. Influence of threshold values of speed fluctuations on the prediction reliability: (a) Increase 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 in speeds of opposing and impeding vehicles (Cases 1-3); (b) decrease in speeds of opposing and impeding vehicles (Cases 1-3). impeding vehicles (Cases 1-3). In the case of Figure 7a2, the opposing vehicle speed increases and the upper boundary limit Other possible combinations, where one of the vehicles increases its speed and the other is decreases violated. its In speed this case, approximately simultaneo the inclination anglesusly of the , give solutions segments abthat andare ab’ not are superior practically in n the ature same. to However, even though the required time has decreased t’ < t , unlike the previous case, the required the changes discussed in Figure 7. Note that measurements of d are available prior to the moment of p p n space X’ decreases due to the larger space needed for the opposing vehicle. The most critical case alignmen pt with the rear of the impeding vehicle, after which the last d value may be fixed, and is when the speed fluctuations of both the opposing and impeding vehicles reach the threshold measurements of D can be carried out until the critical point. Recalculation after the critical point is boundaries simultaneously (Figure 7a3). The displacement of the minimum space segment d + fmin possible if the next impeding vehicle appears in the lane, which does not provide a proper pocket or L for completing the maneuver can be so significant that the time t’ approaches the preset time t , p s p harshly reduces its speed. and the margin t in relation to the minimum performance mode will not be provided. Point t’ mm p Obviously, even in the automatic mode of maneuver execution, deviations of the passing vehicle is located the furthest from Point t , even though the space required for the maneuver may remain speed are possible due to the influence of various random factors. However, within the thresholds almost unchanged X’ X . Similarly, a decrease in the speed of opposing and impeding vehicles p p set by f , f (Figure 3), forecast recalculation is not required. Therefore, the autonomous control U L will give a lower limit of time fluctuations f (Figure 3). However, such decreases are not dangerous, system must adjust the speed mode of the passing vehicle not only according to measurement and it makes sense to only recalculate the forecast to save energy and increase the movement stability. 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 ȝ ȝ ݐ ݐ ݐ ݐ ݐ ǡ ݂݅ ݐ ൏ݐ ቊ (37) ȝ ȝ ݐ ݐ ݐ െ൫ݐ ݐ ݂݅ ݐ ݐ where t' denotes the instantaneous value of the hypothetical accident time compatible with predefined t ; t = ̇ t + p·̇ t = (m+p)·̇ t , where p denotes the number of spare measurement cycles, a pr pr m m by default, p = 2; and t denotes the unaccounted time expenses (e.g., engine transition mode and un control delay). ௨ ൯ǡ ௨ Infrastructures 2020, 5, 44 17 of 25 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’ < X and t’ > t . For the case in Figure 7b2, p p p p the diminished opposing vehicle speed demonstrates the need for a longer distance and time because of the reduced space X for the opposing vehicle: X’ > X and t’ > t . For the case in Figure 7b3, o p p p p 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’ X and t’ > t . This may cause the double p p p p margin time t with unreasonable energy consumption. Therefore, the passing vehicle speed mode mm 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 d are available prior to the moment of alignment with the rear of the impeding vehicle, after which the last d value may be fixed, and measurements of D 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 f , f (Figure 3), forecast recalculation is not required. Therefore, the autonomous control system U L 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 > 0 0 t > t + t + t + t , i f t < t < p mm pr un a a a > (37) > 0 0 : t > t + t t + t , i f t > t a mm pr un a a a where t’ denotes the instantaneous value of the hypothetical accident time compatible with predefined t ; t = Dt + pDt = (m+p)Dt , where p denotes the number of spare measurement cycles, by default, a pr pr m m p = 2; and t denotes the unaccounted time expenses (e.g., engine transition mode and control delay). un Hence, t’ can be recalculated using Equation (13) for every n-th measurement at t as follows: a n D d L d L n n i f min p t = + t (38) v v in on 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: dx/dt = Ax + Bu and y = Cx + Du (39) 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 V from Equation (34), and the state vector x only contains parameters for the front wheel steering control u = Q . Therefore, x = (V , Y, !, f y Infrastructures 2020, 5, 44 18 of 25 ') , 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 0 1 0 1 k +k k x +k x f r f f r r k 0 1T B C B 0 V 0 C B C 0 0 B mV mV C B C B C x x B C B C B C B C B C B C B C B C B C B 1 0 0 V C B C B C x 0 1 0 B C B C B C B C B C B C A = B 2 2 C, B = B C, C = B C (40) k x +k x B C B x k C B C k x +k x f r r f f r r B f f f C B C B C 0 0 B C B C B C B 0 0 C B C B C B C B C @ A IV IV B x x C @ A @ A 0 1 0 0 1 0 where m and I denote the vehicle mass and inertia, respectively; k and k denote the front and rear tires’ side stiness, respectively; and x and x denote the local longitudinal coordinates of the front and f r rear tire spots, correspondingly. Therefore, the output variables for reference tracking are y = (Y, ') , 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: p 1 2 T T T min J(z ) = " + e Q e + e Q e + e Q e (41) k " y y,k+i u u,k+i Du Du,k+i k y,k+i u,k+i Du,k+i i=0 subject to e = y y , e = u u , e = u u (42) y,k+i k+i+1jk u,k+i k+ijk Du,k+i k+ijk k+i 1jk k+i+1jk k+ijk where Q , Q , and Q denote positive semi-defined weight matrices; y denotes the Plant output y u Du k+i+1|k reference signals at the ith prediction horizon step; y denotes the Plant outputs at the ith prediction k+i+1|k horizon step; u denotes the Plant target reference signals at the ith prediction horizon step; u k+i|k k+i|k T T denotes the Plant inputs (manipulated variables) at the ith prediction horizon step; z = (u , u , k k|k k+1|k u , " ) denotes the solution; " denotes the scalar dimensionless slack variable used for constraint k+p 1|k k k 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) (y) y " h y y + " h , i = 1::: p, i = 1::: n > y j,min(i) k j,k+ijk j,max(i) k > j,min(i) j,max(i) < (u) (u) u " h u u + " h , i = 1::: p, i = 1::: n (43) > u j,min(i) k j,k+i 1jk j,max(i) k j,min(i) j,max(i) (Du) (Du) Du " h Du Du + " h , i = 1::: p, i = 1::: n j,min(i) k j,k+i 1jk j,max(i) k Du j,min(i) j,max(i) where y and y denote the minimum and maximum values of the jth output at the ith prediction j,min(i) j,max(i) horizon step, respectively; u and u denote the minimum and maximum values of the jth input at the j,min(i) j,max(i) ith prediction horizon step, respectively; Du and Du denote the minimum and maximum values of j,min(i) j,max(i) (y) (y) the jth input rate at the ith prediction horizon step, respectively; h and h denote the minimum j,min(i) j,max(i) (u) and maximum values of the jth output’s hard constraints at the ith prediction horizon step, respectively; h j,min(i) (u) and h denote the minimum and maximum values of the jth input’s hard constraints at the ith prediction j,max(i) (Du) (Du) horizon step, respectively; h and h denote the minimum and maximum values of the jth j,min(i) j,max(i) input rates’ hard constraints at the ith prediction horizon step, respectively; n denotes the number of output parameters; n denotes the number of input parameters; and n denotes the number of input rate parameters.6.3. u Du 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. Infrastructures 2020, 5, x FOR PEER REVIEW 19 of 27 ሺ ௬ ሻ ሺ ௬ ሻ ݕ െߝ ڄ݄ ݕ ݕ ߝ ڄ݄ ݅ǡ ൌ ͳ ǤǤǤǡ ݆ ൌǤǤǤ݊ͳ ǡ ሺ ሻ ǡȁା ǡ௫ ሺ ሻ ௬ ǡ ሺ ሻ ǡ௫ ሺ ሻ ሺ ௨ ሻ ሺ ௨ ሻ ݑ െߝ ڄ݄ ݑ ݑ ߝ ڄ݄ ǡ ݅ ൌǤǤǤͳ ǡ ݆ ൌǤǤǤ݊ͳ (43) ǡ ሺ ሻ ǡାଵିȁ ǡ௫ ሺ ሻ ௨ ǡ ሺ ሻ ǡ௫ ሺ ሻ ሺ ሻ ሺ ሻ ௱௨ ߂ݑ െߝ ڄ݄ ߂ݑ ߂ݑ ߝ ڄ݄ ǡ ݅ ൌǤǤǤͳ ǡ ݆ ൌǤǤǤ݊ͳ ሺ ሻ ሺ ሻ ǡ ሺ ሻ ǡାଵିȁ ǡ௫ ሺ ሻ ௱௨ ǡ ǡ௫ where y and y denote the minimum and maximum values of the jth output at the ith prediction j,min(i) j,max(i) horizon step, respectively; u and u denote the minimum and maximum values of the jth input at the j,min(i) j,max(i) ith prediction horizon step, respectively; ̇ u and ̇ u denote the minimum and maximum values of j,min(i) j,max(i) (y) (y) the jth input rate at the ith prediction horizon step, respectively; h j,min(i) and h j,max(i) denote the minimum and maximum values of the jth output's hard constraints at the ith prediction horizon step, respectively; (u) (u) h and h denote the minimum and maximum values of the jth input's hard constraints at the ith j,min(i) j,max(i) (NJ u) (NJ u) prediction horizon step, respectively; h and h denote the minimum and maximum values of the j,min(i) j,max(i) jth input rates' hard constraints at the ith prediction horizon step, respectively; n denotes the number of output parameters; n denotes the number of input parameters; and n denotes the number of input rate u ̇ u Infrastructures 2020, 5, 44 19 of 25 parameters.6.3. Simulink Model A simplified Simulink model (Figure 8) that implements a virtual overtaking scenario on a two- Block 2 (SUV Model) calculates the vector of discrete states of the vehicle dynamic model, updating lane highway has been developed. The main block 1 (SUV Plant) calculates the state vector of the the necessary matrices and vectors at each time step. For simplicity, the same bicycle vehicle model vehicle's continuous dynamic model, which presents a real vehicle and its sensor system is used as a Plant and Model. Block 3 (MPC) implements the Adaptive Model Predictive Controller, 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 which calculates the optimal control values (steering angle), based on minimizing the sum of the square vehicle model is used as a Plant and Model. Block 3 (MPC) implements the Adaptive Model Predictive of the dierences measured and predicted parameters: lateral displacement Y and yaw angle Phi, Controller, which calculates the optimal control values (steering angle), based on minimizing the sum which is extracted from the Plant state vector by block 5 (MO Extractor). The vectors of the reference of the square of the differences measured and predicted parameters: lateral displacement Y and yaw tracks Ref, being the desirable values of the vehicle model state parameters, are stored in the memory angle Phi, which is extracted from the Plant state vector by block 5 (MO Extractor). The vectors of the after optimization for reading at the corresponding time step. Block 4 (Conditions) sets the values for 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 vehicle’s local longitudinal speed V and the desired reference values Ref = (Y, Phi) at the current the values for the vehicle's local longitudinal speed V and the desired reference values Ref = (Y, Phi) time. Block 6 (Result) accumulates the calculated outputs. The model does not comprise external at the current time. Block 6 (Result) accumulates the calculated outputs. The model does not comprise disturbances and measurement noise. external disturbances and measurement noise. Figure 8. Simulink model of overtaking scenario execution. Figure 8. Simulink model of overtaking scenario execution. 7. Application 7. Application The purpose of this application is to achieve stable control and ensure that the Plant state 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 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 conditions data, defining the parameters of the AMPC controller, and determining the desired reference reference tracks for speed, acceleration, and displacement in the global coordinates. tracks for speed, acceleration, and displacement in the global coordinates. 7.1. Initial Conditions Data 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: The Matlab/Simulink example for simulating overtaking was used [26]. According to the (D , V , d , V , L , X , V ) = (480, 70, 35, 65, 22.5, 0, 70), measurements, at time (-1T ) o(= -1) (0.1 -1) s i, (-1the ) i(-1 initial ) p(-1) data p(-1) vector is formed as follows: (D , V , d , V , L , X , V ) = (480, 70, 35, 65, 22.5, 0, 70), ( 1) o( 1) ( 1) i( 1) i( 1) p( 1) p( 1) 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 t = 7.79 s and S = 25 m. min fmin The minimum time margin was set as t = 1 s. Substituting these values into Equations (12)–(18) and mm (27)–(33) gives the following rational values (Figure 9a,b): overtaking global longitudinal projection X = 250 m, overtaking time t = 8.9 9 s, bypass time during lane change t = 5.1 s, and time to the p p t critical Point t = 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. ௱௨ Infrastructures 2020, 5, x FOR PEER REVIEW 21 of 26 Infrastructures 2020, 5, 44 20 of 25 (a) (b) (c) (d) (e) Figure 9. Planning reference tracks for state parameters: (a) Vehicle path's prognosis; (b) definition of 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 point t ; (c) predicted passing vehicle’s global displacements; (d) plan of global velocities; (e) plan of global accelerations. global accelerations. 7.3. Reference Speeds, Accelerations, and Displacements 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 Using the proposed optimization model, the desired reference tracks for speed, acceleration, path on the global X axis clearly corresponds to 250 m (Figure 9d). This, when copied to Figure 9a's and displacement in the global coordinates were determined, using the time grid with the increment of curve, shows that in the initial phase, the vehicle uses a potential close to the upper limit (black line). 0.1 s (Figure 9c–e). Note that the setting of linear constraints in Equation (40) should be consistent with The next important point is the conditionality of the weighting factors in the optimization of Equation the thresholds of the vehicle performance set by the distribution of acceleration upon speed (Figure 4) (19). Note that the ratio of weight coefficients significantly changes the optimization picture in for the current conditions. This means that each speed value to be optimized in a time grid node is tied connection with the change of priorities. Increasing the W coefficient very much reduces the speed to the maximum possible acceleration at this speed, which creates non-linear constraints. In this regard, consumption, but significantly increases the need for acceleration at the beginning of the maneuver. for each set of speed and acceleration thresholds in the optimization process, some rationale is needed, The increase of the W coefficient reduces the cumulative consumption of acceleration, but does not as previously described. Another dicult point before optimization is setting the final values of the provide smoothness in the boundary zones of the speed plan, and the peak speed value rises. speed and acceleration, since they significantly aect the trend of the entire speed plan. The speed Increasing the W coefficient distributes speeds evenly over time. value in the last node S close to the average speed V (Figure 9d) may lead to the appearance of such a Xa peak near the critical point, when the longitudinal accelerations in the phase of maneuver completion Infrastructures 2020, 5, x FOR PEER REVIEW 22 of 26 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 Infrastructures 2020, 5, 44 21 of 25 values for the entire overtaking maneuver are chosen: W = 0.2, W = 0.2, and W = 0.6. In the V A S distribution of transverse speeds of the bypass phase, the priority is divided between the control ar smoothness and e negative and lar the ger cumulative acceleration intake: than the absolute value of 0.5 W m/s = . 0. That 2, Wwould = 0.4, and mean W the = use 0.4. of In the fin serviceal phase, braking V A S and due activation to the lane ch of the vehicle’s ange at high working spee brake ds, the m system.ain Fr priority om the point is focused of view on r of ensuring educin the g the maximum lateral vehicle stability during the lane change, it is undesirable to use the tire longitudinal force values close accelerations, respectively: W = 0.1, W = 0.6, and W = 0.3. It is obvious, however, that priorities may V A S to those which may considerably reduce the tire’s lateral adhesion. In connection with the foregoing, vary, depending on the situation. it may be recommended to focus on the value of the speed V at which the distribution of the speed It should also be noted that the value of the vehicle's initi pfal lateral position does not correspond plan requires decelerations, provided only by limiting the engine power consumption. In this case, to the lane center, but is offset by 0.4 m from the dashed line to ensure the conditions previously the selected value V ensures maneuver completion with acceleration close to zero. described. pf As can be seen, the combination of the plan for longitudinal speeds and accelerations fits the Figure 10 shows the overtaking results by predicting the lateral offset and yaw angle. As can be performance limitations well in Figure 4. At the same time, the projection curve of the overtaking path seen in Figure 10a, at the 9th second of overtaking, the trajectory longitudinal component practically on the global X axis clearly corresponds to 250 m (Figure 9d). This, when copied to Figure 9a’s curve, corresponds to the pre-set one, with a final value of X = 250 ƿ 249.4 m, and the transverse component shows that in the initial phase, the vehicle uses a potential close to the upper limit (black line). The Y is strictly within 3.6 m, but has a residual of 0.23 m at the time t = 9 s. At this moment, the passing p p next important point is the conditionality of the weighting factors in the optimization of Equation (19). vehicle is almost in the middle of its lane and continues stable movement, i.e., the situation is Note that the ratio of weight coecients significantly changes the optimization picture in connection uncritical. The AMPC controller calculates the discrete control signal based on the information on the with the change of priorities. Increasing the W coecient very much reduces the speed consumption, previous value and reference tracks. However, it is almost impossible to avoid tracking delay but significantly increases the need for acceleration at the beginning of the maneuver. The increase completely. The same effect can be observed in relation to the lateral speed V (Figure 10b), which of the W coecient reduces the cumulative consumption of acceleration, but does not provide coincides in terms of shape and values with the initial one in Figure 9d, but lags a bit in time. smoothness in the boundary zones of the speed plan, and the peak speed value rises. Increasing the In general, by using the vehicle refined models of Plant and Model (Figure 8) and including a W coecient distributes speeds evenly over time. larger number of state parameters for tracking and other measures, the model convergence can be Therefore, in the current case of optimizing the longitudinal plan, the stable engine’s performance improved. is the most important, minimizing abrupt transitions in its control; respectively, the values for the entire overtaking maneuver are chosen: W = 0.2, W = 0.2, and W = 0.6. In the distribution of transverse 8. Concluding Remarks V A S speeds of the bypass phase, the priority is divided between the control smoothness and the cumulative This article has presented a methodology for distributing the speed in the longitudinal and acceleration intake: W = 0.2, W = 0.4, and W = 0.4. In the final phase, due to the lane change at V A S lateral directions when a vehicle is overtaking on two-lane highways in an automated mode. An high speeds, the main priority is focused on reducing the lateral accelerations, respectively: W = 0.1, advantage of the kinematic technique used in the model is its ability to predict both speed and W = 0.6, and W = 0.3. It is obvious, however, that priorities may vary, depending on the situation. A S acceleration references, providing subsequent tracking control based on sensor measurements. In It should also be noted that the value of the vehicle’s initial lateral position does not correspond to addition, this technique can be successfully used as a component of the model predictive control for the lane center, but is oset by 0.4 m from the dashed line to ensure the conditions previously described. generating reference trajectories. Based on this study, the following comments are offered. Figure 10 shows the overtaking results by predicting the lateral oset and yaw angle. As can be In this study, the ‘kinematic model’ was not exactly used in the sense of a vehicle model, but the seen in Figure 10a, at the 9th second of overtaking, the trajectory longitudinal component practically output kinematic parameters of the vehicle motion were used to predict both the trajectory and the corresponds to the pre-set one, with a final value of X = 250 249.4 m, and the transverse component overtaking mode, considering the speed distribution directly and acceleration distribution indirectly, Y is strictly within 3.6 m, but has a residual of 0.23 m at the time t = 9 s. At this moment, the p p as part of the FE speed model. In this regard, we followed the well-established trend of planning passing vehicle is almost in the middle of its lane and continues stable movement, i.e., the situation is motion paths with various curves and graphs, as presented in the literature [17]. However, we uncritical. The AMPC controller calculates the discrete control signal based on the information on obtained trajectories as integrals of the velocity plans. Unlike other studies, the proposed approach the previous value and reference tracks. However, it is almost impossible to avoid tracking delay focused on using an FE grid that simultaneously ensured a high accuracy and compliance with the completely. The same eect can be observed in relation to the lateral speed V (Figure 10b), which real capabilities of the vehicle engine-drive system. In particular, the interaction between possible coincides in terms of shape and values with the initial one in Figure 9d, but lags a bit in time. speeds and the respective accelerations was considered. (a) Figure 10. Simulation results of vehicle steering control prognosis during overtaking: (a) Global displacements; (b) steering control and lateral speed projection. Infrastructures 2020, 5, 44 22 of 25 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 oered. 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 (t and X ) characterize the p p average speed V and are obviously not enough, because many curves integrable in the interval (t , t ) xa 0 p can give the same X . 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, dierentiable 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 2 2 the curve, no other parameters except for the nodal V , dV /dt, and d V /dt can be included, because x x x 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 eective, 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. Infrastructures 2020, 5, 44 23 of 25 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 aected 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 coecients 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 eect 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. Funding: This research was sponsored by the Natural Sciences and Engineering Research Council of Canada (NSERC). Acknowledgments: 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. Abbreviations The following abbreviations are used in this manuscript: NMPC Model Predictive Control RRT Rapid Random Tree GPS Global Positioning System LQP Linear Quadratic Programming AMPC Adaptive Model Predictive Control SUV Single Unit Vehicle MO Measured Outputs FE Finite Element Appendix A : Representing the Speed Function by Finite Elements Suppose that the speed of the passing vehicle within the time interval (t , t ) varies along the X-coordinate of the road segment 0 X , according to the law V (t). Then, for a grid of n time intervals (t , t , t , ::: , t ), p n X 0 1 2 Z Z Z n n t t DT p X i X i X = V (t)dt = V (t)dt = V (,DT )d (A1) p X X Xi i t t 0 0 i i=1 i=1 where DT is the time interval (t t ), which is generally variable. i i i 1 Using the FE method for the piecewise representation of the speed function of each interval, for the i-th time segment (t , t ), i 1 i V (,DT ) = q f (,DT ) (A2) Xi i ki k i k=1 Infrastructures 2020, 5, 44 24 of 25 where 2 [0, DT ] is the FE local time; q , q , q , and q represent impact coecients, where q and q are speeds i 1i 2i 3i 4i 1i 3i at FE nodes and q and q are accelerations (derivatives) at the corresponding nodes; and f , f , f , and f are 2i 4i 1 2 3 4 basis functions. Thus, using matrix notation, T T V (,DT ) = f (,DT )q = f q (A3) Xi i i b i b i where T T f = ( f f f f ) , q = ( q q q q ) (A4) 1 2 3 4 1i 2i 3i 4i b i The normalized basis function f for an FE of a unitary length (DT = 1) is based on the cubic polynomial with two degrees of freedom at a node, providing smoothness and continuous dierentiability, as follows: 0 1 0 1 2 0 1 (2 + 1)( 1) f B C 1 B 1 C B C B C B C B C B C B C B 2 C B C B f C B C B DT C ( 1) B 2 C B C B C B C B C B C f = = , l = diag (A5) B C B C DT B C B C B 2 C B C f 1 B 3 C B (2 3) C B C @ A B C @ A @ A f 2 DT ( 1) where 2 [0, 1] is the normalized coordinate. Let = DT. Then, the transition between the absolute and normalized basis functions is given by ( ) ( ) f = f ,DT = f DT,DT = l f . (A6) DT b b b Considering the basis functions defined only within the FE interval, the relation for the entire speed can be written as n n X X T T V (t) = f (,DT )q = f q . (A7) X i i i b bi i=1 i=1 Since the function also uses (up to second) derivatives of FE basis functions, the first and second derivatives are obtained, respectively, as df df b DT = (A8) d DT d d f d f b DT = (A9) 2 2 2 d DT d Consider the formation of common integrals, replacing the dierential d = dDT and thresholds. Since q does not depend on , only the basis functions (Equation (A5)) are integrated. Z Z DT 1 ( ) g = f ,DT d = DTl f d (A10) DT b b 0 0 Z Z ! DT 1 T T T D = f f d = DT l f f d l (A11) b i DT b DT 0 0 0 1 Z Z DT 1 df B df df C df l B C b b DT B C T B C G = d = d l (A12) b B C @ A DT d d DT d d 0 0 0 1 Z Z T T 2 2 DT 2 2 1 B d f d f C d f d f l B C b b DT B C T B C K = d = d l (A13) B C DT 2 2 3 @ 2 2 A d d DT d d 0 0 References 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. 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. [CrossRef] 3. Narla, S.; Stowell, H. Connected and Autonomous Vehicles; Institute of Transportation Engineers: Washington, DC, USA, 2019. 4. Ozbay, K. Developments in connected and automated vehicles. Intell. Transp. Syst. J. 2018, 22, 187–189. [CrossRef] 5. Shladover, S. Connected and automated vehicle systems: Review. Intell. Transp. Syst. J. 2018, 22, 190–200. [CrossRef] Infrastructures 2020, 5, 44 25 of 25 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. 7. Goldin, P. 10 Advantages of Autonomous Vehicles. ITSdigest. 2018. Available online: https://www.itsdigest. com/10-advantages-autonomous-vehicles (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. [CrossRef] 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. [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. [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. 12. Wang, F.; Yang, M.; Yang, R. Conflict-probability-estimation-based overtaking for intelligent vehicles. IEEE Trans. Intell. Transp. Syst. 2009, 10, 366–370. [CrossRef] 13. Kala, R.; Warwick, K. Motion planning of autonomous vehicles on a dual carriageway without speed lanes. Electronics 2015, 4, 59–81. [CrossRef] 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. 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. [CrossRef] 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. 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. 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. [CrossRef] 19. Molinari, F.; Anh, N.N.; Re, L.D. Ecient mixed integer programming for autonomous overtaking. In Proceedings of the American Control Conference, Seattle, WA, USA, 1 May 2017; pp. 2303–2308. 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. 21. Németh, B.; Gáspár, P.; Hegedus, ˝ T. Optimal control of overtaking maneuver for intelligent vehicles. J. Adv. Transp. 2018. [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. [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. 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. [CrossRef] 25. Diachuk, M.; Easa, S.M. Guidelines for roundabout circulatory and entry widths based on vehicle dynamics. J. Trac Transp. Eng. 2018, 5, 361–371. [CrossRef] 26. MathWorks. Available online: www.mathworks.com/help/mpc/ug/adaptive-mpc.html (accessed on 1 October 2019). 27. American Association of State Highway and Transportation Ocials (AASHTO). A Policy on Geometric Design of Highway and Streets; AASHTO: Washington, DC, USA, 2018. 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. © 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).
http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png
Infrastructures
Multidisciplinary Digital Publishing Institute
http://www.deepdyve.com/lp/multidisciplinary-digital-publishing-institute/optimal-speed-plan-for-the-overtaking-of-autonomous-vehicles-on-two-8vKV0lFIBC