High-Speed Trajectory Planning for Autonomous Vehicles Using a Simple Dynamic Model

TL;DR

This paper presents a real-time trajectory planning method for autonomous vehicles that uses a simple dynamic model within a model predictive control framework to adapt velocities near dynamic limits, improving safety and efficiency.

Contribution

It introduces a trajectory planner based on a simplified dynamic model that can select feasible velocities and operate in real-time, outperforming kinematic models in robustness and trajectory quality.

Findings

Real-time feasible trajectories generated for high-speed driving.

Simplified dynamic model enhances robustness and trajectory quality.

Outperforms traditional kinematic models in simulations.

Abstract

To improve safety and energy efficiency, autonomous vehicles are expected to drive smoothly in most situations, while maintaining their velocity below a predetermined speed limit. However, some scenarios such as low road adherence or inadequate speed limit may require vehicles to automatically adapt their velocity without external input, while nearing the limits of their dynamic capacities. Many of the existing trajectory planning approaches are incapable of making such adjustments, since they assume a feasible velocity reference is given. Moreover, near-limits trajectory planning often implies high-complexity dynamic vehicle models, making computations difficult. In this article, we use a simple dynamic model derived from numerical simulations to design a trajectory planner for high-speed driving of an autonomous vehicle based on model predictive control. Unlike existing techniques, our formulation includes the selection of a feasible velocity to track a predetermined path while avoiding obstacles. Simulation results on a highly precise vehicle model show that our approach can be used in real-time to provide feasible trajectories that can be tracked using a simple control architecture. Moreover, the use of our simplified model makes the planner more robust and yields better trajectories compared to kinematic models commonly used in trajectory planning.

Tables

Table 1. Table I: Notations

$X$, $Y$, $Z$	Position of the vehicle’s CoM (ground frame)
$\theta$, $\varphi$, $\psi$	Roll, pitch and yaw angles of the car body
$V_x$, $V_y$	Longitudinal and lat. vehicle speed (vehicle frame)
$V_{x{w}_i}$	Longitudinal speed of wheel $i$ (wheel frame)
${\omega }_i$	Angular velocity of wheel $i$
${\zeta }_i$	Displacement of suspension $i$
$\delta$	Steering angle of the front wheels
$T_{{\omega }_i}$	Total torque applied to wheel $i$
$F_{x{w}_i}$, $F_{y{w}_i}$	Longitudinal and lateral forces on wheel $i$ (wheel frame)
$F_{x_i}$, $F_{y_i}$	Longitudinal and lat. forces on wheel $i$ (vehicle frame)
$F_{z_i}$	Normal ground force on wheel $i$
$F_{aero}$	Air drag force on the vehicle
$M_T$	Total mass of the vehicle
$I_x$, $I_y$, $I_z$	Roll, pitch and yaw inertia of the vehicle
$I_{r_i}$	Inertia of wheel $i$ around its axis
$l_f$, $l_r$	Distance between the front/rear axle and the CoM
$l_w$	Half-track of the vehicle
$r_w$	Effective radius of the wheels
$k_s$, $d_s$	Suspensions stiffness and damping

Table 2. Table II: Absolute lateral positioning error for both planners.

Model	RMS error ($m$)	Maximum error ($m$)
Proposed	$0.25$	$0.70$
Kinematic	$0.16$	$0.95$

Equations

$$\displaystyle\dot{X}=\$$

$$\displaystyle\dot{Y}=\$$

$$\displaystyle\dot{V}_{x}=\$$

$$\displaystyle\dot{V}_{y}=\$$

$$I_z\ddot{\psi }=l_f(F_{y_1}+F_{y_2})-l_r(F_{y_3}+F_{y_4})$$

