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

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


Introduction
On two-lane highways, the driver's decision to pass an impeding vehicle is usually subjective and risky. The main advantage of autonomous driving is that prediction, decision making, and continuous monitoring of vehicle's performance during overtaking are performed by an onboard system. The operation of autonomous vehicles is based on planning the movement trajectory and other reference trajectories as well. The system searches for an optimal way to bypass obstacles while maintaining traffic safety. For overtaking on two-lane highways, the vehicle's on-board system must estimate the required distance and time for the passing maneuver and distribute the passing vehicle speed and steering angle based on some conflicting criteria. Thus, optimization of passing vehicle control for safe and efficient passing maneuver 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. [1] 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 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 2 of 22 convergence. The only limitation of this work is that the acceleration input parameter is not related to the engine's potential. Talamino and Sanfeliu [2] presented a technique for planning movement trajectory and speed plan of an autonomous vehicle in urban areas based on G 2 -splines. The polynomial fitting involved iterations equivalent to the optimization of curvature parameters. To simulate a sufficiently long path, a 5th 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. Enough information about the overtaking maneuver parameters was not provided. González et al. [3] 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 the state-space mesh generation. Sampling Based Planners generated random state-spaces and looked for their ties. 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 to optimize a function subject to different constraints was used.
Gu et al. [4] proposed a planning method that automatically discovers tactical maneuver patterns and fuses pattern reasoning trajectory based on the idea of using pseudo-homology along with characterizing workspace regions. Different patterns can be extracted depending on the spatial area where the trajectory terminates (region-based distinction), how it gets there around the obstacles (homology-based distinction), and what overtaking (if any) order it follows (sequence-based distinction). A series of virtual tests were conducted and confirmed the effectiveness of the method. Wang [5] considered the process of building optimal overtaking route based on minimizing the probability of the vehicles' presence in the area with the close coordinates. An integration process was used to solve the nonlinear optimization. Kala and Warwick [6] considered the process of overtaking based on the conditions of maximum speed movement but limited in 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. [7] presented an MPC framework based on path speed decomposition for autonomous driving. The concept of time scaled collision cone, which constraints and formulate forward-speed quadratic optimization was presented. Collision modeling between rectangular objects was presented. The planned vehicle was reduced to a point and the dynamic obstacle was enlarged using the concept of Minkowski sum. The autonomous driving scenarios were validated on computations of lane change, overtaking, and merging maneuvers among multiple dynamic obstacles. Tomas-Gabarron et al. [8] considered how to trace the optimum trajectory of a high-speed vehicle that changed its lateral position within a time interval. Four different functions were proposed along with their relative merits. The presence of Gaussian noise in the sensors' measurements were studied regarding its influence on final trajectories. Different performance criteria for the optimization of such maneuvers are presented, and an analysis is shown on how path deviations can be minimized by using trajectory smoothing techniques, like Kalman filter. Liu et al. [9] 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. The nonconvex temporal optimization was approximated by a set of quadratic programs that were solved iteratively using a slack convex feasible algorithm to speed up computations. This paper presents a new technique of speed planning for 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 required time and distance for safe passing maneuver. The algorithm relies on uncertainty-based thresholds of the opposing and impeding vehicles and the minimum and maximum performance of the passing vehicle. The second tool is a quadratic optimization model that determines the optimal speed distribution to ensure a smooth path of the passing vehicle. If needed, the speed plan is update along the maneuver.

of 22
The next section presents system description, including the logic of the speed control model and overtaking phases. The following sections present the operational thresholds, heuristic algorithm, quadratic optimization model, and the updating process. Model implementation using Matlab\Simulink is then presented, followed by the conclusions.

Logic of Speed Control Model
The logic of the speed control model is shown in Figure 1. The input data to the model include geometric (e.g. lane width and speed limit) and vehicle characteristics (e.g. acceleration-speed relation). Then, 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) minimum performance limit of the passing vehicle, (3) maximum performance limit of the passing vehicle. Second, the speed profiles of the three involved vehicles are established and initial values of the required time tp and distance Xp for safe maneuver completion are established using a heuristic algorithm ( Figure 3). These variables are used for determining 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 laws implementation while maneuver realization. During maneuver execution, if the upper (in terms of speed values) confidence thresholds of the predicted speed of the opposing or impeding vehicles are violated, the required time and distance for completing the maneuver safely are updated. As noted in Figure 1, if t < tp, the system continues with the last prediction and update it if the speed thresholds are violated. Otherwise, the maneuver has been successfully completed. The proposed system assumes a straight horizontal road, ideal friction with the road surface, and absence of additional external forces such as wind.

