A Simple Dynamic Model for Aggressive, Near-Limits Trajectory Planning
Abstract
In normal on-road situations, autonomous vehicles will be expected to have smooth trajectories with relatively little demand on the vehicle dynamics to ensure passenger comfort and driving safety. However, the occurrence of unexpected events may require vehicles to perform aggressive maneuvers, near the limits of their dynamic capacities. In order to ensure the occupant’s safety in these situations, the ability to plan controllable but near-limits trajectories will be of very high importance. One of the main issues in planning aggressive maneuvers lies in the high complexity of the vehicle dynamics near the handling limits, which effectively makes state-of-the-art methods such as Model Predictive Control difficult to use. This article studies a highly precise model of the vehicle body to derive a simpler, constrained second-order integrator dynamic model which remains precise even near the handling limits of the vehicle. Preliminary simulation results indicate that our model provides better accuracy without increasing computation time compared to a more classical kinematic bicycle model. The proposed model can find applications for contingency planning, which may require aggressive maneuvers, or for trajectory planning at high speed, for instance in racing applications.
algocfalg.algs. \CrefnamealgocfAlgorithmAlgorithms
1 Introduction
Planning safe and efficient trajectories remains an important challenge for autonomous driving, in particular when approaching the limits of handling of the vehicle. Arguably, most driving situations do not require pushing the vehicle to its limits; however, some situations may require the ability to plan “aggressive” maneuvers to guarantee the safety of the vehicle and its occupants, for instance when driving at high speed or in low adherence conditions. The ability to plan aggressive maneuvers can also be beneficial to compute contingency trajectories in parallel with a comfortable “reference” one. For instance, aborting an overtaking maneuver due to new perception data involves combined braking and steering, which can result in loss of adherence at high speed. To ensure that contingency trajectories are feasible, such planners should be able to precisely take into account the vehicle’s dynamic limitations.
One of the main difficulties of aggressive trajectory planning lies in the heavy nonlinearities of the vehicle dynamics when close to its handling limits. These nonlinearities arise from various phenomenons, and are often difficult to take into account in a trajectory planner. For this reason, the road-tire forces and the variation of the vertical forces on each wheel (load transfer) are often ignored or extremely simplified at the planning stage. In the existing literature, many authors (see, e.g. [1, 2]) simply rely on a kinematic modeling of the vehicle as a bicycle, which can result in planned trajectories which are infeasible in practice, or inefficient because safety margins have been chosen too large, leading to overcautious driving.
Some authors have considered more realistic vehicle dynamics for aggressive maneuvers or when driving on slippery roads, for instance in the presence of snow [3]. For such demanding scenarios, a majority of references use Model Predictive Control (MPC) techniques with a more precise vehicle model, allowing to simultaneously plan a trajectory and compute the corresponding feasible control. In this case, most authors consider a dynamic bicycle model [4, 5, 6, 7, 8] with various levels of complexity in the modeling of the tire forces.
The main limitation of finer vehicle models which include wheel dynamics is that the wheel velocities generally have much shorter characteristic times (around \SI1\milli\second [9]) than the vehicle’s dynamics (typically \SI100\milli\second). As a result, models taking wheel dynamics into account require a very short integration time step in MPC formulations, which greatly reduces the planning horizon that can be considered in real-time computation. The main contribution of this article is an alternative approach to take into account finer information about the vehicle dynamics while remaining computationally tractable over a planning horizon of a few seconds. Instead of directly using the (highly complex) dynamic equations of the vehicle during online solving, we first compute offline the set of feasible longitudinal, lateral and angular accelerations for various initial states of the vehicle. We then propose a convex approximation of this feasible region, which allows reformulating the vehicle dynamics using a carefully constrained second-order integrator model. The reduced complexity of this model makes it easy to implement as an MPC planner, and allows using longer time steps for numerical optimization.
Note that some authors have already studied approximations for the set of reachable accelerations. A commonly used model is the so-called “friction circle” [10] (or ellipse [11, 12]), in which this set is approximated by a simple Coulomb modeling of the friction forces. However, this approximation is generally used to account for slip at the wheel level, and thus requires modeling wheel dynamics. Another possible approach is to directly measure actual acceleration data on a test vehicle; such results have been summarized in a so-called “g-g diagram” in [13], but these experiments are difficult and costly to perform. In this article, we use a simulation-based approach to compute synthetic feasibility envelopes for the vehicle. Interestingly, our results show that these envelopes (computed for the whole vehicle) are actually closely related to the g-g diagram used in [13, 14].
The rest of this article is structured as follows: in \Crefsec:vehicle-model, we present a 9 degrees of freedom dynamic model of the vehicle’s body, that we will use throughout the rest of this article. In \Crefsec:computation-feasible, we describe an offline method to compute the sets of feasible accelerations for the vehicle, and we derive a constrained second-order integrator dynamic model in \Crefsec:2d-integrator. In \Crefsec:numerical-sim, we provide preliminary simulation results, comparing the proposed model with a more classical kinematic bicycle one. Finally, \Crefsec:conclusion concludes the study.
2 Vehicle model
In this section, we describe a 9 degrees of freedom vehicle body model that we will use throughout this article. Alongside with the usual 2D state (with the yaw rotation) of the vehicle, the model takes into account its roll and pitch movements, the dynamics of the wheels and the coupling of longitudinal and lateral slips of the tires. Being a chassis model, it does not take into account the dynamics of the car engine or brakes. The control inputs of the vehicle are the torque applied to each wheel and the steering angle of the front wheels, . In this article, we use uppercase letters (e.g., , ) to denote coordinates in the ground (global) frame, and lowercase letters for coordinates in the vehicle (local) frame; the coordinate in the local frame corresponds to the longitudinal component. The notations are given in \Creftab:notations and illustrated in \Creffig:carSim.
, , | Position of the vehicle’s CoM (ground frame) |
, , | Roll, pitch and yaw angles of the car body |
, | Longitudinal and lat. vehicle speed (vehicle frame) |
Longitudinal speed of wheel (wheel frame) | |
Angular velocity of wheel | |
Displacement of suspension | |
Steering angle of the front wheels | |
Total torque applied to wheel | |
, | Longitudinal and lateral forces on wheel (wheel frame) |
, | Longitudinal and lat. forces on wheel (vehicle frame) |
Normal ground force on wheel | |
Air drag force on the vehicle | |
Total mass of the vehicle | |
, , | Roll, pitch and yaw inertia of the vehicle |
Inertia of wheel around its axis | |
, | Distance between the front/rear axle and the CoM |
Half-track of the vehicle | |
Effective radius of the wheels | |
, | Suspensions stiffness and damping |
In what follows, we assume that the body of the vehicle rotates around its center of mass, and that the aerodynamic forces do not create a moment on the vehicle. Moreover, we assume that the road remains horizontal, and any slope or banking angle is neglected; this assumption could be relaxed using a slightly more complex vehicle model. Under these hypotheses, the dynamics of the vehicle’s center of mass are written as:
(1a) | ||||
(1b) | ||||
(1c) | ||||
(1d) |
where and are respectively the longitudinal and lateral tire forces generated on wheel , expressed in the local vehicle frame . The yaw, roll and pitch motions of the car body are computed as:
(2a) | ||||
(2b) | ||||
(2c) |
where , with the displacement of suspension for the given roll and pitch angles of the car body. The variation of models the impact of load transfer between tires. Finally, the dynamics of each wheel can be written as
(3) |
In general, the longitudinal and lateral forces and depend on the longitudinal slip ratio , the side-slip angle , the reactive normal force and the road friction coefficient . The slip ratio of wheel can be computed as
(4) |
The lateral slip angle of tire is the angle between the wheel orientation and its velocity, and can be expressed as
(5) | ||||
(6) |
where and denote the front and rear wheels.
In this article, we use Pacejka’s combined slip tire model (equations (4.E1) to (4.E67) in [15]), which takes into account the interaction between longitudinal and lateral slips, thus encompassing the notion of friction circle [16]. For clarity purposes, we do not reproduce the complete set of equations here.
3 Feasible acceleration sets
In theory, it is possible to use the dynamic model presented in \Crefsec:vehicle-model inside a Model Predictive Control (MPC) scheme to compute an optimal control in the form of applied engine and braking torques on the wheels, and a steering angle for the front wheels. However, the corresponding optimization problem would involve a highly nonlinear, nonconvex objective function which furthermore is non-differentiable due to the disjunction (4). In practice, most available solvers seem unable to handle this problem, except for extremely simple situations.
Several ways around this limitation have been proposed in the literature, in order to take into account chassis and tire dynamics in an MPC formulation. In [3], the authors use the wheels slip ratios instead of the applied torque as control variables, and assume that a low-level controller can adjust wheel velocities accordingly. However, the feasible dynamics of the slip ratio have not been studied yet, and the low-level control proposed by the authors is limited to relatively low slip, remaining in the linear portion of the Pacejka model.
In this article, we only consider the dynamic response of the car body, and in particular we do not precisely model engine response. Instead, we assume that the engine can deliver a torque comprised between \SI0\newton\meter and , and that the brakes can apply a negative torque between and \SI0\newton\meter on each wheel. The engine torque is equally split between the two front wheels, and braking torques are supposed equal for wheels on a same axle. Note that torque vectoring [17], in which the accelerating and braking torques are not equally divided between the wheels of an axle, can also be treated using the same method. The steering angle of the front wheels is supposed to be bounded between and . With these hypotheses, we note the set of admissible controls and a control, where is the torque applied on each of the front wheels, the torque on the rear wheels, and the steering angle for the front wheels.
Using the dynamic model of \Crefsec:vehicle-model and starting from a known system state , it is possible to compute future states of the vehicle under a known control input using numerical integration. In this article, we use a fourth order Runge-Kutta integration scheme with a time step duration of \SI1\milli\second, which appears to be sufficient to correctly handle the wheel dynamics. We compute an approximation of the set of feasible accelerations starting from as presented in \Crefalg:sampling. In the algorithm, the function fitpolynom(, , 2) returns the coefficients of the best fitting polynom of order for the component of , with leading coefficient first. Therefore, the variable feas contains the set of resulting accelerations in the and directions (noted and ) as well as the yaw rate acceleration (noted ), all expressed in the ground coordinates frame. In what follows, we present outputs from \Crefalg:sampling for varying conditions. The control bounds are chosen as , and .
\KwDatastate , num. of samples , horizon , step set feas := [] \For randomly choose \For set RK4 (, set := () set := fitpolynom(, X, 2) set := fitpolynom(, Y, 2) set := fitpolynom(, , 2) append to: feas,
3.1 Longitudinal velocity
In \Creffig:feasible, we present the computed shapes of the set of reachable accelerations in the , and planes for a standard berline car (, , , , front-wheel drive), over a horizon of \SI0.1\second and for various initial longitudinal velocities . The initial state of the vehicle is taken with all angles and initial velocities (except the longitudinal one) equal to zero for the car body, and the wheels are initially rolling without slipping (i.e. for all ). The friction coefficient for the road-tire contact is chosen equal to . Note that this technique assumes a constant control over a time interval of \SI0.1\second; therefore, the impact of ABS or ESP cannot be measured, and may be the cause of the concavity at maximum braking observed in \Creffig:feasible-axay,fig:feasible-axaY at higher velocities.
Remarkably, the projections of this set on the and planes remain very similar throughout the whole speed range; namely. In the plane (\Creffig:feasible-ayaY), the projections are all located along the same line, except for high lateral accelerations at high speed in which over- and understeering can occur. We will use these properties to derive efficient bounds in the next section.
3.2 Lateral velocity
In \Creffig:feasible-vy, we present similarly computed shapes for the sets of reachable accelerations when varying the initial lateral velocity with an initial longitudinal velocity \SI20\meter\per\second. Initial wheel velocities are chosen, as before, as for all and initial angles and angular velocities for the car body are chosen as .
As the initial lateral velocity increases, the sets in the and planes is shifted mostly along the and axes respectively. Note that we also observe a slight gain in longitudinal acceleration, which corresponds to the fact that part of the initial lateral velocity can be “redirected” into longitudinal velocity by turning the vehicle, though this gain is very marginal. Moreover, the sets are progressively skewed as the lateral velocity increases. Interestingly, we note that increasing lateral velocity further than does not provide additional acceleration performance, and instead reduces the commandability of the vehicle thus motivating to avoid these regions during planning.
3.3 Friction coefficient
In the Pacejka combined slip tire model [15], the tire-road friction coefficient appears both as a multiplier and a nonlinear term in the tire-road forces. In \Creffig:feasible-mu, we show the variation of the envelope of feasible accelerations with . As for the study on initial longitudinal velocity, we observe that the envelopes keep a similar shape in the and planes, despite the nonlinearity of the tire model. In the , and in spite of more important slip occurring, the reachable sets also remain aligned along the same line.
Moreover, our sampling-based method evidences an interesting pattern as the friction coefficient decreases. \Creffig:sampled-mu compares the distribution of the sampled points for (icy road) and (dry road); different scales have been chosen for better readability. For , we observe that the sampled points are almost uniformly located inside the feasible envelope. However, for , the sampled points accumulate near several regions of attraction, with wide areas with relatively few sampled points. This observation suggests that caution should be exercised when using direct planning methods based on sampling the control space, such as proposed in [18], since the planner would be heavily biased towards these attraction regions, especially in low adherence situations.
3.4 Initial rotation
Equations (1) and (2) show that the vehicle dynamics (except for the position in the ground coordinates) do not depend on the initial yaw angle ; therefore, the feasible regions presented above remain invariant (up to a rotation) with respect to the initial yaw angle. Moreover, we observe only very small variations of these regions for small initial values of the pitch and roll angle or rates, and variations of initial velocities of the wheels; these effects are neglected in the rest of this article.
4 Second-order integrator model
Using the results from the previous section, we propose a constrained double integrator model for the vehicle dynamics. In general, such models are considered very rough approximations for the actual dynamics; however, using well-chosen constraints to couple the longitudinal, lateral and yaw accelerations, a relatively precise approximation can be obtained in this case. The proposed model considers a state vector and a control , with the same notations and reference frames as presented in \Crefsec:vehicle-model. The dynamic equation of the system is with
(7) |
In theory, it is necessary to take into account the initial state of the vehicle at each time step, and use the sets shown in \Creffig:feasible,fig:feasible-vy to determine the feasible accelerations for the vehicle. However, the complex shape of these sets makes it impractical for trajectory planning. Instead, we propose to compute the “complete” set of feasible accelerations for the vehicle, i.e. the union of the sets shown in \Creffig:feasible-vy, which does not depend on the initial lateral velocity. These sets are shown in \Creffig:feasible-fullvy; interestingly, the boundary of \Creffig:feasible-axay-fullvy can be reasonably well approximated as a truncated ellipse, which is very close to the “g-g diagram” presented in [13], although slightly smaller.
Using these results, we propose to approximate the sets shown in \Creffig:feasible-axaY as a cropped ellipse in the plane, and a parallelogram in the plane. The parallellogram is chosen constant with the initial velocity, whereas the lower and upper bound on slightly var with the initial longitudinal speed. Note that a study of the 3D set of feasible accelerations (not displayed here) shows that this region is roughly convex, except for the lowest values of . Therefore, it is only necessary to consider constraints in two of these planes to ensure that a corresponding point exists in the 3D feasible region.
The resulting set of constraints on can be written as:
(8) | |||
(9) | |||
(10) |
where is a constant matrix, a constant vector and , depend on . For the proposed vehicle, the experimental data of \Creffig:feasible-fullvy yield \SI9.4\meter\per\second\squared, \SI9.0\meter\per\second\squared, and \SI\meter\per\second\squared. The evolution of and with are shown in \Creffig:alpha-beta; a polynomial fit yields and (with expressed in \SI\meter\per\second and accelerations in \SI\meter\per\second\squared).
Note that these constraints only guarantee the feasibility of a trajectory. To actually drive the vehicle, it is necessary to find a high-frequency low-level control loop capable of following this feasible trajectory. Moreover, the current set of constraints does not account for limitations on the actuator dynamics, which will be the subject of future work.
5 Numerical results
The previous section provides a set of conditions for the dynamic feasibility of a trajectory, in the form of bounds on vehicle acceleration. These constraints can be used to design a trajectory planner for the vehicle, for instance using model predictive control (MPC). The specifics of our MPC implementation are out of the scope of this paper, and are presented in details in [19]. In this section, we simply present figures extracted from [19] to illustrate the good performance of our model when compared to a more classical kinematic bicycle one [20]. For both models, the planner is used to drive a vehicle along a circuit at high speed; the planning horizon is chosen as , and the time step of the MPC solver (based on the ACADO Toolkit [21]) is \SI0.2\second.
Model | Avg. comp. time | RMS lat. error | Max. lat. error |
---|---|---|---|
Proposed | \SI59.8\milli\second | \SI0.25\meter | \SI0.70\meter |
Kinematic | \SI57.8\milli\second | \SI0.16\meter | \SI0.95\meter |
tab:comput-time provides synthetic performance data for both planners when driving around a circuit, showing roughly similar computation time and lateral error. However, \Creffig:comp-speed shows that our planner achieves significantly higher speeds in curves, nearing lateral accelerations of while reducing the maximum lateral error. Moreover, an interesting property of our proposed model is that it makes the corresponding MPC planner more robust: in presence of obstacles, the kinematic planner sometimes fails to output a solution in real-time (as shown in \Creffig:comput-time); this phenomenon does not occur with our proposed model. In both cases, the superior performance obtained with the proposed model is likely due to the simpler relations between the outputs (future positions) and the control inputs, which allows the solver to converge faster, towards better solutions.
6 Conclusion
In this article, we proposed a new modeling of vehicle dynamics as a constrained second-order integrator. First, we described a high fidelity 9 degrees of freedom vehicle model including tire slip and load transfer. We used this model with an offline random sampling technique to show that the proposed second-order model, despite its simplicity, is able to capture most of the relevant dynamics of the vehicle up to its handling limits. Moreover, we showed that this model is also compatible for driving on slippery roads, at the cost of a changing a few parameters. Implementation of the second-order integrator model inside an MPC-based trajectory planner shows that computation time remains roughly similar compared to using a kinematic bicycle model, but solution quality and robustness seem to be improved, notably in presence of obstacles. These results open several research perspectives since planning aggressive trajectories has often been thought to necessitate highly precise, and thus complex, vehicle models. Future research will focus on designing a low-level control law capable of precisely tracking the generated feasible trajectories, and validating these results on a real, scale model of a vehicle.
References
- M. A. Abbas, R. Milman, and J. M. Eklund, “Obstacle avoidance in real time with Nonlinear Model Predictive Control of autonomous vehicles,” in 2014 IEEE 27th Canadian Conference on Electrical and Computer Engineering (CCECE). IEEE, may 2014, pp. 1–6.
- V. Cardoso, J. Oliveira, T. Teixeira, C. Badue, F. Mutz, T. Oliveira-Santos, L. Veronese, and A. F. De Souza, “A Model-Predictive Motion Planner for the IARA Autonomous Car,” arXiv preprint arXiv:1611.04552, nov 2016.
- F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat, “MPC-based approach to active steering for autonomous vehicle systems,” International Journal of Vehicle Autonomous Systems, vol. 3, no. 2/3/4, p. 265, 2005.
- P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, and D. Hrovat, “Predictive Active Steering Control for Autonomous Vehicle Systems,” IEEE Transactions on Control Systems Technology, vol. 15, no. 3, pp. 566–580, may 2007.
- J.-M. Park, D.-W. Kim, Y.-S. Yoon, H. J. Kim, and K.-S. Yi, “Obstacle avoidance of autonomous vehicles based on model predictive control,” Proc. of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, vol. 223, no. 12, pp. 1499–1516, 2009.
- Y. Gao, T. Lin, F. Borrelli, E. Tseng, and D. Hrovat, “Predictive Control of Autonomous Ground Vehicles With Obstacle Avoidance on Slippery Roads,” in ASME 2010 Dynamic Systems and Control Conference, Volume 1. ASME, 2010, pp. 265–272.
- J. Liu, P. Jayakumar, J. L. Stein, and T. Ersal, “A Multi-Stage Optimization Formulation for MPC-Based Obstacle Avoidance in Autonomous Vehicles Using a LIDAR Sensor,” in ASME 2014 Dynamic Systems and Control Conference. ASME, oct 2014.
- J. Ji, A. Khajepour, W. Melek, and Y. Huang, “Path Planning and Tracking for Vehicle Collision Avoidance based on Model Predictive Control with Multi-Constraints,” IEEE Transactions on Vehicular Technology, vol. 9545, no. April, pp. 1–1, 2016.
- C. V. Altrock, “Fuzzy logic technologies in automotive engineering,” in WESCON/94. Idea/Microelectronics. Conference Record, Sep 1994, pp. 110–117.
- J. Y. Goh and J. C. Gerdes, “Simultaneous stabilization and tracking of basic automobile drifting trajectories,” in 2016 IEEE Intelligent Vehicles Symposium (IV). IEEE, jun 2016, pp. 597–602.
- R. V. Cowlagi and P. Tsiotras, “Hierarchical motion planning with kinodynamic feasibility guarantees: Local trajectory planning via model predictive control,” in 2012 IEEE International Conference on Robotics and Automation. IEEE, may 2012, pp. 4003–4008.
- M. Choi and S. B. Choi, “Model Predictive Control for Vehicle Yaw Stability With Practical Concerns,” IEEE Transactions on Vehicular Technology, vol. 63, no. 8, pp. 3539–3548, oct 2014.
- J. Funke, P. Theodosis, R. Hindiyeh, G. Stanek, K. Kritatakirana, C. Gerdes, D. Langer, M. Hernandez, B. Muller-Bessler, and B. Huhnke, “Up to the limits: Autonomous Audi TTS,” in 2012 IEEE Intelligent Vehicles Symposium. IEEE, jun 2012, pp. 541–547.
- W. Zhan, J. Chen, C.-y. Chan, C. Liu, and M. Tomizuka, “Spatially-Partitioned Environmental Representation and Planning Architecture for On-Road Autonomous Driving,” in 2017 IEEE Intelligent Vehicles Symposium. IEEE, jun 2017.
- H. Pacejka, Tire and vehicle dynamics. Elsevier, 2005.
- R. Guntur and S. Sankar, “A friction circle concept for dugoff’s tyre friction model,” International Journal of Vehicle Design, vol. 1, no. 4, pp. 373–377, 1980.
- E. Siampis, E. Velenis, and S. Longo, “Torque Vectoring Model Predictive Control with Velocity Regulation Near the Limits of Handling,” Vehicle System Dynamics, pp. 2553–2558, 2015.
- G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou, “Aggressive driving with model predictive path integral control,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), vol. 2016-June. IEEE, may 2016, pp. 1433–1440.
- F. Altché, P. Polack, and A. de La Fortelle, “High-Speed Trajectory Planning for Autonomous Vehicles Using a Simple Dynamic Model,” arXiv preprint arXiv:1704.01003, apr 2017.
- J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic and dynamic vehicle models for autonomous driving control design,” in 2015 IEEE Intelligent Vehicles Symposium (IV). IEEE, jun 2015, pp. 1094–1099.
- B. Houska, H. Ferreau, and M. Diehl, “ACADO Toolkit – An Open Source Framework for Automatic Control and Dynamic Optimization,” Optimal Control Applications and Methods, vol. 32, no. 3, pp. 298–312, 2011.