$$+l_w(F_{x_2}+F_{x_4}-$$

$$I_x\ddot{\theta }=l_w(F_{z_1}+F_{z_3}-F_{z_2}-F_{z_4})+$$

$$I_y\ddot{\varphi }=l_r(F_{z_3}+F_{z_4})-l_f(F_{z_1}+F_{z_2})-$$

$$I_r{\dot{\omega }}_i=T_{{\omega }_i}-r_w{F}_{x{w}_i}.$$

$$\tau_{i}=\left\{\begin{array}[]{l l}\frac{r_{w}\omega_{i}-V_{xw_{i}}}{r_{w}\omega_{i}}&\text{if }r_{w}\omega_{i}\geq V_{xw_{i}}\\ \frac{r_{w}\omega_{i}-V_{xw_{i}}}{V_{xw_{i}}}&\text{otherwise}.\end{array}\right.$$

$${\alpha }_f$$

$${\alpha }_r$$

$$f_{2di}\left(\xi,\mathbf{u}\right)=\left[\begin{array}[]{c}v_{x}\cos\psi-v_{y}\sin\psi\\ v_{x}\sin\psi+v_{y}\cos\psi\\ {[}v_{\psi},u_{x},u_{y},u_{\psi}{]^{T}}\end{array}\right].$$

$${\left(\frac{a_X}{\alpha }\right)}^2+{\left(\frac{a_Y}{\beta }\right)}^2\le 1$$

$$a_X^{min}(v_{x,0})\le a_X\le a_X^{max}(v_{x,0})$$

$$A[a_X,a_Y,a_{\psi }{]}^T\le b$$

$$\displaystyle\min_{(\mathbf{u}_{k})_{k=0\dots K-1}}\ \$$

$${\xi }_{k+1}-{\xi }_k=h{f}_{2di}({\xi }_k,{\mathbf{u}}_k)$$

$${\xi }_0=\xi (t_0)$$

$${\left(\frac{u_X}{\alpha }\right)}^2+{\left(\frac{u_Y}{\beta }\right)}^2\le 1$$

$$a_X^{min}(v_{x,0})\le a_X\le a_X^{max}(v_{x,0})$$

$$A[u_X,u_Y{]}^T\le b$$

$$u_{\psi }=\gamma u_Y$$

$$\forall o\in \mathcal{O},p_o(X_k,Y_k)\le 0$$

$$k=0\dots K-1.$$

$$J\big{(}(\xi_{k}),(\mathbf{u}_{k})\big{)}=\sum_{k=0}^{K}(X_{k}-X_{k}^{ref})^{2}+(Y_{k}-Y_{k}^{ref})^{2}.$$

$$J=\sum _{k=0}^{K-1}{w}_v{v_{tol}}_k^2+w_X{X_{tol}}_k^2+w_Y{Y_{tol}}_k^2+w_o{O_{tol}}_k^2$$

$${v_{tol}}_k$$

$${X_{tol}}_k$$

$${Y_{tol}}_k$$

$$\forall o\in \mathcal{O},{O_{tol}}_k$$

$$\text{for }k=0\dots K-1.$$

$$\dot{X}$$

$$\dot{Y}$$

$$\dot{\psi }$$

$$\dot{v}$$

Full text

I Introduction

In order to improve safety and energy efficiency and not to surprise other road users, autonomous vehicles are widely expected to drive smoothly; moreover, traffic efficiency concerns will likely lead these vehicles to follow relatively closely the road speed limit. However, such speed limits may not always exist (e.g., on some dirt roads) or they may not be followed safely due to the road topology (e.g., mountain roads with sharp curves) or weather conditions (e.g., wet or icy road).

Many trajectory planning algorithms rely on an a-priori knowledge of a target velocity that can either be an explicit parameter of the problem [1, 2] or be implicitly given by requiring a set of target positions at fixed times [3, 4]. Some authors have proposed using the road curvature [5] to provide an upper bound on the velocity, which corresponds to a maximum lateral acceleration; this method can be extended to obstacle avoidance by first planning a collision-free path, then adjusting the velocity consequently [6]. However, as path selection and velocity planning are intrinsically linked, this approach can lead to severe inefficiencies.

For this reason, model predictive control (MPC) techniques are often used in the literature, since they allow to simultaneously compute a feasible trajectory and a sequence of control inputs to track it. However, high-speed trajectory planning requires a complex modeling of the vehicle to account for its dynamic limitations, which mainly come from the complex and highly non-linear [7] tire-road interactions. Adding to this difficulty, wheel dynamics are generally much faster (around $1\text{\,}\mathrm{ms}$ [8]) than changes of the vehicle’s macroscopic state (typically $100\text{\,}\mathrm{ms}$). Therefore, the existing literature is generally divided between medium-term (a few seconds) trajectory planning including obstacle avoidance for low-speed applications, mainly relying on simple kinematic models (see, e.g. [9, 10]), and short-term (sub-second) trajectory tracking for high-speed or low-adherence applications using wheel dynamics modeling (see, e.g., [11, 12, 13, 14]). In the second case, obstacle avoidance is generally not considered (with the notable exception of [15]), and the existence of a feasible collision-free trajectory is not guaranteed in the case of an unexpected obstacle. Note that sampling-based approaches [16] have also been proposed in the literature; however, they do not provide optimality guarantees when a finite number of samples is selected, and may have trouble finding a feasible solution in complex scenarios.

In this article, we propose a middle-ground approach to allow trajectory planning over a few seconds in high dynamic situations. This approach relies on the use a simple vehicle model (initially proposed in [17]) derived from a realistic dynamic modeling of the vehicle body and wheels, which accounts for tire slip effects using carefully estimated bounds on the dynamics. Using this model, we design a non-linear trajectory planning framework, which is able to follow a predefined path (e.g., the road centerline) at high speed while avoiding obstacles. Comparison with a more classical kinematic bicycle model shows that our proposed planner provides better trajectories and is more robust. Moreover, our simulations showed that computation time remains below $80\text{\,}\mathrm{ms}$ on a standard computer and could be further reduced using auto-generation [4], thus allowing real-time use. To the best of the authors’ knowledge, no comparable framework has been published for medium- or long-term planning in high dynamic situations.

The rest of this article is structured as follows: in Section II, we present a 9 degrees of freedom dynamic model of the vehicle’s body, that we use throughout the rest of this article. In Section IV, we briefly describe our simplified dynamic model and to formulate a trajectory planner based on Model Predictive Control, which computes an aggressive yet feasible trajectory. This planner is validated and its performance is compared to a classical kinematic bicycle model in Section V using a realistic physics simulation suite; finally, Section VI concludes the study.

II Vehicle model

In this article, we consider a 9 degrees of freedom modeling of the vehicle body, which provides a satisfying balance between accuracy and computational cost. Alongside with the usual 2D state $[X,Y,\psi]$ (with $\psi$ the yaw rotation) of the vehicle, the model takes into account roll and pitch movements, wheel dynamics and coupling of longitudinal and lateral tire slips. 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 $T_{i}$ applied to each wheel $i$ and the steering angle of the front wheels, $\delta$. We use uppercase letters (e.g., $X$, $Y$) to denote coordinates in the ground (global) frame, and lowercase letters for coordinates in the vehicle (local) frame; the $x$ coordinate in the local frame corresponds to the longitudinal component. The notations are given in Table I and illustrated in Figure 1.

We make the assumptions 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:

[TABLE]

where $F_{x_{i}}$ and $F_{y_{i}}$ are respectively the longitudinal and lateral tire forces generated on wheel $i$, expressed in the local vehicle frame $(x,y)$. The yaw, roll and pitch motions of the car body are computed as:

[TABLE]

where $F_{z_{i}}=-k_{s}\zeta_{i}(\theta,\phi)-d_{s}\dot{(\zeta_{i})}(\theta,\phi)$, with $\zeta_{i}(\theta,\phi)$ the displacement of suspension $i$ for the given roll and pitch angles of the car body. The variation of $F_{z}$ models the impact of load transfer between tires. Finally, the dynamics of each wheel $i$ can be written as

[TABLE]

In general, the longitudinal and lateral forces $F_{xw_{i}}$ and $F_{yw_{i}}$ depend on the longitudinal slip ratio $\tau_{i}$, the side-slip angle $\alpha_{i}$, the reactive normal force $F_{z_{i}}$ and the road friction coefficient $\mu$. The slip ratio of wheel $i$ can be computed as

[TABLE]

The lateral slip-angle $\alpha_{i}$ of tire $i$ is the angle between the wheel’s orientation and its velocity, and can be expressed as

[TABLE]

where $f$ and $r$ denote the front and rear wheels respectively.

In this article, we use Pacejka’s combined slip tire model (equations (4.E1) to (4.E67) in [7]), which takes into account the interaction between longitudinal and lateral slips, thus encompassing the notion of friction circle [18]. For clarity purposes, we do not reproduce the complete set of equations here.

III Constrained second-order integrator model

Theoretically, it is possible to use the 9 degrees of freedom model inside a model predictive control framework to directly compute an optimal trajectory and the corresponding controls. However, classical optimization tools often struggle to solve problems involving highly nonlinear constraints or cost functions, as it is the case in the model presented in Section II – notably due to disjunction (4) which makes $\tau$ non-differentiable. Additionally, wheel dynamics generally occur over very small characteristic times – typically a few milliseconds – which requires choosing a correspondingly small discretization time step, making planning over long horizons impractical at best. For this reason, simplified models are very often preferred. Kinematic bicycle (or single-track) models [19, 9, 10], or even simpler second-order integrator models [2] are therefore common in the trajectory planning literature.

One of the main issues of these simplified models is that they are generally considered to be imprecise when nearing the handling limits of the vehicle. To counter this problem, previous work [17] presented a constrained second-order dynamic model derived from simulation data using the 9 degrees of freedom model of Section II. For lack of space, we only briefly described these previous results here.

Using a random sampling method, we first compute (off-line) an envelope for the set of feasible longitudinal ($a_{X}$), lateral ($a_{Y}$) and angular ($a_{\psi}$) accelerations, as shown in Figure 2. More specifically, these sets are computed for a given longitudinal velocity $v_{x,0}$ and a lateral velocity $v_{y,0}\in[-0.2v_{x,0},0.2v_{x,0}]$. Therefore, only part of this region is actually reachable from an initial vehicle state (corresponding to a determined lateral velocity). In this article, we assume that the lateral dynamics of the vehicle is sufficiently fast to neglect this effect. Simulation results (see Section V) show this assumption seems reasonable.

Using these results, we propose a constrained double integrator model for the vehicle dynamics. This model considers a state vector $\xi=[X,Y,\psi,v_{x},v_{y},v_{\psi}]^{T}$ and a control $\mathbf{u}=[u_{x},u_{y},u_{\psi}]^{T}$, with the same notations and reference frames as presented in Section II. The dynamic equation of the system is $\dot{\xi}=f_{2di}(\xi,\mathbf{u})$ with

[TABLE]

To allow the use of this model in planning, we approximate the sets shown in Figure 2 as a set of convex linear and nonlinear constraints in the $(a_{X},a_{Y})$ plane as shown in Figure 3. These constraints are expressed as:

[TABLE]

where $A$ is a constant matrix, $b$ a constant vector and $a_{X}^{min}$, $a_{Y}^{min}$ depend on $v_{x,0}$. For our model vehicle, the experimental data of Figure 2 yield $\alpha=$ $9.4\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-2}$, $\beta=$ $9.0\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-2}$, $A=\left(\begin{smallmatrix}2.6&1\\ 2.6&-1\end{smallmatrix}\right)$ and $b=\left(\begin{smallmatrix}15.3\\ 15.3\end{smallmatrix}\right)$ $\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-2}$. Figure 4 shows the variation of $a_{X}^{min}$ and $a_{X}^{max}$ with the initial longitudinal velocity; a polynomial fit yields $a_{X}^{min}(v_{x,0})=-9.3-0.013v_{x,0}+0.00072{v_{x,0}}^{2}$ and $a_{X}^{max}(v_{x,0})=4.3-0.009v_{x,0}$ (with $v_{x,0}$ expressed in $\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-1}$). In the $(a_{Y},a_{\psi})$ plane, we suppose a linear relation between $a_{\psi}$ and $a_{Y}$ in the form: $a_{\psi}=\gamma a_{Y}$, with $\gamma=$ $0.56\text{\,}\mathrm{rad}\text{\,}{\mathrm{m}}^{-1}$. Although seemingly restrictive, this approximation provides better results than using a parallelogram acceptable region, and in practice corresponds to minimizing the vehicle slip angle.