Overtaking Phases
The phases of overtaking on two-lane highways are shown in Figure 2. It is assumed that the estimation of vehicles' position has already been carried out, the forecast has been made, and the passing vehicle is ready starting the maneuver (Figure 2a). In this state, the distance D0 between the passing and opposing vehicles, and the distance d0 between the impeding and passing vehicles are estimated using long and short-range radars, respectively. The length Li of the impeding vehicle is estimated using machine vision technology. Assume that for safety reasons the passing vehicle is fully driving into the oncoming traffic lane by the time moment of reaching the rear edge of the impeding vehicle (State 2 in Figure 2a). Thus, the longitudinal component of the passing vehicle path during the bypassing phase ( Figure 2a) is Xpb, and the impeding vehicle by this time travels a distance Xib.
In Phase b of outrunning (Figure 2b), the passing vehicle travels from the bypass point to the critical point, where its front aligns with that of the impeding vehicle. During this time, the passing and impeding vehicles travel distances Xpo and Xio respectively. In Phase c -maneuver completion (Figure 2c), the passing vehicle passes the longitudinal distance Xpf to ensure an adequate safe distance df between itself and the impeding vehicle traveling a distance Xif. The passing and opposing vehicles must provide between their fronts a safety margin distance Xm being equivalent to the minimum safety margin time tmm.

Establishing Operational Speed Thresholds
The measured speeds of the opposing and impeding vehicles have uncertainty. The speed of the opposing vehicle is estimated using radar sensors located in the passing vehicle. Using four signals, Δt apart, four distances to the opposing vehicle di and the corresponding azimuth angles θi are recorded, where the polar coordinates used with the origin point lies with the sensor location. Then, the deterministic distance crossed by the vehicle between consecutive time intervals is given by Hassein et al. [10] (2018): where dVoppi = distance traveled by the opposing vehicle during Δt; dpi = distance traveled by the passing vehicle during Δt. The speed of the opposing vehicle, Vopp, can then be derived as Let the errors in di and θi measurements of the radar be denoted by ed and ea, respectively. Then, these errors will propagate and produce an error in Vopp of Eq. 2. To calculate this error, let the four variables Eq. 1 (di, θi, di+1, θi+1) be denoted by xi, i = 1 to 3. Then, using Taylor series and assuming the variables are independent, the standard deviation of Y, σy, is given by Benjamin and Cornell [11] = �∑ � � where σxi = standard deviation (SD) of random variable xi. Applying Eq. 3 to Eq. 1, the standard deviation of dVoppi, σdVoppi, can be derived as Then, applying Eq. 3 to Eq. 2, the standard deviation of the speed of the opposing vehicle, σVopp, is obtained as Similarly, the deterministic distance travelled by the impeding vehicle between consecutive time intervals before overtaking The standard deviation of the distance dvi -impi, σdvi-impi, of Eq. 6 is given by Then, the speed of the impeding vehicle Vimp and its standard deviation σVimp are calculated using Eqs. (2,5) by after replacing dVoppi with dVimpi (i = 1, 2, 3).
For the 95% confidence level, the true speed of the opposing or impeding vehicle lies within approximately two standard deviations of the measured value. Thus, the confidence ranges used in the proposed heuristic algorithm for the opposing and impeding speeds are calculated as Vopp ± 2·σVopp and Vimp ± 2·σVimp, respectively.

