1. Introduction
Usually, planning and tracking stages are separated as hierarchical components; however, a complex approach is also possible [
1]. The planning quality reduces the time cost needed for optimizing the control at the tracking stage, as well as minimizes the probability of AV’s unstable motion modes. The variant of decomposing path and speed planning is caused by the need to decrease the number of space-time variations during optimization when the number of constraints increases. Thus, in studies such as [
2], first, a visible foresighted trajectory was generated, and the speed plan was then optimized with respect to space. Afterward, the optimal control problem was formulated in the space instead of the time domain.
Models. The problem of smoothed speed arose as a need for high-quality control preventing the appearance of sudden loads and unstable transients. Some studies as [
3] attempted to compose speed plans based on cubic polynomials in such a way as to ensure the continuity of accelerations at the transition nodes under conditions of minimizing the jerk. However, the function inflections did not occur directly in the nodes, which led to some excesses of the preset limits followed by the need for complicating the model to compensate for this fluctuation effect. This approach allows obtaining continuous speed values at any point, considering the initial and final values.
The range of models used to represent the AV at the planning stage is quite wide and depends on the control objective. Kinematic models are the simplest and meet the requirements of maximum performance. The bicycle model [
4] is traditional and has proven itself well when using model predictive control (MPC). The best quality corresponds to the state-space kinematic models with the control actions such as jerk and the steered wheels’ angles [
5].
Dynamic models can be both complex and simple to control a single aspect. For example, in [
6] a longitudinal dynamic model is identified to tune a low-level speed-tracking controller. In [
7] a simplified constrained model uses a second-order integrator to match the feasible AV dynamic behavior. This also includes speed planning to automatically adjust the vehicle’s capabilities relative to the road conditions. The yaw, roll, and pitch of AV body’s DOFs, each wheel’s dynamics based on slip ratios, and Pacejka’s combined slip tire model are computed. The comparison of kinematic and dynamic models concerning experimental data [
4] shows different statistics of forecast errors, including the effect of discretization error accumulation. In [
8], a search-based method solves the problem with nonlinear and unstable dynamics.
Objective functions determine the planning and control strategies. Most studies proceed from the control strategy regarding minimizing the motion time while keeping the vehicle within roadway boundaries [
9]. Thus, in [
10], the time-optimal trajectory around a racetrack is obtained by solving a minimum lap time problem (MLTP) for a racing vehicle. In [
11], the path planning strategy includes preventing rear-end collisions during overtaking while minimizing traveling time.
Controllers. Different types of controllers are used for both planning and tracking. The class of controllers based on MPC can be considered the most used that allows obtaining high-quality forecasts due to a combination of hard and soft constraints [
5]. Another popular class is the LQR-type controllers. For increasing performance and overcoming physical limitations, a technique can be represented by the augmented Lagrangian framework [
6], which refers to iterative LQR (ILQR) and Constrained Iterative LQR (CILQR), respectively. In [
12], the sliding mode control (SMC) calculates the total driving force for longitudinal control. In [
7], the reference path is followed by low-level tracking using PID controllers. In [
13], a learning-based Interaction Point Model (IPM) describes the interaction between agents. In [
9], the Nonlinear Model Predictive Control (NMPC) strategy was aimed at controlling a small-scale car model for autonomous racing competitions. Study [
6] presented a hierarchical framework with neural physics-driven models to enable the online planning and tracking of minimum-time maneuvers. A lateral speed prediction model for high-level motion planning was considered with economic nonlinear model predictive control (E-NMPC). In [
1], a linear parameter-varying (LPV) MPC was deployed for AV trajectory tracking and compared with the linear MPC. The outcomes satisfy the processing rate and high-precision criteria, including safely avoiding obstacles. In [
14], the path-following control strategy was based on the linear quadratic regulator (LQR) to compare the performance of models. In [
12], the MPC controller was used to calculate the steering wheel angle and the total yaw moment for lateral control, and the sliding mode control (SMC) was used to calculate the total driving force for longitudinal control.
ADAS. Note that the problem of speed planning is typical not only for AVs but also for ADAS systems. In [
5], it was proposed to form a speed profile for functioning the adaptive cruise control (ACC) based on MPC. The study [
15] considered a preview servo-loop speed control algorithm to achieve smooth, accurate, and computationally inexpensive speed tracking for connected automated vehicles (CAVs). A classic PID with an optimal controller was implemented in an automated vehicle platform for lowering speed-tracking errors, mitigating operations, and smoothing brake/throttle activations. The paper [
11] proposed a system for speed planning using MPC to estimate the vehicle overtake safety while controlling the maneuver for a dynamic vehicle model.
Paper [
16] presented an AV’s trajectory planning and speed control algorithm for static traffic agent avoidance in multi-vehicle urban environments. Decision-making and motion planning frameworks were presented considering both the preceding static and surrounding vehicles. Paper [
17] considers hierarchical architecture for decision making, motion planning, and control for an autonomous racing vehicle. The supervisor determined whether the subject vehicle should stay behind the preceding vehicle or start overtaking. A high-level trajectory planner generated the desired path and velocity profile in a receding horizon mode. A low-fidelity kinematic vehicle model was utilized for planning, and a low-level trajectory tracker based on MPC and dynamic vehicle model generated the lateral and longitudinal control inputs.
Some studies implemented the use of four-wheel models with increased degrees of freedom, refined tire models, actuators, etc. Thus, in [
14], the question of tire dynamics’ influence on vehicle dynamics’ control was raised. The influence of tire nonlinearity on path offset was revealed. In [
12], the longitudinal and lateral motion control for the four-wheel independent drive intelligent vehicle was designed: the upper layer was the motion controller, and the lower layer was the control distributor. In [
8], complex dynamics in a predictive manner were applied to achieve optimal robot behavior in dynamic scenarios. In paper [
18], a new real-time motion planning technique for an autonomous mobile robot is proposed considering actuator capacities’ limits and tire-ground adhesion constraints.
Actuators. Study [
9] includes a simple drivetrain model in the optimization problem to limit the lateral and longitudinal forces acting on a car. An Autonomous Emergency Braking Pedestrian (AEB-P) was introduced in [
19] to prevent collisions between vehicles and pedestrians. The emergency brake planner generates vehicle deceleration followed by tracking the trajectory, where the PI-controller was adapted to provide the optimum braking force. In [
15], the brake/throttle control laws were introduced in five parts: three feedback controls of system states and two feedforward items previewing road slope and target speed. Study [
20] proposes a clothoid-curve-based trajectory tracking control method to solve the problem of tracking errors caused by the discontinuous curvature of the control curve calculated by simple algorithms. The parameters of clothoid reference curves are tied to the vehicle’s safe motion constraints.
Despite the intensive studies on motion planning, in our opinion, there still are some issues that can be conditionally divided into three parts: vehicle model, restrictions, and planning techniques.
Vehicle model. Note that most studies in the field of AV motion planning are focused on generating reference curves using relatively simple (often kinematic bicycle) models. However, the motion planner’s high performance does not yet mean high forecast quality. The most important indicators are the prognosis feasibility and ensuring good accuracy in reflecting features of a specific vehicle design. Considering the intensive use of control systems in modern vehicles (for example, traction distribution), it is often not enough to obtain the trajectory and kinematic parameters’ references. Moreover, the desirable parameters themselves may significantly depend on the technologies used in a real vehicle. For example, the distribution of drive torques between the same axle’s wheels can be carried out through a symmetrical differential, through a sport differential (torque vectoring), and by activating the inner wheels’ brakes. All three options will cause different axle drive power, different traction and lateral forces, the tire–road adhesion degree, and, as a result, affect differently on forming the yaw moment. In a classic uncontrolled AWD drivetrain, a wheel with the worst adhesion conditions limits the traction potential. And vice versa, in transmissions design with individual wheel control, the maximum realization of total adhesion potential is possible. In the case of using a standard vehicle model, there will be no difference between these two variants when speed is the planning object. If an extended model is used, including the drive type and redistribution of vertical reactions, then, as shown in our previous articles, the difference between predictions of kinematic parameters may be significant especially for cases with low tire adhesion. Thus, this study’s target consists of composing an inverse vehicle dynamic model that allows estimating physical factors based on the kinematic parameters.
A set of constraints is a critical factor in reflecting the realism of forecasting and the optimization procedure’s performance. On the one hand, increasing the number of restrictions complicates the mathematical optimization model; on the other hand, narrowing the boundaries of AV motion parameters contributes to diminishing solution iterations and time costs.
When using a conventional kinematic model, constraints are also basically kinematic (linear and angular velocities, linear and angular accelerations, jerk, etc.), which does not always correspond to natural physical limits. For example, many researchers use a constant upper acceleration limit without relating it to the throttle response properties (speed–acceleration) for a specific vehicle design. At the same time, the traction potential decreases with speeding even for the case of electric drive, which especially should be considered for a medium-speed range.
The physical limitations are the most important but require an extended vehicle model. First, these restrictions are responsible for preventing motion instability, critical modes of vehicle-road interaction, and are associated with the design solutions for distributing the power by wheels. Controlling the tires’ adhesion limits is the most important aspect, requiring predicting both a redistribution of normal reactions and torques on wheels. In turn, the torque depends on the drive design and control. In addition, pure kinematic models do not allow direct operation with longitudinal and lateral slip. Nevertheless, it is possible to indirectly estimate tires’ slip angles and lateral deformations followed by including them in the constraints.
Safety restrictions. Most often, this includes speed limits when approaching moving and static obstacles. Within this planning technique, we suppose that a trajectory considering obstacles and speed mode has already been built. Therefore, in the framework of this study, we assume that the AV uses a safe space for realizing a maneuver. As a safety measure, the body roll angle caused by lateral inertia forces is applied. Restricting the maximum roll angle, limitations on lateral acceleration are automatically imposed.
Thus, in advance, we proceed from the fact that growth in the number of restrictions positively affects the rapidity and quality of forecasting the AV motion.
Planning technique. Here, the most important requirements concern ensuring the smoothness and unambiguousness of predicted functions. The most common approach implies using polynomials that describe the change in speed concerning time along a trajectory section. However, this does not always lead to a qualitative consistency between the speed and curvature derivatives involved in the formation of kinematic parameters such as jerk and angular acceleration. That is, there may be disruptions in the piecewise representation of these functions. This, in turn, does not contribute to the possibility of using inverse dynamics to determine the force factors acting on the vehicle. In this regard, our method consists of using an inverse approach, when instead of speed its second derivative is directly modeled by smooth functions. In this case, the continuity and smoothness of the vehicle’s linear and angular accelerations will be guaranteed.
Figure 1 shows a general approach scheme reflecting the modeling stages and the parameters used. The SQP optimization ensures iterative process, within which kinematic, dynamic, and physical parameters are evaluated to form optimization criteria and constraints. To model vehicle kinematics, the inverse method is used, allowing the representation of the longitudinal speed function via finite elements. The external data are the preset boundary values of the kinematic parameters, initial conditions, and the precalculated trajectory model. The SQP-loop at each iteration generates a vector of nodal parameters for the longitudinal speed’s second derivative. A simple and smooth form of the speed function is obtained by double numerical integration. After evaluating accelerations, the distribution of normal reactions over the wheels can be found. Using the AWD transmission model, vehicle steerability and nonlinear tire models, and algorithms for controlling traction distributing actuators, the inverse dynamics problem is solved, followed by determining the needed longitudinal and lateral forces and estimating degrees of longitudinal and total tire adhesion use. Knowing the lateral forces and accelerations allows estimating the equivalent sideslip angles, tire lateral deformations, and the body roll angle, including them in a set of nonlinear constraints. As a result, the optimization output contains kinematic, dynamic, and physical parameters, as well as the necessary control for actuators.
4. Constraints
General Integral Approach to Nonlinear Equality Constraints. Since the kinematic, dynamic, and physical vehicle motion parameters have been formed, let us consider the integral technique of composing equality constraints. Suppose that smooth piecewise polynomial functions describe all the parameters based on the nodal DOFs of the speed and curvature derivatives. Since the numerical integration based on the Gaussian scheme is applied for optimization, the same scheme will be used to form restrictions. Assume that some parameter
Ψ changes along the path
s so that it does not exceed the upper
ΨU and lower
ΨL boundaries. Then, the sum of the areas between the upper limit
ΨU and the function
Ψ and between the lower limit
ΨL and the function
Ψ must be strictly equal to the area within boundaries. That is, for
i-th FE and along the trajectory
Integrals can be calculated numerically, for which we introduce the following denotations.
For integrals between bounds (
UL), upper bound and function (
UF), and function and lower bound (
FL), we may denote:
Then, for the
i-th path segment, using Equation (131), vectors of Equation (146) functions are represented as follows
Combining all the segments, the integrals can be calculated numerically as
Thus, the requirement of nonlinear equality constraint along all the segments for a parameter
Ψ, according to Equation (145) is expressed as follows
Kinematic Parameters’ Constraints. Using this scheme above, we form a vector
Ψk of nonlinear integral constraints for a series of kinematic parameters such as longitudinal speed, yaw rate, angular acceleration, and longitudinal jerk, where each
j-th element corresponds to
Ψkj.
where
ΨkU,
ΨkL = upper and lower limits of kinematic parameters,
ck = vector of constraints corresponding to kinematic parameters.
Dynamic Parameters’ Constraints. The vector Ψd of dynamic parameters includes slip angles δ, tire lateral deformations Δμ, roll angle ψ, and longitudinal acceleration aζ as a function of speed. The maximum allowable roll angle at 0.4 g should not exceed 7°. For passenger cars, the recommended limit value is ψU = (10.8 − 4.3 B24/hg/2)°. We may accept 7.5°. Thus, introducing the limits ψL = −ψU. Let us use symmetrical limits.
A particular case is the limitation of the vehicle’s traction potential. The vehicle’s maximum acceleration depends on the speed and is due to design features. Thus, if the vehicle speed–acceleration characteristic is known, the condition must be met
where
aζU,
aζL = upper and lower limit values of acceleration potentially implemented by the vehicle.
The tire’s lateral deformation limits are Δ
μL = −Δ
μU. Combining the parameters, we obtain the vectors
Adhesion Constraints. Having calculated the necessary traction forces on the wheels according to Equation (54), including Equations (55) and (56), we determine the degree of using the longitudinal and lateral adhesions on each
j-th wheel.
Then, the basic condition regarding the
j-th wheel’s adhesion potential can be formed as follows
Using Equation (154), it is possible to impose physical restrictions on the conditions of all tire–road adhesions. Then
Boundary Parameters’ Constraints. Another type of constraint determines the boundary conditions of kinematic parameters. Thus, one can require, for example, that the initial (
0) and final (
f) values of the predicted acceleration and jerk must correspond to preset constant values
Aζ0(f) and
Jζ0(f). That is,
The desired final speed may also be included.
Total restricting conditions. Thus, the complete sets of parameters, bounds, and nonlinear constraints are
5. Simulation
Trajectory. As an example, consider planning a speed mode on a curvilinear section shown in
Figure 7. Let us assume that the maneuver trajectory has already been determined by the inverse method described in our previous studies.
Before optimizing the speed mode, let us set the values of weight coefficients
Wv in Equation (144) and limit values.
Speed derivative model limits
Initial and limiting values of kinematic parameters
We use the data of Audi A4 3.2 FSI [
24] to represent the AV. All the calculations are accomplished by using MATLAB tools [
25].
Comparison of drive cases’ outputs.
Figure 8 shows the results of output parameters for the various torque vectoring schemes corresponding to the variants (
A,
B,
C,
D) in
Figure 4. Note that the same initial conditions and vehicle data are applied for all variants. Let us consider a case of road conditions that provide the maximum adhesion coefficient
φmax = 0.5.
Longitudinal speed and acceleration. As seen in
Figure 8a,b, all variants provide a smooth and similar shape increase in speed. The lowest peak values of speed and acceleration belong to variant (
B) with single rear sport DM. Variants (
A) and (
D) show close results, which are explained by the same strategy of using the longitudinal adhesion potential for the same axle wheels. In this case, it does not matter in which way the redistribution of drive torques on the same axle’s wheels is achieved—due to internal or external moments relative to the semi-axles of the inter-wheel differential. Finally, the greatest speed and acceleration are achieved with variant (
C), combining the rear sport DM and front brakes. This is because at the moment of vehicle acceleration, the inner front wheel is braked, which leads to an increase in resistance on its semi-axle with a subsequent torque raising on the opposite outer semi-axle. This results in growing the torque on the inter-axle differential’s output shafts. That is, this variant provides not only the torque distribution over the same axle wheels but partially over the axles too.
Jerk and roll angle. As seen in
Figure 8c, longitudinal jerks differ in amplitude but do not exceed the specified limits, while simultaneously meeting the requirements of zero initial and final values. Obviously, with an increase in acceleration amplitude, the jerk also grows; nevertheless, the curves’ unambiguity and smoothness are ensured for all drive variants. This indicates a certain positive effect of the torque vectoring on the smoothness and stability of the curvilinear motion.
Figure 8d shows pseudo-roll angles reflecting the potential vehicle’s body roll due to predominantly centrifugal forces. Since the trajectory curvature determines the lateral acceleration, which is the same for all variants, the roll angles differ little and do not exceed the critical values.
Yaw rate and angular acceleration. As seen in
Figure 8e,f, the curves of yaw rates and angular accelerations are almost the same for different drive control options. This is because the trajectory for all variants is the same and only the motion speed character stipulates the differences. Owing to the unprecedented smoothness and continuity of the angular acceleration functions, it is possible to successfully apply the inverse modeling approach, i.e., to evaluate force and dynamic parameters using the predefined kinematic ones. Note that the smoothness of all reference functions is one of the most important factors for the AV’s stable control.
Inter-axle (central) DM torques.
Figure 8g shows the torques on the central differential’s carrier. As expected, the highest power costs correspond to variant (
A), since the drive torques on the inner wheels are limited by the braking moments. That is, despite the high-quality regulation, this option requires excessive energy costs. The lowest energy consumption is provided by variant (
B) with a rear sport DM, which requires almost 250 Nm less than in the case of variant (
A). Variant (
C) also ensures less torque than (
A) and, nevertheless, more speed. Variant (
D) is characterized by the lowest specific power costs regarding the speed mode since the torque redistribution is provided by internal and not external moments relative to the wheels’ driving semi-axles.
Figure 8h shows the distribution of torques between the vehicle axles by the variants. Note that the main difference in the values of the axles’ drive torques is observed in the first phase of acceleration since the effect of redistributing the vertical reactions is especially sensitive for the rear axle. As the steering angles and acceleration begin to diminish when the curvature decreases (about 35 m of the path), the rear tires’ kinematic radii instantly become smaller than the same radii of the front wheels. As a result, this leads to the temporal effect of equalizing the axles’ drive torques. Subsequently, owing to decreasing the influence of redistributing the vertical reactions and approaching the trajectory to a straight line, the ratio of torques becomes close to the mechanically preset in the inter-axle differential (i.e., 40/60).
Case A.
Figure 9 summarizes the results of optimizing the parameters for the variant by controlling the rear and front brakes. As seen (
Figure 9a,b), the sideslip angles and the tires’ lateral deformations change smoothly and do not exceed the preset limits. The peak sideslip angles are reached by the front inner and rear outer wheels. In this case, the front inner wheel moves along the path with the greater curvature, and the rear outer wheel has the maximum vertical load (
Figure 9c). The distribution of lateral forces (
Figure 9d) is mostly proportional to the wheel loads. Due to individual adjustment, all longitudinal forces (
Figure 9e) on the wheels are different. The braking moments are of greatest interest (
Figure 9f).
As seen, the braking moments of the inner wheels change smoothly, and the front inner wheel’s braking moment is greater than that of the rear one. This is due to the need for withdrawing a larger moment from the front inner wheel, which is in worse adhesion conditions. At the same time, the braking moments of the outer wheels remain zero. Owing to the proposed strategy for distributing torques between the same axle wheels, the longitudinal adhesion potentials are pairwise the same for the wheels of the front and rear axles, respectively (
Figure 9g). The lateral adhesion factor (
Figure 9h) and total adhesion (
Figure 9i) are represented by relatively narrow bunches of the common tendency curves. At the same time, none of the wheels violates the adhesion limit requirements.
Figure 9j depicts the relation between the acceleration’s longitudinal and transversal components, showing continuity, smoothness, and good agreement with the trajectory curvature and the longitudinal speed’s derivatives as well.
Case B.
Figure 10 shows the results of optimizing parameters for the variant with a controlled rear sport DM. As seen (
Figure 10a,b), the sideslip angles and the tires’ lateral deformations have the smallest values among all the variants since the speed is also the lowest. In
Figure 10e reflecting the longitudinal reactions, as noted, the traction forces on the front wheels are almost coincident, which is explained by the equality of the drive torques on the semi-axles of the symmetric (open) front DM.
Figure 10f depicts the internal moment of the rear sport DM, which is positive for the outer wheel’s clutch and zero for the inner wheel’s clutch. In
Figure 10g, the rear wheels’ adhesion potentials are the same, unlike the front wheels. Thus, the adjustment possibilities are limited by the adhesion of the inner front wheel. Given the drive design at the specified road and initial conditions, this variant partially underuses the adhesion limit but provides good smoothness of the vehicle dynamics.
Case C.
Figure 11 reflects the results of optimizing the parameters for the combined control variant with the rear sport DM and front brakes. As seen (
Figure 11a,b), the sideslip angles and tires’ lateral deformations reach the largest values among all options since the speed is also the highest. In
Figure 11e the wheels’ traction forces differ significantly in the maneuver’s first phase and converge in the second phase after decreasing the curvature.
Figure 11f depicts the moments required for the rear sport DM’s clutches and front brakes. These moments are positive for the rear outer wheel’s clutch and front inner wheel’s brake, and they are zero for the rear inner wheel’s clutch and front outer wheel’s brake. In
Figure 11g, the adhesion potentials of the front and rear wheels are pairwise coincident, which emphasizes the strategy’s effectiveness. Thus, the control possibilities are expanded and make it possible to bring the wheels to modes close to the limiting adhesion conditions if needed.
Case D.
Figure 12 shows the results of optimizing the parameters for the variant with two controlled inter-wheel sport DMs. As seen (
Figure 12a,b), the sideslip angles and tires’ lateral deformations are characterized by smoothness and their values do not exceed the corresponding limits. In
Figure 12e the front wheels’ traction forces for this variant play the greatest role in comparison with other schemes, which has a positive effect on the vehicle steerability.
Figure 12f depicts the internal moments of the sport DMs’ clutches, which are positive for the outer wheels’ clutches and zero for the inner wheels’ clutches. In
Figure 12g, the front and rear wheels’ adhesion potentials are pairwise coincident. Thus, the possibilities of regulating motion stability and vehicle controllability are maximized.
Note that this drive variant is characterized by the highest cost despite the high efficiency and economy since there is no external resistance applied to the wheels’ semi-axle.