IV MPC formulation for trajectory planning

We now use the second-order integrator model developed in the previous section to design a trajectory planner based on model predictive control. In this section, we assume that the vehicle tries to follow a known reference path, for instance the centerline of a given lane. The reference path $\gamma_{ref}$ is supposed to be given as a set of positions $(X_{ref},Y_{ref})$.

In this article, we assume that the vehicle has no information on a safe choice of longitudinal speed. Such situations can arise in various scenarios, such as poor adherence conditions where the speed limit cannot be safely followed, or in the absence of speed limits, for instance on dirt roads or for racing. Moreover, we assume that the vehicle also needs to avoid obstacles on the road; for now, we only consider fixed obstacles with known positions and shapes. Additionally, we do not consider varying road adherence, and we suppose that the tire-road friction coefficient $\mu$ is constant and equal for all four wheels. Future work will study real-time estimation of this friction coefficient to adapted the planner accordingly.

The planning and control scheme works in several steps, as presented in Algorithm 1. We first approximate $X_{ref}$ and $Y_{ref}$ over the next planning horizon $T$ as fifth order polynomials of the curvilinear position, starting at the point of $\gamma_{ref}$ closest to the vehicle’s current position. Using these polynomials, we compute the maximum (in absolute value) of the path curvature over the planning horizon, noted $\kappa$, to determine an upper bound $v_{max}=\sqrt{\mu g/{\kappa}}$ for the speed of the vehicle in order to limit the lateral acceleration to $\mu g$ (as proposed, e.g., in [5]). Only the relevant obstacles, i.e. those for which a risk of collision exists during the next planning horizon $T$, are effectively considered for collision avoidance; we note $\mathcal{O}$ the set of these obstacles. Generalizing the ideas of [20], we determine a bounding parabola for each obstacle $o\in\mathcal{O}$ as shown in Figure 5, such that the collision avoidance constraints can be written as $p_{o}(X,Y)\leq 0$, with $p_{o}$ a second-order multinomial function. Note that the use of unbounded parabolas is preferred to bounded shapes such as ellipses, since they do not create local minima.