General
The heuristic algorithm estimates the rational time and distance required for overtaking. The logic of the algorithm is presented in Figure 3. For each relative position of the vehicles involved in the overtaking, a combination of measurements will be unique, and the number of possible maneuvers will be numerous. Suppose that at a certain time the speed values (sample) of the impeding and opposing vehicles have been estimated using the passing vehicle sensory system (noises' variances are supposed to be known). At the initial time the impeding and opposing vehicles are located at distances d(-m) and D(-m), respectively, relative to the passing vehicle, where the index (m) means backward number of radar measurement cycles before the prediction is made. Considering the time required for prediction, the values at the maneuver beginning time t0 will become d0 and D0, respectively. If the opposing and impeding vehicles continue motion at speeds close to the measured ones, then the change in their positions will be approximately linear, which determines the slope (dX/dt) of the corresponding curves (blue and green curves in Figure 3). In turn, the speed measurements also have uncertainty. It is assumed that the actual speed fluctuations detected by the radars remain within the uncertainty thresholds, for which the forecast is considered reliable. In this regard, 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. Point a (tmin, Xpmin) corresponds to the minimum time and distance while Point b (tmax, Xpmax) corresponds to the maximum time and distance. Consider the vehicle's maximum performance with a full fuel supply along the oa curve. Considering the mean performance at Point f (tp, Xp), a distance (df + Lp) would be needed to complete the maneuver, where the linear segment ab represents a set of solutions that correspond to the desired time and distance of the 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 2c), the space dfmin ( Figure 3) should be predetermined.
The safety margin tmm guarantees a distance between the passing and opposing vehicles after the maneuver completion. This corresponds to Point c on the segment ab, and 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 won't reach Point c, keeping the safety margin till time ts. This may be the key point for determining the threshold conditions.
The main idea of the rational point search is to meet simultaneously the criteria of safety margin and vehicle performance reserve for unpredicted circumstances. Suppose the opposing vehicle moves according the nominal straight line yp, and the impeding vehicle moves along the nominal straight line rp. Then, the nominal distance (df + Lp) for completing the maneuver (segment bk) will correspond to ta. Considering tmm, the new boundary will imply the safety limit at time ts. Now, it is necessary to choose a point on segment ac that would meet the required criteria. There many approaches 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 intersects 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 required tp and Xp for the passing vehicle are obtained. At this moment, the distance traveled by the opposing vehicle is Xo, which corresponds to its final position Xof (Point x). This approach ensures stable and gradual redistribution of Point f by increasing the minimum safety margin.
The feasible vehicle acceleration performance corresponds to an upper limit oa and a lower limit that achieves the safety margin oc.
The search for rational values and the law of speed change in the third phase ( Figure 2c) depends on the difference between the speeds of the passing and impeding vehicles, which may take 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 for optimizing the movement trajectory in such a way to ensure both criteria (safety and power margins) for considering possible changes in maneuver conditions (e.g. vehicles' speeds and/or unpredictable forces).

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

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

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

Objective function
The objective function of the model minimizes speed variation, acceleration variation, and sharpness (differences between tangent coefficients of adjacent nodes). Thus, the optimal speed distribution is written as where J = objective function and WV, WA, WS = weighting factors of speed, acceleration, and sharpness, respectively, and JV, JA, JS = corresponding integral functions.
The speed integral function is given by where the last member that does not contain the variable qi (see Eq. (A4), Appendix A) is omitted.
The speed integral must correspond to the distance Sp, considering Eqs. (A1, A3, A8, A10) (Appendix A). For the integral function JV'', where G and K = matrices formed in the same format as D, Eq. (23).
By leaving only the members containing the variables, the problem becomes equivalent to quadratic optimization. Substituting Eqs. (20-25), the objective function of Eq. (19) becomes where Н = equivalent of Hessian for combined speed, acceleration, and sharpness factors, L = vector reflecting the average level of speed.

Lane Change Related Constraint
The lane change is the first phase depicted in Figure 2a. The maneuver starts from a position that satisfies the condition of avoiding a blind spot and to determine the length of the impeding vehicle, i.e. extremely close to the dashed marking line. The main requirement is that the full departure on 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. Also, the tracking of the opposing vehicle's position and speed is retained until the departure to the opposite lane. In the extreme case, a transverse movement may be allowed, in which the minimum safe side distance between the passing and impeding vehicles is provided.
The time required for the passing vehicle to pass from States 1 to 2 ( Figure 2a) is tt (Figure 3). Then, the longitudinal component of the path: The condition for achieving State 2 (Figure 2a), respectively The time tt can be determined iteratively ( Figure 3) after the distribution of the longitudinal speed, based on Eqs. (27-28), considering Xpb = Xp(tt). According to possible slight decrease in the impeding vehicle speed, Xpb may correspond to a small longitudinal gap between the front of the passing vehicle and the rear of the impeding vehicle. Thus, The path's transverse component, respectively: where VY(t) is represented similar to Eq. (21).

Location in Opposite Lane Constraint
The upper limit of the passing vehicle transversal movement Ypo may correspond to its position on the middle of the opposite lane. The possible deflections of this position are restricted by a safe clearance to the road edge (Figure 2b). A lower limit of this clearance is the minimum safe distance between passing and impeding vehicles. Thus, for Xpo and Ypo where tc can be found iteratively according to the condition when the passing and impeding vehicles are abreast at the critical point (State 2, Figure 2b), considering Xp(tc) = Xpb + Xpo (Point e, Figure 3).

Preprints (www.preprints.org) | NOT PEER-REVIEWED | Posted: 6 April 2020
Peer-reviewed version available at Infrastructures 2020, 5, 44; doi:10.3390/infrastructures5050044 According to possible slight increase of the impeding vehicle speed, the passing vehicle's front could be a bit ahead of the front of impeding vehicle. Thus,

Maneuver Completion
The lane change planning is similar to the first phase. Thus,

Preparing the Reference Trajectories
As a result of the optimization, the components of speeds (VX, VY) Т in global coordinates are determined. Therefore, it is necessary to transfer the speeds to the local coordinates of the passing vehicle (Vx, Vy) Т to allow it to consider its maneuvering. Since the yaw angle φ is small, then The ideal forecast for the yaw angle can be obtained as a tangent to the motion trajectory: Thus, to track a virtual trajectory, a state vector may be used corresponding to the capabilities of the current measurements: absolute displacements in global coordinates Xp, Yp, speeds in local coordinates Vx, Vy, and yaw angle φ. As additional parameters, which can be measured directly 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 being estimated indirectly as dφ/dt considering Eq. (35).

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

Vehicle Model Description
Consider the simple linear single track ("bicycle") vehicle model in the standard form of statespace: where x = state vector, y = output vector, u = control vector, and A, B, C, D = matrices.
Suppose that the vehicle maintains its longitudinal speed Vx from Eq. (34a), the state vector x will contain only parameters for the front wheel steering control u = Θf. Thus, x = (Vy, Y, ω, φ) T , where Y = lateral displacement in global coordinates, ω = dφ/dt = yaw rate. Other parameters are denoted above. Provided D = 0, the matrices A, B, C could be derived as: where m, I = vehicle mass and inertia; kf, kr = front and rear tires' side stiffness; xf, xr = local longitudinal coordinates of front and rear tire spots, correspondingly.
Therefore, the output variables for reference tracking are y = (Y, φ) T , which in the real world can be measured using camera and sensors.

Adaptive Model Predictive Control (AMPC) tracking optimization problem
According to the tracking problem, the control parameters should provide closest values to the reference signals. Thus, the cost function for the AMPC [13] controller may by composed of the squared errors' sum to be minimized: Subject to where Qy, Qu, Q∆u -positive semi-defined weight matrices; y * k+i+1|k -Plant output reference signals at the ith prediction horizon step; yk+i+1|k -Plant outputs at the ith prediction horizon step; u * k+i|k -Plant target reference signals at the ith prediction horizon step; uk+i|k -Plant inputs (Manipulated Variables) at the ith prediction horizon step; zk = (u T k|k, u T k+1|k, ··· u T k+p-1|k, εk) -solution; εk -scalar dimensionless slack variable used for constraint softening at control interval k; ρε -constraint violation penalty weight; k -current control interval; p -prediction horizon (number of intervals).
The system of constraints is written as

Application
The purpose of the test is to find stable control and to ensure that the Plant state parameters fit those generated by the proposed methodology.

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

Conclusions
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 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.
Further research can be conducted to improve the algorithm for finding the optimal speed distribution for overtaking. Areas of focus may include: (1) influence of the final speed of the maneuver on the nature of the optimal speed plan; (2) influence of the weight coefficients on the speed plan; (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.

Conflicts of Interest:
The authors declare no conflict 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 Assume that the speed of the passing vehicle within the time interval [t0, tp] varies along the Xcoordinate of the road segment 0-Xp according to the law VX(t). Then, for a grid of n time intervals (t0, t1, t2, …, tn) yield: where ΔTi = time interval (ti -ti-1), which is generally variable.
Assuming τ = ξ·ΔT, the transition between the absolute and normalized basis functions is given by = ( , ) = ( ⋅ , ) = ΔT ⋅ (A6) Considering the basis functions defined only within the finite-element interval, the relation for the entire speed can be written as Since the function also uses (up to second) derivatives of finite element basis functions, the first and second derivatives are obtained, respectively, as