At a time $t_{0}$ corresponding to a vehicle state $\xi(t_{0})$ (with an initial longitudinal speed $v_{0}$), we formulate the motion planning problem using model predictive control with time step duration $h$ and horizon $T=Kh$ as follows:

[TABLE]

Note that, as it is often the case in the planning literature (see, e.g., [21]), collision avoidance (eq. (11h)) is actually implemented as soft constraints to avoid infeasibility caused by numerical errors. Additionally, note that our formulation can be slightly modified in order to take moving obstacles into account, by using a different parabola $p_{o}^{k}$ for each obstacle and at each time step.

In this article, we only focus on minimizing the deviation from the reference trajectory. In most of the existing MPC literature where a reference speed is supposed to be known in advance, the cost function $J$ is expressed as:

[TABLE]

In these formulations, $X_{k}^{ref}$ and $Y_{k}^{ref}$ implicitly encode the speed at which the reference path should be followed. Since we do not assume that a reference speed is known in advance, this method cannot be applied directly. A possible way to handle this difficulty is to express $Y_{k}^{ref}$ as a function of $X_{k}$ (see, e.g, [22]). However, this method cannot be applied to all shapes of reference paths, and is notably not suited to sharp turns even when using local instead of global coordinates. For this reason, we introduce an auxiliary state $s$ to denote the curvilinear position of the vehicle along $\gamma_{ref}$, so that $X_{k}^{ref}=p_{X}(s_{k})$ and $Y_{k}^{ref}=p_{Y}(s_{k})$. In this article, we use a simple first-order integrator dynamic for $s$ with $\dot{s}=\sqrt{v_{x}^{2}+v_{y}^{2}}$. Noting $\xi^{\prime}$ the extended state of the vehicle, we instead use the objective function:

[TABLE]

where $w_{v}$, $w_{X}$, $w_{Y}$ and $w_{o}$ are positive weighting terms, and we add the following constraints to problem (11):

[TABLE]

V Simulation results

We used the realistic physics simulator PreScan [23] to validate the proposed MPC trajectory planner. The simulator uses the 9 degrees of freedom model presented in Section II, with the same parameters that were used to obtain the sets of feasible accelerations in Section III. Robustness of the planner to variations of the vehicle parameters is a subject for further study.

The MPC problem (11)-(14) is solved using the ACADO Toolkit [24]; due to the inner workings of the simulator, the simulation is paused during resolution. The output of the solver is the set of future target longitudinal and lateral accelerations, as well as target future positions and longitudinal velocities for the vehicle in the horizon $T$. These outputs are fed into two low-level controllers, one being tasked with velocity tracking and the other with steering.

Low-level tracking of the planned trajectory is achieved using PID controllers; the lateral control also uses a $\tau$ seconds look-ahead [25]. At time $t$, the predicted position of the vehicle at $t+\tau$ is computed as $\hat{X}=X+\tau v_{x}\cos(\hat{\psi})$ and $\hat{Y}=Y+\tau v_{x}\cos(\hat{\psi})$, with $\hat{\psi}=\psi+\frac{1}{2}\tau\dot{\psi}$. Instead of tracking the target position at time $t$, the lateral control uses the error between predicted and desired positions at time $t+\tau$. This method was found to provide better performance and stability than a simple PID in our simulations, with a look-ahead duration of $\tau=$0.2\text{,}\mathrm{s}$$. The lateral control takes into account a limited angular velocity for the steering wheel of $12\text{\,}\mathrm{rad}\text{\,}{\mathrm{s}}^{-1}$, which is in the average of recorded steering velocities for human drivers in obstacle avoidance scenarios [26]. However, we do not consider the dynamics of the engine or brakes, which are supposed to respond instantaneously.

The reference path used in our simulations is presented in Figure 6; the path consists of a $60\text{\,}\mathrm{m}$ straight line, a half circle with radius $20\text{\,}\mathrm{m}$, a $200\text{\,}\mathrm{m}$ straight line, a Bezier arc corresponding to a 135 degrees turn, a $100\text{\,}\mathrm{m}$ straight line, a half circle with radius $10\text{\,}\mathrm{m}$ and a final -135 degrees turn Bezier arc.

In all simulations, the weights are chosen as $w_{v}=1$, $w_{X}=w_{Y}=10$ and $w_{o}=100$. The planning horizon is chosen as $T=$3\text{,}\mathrm{s} and the time step duration of the MPC is $h=$200\text{\,}\mathrm{ms}; replanning is performed every $100\text{\,}\mathrm{ms}$. To achieve real-time computation speeds, the solver is limited to five SQP iterations, which in our experience is sufficient to achieve a good convergence. Auto-generation techniques can also be used to further reduce computation time [4].

For comparison purposes, we also implemented the same MPC planner with a classical kinematic bicycle model such as presented in [19]; the model is written as follows:

[TABLE]

with $v$ the longitudinal velocity and $\beta=\frac{l_{r}}{l_{r}+l_{f}}\tan\delta$ the side slip angle. The control inputs are the longitudinal acceleration $a$, and the steering angle of the front wheel $\delta$.

All other parts of the planning and control algorithm are otherwise equal, including the low-level controller. The maximum and minimum acceleration in the bicycle model are chosen equal to $a_{X}^{min}$ and $a_{X}^{max}$ (see Figure 3 for the notations) respectively. Note that the solver is slightly faster using this formulation; therefore, the maximum number of SQP iterations is set to 6 for the kinematic model to yield comparable computation times, which increases solution quality.

V-A Planning without obstacles

We first consider trajectory planning without obstacles; Figure 7 presents the actual speed of the vehicle during the simulation as a function of its position along the path $\gamma_{ref}$ for the two MPC planners, as well as the speed bound $v^{2}\leq\kappa g$ (with $\kappa$ the path curvature), corresponding to a centripetal acceleration of $1\,g$ used, e.g., in [5]. First, we notice that both planners have similar performance in the straight portions of the road; however, the planner based on our proposed second-order integrator model systematically achieves higher speeds in curves. Second, no solver reaches the upper bound $\sqrt{g/\kappa}$, thus confirming that including a speed selection phase during planning (as opposed to tracking a predefined velocity solely computed from path curvature) is relevant to allow proper tracking.

The superior performance of the planner based on the second-order integrator model is likely due to the simpler relation between the optimization variables (the input controls) and the objective value (the deviation from the target state) than in the bicycle model, which allows a much faster convergence towards the solution. Indeed, when allowing the solver to run until convergence with the kinematic bicycle model, the resulting velocity becomes comparable to that obtained with the real-time second-order model (but computation time is above $500\text{\,}\mathrm{ms}$).

Figure 8 shows the lateral error when tracking the reference path, for both MPC planners. Table II presents synthetic data about the lateral error of the complete planning and control architecture in both cases, showing satisfying overall performance for high-speed applications. Note that better precision can be achieved (at the cost of speed) by selecting different values for the weighting coefficients. Moreover, a more precise low-level controller can probably achieve better performance.

V-B Planning with obstacle avoidance

In this section, we compare the behavior of both planners in the presence of obstacles, modeled as parabolas as explained in Figure 5. Figure 6 shows a detail of the actual path followed by the vehicle using the proposed planner while avoiding obstacles. The attached video file111Also available at https://youtu.be/BRpmdIxTz-0 shows the corresponding simulation.

Figure 9 presents the lateral deviation from the reference trajectory while avoiding obstacles using both planners. Generally speaking, the proposed planner allows a smaller deviation from the reference and a higher average speed of $12.7\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-1}$ compared to $10.2\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-1}$ using the kinematic bicycle planner (for lack of space, the velocity curves are not shown). More importantly, the kinematic planner is sometimes unable to output a trajectory in less than $100\text{\,}\mathrm{ms}$, as shown in Figure 10. This situation happens when approaching obstacles in high-curvature portions of the road; as before, the better behavior of the proposed planner is likely due to the simpler search space since the dynamic model presents much less non-linearity. A more in-depth analysis could provide useful insights on desirable model properties to allow fast and robust convergence.

VI Conclusion

In this article, we considered the trajectory planning and control of a vehicle at high velocity near the limits of handling. Instead of using a highly complex model during online resolution, as it is often the case in the literature, we used a simpler vehicle model obtained from precise dynamic simulations. This model was implemented into a novel MPC-based trajectory planner; contrary to most existing algorithms, our formulation does not need a predefined target speed. Instead, the MPC adjusts the vehicle’s speed in real time to track a predefined path (such as the centerline of a road) as fast as possible while maintaining a low tracking error. Using the high fidelity physics simulator PreScan, we demonstrated that the combination of this planner with a simple, PID-based low-level controller is capable of driving along a demanding path at high speed with a low lateral error. Moreover, comparison with a simple and widely used bicycle model shows that the use of our simpler model allows the solver to converge faster towards a better quality solution under real-time constraints.

Although this work remains mainly theoretical, it opens several perspectives for future research. First, the good performance of our simple dynamic model even at high speeds allows envisioning longer planning horizons without sacrificing computation speed. Future work should also study the consistency of using fully linear models, that can be coupled with efficient mixed-integer optimization techniques to allow optimal decision-making, for instance for overtaking or lane-change decisions [2]. Second, we believe that the ability of the proposed MPC formulation to automatically adapt the vehicle’s velocity can find practical applications, for instance when driving in low-adherence situations. Therefore, the behavior of the planner with a variable friction coefficient should be studied further. A more precise low-level controller, taking into account the very particular response curve of the tires to precisely track a target force, which would be more consistent with the second order integrator model, can also prove interesting. Finally, implementation of the planner and controller on an actual (scale model) vehicle is a necessary step to validate the suitability of the proposed planner for real-world use.